基于CRS-RRT算法的无人机航迹规划_刘靠.pdf

引用格式:刘靠，蒋海峰，董磊，等 基于 CS-T 算法的无人机航迹规划J 电光与控制，2023，30(7):35-39 LIU K,JIANG H F,DONG L,etal UAV path planning based on CS-T algorithm J Electronics Optics Control,2023,30(7):35-39基于 CS-T 算法的无人机航迹规划刘靠，蒋海峰，董磊，任学文(南京理工大学，南京210000)摘要:针对快速搜索随机树(T)算法在航迹规划过程中存在采样点扩展随机性强、航迹曲折不平滑等问题，提出了一种基于约束随机采样点的 T(Constrained andom Sampling-based T，CS-T)算法。该算法引入人工势场法中的引力场势能函数约束随机采样点在目标点附近采样，引导随机树朝着目标点生长，提高算法的规划速度，并结合去除冗余节点策略和 Minimum Snap 航迹平滑方法，在复杂三维环境中可快速生成一条安全、平滑且满足无人机动力学约束的航迹。仿真结果表明，该算法有效提高航迹规划速度并缩短航迹长度。关键词:无人机;航迹规划;快速扩展随机树算法;约束采样点;动力学约束中图分类号:V279文献标志码:Adoi:10 3969/j issn 1671 637X 2023 07 006UAV Path Planning Based on CS-T AlgorithmLIU Kao,JIANG Haifeng,DONG Lei,EN Xuewen(Nanjing University of Science and Technology,Nanjing 210000,China)Abstract:Aiming at the problems of strong randomness of sampling point expansion and unsmooth path inthe path planning process of T(apid-exploration andom Tree)algorithm,a constrained randomsampling based T algorithm-CS-T(Constrained andom Sampling-based T)is proposed In thismethod,the potential energy function of gravitational field in artificial potential field method is introduced toconstrain random sampling points to sample near the target point,so as to guide the random tree to growtowards the target point and improve the planning speed of the algorithm Combined with the strategy ofremoving redundant nodes and Minimum Snap track smoothing method,a safe and smooth track that meetsthe dynamic constraints of UAV can be quickly generated in a complex three-dimensional environment Thesimulation results show that the algorithm can effectively improve the speed of trajectory planning andshorten the trajectory lengthKey words:UAV;path planning;rapid-exploration random tree algorithm;constraint sampling point;dynamic constraint0引言无人机的航迹规划问题就是根据先验信息和传感器信息在空间中规划出一条从起始点到目标点的安全、平滑的可飞行路径。目前，常用的路径规划算法主要包括数学优化算法、人工势场法、基于栅格搜索的算法以及基于采样的算法等。数学优化算法主要有非线性优化方法1、混合整数线性规划方法2 等，然而当约束条件较为复杂时，计算量会大大增加，因此只能适用于低维路径规划;人工势场法虽然收敛速度较快且生收稿日期:2022-05-25修回日期:2022-07-02作者简介:刘靠(1996)，男，江苏淮安人，硕士生。通讯作者:蒋海峰(1978)，男，江苏扬州人，博士，副教授，硕导。成的路径较为平滑，但是存在易陷入局部最优、目标不可达等问题 3;基于栅格搜索的算法主要有 A*算法 4 5、蚁群算法6 7 等，虽然能够取得良好的效果，但扩展到高维空间后相应的计算时间以及内存空间将呈指数级增长。基于采样的算法主要有概率路线图算法(PM)8 和快速搜索随机树算法(T)9。相较于 PM，T 算法由于其以增量方式构建随机树、随机采样等特点，无需对任务空间进行几何划分，因此在高维路径规划中得到了广泛的应用，并且在过去的10 年中，产生大量的 T变体 10 12。KUFFNE 等13 在 T 算法的基础上提出了 T-Connect 算法，将单向随机树改进为双向随机树，提高了算法的搜索效率，但航迹平滑度仍然较差;KAAMAN 等14 通过对扩展节点的相邻节点进行搜第 30 卷第 7 期2023 年 7 月电光与控制Electronics Optics ControlVol 30No 7July 2023索以选择低代价的父节点以及重新布线过程为目的而提出了 T*算法，保证了算法的渐近最优性，但降低了算法的收敛速度，无法满足无人机在复杂环境下的实时规划要求;WANG 等15 根据障碍物的位置自适应改变随机树的步长并通过保存障碍物周围的采样点作为指标引导随机树向未探索空间生长，提高了算法的规划速度，但只将无人机作为一个质点并没有考虑到无人机的动力学约束。综上所述，T 算法在高维路径规划中存在采样点扩展随机性强、航迹曲折不平滑等问题。针对这些问题，考虑引入人工势场法中的“引力”思想让随机树有目的地朝着目标点生长，提高算法的规划速度，并且采用去除冗余节点策略以及 Minimum Snap 方法对航迹进行平滑处理，满足无人机的动力学约束。1问题描述1 1飞行环境建模无人机飞行环境中常常存在各种类型障碍物，这些障碍物大多数情况下都是不规则的，这将导致算法在对障碍物进行碰撞检测时大大增加计算量，降低算法的搜索效率，因此需要对障碍物进行简化建模。常用的障碍物建模方法一般包括轴向包围盒 AABB、包围球等16，因包围球方法具有建模简单且容易进行碰撞检测等优点，基于上述考虑采取包围球方法对障碍物进行建模。一般选取障碍物上距离最远两点之间的连线作为直径，可表示为=(x，y，z)(x a)2+(y b)2+(z c)2r2(1)式中:(a，b，c)为包围球在三维环境中的中心坐标;(x，y，z)为障碍物表面任意一点的坐标。障碍物建模过程如图 1 所示，其中，灰色部分表示障碍物，1为包围球的半径，算法在规划过程中将无人机当成质点处理，所以为保障无人机飞行安全，需要对包围球表面进行膨胀处理，膨胀后的包围球半径为 2。图 1障碍物建模示意图Fig 1Schematic diagram of obstacle modeling1 2无人机性能约束无人机在飞行过程中除了受到飞行环境约束外，还需要满足无人机自身的运动学和动力学约束。因此在进行航迹规划时需充分考虑各类约束条件17，主要包括以下 3 种约束。1 2 1最大航程约束无人机在整个飞行过程中，航程受到无人机燃油和时间配给的限制，因此存在最大航程，记为 Lmax，设共有 m 个航迹节点，其中，第 i 段航迹航程为 li，则该条航迹总航程 L 必须满足L=m1i=1liL Lmax。(2)1 2 2最小航迹段约束为确保无人机在飞行姿态调整时的安全性，需要一定的直飞距离，最短直飞航路段用 Lmin表示，则最小航路段约束表示为LiLmin。(3)1 2 3最大偏航角/俯仰角约束由于自身飞行性能约束，在额定速度下，无人机只能在最大偏航角的范围内进行转弯，最大俯仰角约束与之类似。对于航迹段 i，其在 xoy 平面的投影为 i=(xi xi 1，yi yi 1)，纵向高度差 zi zi 1，设无人机的最大偏航角和最大俯仰角分别为，如图 2 所示。图 2偏航角和俯仰角示意图Fig2Schematic diagram of yaw angle and pitch angle最大偏航角和最大俯仰角约束分别表示为Tii+1i i+1cos(4)zi zi 1(xi xi 1)2+(yi yi 1)2tan 。(5)2CS-T 算法T 算法在选择采样点时具有较强的随机性，导致随机树盲目扩展，降低算法的规划速度。这一问题可以通过约束随机采样点的方式解决，即将人工势场法中的引力场势能函数融入 T 算法中，使得随机采样点在目标点附近采样，从而引导随机树朝着目标点生长，改善算法盲目扩展节点问题，提高算法的规划速度。约束随机采样点的主要思想是引入人工势场法中的引力场势能函数，其表现形式为18 Uatt(X)=12katt2(Xrand，Xgoal)(6)63第 30 卷电光与控制刘靠等:基于 CS-T 算法的无人机航迹规划式中:katt为引力势场系数;Xrand为随机采样点坐标;Xgoal为无人机目标点坐标;(Xrand，Xgoal)=Xgoal Xrand表示随机采样点与目标点之间的距离。由式(6)可得到目标点和随机采样点之间的引力为Fatt=kattXgoal Xrand。(7)由式(7)中目标点和随机采样点之间的引力公式可以构造新的 Xnrand为Xnrand=Xrand+kattXgoal XrandXgoal Xrand。(8)由于引入了人工势场法中的引力场势能函数，使得算法偏向贪婪可能出现无法找到可行路径的问题。对此，在算法中设置了安全阈值 dsafe，主要思想是在进行上述操作之前首先计算随机采样点和所有障碍物之间距离的最小值 dmin，若 dmindsafe，则采用式(8)构造新的随机采样点引导随机树朝着目标点生长，否则Xnrand可以在任务空间中随机采样。3航迹优化由于 T 算法具有随机采样特性，导致在航迹规划过程中扩展了很多冗余节点，形成的航迹往往比较曲折且不平滑，因此通过去除冗余节点策略筛除航迹中的不必要节点，并利用 Minimum Snap 对航迹进行平滑处理，使之满足无人机的动力学约束。3 1去除冗余节点由于 T 算法规划出来的航迹往往比较曲折，为了去除航迹中不必要的节点，设计了一种去除冗余节点策略，如图 3 所示，其原理是首先连接起始节点 S 和p1，然后判断连线是否和障碍物发生碰撞，若没有则删除节点 p1，接着依次对Sp?2，Sp?3进行碰撞检测，直到Sp?4经判断和障碍物发生碰撞，此时保留 p4的上一个节点p3。按照上述方法从起始节点逐点向前递推直到目标点，可以有效筛除航迹中的冗余节点。图 3去除冗余节点示意图Fig3Schematic diagram of removing redundant nodes3 2Minimum Snap 航迹平滑在去除冗余节点之后，无人机的航迹平滑度得到改善，但想要获得符合无人机动力学约束的安全、平滑、可飞行航迹，需要对航迹进行进一步处理。为此，可采用 Minimum Snap 对航迹进行平滑19。对于无人机航迹段 i 一般可由 n 阶多项式描述，即x(t)=p0+p1t+p2t2+pntn(9)则 M 段航迹 f(t)可表示为f1(t)p1，0+p1