半潜式水面机器人轨迹跟踪控制方法研究

冯迎宾，于 洋，杜学历

(沈阳理工大学 自动化与电气工程学院，沈阳110159)

水面机器人是一种能够在水面自主运动，并且能够完成指定作业的小型海洋观测平台。水面机器人已被广泛应用于水面搜索、水面环境监测、抢险搜救、反潜作战及水雷探测等领域[1]。水面机器人相关技术研究对于开发海洋资源和保护我国海洋领土具有重要意义。

水面机器人轨迹跟踪控制是机器人在水面自主完成作业任务的前提，一直是科研人员研究的热点问题。杨泽文等针对水下机器人水平面轨迹跟踪控制问题，采用高增益观测器结合反步控制方法设计了欠驱动水下机器人轨迹控制器[2]。赵杰梅等建立了水下机器人动力学模型，提出了神经网络自适应输出反馈轨迹跟踪控制器，仿真验证了算法的有效性[3]。张凯等针对水面无人船外界干扰未知的问题，设计了基于扰动观测器的反步轨迹跟踪控制器[4]。余亚磊等针对无人船系统建模误差和参数不确定等问题，提出了基于视线引导法的反步自适应无人船路径跟踪控制方法[5]。刘志民研究了水下机器人的曲线轨迹跟踪控制方法，采用非线性级联系统理论设计了水下机器人跟踪有限时间控制器[6]。以上这些方法都是基于机器人的动力学模型设计的轨迹跟踪控制器，在仿真试验中取得了良好的轨迹跟踪控制效果。但机器人在海洋运动的过程中难免受到洋流、风浪的影响，很难获得精确的机器人动力学模型，因此这些方法很难应用到实际的水下机器人系统中。

因此，本文提出了基于视觉导引法的水面机器人轨迹跟踪控制策略，利用机器人当前位置和期望航路点计算机器人航向角；同时设计了基于单神经元自适应PID的机器人航向角闭环控制器，该控制器不需要精确的水下机器人动力学模型，具有鲁棒性强的优点，可应用于实际的机器人控制系统中。

1 问题描述

半潜式水面机器人在海平面运动，需克服海面涌大、浪高和海流的影响，载体结构采用流线型的上下双体的分体结构，上浮筒与下浮筒用铝制焊接桥架相连，其外形结构如图1所示。此外，为保证载体在大涌、大浪运动时推进器不被抬出水面，避免所谓的“失控”，载体螺旋桨布置在下浮筒电子舱的左右，并与电子舱布置在一个水平面上。

图1 机器人外形结构

由机器人的结构可知，机器人仅在尾部搭载了两个螺旋桨，通过调节螺旋桨的旋转方向实现机器人前进、后退、旋转运动。但螺旋桨不能给机器人提供横向运动的推力，因此机器人属于欠驱动的运动载体。机器人在水平面的运动方程可描述为

(1)

水面机器人在水平面运动会受到重力、浮力、流体阻力、螺旋桨推力和外界干扰的影响。由于机器人悬浮于水平面，重力与浮力方向相反，大小相等，因此可忽略不计。螺旋桨推力可通过建立的数学模型和控制电压估算，然而流体阻力和风浪、洋流产生的外界干扰很难估算。因此，精确的机器人动力学模型不能被建立。本文要研究的问题可描述为：在未知的机器人动力学模型的前提下，研究机器人轨迹跟踪控制方法。

2 轨迹跟踪控制方法

由于视线导引法不依赖于机器人精确的数学模型，能够在外界扰动的前提下获得良好的轨迹跟踪控制效果，因此被广泛应用于机器人轨迹跟踪。

机器人未来的运动轨迹可用一系列的航路点(Pk-1，Pk，Pk+1，…)表示，如图2所示。在大地坐标系中，水面机器人的位置可表示为G(xt，yt)，机器人的航向角可表示θ(t)。假设，机器人下一个目标点为Pk(xk，yk)，机器人到目标点的距离为d(t)，机器人与轨迹的航向误差为e(t)，GPk与OY轴之间的夹角为β(t)。当机器人到达以Pk为圆心，以R为半径的圆内，说明机器人到达了目标点Pk，此时就要跟踪下一个航路点。航路点之间的距离Δ一般为机器人长度的3～6倍，选择太长会使得路径跟踪效果变差，选择太短会影响机器人控制系统的稳定性。

图2 机器人轨迹跟踪示意图

由图中的几何关系可知

(2)

β(t)=90°-arctan((yk-yt)/(xk-xt))

(3)

基于视线导引法的水面机器人轨迹跟踪控制策略步骤如下。

(1)首先设定航路点。根据规划的机器人运动轨迹和航路点之间距离Δ值生成航路点(Pk-1，Pk，Pk+1，…)。

(2)根据机器人当前位置计算距离第k个航路点的距离d(t)和偏航角β(t)。

(3)机器人自动调整航向角，使其等于偏航角，然后机器人以恒定的速度向第k个航路点运动。

(4)在运动过程中，实时计算当前位置与第k个航路点的偏航角β(t)，当偏航角大于角度阈值φ时，重新计算机器人航向角，调整机器人运动方向。

(5)实时计算机器人当前位置与第k个航路点的距离d(t)，当d(t)

(6)重复步骤(2)～(5)，直到机器人到达最后航路点为止。

由机器人轨迹跟踪控制策略可知，航向角的控制精度决定了机器人轨迹跟踪的控制效果。

