CN107957727B - 水下机器人控制系统及动力定位方法 - Google Patents

Abstract

本发明公开了一种水下机器人控制系统及动力定位方法，提供一种能够实现浮游和爬行两种功能的模块化控制系统，以及一种能够提高水下机器人稳定性的基于自适应无迹卡尔曼滤波状态观测器和多变量、多模态快速非奇异终端滑模控制器的水下机器人动力定位方法。所采用的自适应无迹卡尔曼滤波算法，在外界干扰、加速度的物理特性和人为操纵等因素影响的情况下跟踪滤波效果更好，关于不确定模型的鲁棒性较好，状态估计精度高；使得水下机器人在推进器推力范围内能够更加快速、平稳地到达指定地点并且在靠近目标位置附近区域，推力补偿可以适当减小，以便水下机器人可以进行微小调节，确保定位精度。

Description

水下机器人控制系统及动力定位方法

技术领域

本发明涉及一种检测与作业水下机器人，尤其涉及一种水下机器人控制系统及动力定位方法，属于机器人技术领域。

背景技术

随着陆地资源的枯竭，海洋资源的开发利用对人类发展和社会进步起着巨大的推动作用。水下机器人作为开发海洋的手段之一，一直得到广泛的关注和研究。近几年来，国内外多数水下机器人只有浮游能力或爬行能力，能够实现浮游和爬壁的两种模式的水下机器人较为少见。水下检测与作业机器人，是一种新颖的模块化、多功能、带缆遥控水下机器人，可以在浮游和爬行模态之间自由切换，并进行水下作业，被广泛应用于水下观察、浮游勘察、海底勘探、水下结构检修、海底管线检测以及水下安装等作业，已经成为海洋工程水下结构安全检测及维护的关键装备。

随着水下机器人的研究与发展，水下机器人的功能已经不仅仅局限于在水下低速或高速航行，而且需要具有如：悬停、旋转及爬潜等更高的机动性能。在水下机器人执行任务的过程中，不但要求水下航行器在环境扰动作用下按照预定的轨迹运动，而且在许多情况下需要利用水下机器人对目标物进行更细致的观察和作业，这就需要水下机器人相对于目标物的位置保持不变，即要求水下机器人具有能够抵抗环境扰动的动力定位能力。由于动力定位系统具有不受水深限制，投入和撤离迅速等优点并且可以实现上述的精确的机动性能。因此，为使水下机器人在深海中具有更强的能力，研究水下机器人的动力定位技术是十分有必要的。

随着计算机技术、GPS定位技术及通讯传感技术的快速发展，远程智能水下结构物检测机器人在复杂水下环境中的观察能力、顶流作业能力、高精度的运动控制及定位能力等均需要进一步提高，同时通过改善水面支持系统与水下观察作业系统之间的人机交互界面，加大数据处理容量，全面提高水下结构物检测机器人操作控制水平和操作性能是发展趋势之一。结合所研制的新型水下检测与作业机器人，基于流体动力学仿真法建立动力学模型，进行六自由度动力定位分析。通过传感器获得ROV的位置和艏向，采用自适应无迹卡尔曼滤波算法(AUKF)实时估计水下机器人的状态，并采用多变量、多模态快速非奇异终端滑模控制(FNTSMC)对推力予以补偿，以减小由波浪等随机带来的影响，根据定位误差设计力和力矩分配策略。该方法具有良好的动力定位效果，并且在受扰动后可以迅速调整动力分配策略，使得ROV能够迅速的收敛到目标点。申请号为200310105200,名称为“一种分布式水下机器人控制系统”，它仅仅采用了一路RS-485网络通信，可靠性低，数据传输量也少；申请号为201310357116.6名称为“一种浅水用小型水下机器人甲板的控制装置”，没有紧急拍停按钮，遇到特殊情况处置不方便，也没有配备平板电脑，不能实现移动监控机器人，不方便其他人员观察机器人作业。申请号为“201410090281.4”的专利文献公开了一种“船舶动力定位系统的控制方法及其控制系统”，但所采用的PID控制算法收敛性较差，使得船舶接近目标点较慢，并且容易发生振荡；申请号为“201510016643.X”的专利文献公开了“一种基于改进强跟踪滤波状态观测器的船舶动力定位方法”，其改进强跟踪滤波状态观测器关于不确定模型的鲁棒性较差，状态估计精度不高，容易出现发散等问题。

发明内容

