CN109062230B - 水下辅助采油机器人控制系统及动力定位方法 - Google Patents

Abstract

本发明公开一种水下辅助采油机器人控制系统系统及动力定位方法，针对水下辅助采油机器人运动受到海流干扰而产生的不确定性问题，结合所研制的水下辅助采油机器人，基于流体动力学数字模拟辨识参数法建立动力学模型，进行六自由度动力定位分析。通过传感器获得水下辅助采油机器人的位置和艏向，采用基于遗传算法结合马尔可夫链蒙特卡罗方法自适应无迹卡尔曼粒子滤波算法实时估计水下机器人的状态，并将快速终端趋近律引入到非奇异快速终端滑模控制中对推力予以补偿，以减小由海流等干扰带来的影响，再根据定位误差去设计力和力矩分配策略。该方法具有良好的动力定位效果，并且在受扰动后可以迅速调整动力分配策略，以减小随机海流带来的干扰影响。

Description

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

技术领域

本发明涉及一种水下辅助采油机器人，尤其涉及一种水下辅助采油机器人控制系统及动力定位方法，属于机器人技术领域。

背景技术

海洋油气资源的开发，从勘探、钻井、开采到原油输送等每一个环节，都需要大量的水下设备安装、维护和检修等作业任务，并且水下操作技术复杂，精准度要求高。水下油气设备具有集成化程度高、重量大、结构复杂、成本大等特点，无人遥控潜水器(RemoteOperated Vehicle，ROV)是海上油气田水下作业和应急维修必不可少的重要装备之一，承担着水下搜索观察、水下井口对接、防喷器安装、水下采油树、水下管汇和跨接管辅助安装、水下控制缆安装、水下连接系统安装、海底管道检测和辅助维修、紧急工况下水下阀门开关等重型作业任务。

水下辅助采油机器人不仅仅要在海底低速或者高速航行，还需要悬停、旋转等更高的机动性能。执行任务的过程中，不但要求水下辅助采油机器人在环境扰动作用下按照预定的轨迹运动，而且在许多情况下需要利用水下辅助采油机器人对输油设备进行更细致的观察和作业，这就需要水下辅助采油机器人相对于目标物的位置保持不变，即要求水下机器人具有能够抵抗环境扰动的动力定位能力。因此，为使水下机器人在深海中具有更强的能力，研究水下机器人的动力定位技术也是十分有必要的。

随着计算机技术、GPS定位技术及通讯传感技术的快速发展，远程智能水下辅助采油机器人在复杂水下环境中的观察能力、顶流作业能力、高精度的运动控制及定位能力等均需要进一步提高，同时通过改善水面支持系统与水下观察作业系统之间的人机交互界面，加大数据处理容量，全面提高水下辅助采油机器人操作控制水平和操作性能是发展趋势之一。申请号为“201510107334.3”，名称为“一种水下机器人水面控制装置”的专利文献，没有考虑到摄像头拍摄的视频的保存问题，水下机器人实验结束后，无法再次分析机器人的性能和观察到的水下生物的信息。申请号为“201610952988.0”，名称为“一种海底管线检测用水下机器人”的专利文献，在检测装置中没有提到水下灯和水下摄像头的设计，没有水下灯的照射和摄像头拍摄，水面操作人员无法根据实际情况操纵机械手和机器人的运动。申请号为“201610124928.X”，名称为“在动力定位中跟踪环境力突变的自适应滤波方法”的专利文献，未考虑海洋情况的复杂性，扰动模型的不精确性，导致动力定位的精度不高。

本发明结合所研制的水下辅助采油机器人，基于流体动力学仿真法建立动力学模型，进行六自由度动力定位分析。通过传感器获得ROV的位置和艏向，采用基于遗传算法结合马尔可夫链蒙特卡罗方法(GA-MCMC)自适应无迹卡尔曼粒子滤波实时估计水下机器人的状态，并将引入快速终端趋近律的非奇异快速终端滑模控制对推力予以补偿，以减小由海流等随机带来的影响，再根据定位误差去设计力和力矩的分配策略。该方法具有良好的动力定位效果，并且在受扰动后可以迅速调整动力分配策略，使得ROV能够迅速的收敛到目标点。

