CN104199447A - 水下结构物检测机器人控制系统及运动控制方法 - Google Patents

Abstract

本发明公开了一种水下结构物检测机器人控制系统及运动控制方法。水面系统包含水面控制台，水面通信收发器，脐带缆，水下系统包含水下通信收发器，供电单元，嵌入式微控制器，动力推进单元，视觉照明单元，运动切换单元，安全保护单元，传感器单元。各功能模块采用模块化设计，方便安装与拆卸，水面控制台具有紧急拍停功能，配备了无线连接的平板电脑，可以实现移动化监控；通信方式多样，数据量丰富；运动切换单元支持浮游和爬行两种运动方式切换；拥有数据存储、电压电流检测等可靠的安全保护。本发明基于广义预测PID控制算法的运动控制方法，有效减少系统能源消耗，降低元器件负载，更加方便和稳定地控制机器人。

Description

水下结构物检测机器人控制系统及运动控制方法

技术领域

本发明涉及一种水下结构物检测机器人，特别是一种水下结构物检测机器人的控制系统和运动控制方法，属于机器人技术领域。

背景技术

国外很早就开始了对水下结构物检测机器人的研究，而且也取得了一定的研究成果。如美国生产的大型水下机器人，SUPER SCORPIO、SUPER PHANTOM、TRITON等都成功地用于海上石油钻井平台设备的安装、检测、辅助及维护水下生产和常规作业。法国国家海洋开发中心与一家公司合作，共同建造“埃里特”声学遥控潜水器。用于水下钻井机检查、海底油机设备安装、油管铺设、锚缆加固等复杂作业。英国从上个世纪九十年代初开始，开展了用于海洋石油开发水下检测的自动远程操纵系统项目研究开发。该系统布于近海现场，用于检测Mobile公司的Beryl Bravo平台。挪威开发的REMO是一种基于遥控潜水器的水下检测机器人，用于复杂钢管节点焊缝的清洁与检测，近年来，随着用于消除焊缝裂纹的机器人系统的发展，REMO系统的性能也已经大大发展。

在我国对于水下结构物检测机器人的研究还只能说是起步不久，相关的经验不是很丰富，国产水下机器人(ROV)主要集中在上海交通大学、中国科学院沈阳自动化研究所等几家单位，从事应用型ROV产品开发的公司较少，而且探测技术、工艺水平、导航与定位、复杂环境中的运动控制等方面与国外存在较大差距。

随着计算机技术、GPS定位技术及通讯传感技术的快速发展，远程智能水下结构物检测机器人在复杂水下环境中的观察能力、顶流作业能力、高精度的运动控制及定位能力等均需要进一步提高，同时通过改善水面支持系统与水下观察作业系统之间的人机交互界面，加大数据处理容量，全面提高水下结构物检测机器人操作控制水平和操作性能是发展趋势之一。申请号为201110373831.X，名称为“一种复合吸附船体清刷机器人”，其只能实现水下行走功能，功能单一；申请号为200310105200，名称为“一种分布式水下机器人控制系统”，它仅仅采用了一路RS-485网络通信，可靠性低，数据传输量也少；申请号为201310357116.6名称为“一种浅水用小型水下机器人甲板的控制装置”，没有紧急拍停按钮，遇到特殊情况处置不方便，也没有配备平板电脑，不能实现移动监控机器人，不方便其他人员观察机器人作业。

发明内容

本发明的目的在于提供一种水下结构物检测机器人控制系统及运动控制方法，解决现有技术缺陷，提供一种能够传输多种信号，带有紧急拍停按钮，平板电脑显示，运动切换单元，更强的安全保护，可以实现浮游和爬行两种功能的模块化控制系统，以及方便可靠、低消耗的运动控制方法。

本发明的目的通过以下技术方案予以实现：

一种水下结构物检测机器人控制系统，包括水面系统、水下系统，所述水面系统放置在岸边或者母船上，水下系统安装在水下结构物检测机器人上；所述水面系统包括水面控制台1、水面通信收发器2、脐带缆3，所述水面控制台1与水面通信收发器2相连，所述水面通信收发器2与脐带缆3相连；

所述水下系统包括水下通信收发器4、供电单元5、嵌入式微控制器6、动力推进单元7、视觉照明单元8、运动切换单元9、安全保护单元10、传感器单元11；所述水下通信收发器4与脐带缆3相连，接收来自水面系统的控制信息，所述嵌入式微控制器6与水下通信收发器4相连，所述供电单元5、动力推进的单元7、控制视觉照明单元8、运动切换单元9、安全保护单元10、传感器单元11分别与嵌入式微控制器6相连，所述供电单元5给水下系统供电，动力推进单元7接收嵌入式微控制器6的速度和方向信息，驱动螺旋桨旋转，视觉照明单元8采集视频以及接收来自嵌入式微控制器6的云台和照明灯信号，运动切换单元9用以改变水下机器人的运动方式，安全保护单元10检测机器人内部环境信息发送给嵌入式微控制器6，并作数据存储，传感器单元11采集深度、导航、姿态、声呐数据传输给嵌入式微控制器6进行控制；所述嵌入式微控制器6采集水下系统各单元数据，经由水下通信收发器4发送到水面系统。

一种水下结构物检测机器人运动控制方法，该方法使用广义预测PID控制算法对机器人的深度和航向角进行控制以提高机器人的稳定性，具体步骤如下：

第一步：水下机器人采用受控自回归积分滑动平均模型，即CARIMA模型描述：

$A\left(z^{-1}\right)y\left(t\right)=B\left(z^{-1}\right)u(t-1)+\frac{C\left(z^{-1}\right)}{\mathrm{\Δ}}e\left(t\right)---\left(1\right)$

其中A(z^-1)、B(z^-1)、C(z^-1)是后移算子z^-1的多项式，y(t)＝[h，yaw]^T是t时刻深度h和航向角yaw的矩阵，u(t-1)是t-1时刻推进器输入电压，Δ＝1-z^-1表示差分算子，e(t)为均值为零的白噪声序列；