本发明的目的在于提供一种水下机器人控制系统及动力定位方法，提供一种能够传输多种信号，带有急停按钮，控制台视频显示，更可靠的安全报警模块，可以实现浮游和爬行两种功能的模块化控制系统，以及一种能够提高水下机器人稳定性的基于自适应无迹卡尔曼滤波状态观测器和多变量、多模态快速非奇异终端滑模控制器的水下机器人动力定位方法。

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

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

所述水下系统包括水下通信收发器4、ARM9主控制器6、ARM-M0从控制器7、水下供电模块8、电源安全检测模块9、漏水检测模块10、一号低压电源11、通信模块12、二号低压电源13、下载口14、云台摄像头15、传感器模块16、水下灯17、机械手18、推进器控制模块19；所述水下通信收发器4与脐带缆3相连，接收来自水面控制系统的控制信息，所述ARM9主控制器6与水下通信收发器4相连，所述水下供电模块8与水下通信收发器4、ARM9主控制器6、ARM-M0从控制器7、电源安全检测模块9、漏水检测模块10、一号低压电源11、通信模块12、二号低压电源13、云台摄像头15、传感器模块16、水下灯17、机械手18、动力推进模块19相连，所述水下供电模块8给水下控制系统供电，推进器控制模块19接收ARM-M0从控制器7的速度和方向信息，驱动螺旋桨旋转，水下灯17和机械手18接收来自ARM-M0从控制器7的信号，云台摄像头15直接通过水下通信收发器4接收水面控制台1的信息，电源检测模块9和漏水检测模块10检测机器人内部环境信息及电源信息发送给ARM-M0从控制器7，并作数据存储，传感器模块16采集深度、导航、姿态、声呐数据传输给ARM9主控制器6进行控制；所述ARM9主控制器6通过ARM-M0从控制器7采集水下系统各单元数据，经由水下通信收发器4发送到水面控制系统。

一种水下机器人控制系统的动力定位方法，包括以下步骤：

步骤1：在有环境干扰的情况下，通过传感器系统来采集动力定位水下机器人的位置和艏向信息，通过位置和艏向信息得出水下机器人运动的动力学和运动学过程，其中水下机器人的运动学数学模型为：

式中，

为水下机器人相对于固定坐标系的位置和姿态角，其中

为横滚角，θ为俯仰角，ψ为航向角；v＝[u v w p q r]^Τ，为水下机器人在艇体坐标系内的线速度和角速度，其中u,v,w分别为x,y,z坐标轴方向的线速度，p,q,r为x,y,z坐标轴方向的角速度；J(η)为坐标转换矩阵；M为水下机器人惯性矩阵，M∈R^6×6；C(v)＝C_RB+C_A，为水下机器人附加质量的科氏力及向心力矩阵，C_RB为水下机器人附加质量的科氏力，C_A为水下机器人向心力矩阵，C(v)∈R^6×6；D(v_ξ)是水下机器人流体阻力矩阵，D(v_ξ)∈R^6×6；v_ξ＝v-v_d为去除环境干扰力时水下机器人的速度；g(η)是由重力和浮力组成的回复力矩阵，g(η)∈R^6×1；τ是水下机器人推进器提供的推力，τ∈R^6×1；ξ是环境干扰力，ξ∈R^6×1；

将式(1)变换为：

自适应无迹卡尔曼滤波算法基于如下非线性离散状态空间模型：

式中，k表示系统离散采样时间点，k≥0；x_k|k-1＝[η v]^Τ为k时刻的系统估计状态量；f_k(x_k-1|k-2)＝[J(η)v -M^-1C_RBv-M^-1C_Av_ξ-M^-1Dv_ξ-M^-1g]^Τ，f_k(x_k-1|k-2)为x_k-1到x_k的一步转移矩阵；B＝[0_6×6 M^-1]^Τ为系数矩阵；u∈R^6×1是推进器推力；w_k为系统激励高斯白噪声序列；h_k为量测矩阵，反应量测量和估计量之间的数学关系；v_k为量测值高斯白噪声序列；y_k为k时刻的传感器量测值；

步骤2：自适应无迹卡尔曼滤波状态观测器通过以下设计步骤实现：

1)状态初始条件为

其中,x₀为状态初始值；

为x₀的均值；P₀为预测协方差初始值；

为系统噪声初始值；

2)预测更新

对于给定的

和P_k-1|k-1，根据式(7)—(13)用UT法求状态预测

和

步骤A.计算Sigma点：

