彭求志 田丽 程彬
摘 要：针对四轴飞行器很难通过运动控制来完成快速扩展随机树（Rapidly-exploring Random Tree， RRT）所生成运动轨迹的问题，提出动力学和运动学约束的RRT算法.采用固定最终状态和固定最终时间控制器改进算法，通过动力学和运动学约束产生四轴飞行器可执行的路径，对比实际四轴飞行器行走的路径与期望的路径，对RRT重新布线，调节其行走路径，保证算法的渐近最优性.仿真实验及实物测试结果表明，该算法可以运用到四轴飞行器运动规划中，在实际应用中具有一定的价值.
关键词：快速扩展随机树;运动规划;四轴飞行器;ROS
[中图分类号]TP301.6 [文献标志码]A
Path Planning Based on ROS Quadcopter
PENG Qiuzhi，TIAN Li*，CHENG Bin
（Key Laboratory of Advanced Perception and Intelligent Control of High-end Equipment，Ministry ofEducation，Anhui University of Engineering，Wuhu 241000，China）
Abstract：Aiming at the problem that it is difficult for a quadcopter to complete the motion trajectory generated by the Rapidly-exploring Random Tree through motion control，an RRT algorithm with dynamics and kinematics constraints is proposed.The algorithm is improved by the method of fixed final state and fixed final time controller，and the executable path of the quadcopter is generated through the dynamics and kinematics constraint RRT algorithm，and the error between the actual quadcopter"s path and the expected path is compared，and the wiring is rerouted，adjust its walking path to ensure the asymptotic optimality of the algorithm. Simulation experiments and physical test results show that the algorithm can be used in the motion planning of quadcopter and has certain value in practical applications.
Key words：rapidly-exploring random tree;path planning;quadcopter;ROS
四軸飞行器自主导航应用广泛，其导航包含感知、定位、路径规划以及运动控制[1]四部分，路径规划（Path Planning）[2-3]是研究的核心之一.路径规划大致分为四类：传统算法、图形学方法、智能算法和其他算法，研究成果丰硕.[4]S.Karaman等证明了基于采样的路径规划算法RRT和PRM具有概率完备性和渐进最优性.[5]Kontoudis George P.等提出了一种基于渐进最优快速探索随机树（RRT），利用积分强化学习，用化直为曲的连续时间线性系统的最优成本和最优策略.[6]Tang Zhilin等提出了一种基于采样的运动规划器，在快速探索随机树（RRT）中，通过考虑初始状态和最终状态对，为每个对确定与系统动力学和约束条件兼容的最佳轨迹，使成本最小化.[7]Moon Chang-bae等提出了一种双向快速探索随机树（DT-RRT），增加节点扩展的成功率，节省计算成本.[8]Chi Wenzheng等提出了一种基于风险的快速探索随机树算法（Risk-DTRRT），该算法在启发式轨迹的基础上提供了最优的轨迹、导航运行时间和计划轨迹的长度，证明所提出算法的有效性.[9]Aoude Georges S.等提出了一种实时路径规划算法（RR-GP），该算法通过结合高斯过程（GP）的灵活性和基于采样的可达性计算的效率构建学习的运动模式模型，显著降低与动态障碍物碰撞的风险.
笔者针对四轴飞行器很难通过运动控制来完成RRT所生成运动轨迹的问题，提出一种算法，可以生成连续且平滑的路径.算法通过对固定最终状态和固定最终时间最优控制问题，得出最优的、开环的、固定的、最终状态自由的最终时间控制策略，通过局部路径规划与全局路径规划相结合的方法，通过增多采样点缩短路径的长度，节约成本.
1 Kinodynamic RRT
1.1 问题的定义
令X=Rn和U=Rm分别是四轴飞行器的状态空间和控制输入空间，并让飞行器的动力学由以下线性系统定义为：
x·[t]=Ax[t]+Bu[t]+c.（1）
其中，x[t]∈C是飞行器的状态，u[t]∈U是四轴飞行器的控制输入，且A∈Rn×n，B∈Rn×m，给定常数c∈Rn×1.
四轴飞行器的轨迹由元组变量π=x[]，u[]，τ定义，其中τ是轨迹的到达时间或持续时间，给定初始状态x0，在有限时间（到达时间或持续时间），存在控制u：[0，τ]→U，使得x：[0，τ]→X是沿着相应的状态轨迹运动.
轨迹π的成本函数定义为：
c（π）=∫τ01+u[t]TRu[t]dt.（2）
惩罚函数指轨迹的持续时间和控制工作量的能量损耗.其中，R∈Rm×m是正定的、恒定的、给定的，且权衡了相对于彼此以及轨迹持续时间的控制输入成本.
设C为系统的状态空间，Cfree∈X定义飞行器的自由状态空间，由用户定义范围内且相对于环境中障碍物无碰撞的状态组成.Ufree∈U定义四轴飞行器的自由控制输入空间，该空间由放置在其上的控制输入组成.路径规划的定义：给定一个起始状态xstart∈Cfree和一个目标状态xgoal∈Cfree，在xstart和xgoal目标之间找到一个无碰撞轨迹π*free的成本最低：
π*free=argmin π|x[0]=xstart∧x[τ]=xstart∧t∈[0，τ]x（t）∈Cfree∧u（t）∈Ufree c[π].（3）
轨迹函数：设π*[x0，x1]=x[]，u[]，τ是x0∈C和x1∈C之间的最佳轨迹，与边界和障碍无关.成本函数：c*[x0，x1]是x0∈C和x1∈C之间轨迹的持续时间和控制工作量的能量损耗.
c*[x0，x1]=minπ|x[0]=x0∧x[τ]=x1c[π].（4）
π*[x0，x1]=argminπ|x[0]=x0∧x[τ]=x1π[π].（5）
对于所有0 1.2 动力学和运动学问题 解决飞行器动力学和运动学转化为数学的问题即最优边界值问题.公式（3）能够计算出任意两个状态x0∈X和x1∈X之间的最优轨迹π*[x0，x1]及其成本c*[x0，x1]，分别在等式（4）和（5）中定义.在固定到达时间的情况下，实现最优控制策略，最佳自由到达时间，相应的最佳轨迹. 1.2.1 固定最终状态和固定最终時间的最优控制 固定最终状态和固定最终时间的最优控制是给定一个固定的到达时间τ和两个状态x0，x1，找到一个轨迹（x[]，u[]，τ）使得x[0]=x0，x[τ]=x1，x·[t]=Ax[t]+Bu[t]+c（对于所有0 设G[t]为能控标准型性公式： G·[t]=∫τ0expA（t-t′）BR-1BTexpAT（t-t′）dt′.（6） 这是Lyapunov方程的解： G[t]=AG[t]+G[t]AT+BR-1BT， G（0）=0.（7） 设该系统为动力学系统，则G[t]是t>0的正定矩阵且是可控的.x-[t]描述系统的零输入响应为： x-[t]=exp[At]x0+∫t0expA（t-t′）cdt′.（8） 微分方程解为： x=[t]=Ax-[t]+c.（9） 固定最终状态、固定最终时间、最优控制问题的最优控制策略为： u[t]=R-1BTexpAT（τ-t）G[τ]-1（x1-x-[τ].（10） 1.2.2 寻找最佳到达时间 为了找到定义在x0和x1之间的最佳轨迹π*[x0，x1]，解决固定的最终状态、自由的最终时间最优控制问题，可以通过选择到达时间τ来最小化方程的成本函数. 为了找到最佳到达时间τ*，通过把（10）代入成本函数（2）进行积分，得到固定到达时间τ，x0和x1之间的最佳轨迹成本的封闭式解： c[τ]=τ+x1-x-[τ]TG[τ]（x1-x-[τ].（11） 最佳到达时间τ*是此函数最小的τ值： τ*=argmin{τ>0}c[τ].（12） x0和x1之间的最佳轨迹成本由c*[x0，x1]=c[τ*]给出，最佳到达时间τ*通过将c[τ]相对于τ的导数c[τ]=0得到： c·[τ]=τ-2（Ax1+c）Td（τ）-d（τ）TBR-1BTd（τ）.（13） 定义： d（τ）=G[τ]-1x-1-x-[τ].（14） c[τ]可以具有多个局部最小值. 计算具有动力学A=（0，1），（0，0），B=（0，1）T，c=0，R=1的四轴飞行器的函数c[τ]的图在x0=（0，0）T和x1=（1，1）T之间，最佳到达时间是τ*=7-1≈1.65，这是函数c[τ]最小的地方. 1.2.3 计算最佳轨迹 给定最佳到达时间τ*，找到相应的最佳轨迹π*[x0，x1]=x[]，u[]，τ*. y[t]=expAT（τ*-t）d[τ*].（15） 最优控制策略为： u[t]=R-1BTy[t].（16） 最佳控制策略为： x·[t]=Ax[t]+BR-1BTy[t]+c， x[τ*]=x1.（17） 等式（15）是微分方程的解： y·[t]=-ATy[t]， y[τ*]=d[τ*].（18）
复合微分方程为：
x·[t] y·[t]=ABR-1BT0-ATx[t]y[t]+c0x[τ*]y[τ*]=x1d[τ*].（19）
微分方程的解为：
x[t]y[t]=expABR-1BT0-AT（t-τ*）x1d[τ*]+∫tτ*expABR-1BT0-AT（t-t′）c0dt′.（20）
对于所有给定的x[t]，在0 1.3 Kinodynamic RRT★算法 Kinodynamic RRT★由局部路径规划和全局路径规划组成，如图1所示.为了找到最佳的无碰撞轨迹π*free，首先由全局路径规划的RRT★产生初始路径，根据动力学和运动学约束产生四轴飞行器可执行的路径.然后对比四轴飞行器行走的路径与期望的理想路径的误差，调节四轴飞行器的行走路径.最后局部路径规划.考虑到RRT有能够在任何一对状态之间找到最佳轨迹的能力，通过对RRT★的改进，可以实现渐近最优性.随着算法迭代次数接近无穷大，找到最佳路径的概率接近1.由于随着RRT★算法节点的增多，Kinodynamic RRT★会重新布线，很大程度上减少了路径的长度. Kinodynamic RRT★：算法与标准RRT★算法略有不同（表1）.首先，笔者将问题定义为找到恰好到达目标状态的轨迹，而不是RRT★中常见的目标区域.由于RRT★算法计算的部分轨迹是不平滑的，而动力学和运动学规划的算法会平滑的连接到采样状态，并将采样状态本身作为节点添加到树中，不会影响算法的渐近最优性. RRT★：算法以起始点为根节点，在自由状态空间中构建轨迹树T（表2）.算法每次迭代，从自由状态空间Cfree采样状态xi成为树的新节点（第5行）.对于每个新节点，在树中已有的相邻节点中找到一个父节点，计算其成本.对于某个相邻半径r，c*[x，xi] 新轨迹的最佳轨迹π[x，xi]无碰撞的节点，并且找到xstart和xnew之间的最小路径即当前最小 的成本. Local Planning（表3）：通过局部路径两个最接近的状态xlocstart，xlocgoal，以在中心Oloc和半径rloc处构造一个圆如图1所示.在xaugobs产生一个新的节点xtest，根据节点产生如图1所示的黑色路径. Parent：此操作通过将新节点连接到树中的相邻节点，即c*[xi，x]的节点x，降低从树的起点到树中其他节点的成本（表4）.对于每个状态x来说，连接都是无碰撞的，并且从一开始就可以降低成本到x，则将新节点xi设为x的父级，该算法继续进行新的迭代.如果无限期重复此操作，则树中出现xstart与xgoal之间的最佳路径. Replanning RRT★：对局部路径进行碰撞检测，如果无碰撞，根据Kinodynamic RRT★产生曲线路径（表5）. 2 实验结果 2.1 四轴飞行器模型 四轴飞行器是又称四旋翼飞行器，可以通过控制其加速度沿任意方向移動.它的状态空间是四维的，其线性动力学描述为[10-11]： x=pvrw，u=uxuyuf，A=0I0000I0000I0000，B=00I0000LI.（21） p∈[0，100][0，200]（m），v∈[-10，10]2（m/s），r∈[-1，1]2（rad），w∈[-5，5]2（rad）.其中，p描飞行器在平面中的位置，v为速度，r为角速度，w为角加速度，且uf∈[-4.545，9.935]（N），ux=uy∈[-3.62，3.62]（N）.设置控制惩罚r为0.5，即允许四轴飞行器达到最大速度，但不会经常超过 它们. 通过在MATLAB进行仿真实验，在RRT树中添加5 000个节点后，规划出四轴飞行器的渐近最优轨迹（图2）. 2.2 仿真实验及实验结果分析 通过Kinodynamic RRT★的算法进行四轴飞行器的运动规划，MATLAB计算出其渐近最优轨迹如图2.可以看出，Kinodynamic RRT★规划出来的路径是连续且平滑的，适用于四轴飞行器.笔者构建了ROS平台开发环境，使用地图构建、四轴飞行器定位和导航三个软件包[12-13]，在GAZEBO搭建仿真环境，通过SLAM构建地图. Cartographer：可以实现基于激光雷达的即时定位与地图构建（SLAM）. Localization：采用扩展卡尔曼滤波和自适应蒙特卡洛定位的定位算法，融合各种传感器的信息，获得准确的定位效果. Navigation：基于Kinodynamic RRT★的路径规划，实现二维四轴飞行器的导航. 实物测试：采用PX4四轴飞行器.四轴飞行器自动驾驶技术包含环境感知、定位与导航、路径规划、控制.环境感知技术以“摄像头+激光雷达”的多传感器融合为主;定位技术以“激光雷达+摄像头+IMU+GPS”的多传感器融合定位为主.采用Kinodynamic RRT★的路径规划如图3所示. 实物测试结果（图3）表明，因满足运动学和动力学的条件即最优边界问题，四轴飞行器可以按照Kinodynamic RRT★规划出来的路径行走，且与MATLAB仿真结果保持一致.该轨迹在RRT算法中不仅减少了飞行器急加速和急减速中的能量损耗，而且降低了飞行控制实现的难度，减少了飞行器运动中的成本.
表6的数据显示，随着节点的增加，路径的长度会缩短，接近一个渐进最优值，说明局部路径规划在Kinodynamic RRT★起着很重要的作用，体现了该算法的优越性，保持了RRT★的最优性.
3 结论
本文提出动力学和运动学约束的一种基于增量式采样的RRT算法，可以生成连续且平滑的路径，适用于具有四轴飞
行器的渐近最优运动规划.研究采用动力学和运动学方法改进RRT★算法，确保任何状态空间中具有可控性动力学的任何系统的渐近最优性.Kinodynamic RRT★算法通过动力学和运动学约束RRT算法产生四轴飞行器可执行的路径，对比四轴飞行器行走的路径与期望的路径误差，对RRT★的重新布线，从而调节其行走路径.ROS的仿真实验以及实物测试结果表明，该算法可以运用到四轴飞行器运动规划中.四轴飞行器实际与理想的路径虽然存在一定偏差，但在允许的范围之内，说明该算法在实际应用中具有良好的效果，可以应用到四轴飞行器之中.
参考文献
[1]C.Goerzen，Z.Kong，B.Mettler.A Survey of Motion Planning Algorithms from the Perspective of Autonomous UAV Guidance[J].Intelligent & Robotics Systems，2010，57（1-4）：65-100.
[2]E.Galceran，M.Carreras.A survey on coverage path planning for robotics[J].Robotics and Autonomous Systems，2013，61（12）：1258-1276.
[3]ThiThoa Mac，Copot Cosmin，Duc Trung Tran，Heuristic approaches in robot path planning：A survey[J].Robotics and Autonomous Systems，2016，86：13-28.
[4]S.Karaman，E.Frazzoli.Sampling-based algorithms for optimal motion planning[J].Robotics Research，2011，30（7）：846-894.
[5]Kontoudis，George P.，Vamvoudakis，Kyriakos G..Kinodynamic Motion Planning With Continuous-Time Q-Learning：An Online，Model-Free，and Safe Navigation Framework[J].IEEE Transactions on neural networks and learning systems，2019，30（12）：3803-3817.
[6]Tang，Zhiling，Chen，Bowei，Lan，Rushi.Vector Field Guided RRT★ Based on Motion Primitives for Quadrotor Kinodynamic Planning[J].Intelligent & Robotics Systems.2020.
[7]Moon Chang-bae，Chung Woojin.Kinodynamic Planner Dual-Tree RRT （DT-RRT） for Two-Wheeled Mobile Robots Using the Rapidly Exploring Random Tree[J].IEEE Transactions on industrial electronics，2015，62（2）：1080-1090.
[8]Chi Wenzheng，Wang，Chaoqun，Wang Jiankun.Risk-DTRRT-Based Optimal Motion Planning Algorithm for Mobile Robots[J].IEEE Transactions on Science and engineering，2019，16（3）：1271-1288.
[9]Aoude，Georges S.，Luders，Brandon D.，Joseph，Joshua M.Probabilistically safe motion planning to avoid dynamic obstacles with uncertain motion patterns[J].Autonomous Robots，2013，35（1）：51-76.
[10]白天.室內环境中移动四轴飞行器的多目标点路径规划方法研究[D].杭州：浙江大学，2019.
[11]闫晓东.基于视觉的四轴飞行器自主避障系统的研究与实现[D].成都：电子科技大学，2018.
[12]杨小育.中高速超车场景下基于模型预测的无人车运动规划[D].合肥：中国科学院大学（中国科学院深圳先进技术研究院），2020.
[13]刘昊.基于ROS的移动操作四轴飞行器设计与开发[D].上海：上海交通大学，2018.
编辑：琳莉
收稿日期：2020-10-13
基金项目：安徽省自然科学基金面上项目（1908085MF179）;安徽高校自然科学研究重点项目（KJ2018A0121）;安徽省高校优秀青年人才支持计划项目（gxyq2017013）
作者简介：彭求志（1995-），男，安徽安庆人.硕士研究生，主要从事移动机器人四轴飞行器路径规划研究;田丽（1962-），女，安徽芜湖人.教授，硕士，硕士生导师，主要从事智能电网和四轴飞行器控制技术研究;程彬（1998-），男，安徽安庆人.本科生，主要从事四轴飞行器路径规划研究.