令C(z^-1)＝1单位矩阵，(1)式变为：

$A\left(z^{-1}\right)y\left(t\right)=B\left(z^{-1}\right)u(t-1)+\frac{e\left(t\right)}{\mathrm{\Δ}}---\left(2\right)$

式中， $A\left(z^{-1}\right)=I_{n\×n}+A_1{z}^{-1}+A_2{z}^{-2}+...+A_{n_a}{z}^{{-n}_a},$

$B\left(z^{-1}\right)=B_0+B_1{z}^{-1}+B_2{z}^{-2}+...+B_{n_b}{z}^{{-n}_b},$

其中是模型系数，n_a，n_b分别为输出、输入的后移算子多项式阶次，对于(2)式方程，引入Diophantine丢番图方程：

1＝E_j(z^-1)A(z^-1)Δ+z^-jF_j(z^-1)

式中，E_j(z^-1)＝e_j0+e_j1z^-1+…+e_j，j-1z^-j+1，e_j0＝1

F_j(z^-1)＝f_j0+f_j1z^-1+…+f_j，nz^-n

用(1)式求出k时刻后j步的输出预测方程(3)：

y(k+j)＝G_j(z^-1)Δu(k+j-1)+y_m    (3)

其中y(k+j)为k+j时刻深度和航向角预测输出，Δu(k+j-1)是推进器增量，其中令G_j(z^-1)＝E_j(z^-1)B_j(z^-1)，y_m＝F_j(z^-1)y(k)；

第二步：选取以下初始值：遗忘因子ρ，ρ取0.95～1；输出柔化系数α，α取0.75；输入柔化系数β，β取0.4；正定矩阵P(-1)；系数矩阵θ(0)；

第三步：在线估计A(z^-1)、B(z^-1)：

令ε(k)＝Δy(k)-X(k-1)^Tθ(k-1)，X(k-1)为过去时刻输入，Δy(k)为当前输出变化，

模型的系数 $\mathrm{\θ}\left(k\right)=\mathrm{\θ}(k-1)+\frac{P(k-1)X(k-1)\mathrm{\ϵ}\left(k\right)}{\mathrm{\ρ}+X{(k-1)}^{T}P(k-2)X(k-1)}$

其中 $\mathrm{\θ}\left(k\right)=[a_0\left(k\right),a_1\left(k\right),...,a_{n_a}\left(k\right),b_0\left(k\right),b_1\left(k\right),...,b_{n_b}\left(k\right){]}^T,$ 由此得到变化后的A(z^-1)、B(z^-1)；

第四步：递推E_j(z^-1)、F_j(z^-1)：

E_j+1(z^-1)＝E_j(z^-1)+e_jz^-j

$F_{j+1}\left(z^{-1}\right)=z(F_j\left(z^{-1}\right)-e_j\stackrel{\‾}{A})$

式中，E_j(z^-1)、F_j(z^-1)表示不断更新的两个变量；

第五步：计算G_j(z^-1)、y_m；

根据上述推算可求得：

G_j(z^-1)＝E_j(z^-1)B_j(z^-1)

y_m＝F_j(z^-1)y(k)

第六步：引入输入柔化系数β；

$\mathrm{\Δu}(t+k)=(1+\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{\mathrm{\β}}^i)\mathrm{\Δu}\left(k\right)---\left(4\right)$

令β≥0且β≤1，最优取为0.4；

从而，式(3)变为y(k+j)＝G_j(z^-1)HΔu(k)+y_m＝LΔu(k)+y_m

式中，H＝[1，1+β，...，1+β+…β^n-1]^T，L＝G_j(z^-1)H＝[l₁ l₂ ... l_n-1]^T，n为预测步长与时滞数之差；

第七步：对输入u(k+j)、Δu(k+j)添加约束条件：

u_min≤u(k+j)≤u_max，Δu_min≤Δu(k+j)≤Δu_max

第八步：设定参考轨迹y_r(k+1)：

系统输出不直接跟踪设定值r(k)，而是跟踪参考轨迹y_r(k+1)，采用如下一阶滤波：

y_r(k)＝y(k)

y_r(k+1)＝αy_r(k)+(1-α)r(k)

第九步：求取目标函数Δu(k)，控制量u_a(k+1)；

在上述约束条件下，取极小化性能指标

J＝min{(y_r(k)-y(k))^T(y_r(k+1)-y(k+1))}＝min{(y_r(k+1)-y_m-LΔu)^T(y_r(k+1)-y_m-LΔu)}

求得

Δu(k)＝(L^TL)^-1L^T(y_r(k+1)-y_m)＝L^T(y_r(k+1)-y_m)/∑l²

控制量为u_a(k+1)＝u(k)+Δu(k)    (6)

第十步：PID控制过程，以Δy(k)作为PID输入，计算出下一刻控制量u_b(k+1)；

u_b(k+1)＝u(k)+K_p(Δy₂-Δy₁)+K_iΔy₂+K_d(Δy₂-2Δy₁+Δy₀)   (7)

式中，Δy₂＝y(k+1)-y(k)，Δy₁＝y(k)-y(k-1)，Δy₀＝y(k-1)-y(k-2)，K_p、K_i、K_d分别为比例、积分、微分系数；

第十一步：对(6)式、(7)式计算的控制量均值滤波，求出水下机器人下一时刻推进器的输入u(k+1)；

u(k+1)＝(u_a(k+1)+u_b(k+1))/2

所求u(k+1)即为水下机器人下一时刻推进器的输入；

第十二步：t＝t+1，返回到第三步，更新各项数据。

本发明的目的还可以通过以下技术措施来进一步实现：

