动态环境下固定翼无人机的路径跟踪控制

动态环境下固定翼无人机的路径跟踪控制

1无人机模型预测控制方法设计无人机在军事和民用领域都有广泛的应用前景。提高无人机的独立性已成为研究的热点。无人机的独立性决定了飞机在动态环境中能够实时避免未知物体。问题的难点在于，动态环境中未知物体的不确定性以及在线避免障碍的现时性要求。目前已有很多学者通过在线路径规划算法解决这一问题.文采用A*算法,在3维空间内在线生成无人机的飞行航路.由于生成的航路由一系列直线段构成,因此要使航路满足固定翼无人机的运动学约束还需进行光顺处理,但这需要耗费更长的运算时间.文分别采用改进的Grossberg神经网络(GNN)法、遗传算法和人工势场法在线生成无人机的避障航路,但仅考虑了环境中的静止未知障碍物而没有考虑移动障碍物.其它的用于无人机路径规划的算法,如线性规划法、非线性规划法以及混合整数规划法,能在给定约束下对目标函数进行优化并产生满足约束条件的可飞行路径.但由于计算量过大,这些方法并不适用于动态环境的在线路径规划.另一方面,当无人机遇到未知障碍物时,大多数在线路径规划算法首先是在线重新生成一条可飞行的避障路径,然后再由无人机跟踪该避障路径从而避开环境中的未知障碍物.综上,这些算法仍不能满足实时性要求较高的情况(如存在移动障碍物).模型预测控制方法通过滚动优化自定义的性能指标,能实时得到控制对象的控制输入.文指出滚动优化控制法(即模型预测控制)在解决不确定环境下无人机避障问题时具有很好的实时性.文给出了模型预测控制方法的详细综述.文在2维平面内采用非线性模型预测控制方法使无人机在线跟踪直线参考路径的同时能实时避开环境中的未知障碍物,但该方法无法使无人机跟踪任意的曲线路径且仅能避开环境中的小型静止障碍物.根据以上分析,本文研究了一种能使固定翼无人机及时避开环境中未知障碍物的路径跟踪控制算法.当机载传感器未探测到未知障碍物时,采用非线性模型预测控制器(NMPC)实时优化得到跟踪参考路径的控制输入,当探测到未知障碍物时,通过NMPC控制器实时优化得到避障控制输入,使无人机产生避障动作及时避开环境中的未知障碍物.为了使参考路径上的每一点都能满足固定翼无人机的运动学约束(如曲率连续、最大曲率约束和最大挠率约束等),直接将曲线用于路径规划得到适于无人机飞行的参考路径显得更加合理.因此,这里利用毕达哥拉斯速端曲线的有理性、曲率连续以及曲线平滑等特性,首先通过离线路径规划器生成基于PH曲线的参考路径,使参考路径上的每一点都是无人机可飞行的.然后采用非线性模型预测控制器跟踪PH参考路径,同时避开动态环境中的未知障碍物,并确保无人机在避障后继续沿参考路径朝目标点飞行.这里,未知障碍物包括了静止障碍物和移动障碍物.算法借鉴了文中的目标函数形式,但相比文做出了如下改进:(1)算法能稳定跟踪任意形式的参数化曲线参考路径;(2)能使无人机避开环境中的大型静止障碍物和移动障碍物;(3)在3维空间内验证了算法的有效性.2问题描述procedural2.1跟踪参考ph路径无人机的飞行任务是以匀速V从指定起点沿参考轨迹飞往目标点.其中,参考轨迹是由离线路径规划器根据已知的作战环境(如已知的静止障碍物/威胁)和飞行任务规划出的一条可飞行的PH曲线路径.为了能完成既定的飞行任务,要求无人机能尽量沿离线规划的PH参考路径飞行,但当遭遇未知障碍物/威胁时,则要求无人机能及时避开障碍物,并在避障后继续沿参考路径飞行.这里假定无人机上的机载传感器能及时探测到未知障碍物的位置和大小,并用球体近似表示障碍物.图1给出了无人机跟踪参考PH路径的俯视示意图.图中实线表示PH参考路径,虚线表示无人机跟踪参考路径时的实际飞行轨迹.2.2无人机飞行过程中ph曲线路径的选择文中选用PH曲线作为无人机的可飞行路径.PH曲线由Farouki首次提出,是一种具有有理特性的参数化曲线.有关PH曲线的详细介绍可参见文[10-12],这里仅简要介绍PH曲线的定义和它的数学表达式.设r(q)=[x(q),y(q),z(q)]为以q为参数的任意曲线,若r(q)各分量的导数能满足以下条件:其中σ(q)为多项式,则称r(q)为PH曲线.PH曲线的贝塞尔形式及其1阶导数如下所示:其中pk为曲线的控制顶点,△pk=pk+1-pk,n为曲线多项式中最高次项的次数.文中采用五次PH曲线作为参考曲线路径(即n=5),因为它是具有足够灵活性且次数最低的PH曲线.根据无人机在起点和目标点位置和飞行姿态的约束、环境中已知的障碍物、无人机的最小转弯半径以及油耗等约束可以通过离线优化算法(如遗传算法)得到满足约束条件的五次PH曲线路径.该PH曲线路径可作为无人机实际飞行的参考路径.由于本文篇幅有限,不介绍离线获取PH曲线路径的过程,而认为PH参考路径为已知,也即控制点pk(k=0,···,5)为已知.2.3无人机运动学模型的建立这里将无人机视为一个质点以匀速V飞行,并假设内环控制器能很好地保持无人机的飞行姿态(即偏航角、俯仰角和滚转角)且通过控制油门保持无人机匀速飞行.因此,这里仅考虑无人机的外环轨迹跟踪控制.那么,3维空间内无人机的运动学模型如下:式中状态向量X=(χ,γ,x,y,z)T中各分量分别表示航迹坐标系下的航向角、爬升角以及无人机在地球坐标系下的位置坐标,控制输入U=(ny,nz)T的各分量分别为过载在航迹坐标系的y轴和z轴的投影,并限定了控制输入的取值范围.过载控制输入ny和nz取值的限定相当于给出了无人机最小转弯半径的约束.在实际应用中采用四级四阶龙格–库塔法近似得到式(4)的离散化模型:式中△T为采样时间.通过非线性模型预测控制器(NMPC)在线实时优化目标函数可以得到式(4)中的控制输入U=(ny,nz)T.3非线性模型预测控制器非线性模型预测控制(NMPC)又称为滚动时域控制,是一种基于模型的有限时域开环最优控制算法,其本质是在每个采样时刻以当前时刻的系统状态(即无人机在当前时刻的航向角、爬升角和在地球坐标系下的位置坐标)为初始状态使用系统模型(即式(4)给出的无人机运动学模型)对未来N步内的系统状态或输出进行预测,通过优化算法生成使目标函数(例如未来N步内的输出与参考轨迹间的累积误差)达到最小的最优控制序列{U0,U1,···,UN-1},其中将第一个分量U0作为无人机运动学方程(如式(4)所示)的控制输入,而其它分量被存储,即以{U1,···,UN-1,UN-1}作为下一步优化中的控制输入的猜测值.若设非线性模型预测控制的有限时间域为t,其离散时间步长就为N:文中非线性模型预测控制的目标函数形式如下:Jnav为未来N步内无人机的预测状态量与参考轨迹的累积误差,Jobs是当无人机接近障碍物时产生的惩罚项,Jadd为将无人机引向目标点的函数,该函数的作用是防止无人机在避开障碍物后背离目标点飞行,R为正定对角矩阵.在每一个采样时间内,采用非线性优化算法(如牛顿法)最小化目标函数J,可得到最优控制输入序列{U0,U1,···,UN-1},其中U0则作为当前时刻下无人机的实际控制输入,其值也被限定在给定范围内(如式(4)所示).而其余的控制输入{U1,···,UN-1,UN-1}则作为下一个采样时刻中式(5)的控制输入预测未来N步内无人机的状态向量{X(1),···,X(N)},本文中均假定当前时刻为0时刻.非线性模型预测控制器的计算流程如图2所示.式(7)中Jnav用于跟踪PH参考路径,Jobs用于避开障碍物,Jadd则用于引导无人机避障后继续沿参考路径朝目标点飞行.其中,Jobs和Jadd仅在无人机遭遇未知障碍物时起作用.下面重点介绍目标函数J中Jnav、Jobs以及Jadd的计算方法.3.1稳定跟踪参考路径这里介绍稳定跟踪PH参考路径的方法,也即式(7)中Jnav的计算方法.首先可以将上一采样时刻优化得到的控制序列{U1,···,UN-1,UN-1}作为当前时刻下未来N步内控制输入的初始猜测值,通过式(5)估计无人机在未来N步内的状态向量,即S(k)=(χ(k),γ(k),x(k),y(k),z(k))T,k=1,2,···,N.若设Sr(k)=(χr(k),γr(k),xr(k),yr(k),zr(k))T为未来第k时刻的参考状态向量,那么Jnav的表达式为其中N为预测时间长度,Q为五阶正定对角矩阵.正确选取未来N步内的参考状态向量是稳定跟踪PH参考路径的关键.在介绍参考状态向量Sr(k)的选取方法前,先讨论连续时间内稳定跟踪参考路径的条件.这里设无人机的飞行速度为V,P=(x,y,z)T为无人机的位置向量,γ、χ为无人机的当前爬升角和航向角.Pd=(xd,yd,zd)T为参考曲线路径r(q)上的目标点位置向量,γd、χd为该点切向量的爬升角和航向角.位置向量P和Pd均随时间t连续变化.若设Oxgygzg为地球坐标系,Oxkykzk为由目标点Pd上的切向量、法向量以及副法向量构成的动坐标系(如从图3可以看出,若要使无人机稳定跟踪点Pd,应保证当时间t→∞时,∥ε∥→0.这里通过李亚普诺夫稳定判据求解稳定跟踪参考路径r(q)的条件.首先定义李亚普诺夫函数:图3所示),那么由地球坐标系到动坐标系的转换矩阵Rkg为无人机与目标点间的位置误差在动坐标系Oxkykzk上的投影为其导数为无人机位置向量P和目标位置向量Pd的导数分别为其中Vd为目标点Pd移动的速度.将式(12)、(13)代入式(11)可以得到:对式(15)求导并代入式(14)可以得到:将式(9)代入式(16)可以得到使˙V<0,也即稳定跟踪参考曲线路径r(q)的充分条件:又因˙,所以可以得到参考路径r(q)的参数q的变化率:式中η1和η2为自定义的正常数,(x′d,y′d,z′d)为目标点上的切向量.式(20)决定了稳定跟踪参考路径时目标点Pd的位置随时间的变化,式(18)和式(19)确定了稳定跟踪参考路径时无人机的飞行方向.将式(18)~(20)离散化,可以确定参考状态向量Sr(k)中的各分量:由式(21)可以看出Sr(k)中的(xr(k),yr(k),zr(k))实际就是参考路径上目标点Pd的位置坐标,χr(k)和γr(k)通过Pd上切向量的航向角χd(k)和爬升角γd(k)计算得到.第k时刻的参考状态向量Sr(k)由前一时刻无人机的状态向量S(k-1)和参考状态向量Sr(k-1)递推得到.另外,值得说明的是,以上给出的稳定跟踪参考路径的方法适用于任意参数化曲线路径.3.2惩罚函数的改进这里未知障碍物包括了静止障碍物和移动障碍物,并假设障碍物的位置、大小信息可以由机载传感器(如视频、光电或红外等设备)探测得到.同时,用半径为robs、完全包含了障碍物的球体近似表示环境中的障碍物.假定移动障碍物的运动学模型如下:其中Vobs为障碍物的速度,这里认为是常数.γobs、χobs分别为障碍物的爬升角和航向角,γobs0为爬升角初值,是常值,u1为航向角的控制输入.(xobs,yobs,zobs)T为障碍物中心在地球坐标系下的位置坐标.在每一个采样时刻移动障碍物都被视为一个静止障碍物.当未知障碍物(包括静止障碍物和移动障碍物)与无人机间的最小距离小于给定阈值ξd时可在目标函数中加入惩罚项Jobs以避开障碍物.文提出了一种避障惩罚函数形式:式中N为预测时间长度,Nobs为当前时刻下机载传感器探测到的障碍物的个数,(x(i),y(i),z(i))表示未来第i时刻无人机的位置坐标,其值由式(5)预测得到,(xobs(j),yobs(j),zobs(j))为当前时刻下第j个障碍物圆心的位置坐标,kj、σj均为大于0的常数.从式(23)可以看出,无人机离障碍物圆心越近其惩罚值越大,但惩罚值的大小仅与无人机到障碍物圆心的距离相关,与障碍物的大小无关.因此当障碍物较大时,式(23)不能产生足够大的惩罚值使无人机及时避开障碍物.其中,robs(j)表示探测到的第j个障碍物半径,(x(i),y(i),z(i))表示未来第i时刻下无人机的位置坐标,(xobs(j),yobs(j),zobs(j))表示当前采样时刻下传感器探测到的第j个障碍物(包括静止障碍物和移动障碍物)的位置坐标.从式(24)可以看出,改进后的惩罚函数考虑了障碍物的大小,当无人机与障碍物圆心距离D大于障碍物半径robs时,惩罚值随无人机离障碍物边缘距离的减小而增大;当D小于障碍物半径robs时其惩罚值继续随无人机与障碍物圆心距离的减小而增大.因此,改进后的避障惩罚函数能使无人机及时避开环境中的大型障碍物.此外,值得注意的是式(8),即目标函数中的Jnav可以使无人机稳定跟踪PH参考路径,但并不能保证无人机是朝着终点(即图1中“?”所标记的终点位置)的方向沿PH参考路径飞行的.换句话说,当无人机遭遇大型障碍物时,目标函数中避障惩罚函数Jobs的作用使无人机能及时避开大型障碍物,但是为了能避开障碍物,无人机可能朝着背离终点的方向飞行.因此,为了使无人机在避开障碍物后仍能朝着终点方向飞行,当无人机遭遇大型障碍物时还需在目标函数中添加一个额外惩罚值:N为预测时间,λ为正常数,(x(i),y(i),z(i))为通过式(5)预测得到的未来第i时刻下无人机的位置坐标,(xgoal,ygoal,zgoal)为终点位置.额外惩罚值Jadd相当于在目标函数中加入了终点对无人机的吸引作用.从式(25)中可以看出,无人机距离终点位置越远这种吸引作用越大,越接近终点则吸引作用越小.3.3无人机在n步内的状态向量及状态向量的预测综上所述,为了确保无人机能稳定跟踪PH参考路径,并在遭遇障碍物时能及时避开,非线性模型预测控制器在每一个采样时刻按以下步骤得到当前时刻下无人机的控制输入:步骤1:以上一时刻存储下来的控制输入序列{U1,···,UN-1,UN-1}作为初始猜测值,根据式(5)预测N步内无人机的状态向量S(k)(k=1,···,N).步骤2:按照式(21)计算无人机在N步内的参考状态向量Sr(k)(k=1,···,N),并按照式(8)计算Jnav.步骤3:计算无人机在当前时刻下与环境中障碍物间的最短距离,若均大于给定阈值ξd,则令式(7)中的Jobs和Jadd为0;若有障碍物与无人机间的最短距离小于ξd,则按照式(24)和式(25)分别计算Jobs和Jadd.步骤4:最小化式(7)所示的目标函数,得到最优控制序列.步骤5:输出最优控制序列{U0,U1,···,UN-1}中的第1个分量U0作为无人机运动学方程(如式(4)所示)的控制输入,而其它分量被存储作为下一个采样时刻控制输入的初始猜测值.4初始控制设置仿真实验在IntelCoreTM2、3GHz、2G内存的计算机上进行,编程环境为Matlab7.0.4.非线性模型预测控制器在每一采样时刻按照3.2节所示步骤计算当前时刻下无人机的控制输入,其中在步骤4中采用Matlab自带的函数fmincon最小化式(7)所示的目标函数得到最优控制输入序列.仿真中一些初始条件的设置如下:无人机的飞行速度V=50m/s;采样时间△T和有限时间域长度N分别为0.1s和10;式(22)中移动障碍物的控制输入u1为常值0.02,飞行速度Vobs=70m/s,它的初始爬升角γobs0为0,初始航向角χobs0为-90°;静止障碍物和移动障碍物的半径均为500m;PH参考路径的6个控制点分别为其中p0是参考路径的起点,p5是参考路径的终点;无人机的控制输入ny和nz分别限定在[-0.5,0.5]和[-1,3]范围内;由于预测时间长度N=10,在t=0时刻控制输入ny和nz的初始猜测值分别为[0,0,0,0,0,0,0,0,0,0],[1,1,1,1,1,1,1,1,1,1].基于上述仿真环境和初始条件的设置,开展相关的算法仿真实验验证,主要内容包括:跟踪PH参考路径的稳定性验证改进避障惩罚函数Jobs的有效性验证额外惩罚值Jadd的有效性验证4.1无人机初始位置与参考路径的关系首先在环境中没有障碍物的情况下给出了无人机跟踪PH参考路径的仿真结果(如图4、5所示).此时目标函数中的Jobs和Jadd始终为0,仅Jnav起作用.无人机的初始状态向量为(χ0,γ0,x0,y0,z0)T=(0,0,-4050,100,2050)T.由于PH参考路径的起点位置为(-4000,0,2000)T,因此无人机的初始位置与参考路径的起点位置有偏差.图4给出了无人机跟踪PH参考路径的轨迹,其中实线表示PH参考路径,小三角形表示无人机的实际飞行轨迹,“”表示参考路径的起点,“?”表示参考路径的终点,“?”表示无人机的初始位置.从图中可以看出虽然在初始状态下无人机的位置与参考路径的起点位置有偏差,但无人机很快能回到参考路径上来并继续沿参考路径飞行.图5给出了由式(10)定义的无人机与目标点Pd间的位置误差在动坐标系Oxkykzk上的投影εx、εy和εz随时间的变化,它们分别由实线、短虚线和长虚线表示.从图中可以看出,无人机与目标点Pd间的位置误差最终都收敛到0附近.虽然控制输入ny和nz分别限定在[-0.5,0.5]和[-1,3]范围内,但无人机仍能紧密跟踪PH参考路径,这说明PH参考路径能满足无人机的运动学约束,适于无人机飞行.4.2飞行轨迹仿真为了验证改进惩罚函数Jobs的有效性,分别采用了文提出的避障惩罚函数形式(如式(23))以及改进后的避障惩罚函数形式(如式(24))进行仿真,且式(23)和式(24)中kj和σj的设置相同分别为kj=2800,σj=40.当无人机与障碍物间的最短距离小于1000m时,目标函数中的避障惩罚值和额外惩罚值开始起作用.此时,无人机的初始状态向量为(χ0,γ0,x0,y0,z0)T=(0,0,-4000,-1000,2000)T,与参考路径的起点位置重合.仿真结果分别如图6和图7所示.图中实心球表示静止障碍物,空心球表示移动障碍物并用点划线表示移动障碍物的运动轨迹.从图6可以看出文提出的Jobs并不能使无人机很好地避开较大的障碍物,因此无人机的飞行轨迹穿过了障碍物,这说明在实际飞行中此时已发生碰撞.而当采用了本文提出的改进的Jobs后无人机的飞行轨迹能成功地绕开环境中的障碍物,并在避障后继续沿参考路径飞行(见图7).此时的控制输入ny和nz以及无人机的路径跟踪航迹角和位置随时间的变化如图8和图9所示.从图中可看出过载控制输入ny和nz均被限定在指定范围以内(见图8).图9中的目标值指的是无人机在当前时刻的目标点,即PH参考轨迹上目标点Pd的位置坐标以及该点切向量的航向角和爬升角.当遭遇障碍物时,为了避开大型障碍物,无人机的航向角和爬升角较大,偏离目标值较多,且由于nz限定的范围大于ny,使得无人机的爬升角变化更快.但在避开障碍物后无人机能及时回