水上搜救机器人航迹跟踪控制器设计*

肖长诗 高欣国 文元桥 周 阳 隋忠义 向皓闻

(武汉理工大学航运学院1) 武汉 430063) (国家水运安全工程技术研究中心2) 武汉 430063)(内河航运技术湖北省重点实验室3) 武汉 430063) (武汉理工大学智能交通系统研究中心4) 武汉 430063)

0 引 言

针对某些水域来往船只稀少、气象和水文环境复杂，实施快速人工搜救困难的问题，设计了一种能水上自主搜救的机器人模型，机身采用呈正三角分布的三个浮筒形式，既保障搜救机器人自身稳定性，又可避免搜救过程对落水人员的伤害.该机器人运动灵活，可原地转向；配备三个推进器，速度较快；结构简单，易于回收和维护.

实现机器人自主搜救首先需要解决航迹跟踪控制.航迹跟踪是对规划好的路径进行跟踪，有间接航迹跟踪和直接航迹跟踪两种方式[1].间接航迹跟踪分为外环制导环和内环控制环，直接航迹跟踪则是将两个环节集成在一起，这种航迹跟踪通常基于神经网络等方法实现.由于间接航迹跟踪结构简单，较容易实现，在工程应用上较广泛.而间接航迹跟踪内环控制方法主要有反步法、内模控制、模糊控制和神经网络等等.反步法和内模控制能达到较好的跟踪效果，但需要精确的被控对象数学模型[2-3].模糊控制和神经网络不需要精确的被控对象模型，但要求设计人员具有丰富的知识和实际经验，且航迹跟踪误差波动较大[4-5].S面控制方法是模糊思想与PID控制结构的结合，它具有不依赖精确数学模型、适用于非线性控制和便于调节参数等优点[6].文献[7-9]将S面控制方法运用于水下机器人的运动控制中，并进行了参数自调整的优化，通过实验验证了S面控制器具有既快又稳的控制效果.在外环制导研究中，LOS(line-of-sight)循迹已经在轮式机器人、飞行器和水面船舶上得到了广泛的应用.Fossen等[10]在传统的LOS制导算法加入积分环节，消除缓变环境载荷对船舶位置所产生的横向偏离.与传统船舶的路径跟踪相比，三推进器水上机器人还需要解决推进器布置冗余的分配问题，常规的伪逆法分配由于没有考虑推力极限的约束，会造成推力分配过饱和的情况.

本文以三推进器水上机器人为研究对象，对其在航迹跟踪控制问题进行研究.首先，运用二次规划方法实现推力分配的优化；其次，选择S面控制方法进行航向航速控制，在此基础上加入基于反馈误差的自适应调整项，提高航向航速控制的准确性，再结合改进的LOS制导算法设计内外环分离的航迹跟踪控制器.最后，通过仿真和水池试验验证航迹跟踪控制器的性能.

1 三推进器水上搜救机器人介绍与数学建模

三推进器水上机器人的外形结构见图1.机器人由三个圆柱浮体构成，浮体下方安装三个固定方向推进器，机器人运动形式类似于轮式机器人，上方圆台是控制箱.通过三个推进器推力的分配，可实现水上机器人自转、回旋、跟踪等多种姿态控制.

图1 水上机器人结构外形图

三推进器水上机器人的运动控制系统框架见图2，分为控制层、通信层、感知层,以及执行层.

图2 水上搜救机器人运动控制系统框图

图3 三推进器水上机器人平面运动示意

水上搜救机器人的运动学模型为

(1)

水上搜救机器人动力建模的水动力阻力项采用线性形式，即所受阻力与速度呈线性关系，则机器人在水平面的水动力方程为

(2)

式中：M为机器人的惯性质量矩阵；C为向心力和科氏力系数矩阵；D为阻力系数矩阵；τ为水上机器人的推力及力矩矩阵.

由于推进器的安装方向为纵向，所以横向上没有推力输出，而式中的系数则通过实船实验数据辨识得到.因此，水上机器人的动力学模型为

(3)

2 航迹跟踪控制器设计

