[发明专利]水下机器人控制系统及动力定位方法

权利要求书

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⁺。