发明内容

本发明的目的在于提供一种水下辅助采油机器人控制系统及动力定位方法，提供一种能够传输多种信号，水面控制台多屏显示，能够实时录像，有可靠的安全报警模块，可以实现浮游和爬行两种功能的模块化控制系统，以及一种能够提高水下辅助采油机器人稳定性的基于GA-MCMC自适应无迹卡尔曼粒子滤波观测器和引入快速终端趋近律的非奇异快速终端滑模控制器的动力定位方法。

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

一种水下辅助采油机器人控制系统，包括水面控制系统、水下控制系统，所述水面控制系统放置在岸边或者母船上，水下控制系统安装在水下辅助采油机器人上；所述水面控制系统包括水面控制台1、供电系统2、脐带缆3，水面控制台1和脐带缆3分别与供电系统2相连，为水下辅助采油机器人本体提供能量及数据传输，所述水下控制系统包括在电源舱4内的水下供电模块7，水下灯模块10，机械手模块11，声呐模块12，动力推进模块13，控制舱5内的云台摄像头模块8，导航舱6内的传感器模块9，安全检测模块14；所述水面控制系统与水下控制系统通过脐带缆3传输信息，所述电源舱4与控制舱5相连提供电能和传输信息，控制舱与导航舱相连提供电能和信息传输；所述水下供电模块7给水下控制系统供电，水下灯模块10、机械手模块11和动力推进器13接收来自水面的控制信号工作，声呐模块12向水面控制台实时反映水下情况，传感器模块9实时测量反馈惯导信息和深度信息，安全检测模块14检测每一个舱体内的情况并反馈给水面控制台，云台摄像模块8将水下的信息实时传到水面控制台。

前述水下辅助采油机器人控制系统的动力定位方法，包括以下步骤：

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

式中，

x,y,z分别为水下辅助采油机器人相对于固定坐标系的位置，其中

为横滚角，δ为俯仰角，ψ为航向角；v＝[a b c p q r]^T，为水下辅助采油机器人在艇体坐标系内的线速度和角速度，其中a、b、c分别为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；

步骤2：基于GA-MCMC自适应无迹卡尔曼粒子滤波算法包括以下步骤，

1.初始化：k＝0，从先验概率p(x₀)采集N个粒子，如式(2)

式中：x₀为状态初始值，

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

2.自适应无迹卡尔曼算法作为重要密度函数

将无迹卡尔曼滤波作为重要性函数与粒子滤波结合的算法：

(1)Sigma采样：根据对称策略对初始状态采样得到sigma点和对应权值如式(3)

式中，L为常数，λ＝α²(L+k)-L表示一个尺度参量，α参量表示采样点在

附近的遍布范围，1e-4≤α≤1，k被设置为0或者3-L，X_i为sigma点，

为随机变量x的均值，P_x为协方差，构造一个由X_i组成的2L+1维矩阵X,其中X_i对应权重W_i因子如式(4)

式中，W_i ^(m)为均值，W_i ^(c)为协方差的加权值，β包含x的先验信息，对于高斯分布β＝2；

表示矩阵平方根的第i列；

(2)一步预测方程：

式中，X_k-1为k-1时刻的sigma点，f(X_k-1)为X_k-1到X_k的一步转移矩阵；

为k-1时刻的状态值，P_k|k-1为k-1时刻的预测协方差；

(3)sigma重采样过程：根据对称采样策略，对一步预测状态

进行重采样如式

式中S_k|k-1为P_k|k-1的矩阵平方根，X_k|k-1每列分别代表一个采样向量点X_i,k|k-1；