前述水下结构物检测机器人控制系统，其中水面控制台1包括水面控制电脑102、紧急拍停开关103、操纵摇杆104、键盘鼠标105、液晶显示器106、平板电脑107；所述紧急拍停开关103与水面控制电脑102相连，在紧急情况下按下紧急拍停开关103，系统切断电源停止控制；操纵摇杆104与水面控制电脑102相连，操纵机器人前进、后退、旋转、上浮、下潜动作，键盘鼠标105与水面控制电脑102相连，输入控制信息；液晶显示器106实时显示水面控制电脑102安装的上位机软件界面，平板电脑107通过无线通信与水面控制电脑102连接，移动访问机器人控制信息，辅助非操作人员决策；水面控制电脑102与水面通信收发器2相连，进行数据收发。

前述水下结构物检测机器人控制系统，其中动力推进单元7包括光电隔离模块701、电机驱动模块702、吸附推进器电机703、首部垂向电机704、尾部垂向电机705、左侧纵向电机706、右侧纵向电机707、吸附螺旋桨708、首部垂向螺旋桨709、尾部垂向螺旋桨710、左侧纵向螺旋桨711、左主动车轮712、右侧纵向螺旋桨713、右主动车轮714；所述光电隔离模块701的输出端接电机驱动模块702的输入端，电机驱动模块702的输出端接吸附推进器电机703、首部垂向电机704、尾部垂向电机705、左侧纵向电机706、右侧纵向电机707，所述吸附推进器电机703驱动吸附螺旋桨708，所述首部垂向电机704驱动首部垂向螺旋桨709，所述尾部垂向电机705驱动尾部垂向螺旋桨710，所述左侧纵向电机706驱动左侧纵向螺旋桨711、左主动车轮712，所述右侧纵向电机707驱动右侧纵向螺旋桨713、右主动车轮714；所述嵌入式微控制器6发出的速度信号PWM和方向信号DIR，通过光电隔离模块701进行信号隔离，将隔离后的信号传给电机驱动模块702，电机驱动模块702输出端控制吸附推进器电机703、首部垂向电机704、尾部垂向电机705、左侧纵向电机706、右侧纵向电机707的速度和方向来控制吸附螺旋桨708、首部垂向螺旋桨709、尾部垂向螺旋桨710、左侧纵向螺旋桨711、左主动车轮712、右侧纵向螺旋桨713、右主动车轮714的旋转。

前述水下结构物检测机器人控制系统，其中运动切换单元9包括上部磁性接近开关901、下部磁性接近开关902、步进电机903、磁铁904、滑台905、丝杠906、导轨907；所述左侧纵向电机706和右侧纵向电机707安装在一个可以垂直活动的滑台905上，步进电机903与滑台905通过丝杠906连接，可以驱动滑台905沿导轨907上下垂直移动，上部磁性接近开关901与下部磁性接近开关902分别安装在固定滑台架子的上部和下部，通过滑台上部和下部的磁铁904检测滑台905位置，判断切换爬行和浮游状态，实现运动切换功能。

前述水下结构物检测机器人控制系统，其中安全保护单元10包括电压电流检测模块1001、漏水检测模块1002、温湿度计1003、数据存储卡1004；所述电压电流检测模块1001、漏水检测模块1002、温湿度计1003、数据存储卡1004与嵌入式微控制器6连接。

前述水下结构物检测机器人控制系统，其中传感器单元11包括深度传感器1101、导航定位模块1102、姿态传感器1103、声呐传感器1104；所述深度传感器1101采集机器人深度信息，当机器人浮在水面时用导航定位模块1102确定和校正位置，所述姿态传感器1103实时采集机器人姿态信息，包括横滚角、俯仰角、航向角，所述声呐传感器1104采集水下结构物信息，所述深度传感器1101、导航定位模块1102、姿态传感器1103、声呐传感器1104与嵌入式微控制器6相连，将数据传给嵌入式微控制器6。

与现有技术相比，本发明的有益效果是：本发明的水下结构物检测机器人了控制系统采用模块化设计，方便安装与拆卸；水面控制台具有紧急拍停功能，配备了无线连接的平板电脑，可以实现移动化监控；通信系统支持485、以太网、差分视频传输；运动切换单元支持浮游和爬行两种运动方式切换；本发明使用一种基于广义预测PID控制算法的运动控制方法，有效减少系统能源消耗，降低元器件负载，更加方便和稳定地控制机器人。

附图说明

图1为本发明机器人系统结构框图；

图2为本发明机器人水面控制台结构示意图；

图3为本发明机器人水面、水下通信收发器结构框图；

图4为本发明机器人供电单元结构示意图；

图5为本发明机器人动力推进单元结构框图；

图6为本发明运动切换单元结构图；

图7为本发明运动控制方法原理框图；

图8为本发明运动控制方法的算法流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

本发明的水下结构物检测机器人控制系统，如图1所示，系统包括水面和水下两个系统，水面系统放置在岸边或者母船上，水下系统安装在水下结构物检测机器人上；其中水面系统包括水面控制台1，水面通信收发器2，脐带缆3，他们之间用电缆连接，水面控制台1发送接收信号，水面通信收发器2转发来自水面、水下系统的信息，脐带缆3传递信号和电能；水下系统包括水下通信收发器4，供电单元5，嵌入式微控制器6，动力推进单元7，视觉照明单元8，运动切换单元9，安全保护单元10，传感器单元11。