将航迹跟踪控制器设计分为三部分：航向航速控制器、制导律和推力分配三部分，对于冗余的推力分配问题，运用二次规划进行优化；航向航速控制采用改进S面控制方法；在航迹跟踪制导环节，在LOS制导算法加入时变的前世距离，使得机器人实际运动更加灵活，最后通过仿真和水池实验验证控制器的效果.航迹跟踪控制器的基本框架见图4.

图4 航迹跟踪控制器基本框架

2.1 基于二次规划的推力分配

三推进器水上机器人的三个方向的合力(矩)向量为

(4)

由于在横向推力上有三个推进器提供动力，属于冗余推力分配问题，而推进器的布置矩阵是一个非满秩矩阵，无法通过矩阵的逆来进行计算.传统的方法是采用B的伪逆矩阵进行推力的分配，而该方法没有考虑推力幅值的约束因素，容易造成推力分配过饱和现象.因此,本节利用二次规划数学优化的方法，引入性能指标来建立三个推进器的推力分配函数：三个推进器输出的合力(矩)与期望的合力(矩)误差最小化，为

minJ=(Bτ-Td)T(Bτ-Td)

s.t.τmin≤τ≤τmax

(5)

将上式展开

(6)

再令，

(7)

(8)

为验证基于结合二次规划推力分配的有效性，设计了同时用二次规划方法和伪逆法的对比仿真实验.仿真参数见表1.

表1 推力分配参数表

三推进器实际航行中，航向变化较频繁，而航速改变情况较少，设定沿纵向方向的推力不变，绕Z轴的合力矩为正弦信号输入，即Td=[40,0,10+17sin0.25t]，具体的仿真图见图5.

图5 推力分配仿真对比效果图

由推进器2的对比效果图可知，伪逆法求出来的推力超过了推进器最大的幅值，基于二次规划方法的推力分配解决了伪逆法存在的不足，且三个推进器的推力变化较为平滑，符合实际工程运用中的要求.

2.2 航向航速控制

常规S面控制器的控制模型为

(9)

文献[6]将风浪流等影响因素，考虑成在某一段时间内的固定干扰，通过调整S面的偏移来达到消除固定干扰的作用，这样在整体的运动时间内，实现S面自调整的目的.该种策略相当于在控制模型中加入了有条件的积分环节，适合用于平稳的运动控制中，当外界环境或期望值变化较明显时，该种策略的改进效果不明显.

本节针对实际应用中三推进器水上机器人航向航速变化频繁的特点，为解决由于三个推进器安装方向误差、外界干扰对航向航速控制的影响.对S面中的调整项策略做改进，提出一种基于反馈误差的自适应调整策略.航向和航速分别用改进后的S面进行控制.

在传统教学过程中，都是由教师讲授知识和技能，学生一味地跟着学习，简单的采用期末考试来衡量学生成绩的好坏，这些方式都无法激发学生的创新意识。计算机专业是实践性很强的专业，教师通过讲解和演示，学生最多学会了一些技术方法和简单的应用，根本没有创新的意识。学生通过参加大赛，可以激发他们学习的兴趣和潜能，对知识面进行更加深入的学习，从而引导和激发学生的创新意识，逐渐可以提出自己解决问题的新思路和新方法。

改进后的S面表达式式为

(10)

步骤1记录最近N条航向或航速偏差值.

步骤4对当前调整项进行削减.

步骤5如果计算出来的调整项数值大于当前偏差，对调整项进行低通滤波处理.

工程运用中具体表示形式为

(11)

式中：et为t时刻的航向或航速偏差；λ为低通滤波参数；η为调整因子.

(12)

βt为削减因子.

(13)

分析改进后的S面控制器，调整项的加入相当于在整个控制模型中增加了积分环节，提高S面控制对局部小偏差的反应能力，削减环节与低通滤波环节使得调整项的输出更加的平滑.

2.3 航迹制导律

航迹跟踪中最常用的航迹制导律是LOS导航算法，通过将当前位置与期望航迹的距离误差带入制导律中，求得期望航向，水上机器人只需跟踪指令航向即可有收敛的保持航迹.LOS制导策略可分为基于包围圈和前视距离两种，本文采用基于前视距离的LOS制导律来进行优化.基于前视距离的LOS制导原理见图6.