(4)输出的一步预测：

式中，

为量测矩阵，z_k|k-1为输出预测值；

(5)量测更新方程

式中，

为误差协方差，

为互协方差，

和P_k为量测更新值，K为滤波增益，

(6)自适应更新

其中λ_k＝diag{λ₁,λ₂,…,λ_m}_k，

其中ξ衡量载体的运动规律，取值为2，参数ε用于改善估计状态量的精度，ε取值4.5；

R_k为高斯白噪声矩阵；

V_k代表预测信息估计的协方差，改进后如式：

(7)采样：

式中，q(x_k|x_0:k-1,z_1:k)为重要密度函数；

(8)重要性权值更新为，如式:

式中，p(x_0:k|z_1:k)为后验概率密度

3.权值归一化处理

4.遗传算法结合马尔可夫链蒙特卡罗方法重采样算法由三步组成：交叉过程、变异过程和选择过程；

(1)若随机数U_c＜P_c，交叉概率P_c子代粒子的数目和总粒子数的比值

式中：

为交叉因子：

和

为k时刻父代粒子：

和

为子代粒子；

若

接受

否则用以下概率接受，如式

若

接受

否则用以下概率接受，如式

其中交叉率P_c使用改进后的交叉率概率，公式如下：

式中P_c1，P_c2表示P_c的变范围，常数A对P_c的变化趋势进行调节，且使得P_c的变化比较缓慢，f_max是种群中最大适应度值，f′是两个交叉个体中较大的适应度值，f_avg是种群适应度的平均值，f是变异个体的适应度值；

(2)马尔科夫链蒙特卡罗变异算子

Metropolis-Hasting算法的流程为：给定一个一般的目标分布函数π(θ)，从随机值θ出发，进如下步骤的转换；

1)根据扰动，产生一个候选θ′，如

2)计算接受概率

3)按照u～U(0,1)均匀采样。

4)如果u≤A(θ,θ′)，则θ^k+1＝θ′；否则，θ^k+1＝θ；

(3)粒子选择阶段

本算法中将权值

作为符合[0,1]的随机数，

为适应度函数，每次随机选取出一个

作为选择指针，若

则选择

作为适应度最佳的个体，以此选择出适应度佳的N个个体；

5、状态估计

最终得到粒子集和对应权重

则状态估计值：

步骤3：将快速趋近律引入非奇异终端滑模控制器根据当前水下辅助采油机器人的位置和艏向估计值与设定目标的位置坐标值和艏向值坐标值进行相减绝对值得到定位误差，得到动力补偿值：

如式所示的二阶非线性单输入单输出系统:

式中，x＝[x₁,x₂]^T∈R²为系统状态；u∈R为系统控制输入；f(x,t),g(x,t)≠0为x的光滑非线性函数；

其滑模超平面s、控制律u和趋近律

分别为

式中k∈R⁺且满足，0＜k＜1，α，β，ρ₁，ρ₂∈R⁺，γ∈R⁺且满足γ＞1，p,q∈N⁺为奇数，且1＜p/q＜2。

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

前述水下辅助采油机器人控制系统，其中水面控制台1包括工控机15、显示屏16、控制按钮17和操纵摇杆30和硬盘录像机18；所述工控机15用于运行上位机以及连接显示屏16；所述控制按钮17和操纵摇杆30均与工控机15进行串口通讯用于操控水下辅助采油机器人；所述显示器16由四个显示屏组成，第一块和第二块显示屏显示云台摄像模块8采集的机器人周围的水下图像和机械手模块11的运动，第三块显示屏显示声呐模块12的信息、第四块显示屏显示推进器模块13、传感器模块9的数据信息，且显示屏显示的信息可以互相切换；所述硬盘录像机18直接与显示屏相连用于记录云台摄像模块8所拍摄的所有画面。