图1中嵌入式微控制器6分别与水下通信收发器4、供电单元5、动力推进的单元7、控制视觉照明单元8、运动切换单元9安全保护单元10、传感器单元11相连。嵌入式微控制器6选择是基于ARM Cortex-M4内核的微控制器，包括有定时器、模数转换器、存储器、串行通信模块、以太网控制、通用输入输出口，系统上电后，嵌入式微控制器6首先对各模块功能进行初始化，完成后向水面系统发送一个自检完成的信号，之后进入从机状态，接收水面系统的指令，并作出相应控制。嵌入式微控制器6接收安全保护单元10内部的电压电流检测模块1001、漏水检测模块1002、温湿度及1003信号，定时把关键运行信息写入数据存储卡。嵌入式微控制器6接收来自传感器单元11的深度、位置、姿态、声呐数据，作为运动控制的反馈信息，根据这些信息配合运动控制方法输出运动控制信号到动力推进单元7，实现机器人的前进、后退、旋转、上浮、下潜，如果要进行运动切换，还需要运动切换单元9配合。视觉照明单元8接收来自嵌入式微控制器6的云台和照明灯信号，云台802通过PWM信号控制，实现三维空间运动，调节水下摄像机801的视角，水下摄像机801使用CCD摄像头采集视频，水下照明灯803通过继电器实现开关。

安全保护单元10包括电压电流检测模块1001、漏水检测模块1002、温湿度计1003、数据存储卡1004；电压电流检测模块1001可以检测水下系统电源的电压电流值，漏水检测模块1002可以采集密封舱内四个点的漏水情况，温湿度计1003监测舱内环境的温度和湿度值，以上信息一旦超过阀值，嵌入式微控制器6都会接收到信号，并做出相应处理，同时定时将关键运行信息写入数据存储卡1004方便出故障时分析故障原因。

所述传感器单元11包括深度传感器1101、导航定位模块1102、姿态传感器1103、声呐传感器1104。深度传感器1101采集的机器人深度信息，当机器人浮在水面时用导航定位模块1102确定和校正位置，姿态传感器1103实时采集机器人姿态信息，包括横滚角、俯仰角、航向角，声呐传感器1104采集水下结构物信息，它们都把数据传给嵌入式微控制器6。

如图2所示，水面控制台1包括水面控制电脑102、紧急拍停开关103、操纵摇杆104、键盘鼠标105、液晶显示器106、平板电脑107。水面控制电脑102装载上位机软件，软件使用VB开发；紧急拍停开关103与水面控制电脑102相连，在紧急情况下只需要拍下这个开关，系统就会切断电源，停止控制；操纵摇杆104和键盘鼠标105插在水面控制电脑102上，来操纵机器人前进、后退、旋转、上浮、下潜等运动，以及输入控制信息；液晶显示器106实时显示上位机软件界面，平板电脑107通过无线WiFi与水面控制电脑102连接，可以移动访问机器人信息，辅助其他人员观察机器人作业；水面控制电脑102与水面通信收发器2相连，进行数据收发。

如图3(a)所示，水面通信收发器2包括一号USB 201、二号USB 202、网口203、USB转485模块204、USB视频采集卡205、水面通信接口206。一号USB 201与USB转485模块204相连，传递RS0485信号，二号USB 202与USB视频采集卡205相连，接收差分视频信号，水面通信接口206分出三路信号与网口203、USB转485模块204、USB视频采集卡205相连，实现一个接口传递三种信号，方便安装与插拔。如图3(b)，水下通信收发器4包括水下通信接口401、485转TTL模块402、以太网模块403、视频传输器404。水下通信接口分出三路信号分别与485转TTL模块402、以太网模块403、视频传输器404相连。485转TTL模块402可以将接收的差分信号转变成嵌入式微控制器6能识别的TTL电平信号，以太网模块403用来控制大容量以太网数据的收发，视频传输器404将PAL模拟视频信号差分传输，减少信号干扰，这三个模块信号最后都与嵌入式微控制器6连接。

如图4所示，供电单元5包括24V直流电源501、保险丝502、24V转24V隔离电源503、24V转12V模块504、12V转3.3V模块505、12V转6V模块506、24V电机电源507、24V转5V模块508。供电单元5将脐带缆输出的24V直流电源501分为两路，一路作为小功率系统用电，先接保险丝502保护电路，通过24V转24V隔离电源503将供电电源与小功率系统电源电气隔离开来，隔离后的24V电源给水下照明灯803供电；再接24V转12V模块504，降压得到12V电压给水下摄像机801、458转TTL模块402、深度传感器1101供电；最后通过12V转3.3V模块505得到3.3V电压，给嵌入式微控制器6、安全保护单元10、传感器单元11供电，通过12V转6V模块506得到6V电压，给云台802供电；另一路作为电机用电，24V电机电源507接在24V直流电源501上，给推进器电机和步进电机903供电，在24V电机电源507上接24V转5V模块508，降压得到5V电源，给驱动逻辑芯片和磁性接近开关供电。

如图5所示，动力推进单元7包括光电隔离模块701、电机驱动模块702、吸附推进器电机703、首部垂向电机704、尾部垂向电机705、左侧纵向电机706、右侧纵向电机707、吸附螺旋桨708、首部垂向螺旋桨709、尾部垂向螺旋桨710、左侧纵向螺旋桨711、左主动车轮712、右侧纵向螺旋桨713、右主动车轮714。嵌入式微控制器6发出的速度信号PWM和方向信号DIR，通过光电隔离模块701进行信号隔离，将隔离后的信号传给电机驱动模块702，电机驱动模块702输出端接着吸附推进器电机703、首部垂向电机704、尾部垂向电机705、左侧纵向电机706、右侧纵向电机707，通过控制它们的速度和方向来控制吸附螺旋桨708、首部垂向螺旋桨709、尾部垂向螺旋桨710、左侧纵向螺旋桨711、左主动车轮712、右侧纵向螺旋桨713、右主动车轮714的旋转。其中左侧纵向螺旋桨711和左主动车轮712动力都由左侧纵向电机706提供，由运动切换单元9实现动力变换，右侧纵向螺旋桨713和右主动车轮714动力由右侧纵向电机707提供，也由运动切换单元9实现动力变换。