其中，χ_k-1为k-1时刻的Sigma点；

为k-1时刻的状态值；P_k-1|k-1为k-1时刻的预测协方差；n为大于1正整数，λ为比例系数；

步骤B.预测更新:

χ_k|k-1＝f(χ_k-1) (8)

其中，f(χ_k-1)为χ_k-1到χ_k的一步转移矩阵；W_i ^m和W_i ^c为均值和协方差的加权值；

为k-1时刻的系统噪声；h(χ_i,k|k-1)为量测矩阵；

预测协方差为：

3)发散判断

由式

判断是否发散,如发散按照式(14)—(16)修正P_k|k-1,不发散则进入下一步，其中

为残差序列；

4)量测更新

根据式(17)-(19),求得量测方差P_yy,P_xy和滤波增益K_k

5)递推估计系统噪声统计特性

其中，d_k-1＝(1-b)/(1-b^k),j＝0,1,…,k-1,b成为遗忘因子,b的范围为0.95＜b＜1；

6)得到当前水下机器人的量测更新值为

和P_k|k：

步骤3：非奇异终端滑模控制器根据当前水下机器人的位置和艏向估计值与设定目标的位置和艏向值进行对比得到定位误差，得到动力补偿值：

定义

与η等同，是机器人实时的位置和姿态，则

表示线速度和角速度，

表示线加速度和角加速度，令n_d(常数)为定位目标，那么定位误差表示为n_e＝n-n_d，算法实现目标为寻找适当的控制律，使水下机器人的位置n能够尽快地到达期望值n_d，即n_e要在有限时间内快速收敛到零；

定义运算

其中，sign为符号函数，x₁,…x₆为各状态变量，γ₁,…γ₆为状态变量系数；

则本算法的FNTSMC设计如下：

s为滑模面，为了使连接点处的速度连续，即n_e＝ε时

要相等，所以有

ε＝β^q/p-q (25)

式中，ε＝diag{ε₁,…ε₆}，β＝diag{β₁,…β₆}，β_i∈R⁺，s＝[s₁,…,s₆]^T，n_e,

p_i,q_i∈N⁺，i＝1,2,…,6且1＜p_i/q_i＜2；

采用如下终端吸引子作为趋近率：

其中，

k₁＝diag(k₁₁,…,k₁₆),k₂＝diag(k₂₁,…,k₂₆)，k_1i,k_2i∈R⁺，i＝1,2,…,6；

式(24)的一阶导数表示如下：

其中，I∈R^6×6，i＝1,2,…,6,

由式(23)～(27)可得，对于ROV的非线性动力学模型公式(1)，如果变量FNTSM选取式(24)，趋近率选取式(26)，采用如下控制率：

u＝u₁+u₂ (30)

其中，为了减轻当|n_e|＞ε时，SMC中的颤振现象，利用饱和函数sat(s/Δ)替代SMC中的指数趋近率的符号函数sig(s)，并取边界层Δ＝0.01，α＝diag{β₁,…β₆}，α_i∈R⁺。

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

前述水下机器人控制系统，其中水面控制台1包括工控机101、液晶屏102、Arduino平台103(Arduino平台103是一款开源的电子原型平台)、旋钮104、急停开关105、操纵摇杆106、指示灯107；所述急停开关105通过Arduino平台103与工控机101相连，在紧急情况下按下急停开关105，系统切断电源停止控制；操纵摇杆106通过Arduino平台103与工控机101相连，操纵机器人前进、后退、旋转、上浮、下潜、横移、翻滚动作，所述旋钮104、指示灯107通过Arduino平台103与工控机101相连，输入、输出控制信息；液晶屏102实时显示在工控机101上安装的上位机软件界面，平板电脑108通过无线通信与工控机101，移动访问机器人控制信息，辅助非操作人员决策；工控机101与水面通信收发器2相连，进行数据收发。

前述水下机器人控制系统，其中动力推进模块19包括I²C通信模块1901、写电机转速模块1902、读电机转速模块1903、读电机电压模块1904、读电机电流模块1905、读电机温度模块1906、电机驱动模块1907、左纵向电机1908、右纵向电机1909、左垂向电机1910、右垂向电机1911、侧向电机1912；所述写电机转速模块1902、读电机转速模块1903、读电机电压模块1904、读电机电流模块1905、读电机温度模块1906通过I²C通信模块1901与ARM-M0从控制器7通信；所述ARM-M0从控制器7发出的速度信号PWM和方向信号DIR通过写电机转速模块1902控制电机驱动模块1907，从而控制左纵向电机1908、右纵向电机1909、左垂向电机1910、右垂向电机1911、侧向电机1912进行前进后退、转向、上浮下潜、横移、横滚5个自由度的运动。

