四旋翼无人机的运动控制与轨迹规划.pdf

0ct.2023Journal of Xian UniveiNatural Science Edition2023年10 月No.4Vol.26西安文理学院自然科学版第2 6 卷第4期文章编号：10 0 8-556 4(2 0 2 3)0 4-0 0 40-0 9四旋翼无人机的运动控制与轨迹规划张?伟,张三乐,宋小康,张佳豪,张鲁冀,田飞龙（西安文理学院机械与材料工程学院，西安7 10 0 6 5）摘要：四旋翼无人机的飞行控制系统具有耦合性强、非线性、驱动数量比自由度少的特点，在飞行过程中存在诸多不可预知的障碍等，使得控制系统设计难度较大.课题针对四旋翼无人机控制存在的不足之处进行探究改进，对四旋翼无人机的运动控制与轨迹规划做了研究，设计了四旋翼无人机的控制系统、对飞行轨迹进行跟踪、并规划了无人机飞行轨迹，最后通过MATLAB软件对三种飞行轨迹进行模拟仿真.仿真结果表明，改进后的无人机能够平稳地运行，具有较强的自适应能力，能实现自主避障规划路线、快速灵活地飞行，从而验证了设计方案合理可行，具有实际应用价值.关键词：四旋翼无人机；动力学模型；PID控制；轨迹规划A*算法中图分类号：V279文献标志码：AMotion Control and Trajectory Planningof Quadrotor Unmanned Aerial VehicleZHANG Wei,ZHANG San-le,SONG Xiao-kang,ZHANG Jia-hao,ZHANG Lu-ji,TIAN Fei-long(School of Mechanical and Materials Engineering,Xian University,Xi an 710065,China)Abstract:The flight control system of a quadcopter unmanned aerial vehicle(UAV)has thecharacteristics of strong coupling,nonlinearity,and fewer drives than degrees of freedom.Thereare many unpredictable obstacles during the flight process,making the design of the controlsystem more difficult.The project explores and improves the deficiencies in the control of quad-copter UAV.It studies the motion control and trajectory planning of quadcopter UAV,designsthe control system of quadcopter UAV,tracks flight trajectories,and plans the flight trajectoriesof UAV.Finally,three flight trajectories are simulated using MATLAB software.The simulationresults show that the improved UAV can operate smoothly,has strong adaptive ability,can a-chieve autonomous obstacle avoidance planning route,and fly quickly and flexibly,thus verif-ying the rationality and feasibility of the design scheme,and has practical application value.Key words:quadrotor unmanned aerial vehicle(UAV);dynamic model;proportion integraldifferential(PID)control;trajectory planning;A*algorithm四旋翼无人机由于设计简单可靠，是旋翼无人机中被研究最多与使用的类型1-4.从2 0 0 5开始，电收稿日期：2 0 2 2-11-0 8基金项目：西安市科技计划项目（2 2 GXFW0120-2）；陕西省大学生创新创业项目（10 6 0 2 0 17 7）作者简介：张伟（197 6 一），女，陕西西安人,西安文理学院机械与材料工程学院教授，硕士，主要从事开关电源的电磁兼容性及机器人控制技术研究41第4期张伟，等.四旋翼无人机的运动控制与轨迹规划子信息领域高速发展，定位技术、通信技术逐渐成熟，为四旋翼无人机的广泛普及打下基础 5-8 .近几年，由于微型航电系统（MEMS）以及飞控、通信算法的蓬勃发展，无人机这个行业经历着一场剧变，不再是仅仅应用在军事领域和经济领域中，也被广泛地应用到人们的日常生活中.全球不少商业化无人机公司抓住了这个契机，实现了市值的成倍增长，也侧面验证了该行业的火热 9.政府和企业都纷纷意识到四旋翼无人机将会有较为广泛的应用场景，例如：新闻与电影的外景拍摄、物流快递的运送、地形图与地图的绘制、执法与管制的监控等 10-13.无人机在飞行时，外界环境复杂多变，可能出现许多无法预先知道的障碍，如行人、树林、建筑物等，工作时如果不能及时发现危险并采取有效措施，有可能无法完成预先设定的任务，甚至出现人员受伤、无人机摔毁等更严重的情况 14-15.所以，研究稳健、可靠的飞行控制算法，保证四旋翼无人机能够平稳、具有较强的自适应能力、自主避障规划路线、快速灵活地飞行，具有重要的意义.文中主要对无人机的运动控制与轨迹规划进行研究，1理论分析1.1四旋翼无人机控制方法设计1.1.1四旋翼无人机飞行原理控制四旋翼无人机本质是控制无人机上四个螺旋桨对应电机的各个参数.根据飞行器定义的机头方向不同，四旋翼无人机可分为“十”字型和“x型两种模型 16 “十”字型相比“x”型控制更容易，但“X”型因为可以同时控制四个电机来调整无人机姿态，所以更加灵活，飞行姿态联动性更好.四旋翼无人机的两种飞行姿态如图1所示（1）“X”型飞行姿态飞行原理“X”型飞行姿态如图1（a）所示.通过改变M1、M 2、M 3、M4四个电机的转速，可实现无人机的飞行姿态切换.若将M1、M 2、M 3、M 4四个电机的转速同时改变则可以实现无人机的垂直升降.将M1、M 2 的转速减小或M3、M 4增加，无人机向前飞行.将M1、M 2 的转速增加或M3、M 4转速减小，无人机会向后飞行.将M2、M 3的转速增加或M1、M 3的转速减小，无人机向右飞行.将M2、M 3的转速减小或增加M1、M 4的转速，无人机向左飞行.将M1、M 3的转速增加或M2、M 4的转速减小，(a)“X型(b）“十”字型图1四旋翼的飞行姿态无人机向右偏航.将M1、M 3的转速减小或M2、M 4的转速增加，无人机向左偏航（2）“十”字型飞行姿态飞行原理“十”字型飞行姿态如图1（b）所示.“十”字型飞行姿态若将M1、M 2、M 3、M 4四个电机的转速同时改变则可以实现无人机的垂直升降.当M1的转速减小或M3的转速增大，M2、M 4保持转速不变时，无人机向前飞行.将M1的转速增大或M3的转速减小，M2、M 4保持转速不变的时，无人机向后飞行.当增加M2或减小M4的转速，保持M1、M 3的转速不变，无人机向右飞行.减小M2或者增加M4的转速，保持M1、M 3的转速不变时，无人机向左飞行.当M1、M 3的转速增加或M2、M 4转速减小，无人机向右偏航.将M1、M 3的转速减小或M2、M 4的转速增加，无人机向左偏航.1.1.2四旋翼无人机在坐标系中姿态表示在描述无人机的位置和姿态时，往往需要用两个坐标系共同描述，即大地坐标系和无人机自身的机体坐标系.大地坐标系通常用来表示无人机的位置，机体坐标系表示无人机的姿态.需要通过欧拉角、四元数、旋转矩阵来对无人机姿态进行描述(1)欧拉角四旋翼无人机在起飞前停放于地面，机体坐标系与大地坐标系重合.四旋翼无人机开始工作时，位置和其本身姿态会发生变化，此时大地坐标系与机体坐标系将不再重合，这时可以使用欧拉角将两个坐标系联系起来.绕度，则：42第2 6 卷西安文理学院自然科学版）欧拉角包含俯仰角?、偏航角山、滚转角?.无人机绕x轴旋转的角为俯仰角，机体向上俯仰，俯仰角为正；无人机绕y轴旋转的角为滚转角，机体向右翻滚，滚转角为正；无人机绕z轴旋转的角为偏航角出，机体向右偏航，偏航角为正 17(2)四元数四元数最早是在18 43年提出，198 5年引入到计算机图形学中.四元数相比欧拉角不存在万向节锁死的问题，同时在三维空间内任意一个刚体的转动都可以将其表示为围绕某一特定的轴旋转，四元数可以根据旋转轴和其旋转角度来对刚体旋转进行表示四元数可以表示为q=w+xi+yj+zk(1)四元数也可以与欧拉角进行转换：若给定一个欧拉旋转为（X，Y，Z），即拉旋转为（X,Y,Z）即绕xyz三轴分别旋转X、Y、Z(3)旋转矩阵YZXYx=sinsincos+coscossin2）（2）（2）YYZy=sincoscos+coscossin（2）2）（2）（2）ZZ(2)z=cossin2）cos-sincossin2）（2）（2）w=coscoscos+sin()sinsin22222q=(x,y,z),w)轴旋转角，绕z轴旋转山角的旋转矩阵分别为：轴旋转角，绕y轴旋转角,绕z轴旋转山角的）以Z-X-Y欧拉角为例：100R,(p)=0cosP-sinpL0singcoScosO0singR,(0)=010(3)L-sino0cosOcost-sin山0R.(山)=sindcost0001R=R,()R()R,(0)cc-spstso-cpstcso+cospst(4)R=cost+cspse-cpsOScco若四旋翼无人机旋转角速度为：=，则无人机大地坐标系下角速度可转换为：co0-cpsO01(5)二Ls00cpc43第4期张伟，等动控制与轨迹规划1.1.3四旋翼无人机动力学模型为了便于对四旋翼无人机进行受力分析，所建的无人机模型为理想情况，并对其做如下假设：(1)无人机采用对称结构，且质量分布均匀，并假设中心点为质心；在飞行时的机体形变和机械振动都忽略不计；(2)四旋翼无人机的四个螺旋桨下分别直连四个电机，四个电机相互对称，且安装处于同一高度；(3)四旋翼无人机的四个螺旋桨产生的升力与阻力和螺旋桨转速的平方成正比，螺旋桨为刚性；四个螺旋桨下的电机工作电压均相同,且同一电压下螺旋奖转速相等；（4)四旋翼无人机所受重力始终保持不变，不会受到高度、纬度等因素的影响；（5)无人机飞行的环境稳定，保持在无风环境下飞行，忽略无人机在飞行时所受到的空气阻力以及螺旋浆产生的空气动力；（6)无人机在飞行时处在理想环境，不受摩擦力的影响，电机旋转时也不受到因摩擦产生的阻力矩的影响 18 建立无人机动力学模型就是将运动与力学建立联系，无人机的上升力来自螺旋桨的电机，所以首先需要建立电机方程、推力方程、力矩方程.四旋翼无人机姿态示意图 19 如图2 所示：电机模型：0;=km(o-0,)(6)推理方程：F;=kp?(7)力矩方程：M,=kmw?(8)其中；为第i个电机的角速度，F,为第i个电机所产生的力，M，为第i个电机的力矩，km与km为不同电机的固定系数.根据牛顿方程得到：厂mp=0+Rab(9)abL-mgLF,+F,+F;+F4其中p为无人机的质心.建立欧拉方程与角速度的关系可以得到：根据欧拉方程可以得到：34山1图2四旋翼姿态示意图cO0-cps001S(10)Ls0cPcoLI(F2-F4)I(F;-F,)(11)+XYM,-M,+M,-M4其中I为无人机的惯性张量1.1.4四旋翼无人机控制器设计PID控制因为使用便捷易用,稳定性好，是目前最常用到的控制算法 2 0 .文中选用串级PID控制器，采用两环进行控制，外环为位置环，内环为姿态环.控制系统框图如图3所示.期望值位置控制中e.e.中e姿态控制四旋翼无人机P.力图3四旋翼无人机控制系统框图44第2 6 卷西安文理学然科学版位置控制：姿态控制：Pi.e=p.ds+Kd.:(pde-p.)+K,.(p.des-p.)(12)K.(p。-p)+K d.(g。-)Kp.。(0。-0)+K d,(0。-0)(13)出。K（。-山)+Kd(山。-山)假定无人机以最小扰动状态飞行，通过牛顿和欧拉方程可以得到：u,=m(g+p3,c)Xu2=I.+(14)1.sin-P2,.cost)g1.cost+p2,esint)g其中u为无人机总推力，uz为无人机总力矩.文中选用PID控制方式，无人机系统需要快速响应，是一个动态的控制过程，引入积分项的作用是减小静态误差，而在实际飞行过程中过多追求的是更快的响应速度.双环PID调节时，先调节内环的姿态环.在进行PID参数整定时，首先对比例系数进行调节，增加比例系数可以提高整个系统的响应速率，但如果比例系数过大则会对控制系统的稳定性有较大的影响.调节参数过程中若无人机响应缓慢则需要增大比例系数，若无人机出现激烈震荡则证明比例系数过大.当比例系数调节到无人机基本不出现大幅度的抖动时，开始调节微分系数，微分系数值越大微分作用越强，微分系数影响误差信号的变化速率.当变化速率快时，微分作用加大，从而抑制整个控制系统的震荡，大大提高系统的稳定性.如果微分系数过大则会影响系统的响应时间.1.2四旋翼无人机轨迹生成无人机的飞行轨迹采用多项式拟合的方式进行表述，因四旋翼无人机是一个完整的动力学系统，所以它的运动状态没有办法突变，为了使无人机在飞行过程中轨迹连续平滑且便于控制，需要对生成的多项式进行约束.同时若需要对路径上的某一点的速度、加速度等参数进行设定，则同样需要引人一定约束条件.飞行轨迹用多项式进行表达 19，由于实际飞行过程中轨迹复杂，很难用一个多项式进行表述，故整段轨迹按时间历程划分为分段多项式：fi(t)=P.(t-T.)VT。t Tf:()=Z。P2.(-T,)T,tT2(15)t)=ZNPM,i(t-TM-1)iTM-1tTM假设总共有M段轨迹则存在M+1个轨迹点，多段多项式进行轨迹拟合与一段多项式拟合的方法不尽相同，在四旋翼无人机飞行中的大多数场景，难以将飞行中路过的中间航迹点的速度、加速度、jerk、s n a p 等参数进行设定.假定路径中间的轨迹点的速度、加速度、jerk、s n a p 等参数可以由四旋翼无人机系统自主调节，面对轨迹中某点各项参数不确定的情况，可以构建优化问题，使得中间状态与某一设定值相等时，制定的代价函数达到最小值，通过此方法可以不用精确控制无人机在飞行时的某一轨迹的具体参数，但可以通过优化代价函数使无人机飞行的轨迹达到最优解.根据上述思路可将轨迹最小化10）45张伟，等.四旋翼无人机的运动控制与轨迹规划第4期snap进行约束:f(t)=Z,P:t(4)(t)=Zi(i-1)(i-2)(i-3)t-*p:i4(*()=Z,i(i-1)(i-2)(i-3)jG-1)(G-2)G-3)i-8P:Pi4j4=J(T)=J(r()d=Z,Ti(i-1)(i-2)(i-3)jG-1)G-2)(j-3)Ti+j-7PiPji4,j4i+j-70Ti(i-1)(i-2)(i-3)jGj-1)(j-2)(-3)=J(T)=J(f(t)dt=PiPji+j-70=J(T)=PTQkPk完成最小化snap的约束，推导出了代价函数的具体形式，但如果只对（16)式进行优化得到其最小值，那么轨迹点之间的速度、加速度等参数不连续，只能实现无人机的轨迹平滑运动，两种轨迹之间的衔接不连续，所以需要继续增加额外的约束使得无人机轨迹相互连续.通过以下约束：k)(T)=Ji(T)！T*Pj.-Zj+1,/=0(i-k)!(l-k)!ik！(i-k)!(-k)!=0(17)A,-A0实现轨迹的连续性运行，无人机可以光滑地沿某一轨迹进行飞行，若要经过特定的某一点，或在某一点按照给定的速度或加速度进行飞行，还需要给额外规定的中间轨迹点增加一定的约束.在实际飞行中，只对无人机轨迹的起点和终点进行速度、加速度、jerk、s n a p 的约束.无人机中间轨迹点只对位置信息进行约束，速度、加速度、jerk、s n a p 无法约束，对轨迹中的点只进行连续性约束，通过优化器使相关参数分配最优的状态：(T,)=x)()！Ti-k=xT-kPj.iik(i-k)!=xT.j（h）Pj.(i-k)!(18)！T(i-k)！!0.jPj.i！(k)XT.j-k(i-k)!A;P;=d,第2 6 卷46西安文理学院（自然科学版）1.3四旋翼无人机路径规划无人机的路径规划常用的是基于搜索的方法,基于搜索的方法主要使用Dijkstra算法和A*算法.A*算法是一种基于传统图搜索的智能启发式算法，因其原理简洁、便于理解操作且搜索路径简单得以在路径规划领域被广泛应用 2 1.文中选用A*算法作为路径规划的算法，A*算法的一般表达式如下：f(n)=g(n)+h(n)(19)其中g（n）为耗散函数,表示从起点到路程中间某一结点需要花费的实际代价.h（n）为启发函数，表示路程中间某一结点到目标点的估算代价.最终找到一条f（n）最近的路程为最优路径.A*算法的流程图如图4所示.2实验验证本文基于MATLAB对上述无人机控制系统设计、轨迹生成、路径规划进行了实验仿真.2.1四旋翼无人机控制系统设计将无人机动力学模型与PID控制器结合，在MATLAB中仿真实现无人机的悬停、螺旋上升、菱形螺旋前进三种姿态飞行.三种姿态飞行的仿真结果通过图5图7 进行说明，图5 图7 每幅图中左上角为无人机开始创建NodeGList将起点值设为0，并将起点设置为初始其余元素设为infinite创建ParentList是否已经到达终点，或无法到达终点找出NodeGList中最小节点，设为CurrentNode是依次选择CurrentNode相邻节点中的一个是否访问过所有节点N.g=CurrentNode+edge.CostN.f=N.g+h(N)N.Parent=CurentNode井标记完成所有遍历结束图4A*算法流程图飞行轨迹，右侧部分为无人机的部分参数，红色线为仿真中无人机实际参数，蓝色线为目标参数.M205335223triets22202TmesRrumBueCmi5ma(4)(Rat25152025Tme(s;jRotTove.the-Dest2fee(siReTeeisilRdToresiRadThueBieDeslTnE25图5无人机悬停控制仿真图图6无人机螺旋上升仿真图TF图7无人机菱形前进仿真图从三幅仿真结果图可以看出，无人机仿真结果的实际参数与目标参数的拟合度很高，说明所建无人机动力学模型正确，参数设置合理，47第4期张伟，等.四旋翼无人机的运动控制与轨迹规划2.2四旋翼无人机轨迹生成给定三个运行轨迹：波浪轨迹，圆形轨迹，AIR字母轨迹，将无人机轨迹生成的结果在MATLAB中进行仿真，对三种路径轨迹进行模拟，图8 是波浪式轨迹仿真图、图9是圆形轨迹仿真图、图10 是无人机AIR字母轨迹仿真图.2229221522fiane:txiait22022TirelsisRed:图：无人机波浪轨迹仿真图图9无人机圆形轨迹仿真图22152tere tipRecTrurBleCesl23UED2202De图10无人机AIR字母轨迹仿真图2.3四旋翼无人机路径规划在MATLAB中建立栅格地图,设定起始点与目标点，并在地图中建立障碍物.结合无人机控制、轨迹生成相关知识，利用A*算法实现无人机自主路径规划，路径的自主路径规划图如图11、图12 所示.WwwwMw2252TTma!TeeilM2921522215221525图11无人机路径规划地图一图12无人机路径规划地图二3结论本文首先建立了无人机动力学模型，设计了串级PID控制器，对四旋翼无人机的位置和姿态进行控制，并在MATLAB中仿真实现了无人机的悬停、螺旋上升、菱形前进飞行.其次根据给定的目标点，给出了相应数学表达式，并对其增加了相关约束条件,实现了无人机按给定路径飞行，在MATLAB中稳定平滑的完成圆形轨迹、波浪轨迹、AIR字母轨迹的仿真飞行.最后在给定目标的基础上，选取A*算法实现了无人机的自主路径规划.仿真结果表明，改进后的无人机能够平稳地运行，具有较强的自适应能力，能实现自主避障规划路线、快速灵活地飞行，从而验证了设计方案合理可行，具有理论和工程实用价值。马云彤责任编辑48第2 6 卷西安文理学日然科学版）参考文献1吕厚谊.无人机发展与无人机技术 J.世界科技研究与发展，1998（6)：113-116.2邹湘伏，何清华，贺继林.无人机发展现状及相关技术 J.飞航导弹，2 0 0 6（10）：9-14.3范承啸，韩俊，熊志军，等.无人机遥感技术现状与应用 J.测绘科学，2 0 0 9,34（5）：2 14-2 15.4岳基隆，张庆杰，朱华勇.微小型四旋翼无人机研究进展及关键技术浅析 J.电光与控制，2 0 10,17（10）：46-52.5杨明志，王敏.四旋翼微型飞行器控制系统设计 J.计算机测量与控制，2 0 0 8（4）：48 5-48 7+490.6梁延德，程敏，何福本，等.基于互补滤波器的四旋翼飞行器姿态解算 J.传感器与微系统,2 0 11,30（11：56-58+61.7宿敬亚，樊鹏辉，蔡开元.四旋翼飞行器的非线性PID姿态控制J.北京航空航天大学学报，2 0 11,37（9)：10 54-1058.8李尧.四旋翼飞行器控制系统设计 D.大连：大连理工大学，2 0 139朱奕航.四旋翼无人机路径规划及跟踪控制技术 D.南京：南京航空航天大学,2 0 2 0.10李一波，宋述锡.基于模糊自整定PID四旋翼无人机悬停控制 J.控制工程,2 0 13,2 0（5）：910-914.【11】杨庆华，宋召青，时磊.四旋翼飞行器建模、控制与仿真J.海军航空工程学院学报，2 0 0 9,2 4（5）：499-50 2.12 刘丽丽.四旋翼飞行仿真器的建模及控制方法的研究 D.长沙：中南大学,2 0 0 9.【13王璐，李光春，王兆龙，等.欠驱动四旋翼无人飞行器的滑模控制J.哈尔滨工程大学学报，2 0 12,33（10）：12 48-1253.14】刘金琨，滑模变结构控制MATLAB仿真：基本理论与设计方法M.北京：清华大学出版社,2 0 19.【15周钰婷，郑健壮.全球无人机产业：现状与趋势 J.经济研究导刊,2 0 16(2 6）：2 6-30.16樊宝安，曾桂根.四旋翼无人机巡航飞行模式下的自主避障方案设计 J.电子设计工程，2 0 2 0,2 8（2）：18 0-18 3.17浴潘春荣，许化.基于STM32的X型四旋翼无人机设计 J.工程设计学报，2 0 17,2 4（2）：196-2 0 2+2 10.【18 莫小章.四旋翼无人机的线性自抗扰控制方法研究 D.桂林：桂林电子科技大学，2 0 2 1.【19】杨荟燎，姜斌，张柯.四旋翼直升机姿态系统的直接自修复控制 J.控制理论与应用，2 0 14,31（8）：10 53-10 6 0.【2 0】孙伟杰。四旋翼巡线无人机控制器研究 D.镇江：江苏科技大学，2 0 2 1.21高涛.基于A*算法的无人车路径规划J.江苏工程职业技术学院学报，2 0 2 1,2 1（4）：11-15.