如图6所示，运动切换单元9包括上部磁性接近开关901、下部磁性接近开关902、步进电机903。机器人具有运动切换功能，可以实现爬行和浮游两种功能，左侧纵向电机706和右侧纵向电机707安装在一个可以垂直活动的滑台905上，步进电机903与滑台905通过丝杠906连接，可以驱动滑台905上下垂直移动，上部磁性接近开关901与下部磁性接近开关902安装在固定滑台架子的上部和下部，滑台905上部和下部安装有磁铁904，磁性接近开关遇到磁铁会输出信号，嵌入式微控制器6驱动步进电机903旋转丝杠移动滑台，同时检测磁信号感知滑台位置，判断切换状态。例如，当滑台在上面，这时上部磁性接近开关901检测到安装在滑台上部的磁铁，这时处于浮游状态，左侧纵向电机706与左侧纵向螺旋桨711齿轮啮合，右侧纵向电机707与右侧纵向螺旋桨713齿轮啮合，当需要切换为爬行状态时，嵌入式微控制器6控制步进电机903旋转，滑台向下移动，当下部磁性接近开关902检测到滑台下部磁铁时，运动切换完成，此时左侧纵向电机706和右侧纵向电机707分别与左主动轮712和右主动轮714的齿轮啮合，可以实现爬行功能。

如图7所示，为本发明的运动控制方法原理框图，机器人运动控制使用广义预测PID控制算法对机器人的深度和航向角进行精确控制来提高机器人的稳定性，减少系统能源消耗。算法基于三项基本原则：预测模型、滚动优化、反馈校正。基本方法包括推导输出预测方程、选取初始值、在线估计Diophantine方程的递推求解、设定参考轨迹、求取目标函数、PID控制等。本发明运动控制方法的算法流程图如图7所示，具体步骤如下：

1)水下机器人采用受控自回归积分滑动平均模型，即CARIMA模型描述：

$A\left(z^{-1}\right)y\left(t\right)=B\left(z^{-1}\right)u(t-1)+\frac{C\left(z^{-1}\right)}{\mathrm{\Δ}}e\left(t\right)---\left(1\right)$

其中A(z^-1)、B(z^-1)、C(z^-1)是后移算子z^-1的多项式，y(t)＝[h，yaw]^T是t时刻深度h和航向角yaw的矩阵，u(t-1)是t-1时刻推进器电压，Δ＝1-z^-1表示差分算子，e(t)为均值为零的白噪声序列。

通常令C(z^-1)＝I单位阵，变为

$A\left(z^{-1}\right)y\left(t\right)=B\left(z^{-1}\right)u(t-1)+\frac{e\left(t\right)}{\mathrm{\Δ}}---\left(2\right)$

式中， $A\left(z^{-1}\right)=I_{n\×n}+A_1{z}^{-1}+A_2{z}^{-2}+...+A_{n_a}{z}^{{-n}_a},$

$B\left(z^{-1}\right)=B_0+B_1{z}^{-1}+B_2{z}^{-2}+...+B_{n_b}{z}^{{-n}_b},$

是模型系数，n_a，n_b取决于模型系数

对于(2)式方程，引入Diophantine丢番图方程：

1＝E_j(z^-1)A(z^-1)Δ+z^-jF_j(z^-1)

式中，E_j(z^-1)＝e_j0+e_j1z^-1+…+e_j，j-1z^-j+1，e_j0＝1

F_j(z^-1)＝f_j0+f_j1z^-1+…+f_j，nz^-n

式(1)两边同时乘以E_jΔ，并利用Diophantine方程得到时刻k后j步的预测方程为：

y(k+j)＝E_j(z^-1)B(z^-1)Δu(k+j-1)+F_j(z^-1)y(k)+F_j(z^-1)w(k+j)

令G_j(z^-1)＝E_j(z^-1)B(z^-1)，则有

$G_j\left(z^{-1}\right)=E_j\left(z^{-1}\right)B\left(z^{-1}\right)=\frac{B\left(z^{-1}\right)}{A\left(z^{-1}\right)}[1-z^{-j}{F}_j\left(z^{-1}\right)]$ $=g_0+g_1{z}^{-1}+...+g_j{z}^{-j}$

上式可以简记为：

y(k+j)＝G_j(z^-1)Δu(k+j-1)+F_j(z^-1)y(k)+E_j(z^-1)w(k+j)

对未来输出预计时忽略未来白噪声影响，得到

$\hat{y}(k+j)=G_j\left(z^{-1}\right)\mathrm{\Δu}(k+j-1)+F_j\left(z^{-1}\right)y\left(k\right),(j=\mathrm{1,2},...,n)$

推导出输出预测方程 $\hat{y}(k+j)=G_j\left(z^{-1}\right)\mathrm{\Δu}(k+j-1)+y_m---\left(3\right)$

其中为k+j时刻深度和航向角预测输出，Δu(k+j-1)是推进器增量，G_j(z^-1)＝E_j(z^-1)B_j(z^-1)，y_m＝F_j(z^-1)y(k)是j时刻输出中已知量部分。

以下计算全部根据(3)式展开。

2)选取初始值：遗忘因子ρ(一般取0.95～1，可以加强新数据作用，削弱老数据)、输出柔化系数α(0.75左右)、输入柔化系数β(0.4左右)、足够大的正定矩阵P(-1)、系数矩阵

3)在线估计

考虑到被控对象可能慢时变的情况选取具有遗忘因子的递推最小二乘法。过去时刻输出、输入记为：

X(k-1)^T＝[-Δy(k-1)，...，-Δy(k-n_a)，Δu(k-1)，...Δu(k-n_a)]

正定阵递推如下：

$P(k-1)=\frac{1}{\mathrm{\ρ}}[P(t-2)-\frac{P(k-2)X(k-1)X{(k-1)}^{T}P(t-2)}{\mathrm{\ρ}+X{(t-1)}^{T}P(k-2)X(k-1)}]$

令 $\mathrm{\ϵ}\left(k\right)=\mathrm{\Δy}\left(k\right)-{X(k-1)}^T\widehat{\mathrm{\θ}}(k-1),$