前述水下辅助采油机器人控制系统，其中云台摄像模块8共有3个摄像头，两个装于水下辅助采油机器人前部便于观察前方情况和机械手模块11中的两个机械手的运动情况，一个摄像头装于水下辅助采油机器人的尾部，便于观察机器人后部的情况；所述机械手模块11由一台3自由度液压机械手和一台5自由度液压机械手，用于携带采油树扭转工具进行辅助采油作业、以及进行抓取、采样作业。

前述水下辅助采油机器人控制系统，其中脐带缆3缆芯为两根电缆，四根光纤，两根电缆线传输电力，通过电缆线将供电系统2与水下辅助采油机器人连接，为水下辅助采油机器人提供能量，四根光纤中一根用于水下控制系统与水上控制系统的数据通信、另外两根光纤分别用于云台摄像模块8的视频传输和声纳信息的传输，还有一根备用。

前述水下辅助采油机器人控制系统，其中水下供电模块7包含了9块400v直流转48v直流给电源舱4和导航舱6供电,1块400v直流转13.8v直流给控制舱5供电。

前述水下辅助采油机器人控制系统，其中动力推进模块13包含控制器模块23、电机驱动模块24、推进器25；所述的控制器模块23为Arduino系列单片机，用于控制电机驱动模块24，电机驱动模块24的输出直接控制推进器25，所述推进器25共6个推进器，采用水平四个水下推进器矢量分布和垂向两个推进器的布局，从而实现ROV的前进、后退、上浮下潜、转艏、俯仰、横移运动。

前述水下辅助采油机器人控制系统，其中传感器模块9包含惯性导航模块21，深度计模块22；所述惯性导航模块21实时采集机器人的姿态信息，包括航向角、俯仰角以及横滚角；所述深度计模块22通过感知水压来采集水下辅助采油机器人的深度信息。

前述水下辅助采油机器人控制系统，其中安全检测模块14包含包括电压温度检测26、电流温度检测27、温湿度检测28、漏水检测29；所述电压温度检测26和电流温度检测27是对电源模块监测，防止电源报错；所述温湿度检测28用于测量密封舱内的温湿度，以防温度过高或者舱内湿度过高的情况；所述漏水检测29用于检测舱内是否有漏水现象。

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

1.本发明的水下辅助采油机器人开发了一套控制系统，该系统具有良好抗干扰的能力，能够更好的实现串口和网络通信，在水下辅助采油机器人的操作上和人机交互界面上都符合用户使用习惯。

2.本发明所采用的基于GA-MCMC自适应无迹卡尔曼粒子滤波算法摆脱了粒子滤波随机量必须满足高斯分布的制约条件，还可以进行并行计算，提高了状态估计的精度。

3.本发明将快速趋近律引入非奇异终端滑模控制器是在已有算法的基础上进行改进创新，使得水下辅助采油机器人在推进器推力范围内能够更加快速、平稳地到达指定地点并且在靠近目标位置附近区域，推力补偿可以适当减小，以便水下机器人可以进行微小调节，确保定位精度。

附图说明

图1是本发明水下辅助采油机器人系统结构框图；

图2是本发明水下辅助采油机器人动力定位系统的算法流程图；

图3是本发明基于GA-MCMC自适应无迹卡尔曼粒子滤波算法流程图；

图4是本发明将快速趋近律引入非奇异终端滑模控制算法流程图。

具体实施方式

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