前述水下机器人控制系统，其中电源安全检测模块9包括三路电源使能模块901、三路电源报错模块902、三路电源电压检测模块903、三路电源电流检测模块904、三路电源温度检测模块905，所述三路电源使能模块901、三路电源报错模块902、三路电源电压检测模块903、三路电源电流检测模块904、三路电源温度检测模块905与ARM-M0从控制器7相连。

前述水下机器人控制系统，其中传感器模块16包括惯性导航模块1601、声呐模块1602、深度计模块1603、导航定位模块1604；所述惯性导航模块1601实时采集机器人姿态信息，包括横滚角、俯仰角、航向角，所述声呐模块1602采集水下结构物信息，所述深度计模块1603采集机器人深度信息，当机器人浮在水面时用导航定位模块1604确定和校正位置，所述深度计模块1603、导航定位模块1604、惯性导航模块1601、声呐模块1602与ARM9主控制器6相连，将数据传给ARM9主控制器6。

与现有技术相比，本发明的有益效果是：

1.本发明的水下检测与作业遥控机器人开发了一套控制系统，该系统具有多功能、模块化的优点，方便安装与拆卸；水面控制台具有紧急拍停功能，配备了无线连接的平板电脑，可以实现移动化监控；通信系统支持485、网络通信、I2C通信、差分视频、CAN总线传输；

2.所采用的自适应无迹卡尔曼滤波算法相对于其他的滤波算法来说，在外界干扰、加速度的物理特性和人为操纵等因素影响的情况下跟踪滤波效果更好，关于不确定模型的鲁棒性较好，状态估计精度高；

3.本发明所采用的多变量、多模态快速非奇异终端滑模控制器是在已有算法的基础上进行改进创新，首次应用在动力定位系统中，使得水下机器人在推进器推力范围内能够更加快速、平稳地到达指定地点并且在靠近目标位置附近区域，推力补偿可以适当减小，以便水下机器人可以进行微小调节，确保定位精度；

4.在六自由度动力定位时，采用有序定位的策略，即按照先定艏向，再定位置，最后定姿态的顺序依次完成整个定位系统。这样可以使动力分配更加条理，避免动力分配混乱的情况。

附图说明

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

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

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

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

图5为本发明机器人供电模块结构示意图；

图6是本发明水下机器人动力定位系统的算法流程图；

图7是本发明自适应无迹卡尔曼滤波算法流程图；

图8是本发明多变量、多模态快速非奇异终端滑模控制算法流程图。

具体实施方式

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

如图1所示，水下机器人控制系统包括水面控制系统、水下控制系统，所述水面控制系统放置在岸边或者母船上，水下控制系统安装在水下检测与作业机器人上；所述水面控制系统包括水面控制台1、水面通信收发器2、VR眼镜5，所述水面控制台1与水面通信收发器2相连，所述水面通信收发器2与脐带缆3相连；所述水下系统包括水下通信收发器4、ARM9主控制器6、ARM-M0从控制器7、水下供电模块8、电源检测模块9、漏水检测模块10、一号低压电源11、通信模块12、二号低压电源13、下载口14、云台摄像头15、传感器模块16、水下灯17、机械手18、动力推进模块19。

图1中水下通信收发器4与脐带缆3相连，接收来自水面控制系统的控制信息，所述ARM9主控制器6与水下通信收发器4相连，所述水下供电模块8与水下通信收发器4、ARM9主控制器6、ARM-M0从控制器7、电源检测模块9、漏水检测模块10、一号低压电源11、通信模块12、二号低压电源13、云台摄像头15、传感器模块16、水下灯17、机械手18、推进器控制模块19相连，所述水下供电模块8给水下控制系统供电，推进器控制模块19接收ARM-M0从控制器7的速度和方向信息，驱动螺旋桨旋转，水下灯17和机械手18接收来自ARM-M0从控制器7的信号，云台摄像头15直接通过水下通信收发器4接收水面控制台1的信息，电源检测模块9和漏水检测模块10检测机器人内部环境信息及电源信息发送给ARM-M0从控制器7，并作数据存储，传感器模块16采集深度、导航、姿态、声呐数据传输给ARM9主控制器6进行控制；所述ARM9主控制器6通过ARM-M0从控制器7采集水下系统各单元数据，经由水下通信收发器4发送到水面控制系统。

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

