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

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


背景技术：

2.近年有缆水下机器人(rov)作业技术的发展，越来越多的复杂作业任务被有缆水下机器人(rov)所承担，水下机器人的人机交互也成为了水下机器人作业技术的研究热点以及未来方向。基于动作捕捉技术研制的数据手套可以实现对人手手指姿态采集，将采集到的手指弯曲角度和手臂姿态数据转换为机械手驱动电机的旋转角度，从而实现对机械手姿态的远程控制。动作捕捉技术是一种将人体在三维空间的运动姿态转换为数字信息的技术，目前已经广泛使用于电脑动画、体育和教育、互动游戏、虚拟现实、医学研究等人机交互领域，从捕捉原理层面分类，动作捕捉技术可以分为光学式、机械式、惯性传感器等。其中常用的动作捕捉技术有光学式、机械式和基于惯性传感器六大类。其中光学式动作捕捉需要在捕捉对象身上预先设置光学标记点，然后用摄像机对标记进行跟踪，通过对拍摄到的标记点视频进行图像分析来完成高精度的动作捕捉，光学式动作捕捉系统整体造价昂贵，并且对环境的光照和反射情况要求比较严格，因此往往适用于3d电影拍摄等场景；
3.现在最接近本文算法的是金杰在一种基于视觉与惯性信息融合的手部姿态估计方法及系统。其提出首先构建手部姿态数据。然后进行特征提取，包括通过resnet50残差网络对ar眼镜获取的彩色图像进行视觉信息特征提取最终得到图像特征向量；通过构建卷积神经网络进行惯性信息特征提取，得到惯性信息特征向量；将图像特征向量和惯性信息特征向量连接后得到融合后的特征向量。再进行手部的2d和3d姿态估计。而后通过网络训练及测试将训练好的手部姿态估计网络模型部署到ar眼镜，通过调用彩色相机以及数据手套，进行实时手部姿态估计。
4.上述最接近本文的技术方案，虽优点很明显，对本身不确定参数及外部环境的适应力强，但加速度计在运动时容易产生高频噪声，其动态特性差，同时陀螺仪低段动态频特性差，会随着时间逐渐变大积分误差。以及惯性测量单元中只有三轴加速度计和三轴陀螺仪，无法测得手指的横移，导致该算法无法精准的捕捉手部的精准位置，不容易应用于实际工程。
5.同时水下软体作业手在进行抓取的过程中软体机作业手抓取的目标具有高价值，水下软体作业手运动位置对保证目标安全完好具有重要意义。普通滑模控制或者pid控制位置会超过目标承受能力，导致目标与水下软体作业手运动不协调而再次受伤；位置状态超出空间环境限制，水下软体作业手也会碰撞周围的物体。


技术实现要素：