模型系数 $\widehat{\mathrm{\θ}}\left(k\right)=\widehat{\mathrm{\θ}}(k-1)+\frac{P(k-1)X(k-1)\mathrm{\ϵ}\left(k\right)}{\mathrm{\ρ}+X{(k-1)}^{T}P(k-2)X(k-1)}$

其中 $\widehat{\mathrm{\θ}}\left(k\right)=\left[\begin{array}{cccc}{A}_0& A_1...A_{n_a}& B_0& B_1...B_{n_b}\end{array}\right],$ 由此得到

4)递推E_j(z^-1)、F_j(z^-1)，按如下公式推算F_j(z^-1)、F_j(z^-1)：

$e_j={\hat{f}}_j\left(0\right)$

${\hat{E}}_{j+1}={\hat{E}}_j+e_j{z}^{-j}$

${\hat{F}}_{j+1}=z({\hat{F}}_j-e_j\stackrel{\‾}{A})$

式中， $\stackrel{\‾}{A}=\hat{A}\left(z^{-1}\right)\mathrm{\Δ}$

5)计算G_j、y_m

根据上述推算可得：

$G_j\left(z^{-1}\right)={\hat{E}}_j\left(z^{-1}\right){\hat{B}}_j\left(z^{-1}\right)$

$y_m={\hat{F}}_j\left(z^{-1}\right)y\left(k\right)$

6)为避免系统输入变化过于剧烈，引入输入柔化系数：

$\mathrm{\Δu}(t+k)=(1+\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{\mathrm{\β}}^i)\mathrm{\Δu}\left(k\right)---\left(4\right)$

β≥0，一般β≤1较好。

从而式(3)变为 $\hat{y}(k+j)=G_j\left(z^{-1}\right)\mathrm{H\Δu}\left(k\right)+y_m=\mathrm{K\Δu}\left(k\right)+y_m$

式中，H＝[1，1+β，...，1+β+…β^n-1]^T，L＝G_j(z^-1)H＝[l₁ l₂ ... l_n-1]^T，n为预测步长与时滞数之差。

7)对输入添加约束条件：

u_min≤u(k+j)≤u_max，Δu_min≤Δu(k+j)≤Δu_max

由(4)式可得

u_min p≤Δu(k)≤u_max p，Δu_min p≤Δu(k)≤Δu_max p   (5)

其中

u_min p＝(u_min-u(k-1))/(1+∑β)

u_max p＝(u_max-u(k-1))/(1+∑β)

Δu_min p＝Δ_min/(1+∑β)

Δu_max p＝Δu_max/(1+∑β)

式(5)应表示为

max{u_min p，Δu_min p}≤Δu(k)≤min{u_max p，Δu_max p}

8)设定参考轨迹：

系统输出不直接跟踪设定值r(k)，而是跟踪参考轨迹y_r(k+1)。采用如下一阶滤波：

y_r(k)＝y(k)

y_r(k+1)＝αy_r(k)+(1-α)r(k)

9)求取目标函数：

在上述约束条件下，取极小化性能指标

J＝min{(y_r-y)^T(y_r-y)}＝min{(y_r-y_m-LΔu)^T(y_r-y_m-LΔu)}

求得

Δu(k)＝(L^TL)^-1L^T(y_r-y_m)＝L^T(y_r-y_m)/∑l²

控制量为u_a(k+1)＝u(k)+Δu(k)    (6)

10)PID控制过程：

Δy(k)作为PID输入，计算出下一刻控制量u_b(k+1)：

u_b(k+1)＝u(k)+K_p(Δy₂-Δy₁)+K_iΔy₂+K_d(Δy₂-2Δy₁+Δy₀)    (7)

式中，Δy₁＝y(k)-y(k-1)，Δy₀＝y(k-1)-y(k-2)。

11)控制量均值滤波：

根据上述(6)和(7)得到真正的控制量输入为

u(k+1)＝(u_a(k+1)+u_b(k+1))/2

所求u(k+1)即为水下机器人下一时刻推进器的输入。

综合以上，得出广义预测PID控制算法流程如下：

第一步：用(1)式推导出输出预测方程(3)；

第二步：选取初始值，遗忘因子ρ(一般取0.95～1)、输出柔化系数α(0.75左右)、输入柔化系数β(0.4左右)、足够大的正定矩阵P(-1)、系数矩阵

第三步：在线估计

第四步：递推E_j(z^-1)、F_j(z^-1)；

第五步：计算G_j、y_m；

第六步：引入输入柔化系数β；

第七步：对输入u(k+j)、Δu(k+j)添加约束条件；

第八步：设定参考轨迹y_r(k+1)；

第九步：求取目标函数Δu(k)，控制量u_a(k+1)；

第十步：PID控制过程，以Δy(k)作为PID输入，计算出下一刻控制量u_b(k+1)；

第十一步：对(6)、(7)式计算的控制量均值滤波，求出水下机器人下一时刻推进器的输入u(k+1)；

第十二步：t＝t+1，返回到第三步。

除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。

Claims (7)

1.一种水下结构物检测机器人控制系统，包括水面系统、水下系统，所述水面系统放置在岸边或者母船上，水下系统安装在水下结构物检测机器人上；其特征在于，所述水面系统包括水面控制台(1)、水面通信收发器(2)、脐带缆(3)，所述水面控制台(1)与水面通信收发器(2)相连，所述水面通信收发器(2)与脐带缆(3)相连；

