动态环境下在线航迹规划的滚动优化方法

魏铁涛，王剑薇，屈香菊

(北京航空航天大学航空科学与工程学院，北京 100191)

引言

基于分层规划的思想，飞行器的航迹规划一般可分为离线航迹规划和在线航迹规划两个阶段［1］。其中，离线航迹规划通常在起飞前进行，根据当时已知的环境和任务信息，生成折线形式的参考航迹，飞行器沿此参考航迹在安全走廊内飞行［2］。但由于离线航迹规划通常没有考虑飞行器的机动性约束，所以得到的航迹不可直接使用，还需要进一步进行更细致的规划，即分层规划的第二层——在线航迹规划。

在线航迹规划的任务，是在飞行过程中利用较细致的实时环境与任务信息，在以参考航迹为中线的安全走廊内规划出一条安全可飞的可行航迹。可行航迹在飞行器的控制中起到导引的作用，飞控系统以此为指令信息引导飞行器按预定航迹飞行，通常也被称为指令航迹。指令航迹应该尽量贴近参考航迹，使得两者间的偏差尽可能小，并满足飞行器的机动性能约束。因此，在线航迹规划属于带约束的最优控制问题。当飞行器在动态环境下执行任务时，环境信息的改变有可能导致预先规划得到的参考航迹不可用，需要重新规划出新的参考航迹才能满足环境约束。在这种情况下，在线航迹规划需要兼顾快速性与精确性，在较短的时间内为飞行器生成新的指令航迹，引导飞行器安全飞行。

大多数军用和民用的在线航迹规划问题都是采用固定时域的优化方法，单纯以指令航迹与参考航迹之间的航迹偏差最小为优化目标。由于飞行器的在线航迹规划是在不断运动中规划前方航迹段的，采用固定时域的搜索寻优方法，指令航迹的实时性将会比较差［3-5］。参考航迹的突然变化，容易引起指令航迹与参考航迹之间的航迹偏差急剧增大，甚至飞出安全走廊，导致任务失败。文献［6］提出了一种偏差微分综合控制方法，在性能指标中引入偏差微分项，能够抑制解的振荡，得到合理有效的控制律，该方法在静态和已知环境中的应用具有较好的性能，但对于动态或未知的环境却有很大的局限性。模型预测控制(Model Predictive Control，MPC)算法对于处理动态环境中的航迹规划问题具有一定的优势，滚动时域的在线优化能更好地适应系统参数以及环境的动态变化［7］。然而，传统MPC的计算量较大，限制了它在很多动态系统中的应用。过去十年中，许多学者提出了许多改进的方法来解决MPC算法的精度和速度之间的矛盾。James和 Vesna［8］简化了有限域MPC优化算法，使用末端惩罚来提高算法的解算速度。David等［9］基于MPC算法，引入一个势函数(Potential Function)，该函数将可能运动障碍物或其它飞行器的状态信息反映到代价函数中，计算量较小，可用于直升机的在线航迹规划。文献［10］用Gradient-Descent方法解决最小化问题，可以将计算量减少到一定程度以用于RUAV的实时航迹规划。文献［11］应用扰动分析理论将系统的非线性模型在标准轨迹(Nominal Trajectory)的基础上线性化，将有限时域最优控制问题变为一个易处理的凸优化问题。

以上几种改进的MPC方法在一定程度上降低了计算量，可应用于低速飞行器的在线航迹规划问题。但是，在飞行速度较大，或参考航迹的航向改变较大的情况下，上述方法的精度可能变低。针对此问题，本文提出一种动态环境下在线航迹规划的滚动优化方法，通过引入一个新的终端惩罚项，使得指令航迹的性能指标能够更好地反映飞行器在未来飞行段指令航迹与参考航迹发生偏差的趋势。有助于飞行器对前方变化后的航迹方向提前作出反应，以提高动态环境下的在线航迹规划性能。

1 飞行器模型

在平面笛卡尔坐标系内建立飞行器的运动学模型。飞行器的运动用其位置(ξ，η)、速度(V)和偏航角(χ)描述。飞行器的运动学离散模型为:

