[发明专利]一种基于数据手套的水下机器人软体作业手位置跟踪方法

权利要求书

1.一种基于数据手套的水下机器人软体作业手位置跟踪方法，其特征在于，包括步骤：

步骤1.搭建互补滤波器，消除加速度计和磁力计的高频噪声以及陀螺仪的低频噪声，设计离散型卡尔曼滤波器，用四阶龙格库塔法求解四元素微分方程，并通过四元素与欧拉角的换算关系求得当前时刻的用欧拉角表示的姿态角，构建出此过程的状态方程；

步骤2.在状态方程和观测方程中，读取当前陀螺仪数据，并计算状态量的预估计，由加速度和磁力计算出的姿态角数据，以及计算测量过程中的残余；

步骤3.计算卡尔曼增益，并更新系统的状态估计以及误差协方差，等待采样时间Δt，返回执行第一步进行下一时刻角度的估计；

步骤4.搭建水下软体作业手运动学控制增量预测模型，设计动态矩阵预测控制器，设此时为[0t]时间内的n时刻，数据手套捕捉的人手关节位置为当前时刻的坐标值，水下软体作业手关节的位置ψ作为上一周期中的坐标值；

步骤5.用数据手套捕捉的人手关节位置减去水下软体作业手关节的位置ψ得到一个增量，经过模型参数修正水下软体作业手运动学模型；

步骤6.将ψ的值输入到水下软体作业手运动学模型中，预测出下一时刻水下软体作业手坐标的预测值然后在下一周期重复此步骤，并设置限制条件，进行滚动优化实现对水下软体作业手的位置跟踪控制。

2.根据权利要求1所述的一种基于数据手套的水下机器人软体作业手位置跟踪方法，其特征在于，所述步骤1通过四元素与欧拉角的换算关系求得当前时刻的用欧拉角表示的姿态角，构建出此过程的状态方程具体步骤为：

步骤1.1：根据陀螺仪输出的角速度数据建立四元素微分方程，其计算公式为：

即：

其中：ω表示角速度，ω_x、ω_y、ω_z表示角速度分量，Q表示姿态角，表示四元素分量；

步骤1.2：通过四阶龙格库塔法求解上式微分方程可得当前时刻的姿态四元素；

步骤1.3：通过四元素与欧拉角的换算关系下可求得当前时刻的用欧拉角表示的姿态角。

3.根据权利要求1所述的一种基于数据手套的水下机器人软体作业手位置跟踪方法，其特征在于，所述步骤2计算状态量的预估计以及计算测量过程中的残余的具体步骤为：

步骤2.1：在状态方程和观测方程中，读取当前陀螺仪数据；

有缆水下机器人(ROV)数据手套卡尔曼滤波状态方程：

θ_k+1＝θ_k+[ω_k-ω_{err_k}]·Δt+v_k (3)

有缆水下机器人(ROV)数据手套卡尔曼滤波观测方程：

其中：θ_k为第k时刻目标的姿态角，ω_k为第k时刻陀螺仪输出的角速度，ω_{err_k}为第k时刻陀螺仪输出角速度的误差，v_k为输入噪声，Δt为系统的采样周期，θ_k+1为已知θ_k时刻的角度的情况下由陀螺仪数据估计出的在k+1时刻的角度值，ω_k为第k时刻陀螺仪输出的角速度，ε_k是随机信号，y_k+1为k+1时刻的变量值；

步骤2.2：由于陀螺仪进行姿态解算产生的误差主要来源于积分积累，而且陀螺仪测量当前时刻的角速度较为稳定，因此可以将陀螺仪的角速度测量误差视为常量，即

ω_{err_k+1}＝ω_{err_k} (5)

步骤2.3：对于系统而言，θ_k和ω_{err_k}为系统观测的状态，ω_k为系统输入变量，则陀螺仪建立的系统状态矩阵方程为：

其中：A和B为系数矩阵，x_k+1为k+1时刻系统状态向量，v_k是随机信号，属于正态分布的白色噪声，v_k～N(0,Q)；

步骤2.4：根据系统的状态方程(1)和(4)可得系统的状态矩阵如下：

步骤2.5：由系统的状态矩阵(7)和观测方程(4)，可得状态量到观测量的增益矩阵：

H＝(1 0) (8)

其中：H为观测量的增益；

步骤2.6：加速度计和磁力计的姿态角数据y_k，即量测方程为：

y_k＝Hx_k+ε_k (9)

其中：x_k为k时刻系统状态向量；

步骤2.7：测量过程中的残余：

其中，s_k为残余，为预测之差，为先验估计；

步骤2.8：先验估计的误差协方差：

x_k＝Ax_k-1+BU_k+v_k (11)

其中：A和B为系数矩阵，U_k为系统输入量；

根据状态方程可以由系统在k-1时刻的状态x_k-1对当前状态做先验估计，即卡尔曼滤波器的预测部份，此时得到的系统状态值为先验估计值，存在一定的误差；

步骤2.9：此时，由(9)得，量测方程为：

y_k＝Hx_k+ε_k (12)

其中：H为系数矩阵，x_k为k时刻系统状态向量，ε_k是随机信号，属于正态分布的白噪声，ε_k～N(0,R)；

步骤2.10：此时基于系统k-1时刻对过程的k时刻进行的先验估计为对其利用量测方程进行校正后状态的后验估计为则有：

式中测量变量y_k及其预测之差是测量过程的革新，反映了预测值与真实值之间的偏离程度。K_k是卡尔曼增益，其作用是使过程的后验估计误差协方差最小；

P_k∣k-1＝AP_k-1A^T+Q (14)

其中：P_k∣k-1为先验估计的误差协方差。