CN114111772B - 一种基于数据手套的水下机器人软体作业手位置跟踪方法 - Google Patents

Abstract

本发明针对数据手套的动态捕捉准确性和水下软体作业手位置跟踪的不准确性，公开了一种基于数据手套的水下机器人软体作业手位置跟踪方法。针对数据手套对人手部关节进行数据捕捉时角度不准确的问题，将姿态融合算法运用到数据手套的姿态采集中，融合三轴磁力计、三轴加速度计和三轴陀螺仪的数据求解出姿态角度。针对水下软体作业手对人手部位置跟踪的问题，本发明提出采用动态矩阵预测控制，通过设计轨迹跟踪误差性能优化指标，建立跟踪误差约束条件，并把满足约束条件的性能优化问题，转化为求解控制增量的二次规划问题，获得时域内满足误差约束条件的预测控制，提高了数据手套动态捕捉和水下软体作业手的位置跟踪的准确性。

Description

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

技术领域

本发明涉及数据手套动态捕捉及水下软体作业手位置跟踪技术领域，尤其涉及一种基于数据手套的水下机器人软体作业手位置跟踪方法。

背景技术

近年有缆水下机器人(ROV)作业技术的发展，越来越多的复杂作业任务被有缆水下机器人(ROV)所承担，水下机器人的人机交互也成为了水下机器人作业技术的研究热点以及未来方向。基于动作捕捉技术研制的数据手套可以实现对人手手指姿态采集，将采集到的手指弯曲角度和手臂姿态数据转换为机械手驱动电机的旋转角度，从而实现对机械手姿态的远程控制。动作捕捉技术是一种将人体在三维空间的运动姿态转换为数字信息的技术，目前已经广泛使用于电脑动画、体育和教育、互动游戏、虚拟现实、医学研究等人机交互领域，从捕捉原理层面分类，动作捕捉技术可以分为光学式、机械式、惯性传感器等。其中常用的动作捕捉技术有光学式、机械式和基于惯性传感器六大类。其中光学式动作捕捉需要在捕捉对象身上预先设置光学标记点，然后用摄像机对标记进行跟踪，通过对拍摄到的标记点视频进行图像分析来完成高精度的动作捕捉，光学式动作捕捉***整体造价昂贵，并且对环境的光照和反射情况要求比较严格，因此往往适用于3D电影拍摄等场景；

现在最接近本文算法的是金杰在一种基于视觉与惯性信息融合的手部姿态估计方法及***。其提出首先构建手部姿态数据。然后进行特征提取，包括通过Resnet50残差网络对AR眼镜获取的彩色图像进行视觉信息特征提取最终得到图像特征向量；通过构建卷积神经网络进行惯性信息特征提取，得到惯性信息特征向量；将图像特征向量和惯性信息特征向量连接后得到融合后的特征向量。再进行手部的2D和3D姿态估计。而后通过网络训练及测试将训练好的手部姿态估计网络模型部署到AR眼镜，通过调用彩色相机以及数据手套，进行实时手部姿态估计。

上述最接近本文的技术方案，虽优点很明显，对本身不确定参数及外部环境的适应力强，但加速度计在运动时容易产生高频噪声，其动态特性差，同时陀螺仪低段动态频特性差，会随着时间逐渐变大积分误差。以及惯性测量单元中只有三轴加速度计和三轴陀螺仪，无法测得手指的横移，导致该算法无法精准的捕捉手部的精准位置，不容易应用于实际工程。

同时水下软体作业手在进行抓取的过程中软体机作业手抓取的目标具有高价值，水下软体作业手运动位置对保证目标安全完好具有重要意义。普通滑模控制或者PID控制位置会超过目标承受能力，导致目标与水下软体作业手运动不协调而再次受伤；位置状态超出空间环境限制，水下软体作业手也会碰撞周围的物体。

发明内容