所述水下系统包括水下通信收发器(4)、供电单元(5)、嵌入式微控制器(6)、动力推进单元(7)、视觉照明单元(8)、运动切换单元(9)、安全保护单元(10)、传感器单元(11)；所述水下通信收发器(4)与脐带缆(3)相连，接收来自水面系统的控制信息，所述嵌入式微控制器(6)与水下通信收发器(4)相连，所述供电单元(5)、动力推进的单元(7)、控制视觉照明单元(8)、运动切换单元(9)、安全保护单元(10)、传感器单元(11)分别与嵌入式微控制器(6)相连，所述供电单元(5)给水下系统供电，动力推进单元(7)接收嵌入式微控制器(6)的速度和方向信息，驱动螺旋桨旋转，视觉照明单元(8)采集视频以及接收来自嵌入式微控制器(6)的云台和照明灯信号，运动切换单元(9)用以改变水下机器人的运动方式，安全保护单元(10)检测机器人内部环境信息发送给嵌入式微控制器(6)，并作数据存储，传感器单元(11)采集深度、导航、姿态、声呐数据传输给嵌入式微控制器(6)进行控制；所述嵌入式微控制器(6)采集水下系统各单元数据，经由水下通信收发器(4)发送到水面系统。

2.如权利要求1所述的水下结构物检测机器人控制系统，其特征在于，所述水面控制台(1)包括水面控制电脑(102)、紧急拍停开关(103)、操纵摇杆(104)、键盘鼠标(105)、液晶显示器(106)、平板电脑(107)；所述紧急拍停开关(103)与水面控制电脑(102)相连，在紧急情况下按下紧急拍停开关(103)，系统切断电源停止控制；操纵摇杆(104)与水面控制电脑(102)相连，操纵机器人前进、后退、旋转、上浮、下潜动作，键盘鼠标(105)与水面控制电脑(102)相连，输入控制信息；液晶显示器(106)实时显示水面控制电脑(102)安装的上位机软件界面，平板电脑(107)通过无线通信与水面控制电脑(102)连接，移动访问机器人控制信息，辅助非操作人员决策；水面控制电脑(102)与水面通信收发器(2)相连，进行数据收发。

3.如权利要求1所述的水下结构物检测机器人控制系统，其特征在于，所述动力推进单元(7)包括光电隔离模块(701)、电机驱动模块(702)、吸附推进器电机(703)、首部垂向电机(704)、尾部垂向电机(705)、左侧纵向电机(706)、右侧纵向电机(707)、吸附螺旋桨(708)、首部垂向螺旋桨(709)、尾部垂向螺旋桨(710)、左侧纵向螺旋桨(711)、左主动车轮(712)、右侧纵向螺旋桨(713)、右主动车轮(714)；所述光电隔离模块(701)的输出端接电机驱动模块(702)的输入端，电机驱动模块(702)的输出端接吸附推进器电机(703)、首部垂向电机(704)、尾部垂向电机(705)、左侧纵向电机(706)、右侧纵向电机(707)，所述吸附推进器电机(703)驱动吸附螺旋桨(708)，所述首部垂向电机(704)驱动首部垂向螺旋桨(709)，所述尾部垂向电机(705)驱动尾部垂向螺旋桨(710)，所述左侧纵向电机(706)驱动左侧纵向螺旋桨(711)、左主动车轮(712)，所述右侧纵向电机(707)驱动右侧纵向螺旋桨(713)、右主动车轮(714)；所述嵌入式微控制器(6)发出的速度信号PWM和方向信号DIR，通过光电隔离模块(701)进行信号隔离，将隔离后的信号传给电机驱动模块(702)，电机驱动模块(702)输出端控制吸附推进器电机(703)、首部垂向电机(704)、尾部垂向电机(705)、左侧纵向电机(706)、右侧纵向电机(707)的速度和方向来控制吸附螺旋桨(708)、首部垂向螺旋桨(709)、尾部垂向螺旋桨(710)、左侧纵向螺旋桨(711)、左主动车轮(712)、右侧纵向螺旋桨(713)、右主动车轮(714)的旋转。

4.如权利要求1所述的水下结构物检测机器人控制系统，其特征在于，所述运动切换单元(9)包括上部磁性接近开关(901)、下部磁性接近开关(902)、步进电机(903)、磁铁(904)、滑台(905)、丝杠(906)、导轨(907)；所述左侧纵向电机(706)和右侧纵向电机(707)安装在一个可以垂直活动的滑台(905)上，步进电机(903)与滑台(905)通过丝杠(906)连接，可以驱动滑台(905)沿导轨(907)上下垂直移动，上部磁性接近开关(901)与下部磁性接近开关(902)分别安装在固定滑台架子的上部和下部，通过滑台上部和下部的磁铁(904)检测滑台(905)位置，判断切换爬行和浮游状态，实现运动切换功能。

5.如权利要求1所述的水下结构物检测机器人控制系统，其特征在于，所述安全保护单元(10)包括电压电流检测模块(1001)、漏水检测模块(1002)、温湿度计(1003)、数据存储卡(1004)；所述电压电流检测模块(1001)、漏水检测模块(1002)、温湿度计(1003)、数据存储卡(1004)与嵌入式微控制器(6)连接。

6.如权利要求1所述的水下结构物检测机器人控制系统，其特征在于，所述传感器单元(11)包括深度传感器(1101)、导航定位模块(1102)、姿态传感器(1103)、声呐传感器(1104)；所述深度传感器(1101)采集机器人深度信息，当机器人浮在水面时用导航定位模块(1102)确定和校正位置，所述姿态传感器(1103)实时采集机器人姿态信息，包括横滚角、俯仰角、航向角，所述声呐传感器(1104)采集水下结构物信息，所述深度传感器(1101)、导航定位模块(1102)、姿态传感器(1103)、声呐传感器(1104)与嵌入式微控制器(6)相连，将数据传给嵌入式微控制器(6)。

7.如权利要求1所述的水下结构物检测机器人控制系统的运动控制方法，其特征在于，该方法使用广义预测PID控制算法对机器人的深度和航向角进行控制以提高机器人的稳定性，具体步骤如下：

第一步：水下机器人采用受控自回归积分滑动平均模型，即CARIMA模型描述：

$A\left(z^{-1}\right)y\left(t\right)=B\left(z^{-1}\right)u(t-1)+\frac{C\left(z^{-1}\right)}{\mathrm{\Δ}}e\left(t\right)---\left(1\right)$