式中，χk为偏航角;(ξk，ηk)为飞行器在采样时刻 k的位置;ρH为水平航迹的曲率半径。飞行器协调转弯飞行，χk+1是经历时间间隔Δt后的偏航角，相对于k时刻的增量为Δχk。图1显示了上述变量的几何关系。

图1 变量几何关系图

飞行器在采样时刻k+1的状态，包括χk+1，ξk+1和ηk+1，可以通过下述公式计算:

2 滚动优化方法

2.1 模型预测控制

模型预测控制是一种滚动的、有限时域内的优化控制算法，使用被控对象的预测模型来预测其未来的响应和状态。在满足输入和状态限制约束下，在有限时域内对控制量进行滚动优化［4］。

模型预测控制在每一个采样时刻，将系统的当前状态作为初始条件，计算在有限控制时域内系统的未来响应，再根据该优化对象的性能指标，求解一个开环最优化问题，得到一个控制量的输入序列，并将该控制序列的第一项作用于被控对象。在下一采样时刻，用新的飞行器状态，重复求解上述优化问题，从而形成闭环控制。图2直观地显示了模型预测控制的基本思想。

图2 模型预测控制基本思想

2.2 性能指标的建立

首先，为了使飞行器的指令航迹尽可能贴近参考航迹，应该寻求最优控制使指令航迹与参考航迹之间的偏差最小。所以，在建立性能指标函数时，首先考虑到在性能指标中对控制域内的航迹偏差进行加权，即按航迹偏差最小确定性能指标。该指标为:

在按航迹偏差最小确定性能指标时，控制变量往往会出现剧烈振荡。为了抑制解的振荡，可以在性能指标中增加一个偏差微分项，对变化过程进行控制［5］。增加微分项后的性能指标可表示为:

2.3 算法流程

基于模型预测控制的动态环境下在线航迹规划方法包含以下步骤:

(1)更新预测时域内的环境信息和参考航迹。(2)在每个采样点k，计算未来有限控制时域内可能的飞行器状态。

(5)从时刻k+1开始，重复上述步骤，直到任务完成。

3 影响因素分析

3.1 滚动优化的影响

假定某型飞行器执行动态环境下的飞行任务，参考航迹会根据前方环境的变化而实时更新，最终执行的参考航迹如图3所示。要求在线航迹规划能够及时精确地跟踪调整后的参考航迹，生成合理的指令航迹。

分别采用传统的固定域内的在线航迹规划方法和本文提出的动态环境下的在线航迹规划方法，对此航迹规划问题进行仿真计算。飞行器采用式(1)的运动学模型，可得指令航迹与参考航迹之间的航迹偏差(e)随时间变化情况如图4所示。仿真结果表明，采用模型预测控制进行在线航迹规划时，指令航迹与参考航迹的偏离程度要比传统的固定时域的控制方法明显改善。

图3 参考航迹

图4 指令航迹偏离误差对比

3.2 控制域长度的影响

控制时域长度Tc的选择对指令航迹偏离程度和计算时间有很大影响。经过多次试算，取预测时域长度Tp=6 s，对3.1节提出的航迹规划任务进行仿真。图5和图6分别给出了在速度为150 m/s，125 m/s，100 m/s，85 m/s 时，采用不同控制域长度情况下的指令航迹偏差和计算时间t。在预测时域给定的前提下，控制时域选得越小，意味着对未来飞行器状态的控制能力越弱，反映在性能指标中效果也就越差。同时较大的控制时域长度将使计算量增加，影响在线航迹规划的实时性。故控制时域长度的选取要在算法的快速性和精确性之间综合权衡，根据具体的飞行任务来确定。

在本例中，综合考虑航迹偏差程度和计算时间的大小，控制时域长度选为4 s时综合性能最好。

图5 不同控制域的航迹偏离误差

图6 不同控制域的计算时间

3.3 预测偏航角偏差权重的影响

综合考虑间隔时间ΔT和性能指标对优化结果的影响，取ΔT=1 s，根据多次的试算，取各项权重G1=0.6，G2=0.3，G3=0.1，对 3.1 节提出的在线航迹规划任务进行仿真。当在预测时域内发生前方参考航迹的方向变化时，才给G4赋值，G4分别取为0.1，0.3，0.6 和 0.9。

