基于模型预测控制的无人艇VO 避障

钟雨轩，陈晓虎，马向峰

(1.上海大学 机电工程与自动化学院，上海 200444；2.中国船舶工业系统工程研究院，北京100094)

0 引 言

目前，无人艇（Unmanned Surface Vessel，USV）的应用还集中在水文测量和环保领域[1]，军事方面[2]的应用还较少。执行水文测量和环保领域任务时，USV一般以较低速度（＜10 kn）航行。而军事方面很多时候需要USV 满足高速[3-4]（＞20 kn）紧急避障性能要求。庄肖波等[5]提出基于蚁群算法（ACA）[6]实现高速无人艇对运动目标避碰规划的方法。但蚁群算法计算量大，求解时间长，在实际应用中，由于循环次数有限，很难得到全局最优解。吉大海等[7]提出一种基于行为的动态障碍物危险规避算法。然而该避障算法只从运动学层面上分析了动态障碍物约束，而未考虑船体动力学对避障性能的影响；该算法中避障被视为独立于控制的问题，容易出现控制无法达到路径规划期望值的问题。Bjørn-Olav Holtung Eriksen 等[8]设计并实施了基于非线性规划（Nonlinear Programming，NLP）的中层COLAV 算法，可避开静态和动态障碍物。但该算法属于策略式规划控制，没有应用到反应式避障控制。

高速场景对避障的实时性有更高要求。在传统算法架构中，避障被视为独立于控制的问题，易出现控制无法达到路径规划期望值的问题。无人艇控制系统是多输入多输出系统，无人艇避障问题属于多变量约束优化问题，如采用PID 控制，需要设计多个控制器，且无法兼顾多个输入输出量间的耦合关系，控制效果较差。而模型预测控制[9]是处理多变量约束优化问题最有效的方法之一。鲁棒控制、最优控制、自适应控制等也是处理多变量约束优化问题有效的方法，但这些方法对船体模型精度要求很高。而模型预测控制采用了近似测量和计算，能在每个步长内不断滚动优化，能够补偿由于模型失配、时变、干扰等引起的不确定性，及时补偿，始终将新的优化建立在实际的基础上，使控制保持实际的最优。为此，本文设计了基于模型预测控制（Model Predictive Control，MPC）的USV 速度障碍物（Velocity Obstacle，VO）避障算法（以下简称MPC_VO 算法）。通过仿真实验，验证了该算法适用于高速紧急避障场景，并分析了模型不确定对该算法避障效果的影响。

1 MPC_VO 算法设计

1.1 MPC 控制算法

MPC 控制[10]原理如图1 所示。k 轴为当前状态，左侧为过去状态，右侧为将来状态。

MPC 控制算法有多种不同的表达式，但都有着相同的基本结构，如图2 所示。其基本特点[11]都是基于模型的预测、滚动优化和前馈-反馈控制结构。

其中： ysp为 系统设定输出； yr为参考轨迹；u 为控制量输入； y为实际输出值； ym为模型输出值； yc为预测输出。

图 1 MPC 原理图Fig.1 The principle of model predictive control

图 2 MPC 原理框图Fig.2 The control structure of model predictive control

本文主要用状态空间模型描述系统。状态空间的非线性离散时间模型的表达式如下：

状态空间的线性连续时间模型的表达式如下：

其中： X(t)为 n 维的状态向量； y(t) 为 p维的输出向量，u(t) 为 m 维的控制向量，是输入向量； e(t)为误差量。A,B,C,D,K为系统矩阵。

对式（3）和式（4）进行一阶欧拉离散化，得到状态空间的线性离散时间模型，其表达式如下：

其中：X（k）为离散的n 维的状态向量；y（k+1）为离散的p 维的输出向量；u（k）为离散的m 维控制向量，是输入向量； e(k)为误差量，A，B，C，D，K 为系统矩阵。

把离散模型作为预测模型，迭代得到如下预测状态：

其中： Np为 预测时域， Nc为控制时域，一般 Nc＜Np。

1.2 基于VO 分析的代价函数构造

在高速紧急避障场景，有效避开障碍物和减小能耗是很必要的。因此，可以构造避障和减小能耗的代价函数，把高速紧急避障问题转换为满足一定约束条件的数值优化问题，可以VO 法基本原理建立VO 避障评价体系。VO 算法是反应式避障算法，要求计算要求低，应对环境的变化快，所以没有参考轨迹，控制向量也没有参考值。另外，考虑能量效率，倾向于采用小的加速度。因此，基于避障和能耗效率构造代价函数：