如图1所示，水下辅助采油机器人控制系统，包括水面控制系统、水下控制系统，所述水面控制系统放置在岸边或者母船上，水下控制系统安装在水下辅助采油机器人上；所述水面控制系统包括水面控制台1、供电系统2、脐带缆3，水面控制台1和脐带缆3分别与供电系统2相连，为水下辅助采油机器人本体提供能量及数据传输，所述水下控制系统包括在电源舱4内的水下供电模块7，水下灯模块10，机械手模块11，声呐模块12，动力推进模块13，控制舱5内的云台摄像头模块8，导航舱6内的传感器模块9，安全检测模块14。所述水面控制系统与水下控制系统通过脐带缆3传输信息。所述电源舱4与控制舱5相连提供电能和传输信息，控制舱与导航舱相连提供电能和信息传输。所述水下供电模块7给水下控制系统供电，水下灯模块10、机械手模块11和动力推进器13接收来自水面的控制信号工作，声呐模块12向水面控制台实时反映水下情况，传感器模块9实时测量反馈惯导信息和深度信息，安全检测模块14检测每一个舱体内的情况并反馈给水面控制台，云台摄像模块8将水下的信息实时传到水面控制台。

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

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

式中，

x,y,z为水下辅助采油机器人相对于固定坐标系的位置，其中

为横滚角，δ为俯仰角，ψ为航向角；v＝[a b c p q r]^T，为水下辅助采油机器人在艇体坐标系内的线速度和角速度，其中a b c分别为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；

步骤2,如图3所示，基于GA-MCMC自适应无迹卡尔曼粒子滤波算法通过以下设计步骤实现：

1.对先验概率p(x₀)采集N个粒子，如式(2)

式中：x₀为状态初始值，

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

2.自适应无迹卡尔曼算法作为重要密度函数

将无迹卡尔曼滤波作为重要性函数与粒子滤波结合的算法：

(1)Sigma采样：根据对称策略对初始状态采样得到sigma点和对应权值如式(3)

式中，L为常数,λ＝α²(L+k)-L表示一个尺度参量，α参量表示采样点在

附近的遍布范围1e-4≤α≤1，k被设置为0或者3-L，X_i为sigma点，

为随机变量x的均值，P_x为协方差，构造一个由X_i组成的2L+1维矩阵X,其中X_i对应权重W_i因子如式(4)

式中，W_i ^(m)为均值，W_i ^(c)为协方差的加权值，β包含x的先验信息，对于高斯分布β＝2；

表示矩阵平方根的第i列；

(2)一步预测方程：

式中，X_k-1为k-1时刻的sigma点，f(X_k-1)为X_k-1到X_k的一步转移矩阵；

为k-1时刻的状态值，P_k|k-1为k-1时刻的预测协方差；

(3)sigma重采样过程：根据对称采样策略，对一步预测状态

进行重采样如式

式中S_k|k-1为P_k|k-1的矩阵平方根，X_k|k-1每列分别代表一个采样向量点X_i,k|k-1；

(4)输出的一步预测：

式中，h_k为观测矩阵，z_k|k-1为输出预测值；

(5)量测更新方程

式中，

为误差协方差，

为互协方差，

和P_k为量测更新值，K为滤波增益，

(6)自适应更新

其中λ_k＝diag{λ₁,λ₂,...,λ_m}_k，

其中ξ衡量载体的运动规律，取值为2，参数ε用于改善估计状态量的精度，ε取值4.5；

R_k为高斯白噪声矩阵；

V_k代表预测信息估计的协方差，改进后如式：

(8)采样：

式中，q(x_k|x_0:k-1,z_1:k)为重要密度函数；

(8)重要性权值更新为，如式:

式中，p(x_0:k|z_1:k)为后验概率密度

3.权值归一化处理

4.遗传算法结合马尔可夫链蒙特卡罗方法重采样算法重采样算法由三步组成：交叉过程、变异过程和选择过程；

(1)若随机数U_c＜P_c，交叉概率P_c子代粒子的数目和总粒子数的比值

式中：

为交叉因子：

和

为k时刻父代粒子：

和

为子代粒子。

若

接受

否则以以下概率接受如式

若

接受

否则用以下概率接受，如式

其中交叉率P_c使用改进后的交叉率概率，公式如下：

式中P_c1，P_c2表示P_c的变范围，常数A对P_c的变化趋势进行调节，且使得P_c的变化比较缓慢，f_max是种群中最大适应度值，f′是两个交叉个体中较大的适应度值，f_avg是种群适应度的平均值，f是变异个体的适应度值；