图6 LOS制导原理图

图6中ψr为制导航向，RP距离为LOS制导律的前视距离，一般取值为1.5～2.5倍船长[11].

当机器人距离期望航迹较远时，需要较大的制导角度(即较小的前视距离RP)来较快的接近航迹，而当机器人距离期望航迹较近时，又需要制导航向较小来保证航迹跟踪的平稳.而固定的前视距离很解决上述问题，因此提出了时变前视距离的改进，增加机器人航迹跟踪的灵活性.

RP=(RPmax-RPmin)e-λ1d2+RPmin

(14)

式中：d为当前位置到期望路径的距离;λ1为大于零的常值参数;RPmax为最大前视距离，这里为2.5倍的水上机器人长度;RPmin为最小前世距离，这里为1.5倍水上机器人长度.

3 仿真与水池实验

3.1 仿真分析

通过上一节的数学模型，运用MATLAB软件进行航迹跟踪的数值仿真.在直角坐标系下，定义x轴为正东方向，y轴为正北方向，航向角为沿y轴正半轴顺时针转动的角度，角度范围为[0°,360°).设计航线(0,20)到(30，60)， S面控制器的参数k1=3.5,k2=0.03，定速1 m/s，初始条件：初始位置(0,0)，初始航向角0°，无干扰条件下，见图7.

图7 改进LOS与传统LOS比较

由图7可知，改进的LOS制导算法能更快的跟踪航迹，且时变的前视距离不会引起跟踪过程的振荡.航迹控制器整体具有良好的稳定性、快速性，为实船实验提供了理论支持.

3.2 水池实验

水上机器人的航迹跟踪实船实验在试验水池进行，水面有风的影响.实物与实验场地见图8.

图8 水上机器人实物图

为验证基于改进S面控制的航向航速控制器的实际控制效果，进行常规S面和改进之后的S面控制器的对比试验，设定期望航向300°.期望速度1.0 m/s，实验效果见图9.实验结果表明，常规S面航向控制最终稳定后会与期望值存在一个稳态误差，这个稳态误差的造成是由于推进器安装偏移造成的.而加入自适应调整后的航向保持可以减少稳定误差，使航向航速保持更为准确.最终航向保持误差在1°以内，改进的航向控制器效果较好.图10为航速保持实验图，综合图9～10可知，航向航速保持的响应时间为15 s左右，验证了基于S面控制方法的航向航速控制器在响应时间上有较好的效果.

图9 航向保持

图10 线速度保持

然后，为验证文中设计的航迹跟踪控制器的实际控制效果，进行航迹跟踪实验，分别直线跟踪与曲线跟踪，曲线设为半径为15 m的圆，直线段设定关键点见表2.水上机器人航迹跟踪运动轨迹见图11.

表2 路径规划信息表 m

图11 航迹跟踪运动轨迹

图11b)每段直线跟踪有两端波峰，其中较陡的波峰是由于在转向点处，期望航迹改变造成的，分析航迹误差可忽略.由图11可知，所设计的航迹控制器在实验中可以完成航迹跟踪的目标，说明基于间接航迹跟踪的两点改进是可行的.航迹跟踪在稳定后误差在1 m以内，满足水上搜救机器人执行任务的要求，但转弯处的误差较大，达到3 m，这一点在后续的研究工作中还需要进一步的改进.

4 结 论

1) 实验的效果证明，基于二次规划的推力分配解决了伪逆法输出过饱和的问题，使得控制量的输出不会因为限幅而达不到控制算法的控制效果；针对S面航向航速控制方法的改进，能有效减少固定干扰带来的误差；在LOS制导加入时变的前视距离，能加快水上机器人跟踪航迹的速度，增加航迹跟踪的灵活性.

2) 以三推进器水上搜救机器人为研究对象，将设计的航迹跟踪控制器应用于实验中，为解决水上搜救机器人快速、精确的自主搜救奠定了理论和实验基础.

3) 在下一步的研究中，将结合三推进器水上机器人转向灵活的特点，设计满足回转半径约束的制导律来实现对复杂路径的跟踪.