[发明专利]一种面向无人机的航迹规划方法

权利要求书

1.一种面向无人机的航迹规划方法，其特征在于：包括以下步骤：

步骤1、搭建系统环境；

在长宽分别为L,W的矩形区域R内有若干处雷达监测点和威胁区域，所以该区域可以分为可拓展区域R_free，和法扩展区域R_obs，R_free与R_obs都是R的子集，且满足R＝R_free∪R_obs与起始位置q_init∈R_free和目标位置q_goal∈R_free；

步骤2、在确定下一跳节点时，利用目标启发式的思想生成多个候选点q_rand，在所有的候选点中选择合适的点作为下一跳点q_near；具体过程如下：

步骤2-1、确定概率阈值p_gc，0＜p_gc＜1，在每迭代m次后产生的随机数；

步骤2-2、在(0，1)之间生成一个随机数，将值设置为概率p_g；

步骤2-3、比较p_g和p_gc；如果p_g＞p_gc，则转到步骤2-4；如果不是，转到步骤2-6；

步骤2-4、在树上找到最近的q_goal＝q_rand的节点，并将其定义为q_near；根据步长，生成未确定的节点q_unrand；

步骤2-5、如果q_unrand位于障碍威胁区域中则返回步骤2-2；如果不在，则将q_unrand添加到搜索树并转到步骤2-2；

步骤2-6、在规划区域中，随机生成m个节点，并用表示；比较最近的点并用表示；

步骤2-7、对m个确定点最小值q_min；

步骤2-8、如果P_min在威胁区域内返回步骤2-2，否则增加P_min到树中并返回步骤2-2；

步骤3、把起始点q_init和目标点q_goal分别当作对方的终点进行双向扩展，直到它们连接起来终止扩展；

步骤4、交换两颗树的搜索路径从而得到起始点与目标点的路径规划；

步骤5、检查路由表，删除路由表中的冗余路有点；具体步骤如下：

步骤5-1、为简化路由航迹建立初始清单Γ点；最初Γ是空的；

步骤5-2、增加目标点p_n到Γ，让j＝n，i＝1；

步骤5-3、如果存在威胁障碍点在p_j与p_i中返回第四步，否则，执行5-5；

步骤5-4、如果i＝j-1，则j＝j-1，i＝1返回5-3；否则i＝i+1，返回5-3；

步骤5-5、增加p_i到Γ并且让j＝i，i＝1；如果j＝1，则删除该点，否则返回5-3；

步骤6、对路径进行满足曲率约束的平滑处理；

假设无人机执行任务的飞行速度为v_t，无人机约束最大转向角速率为ω，此时对应的最小转向半径为R，二维平面下则无人机相对位置(x,y)和航向θ的运动方程简化为以下方程：

x(t)＝vcosθ(t)                                (1)

x(t)＝vsinθ(t)                                (2)

θ(t)＝Ω Ω∈[-ω,ω]                             (3)

R＝v/ω                                  (4)

无人机进行航迹交接处对接看作在二维平面内具有特定方向向量的两点，无人机飞行进入弯道约束区域切点称之为起始点，无人机转向结束离开自适应过度进入下一航段的交接点称之为结束点，交接自适应过度轨迹为Dubins曲线轨迹，该轨迹曲线曲率满足大于1/R；通过以下3个步骤实现平滑，具体步骤如下：

步骤6-1：生成转弯圆弧，圆心为o；

根据进入航段ab建立直角坐标系，其中航迹点a、b、c的坐标分别为(x_a,y_a)，(x_b,y_b)，(x_c,y_c)，航段进入点d的坐标为(x_d,y_d)，航段离开点坐标为(x_n,y_n)；

无人机最小转弯约束半径为R，无人机宽度为D；

根据无人机的约束条件通过几何知识求出转弯Dubins曲线圆弧的圆心o位于锐角的角平分线上，同时求得圆心o的坐标(x_o,y_o)为：

步骤6-2：生成切线轨迹；

从航迹进入点d向转弯轨迹做一条切线，切点为e，得到切线长de；由已知条件及几何知识可知圆弧圆心o点与e点距离为半径R，设圆心o点与d点距离为l_od，则点e与点d点距离为l_ed，切线航迹e点坐标(x_e,y_e)为：

步骤6-3：生成对准轨迹圆心；

对准轨迹圆也是满足转弯约束条件生成的Dubins曲线轨迹圆对准轨迹圆心记为m，转弯轨迹圆心o与b点距离为l_ob，转弯轨迹圆心o点与对准轨迹圆心m点距离为2R，通过求解得到圆心m坐标(x_m,y_m)为：

由上面三个步骤求解出满足约束条件的基于Dubins曲线的锐角自适应过度轨迹；

步骤7、输出一条从起始点到目标点的平滑路径。