(2)马尔科夫链蒙特卡罗变异算子

Metropolis Hasting算法流程为：给定一个一般的目标分布函数π(θ)，从随机值θ出发，进如下步骤的转换。

1)根据扰动，产生一个候选θ′，如

2)计算接受概率

3)按照u～U(0,1)均匀采样；

4)如果u≤A(θ,θ′)，则θ^k+1＝θ′；否则，θ^k+1＝θ；

(3)粒子选择阶段

本算法中将权值

作为符合[0,1]的随机数，

为适应度函数，每次随机选取出一个

作为选择指针，若

则选择

作为适应度最佳的个体，以此选择出适应度佳的N个个体；

5、状态估计

最终得到粒子集和对应权重

则状态估计值：

步骤3：如图4所示，将快速趋近律引入非奇异终端滑模控制器根据当前水下辅助采油机器人的位置和艏向估计值与设定目标的位置坐标值和艏向值坐标值进行相减绝对值得到定位误差，得到动力补偿值：

如式所示的二阶非线性单输入单输出系统：

式中，x＝[x₁,x₂]^T∈R²为系统状态；u∈R为系统控制输入；f(x,t),g(x,t)≠0为x的光滑非线性函数；

其滑模超平面s、控制律u和趋近律

分别为

式中k∈R⁺且满足，0＜k＜1α，β，ρ₁，ρ₂∈R⁺，γ∈R⁺且满足γ＞1，p,q∈N⁺为奇数，且1＜p/q＜2。

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

Claims (1)

1.一种水下辅助采油机器人控制系统的动力定位方法，水下辅助采油机器人控制系统包括水面控制系统、水下控制系统，所述水面控制系统放置在岸边或者母船上，水下控制系统安装在水下辅助采油机器人上；所述水面控制系统包括水面控制台(1)、供电系统(2)、脐带缆(3)，水面控制台(1)和脐带缆(3)分别与供电系统(2)相连，为水下辅助采油机器人本体提供能量及数据传输；所述水下控制系统包括在电源舱(4)内的水下供电模块(7)，水下灯模块(10)，机械手模块(11)，声呐模块(12)，动力推进模块(13)，控制舱(5)内的云台摄像头模块(8)，导航舱(6)内的传感器模块(9)，安全检测模块(14)；所述水面控制系统与水下控制系统通过脐带缆(3)传输信息；所述电源舱(4)与控制舱(5)相连提供电能和传输信息，控制舱与导航舱相连提供电能和信息传输；所述水下供电模块(7)给水下控制系统供电，水下灯模块(10)、机械手模块(11)和动力推进器(13)接收来自水面的控制信号工作，声呐模块(12)向水面控制台实时反映水下情况，传感器模块(9)实时测量反馈惯导信息和深度信息，安全检测模块(14)检测每一个舱体内的情况并反馈给水面控制台，云台摄像模块(8)将水下的信息实时传到水面控制台；其特征在于，水下辅助采油机器人控制系统的动力定位方法包括以下步骤：

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

式中，

x,y,z为水下辅助采油机器人相对于固定坐标系的位置，其中

为横滚角，δ为俯仰角，ψ为航向角；v＝[a b c p q r]^T，为水下辅助采油机器人在艇体坐标系内的线速度和角速度，其中a、b、c分别为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；

步骤2:基于遗传算法结合马尔可夫链蒙特卡罗方法自适应无迹卡尔曼粒子滤波算法通过以下设计步骤实现：

1)对先验概率p(x₀)采集N个粒子，如式(2)

式中：x₀为状态初始值，

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

2)自适应无迹卡尔曼算法作为重要密度函数

将无迹卡尔曼滤波作为重要性函数与粒子滤波结合的算法：