本发明旨在提出一种基于数据手套的水下机器人软体作业手位置跟踪方法，目的在于提高水下软体作业手抓取高价值目标物的能力，保护其完整性，在数据手套动态捕捉方面，本发明提出改进的姿态融合算法对九轴惯性传感器数据进行融合，解算出准确的人手部关节位置，解决数据手套只依靠角度传感器测量人手部关节位置容易出现误差的问题；在水下软体作业手对人手位置跟踪控制方面，本发明提出改进的动态矩阵预测控制算法来提高***的鲁棒性和抗干扰能力，通过添加限制条件，滚动优化，解决水下软体作业手与人手位置误差的问题；本发明设计的数据手套动态捕捉及水下软体作业手位置跟踪方法能使有缆水下机器人搭载水下软体作业手开展水下高价值物体探测、采集工作。

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

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

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

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

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

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

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

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

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

进一步的，步骤(1)具体包括：

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

即：

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

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

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

进一步的，步骤(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为先验估计的误差协方差。

进一步的，步骤(3)具体包括：

步骤(3.1)：卡尔曼增益K_k：

K_k＝P_k∣k-1H^T[HP_k∣k-1H^T+R]^-1 (15)

其中：R为自适应接纳圆半径；

步骤(3.2)：更新状态，即当前状态以及此状态下的误差协方差为：

步骤(3.3)：误差协方差P_k：

P_k＝(1-K_kH)P_k∣k-1 (17)

步骤(3.4)：等待采样时间，返回执行第一步进行下一时刻角度的估计。

进一步的，步骤(4)具体包括：

建立手指单向弯曲运动学模型：

其中：^jP_ij为各关节铰链的中心点，即连接点；表示第i个手指上的连接点从j坐标系向j-1坐标系转化时的齐次坐标变换阵。θ_ij为单向弯曲关节的转动角度；L_ij为各关节杆长，s、c为sin、cos的简写。x_i和y_i分别表示各个手指坐标系原点在手掌坐标系中的坐标值。

进一步的，步骤(5)具体包括：

步骤(5.1)：对手指单向弯曲运动学模型在期望路径点处进行一阶泰勒展开线性化处理，得到如下的预测模型：

其中： A、B为雅可比矩阵；

步骤(5.2)：对模型进行线性化和离散化处理，得到控制增量形式的状态空间模型：

其中： γ表示输出量。

进一步的，步骤(6)具体包括：

步骤(6.1)：将ψ的值输入到水下软体作业手运动学模型中，预测出下一时刻水下软体作业手坐标的预测值然后在下一周期重复此步骤；

步骤(6.2)：设置约束条件，在当前时刻及预测时域内，对控制增量、控制量、输出量进行如下限制：

γ_min≤γ(k+i)≤γ_max,i＝0,1,2,…,N_p (27)

其中：N_C表示控制时域，N_p表示预测时域，表示k+i时刻的控制增量，表示k+i时刻的控制量，γ(k+i)表示k+i时刻的输出量，/>表示控制增量最小值，/>表示控制增量最大值，/>表示控制量最小值，/>表示控制量最大值，根据手指弯曲性能选取。γ_min表示输出量最小值，γ_max表示输出量最大值；

步骤(6.3)：进行滚动优化使未来一段时间内被控变量与期望值偏差最小；

式中，γ表示当前时刻的输出值，γ_ref表示自适应视线法处理后的期望值，表示控制增量，Q和R表示权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，J表示性能指标。

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

1、水下机器人数据手套***在动作捕捉中具有更好的抗干扰能力，互补滤波器中的一阶低通滤波器可以有效抑制加速度计在运动时产生的高频噪声，一阶高通滤波器可以有效地抑制陀螺仪的低频噪声，消除其随时间逐渐变大积分误差的缺点。

2、水下机器人数据手套***在动作捕捉中具有更准确的采集能力，引入三轴磁力计，与三轴加速度计、三轴陀螺仪组成九轴惯性传感器，在增加对手部横移采集能力同时，更加有效的消除噪声，使动态捕捉更加精准。

3、水下机器人数据手套***在动态捕捉中具有更好的解算能力，引入姿态融合算法，结合各自传感器的优势，再结合卡尔曼滤波算法，使传感器的数据结算的更加准确，从而得到精确的手部动作位置。

4、水下软体作业手在抓取目标时具备更加准确的位置跟踪能力，采用动态矩阵预测控制，通过让人手关节位置与水下软体作业手关节位置作差，不断改善和限制水下软体作业手的位置，使水下软体作业手关节的位置大福趋近于人手部的位置，保证抓取的准确性，减少误操作的发生。

5、水下软体作业手采用动态矩阵预测控制设计的控制器，该方法采用滚动优化策略，有较好的动态控制性能，且该方法设计的闭环控制***具有很强的抗干扰能力。

6、本发明结合数据手套、姿态融合算法、动态矩阵控制算法对水下软体作业手进行位置跟踪控制，能高效抓取水下高价值目标，保护其完整性。

附图说明

图1是基于数据手套的水下机器人软体作业手位置跟踪方法框图；

图2是姿态融合算法流程图；

图3是卡尔曼滤波过程图；

图4是水下软体作业手位置动态矩阵预测控制流程图。

具体实施措施

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

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

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

即：

其中：ω表示角速度，Q表示姿态角；

通过四阶龙格库塔法求解上式微分方程可得当前时刻的姿态四元素，通过四元素与欧拉角的换算关系下可求得当前时刻的用欧拉角表示的姿态角：

其中：h为求解步长，k_i为系数。

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

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

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

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

其中：θ_k为第k时刻目标的姿态角，ω_k为第k时刻陀螺仪输出的角速度，ω_{err_k}为第k时刻陀螺仪输出角速度的误差，v_t为输入噪声，Δt为***的采样周期，θ_k+1为已知θ_k时刻的角度的情况下由陀螺仪数据估计出的在k+1时刻的角度值；ω_k为第k时刻陀螺仪输出的角速度，由于陀螺仪进行姿态解算产生的误差主要来源于积分积累，而且陀螺仪测量当前时刻的角速度较为稳定，因此可以将陀螺仪的角速度测量误差视为常量，即

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

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

根据***的状态方程(29)和(32)可得***的状态矩阵如下：

由***的状态矩阵(35)和观测方程(32)，可得状态量到观测量的增益矩阵：

H＝(1 0) (37)

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

y_k＝Hx_k+ε_k (38)

测量过程中的残余：

先验估计的误差协方差：

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

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

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

此时，由(37)得，量测方程为：

y_k＝Hx_k+ε_k (41)

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

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

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

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

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

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

卡尔曼增益K_k：

K_k＝P_k∣k-1H^T[HP_k∣k-1H^T+R]^-1 (44)

更新状态，即当前状态以及此状态下的误差协方差为：

误差协方差P_k：

P_k＝(1-K_kH)P_k∣k-1 (46)

等待采样时间，返回执行第一步进行下一时刻角度的估计。

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

建立手指单向弯曲运动学模型：

其中：^jP_ij为各关节铰链的中心点，即连接点；表示第i个手指上的连接点从j坐标系向j-1坐标系转化时的齐次坐标变换阵；θ_ij为单向弯曲关节的转动角度；L_ij为各关节杆长，s、c为sin、cos的简写；x_i和y_i分别表示各个手指坐标系原点在手掌坐标系中的坐标值。

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

对手指单向弯曲运动学模型在期望路径点处进行一阶泰勒展开线性化处理，得到如下的预测模型：

其中： A、B为雅可比矩阵；

对模型进行线性化和离散化处理，得到控制增量形式的状态空间模型：

其中： γ表示输出量。

将ψ的值输入到水下软体作业手运动学模型中，预测出下一时刻水下软体作业手坐标的预测值然后在下一周期重复此步骤，实现对水下软体作业手的位置跟踪控制；

同时设置约束条件，在当前时刻及预测时域内，对控制增量、控制量、输出量进行如下限制：

其中：N_C表示控制时域，N_p表示预测时域，表示k+i时刻的控制增量，表示k+i时刻的控制量，γ(k+i)表示k+i时刻的输出量，/>表示控制增量最小值，/>表示控制增量最大值，/>表示控制量最小值，/>表示控制量最大值，根据手指弯曲性能选取。γ_min表示输出量最小值，γ_max表示输出量最大值。

进行滚动优化使未来一段时间内被控变量与期望值偏差最小；

式中，γ表示当前时刻的输出值，γ_ref表示自适应视线法处理后的期望值，表示控制增量，Q和R表示权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，J表示性能指标。/>

Claims (7)

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为先验估计的误差协方差。

4.根据权利要求1所述的一种基于数据手套的水下机器人软体作业手位置跟踪方法，其特征在于，所述步骤3计算卡尔曼增益，并更新***的状态估计以及误差协方差，等待采样时间Δt，返回执行第一步进行下一时刻角度的估计具体步骤为：

步骤3.1：卡尔曼增益K_k：

K_k＝P_k∣k-1H^T[HP_k∣k-1H^T+R]^-1 (15)

其中：R为自适应接纳圆半径；

步骤3.2：更新状态，即当前状态以及此状态下的误差协方差为：

步骤3.3：误差协方差P_k：

P_k＝(1-K_kH)P_k∣k-1 (17)

步骤3.4：等待采样时间，返回执行第一步进行下一时刻角度的估计。

5.根据权利要求1所述的一种基于数据手套的水下机器人软体作业手位置跟踪方法，其特征在于，所述步骤4搭建水下软体作业手运动学控制增量预测模型；设计动态矩阵预测控制器，设此时为[0 t]时间内的n时刻，数据手套捕捉的人手关节位置为当前时刻的坐标值，水下软体作业手关节的位置ψ作为上一周期中的坐标值具体步骤为：

建立手指单向弯曲运动学模型：

其中：^jP_ij为各关节铰链的中心点，即连接点；表示第i个手指上的连接点从j坐标系向j-1坐标系转化时的齐次坐标变换阵；θ_ij为单向弯曲关节的转动角度；L_ij为各关节杆长，s、c为sin、cos的简写；x_i和y_i分别表示各个手指坐标系原点在手掌坐标系中的坐标值。

6.根据权利要求1所述的一种基于数据手套的水下机器人软体作业手位置跟踪方法，其特征在于，所述步骤5用数据手套捕捉的人手关节位置减去水下软体作业手关节的位置ψ得到一个增量，经过模型参数修正水下软体作业手运动学模型，具体步骤为：

步骤5.1：对手指单向弯曲运动学模型在期望路径点(ψ_R,)处进行一阶泰勒展开线性化处理，得到如下的预测模型：

其中：A、B为雅可比矩阵；

步骤5.2：对模型进行线性化和离散化处理，得到控制增量形式的状态空间模型：

其中：γ表示输出量。

7.根据权利要求1所述的一种基于数据手套的水下机器人软体作业手位置跟踪方法，其特征在于，所述步骤6预测下一时刻坐标值；对控制增量、控制量、输出量进行限制，并进行滚动优化的具体步骤为：

步骤6.1：将ψ的值输入到水下软体作业手运动学模型中，预测出下一时刻水下软体作业手坐标的预测值然后在下一周期重复此步骤；

步骤6.2：设置约束条件，在当前时刻及预测时域内，对控制增量、控制量、输出量进行如下限制：

γ_min≤γ(k+i)≤γ_max,i＝0,1,2,…,N_p (27)

其中：N_C表示控制时域，N_p表示预测时域，表示k+i时刻的控制增量，/>表示k+i时刻的控制量，γ(k+i)表示k+i时刻的输出量，/>表示控制增量最小值，表示控制增量最大值，/>表示控制量最小值，/>表示控制量最大值，根据手指弯曲性能选取；γ_min表示输出量最小值，γ_max表示输出量最大值；

步骤6.3：进行滚动优化使未来一段时间内被控变量与期望值偏差最小；

式中，γ表示当前时刻的输出值，γ_ref表示自适应视线法处理后的期望值，表示控制增量，Q和R表示权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，J表示性能指标。