[发明专利]基于航向控制律的无人机避障航路规划方法

摘要

本发明提供了一种基于航向控制律的无人机避障航路规划方法，对圆形障碍物进行检测规避，考虑了飞机横侧向控制规律或实际飞行轨迹的要求，规划出的航路是实际可飞的，采用混合模型对飞行环境进行建模，解决单一环境下不能满足航路上障碍物检测、航路计算对环境的多种要求问题。

主权项

1.一种基于航向控制律的无人机避障航路规划方法，其特征在于包括下述步骤：步骤1：根据无人机飞行范围，建立全局的平面直角坐标系XOY，根据飞行起始点位置和飞行范围，建立覆盖飞行范围的全局坐标系XOY，飞行范围用一个矩形区域来表示，以矩形区域的左下角为原点O，以正东方向为X轴正向，以正北方向为Y轴正向，建立平面直角坐标系XOY；坐标系的单位采用角度秒；在东北半球，沿正东方向，经度逐渐增加；沿正北方向，纬度逐渐增加；并将飞行范围栅格化，每个栅格的尺寸为1″×1″；步骤2：调用圆栅格化算法，在栅格化后的全局坐标系XOY中标注圆形障碍区；步骤3：通过上级指挥系统获取无人机任务作业高度下的飞行起始点S和终止点E，单位均为角度秒，并连接直线段SE，得到初始直线段航路SE，假设无人机初始的机头指向与初始直线段航路SE的方向相同；步骤4：调用直线段栅格化算法，将初始直线段航路SE栅格化，栅格化坐标集合为Line，Line＝{(X_i，Y_i)|i＝1，2，…，n}；步骤5：i从1开始取集合Line中的元素，判断每一个元素是否与圆形障碍区相交，如果初始直线段航路SE与圆形障碍区不相交，转步骤8；否则，转步骤6，进行避障航路规划步骤；步骤6：为了确保飞行安全，无人机根据自身的机动性能避开圆形障碍区，采取的策略包括：策略1：在距离禁飞区或障碍物区尽可能近的点开始转弯机动性飞行；策略2：从禁飞区或障碍物区的外圆弧的劣弧一侧进行转弯机动性飞行；建立局部直角坐标系xoy，以初始直线段航路SE作为局部直角坐标系xoy的x轴，方向与初始直线段航路SE的方向相同；在近S点一端做圆形障碍区的切线与x轴的交点作为局部直角坐标系xoy的原点o；切线作为局部直角坐标系xoy的y轴，构成右手坐标系xoy；作圆的外切正方形，正方形的边平行与x轴或垂直于x轴，h是劣弧顶点距x轴的距离；无人机绕过圆形障碍区的5段规划飞行航路为：第1段：从尽可能靠近圆形禁飞区的W₀点在圆形障碍区的劣弧一侧转弯至W₁点后改为直飞；从尽可能靠近圆形禁飞区的W₀点开始是因为直线段SW₀是无人机可以安全飞行的，在这段航路中，航向角的初始值为0，到达W₁点时的方向角为Ψ_c，由航向角控制律的时域表达式以及使给定航向角近似等于实际航向角可得出Ψ(t)≈Ψ_c；第2段：从W₁点直飞至W₂点改为向圆形禁飞区的优弧一侧转弯；第3段：从W₂点向圆形禁飞区的优弧一侧转弯至W₃点后改为直飞；第4段，从W₃点直飞至W₅点后，从圆形禁飞区的优弧一侧转弯；第5段，从W₅点向圆形禁飞区的优弧一侧转弯至W₆点改为直飞；这时，无人机机头指向飞行结束点E，与无人机到达W₀点之前的状态是相同的，在这段航路中，航向角的初始值为0，终值为Ψ_e，由航向角控制律的时域表达式以及使给定航向角近似等于实际航向角可得出Ψ(t)≈Ψ_e；所述规划飞行航路的参数计算如下：a)局部坐标系参数初始化：在平面直角坐标系XOY中有一个圆形障碍区的圆，其圆心坐标为(X_b，X_b)，半径为R_b，直线段SE与圆相交，直线CD与直线段SE垂直且与圆相切，且直线CD在距S点近的SE与CD的交点为o，求交点o的坐标，已知点S(X_s，Y_s)和点E(X_e，Y_e)的坐标；直线段SE的斜率k为：过圆心(X_b，X_b)，作平行于直线段SE的直线AB，直线AB的方程为：Y‑Y_b＝k(X‑X_b)直线AB与圆的交点可通过解方程组获得：解方程组，可得交点如下：计算S点与两个交点的距离：选择与S点近的一个交点F，即交点F(X_f，X_f)：若d₁＜d₂，则X_f＝X₁，Y_f＝Y₁；否则X_f＝X₂，Y_f＝Y₂；过交点F(X_f，X_f)做垂直于直线段SE的直线CD，直线CD的方程为：直线SE与直线CD的交点o的坐标，可通过解方程组获得：解方程组，可得交点o在全局坐标系XOY中的坐标如下：起始点S的坐标转化：其中，(X_o，Y_o)为局部坐标系xoy的原点o在全局坐标系XOY中的坐标，(X_s，Y_s)为S点在XOY坐标系中的坐标，(x_s，y_s)为S点在xoy坐标系中的坐标，θ为x轴与X轴的夹角；终止点E的坐标转化：其中，(X_e，Y_e)为S点在XOY坐标系中的坐标，(x_e，y_e)为S点在xoy坐标系中的坐标；其中，(X_b，Y_b)为圆形障碍区的圆心在XOY坐标系中的坐标，(x_b，y_b)为圆形障碍区的圆心在xoy坐标系中的坐标；令：h＝R_b‑|y_b|其中，R_b为圆形障碍区的半径；可得W₃和W₄在xoy坐标系中的坐标如下：其中，为W₃在xoy坐标系中的坐标；其中，为W₄在xoy坐标系中的坐标；b)：计算航向指令角Ψ_c其中，α＞0为航向常数，已知量；v为无人机速度，d_min为最小航路段长度，设定常值；Ψ_c为航向指令角；c)：计算Δx₁和Δy₁，Δx₁＝|x_W1‑x_W0|，Δy₁＝|y_W1‑y_W0|，；d)：计算Δx₂和Δy₂(Δx₂＝|x_W2‑x_W1|，Δy₂＝|y_W2‑y_W1|：e)：计算Δx₃和Δy₃，Δx₃＝|x_W3‑x_W2|，Δy₃＝|y_W3‑y_w2|；f)：计算航向指令角Ψ_e；g)：计算Δx₅和Δy₅，Δx₅＝|x_W5‑x_W6|，Δy₅＝|y_W5‑y_W6|：h)：计算获得在局部坐标系xoy中的避障航路点坐标；i)：通过旋转、平移变换，将位于局部坐标系xoy的航路点坐标转化为全局坐标系XOY中的坐标：步骤7：根据步骤6计算的全局坐标系XOY中的坐标，得到W₀到W₆六个转折点的坐标，形成避障航路点集合，避障航路规划结束；步骤8：初始直线段航路SE与圆形障碍区不相交，直接从S点飞向E点，形成直飞航路点集合，航路规划结束。