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

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

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

5.如权利要求1所述的水下辅助采油机器人控制系统，其特征在于，所述水下供电模块(7)包含了9块400v直流转48v直流给电源舱(4)和导航舱(6)供电,1块400v直流转13.8v直流给控制舱(5)供电。

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

7.如权利要求1所述的水下辅助采油机器人控制系统，其特征在于，所述传感器模块(9)包含惯性导航模块(21)，深度计模块(22)；所述惯性导航模块(21)实时采集机器人的姿态信息，包括航向角、俯仰角以及横滚角；所述深度计模块(22)通过感知水压来采集水下辅助采油机器人的深度信息。

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

9.一种如权利要求1所述的水下辅助采油机器人控制系统的动力定位方法，其特征在于，包括以下步骤：步骤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∈R6×6；C(v)＝CRB+CA，为水下辅助采油机器人附加质量的科氏力及向心力矩阵，CRB为水下辅助采油机器人附加质量的科氏力，CA为水下辅助采油机器人向心力矩阵，C(v)∈R6×6；D(vξ)是水下辅助采油机器人流体阻力矩阵，D(vξ)∈R6×6；vξ＝v-vd为去除环境干扰力时水下辅助采油机

6×1

器人的速度；g(η)是由重力和浮力组成的回复力矩阵，g(η)∈R ；τ是水下辅助采油机器人推进器提供的推力，τ∈R6×1；ξ是环境干扰力，ξ∈R6×1；

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

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

式中：x0为状态初始值，为x0的均值,P0为预测协方差初始值；

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

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

(1)Sigma采样：根据对称策略对初始状态采样得到sigma点和对应权值如式(3)式中，L为常数,λ＝α2(L+k)-L表示一个尺度参量，α参量表示采样点在 附近的遍布范围，1e-4≤α≤1，k被设置为0或者3-L，Xi为sigma点，为随机变量x的均值，Px为协方差，构造一个由Xi组成的2L+1维矩阵X,其中Xi对应权重Wi因子如式(4)式中，Wi(m)为均值，Wi(c)为协方差的加权值,β包含x的先验信息，对于高斯分布表示矩阵平方根的第i列；

(2)一步预测方程：

式中，Xk-1为k-1时刻的sigma点，f(Xk-1)为Xk-1到Xk的一步转移矩阵； 为k-1时刻的状态值，Pk|k-1为k-1时刻的预测协方差；

(3)sigma重采样过程：根据对称采样策略，对一步预测状态 进行重采样如式式中，Sk|k-1为Pk|k-1的矩阵平方根，Xk|k-1每列分别代表一个采样向量点Xi,k|k-1；

(4)输出的一步预测：

式中， 为量测矩阵，zk|k-1为输出预测值；

(5)量测更新方程

式中， 为误差协方差， 为互协方差，和Pk为量测更新值,K为滤波增益；

(6)自适应更新

其中λk＝diag{λ1,λ2,…,λm}k，

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

Rk为高斯白噪声矩阵；

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

(7)采样：

式中，q(xk|x0:k-1,z1:k)为重要密度函数；

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

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

3.权值归一化处理

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

(1)若随机数Uc＜Pc，交叉概率Pc子代粒子的数目和总粒子数的比值式中：为交叉因子： 和 为k时刻父代粒子： 和 为子代粒子；

若 接受 否则以以下概率接受如式

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

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

式中Pc1，Pc2表示Pc的变范围，常数A对Pc的变化趋势进行调节，且使得Pc的变化比较缓慢，fmax是种群中最大适应度值，f′是两个交叉个体中较大的适应度值，favg是种群适应度的平均值，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＝[x1,x2]T∈R2为系统状态；u∈R为系统控制输入；f(x,t),g(x,t)≠0为x的光滑非线性函数；

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

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