其中： Jvo为避障代价函数； Je为 能耗代价函数； λ1和λ2为权重系数。

为此，可以构造状态量的代价函数，VO 法原理如图3 所示。

图 3 VO 法构造代价函数原理图Fig.3 The principle of velocity obstacle

在相对空间中，以正北方向为基准，USV（A）与障碍物（B）位置的连线和正北方向的夹角为，相对速度和正北方向的夹角为。构造避障代价函数 Jvo为：

能耗代价函数 Je为 ：

uk为当前时刻的船体控制量。

权重系数是调节多项代价函数之间的权重的。由于在高速紧急避障场景中，有效避开障碍物是最重要的，能耗效率其次；又通过计算发现 Je的 量级是 Jvo的106倍，为突出 Jvo的影响，选择 λ1=108,λ2=1。

nstep 为预测步长。MPC 控制是以nstep 为单位滚动优化，nstep 越长，求得的解越接近于全局解，但模型失配、时变、干扰等引起的不确定性会更加明显。反之，nstep 越短，求得的解将为次优解，但模型失配、时变、干扰等引起的不确定性会不明显。仿真实验取nstep=20。MPC 控制采用近似测量与计算，不是实现整个控制输入序列，因为实际产生的USV 状态与模型有很大差异，所以是在每个时间步不断地重新评估控制输入的最优序列。

障碍物信息可以通过USV 搭载的传感器探测到，

其中：

因此，可以把带有约束条件的模型预测控制问题转化为目标函数式和约束条件式的二次规划问题。

2 MPC_VO 算法避障效果

在当障碍物（简称OBS.）在USV 的正上方且两者对遇行驶场景（见图4）中，USV 的起始速度为16 kn，OBS.的速度为20 kn。

图 4 对遇场景Fig.4 Avoidance process of head on scene

从避障效果图4 和图5 中USV 航迹角的变化图与USV 与OBS.距离的变化图可以看出，USV 向左侧转向避开OBS.，距离保持在安全阈值之上，满足安全避障要求。

图 5 对遇场景航迹角和距离的变化Fig.5 Variation trend of angle and distance in head on scene

在当USV 与OBS.交叉相遇的行驶场景（见图6）中，USV 的起始速度为16 kn，OBS.的速度为18 kn。

从图6 和图7 中USV 航迹角的变化图与USV 与OBS.距离的变化图可以看出，USV 与OBS.距离是安全距离阈值的3 倍左右，满足安全避障要求，且安全系数高。航迹角变化率很大，满足迅速紧急避障要求。

多运动障碍物避障仿真如图8 所示，USV 起始速度为16 kn，OBS.1 速度为20 kn，OBS.2 速度为25 kn，OBS.3 速度为16 kn。

从避障效果图8 和图9 中USV 航迹角的变化图与USV 与OBS.距离的变化图可以看出，USV 与3 个OBS.距离在安全距离阈值之上，满足安全避障要求。每次遇到障碍物时航迹角变化率都很大，满足迅速紧急避障要求。

3 MPC_VO 算法与VO 算法避障性能比较分析

分析MPC_VO 算法与传统VO 算法的避障效果，在对遇场景中，设计了与图4 相同的场景和初始条件。传统的VO 算法避障效果如图10 所示。

从避障效果图10（b）和图10（c）与图11（b）距离图可以看出，USV 与OBS.发生碰撞。

在交叉场景中，设计了与图6 相同的场景与初始条件。传统的VO 算法的避障效果如图12 所示。可以看出，USV 成功避开了OBS.。但从图13 可以发现，USV 在避障时航迹角左右摆动，避障效果较差，且U S V 到O B S.的距离较小，比同等条件下的MPC_VO 算法危险。

图 6 左交叉场景Fig.6 Avoidance process of cross from left scene

图 7 左交叉场景Fig.7 Variation trend of angle and distance in cross from left scene

图 8 多障碍物场景Fig.8 Avoidance process of multi-obstacle scene

图 9 右交叉场景Fig.9 Variation trend of angle and distance in multi-obstacle scene

综上所述，在高速场景中，传统的VO 算法存在避不开障碍物或避障时首向摇摆不定避障效果较差的情况，而MPC_VO 算法在相同场景中表现较好。