如图3所示，水面通信收发器2包括水面通信接口201、一号网口202、二号网口203。一号网口202与水面通信接口201相连，传递主通信信号，二号网口203与水面通信接口201相连，传递云台摄像头信号，实现一个接口传递两种信号，方便安装与插拔，最后水面通信接口201与脐带缆3相连。

如图4所示，水下通信收发器2包括水面通信接口401、一号网口402、二号网口403。脐带缆3与水面通信接口401相连，一号网口402左端与水面通信接口401相连，右端与ARM9主控制器6相连，传递主通信信号，二号网口403与左端水面通信接口401相连，右端与云台摄像头15相连，传递云台摄像头信号。

如图5所示，水下供电模块8包括400V/5250W直流电源801、400VDC转48VDC/1750W模块802、400VDC转48VDC/1750W模块803、400VDC转48VDC/1750W模块804、48V转12V模块805、12V转5V模块806、12V转5V模块807、5V转3.3V模块808、5V转3.3V模块809。水下供电模块8将脐带缆输出的400V直流电源801分为三路，400VDC转48VDC/1750W模块802给左纵向电机1908、右纵向电机1909供电，400VDC转48VDC/1750W模块803给左垂向电机1910、右垂向电机1911供电，400VDC转48VDC/1750W模块804给侧向电机1912、云台摄像头15、水下灯17、机械手18和小功率器件供电，经过48V转12V模块805给声呐1602、深度计模块1603、导航定位模块1603供电，再经过12V转5V模块806和12V转5V模块807给ARM9主控制器6、水面通信收发器2、水下通信收发器4、惯性导航模块1601供电，再经过5V转3.3V模块808、5V转3.3V模块809给ARM-M0从控制器7、漏水检测模块10供电。

如图6所示是本发明水下机器人动力定位系统的算法流程图，具体步骤如下：

步骤1：在有环境干扰的情况下，通过传感器系统来采集动力定位水下机器人的位置和艏向信息。通过这些位置和艏向信息就可以抽象出水下机器人运动的动力学和运动学过程，其中水下机器人的运动学数学模型为：

式中，

为水下机器人相对于固定坐标系的位置和姿态角，其中

为横滚角，θ为俯仰角，ψ为航向角；v＝[u v w p q r]^Τ，为水下机器人在艇体坐标系内的线速度和角速度，其中u,v,w分别为x,y,z坐标轴方向的线速度，p,q,r为x,y,z坐标轴方向的角速度；J(η)为坐标转换矩阵；M为水下机器人惯性矩阵，M∈R^6×6；C(v)＝C_RB+C_A，为水下机器人附加质量的科氏力及向心力矩阵，C_RB为水下机器人附加质量的科氏力，C_A为水下机器人向心力矩阵，C(v)∈R^6×6；D(v_ξ)是水下机器人流体阻力矩阵，D(v_ξ)∈R^6×6；v_ξ＝v-v_d为去除环境干扰力时水下机器人的速度；g(η)是由重力和浮力组成的回复力矩阵，g(η)∈R^6×1；τ是水下机器人推进器提供的推力，τ∈R^6×1；ξ是环境干扰力，如水流、波浪等，ξ∈R^6×1。

将式(1)水下机器人的控制模型整理变换为：

因为自适应无迹卡尔曼滤波算法基于如下非线性离散状态空间模型：

式中，k表示系统离散采样时间点，k≥0；x_k|k-1＝[η v]^Τ为k时刻的系统估计状态量；f_k(x_k-1|k-2)＝[J(η)v -M^-1C_RBv-M^-1C_Av_ξ-M^-1Dv_ξ-M^-1g]^Τ，f_k(x_k-1|k-2)为x_k-1到x_k的一步转移矩阵；B＝[0_6×6 M^-1]^Τ为系数矩阵；u∈R^6×1是推进器推力；w_k为系统激励高斯白噪声序列；h_k为量测矩阵，反应量测量和估计量之间的数学关系；v_k为量测值高斯白噪声序列；y_k为k时刻的传感器量测值。

步骤2：如图7所示，为自适应无迹卡尔曼滤波算法的流程图。一种应用与水下机器人动力定位的自适应无迹卡尔曼滤波方法，其特征在于：所述的自适应无迹卡尔曼滤波状态观测器通过以下设计步骤实现：

3)状态初始条件为