其中A(z^-1)、B(z^-1)、C(z^-1)是后移算子z^-1的多项式，y(t)＝[h，yaw]^T是t时刻深度h和航向角yaw的矩阵，u(t-1)是t-1时刻推进器输入电压，Δ＝1-z^-1表示差分算子，e(t)为均值为零的白噪声序列；

令C(z^-1)＝1单位矩阵，(1)式变为：

$A\left(z^{-1}\right)y\left(t\right)=B\left(z^{-1}\right)u(t-1)+\frac{e\left(t\right)}{\mathrm{\Δ}}---\left(2\right)$

式中， $A\left(z^{-1}\right)=I_{n\×n}+A_1{z}^{-1}+A_2{z}^{-2}+...+A_{n_a}{z}^{{-n}_a},$

$B\left(z^{-1}\right)=B_0+B_1{z}^{-1}+B_2{z}^{-2}+...+B_{n_b}{z}^{{-n}_b},$

其中是模型系数，n_a，n_b分别为输出、输入的后移算子多项式阶次，对于(2)式方程，引入Diophantine丢番图方程：

1＝E_j(z^-1)A(z^-1)Δ+z^-jF_j(z^-1)

式中，E_j(z^-1)＝e_j0+e_j1z^-1+…+e_j，j-1z^-j+1，e_j0＝1

F_j(z^-1)＝f_j0+f_j1z^-1+…+f_j，nz^-n

用(1)式求出k时刻后j步的输出预测方程(3)：

y(k+j)＝G_j(z^-1)Δu(k+j-1)+y_m        (3)

其中y(k+j)为k+j时刻深度和航向角预测输出，Δu(k+j-1)是推进器增量，其中令G_j(z^-1)＝E_j(z^-1)B_j(z^-1)，y_m＝F_j(z^-1)y(k)；

第二步：选取以下初始值：遗忘因子ρ，ρ取0.95～1；输出柔化系数α，α取0.75；输入柔化系数β，β取0.4；正定矩阵P(-1)；系数矩阵θ(0)；

第三步：在线估计A(z^-1)、B(z^-1)：

令ε(k)＝Δy(k)-X(k-1)^Tθ(k-1)，X(k-1)为过去时刻输入，Δy(k)为当前输出变化，

模型的系数 $\mathrm{\θ}\left(k\right)=\mathrm{\θ}(k-1)+\frac{P(k-1)X(k-1)\mathrm{\ϵ}\left(k\right)}{\mathrm{\ρ}+X{(k-1)}^{T}P(k-2)X(k-1)}$

其中 $\mathrm{\θ}\left(k\right)=[a_0\left(k\right),a_1\left(k\right),...,a_{n_a}\left(k\right),b_0\left(k\right),b_1\left(k\right),...,b_{n_b}\left(k\right){]}^T,$ 由此得到变化后的A(z^-1)、B(z^-1)；

第四步：递推E_j(z^-1)、F_j(z^-1)：

E_j+1(z^-1)＝E_j(z^-1)+e_jz^-j

$F_{j+1}\left(z^{-1}\right)=z(F_j\left(z^{-1}\right)-e_j\stackrel{\‾}{A})$

式中，E_j(z^-1)、F_j(z^-1)表示不断更新的两个变量；

第五步：计算G_j(z^-1)、y_m；

根据上述推算可求得：

G_j(z^-1)＝E_j(z^-1)B_j(z^-1)

y_m＝F_j(z^-1)y(k)

第六步：引入输入柔化系数β；

$\mathrm{\Δu}(t+k)=(1+\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{\mathrm{\β}}^i)\mathrm{\Δu}\left(k\right)---\left(4\right)$

令β≥0且β≤1，最优取为0.4；

从而，式(3)变为y(k+j)＝G_j(z^-1)HΔu(k)+y_m＝LΔu(k)+y_m

式中，H＝[1，1+β，...，1+β+…β^n-1]^T，L＝G_j(z^-1)H＝[l₁ l₂ ... l_n-1]^T，n为预测步长与时滞数之差；

第七步：对输入u(k+j)、Δu(k+j)添加约束条件：

u_min≤u(k+j)≤u_max，Δu_min≤Δu(k+j)≤Δu_max

第八步：设定参考轨迹y_r(k+1)：

系统输出不直接跟踪设定值r(k)，而是跟踪参考轨迹y_r(k+1)，采用如下一阶滤波：

y_r(k)＝y(k)

y_r(k+1)＝αy_r(k)+(1-α)r(k)

第九步：求取目标函数Δu(k)，控制量u_a(k+1)；

在上述约束条件下，取极小化性能指标

J＝min{(y_r(k)-y(k))^T(y_r(k+1)-y(k+1))}＝min{(y_r(k+1)-y_m-LΔu)^T(y_r(k+1)-y_m-LΔu)}

求得

△u(k)＝(L^TL)^-1L^T(y_r(k+1)-y_m)＝L^T(y_r(k+1)-y_m)/∑l²

控制量为u_a(k+1)＝u(k)+Δu(k)    (6)

第十步：PID控制过程，以Δy(k)作为PID输入，计算出下一刻控制量u_b(k+1)；

u_b(k+1)＝u(k)+K_p(Δy₂-Δy₁)+K_iΔy₂+K_d(Δy₂-2Δy₁+Δy₀)    (7)

式中，Δy₂＝y(k+1)-y(k)，Δy₁＝y(k)-y(k-1)，Δy₀＝y(k-1)-y(k-2)，K_p、K_i、K_d分别为比例、积分、微分系数；

第十一步：对(6)式、(7)式计算的控制量均值滤波，求出水下机器人下一时刻推进器的输入u(k+1)；

u(k+1)＝(u_a(k+1)+u_b(k+1))/2

所求u(k+1)即为水下机器人下一时刻推进器的输入；

第十二步：t＝t+1，返回到第三步，更新各项数据。