基于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，1t+p1，ntn=ni=0p1，itiT0 t T1f2(t)p2，0+p2，1t+p2，ntn=ni=0p2，itiT1 t T2fM(t)pM，0+pM，1t+pM，ntn=ni=0pM，itiTM1 t TM。(10)希望无人机航迹能够满足一系列的约束条件，比如设定起点和终点的位置、速度和加速度，每个航迹段的连接点处平滑等，但满足约束条件的可飞行航迹有很多，因此需要构建一个目标函数，在可飞行航迹中找出最优的航迹，目标函数及相应的等式约束如下min(f(4)(t)2(11)s tf(k)0(Tj 1)=x(k)0，jf(k)M(Tj)=x(k)T，jf(k)j(Tj)=f(k)j+1(Tj)。进一步地，可表示为minp1p MQ1QMp1p M(12)s tAeq(p1 pM)T=deq式中:Q1是一段轨迹的参数矩阵;Aeq和 deq是满足约束条件的经过简化之后的表示。至此，无人机航迹平滑问题转化为求解带约束的二次规划问题，并且可通过求解多项式的系数以及离散化时间 t 确定优化后的航迹。起始点、目标点以及中间点处的 p 由航迹可知，设定无人机在起始点 S、目标点 G 处的位置、速度和加速度均为零，航迹平滑效果如图 4 所示。图 4Minimum Snap 航迹平滑图Fig 4Path smoothing chart of Minimum Snap73第 7 期4仿真分析为验证改进算法在复杂三维环境下的航迹规划性能，仿真环境区域设置为 100 m 100 m 100 m，无人机起始坐标(10 m，10 m，10 m)，目标点坐标(96 m，80m，78 m)。节点扩展步长 D=5 m，目标引力系数 katt=13，安全阈值 dsafe=0 6 m。为验证 CS-T 算法在约束随机采样点方面的有效性，试验 对比了 T 算法和 CS-T 算法在航迹规划过程中的随机采样点扩展及在任务空间中的分布情况，结果如图5 所示，其中，红点表示随机采样点。图 5试验仿真对比结果(俯视图)Fig 5Simulation comparison results ofTest I(top view)从图5(a)可看到，由于 T 算法本身存在较强的节点扩展随机性，导致在任务空间中采样了很多无用节点，占用大量内存，降低算法的规划效率;而图5(b)在通过引入引力场势能函数对随机采样点进行约束之后可看出任务空间的随机采样点得到有效减少。试验为 T 算法、文献 12 中的 IB-T 算法和CS-T 算法在复杂三维环境中进行仿真比较，考虑到T 算法本身的随机性，在经过 50 次仿真后对规划时间、航迹长度以及采样点数取均值进行统计。3 种算法性能对比如表1 所示，图6 为其中一次三维仿真结果。表1算法性能对比数据Table 1Comparison data of algorithm performance算法平均规划时间/s平均采样点数平均航迹长度/mT101614207751IB-T00864173743CS-T01138145409从表 1 可以看出，CS-T 算法较 T 算法规划速度提升了 89%，采样点数减少了 94%，航迹长度缩短了 30%;由于 IB-T 算法采用的是双向随机树生长，因此在规划速度方面优于 CS-T 算法，但在采样点数以及航迹长度方面后者更具有优势，表现为采样点数减少了 41%以及航迹长度缩短了 16%。结合图 6 可以看出，CS-T 算法相比于 T 和 IB-T算法，规划出来的航迹曲折程度有所缓和，但依然存在尖角，不利于无人机飞行。图 6试验仿真对比图Fig 6Simulation comparison results of Test 针对上述问题，通过去除冗余节点策略及 Mini-mum Snap 对航迹进行平滑处理，设定无人机飞行时间T=30 s，航迹点的 p 已知，起始点、目标点的位置、速度和加速度均设为零，航迹平滑效果如图 7 所示。图 7航迹优化示意图Fig 7Schematic diagram of path optimization由图 7 可知，经过处理后的航迹较为光滑，没有出现方向突变情况。航迹规划阶段仿真数据如表 2 所示。由表 2 可以看出，从生成初始航迹到对航迹进行平滑处理共需要 0 15 s，满足无人机在复杂环境下的实时规划要求。值得注意的是，为了满足无人机的动力学约束，经过 Minimum Snap 处理后的航迹长度较去除冗余节点后的航迹长度有所增加。表 2航迹规划阶段仿真数据Table 2Simulation data in path planning stage性能CS-T 初始航迹去除冗余节点Minimum Snap航迹平滑规划时间/s0120010 02航迹长度/m147440135263137 8575结束语针对复杂环境下的无人机三维航迹规划问题，提出了一种基于约束随机采样点的 T(CS-T)算法，通过引入人工势场法中的引力场势能函数对随机采样点进行约束，引导随机树朝着目标点生长，有效降低了节点扩展的随机性，提高了算法的规划速度;另外设计了去除冗余节点的方法有效去除航迹中的无用节点，提高了航迹的平滑程度，并在此基础上利用 Minimum Snap对航迹进行平滑以满足无人机的动力学约束。仿真结83第 30 卷电光与控制刘靠等:基于 CS-T 算法的无人机航迹规划果表明，CS-T 算法能够在复杂环境下进行实时避障，有效提高航迹规划速度并缩短航迹长度。参 考 文 献 1 王祝，刘莉，龙腾，等 基于罚函数序列凸规划的多无人机轨迹规划 J 航空学报，2016，37(10):3149-3158 2 SAIM M,ADMANESH M,DECHEING M,et al Dis-tributed detect and avoid for multiple unmanned aerial ve-hicles in national air spaceJ Journal of Dynamic Sys-tems,Measurement and Control,2019,141(7):071014-1-071014-9 3 郭一聪，刘小雄，章卫国，等 基于改进势场法的无人机三维路径规划方法 J 西北工业大学学报，2020，38(5):977-986 4 马云红，张恒，齐乐融，等 基于改进 A*算法的三维无人机路径规划 J 电光与控制，2019，26(10):22-25 5 赵晓，王铮，黄程侃，等 基于改进 A*算法的移动机器人路径规划 J 机器人，2018，40(6):903-910 6 李宪强，马戎，张伸，等 蚁群算法的改进设计及在航迹规划中的应用J 航空学报，2020，41(增刊 2):724381-1-724381-7 7 王晓燕，杨乐，张宇，等 基于改进势场蚁群算法的机器人路径规划J 控制与决策，2018，33(10):1775-1781 8 曾国奇，赵民强，刘方圆，等 基于网格 PM 的无人机多约束航路规划J 系统工程与电子技术，2016，38(10):2310-2316 9 LAVALLE S M apidly-exploring random trees:a new toolfor path planning Ames:Computer Science Depart-ment,Iowa State University,1998 10ISLAM F,NASI J,MALIK U,et al T*-Smart:rapidconvergence implementation of T*towards optimal so-lution C/IEEE International Conference on Mechatron-ics and Automation Piscataway:IEEE,2012:1651-1656 11 YANG K J,KEAT G S,SUKKAIEH SA Gaussian process-based T planner for explore an unknown and cluttered en-vironment with a UAV J Advanced obotics,2013,27(6):431-443 12 LI M,SONG Q,ZHAO Q J UAV path re-planning basedon improved bidirectional T algorithm in dynamic en-vironmentC/The 3rd International Conference onControl,Automation and obotics(ICCA)Piscataway:IEEE,2017:658-661 13KUFFNE J J,LAVALLE S M T-connect:an efficientapproach to single-query path planning C/IEEE Intern-ational Conference on obotics and Automation Piscata-way:IEEE,2000:995-1001 14 KAAMAN S,WALTE M,PEEZ A,et al Anytimemotion planning using the T*C/IEEE Interna-tional Conference on obotics and Automation Piscata-way:IEEE,2011:1478-1483 15 WANG C Q，MENG M Q H Variant step size T:an ef-ficient path planner for UAV in complex environments C/IEEE International Conference on eal-time Com-puting and obotics(CA)Piscataway:IEEE,2016:555-560 16 朱战霞，靖飒，仲剑飞，等 基于碰撞检测的空间冗余机械臂避障路径规划J 西北工业大学学报，2020，38(1):183-190 17丁明跃，郑昌文，周成平，等 无人飞行器航迹规划 M 北京:电子工业出版社，2009 18 刘成菊，韩俊强，安康 基于改进 T 算法的 obo-Cup 机器人动态路径规划 J 机器人，2017，39(1):8-15 19 MELLINGE D,KUMA V Minimum snap trajectory gen-eration and control for quadrotors C/IEEE InternationalConference on obotics and Automation Piscataway:IEEE,2011:2520-252593第 7 期