其中,x₀为状态初始值；

为x₀的均值；P₀为预测协方差初始值；

为系统噪声初始值。

2)预测更新

对于给定的

和P_k-1|k-1，根据式(7)—(13)用UT法求状态预测

和

步骤1.计算Sigma点

其中，χ_k-1为k-1时刻的Sigma点；

为k-1时刻的状态值；P_k-1|k-1为k-1时刻的预测协方差；n为大于1正整数，λ为比例系数。

步骤2.预测更新

χ_k|k-1＝f(χ_k-1) (8)

其中，f(χ_k-1)为χ_k-1到χ_k的一步转移矩阵；W_i ^m和W_W ^c为均值和协方差的加权值；

为k-1时刻的系统噪声；h(χ_i,k|k-1)为量测矩阵。

预测协方差为：

3)发散判断

由式

判断是否发散,如发散按照式(14)—(16)修正P_k|k-1,不发散则进入下一步，其中

为残差序列。

4)量测更新

根据式(17)-(19),求得量测方差P_yy,P_xy和滤波增益K_k

5)递推估计系统噪声统计特性

其中,d_k-1＝(1-b)/(1-b^k),j＝0,1,…,k-1,b成为遗忘因子,通常b的范围0.95＜b＜1。

6)最后得到当前水下机器人的量测更新值为

和P_k|k：

步骤3：如图8所示，为多变量、多模态快速非奇异终端滑模控制算法的流程图。非奇异终端滑模控制器根据当前水下机器人的位置和艏向估计值与设定目标的位置和艏向值进行对比得到定位误差，得到动力补偿值。

定义

与η等同，是机器人实时的位置和姿态，则

表示线速度和角速度，

表示线加速度和角加速度，令n_d(常数)为定位目标，那么定位误差表示为n_e＝n-n_d。算法实现目标为寻找适当的控制律，使水下机器人的位置n能够尽快地到达期望值n_d，即n_e要在有限时间内快速收敛到零。

定义运算

其中，sign为符号函数，x₁,…x₆为各状态变量，γ₁,…γ₆为状态变量系数。

则本算法的FNTSMC设计如下

s为滑模面，为了使连接点处的速度连续，即n_e＝ε时

要相等，所以有

ε＝β^q/p-q (25)

式中，ε＝diag{ε₁,…ε₆}，β＝diag{β₁,…β₆}，β_i∈R⁺，s＝[s₁,…,s₆]^T，n_e,

p_i,q_i∈N⁺，i＝1,2,…,6且1＜p_i/q_i＜2。

采用如下终端吸引子作为趋近率：

其中，

k₁＝diag(k₁₁,…,k₁₆),k₂＝diag(k₂₁,…,k₂₆)，k_1i,k_2i∈R⁺，i＝1,2,…,6

尽管式(24)中用到了绝对值符号和符号函数，它仍然是连续可导的，它的一阶导数可表示如下：

其中，I∈R^6×6，i＝1,2,…,6,

由式(23)～(27)可得，对于ROV的非线性动力学模型公式(1)，如果变量FNTSM选取式(24)，趋近率选取式(26)，采用如下控制率：

u＝u₁+u₂ (30)

其中，为了减轻当|n_e|＞ε时，SMC中的颤振现象，利用饱和函数sat(s/Δ)替代SMC中的指数趋近率的符号函数sig(s)，并取边界层Δ＝0.01，α＝diag{β₁,…β₆}，α_i∈R⁺。

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

Claims (5)

1.一种水下机器人控制系统的动力定位方法，水下机器人控制系统包括水面控制系统、水下控制系统，所述水面控制系统放置在岸边或者母船上，水下控制系统安装在水下检测与作业机器人上；所述水面控制系统包括水面控制台、水面通信收发器、脐带缆、VR眼镜，所述水面控制台与水面通信收发器相连，所述VR眼镜与水面控制台相连，所述水面通信收发器与脐带缆相连；所述水下系统包括水下通信收发器、ARM9主控制器、ARM-M0从控制器、水下供电模块、电源安全检测模块、漏水检测模块、一号低压电源、通信模块、二号低压电源、下载口、云台摄像头、传感器模块、水下灯、机械手、推进器控制模块；所述水下通信收发器与脐带缆相连，接收来自水面控制系统的控制信息，所述ARM9主控制器与水下通信收发器相连，所述水下供电模块与水下通信收发器、ARM9主控制器、ARM-M0从控制器、电源安全检测模块、漏水检测模块、一号低压电源、通信模块、二号低压电源、云台摄像头、传感器模块、水下灯、机械手、动力推进模块相连，所述水下供电模块给水下控制系统供电，推进器控制模块接收ARM-M0从控制器的速度和方向信息，驱动螺旋桨旋转，水下灯和机械手接收来自ARM-M0从控制器的信号，云台摄像头直接通过水下通信收发器接收水面控制台的信息，电源检测模块和漏水检测模块检测机器人内部环境信息及电源信息发送给ARM-M0从控制器，并作数据存储，传感器模块采集深度、导航、姿态、声呐数据传输给ARM9主控制器进行控制；所述ARM9主控制器通过ARM-M0从控制器采集水下系统各单元数据，经由水下通信收发器发送到水面控制系统；

