基于模型预测控制的仿生海豚路径跟踪研究*

付乐乐，陈 宏，叶镇豪，巩伟杰，何 莉

（深圳大学机电与控制工程学院，广东 深圳 518061）

0 引 言

水下机器人的路径跟踪技术在海洋资源勘探、军事巡航、水下精确制导等方面有着巨大发展前景和应用价值，各国对于海洋资源争夺竞争日趋激烈，对于水下机器人路径规划、路径跟踪等方向的研究受到了十分广泛关注［1，2］。水下机器人的路径跟踪方法有PID控制，模糊控制，反步控制，及模型预测控制（model predictive control，MPC）等［3］。水下机器人属于多路输入和多路输出，是非线性系统，而PID基本情况下适用于单输入单输出线性系统，所以不太适合［4］。模糊控制需人为的控制方式指导，虽不必精确的系统建模，但过度依赖主观性，考虑情况不全面［5］。反步控制常用于水下机器人路径跟踪，其思想是设计一个速度控制器使系统稳定，可解决初始状态误差较大的问题，但是速度和加速度将会发生跳变，不符合实际控制［6］。

综上分析，本文对水下机器人路径跟踪控制方法采用MPC，可预测未来模型，进行反复优化求解并持续滚动控制［7］，针对海流影响进行了干扰分析，以及对于三维的水下路径跟踪研究，均验证了MPC 算法的路径跟踪效果极佳。

1 仿生海豚运动学

描述仿生海豚在空间中运动，通常需要两种坐标系，分别是惯性坐标系和载体坐标系［8］，惯性坐标系E-ξηζ，以地球上的某点作为坐标原点，载体坐标系O-xyz，固定在仿生海豚重心，随着海豚的运动而运动，如图1所示。

图1 惯性坐标系和载体坐标系

在惯性坐标系下，水下仿生海豚的位姿可用位置（x，y，z）和姿态角（φ，θ，ψ）表示，φ，θ，ψ分别为横摆角、纵倾角和艏向角；在载体坐标系下，仿生海豚的线速度和角速度分别为（u，v，w）和（p，q，r），u，v，w分别表示方向为进退、侧移、潜浮，在6自由度下仿生海豚的运动学转换方程为［9］

式中η＝［xyzφθψ］T为惯性坐标系下的位置及姿态量；v ＝［uvwpqr］T为载体坐标系下的线速度和角速度量；J（η）为两种坐标系之间的位姿转换矩阵［10］。

2 仿生海豚MPC算法

2.1 仿生海豚跟踪误差模型

通常情况下不必考虑全部6 个自由度的运动，由于仿生海豚的纵倾与横摇运动微乎其微，可忽略不考虑［11，12］，故仿生海豚的状态向量分别为η＝［x，y，z，ψ］T，v ＝［u，v，w，r］T，由于在本文中只研究了二维环境下路径跟踪效果，故取z，w为零，两者之间的关系由式（1）进一步得出

定义拟跟踪的目标曲线在惯性坐标系下轨迹ηd＝［xd ydzdψd］T，则仿生海豚跟踪控制的误差模型为

2.2 仿生海豚输出预测

误差模型是连续量，不能直接作用于模型预测控制器中，需要对式（3）进行离散化处理，处理后结果为

式中 ˜η（k）为状态量，˜ν（k）为控制量，k为某时刻，其中

式中T为采样时间，考虑到实际建模不准确性带来的误差，为了能够跟踪更加精确，考虑以ΔV作为控制量，对式（4）进行变换，得到

从而，可进一步得到新的状态空间表达式为

式中 ΔV为控制增量矩阵，为系数矩阵，＝

为简化计算，可令

根据新的离散化后的状态空间表达式，可进行输出预测为

其中

可通过当前状态量和控制增量计算得到预测的状态量和输出量，之后在约束条件下进行二次规划求解，得到控制增量的第一个序列，并以此求出控制量，作用于系统。

2.3 约束条件下的目标函数求解

通过MPC进行预测后，最终要转为目标函数优化问题求解，目标函数要确保仿生海豚能平稳地跟踪到参考轨迹，要反映出跟随能力和平稳能力，故对目标函数的设计如下

式中R和Q为权重矩阵。前项能反映对参考路径的跟随能力，后项能反映系统的稳定性。通过一定方式可将目标函数转化为二次规划问题，将式（7）代入式（11）并化简得到标准形式的目标函数

因此，在每一步的目标函数最优解问题都可以转为约束下二次规划问题求解

其中，约束条件为

在每个控制周期内，对式（13）进行求解，得到一系列的控制增量，由此会得到一系列的控制量，并将序列中第一个控制量作为控制输入量作用在系统中，直到下一时刻来临产生新的控制量序列，再次将第一个控制量作用于系统，循环往复，直到跟踪路径完成。

2.4 海流模型建立