3 航向角控制方法

由于水面机器人在运动过程中需要面对动态、未知、复杂的外部扰动和噪声，因此水面机器人航向控制是科研领域的难点问题。夏云鹏等针对水面无人船易受风浪流等扰动导致航向控制困难问题，设计了基于预测函数控制的水面无人船航向控制器[7]。Han J D等针对水面机器人扰动未知的问题，利用无迹卡尔曼滤波算法在线估计风、水流等外界干扰造成的扰动，提高机器人建模精度，并设计了基于干扰的前馈控制器[8]。熊俊峰等针对水面机器人航向跟踪易受外界干扰的问题，提出了基于线性变参模型的H无穷鲁棒航向跟踪控制器[9]。以上算法都是基于机器人的动力学模型提出，虽然在实验室仿真平台取得了良好的控制效果，并没有在真正的机器人平台得到广泛的应用。PID控制器具有原理简单、参数易整定等优点被广泛应用于工程过程控制和机器人控制领域[10]。传统的PID控制器参数整定完毕后不能调整、抗干扰能力差，因此，不适宜应用在动态特性变化的水面机器人控制系统中。神经元PID控制器可根据给定信号与测量信号的差值动态改变比例、积分和微分控制器的权值，以适应外界干扰造成的系统动态变化，保证控制系统的稳定性。本文采用单神经元PID控制器来实现机器人航向角的闭环控制。

神经元数学模型如图3所示，x1、x2、xn表示神经元输入；w1、w2、wn表示神经元的权系数；n表示神经元输入状态维数；u表示神经元输出；K表示神经元比例系数。

图3 神经网络模型

增量式PID控制器可表示为

(4)

式中：e(k)表示系统误差；kP、kI、kD分别表示PID控制器的比例增益、积分和微分系数。Δe(k)、Δ2e(k)可表示为

Δe(k)=e(k)-e(k-1)

(5)

Δ2e(k)=Δe(k)-Δe(k-1)

(6)

由式(4)、式(5)、式(6)可知，增量式PID不需要计算累计误差，控制增量仅与临近三次的采样值有关。

假设e(k)、Δe(k)、Δ2e(k)作为神经网络的输入，将神经网络和增量式PID控制器相结合构成单神经元自适应控制器，其控制结构如图4所示。

图4 机器人航向角控制系统

单神经元自适应控制器通过调整加权系数来实现控制器的自适应、自组织功能。权系数wi(k)的调整根据有监督的Hebb学习规则实现[11-12]。Hebb学习规则可归纳为“如果在某一突触每一边的两个神经元被同步激活，那么突触的强度被选择性的增强，反之则被选择性地减弱或消除”。控制算法及学习算法可表示为

(7)

(8)

w1(k)=w1(k-1)+ηIz(k)u(k)x1(k)

(9)

w2(k)=w2(k-1)+ηPz(k)u(k)x2(k)

(10)

w3(k)=w3(k-1)+ηDz(k)u(k)x3(k)

(11)

式中：x1(k)=e(k)；x2(k)=Δe(k)；x3(k)=Δ2e(k)；z(k)=e(k)；ηP、ηI、ηD分别为积分、比例、微分的学习速率；K>0为神经元的比例系数。

4 试验分析

为验证机器人航向角控制算法和轨迹跟踪控制算法的有效性，机器人在沈阳棋盘山水库做了试验测试。首先，进行机器人航向角控制算法测试，机器人初始角度150°，t<23s时期望航向角60°，t>23s时期望航向角160°。机器人采用单神经元PID控制器完成机器人航向角的闭环控制，控制器参数设置为：ηP=8、ηI=3、ηD=15、K=0.002，试验结果如图5所示。

图5 机器人航向角动态调节过程

从图5中可以看出，航向角稳态误差可控制在3°以内，超调量控制在25°以内，调节时间约为15s，机器人航向角可跟踪真实值。

在机器人航向角闭环控制试验的基础上，开展机器人轨迹跟踪控制试验。在控制试验中参数设置如下：航向角偏差阈值φ=4°，半径R=5m，机器人当前位置与目标点位置小于5m时，表明机器人到达了目标点。机器人直线轨迹跟踪效果如图6所示。

图6 机器人直线轨迹跟踪示意图

目标轨迹长度466m，机器人首先根据公式(3)计算机器人偏航角β(t)，调整机器人运动方向，到达轨迹起点，然后根据目标轨迹生成的航路点，沿着目标轨迹向终点运动，轨迹跟踪误差可控制在3m内。

当目标轨迹是长方形时，轨迹跟踪算法参数设置与前文相同，轨迹跟踪示意图如图7所示。

由图7可知，机器人在跟踪目标轨迹的过程中，存在横向偏差，最大轨迹跟踪误差4m。这是由于机器人仅能依靠调整航向角跟踪轨迹，机器人不具备横向移动能力，不能抵挡横向水流、风浪扰动的影响，横向偏差从原理上不能消除。

图7 机器人长方形轨迹跟踪示意图

5 结论

分析了欠驱动水面机器人轨迹跟踪控制问题，提出了一种基于视线导引法的水面机器人轨迹跟踪控制方法，该方法不需要精确的水面机器人动力学模型。设计了基于单神经元自适应PID控制器，可实现对机器人航向角的闭环控制，并在实际系统进行了验证。试验结果表明，在未知机器人动力学模型的情况下，机器人可以实现航向角的闭环控制和轨迹跟踪控制。