其特征在于，水下机器人控制系统的动力定位方法包括以下步骤：

步骤1：在有环境干扰的情况下，通过传感器系统来采集动力定位水下机器人的位置和艏向信息，通过位置和艏向信息得出水下机器人运动的动力学和运动学过程，其中水下机器人的运动学数学模型为：

式中，

为水下机器人相对于固定坐标系的位置和姿态角，其中

为横滚角，θ为俯仰角，ψ为航向角；v＝[u v w p q r]^T，为水下机器人在艇体坐标系内的线速度和角速度，其中u,v,w分别为x,y,z坐标轴方向的线速度，p,q,r为x,y,z坐标轴方向的角速度；J(η)为坐标转换矩阵；M为水下机器人惯性矩阵，M∈R^6×6；C(v)＝C_RB+C_A，为水下机器人附加质量的科氏力及向心力矩阵，C_RB为水下机器人附加质量的科氏力，C_A为水下机器人向心力矩阵，C(v)∈R^6×6；D(v_ξ)是水下机器人流体阻力矩阵，D(v_ξ)∈R^6×6；v_ξ＝v-v_d为去除环境干扰力时水下机器人的速度；g(η)是由重力和浮力组成的回复力矩阵，g(η)∈R^6×1；τ是水下机器人推进器提供的推力，τ∈R^6×1；ξ是环境干扰力，ξ∈R^6×1；

将式(1)变换为：

自适应无迹卡尔曼滤波算法基于如下非线性离散状态空间模型：

式中，k表示系统离散采样时间点，k≥0；x_k|k-1＝[η v]^T为k时刻的系统估计状态量；f_k(x_k-1|k-2)＝[J(η)v-M^-1C_RBv-M^-1C_Av_ξ-M^-1Dv_ξ-M^-1g]^T，f_k(x_k-1|k-2)为x_k-1到x_k的一步转移矩阵；B＝[0_6×6 M^-1]^T为系数矩阵；u∈R^6×1是推进器推力；w_k为系统激励高斯白噪声序列；h_k为量测矩阵，反应量测量和估计量之间的数学关系；v_k为量测值高斯白噪声序列；y_k为k时刻的传感器量测值；

步骤2：自适应无迹卡尔曼滤波状态观测器通过以下设计步骤实现：

1)状态初始条件为

其中,x₀为状态初始值；

为x₀的均值；P₀为预测协方差初始值；

为系统噪声初始值；

2)预测更新

对于给定的

和P_k-1|k-1，根据式(7)—(13)用UT法求状态预测

和

步骤A.计算Sigma点：

其中，χ_k-1为k-1时刻的Sigma点；

为k-1时刻的状态值；P_k-1|k-1为k-1时刻的预测协方差；n为大于1正整数，λ为比例系数；

步骤B.预测更新:

χ_k|k-1＝f(χ_k-1) (8)

其中，f(χ_k-1)为χ_k-1到χ_k的一步转移矩阵；

和

为均值和协方差的加权值；

为k-1时刻的系统噪声；h(χ_i,k|k-1)为量测矩阵；

预测协方差为：

3)发散判断

由式

判断是否发散,如发散按照式(14)—(16)修正P_k|k-1,不发散则进入下一步，其中

为残差序列；

4)量测更新

根据式(17)-(19),求得量测方差P_yy,P_xy和滤波增益K_k

5)递推估计系统噪声统计特性

其中,d_k-1＝(1-b)/(1-b^k),j＝0,1,…,k-1,b成为遗忘因子,b的范围为0.95＜b＜1；

6)得到当前水下机器人的量测更新值为

和P_k|k：