海洋中海流对水下仿生海豚的影响处处可见，海洋中真实海流运动不规律，时变性较强，文中给出了海流模型，并分析某处海流对仿生海豚产生的路线偏移影响，其分析方法将适用于整片海流扰动之中，本文给出了一种基于流函数的周期变动的双曲流的计算实例，验证存在海流情况下，路径跟踪算法的有效性，其双曲流为

其中，f（x，t）＝a（t）x2＋b（t）x，a（t）＝ϵsin（ωt），b（t）＝1 -2ϵsin（ωt）。

海流速度场如下

式中A取0.1，代表速度矢量大小；ω取0.2π，ω／2π为振荡频率；ϵ取0.25，代表振荡幅度。

3 仿生海豚跟踪实验仿真

3.1 二维环境下圆形跟踪

首先对二维环境下的未封闭圆形轨迹进行跟踪，将圆形轨迹作为期望跟踪点，以R＝10 为半径，以（0，10）为圆心画出圆形，便于观察起点与终点，采用不封闭圆形作为参考跟踪路径。取初始状态量，即各轴姿态及艏向角［x，y，z，ψ］T＝［0，0，0，0］T，将采样点分为3 000 个，采样时间为0.01 s，预测步长为5，并对线速度、角速度控制约束设为［-2 m／s，2 m／s］，模型预测的权重参数Q设为［1 0 0 0，0 1 0 0，0 0 0 0，0 0 0 5］；R设为［10 0 0 0，0 10 0 0，0 0 100 0，0 0 0 1］。MPC与反步控制对圆形跟踪效果如图2所示。

图2 圆形轨迹跟踪效果对比

通过图2（a）中可看出2 种控制算法都可达到跟踪的效果，但是MPC 要比反步控制精度高，更加稳定。在图2（b）中，虽然反步控制跟踪误差较小，在允许范围内，但存在波动情况，并没有MPC 稳定，且跟踪误差极小。在图2（c）中，反步控制进退速度，转艏速度输出变化较大，并在进退速度中超出了预设速度界限，而MPC 进退速度、横移速度和转艏速度的变化均小于反步控制，较稳定且在预设的±2 m／s范围内。

3.2 路径规划曲线跟踪

根据对圆形轨迹跟踪效果对比，发现MPC算法在允许输出速度限制之内，不仅稳定，而且误差值也比反步控制算法小很多，于是采用MPC算法继续进行对改进蚁群算法并使用3次B样条平滑后的路径规划曲线跟踪，观察其效果是否可观。其跟踪结果如图3所示。

在无外界干扰环境下，使用MPC算法对其平滑处理的蚁群算法路径曲线实现了精确跟踪，其起始速度及其速度跳变量均被抑制在仿生海豚允许的速度范围之内，且跟踪误差极小，达到了预期要求。

3.3 海流干扰路径规划曲线跟踪

在无干扰环境下，其跟踪达到预设效果，为验证算法可靠性，在某处海流干扰下进行路径跟踪，验证在扰动环境致其偏离路线下，是否能够继续精确跟踪到路线。其跟踪效果如图4所示。

在改进蚁群算法并平滑处理的路径规划下，跟踪过程中，某处海流干扰致其偏离规划路线，此时会触发重规划，先用直线连接当前点和未来一定距离的规划点，然后用Teb优化算法优化出一条平滑的轨迹到达未来一定距离的规划点上，最终，对重规划的轨迹进行轨迹跟踪，直到跟踪到达终点。在多处海流影响下，继续按照此方法重规划。根据图4（a）跟踪效果可见，偏离路线后，进行重新规划，仍可稳定、精确地跟踪到规划路径。在图4（b）中，海流干扰造成一定误差，但Teb算法路径重规划之后，立即跟踪上规划路径，跟踪误差减小。在图4（c）中，干扰几乎不影响输出速度，均在可控范围之内，相对较稳定。

4 仿生海豚实验环境

本文进行实验验证采用的实验环境及仿生海豚如图5所示，该仿生海豚采用STM32F4 开发，左、右胸鳍分别采用2个舵机控制，可进行拍翼与摇翼运动，头部由3 个舵机组成，可进行左右转、俯仰及旋转运动，尾部由定制的直流无刷电机控制，而海豚沉浮则采用步进电机控制，并在头部装配了声呐，摄像头，在海豚重心位置装配姿态传感器，压力传感器等辅助设备。由于实验设备条件及环境的限制，在水池中进行了简单路径跟踪实验。

图5 实验环境与仿生海豚

5 结束语

在二维环境下，通过MPC算法和反步控制算法对圆形轨迹的对比及在有无海流干扰情况下，本文均研究了仿生海豚路径跟踪问题，其提出算法得到了有效验证，避免了反步控制的缺陷问题，由于受到篇幅及实验环境限制，目前只进行了简单二维曲线路径跟踪实验，后续工作中，将会在海边完成对二维及三维有无海流情况的仿真验证实验，并考虑将三维环境解耦成深度控制和水平面控制进行验证。