基于航向控制律的无人机避障航路规划方法与流程

本发明涉及无人机航路规划领域，尤其是一种无人机避障航路规划方法。
背景技术：
：无人机的发展趋势之一是发展无人机的自主性，而无人机航路规划是无人机自主性的关键问题之一。现有技术只是从数学上考虑了规划航路的连续性或最小转弯半径等，没有从飞机的机动性上考虑规划的航路是否可飞。现有技术对环境的建模是单一的，即对飞行环境只用一种模型来表示(例如，栅格模型、直角坐标系等)，单一环境表示不能满足规划航路时的航路上障碍物检测、航路计算对环境的多种要求。目前未见相关的基于航向控制律的无人机避障航路规划方法。技术实现要素：为了克服现有技术的不足，本发明提供一种基于航向控制律的无人机避障航路规划方法，对圆形障碍物进行检测规避，考虑了飞机横侧向控制规律或实际飞行轨迹的要求，规划出的航路是实际可飞的，采用混合模型对飞行环境进行建模，解决单一环境下不能满足航路上障碍物检测、航路计算对环境的多种要求问题。本发明解决其技术问题所采用的技术方案的步骤如下：步骤1：根据无人机飞行范围，建立全局的平面直角坐标系xoy，根据飞行起始点位置和飞行范围，建立覆盖飞行范围的全局坐标系xoy，飞行范围用一个矩形区域来表示，以矩形区域的左下角为原点o，以正东方向为x轴正向，以正北方向为y轴正向，建立平面直角坐标系xoy；坐标系的单位采用角度秒；在东北半球，沿正东方向，经度逐渐增加；沿正北方向，纬度逐渐增加；并将飞行范围栅格化，每个栅格的尺寸为1″×1″；步骤2：调用圆栅格化算法，在栅格化后的全局坐标系xoy中标注圆形障碍区；步骤3：通过上级指挥系统获取无人机任务作业高度下的飞行起始点s和终止点e，单位均为角度秒，并连接直线段se，得到初始直线段航路se，假设无人机初始的机头指向与初始直线段航路se的方向相同；步骤4：调用直线段栅格化算法，将初始直线段航路se栅格化，栅格化坐标集合为line，line＝{(xi，yi)|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段：从尽可能靠近圆形禁飞区的w0点在圆形障碍区的劣弧一侧转弯至w1点后改为直飞；从尽可能靠近圆形禁飞区的w0点开始是因为直线段sw0是无人机可以安全飞行的，在这段航路中，航向角的初始值为0，到达w1点时的方向角为ψc，由航向角控制律的时域表达式以及使给定航向角近似等于实际航向角可得出ψ(t)≈ψc；第2段：从w1点直飞至w2点改为向圆形禁飞区的优弧一侧转弯；第3段：从w2点向圆形禁飞区的优弧一侧转弯至w3点后改为直飞；第4段，从w3点直飞至w5点后，从圆形禁飞区的优弧一侧转弯；第5段，从w5点向圆形禁飞区的优弧一侧转弯至w6点改为直飞；这时，无人机机头指向飞行结束点e，与无人机到达w0点之前的状态是相同的，在这段航路中，航向角的初始值为0，终值为ψe，由航向角控制律的时域表达式以及使给定航向角近似等于实际航向角可得出ψ(t)≈ψe；所述规划飞行航路的参数计算如下：a)局部坐标系参数初始化：在平面直角坐标系xoy中有一个圆形障碍区的圆，其圆心坐标为(xb，xb)，半径为rb，直线段se与圆相交，直线cd与直线段se垂直且与圆相切，且直线cd在距s点近的se与cd的交点为o，求交点o的坐标，已知点s(xs，ys)和点e(xe，ye)的坐标；直线段se的斜率k为：过圆心(xb，xb)，作平行于直线段se的直线ab，直线ab的方程为：y-yb＝k(x-xb)直线ab与圆的交点可通过解方程组获得：解方程组，可得交点如下：计算s点与两个交点的距离：选择与s点近的一个交点f，即交点f(xf，xf)：若d1＜d2，则xf＝x1，yf＝y1；否则xf＝x2，yf＝y2；过交点f(xf，xf)做垂直于直线段se的直线cd，直线cd的方程为：直线se与直线cd的交点o的坐标，可通过解方程组获得：解方程组，可得交点o在全局坐标系xoy中的坐标如下：起始点s的坐标转化：其中，(xo，yo)为局部坐标系xoy的原点o在全局坐标系xoy中的坐标，(xs，ys)为s点在xoy坐标系中的坐标，(xs，ys)为s点在xoy坐标系中的坐标，θ为x轴与x轴的夹角；终止点e的坐标转化：其中，(xe，ye)为s点在xoy坐标系中的坐标，(xe，ye)为s点在xoy坐标系中的坐标。其中，(xb，yb)为圆形障碍区的圆心在xoy坐标系中的坐标，(xb，yb)为圆形障碍区的圆心在xoy坐标系中的坐标；令：h＝rb-|yb|其中，rb为圆形障碍区的半径；可得w3和w4在xoy坐标系中的坐标如下：其中，为w3在xoy坐标系中的坐标；其中，为w4在xoy坐标系中的坐标；b)：计算航向指令角ψc其中，α＞0为航向常数，已知量；v为无人机速度，dmin为最小航路段长度，设定常值；ψc为航向指令角；c)：计算δx1和δy1，δx1＝|xw1-xw0|，δy1＝|yw1-yw0|，；d)：计算δx2和δy2(δx2＝|xw2-xw1|，δy2＝|yw2-yw1|：e)：计算δx3和δy3，δx3＝|xw3-xw2|，δy3＝|yw3-yw2|；f)：计算航向指令角ψe；g)：计算δx5和δy5，δx5＝|xw5-xw6|，δy5＝|yw5-yw6|：h)：计算获得在局部坐标系xoy中的避障航路点坐标；i)：通过旋转、平移变换，将位于局部坐标系xoy的航路点坐标转化为全局坐标系xoy中的坐标：步骤7：根据步骤6计算的全局坐标系xoy中的坐标，得到w0到w6六个转折点的坐标，形成避障航路点集合，避障航路规划结束；步骤8：初始直线段航路se与圆形障碍区不相交，直接从s点飞向e点，形成直飞航路点集合，航路规划结束。所述圆栅格化算法步骤如下：步骤1：设置八分之一圆弧段的坐标集合的初始位置：circle0＝{(r，0)}，即：(x0，y0)＝(r，0)；说明1：地形造成的威胁指在无人机的固定飞行高度上对飞行安全造成障碍的山峰，用圆锥体近似表示山峰；对于一个固定飞行高度，水平截面是一个圆形区域，边界是一个圆周；数字高程数据不是连续的，而是基于栅格的，栅格大小为1″×1″，由此获得的圆周也是栅格的，同时，为了将一个圆周存储在二维矩阵中，也需要将圆周栅格化；说明2：在xoy坐标系下有一个圆心为o(x0，y0)、半径为r的圆，建立圆的局部坐标系xoy，局部坐标系的原点为圆心，x轴平行x轴、方向相同，y轴平行y轴、方向相同；说明3：圆具有八分对称性，在xoy坐标系下将圆分成8个扇区，x＝0、y＝0、y＝x和y＝-x是其4条对称轴。若已知圆弧上扇区0的点：p(x，y)，由此得到关于4条对称轴的七个对称点：扇区1的对称点：(y，x)、扇区2的对称点：(y，-x)、扇区3的对称点：(-x，y)、扇区4的对称点：(-x，-y)、扇区5的对称点：(-y，-x)、扇区6的对称点：(-y，x)和扇区7的对称点：(x，-y)；因此，求出扇区0的圆弧坐标，就可利用对称性得到整个圆的坐标；说明4：先求出xoy坐标系下，扇区0的圆弧坐标，然后利用对称性得到xoy坐标系下整个圆的坐标集合；说明5：将得到的坐标做相应的平移，得到xoy坐标系下整个圆的坐标集合；步骤2：扇区0对应的圆心角为45°，利用弦长公式计算栅格化扇区0的步数n：n＝[2rsin(45/2)]其中，[]是取整运算符，例如，[2.8]＝2，取2.8的整数部分；步骤3：i从0开始对圆进行栅格化，则：(x0，y0)＝(r，0)；步骤4：计算(xi+1，yi+1)：步骤5：令i加1，若i≤n-1，则转步骤4；否则，转步骤6；步骤6：得到八分之一圆弧段坐标集合circle1，即：circle1＝{(x0，y0)，…，(xn-1，yn-1)}＝{(xi，yi)|i＝0，…，n-1}；步骤8：利用圆的八分对称性得到在xoy坐标系下的整个圆的坐标集合circle：circle＝{(x0，y0)，…，(x8n-1，y8n-1)}＝{(xi，yi)|i＝0，…，8n-1}步骤9：通过平移运算，得到在xoy坐标系下的整个圆的坐标集合circle：circle＝{(x0+xi，y0+yi)|i＝0，…，8n-1}。直线栅格化算法步骤如下：步骤1：s(xs，ys)为无人机的飞行起始点，e(xe，ye)为无人机的飞行终止点，为了搜索栅格化的圆形障碍区，直线段se也需栅格化，并记录在集合line＝{(xi，yi)|i＝0，1，2，…，n}中，其中(x0，y0)＝(xs，ys)为起始点坐标，(xn，yn)＝(xe，ye)为终止点坐标，根据直线段se的斜率k、(xs，ys)与(xe，ye)的相对位置关系，将直线段se所处的区域分为8个区域，以点s为圆心，直线段se为半径画一个圆，圆被平均分成8个扇区，直线段se处在这8个扇区中的其中一个扇区中；步骤2：计算直线段se的斜率k：其中，是一个符号函数，kmax是一个远大于1的实数；步骤3：若|k|≤1，执行步骤4；否则，执行步骤5；其中||是取绝对值运算符；步骤4：当|k|≤1，则依据以下步骤进行：步骤4.1：i从0开始对直线段se进行栅格化，令：x0＝xs，y0＝ys；dx＝xe-xs，dy＝ye-ys；d0＝2*dy-dx；其中dx是x轴位移量，dy是y轴位移量，di是误差；步骤4.2:计算(xi+1，yi+1)及修正误差di+1；步骤4.3：将计算的(xi+1，yi+1)写入line集合的尾部；对i进行递增，若xi＜xe，重复执行步骤4.1至4.3，直到xi＝xe，形成直线段se的集合line＝{(xi，yi)|i＝0，1，2，…，n}；步骤5：当|k|＞1，则依据以下步骤进行：步骤5.1：i从0开始对直线段se进行栅格化，令：x0＝xs，y0＝ys；dx＝xe-xs，dy＝ye-ys；d0＝2*dx-dy；其中dx是x轴位移量，dy是y轴位移量，di是误差；步骤5.2：计算(xi+1，yi+1)及修正误差di+1：步骤5.3：将计算的(xi+1，yi+1)写入line集合的尾部，对i进行递增，重复执行步骤5.1至5.3，直到xi＝xe，形成直线段se的集合line＝{(xi，yi)|i＝0，1，2，…，n}。本发明的有益效果在于本发明所规划出的航路是实际可飞的：采用描述飞机横侧向控制规律的航向控制律作为航路规划的数学模型，完全反映了飞行过程中飞机在横侧向的实际飞行规律或飞行轨迹；飞行环境建模采用混合模型：采用一般平面直角坐标系和栅格混合模型对飞行范围内的环境进行建模，平面直角坐标系适合解析几何的计算，对计算航路点有益；而采用二维矩阵的栅格模型适合圆形障碍区的栅格化和直线段航路的栅格化，对检测航路上是否有障碍区有益。附图说明图1是本发明圆的八分对称性示意图。图2是本发明直线段se与圆形障碍区(xb，yb；rb)相交示意图。图3是本发明直线段se的8种情形示意图。图4是本发明无人机绕过圆形障碍区的航路规划示意图。图5是本发明割线与切线交点示意图。图6是本发明基于航向控制律的无人机避障航路规划方法实现流程图。具体实施方式下面结合附图和实施例对本发明进一步说明。本发明涉及三个算法，圆栅格化算法、直线栅格化算法和基于航向控制律的避障航路规划算法。其中，圆栅格化算法和直线栅格化算法是基于航向控制律的避障航路规划算法的基础，通过圆栅格化算法和直线栅格化算法判断初始直线段航路(从初始点至目标点的直线段航路)是否与圆形障碍物相交，若相交则实施本发明的基于航向控制律的避障航路规划算法，若不相交则使用初始直线段航路。基于航向控制律的避障航路规划算法步骤如下：步骤1：根据无人机飞行范围，建立全局的平面直角坐标系xoy，根据飞行起始点位置和飞行范围，建立覆盖飞行范围的全局坐标系xoy，飞行范围用一个矩形区域来表示，以矩形区域的左下角为原点o，以正东方向为x轴正向，以正北方向为y轴正向，建立平面直角坐标系xoy；坐标系的单位采用角度秒；在东北半球，沿正东方向，经度逐渐增加；沿正北方向，纬度逐渐增加；并将飞行范围栅格化，每个栅格的尺寸为1″×1″(1角度秒×1角度秒)；步骤2：调用圆栅格化算法，在栅格化后的全局坐标系xoy中标注圆形障碍区；步骤3：通过上级指挥系统获取无人机任务作业高度下的飞行起始点s和终止点e，单位均为角度秒，并连接直线段se，得到初始直线段航路se，假设无人机初始的机头指向与初始直线段航路se的方向相同；步骤4：调用直线段栅格化算法，将初始直线段航路se栅格化，栅格化坐标集合为line，line＝{(xi，yi)|i＝1，2，…，n}；步骤5：i从1开始取集合line中的元素，判断每一个元素是否与圆形障碍区相交，如果初始直线段航路se与圆形障碍区不相交，转步骤8；否则，转步骤6，进行避障航路规划步骤；步骤6：为了确保飞行安全，无人机根据自身的机动性能避开圆形障碍区，采取的策略包括：策略1：在距离禁飞区或障碍物区尽可能近的点开始转弯机动性飞行；策略2：从禁飞区或障碍物区的外圆弧的劣弧一侧进行转弯机动性飞行；如图4所示，为了规划避障航路，建立局部直角坐标系xoy，以初始直线段航路se作为局部直角坐标系xoy的x轴，方向与初始直线段航路se的方向相同；在近s点一端做圆形障碍区的切线与x轴的交点作为局部直角坐标系xoy的原点o；切线作为局部直角坐标系xoy的y轴，构成右手坐标系xoy；作圆的外切正方形，正方形的边平行与x轴或垂直于x轴，h是劣弧顶点距x轴的距离；无人机绕过圆形障碍区的5段规划飞行航路为：第1段：从尽可能靠近圆形禁飞区的w0点在圆形障碍区的劣弧一侧转弯至w1点后改为直飞；从尽可能靠近圆形禁飞区的w0点开始是因为直线段sw0是无人机可以安全飞行的，在这段航路中，航向角的初始值为0，到达w1点时的方向角为ψc，由航向角控制律的时域表达式以及使给定航向角近似等于实际航向角可得出ψ(t)≈ψc；第2段：从w1点直飞至w2点改为向圆形禁飞区的优弧一侧转弯；第3段：从w2点向圆形禁飞区的优弧一侧转弯至w3点后改为直飞；第4段，从w3点直飞至w5点后，从圆形禁飞区的优弧一侧转弯；第5段，从w5点向圆形禁飞区的优弧一侧转弯至w6点改为直飞；这时，无人机机头指向飞行结束点e，与无人机到达w0点之前的状态是相同的。在这段航路中，航向角的初始值为0，终值为ψe，由航向角控制律的时域表达式以及使给定航向角近似等于实际航向角可得出ψ(t)≈ψe。所述规划飞行航路的参数计算如下：a)局部坐标系参数初始化：如图5所示，在平面直角坐标系xoy中有一个圆形障碍区的圆，其圆心坐标为(xb，xb)，半径为rb，直线段se与圆相交，直线cd与直线段se垂直且与圆相切，且直线cd在距s点近的se与cd的交点为o，求交点o的坐标，已知点s(xs，ys)和点e(xe，ye)的坐标；直线段se的斜率k为：过圆心(xb，xb)，作平行于直线段se的直线ab，直线ab的方程为：y-yb＝k(x-xb)直线ab与圆的交点可通过解方程组获得：解方程组，可得交点如下：计算s点与两个交点的距离：选择与s点近的一个交点f，即图5中的交点f(xf，xf)：若d1＜d2，则xf＝x1，yf＝y1；否则xf＝x2，yf＝y2；过交点f(xf，xf)做垂直于直线段se的直线cd，直线cd的方程为：直线se与直线cd的交点o的坐标，可通过解方程组获得：解方程组，可得交点o在全局坐标系xoy中的坐标如下：起始点s的坐标转化：其中，(xo，yo)为局部坐标系xoy的原点o在全局坐标系xoy中的坐标，(xs，ys)为s点在xoy坐标系中的坐标，(xs，ys)为s点在xoy坐标系中的坐标，θ为x轴与x轴的夹角；终止点e的坐标转化：其中，(xe，ye)为s点在xoy坐标系中的坐标，(xe，ye)为s点在xoy坐标系中的坐标。其中，(xb，yb)为圆形障碍区的圆心在xoy坐标系中的坐标，(xb，yb)为圆形障碍区的圆心在xoy坐标系中的坐标；令：h＝rb-|yb|其中，rb为圆形障碍区的半径；可得w3和w4在xoy坐标系中的坐标如下：其中，为w3在xoy坐标系中的坐标；其中，为w4在xoy坐标系中的坐标；b)：计算航向指令角ψc其中，α＞0为航向常数，已知量；v为无人机速度，dmin为最小航路段长度，设定常值；ψc为航向指令角；c)：计算δx1和δy1，δx1＝|xw1-xw0|，δy1＝|yw1-yw0|，；d)：计算δx2和δy2(δx2＝|xw2-xw1|，δy2＝|yw2-yw1|：e)：计算δx3和δy3，δx3＝|xw3-xw2|，δy3＝|yw3-yw2|；f)：计算航向指令角ψe；g)：计算δx5和δy5，δx5＝|xw5-xw6|，δy5＝|yw5-yw6|：h)：计算获得在局部坐标系xoy中的避障航路点坐标；i)：通过旋转、平移变换，将位于局部坐标系xoy的航路点坐标转化为全局坐标系xoy中的坐标：步骤7：根据步骤6计算的全局坐标系xoy中的坐标，得到w0到w6六个转折点的坐标，形成避障航路点集合，避障航路规划结束；步骤8：初始直线段航路se与圆形障碍区不相交，直接从s点飞向e点，形成直飞航路点集合，航路规划结束。所述圆栅格化算法步骤如下：步骤1：设置八分之一圆弧段的坐标集合的初始位置：circle0＝{(r，0)}，即：(x0，y0)＝(r，0)；说明1：地形造成的威胁指在无人机的固定飞行高度上对飞行安全造成障碍的山峰，用圆锥体近似表示山峰；对于一个固定飞行高度，水平截面是一个圆形区域，边界是一个圆周；数字高程数据不是连续的，而是基于栅格的，栅格大小为1″×1″，由此获得的圆周也是栅格的，同时，为了将一个圆周存储在二维矩阵中，也需要将圆周栅格化；说明2：如图1所示，在xoy坐标系下有一个圆心为o(x0，y0)、半径为r的圆，建立圆的局部坐标系xoy，局部坐标系的原点为圆心，x轴平行x轴、方向相同，y轴平行y轴、方向相同；说明3：圆具有八分对称性，在xoy坐标系下将圆分成8个扇区，x＝0、y＝0、y＝x和y＝-x是其4条对称轴。若已知圆弧上扇区0的点：p(x，y)，由此得到关于4条对称轴的七个对称点：扇区1的对称点：(y，x)、扇区2的对称点：(y，-x)、扇区3的对称点：(-x，y)、扇区4的对称点：(-x，-y)、扇区5的对称点：(-y，-x)、扇区6的对称点：(-y，x)和扇区7的对称点：(x，-y)；因此，求出扇区0的圆弧坐标，就可利用对称性得到整个圆的坐标；说明4：先求出xoy坐标系下，扇区0的圆弧坐标，然后利用对称性得到xoy坐标系下整个圆的坐标集合；说明5：将得到的坐标做相应的平移，得到xoy坐标系下整个圆的坐标集合；步骤2：扇区0对应的圆心角为45°，利用弦长公式计算栅格化扇区0的步数n：n＝[2rsin(45/2)]其中，[]是取整运算符，例如，[2.8]＝2，取2.8的整数部分；步骤3：i从0开始对圆进行栅格化，则：(x0，y0)＝(r，0)；步骤4：计算(xi+1，yi+1)：步骤5：令i加1，若i≤n-1，则转步骤4；否则，转步骤6；步骤6：得到八分之一圆弧段坐标集合circle1，即：circle1＝{(x0，y0)，…，(xn-1，yn-1)}＝{(xi，yi)|i＝0，…，n-1}；步骤8：利用圆的八分对称性得到在xoy坐标系下的整个圆的坐标集合circle：circle＝{(x0，y0)，…，(x8n-1，y8n-1)}＝{(xi，yi)|i＝0，…，8n-1}步骤9：通过平移运算，得到在xoy坐标系下的整个圆的坐标集合circle：circle＝{(x0+xi，y0+yi)|i＝0，…，8n-1}。直线栅格化算法步骤如下：步骤1：如图2所示，s(xs，ys)为无人机的飞行起始点，e(xe，ye)为无人机的飞行终止点，为了搜索栅格化的圆形障碍区，直线段se也需栅格化，并记录在集合line＝{(xi，yi)|i＝0，1，2，…，n}中，其中(x0，y0)＝(xs，ys)为起始点坐标，(xn，yn)＝(xe，ye)为终止点坐标，根据直线段se的斜率k、(xs，ys)与(xe，ye)的相对位置关系，将直线段se所处的区域分为8个区域，以点s为圆心，直线段se为半径画一个圆，圆被平均分成8个扇区，直线段se处在这8个扇区中的其中一个扇区中，如图3所示；步骤2：计算直线段se的斜率k：其中，是一个符号函数，kmax是一个远大于1的实数；步骤3：若|k|≤1，执行步骤4；否则，执行步骤5；其中||是取绝对值运算符；步骤4：当|k|≤1，则依据以下步骤进行：步骤4.1：i从0开始对直线段se进行栅格化，令：x0＝xs，y0＝ys；dx＝xe-xs，dy＝ye-ys；d0＝2*dy-dx；其中dx是x轴位移量，dy是y轴位移量，di是误差；步骤4.2:计算(xi+1，yi+1)及修正误差di+1；步骤4.3：将计算的(xi+1，yi+1)写入line集合的尾部；对i进行递增，若xi＜xe，重复执行步骤4.1至4.3，直到xi＝xe，形成直线段se的集合line＝{(xi，yi)|i＝0，1，2，…，n}；步骤5：当|k|＞1，则依据以下步骤进行：步骤5.1：i从0开始对直线段se进行栅格化，令：x0＝xs，y0＝ys；dx＝xe-xs，dy＝ye-ys；d0＝2*dx-dy；其中dx是x轴位移量，dy是y轴位移量，di是误差；步骤5.2：计算(xi+1，yi+1)及修正误差di+1：步骤5.3：将计算的(xi+1，yi+1)写入line集合的尾部，对i进行递增，重复执行步骤5.1至5.3，直到xi＝xe，形成直线段se的集合line＝{(xi，yi)|i＝0，1，2，…，n}。本发明实施例的步骤如下：步骤1：建立飞行范围的全局坐标系xoy并将其栅格化。根据无人机飞行范围，建立平面直角坐标系xoy；并将飞行范围栅格化，每个栅格的尺寸为1″×1″(1角度秒×1角度秒)说明1：根据飞行起始点位置和飞行范围，建立覆盖飞行范围的全局坐标系xoy；不失一般性，这个飞行范围可用一个矩形区域来表示，以矩形区域的左下角为原点o，以正东方向为x轴正向，以正北方向为y轴正向，建立平面直角坐标系xoy；坐标系的单位采用角度秒；在东北半球，沿正东方向，经度逐渐增加；沿正北方向，纬度逐渐增加；步骤2：调用圆栅格化算法，在栅格化后的全局坐标系xoy中标注圆形障碍区。步骤3：通过上级指挥系统获取无人机任务作业高度下的全局坐标系xoy中的飞行起始点s和终止点e，单位均为角度秒，并连接直线段se，得到初始直线段航路se。假设无人机初始的机头指向与初始直线段航路se的方向相同。步骤4：调用直线段栅格化算法，将初始直线段航路se栅格化，栅格化坐标集合line，line＝{(xi，yi)|i＝1，2，…，n}。步骤5：i从1开始取集合line中的元素，判断是否与圆形障碍区相交。如果初始直线段航路se与圆形障碍区不相交，转步骤8；否则，转步骤6，进行避障航路规划步骤。步骤6：为了确保飞行安全，无人机根据自身的机动性能避开圆形障碍区，采取的策略包括：策略1：在距离禁飞区或障碍物区尽可能近的点开始转弯机动性飞行；策略2：从禁飞区或障碍物区的外圆弧的劣弧一侧进行转弯机动性飞行。如图4所示，为了规划这个避障航路，建立局部直角坐标系xoy，以初始直线段航路se作为局部直角坐标系xoy的x轴，方向与初始直线段航路se的方向相同；在近s点一端做圆的切线与x轴的交点作为局部直角坐标系xoy的原点o；切线作为局部直角坐标系xoy的y轴，构成右手坐标系xoy；作圆的外切正方形，正方形的边平行与x轴或垂直于x轴，h是劣弧顶点距x轴的距离。由设计策略设计的无人机绕过圆形障碍区的5段可飞行航路为：第1段：从尽可能靠近圆形禁飞区的w0点在圆形障碍区的劣弧一侧转弯至w1点改为直飞。航向角的初始值为o，终值为ψc，由航向角控制律的时域表达式ψ(t)＝ψc(1-e-αt)以及给定航向角近似等于实际航向角可知αt＝5，有ψ(t)≈0.9933ψc由此可得出ψ(t)≈ψc。第2段：从w1点直飞至w2点改为向圆形禁飞区的优弧一侧转弯。第3段：从w2点向圆形禁飞区的优弧一侧转弯至w3点改为直飞。第4段，从w3点直飞至w5点改为向圆形禁飞区的优弧一侧转弯。第5段，从w5点向圆形禁飞区的优弧一侧转弯至w6点改为直飞。所述规划飞行航路的参数计算如下：a)局部坐标系参数初始化:如图5所示，在平面直角坐标系xoy中有一个圆，其圆心坐标为(xb，xb)，半径为rb。直线段se与圆相交，作与直线段se垂直、与圆相切、且距s点近的直线cd，se与cd的交点为o，求交点o的坐标。已知点s(xs，ys)和点e(xe，ye)的坐标。直线段se的斜率k为：过圆心(xb，xb)，作平行于直线段se的直线ab，直线ab的方程为：y-yb＝k(x-xb)直线ab与圆的交点可通过解方程组获得：解方程组，可得交点如下：计算s点与两个交点的距离：选择与s点近的那一个交点，即图5中的交点f(xf，xf)：若d1＜d2，则xf＝x1，yf＝y1；否则xf＝x2，yf＝y2。过交点f(xf，xf)做垂直于直线段se的直线cd，直线cd的方程为：直线se与直线cd的交点o的坐标，可通过解方程组获得：解方程组，可得交点o(即：局部坐标系xoy的原点o)在全局坐标系xoy中的坐标如下：起始点s的坐标转化：其中，(xo，yo)为局部坐标系xoy的原点o在全局坐标系xoy中的坐标，(xs，ys)为s点在xoy坐标系中的坐标，(xs，ys)为s点在xoy坐标系中的坐标，θ为x轴与x轴的夹角，如图6所示；终止点e的坐标转化：其中，(xe，ye)为s点在xoy坐标系中的坐标，(xe，ye)为s点在xoy坐标系中的坐标。其中，(xb，yb)为圆形障碍区的圆心在xoy坐标系中的坐标，(xb，yb)为圆形障碍区的圆心在xoy坐标系中的坐标；令：h＝rb-|yb|，其中，rb为圆形障碍区的半径；可得w3和w4在xoy坐标系中的坐标如下：其中，为w3在xoy坐标系中的坐标；其中，为w4在xoy坐标系中的坐标；b)：计算航向指令角ψc；其中，α＞0为航向常数，已知量；v为无人机速度，设定常值；dmin为最小航路段长度，设定常值；ψc为航向指令角；c)：计算δx1和δy1；d)：计算δx2和δy2；e)：计算δx3和δy3；f)：计算航向指令角ψe；g)：计算δx5和δy5；h)：计算获得在局部坐标系xoy中的避障航路点坐标；i)：通过旋转、平移变换，将位于局部坐标系xoy的航路点坐标转化为全局坐标系xoy中的坐标：步骤7：形成避障航路点集合表如表1所示，避障航路规划结束。表1避障航路点集合表航路点序号航路点位置飞行指令航向指令角飞行速度1(xw0，yw0)左转弯ψc|v|2(xw1，yw1)直飞0|v|3(xw2，yw2)右转弯ψc|v|4(xw3，yw3)直飞0|v|5(xw5，yw5)右转弯ψe|v|6(xw6，yw6)直飞0|v|说明7：初始状态为无人机由s点飞向e点，且在s点时，无人机机头指向e点；如果在飞行过程中，遇有不可逾越的障碍区时，设计避障航路，该航路有6个航路点：①在航路点1的位置左转弯；②在航路点2的位置停止转弯，改为直飞，飞行航程不小于最小航路段长度；③在航路点3的位置右转弯；④在航路点4的位置停止转弯，改为直飞；⑤在航路点3的位置右转弯；⑥在航路点6的位置停止转弯，改为直飞，这时，无人机由w6飞向e点，且此时，无人机机头指向e点，与初始状态相同。步骤8：初始直线段航路se与圆形障碍区不相交，直接从s点飞向e点，形成直飞航路点集合表如表2所示：表2直飞航路点集合表航路点序号航路点位置飞行指令航向指令角飞行速度1(xs，ys)直飞0|v|航路规划结束。当前第1页12