图7表示在不同权重G4条件下，指令航迹与参考航迹之间的航迹偏差;图8为不同G4条件下，在参考航迹转折点附近的指令航迹细节图。可以看出，G4越大，在预测时域内发生参考航迹的转折时，飞行器对前方参考航迹的方向改变能够更早作出相应反应，使得指令航迹与参考航迹的偏差更小。

图7 航迹偏差情况

图8 参考航迹航向变化点附近指令航迹细节图

4 仿真示例

假设飞行器进入一个如图9所示的存在若干需要规避的障碍物(用阴影表示)的未知区域，由于探测能力的限制，飞行器只有在接近障碍物时才能发现并对其当前参考航迹作出调整。要求为飞行器在线规划一条满足机动性能约束的指令航迹，并保证其与不断更新的参考航迹之间的偏差尽可能小，各参数的取值如表1所示。

表1 各参数取值

采用本文提出的在线航迹规划滚动优化方法和传统的固定时域规划方法分别进行仿真计算，规划结果如图9所示。其中传统固定时域的规划方法耗时245.6 s，平均航迹偏差42.8 m;而滚动优化方法耗时175.5 s，平均航迹偏差13.9 m。

图9 在线航迹规划结果

在动态环境下，由于飞行器是在不断运动中规划前方可行航迹的，这时如果采用传统的规划方法，在线规划的实时性将比较差，参考航迹的突然变化，容易引起飞行器指令航迹的航迹偏差急剧增大，甚至偏离出安全走廊的范围导致任务失败，如图9中的A点所示。而采用在线航迹规划的滚动优化方法，在参考航迹方向发生突然改变时，能够迅速作出相应反应，航迹偏离程度有了很大改善，规划速度与固定时域的规划方法相比也有所提高。

5 结束语

本文以平面内的固定翼飞行器为例进行研究，但提出的动态环境下在线航迹规划的滚动优化方法可以扩展应用于更广泛的领域，包括三维空间内、水下、空间及地面交通工具的航迹规划等。但是，三维空间内的航迹规划问题对规划速度的要求将会更高，并且滚动优化方法的影响因素也呈现出一些特点，这些需要进一步的研究。

［1］ Shim D H，Kim H J，Sastry Shankar.Hierarchical control system synthesis for rotorcraft-based unmanned aerial vehicles［C］//AIAA Guidance，Navigation and Control Conference.Denver，CO，2000.

［2］ 魏铁涛，屈香菊.多机协同与多目标分配任务规划方法［J］.北京航空航天大学学报，2009，35(8):917-920.

［3］ Ross IM，Fahroo F.Pseudo spectral knotting method for solving non-smooth optimal control problems［J］.Journal of Guidance，Control，and Dynamics，2004，27(3):397-405.

［4］ Schouwenaars Tom，How Jonathan，Feron Eric.Receding horizon path planning with implicit safety guarantees［C］//Proceeding of the 2004 American Control Conference.Boston，2004.

［5］ 王忠俊.非线性飞行控制方法研究概述［J］.飞行力学，1995，13(3):24-28.

［6］ 何佩，屈香菊，武哲.航迹跟踪的优化方法研究［J］.北京航空航天大学学报，2003，29(7):599-601.

［7］ Wang Liu-ping.Model predictive control system design and implementation using MATLAB ［M］.London:Springer，2008.

［8］ Primbs JA，Nevistic V.A new approach to stability analysis for constrained finite receding horizon control without end constraints［J］.IEEE Transactions on Automatic Control，2000，45(8):1507-1512.

［9］ Shim D H，Kim H J，Sastry Shankar.Decentralized nonlinear model predictive control of multiple flying robots［C］//IEEE Conference on Decision and Control.Hawaii，2003.

［10］ Kim H J，Shim D H，Sastry Shankar.Nonlinearmodel predictive tracking control for rotorcraft-based unmanned aerial vehicles［C］//American Control Conference.Anchorage，AK，2003.

［11］ Singh Leena，Fuller James.Trajectory generation for a UAV in urban terrain，using nonlinear MPC ［C］//Proceedings of the IEEE American Control Conference.Arlington，VA，2001:2301-2308.