6.本发明旨在提出一种基于数据手套的水下机器人软体作业手位置跟踪方法，目的在于提高水下软体作业手抓取高价值目标物的能力，保护其完整性，在数据手套动态捕捉
方面，本发明提出改进的姿态融合算法对九轴惯性传感器数据进行融合，解算出准确的人手部关节位置，解决数据手套只依靠角度传感器测量人手部关节位置容易出现误差的问题；在水下软体作业手对人手位置跟踪控制方面，本发明提出改进的动态矩阵预测控制算法来提高系统的鲁棒性和抗干扰能力，通过添加限制条件，滚动优化，解决水下软体作业手与人手位置误差的问题；本发明设计的数据手套动态捕捉及水下软体作业手位置跟踪方法能使有缆水下机器人搭载水下软体作业手开展水下高价值物体探测、采集工作。
7.本发明的目的通过以下技术方案予以实现：
8.一种基于数据手套的水下机器人软体作业手位置跟踪方法，包括以下步骤：
9.步骤1.搭建互补滤波器，消除加速度计和磁力计的高频噪声以及陀螺仪的低频噪声，设计离散型卡尔曼滤波器，用四阶龙格库塔法求解四元素微分方程，并通过四元素与欧拉角的换算关系求得当前时刻的用欧拉角表示的姿态角，构建出此过程的状态方程；
10.步骤2.在状态方程和观测方程中，读取当前陀螺仪数据。并计算状态量的预估计，由加速度和磁力计算出的姿态角数据，以及计算测量过程中的残余；
11.步骤3.计算卡尔曼增益，并更新系统的状态估计以及误差协方差，等待采样时间δt，返回执行第一步进行下一时刻角度的估计；
12.步骤4.搭建水下软体作业手运动学控制增量预测模型。设计动态矩阵预测控制器，设此时为[0 t]时间内的n时刻，数据手套捕捉的人手关节位置为当前时刻的坐标值，水下软体作业手关节的位置ψ作为上一周期中的坐标值；
[0013]
步骤5.用数据手套捕捉的人手关节位置减去水下软体作业手关节的位置ψ得到一个增量，经过模型参数修正水下软体作业手运动学模型；
[0014]
步骤6.将ψ的值输入到水下软体作业手运动学模型中，预测出下一时刻水下软体作业手坐标的预测值然后在下一周期重复此步骤，并设置限制条件，进行滚动优化实现对水下软体作业手的位置跟踪控制。
[0015]
本发明的目的还可以通过以下技术措施来进一步实现：
[0016]
进一步的，步骤(1)具体包括：
[0017]
步骤(1.1)：根据陀螺仪输出的角速度数据建立四元素微分方程，其计算公式为：
[0018][0019]
即：
[0020][0021]
其中：ω表示角速度，ω
x
、ωy、ωz表示角速度分量，q表示姿态角，表示四元素分量；
[0022]
步骤(1.2)：通过四阶龙格库塔法求解上式微分方程可得当前时刻的姿态四元素；
[0023]
步骤(1.3)：通过四元素与欧拉角的换算关系下可求得当前时刻的用欧拉角表示的姿态角。
[0024]
进一步的，步骤(2)具体包括：
[0025]
步骤(2.1)：在状态方程和观测方程中，读取当前陀螺仪数据；
[0026]
有缆水下机器人(rov)数据手套卡尔曼滤波状态方程：
[0027]
θ
k 1
＝θk [ω
k-ω
err_k
]
·
δt vkꢀꢀꢀꢀꢀꢀ
(3)
[0028]
有缆水下机器人(rov)数据手套卡尔曼滤波观测方程：
[0029][0030]
其中：θk为第k时刻目标的姿态角，ωk为第k时刻陀螺仪输出的角速度，ω
err_k
为第k时刻陀螺仪输出角速度的误差，vk为输入噪声，δt为系统的采样周期，θ
k 1
为已知θk时刻的角度的情况下由陀螺仪数据估计出的在k 1时刻的角度值，ωk为第k时刻陀螺仪输出的角速度，εk是随机信号，y
k 1
为k 1时刻的变量值；
[0031]
步骤(2.2)：由于陀螺仪进行姿态解算产生的误差主要来源于积分积累，而且陀螺仪测量当前时刻的角速度较为稳定，因此可以将陀螺仪的角速度测量误差视为常量，即
[0032]
ω
err_k 1
＝ω
err_k
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0033]
步骤(2.3)：对于系统而言，θk和ω
err_k
为系统观测的状态，ωk为系统输入变量，则陀螺仪建立的系统状态矩阵方程为：
[0034][0035]
其中：a和b为系数矩阵，x
k 1
为k 1时刻系统状态向量，vk是随机信号，属于正态分布的白色噪声，vk～n(0,q)；
[0036]
步骤(2.4)：根据系统的状态方程(1)和(4)可得系统的状态矩阵如下：
[0037][0038]
步骤(2.5)：由系统的状态矩阵(7)和观测方程(4)，可得状态量到观测量的增益矩阵：
[0039]
h＝(1 0)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0040]
其中：h为观测量的增益；
[0041]
步骤(2.6)：加速度计和磁力计的姿态角数据yk，即量测方程为：
[0042]
yk＝hxk εkꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0043]
其中：，xk为k时刻系统状态向量；
[0044]
步骤(2.7)：测量过程中的残余：
[0045][0046]
其中，sk为残余，为预测之差，为先验估计；
[0047]
步骤(2.8)：先验估计的误差协方差：
[0048]
xk＝ax
k-1
buk vkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0049]
其中：a和b为系数矩阵，uk为系统输入量；
[0050]
根据状态方程可以由系统在k-1时刻的状态x
k-1
对当前状态做先验估计，即卡尔曼滤波器的预测部份，此时得到的系统状态值为先验估计值，存在一定的误差；
[0051]
步骤(2.9)：此时，由(9)得，量测方程为：
[0052]
yk＝hxk εkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0053]
其中：h为系数矩阵，xk为k时刻系统状态向量，εk是随机信号，属于正态分布的白噪声，εk～n(0,r)；
[0054]
步骤(2.10)：此时基于系统k-1时刻对过程的k时刻进行的先验估计为对其利用量测方程进行校正后状态的后验估计为则有：
[0055][0056]
式中测量变量yk及其预测之差是测量过程的革新，反映了预测值与真实值之间的偏离程度。kk是卡尔曼增益，其作用是使过程的后验估计误差协方差最小；
[0057]
p
k∣k-1
＝ap
k-1at
q
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0058]
其中：p
k∣k-1
为先验估计的误差协方差。
[0059]
进一步的，步骤(3)具体包括：
[0060]
步骤(3.1)：卡尔曼增益kk：
[0061]kk
＝p
k∣k-1ht
[hp
k∣k-1ht
r]-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0062]
其中：r为自适应接纳圆半径；
[0063]
步骤(3.2)：更新状态，即当前状态以及此状态下的误差协方差为：
[0064][0065]
步骤(3.3)：误差协方差pk：
[0066]
pk＝(1-kkh)p
k∣k-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0067]
步骤(3.4)：等待采样时间，返回执行第一步进行下一时刻角度的估计。
[0068]
进一步的，步骤(4)具体包括：
[0069]
建立手指单向弯曲运动学模型：
[0070][0071][0072][0073][0074]
其中：jp
ij
为各关节铰链的中心点，即连接点；表示第i个手指上的连接点从j
坐标系向j-1坐标系转化时的齐次坐标变换阵。θ
ij
为单向弯曲关节的转动角度；l
ij
为各关节杆长，s、c为sin、cos的简写。xi和yi分别表示各个手指坐标系原点在手掌坐标系中的坐标值。
[0075]
进一步的，步骤(5)具体包括：
[0076]
步骤(5.1)：对手指单向弯曲运动学模型在期望路径点处进行一阶泰勒展开线性化处理，得到如下的预测模型：
[0077][0078]
其中：其中：a、b为雅可比矩阵；
[0079]
步骤(5.2)：对模型进行线性化和离散化处理，得到控制增量形式的状态空间模型：
[0080][0081][0082]
其中：其中：其中：其中：γ表示输出量。
[0083]
进一步的，步骤(6)具体包括：
[0084]
步骤(6.1)：将ψ的值输入到水下软体作业手运动学模型中，预测出下一时刻水下软体作业手坐标的预测值然后在下一周期重复此步骤；
[0085]
步骤(6.2)：设置约束条件，在当前时刻及预测时域内，对控制增量、控制量、输出量进行如下限制：
[0086][0087][0088]
γ
min
≤γ(k i)≤γ
max
,i＝0,1,2,
…
,n
p
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(27)
[0089]
其中：nc表示控制时域，n
p
表示预测时域，表示k i时刻的控制增量，表示k i时刻的控制量，γ(k i)表示k i时刻的输出量，表示控制增量最小值，表示控制增量最大值，表示控制量最小值，表示控制量最大值，根据手指弯曲性能选取。γ
min
表示输出量最小值，γ
max
表示输出量最大值；
[0090]
步骤(6.3)：进行滚动优化使未来一段时间内被控变量与期望值偏差最小；
[0091][0092]
式中，γ表示当前时刻的输出值，γ
ref
表示自适应视线法处理后的期望值，表示控制增量，q和r表示权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，j表示性能指标。
[0093]
与现有技术相比，本发明的有益效果是：
[0094]
1、水下机器人数据手套系统在动作捕捉中具有更好的抗干扰能力，互补滤波器中的一阶低通滤波器可以有效抑制加速度计在运动时产生的高频噪声，一阶高通滤波器可以
有效地抑制陀螺仪的低频噪声，消除其随时间逐渐变大积分误差的缺点。
[0095]
2、水下机器人数据手套系统在动作捕捉中具有更准确的采集能力，引入三轴磁力计，与三轴加速度计、三轴陀螺仪组成九轴惯性传感器，在增加对手部横移采集能力同时，更加有效的消除噪声，使动态捕捉更加精准。
[0096]
3、水下机器人数据手套系统在动态捕捉中具有更好的解算能力，引入姿态融合算法，结合各自传感器的优势，再结合卡尔曼滤波算法，使传感器的数据结算的更加准确，从而得到精确的手部动作位置。
[0097]
4、水下软体作业手在抓取目标时具备更加准确的位置跟踪能力，采用动态矩阵预测控制，通过让人手关节位置与水下软体作业手关节位置作差，不断改善和限制水下软体作业手的位置，使水下软体作业手关节的位置大福趋近于人手部的位置，保证抓取的准确性，减少误操作的发生。
[0098]
5、水下软体作业手采用动态矩阵预测控制设计的控制器，该方法采用滚动优化策略，有较好的动态控制性能，且该方法设计的闭环控制系统具有很强的抗干扰能力。
[0099]
6、本发明结合数据手套、姿态融合算法、动态矩阵控制算法对水下软体作业手进行位置跟踪控制，能高效抓取水下高价值目标，保护其完整性。
附图说明
[0100]
图1是基于数据手套的水下机器人软体作业手位置跟踪方法框图；
[0101]
图2是姿态融合算法流程图；
[0102]
图3是卡尔曼滤波过程图；
[0103]
图4是水下软体作业手位置动态矩阵预测控制流程图。
[0104]
具体实施措施
[0105]
下面结合附图和具体实施例对本发明作进一步说明。
[0106]
根据图1所示，搭建互补滤波器，消除加速度计和磁力计的高频噪声以及陀螺仪的低频噪声，设计离散型卡尔曼滤波器，用四阶龙格库塔法求解四元素微分方程，并通过四元素与欧拉角的换算关系求得当前时刻的用欧拉角表示的姿态角，构建出此过程的状态方程；
[0107]
根据陀螺仪输出的角速度数据建立四元素微分方程，其计算公式为：
[0108][0109]
即：
[0110][0111]
其中：ω表示角速度，q表示姿态角；
[0112]
通过四阶龙格库塔法求解上式微分方程可得当前时刻的姿态四元素，通过四元素与欧拉角的换算关系下可求得当前时刻的用欧拉角表示的姿态角：
[0113][0114]
其中：h为求解步长，ki为系数。
[0115]
根据图2所示，在状态方程和观测方程中，读取当前陀螺仪数据。并计算状态量的预估计，由加速度和磁力计算出的姿态角数据，以及计算测量过程中的残余：
[0116]
1)有缆水下机器人(rov)数据手套卡尔曼滤波状态方程：
[0117]
θ
k 1
＝θk [ω
k-ω
err_k
]
·
δt vkꢀꢀꢀꢀꢀꢀꢀ
(32)
[0118]
2)有缆水下机器人(rov)数据手套卡尔曼滤波观测方程：
[0119][0120]
其中：θk为第k时刻目标的姿态角，ωk为第k时刻陀螺仪输出的角速度，ω
err_k
为第k时刻陀螺仪输出角速度的误差，v
t
为输入噪声，δt为系统的采样周期，θ
k 1
为已知θk时刻的角度的情况下由陀螺仪数据估计出的在k 1时刻的角度值；ωk为第k时刻陀螺仪输出的角速度，由于陀螺仪进行姿态解算产生的误差主要来源于积分积累，而且陀螺仪测量当前时刻的角速度较为稳定，因此可以将陀螺仪的角速度测量误差视为常量，即
[0121]
ω
err_k 1
＝ω
err_k
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(34)
[0122]
对于系统而言，θk和ω
err_k
为系统观测的状态，ωk为系统输入变量，则陀螺仪建立的系统状态矩阵方程为：
[0123][0124]
根据系统的状态方程(29)和(32)可得系统的状态矩阵如下：
[0125][0126]
由系统的状态矩阵(35)和观测方程(32)，可得状态量到观测量的增益矩阵：
[0127]
h＝(1 0)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(37)
[0128]
加速度计和磁力计的姿态角数据yk，即量测方程为：
[0129]
yk＝hxk εkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(38)
[0130]
测量过程中的残余：
[0131][0132]
先验估计的误差协方差：
[0133]
xk＝ax
k-1
buk vkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(40)
[0134]
其中：a和b为系数矩阵，xk为k时刻系统状态向量，uk为系统输入量，vk是随机信号，属于正态分布的白色噪声，vk～n(0,q)。
[0135]
根据图3所示，根据状态方程可以由系统在k-1时刻的状态x
k-1
对当前状态做先验估计，即卡尔曼滤波器的预测部份，此时得到的系统状态值为先验估计值，存在一定的误差；
[0136]
此时，由(37)得，量测方程为：
[0137]
yk＝hxk εkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(41)
[0138]
其中：h为系数矩阵，xk为k时刻系统状态向量，εk是随机信号，属于正态分布的白噪声，εk～n(0,r)；
[0139]
此时基于系统k-1时刻对过程的k时刻进行的先验估计为对其利用量测方程进行校正后状态的后验估计为则有：
[0140][0141]
式中测量变量yk及其预测之差是测量过程的革新，反映了预测值与真实值之间的偏离程度。kk是卡尔曼增益，其作用是使过程的后验估计误差协方差最小；
[0142]
其中：p
k∣k-1
为先验估计的误差协方差。
[0143]
p
k∣k-1
＝ap
k-1at
q
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(43)
[0144]
计算卡尔曼增益，并更新系统的状态估计以及误差协方差，等待采样时间δt，返回执行第一步进行下一时刻角度的估计；
[0145]
卡尔曼增益kk：
[0146]kk
＝p
k∣k-1ht
[hp
k∣k-1ht
r]-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(44)
[0147]
更新状态，即当前状态以及此状态下的误差协方差为：
[0148][0149]
误差协方差pk：
[0150]
pk＝(1-kkh)p
k∣k-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(46)
[0151]
等待采样时间，返回执行第一步进行下一时刻角度的估计。
[0152]
根据图4所示，搭建水下软体作业手运动学控制增量预测模型；设计动态矩阵预测控制器，设此时为[0 t]时间内的n时刻，数据手套捕捉的人手关节位置为当前时刻的坐标值，水下软体作业手关节的位置ψ作为上一周期中的坐标值；
[0153]
建立手指单向弯曲运动学模型：
[0154][0155][0156][0157][0158]
其中：jp
ij
为各关节铰链的中心点，即连接点；表示第i个手指上的连接点从j坐标系向j-1坐标系转化时的齐次坐标变换阵；θ
ij
为单向弯曲关节的转动角度；l
ij
为各关节杆长，s、c为sin、cos的简写；xi和yi分别表示各个手指坐标系原点在手掌坐标系中的坐标值。
[0159]
用数据手套捕捉的人手关节位置减去水下软体作业手关节的位置ψ得到一个增量，经过模型参数修正水下软体作业手运动学模型；
[0160]
对手指单向弯曲运动学模型在期望路径点处进行一阶泰勒展开线性化处理，得到如下的预测模型：
[0161][0162]
其中：其中：a、b为雅可比矩阵；
[0163]
对模型进行线性化和离散化处理，得到控制增量形式的状态空间模型：
[0164][0165][0166]
其中：其中：其中：其中：γ表示输出量。
[0167]
将ψ的值输入到水下软体作业手运动学模型中，预测出下一时刻水下软体作业手坐标的预测值然后在下一周期重复此步骤，实现对水下软体作业手的位置跟踪控制；
[0168]
同时设置约束条件，在当前时刻及预测时域内，对控制增量、控制量、输出量进行
如下限制：
[0169][0170][0171][0172]
其中：nc表示控制时域，n
p
表示预测时域，表示k i时刻的控制增量，表示k i时刻的控制量，γ(k i)表示k i时刻的输出量，表示控制增量最小值，表示控制增量最大值，表示控制量最小值，表示控制量最大值，根据手指弯曲性能选取。γ
min
表示输出量最小值，γ
max
表示输出量最大值。
[0173]
进行滚动优化使未来一段时间内被控变量与期望值偏差最小；
[0174][0175]
式中，γ表示当前时刻的输出值，γ
ref
表示自适应视线法处理后的期望值，表示控制增量，q和r表示权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，j表示性能指标。