4 模型不确定性对算法的性能影响分析

为了分析模型不确定性对MPC_VO 避障算法的影响，在图8 多障碍物场景中分别验证船体的质量m，船体的转动惯量Iz，船体的长度L，线性阻尼项和非线性阻尼项对避障性能的影响。

图 10 对遇场景VO 避障效果图Fig.10 Avoidance process of head on scene using VO method

图 11 对遇场景Fig.11 Variation trend of angle and distance in head on scene using VO method

图 12 左交叉场景VO 避障效果图Fig.12 Avoidance process of cross from left scene using VO method

图 13 左交叉场景Fig.13 Variation trend of angle and distance in cross from left scene using VO method

图14 和图15 分别是改变质量m 后的模型和原始模型的避障航迹角和X Y 坐标。可以看出，质量m±20%的模型避障输出和原始模型避障输出位置点和航迹角都非常接近，且从图15（a）可以看出质量m 增大20%X 坐标和原始模型的X 坐标几乎重合。因此，船体质量m 的不确定性对MPC_VO 避障算法的避障效果影响不显著。

图 14 改变质量m 的模型与原始模型避障航迹角Fig.14 Avoidance angle of changed weight model and initial model

图16 和图17 分别是改变转动惯量Iz后的模型和原始模型的避障航迹角及XY 坐标。从图17（a）可以看出转动惯量Iz减小20%的X 坐标和原始模型的X 坐标几乎重合。在图16 中，虽然转动惯量Iz增大20%的模型在避开第2 个障碍物时的航迹角和原始模型避障航迹角偏差较大，但不超过5°。因此，船体的转动惯量Iz的不确定性对MPC_VO 避障算法的避障效果影响不显著。

图 15 改变质量m 的模型与原始模型避障XY 坐标Fig.15 X and Y coordinate of changed weight model and initial model

图 16 改变转动惯量Iz 的模型与原始模型避障航迹角Fig.16 Avoidance angle of changed rotational inertia model and initial model

图18 和图19 分别是改变船长L 后的模型和原始模型的避障航迹角和XY 坐标。从图18 改变船长L 后的避障航迹图可以看出，船长L 减小20%的模型在避开第2 个障碍物时的航迹角和原始模型避障航迹角偏差较大，最大角度偏差为10°左右。从图19 图（b）可以看出船长L 减小20%的模型与原始模型的输出位置Y 坐标最大偏差为4 m 左右。因此，船长L 的不确定性对MPC_VO 避障算法的避障效果影响显著些。

图20 和21 分别是改变线性阻尼项和非线性阻尼项的模型和原始模型避障航迹角及XY 坐标。从图20可以看出，改变线性阻尼项的模型与原始模型避障航迹角最大偏差有25°左右，Y 坐标位置偏差有10 m 左右，改变非线性阻尼项后的模型与原始模型的避障航迹角最大偏差为5°左右。因此，船体的改变线性阻尼项和非线性阻尼项的不确定性对MPC_VO 避障算法的避障效果影响是比较显著。

图 17 改变转动惯量Iz 的模型与原始模型避障XY 坐标Fig.17 X and Y coordinate of changed rotational inertia model and initial model

图 18 改变船长L 的模型与原始模型避障航迹角Fig.18 Avoidance angle of changed boat length model and initial model

图 19 改变船长L 的模型与原始模型避障XY 坐标Fig.19 X and Y coordinate of changed boat length model and initial model

图 20 改变线性阻尼项和非线性阻尼项的模型与原始模型避障航迹角Fig.20 Avoidance angle of changed linear damping model and initial model

图 21 改变线性阻尼项和非线性阻尼项的模型与原始模型避障XY 坐标Fig.21 X and Y coordinate of changed linear damping model and initial model

综上所述，MPC_VO 算法不断滚动优化，能够补偿由于模型失配、时变、干扰等引起的不确定性，及时补偿。

5 结 语

本文针对路径规划和控制分开设计的策略容易出现控制无法达到路径规划期望值的问题，融合局部路径规划和船体控制，设计了MPC_VO 算法。通过仿真实验实现了该算法的避障功能，并将该算法与传统VO 算法避障效果作对比，验证了该算法的有效性。同时，通过分析模型不确定性对该避障算法性能的影响，证明了该算法的鲁棒性。