(1)Sigma采样：根据对称策略对初始状态采样得到sigma点和对应权值如式(3)

式中，L为常数,λ＝α²(L+k)-L表示一个尺度参量，α参量表示采样点在

附近的遍布范围，1e-4≤α≤1，k被设置为0或者3-L，X_i为sigma点，

为随机变量x的均值，P_x为协方差，构造一个由X_i组成的2L+1维矩阵X,其中X_i对应权重W_i因子如式(4)

式中，W_i ^(m)为均值，W_i ^(c)为协方差的加权值,β包含x的先验信息，对于高斯分布β＝2；

表示矩阵平方根的第i列；

(2)一步预测方程：

式中，X_k-1为k-1时刻的sigma点，f(X_k-1)为X_k-1到X_k的一步转移矩阵；

为k-1时刻的状态值，P_k|k-1为k-1时刻的预测协方差；

(3)sigma重采样过程：根据对称采样策略，对一步预测状态

进行重采样如式

式中，S_k|k-1为P_k|k-1的矩阵平方根，X_k|k-1每列分别代表一个采样向量点X_i,k|k-1；

(4)输出的一步预测：

式中，

为量测矩阵，z_k|k-1为输出预测值；

(5)量测更新方程

式中，

为误差协方差，

为互协方差，

和P_k为量测更新值,K为滤波增益；

(6)自适应更新

其中λ_k＝diag{λ₁,λ₂,...,λ_m}_k，

其中ξ衡量载体的运动规律，取值为2，参数ε用于改善估计状态量的精度，ε取值4.5；

R_k为高斯白噪声矩阵；

V_k代表预测信息估计的协方差，改进后如式：

(7)采样：

式中，q(x_k|x_0:k-1,z_1:k)为重要密度函数；

(8)重要性权值更新为，如式:

式中，p(x_0:k|z_1:k)为后验概率密度；

3)权值归一化处理

4)遗传算法结合马尔可夫链蒙特卡罗方法重采样算法由三步组成：交叉过程、变异过程和选择过程；

(1)若随机数U_c＜P_c，交叉概率P_c子代粒子的数目和总粒子数的比值

式中：

为交叉因子：

和

为k时刻父代粒子：

和

为子代粒子；

若

接受

否则以以下概率接受如式

若

接受

否则用以下概率接受，如式

其中交叉率P_c使用改进后的交叉率概率，公式如下：

式中P_c1，P_c2表示P_c的变范围，常数A对P_c的变化趋势进行调节，且使得P_c的变化比较缓慢，f_max是种群中最大适应度值，f′是两个交叉个体中较大的适应度值，f_avg是种群适应度的平均值，f是变异个体的适应度值；

(2)马尔科夫链蒙特卡罗变异算子

Metropolis-Hasting算法流程为：给定一个一般的目标分布函数π(θ)，从随机值θ出发，进如下步骤的转换；

①根据扰动，产生一个候选θ′，如

②计算接受概率

③按照u～U(0,1)均匀采样；

④如果u≤A(θ,θ′)，则θ^k+1＝θ′；否则，θ^k+1＝θ；

(3)粒子选择阶段

本算法中将权值

作为符合[0,1]的随机数，

为适应度函数，每次随机选取出一个

作为选择指针，若

则选择

作为适应度最佳的个体，以此选择出适应度佳的N个个体；

5)状态估计

最终得到粒子集和对应权重

则状态估计值：

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

如式所示的二阶非线性单输入单输出系统:

式中，x＝[x₁,x₂]^T∈R²为系统状态；u∈R为系统控制输入；f(x,t),g(x,t)≠0为x的光滑非线性函数；

其滑模超平面s、控制律u和趋近律

分别为

式中k∈R⁺且满足，0＜k＜1，α，β，ρ₁，ρ₂∈R⁺，γ∈R⁺且满足γ＞1，p,q∈N⁺为奇数，且1＜p/q＜2。

Images