步骤3：非奇异终端滑模控制器根据当前水下机器人的位置和艏向估计值与设定目标的位置和艏向值进行对比得到定位误差，得到动力补偿值：

定义

与η等同，是机器人实时的位置和姿态，则

表示线速度和角速度，

表示线加速度和角加速度，令n_d为定位目标，那么定位误差表示为n_e＝n-n_d，算法实现目标为寻找适当的控制律，使水下机器人的位置n能够尽快地到达期望值n_d，即n_e要在有限时间内快速收敛到零；

定义运算

其中，sign为符号函数，x₁,…x₆为各状态变量，γ₁,…γ₆为状态变量系数；

则本算法的FNTSMC设计如下：

s为滑模面，为了使连接点处的速度连续，即n_e＝ε时

要相等，所以有

ε＝β^q/p-q (25)

式中，ε＝diag{ε₁,…ε₆}，β＝diag{β₁,…β₆}，β_i∈R⁺，s＝[s₁,…,s₆]^T，n_e,

p_i,q_i∈N⁺，i＝1,2,…,6且1＜p_i/q_i＜2；

采用如下终端吸引子作为趋近率：

其中，

k₁＝diag(k₁₁,…,k₁₆),k₂＝diag(k₂₁,…,k₂₆)，k_1i,k_2i∈R⁺，i＝1,2,…,6；

式(24)的一阶导数表示如下：

其中，I∈R^6×6，i＝1,2,…,6,

由式(23)～(27)可得，对于ROV的非线性动力学模型公式(1)，如果变量FNTSMC选取式(24)，趋近率选取式(26)，采用如下控制率：

u＝u₁+u₂ (30)

其中，为了减轻当|n_e|＞ε时，SMC中的颤振现象，利用饱和函数sat(s/Δ)替代SMC中的指数趋近率的符号函数sig(s)，并取边界层Δ＝0.01，α＝diag{β₁,…β₆}，α_i∈R⁺。

2.如权利要求1所述的水下机器人控制系统的动力定位方法，其特征在于，所述水面控制台包括工控机、液晶屏、Arduino平台、旋钮、急停开关、操纵摇杆、指示灯；所述急停开关通过Arduino平台与工控机相连，在紧急情况下按下急停开关，系统切断电源停止控制；操纵摇杆通过Arduino平台与工控机相连，操纵机器人前进、后退、旋转、上浮、下潜、横移、翻滚动作，所述旋钮、指示灯通过Arduino平台与工控机相连，输入、输出控制信息；液晶屏实时显示在工控机上安装的上位机软件界面，平板电脑通过无线通信与工控机，移动访问机器人控制信息，辅助非操作人员决策；工控机与水面通信收发器相连，进行数据收发。

3.如权利要求1所述的水下机器人控制系统的动力定位方法，其特征在于，所述动力推进模块包括I²C通信模块、写电机转速模块、读电机转速模块、读电机电压模块、读电机电流模块、读电机温度模块、电机驱动模块、左纵向电机、右纵向电机、左垂向电机、右垂向电机、侧向电机；所述写电机转速模块、读电机转速模块、读电机电压模块、读电机电流模块、读电机温度模块通过I²C通信模块与ARM-M0从控制器通信；所述ARM-M0从控制器发出的速度信号PWM和方向信号DIR通过写电机转速模块控制电机驱动模块，从而控制左纵向电机、右纵向电机、左垂向电机、右垂向电机、侧向电机进行前进后退、转向、上浮下潜、横移、横滚5个自由度的运动。

4.如权利要求1所述的水下机器人控制系统的动力定位方法，其特征在于，所述电源安全检测模块包括三路电源使能模块、三路电源报错模块、三路电源电压检测模块、三路电源电流检测模块、三路电源温度检测模块，所述三路电源使能模块、三路电源报错模块、三路电源电压检测模块、三路电源电流检测模块、三路电源温度检测模块与ARM-M0从控制器相连。

5.如权利要求1所述的水下机器人控制系统的动力定位方法，其特征在于，所述传感器模块包括惯性导航模块、声呐模块、深度计模块、导航定位模块；所述惯性导航模块实时采集机器人姿态信息，包括横滚角、俯仰角、航向角，所述声呐模块采集水下结构物信息，所述深度计模块采集机器人深度信息，当机器人浮在水面时用导航定位模块确定和校正位置，所述深度计模块、导航定位模块、惯性导航模块、声呐模块与ARM9主控制器相连，将数据传给ARM9主控制器。

Images