一种基于惯导网络的灵巧手末端位姿信息在线获取方法与流程

1.本发明涉及一种基于惯导网络的灵巧手末端位姿信息在线获取方法，用于在线获取灵巧手等智能机械装置末端位置/姿态信息，为高精度位置/姿态控制提供闭环反馈数据。


背景技术：

2.针对复杂灵巧精细作业任务需求，灵巧手需要高精度实时反馈手指末端位置与姿态信息，从而实现多指手与物体之间的接触位置规划和抓取姿态规划，确保手指末端在抓取操作的最佳位置和姿态，保证灵巧手的构型具有最佳抓取平面。常用的位姿检测方法：光学测量方法，虽然精度高，但是需要外围光学设备辅助，成本高，使用场景受限，且容易受遮挡，影响测量效果；双目相机测量方法，成本高，多手指情况容易受遮挡。
3.灵巧手各关节角度信息通过正运动学解算方法可以获得末端位姿信息，但是由于传动机构复杂，存在一定柔性和死区，通过正运动学解算获得的位姿信息难以准确地描述灵巧手手指末端的真实状态。


技术实现要素：

4.本发明的技术解决问题是：为实现灵巧手高精度实时反馈手指末端位置与姿态信息在线获取，从而实现多指手与物体之间的接触位置规划和抓取姿态规划，提出一种基于惯导网络的灵巧手末端位姿信息在线获取方法，该方法通过传感器信息融合，综合了惯性传感器独立自主、不易受外界干扰、短时间内精度高和关节端角度位置信息稳定的优点，为实现灵巧手多指操作运动控制奠定基础。
5.本发明的技术解决方案为：
6.一种基于惯导网络的灵巧手末端位姿信息在线获取方法，包括：
7.在灵巧手每个手指末端布置一个惯性传感器；
8.在线采集并存储灵巧手惯性传感器输出数据与各关节伺服系统角度信息；
9.根据惯性传感器输出数据，利用捷联解算方法计算每个手指末端位置与姿态信息；
10.根据各关节伺服系统角度信息，利用正运动学解算每个手指末端位置与姿态信息；
11.利用卡尔曼滤波方法实现惯性传感器解算的位置与姿态信息和正运动学解算的位置与姿态信息的融合，获得最终各个手指末端位置与姿态信息。
12.优选的，在灵巧手每个手指末端布置惯性传感器时，传感器基准坐标系与其所在灵巧手手指的末端位姿坐标系平行，且安装在手指末端中心处。
13.优选的，在线采集并存储灵巧手惯性传感器输出数据与各关节伺服系统角度信息的方法为：
14.分别采集并存储灵巧手各个关节伺服系统位置传感器的输出数据，各个惯性传感
器输出的陀螺仪与加速度计数据。
15.优选的，根据惯性传感器输出数据，利用捷联解算方法计算每个手指末端位置与姿态信息，具体方法为：
16.①
利用四元数方法更新惯性传感器姿态矩阵
17.初始四元数计算公式为：
[0018][0019]
ψ、θ、γ分别为惯性传感器的航向角、俯仰角、横滚角；
[0020]
采用角增量法，根据t时刻的四元数q(t)计算t+1时刻的四元数q(t+1)，离散化四阶计算公式为：
[0021][0022]
式中：δθ
x
、δθy、δθz为姿态速率在载体系的三个分量；表示载体系相对导航系的旋转角速度在载体系上的投影，所述载体系即为b系，导航系即为n系；
[0023]
得到四元数更新值q(t+1)后，进行姿态矩阵更新；
[0024]
②
计算惯性传感器姿态
[0025]
惯性传感器的姿态计量方式和有效范围定义如下：
[0026]
航向角ψ为载体系y轴在导航系的投影与导航系y轴的夹角，从导航系y轴起算，逆时针为正，有效范围为[0
°
，360
°
]；俯仰角θ为载体系y轴与其在导航系的投影间的夹角，以惯性传感器抬头为正，有效范围为[-90
°
，90
°
]；横滚角γ为载体系x轴与其在导航系的投影间的夹角，以惯性传感器右倾为正，左倾为负，有效范围为[-180
°
，180
°
]；
[0027]
记为t，在姿态矩阵更新以后，由以下公式计算更新后的姿态角：
[0028][0029]
t
23
为矩阵t的第二行第三列数据，t
13
为矩阵t的第一行第三列数据，t
33
为矩阵t的第三行第三列数据，t
21
为矩阵t的第二行第一列数据，t
22
为矩阵t的第二行第二列数据；
[0030]
③
计算惯性传感器速度
[0031]
由下列速度方程进行惯性传感器速度的更新：
[0032][0033]
表示惯性传感器在n系下x、y、z轴的运动加速度，表示惯性传感器的加速度计在n系下输出的x、y、z轴的比力加速度信息；
[0034]
表示在x、y、z轴的分量，表示在x、y、z轴的分量；表示地球自转角速率ω
ie
在n系中的投影，表示n系相对于地球坐标系的角速率；表示惯性传感器在n系下x、y、z轴的速度；
[0035]
计算惯性传感器加速度则t
′
为采集周期；
[0036]
g为惯性传感器所处位置的重力加速度数值。
[0037]
④
计算惯性传感器位置
[0038]
惯性传感器位置(λb,lb,hb)通过初始位置信息(λ0,l0,h0)对速度进行积分获得，λb为经度，lb为经度，hb为高度，λ0为初始经度，l0为初始经度，h0为初始高度；
[0039][0040]
tk第k个采集时刻，t
k+1
第k+1个采集时刻；
[0041]
优选的，的计算公式为：
[0042][0043]
式中：为惯性传感器陀螺仪输出角速率；表示n系相对惯性系的旋转角速度在b系上的投影，惯性系即为i系；有表示地球自转角速率ω
ie
在n系中的投影，表示n系相对于地球坐标系的角速率，二者的计算式分别为：
[0044]
[0045]
式中：ve、vn、vu为计算速度v在n系中的东向、北向和天向分量；rm和rn分别为沿子午圈和卯酉圈的主曲率半径。
[0046]
优选的，
[0047]
在wgs-84全球大地坐标系下，载体所处位置的重力加速度数值g为：
[0048][0049]
优选的，根据各关节伺服系统角度信息，利用正运动学解算每个手指末端位置与姿态信息，实现方法如下：
[0050]
采用d-h法建立手指坐标系，其中，x0y0z0为手指定参考坐标系，其他为随动坐标系；各指节长度分别为a1、a2、a3、a4，各关节转角分别为θ1、θ2、θ3、θ4，侧摆轴与屈曲轴垂直，夹角用α表示，指尖在手指定参考坐标系中的位置坐标为(x，y，z)；
[0051]
为减少坐标变换，令关节1的坐标系与对应手指定参考坐标系重合；
[0052]
灵巧手手指正运动学变换矩阵0th为：
[0053][0054]
式中，si′
＝sinθi′
，ci′
＝cosθi′
，si′j′
＝sin(θi′
+θj′
)，ci′j′
＝cos(θi′
+θj′
)，si′j′k′
＝sin(θi′
+θj′
+θk′
)，ci′j′k′
＝cos(θi′
+θj′
+θk′
)，i
′
,j
′
,k
′
＝1,2,3,4；
[0055]0th第4列的前3个元素即为指尖在手指定参考坐标系中的位置坐标为(x，y，z)，因而有：
[0056]
x＝(a4c
234
+a3c
23
+a2c2+a1)c1[0057]
y＝(a4c
234
+a3c
23
+a2c2+a1)s1[0058]
z＝-(a4s
234
+a3s
23
+a2s2)
[0059]
手掌基坐标系固定，所以n系到手掌基坐标系的转换矩阵固定且已知；灵巧手构型确定，手掌基坐标系到每个手指的定参考坐标系的转换矩阵固定且已知；所以则正运动学解算获得的姿态旋转矩阵为手指末端相对于跟关节的姿态变换矩阵；
[0060]
正运动学计算得到的手指末端姿态信息θ
b1
,γ
b1
,根据如下公式获得：
[0061][0062]
t
″
23
为矩阵的第二行第三列数据，t
″
13
为矩阵的第一行第三列数据，t
″
33
为矩阵的第三行第三列数据，t
″
21
为矩阵的第二行第一列数据，t
″
22
为矩阵的第二行第二列数据；
[0063]
正运动学利用如下公式计算手指末端位置信息λ
b1
为经度，l
b1
为经度，h
b1
为高度，λ
base
为初始经度，l
base
为初始经度，h
base
为初始高度。
[0064]
优选的，手指末端相对于跟关节的姿态变换矩阵为：
[0065][0066]
优选的，利用卡尔曼滤波方法实现惯性传感器解算的位置与姿态信息和正运动学解算的位置与姿态信息的融合，获得最终各个手指末端位置与姿态信息，具体实现方法如下：
[0067]
设卡尔曼滤波的状态方程
[0068]
状态量x选取手指末端惯性传感器的误差参数
[0069][0070]
式中：ε
x
,εy,εz为载体系三个坐标轴上陀螺的常值漂移，为载体系三个坐标轴上加速度计的常值偏置；φ
x
,φy,φz为载体系三个坐标轴上的失准角误差，δv
x
,δvy,δvz为载体系三个坐标轴上的速度误差，δl,δλ,δh为载体系三个坐标轴上的位置误差；f为状态转移矩阵，q为随机驱动向量；
[0071]
选取由正运动学解算得到的位置信息和惯性传感器解算得到的位置信息之间的误差、正运动学解算得到的姿态信息和惯性传感器解算得到的姿态信息之间的误差作为组合系统的量测量z，则组合系统的量测方程为：
[0072][0073]
其中，θ
b1
,γ
b1
,为正运动学解算得到的姿态信息，θ,γ,为对应的手指末端惯性传感器解算得到的姿态信息；l
b1
,λ
b1
,h
b1
为正运动学解算得到的位置信息，lb,λb,hb为对应的手指末端惯导传感器解算得到的位置信息,g代表正运动学计算的量测噪声,h
′
为量测方程；
[0074]
根据状态方程和量测方程，利用卡尔曼滤波算法获得最终各个手指末端位置与姿态信息。
[0075]
优选的，i3×3为3
×
3的单位阵。
[0076]
本发明与现有技术相比的优点在于：
[0077]
本发明采用惯导组网的形式，利用卡尔曼滤波方法，实现惯性传感器解算位姿信
息和正运动学解算位姿信息的数据融合，其中，正运动学解算位姿信息作为卡尔曼滤波的量测信息，惯性传感器信息作为卡尔曼滤波的状态信息，该方法综合了惯性传感器独立自主、不易受外界干扰、短时间内精度高和关节端角度位置信息稳定的优点，实现了灵巧手高精度实时反馈手指末端位置与姿态信息在线获取，从而为多指手与物体之间的接触位置规划和抓取姿态规划奠定基础。
附图说明
[0078]
图1为本发明涉及基于惯导网络的灵巧手末端位姿信息在线获取方法流程图；
[0079]
图2为本发明涉及传感器布局图；
[0080]
图3为本发明涉及手指机构简图及空间坐标系。
具体实施方式
[0081]
本发明采用惯导组网的形式，利用卡尔曼滤波方法，实现惯性传感器解算位姿信息和正运动学解算位姿信息的数据融合，其中，正运动学解算位姿信息作为卡尔曼滤波的量测信息，惯性传感器信息作为卡尔曼滤波的状态信息，该方法综合了惯性传感器独立自主、不易受外界干扰、短时间内精度高和关节端角度位置信息稳定的优点。
[0082]
本发明的具体实施步骤如图1所示，具体实施步骤如下：
[0083]
1、传感器布局：将五个惯性传感器布置在灵巧手五个手指末端，传感器基准坐标系与灵巧手末端位姿坐标系平行，位置安装在手指末端中心处。如图2所示。
[0084]
2、分别采集并存储手指各个关节伺服系统位置传感器的输出数据，各个惯性传感器输出的陀螺仪与加速度计数据。
[0085]
3、惯性传感器输出数据利用捷联解算方法计算末端位置与姿态信息，以一片惯性传感器为例进行介绍：
[0086]
①
利用四元数方法更新惯性传感器姿态矩阵
[0087]
初始四元数计算公式为：
[0088][0089]
ψ、θ、γ分别为惯性传感器的航向角、俯仰角、横滚角；
[0090]
采用角增量法，根据t时刻的四元数q(t)计算t+1时刻的四元数q(t+1)，离散化四阶计算公式为：
[0091]
[0092]
式中：δθ
x
、δθy、δθz为姿态速率在载体系的三个分量；表示载体系(b系)相对导航系(n系)的旋转角速度在载体系上的投影；
[0093]
得到四元数更新值q(t+1)后，进行姿态矩阵更新；
[0094]
式(2)中的的计算公式为：
[0095][0096]
式中：为陀螺仪输出角速率；表示n系相对惯性系(i系)的旋转角速度在b系上的投影；有表示地球自转角速率ω
ie
在n系中的投影，表示n系相对于地球坐标系的角速率，二者的计算式分别为：
[0097][0098]
式中：ve、vn、vu为计算速度v在n系中的东向、北向和天向分量；rm和rn分别为沿子午圈和卯酉圈的主曲率半径。rm＝re(1-2e+3esin2l)，rn＝re(1+esin2l)，其中椭圆度e＝1/298.257，地球椭球长半轴re＝6378137m；
[0099]
②
计算惯性传感器姿态
[0100]
载体的姿态计量方式和有效范围定义如下：
[0101]
航向角ψ为载体系y轴在导航系的投影与导航系y轴的夹角，从导航系y轴起算，逆时针为正，有效范围为[0
°
，360
°
]；俯仰角θ为载体系y轴与其在导航系的投影间的夹角，以载体抬头为正，有效范围为[-90
°
，90
°
]；横滚角γ为载体系x轴与其在导航系的投影间的夹角，以载体右倾为正(从载体正后朝前看)，左倾为负，有效范围为[-180
°
，180
°
]。
[0102]
记为t，在姿态矩阵更新以后，由以下公式计算更新后的姿态角：
[0103][0104]
t
23
为矩阵t的第二行第三列数据，t
13
为矩阵t的第一行第三列数据，t
33
为矩阵t的第三行第三列数据，t
21
为矩阵t的第二行第一列数据，t
22
为矩阵t的第二行第二列数据；
[0105]
③
计算惯性传感器速度
[0106]
由下列速度方程进行惯性传感器速度的更新：
[0107]
[0108]
表示惯性传感器在n系下x、y、z轴的加速度，表示惯性传感器在n系下输出的x、y、z轴的比力加速度信息。
[0109]
表示在x、y、z轴的分量，表示在x、y、z轴的分量；表示惯性传感器在n系下x、y、z轴的速度；
[0110]
由式(6)求出惯性传感器加速度则t
′
为采集周期；
[0111]
在wgs-84全球大地坐标系下，载体所处位置的重力加速度数值g为：
[0112][0113]
④
计算惯性传感器位置
[0114]
惯性传感器位置(λb,lb,hb)通过初始位置信息(λ0,l0,h0)对速度进行积分获得，λb为经度，lb为经度，hb为高度，λ0为初始经度，l0为初始经度，h0为初始高度；
[0115][0116]
tk第k个采集时刻，t
k+1
第k+1个采集时刻；
[0117]
五个惯性传感器算法相同，其中手掌基坐标系为base。
[0118]
4、各关节伺服系统角度信息利用正运动学解算末端位置与姿态信息，以单手指为例：
[0119]
采用d-h法建立手指坐标系，如图3所示。其中，x0y0z0为手指定参考坐标系，其他为随动坐标系。各指节长度分别为a1、a2、a3、a4，各关节转角分别为θ1、θ2、θ3、θ4，侧摆轴与屈曲轴垂直，夹角用α表示，指尖在手指定参考坐标系中的位置坐标为(x，y，z)。
[0120]
为减少坐标变换，令关节1的坐标系与手指定参考坐标系重合。灵巧手手指机构对应的d-h参数见表1。
[0121]
表1灵巧手单手指机构d-h参数
[0122]
[0123][0124]
灵巧手手指正运动学变换矩阵0th为：
[0125][0126]
式中，si′
＝sinθi′
，ci′
＝cosθi′
，si′j′
＝sin(θi′
+θj′
)，ci′j′
＝cos(θi′
+θj′
)，si′j′k′
＝sin(θi′
+θj′
+θk′
)，ci′j′k′
＝cos(θi′
+θj′
+θk′
)，i
′
,j
′
,k
′
＝1,2,3,4。
[0127]
式(9)中第4列的前3个元素即为指尖在手指定参考坐标系中的坐标(x，y，z)，因而有：
[0128]
x＝(a4c
234
+a3c
23
+a2c2+a1)c1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0129]
y＝(a4c
234
+a3c
23
+a2c2+a1)s1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0130]
z＝-(a4s
234
+a3s
23
+a2s2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0131]
手指末端相对于跟关节的姿态变换矩阵为：
[0132][0133]
手掌基坐标系固定，所以n系到手掌基坐标系的转换矩阵固定且已知；灵巧手构型确定，手掌基坐标系到每个手指的定参考坐标系的转换矩阵固定且已知；所以则正运动学解算获得的姿态旋转矩阵为手指末端相对于跟关节的姿态变换矩阵；正运动学计算得到的手指末端姿态信息θ
b1
,γ
b1
,根据如下公式获得。
[0134][0135]
t
″
23
为矩阵的第二行第三列数据，t
″
13
为矩阵的第一行第三列数据，t
″
33
为矩阵的第三行第三列数据，t
″
21
为矩阵的第二行第一列数据，t
″
22
为矩阵的第二行第二列数据；
[0136]
正运动学计算得到的手指末端位置信息λ
b1
为经度，l
b1
为经度，h
b1
为高度，λ
base
为初始经度，l
base
为初始经度，h
base
为初始高度。
[0137]
5、利用卡尔曼滤波方法实现惯性传感器解算位姿信息和正运动学解算位姿信息的数据融合：
[0138]
①
状态方程确定
[0139]
设卡尔曼滤波的状态方程
[0140]
状态量x选取手指末端惯性传感器的误差参数
[0141][0142]
式中：ε
x
,εy,εz为载体系三个坐标轴上陀螺的常值漂移，为载体系三个坐标轴上加速度计的常值偏置；φ
x
,φy,φz为载体系三个坐标轴上的失准角误差，δv
x
,δvy,δvz为载体系三个坐标轴上的速度误差，δl,δλ,δh为载体系三个坐标轴上的位置误差；f为状态转移矩阵，q为随机驱动向量；
[0143]
②
量测方程确定
[0144]
选取由正运动学解算得到的位置信息和惯性传感器解算得到的位置信息之间的误差、正运动学解算得到的姿态信息和惯性传感器解算得到的姿态信息之间的误差作为组合系统的量测量z，则组合系统的量测方程为：
[0145][0146]
其中，θ
b1
,γ
b1
,为正运动学计算得到的姿态信息，θ,γ,为对应的手指末端惯性传感器计算得到的姿态信息；l
b1
,λ
b1
,h
b1
为正运动学计算得到的位置信息，lb,λb,hb为对应的手指末端惯导传感器计算得到的位置信息,g代表正运动学计算的量测噪声,
[0147]
6、利用卡尔曼滤波获得最终各个手指末端位置与姿态信息。
[0148]
本发明的原理是：首先采用惯导组网的形式，将五个惯性传感器分布在五个手指的末端，同时测量五个手指的位姿信息；然后利用捷联解算方法计算惯性传感器输出位置与姿态信息；再并充分利用各手指关节伺服系统的角度信息，通过正运动学解算获得一组末端位姿信息；最后利用卡尔曼滤波方法，实现惯性传感器信息和正运动学解算位姿信息的数据融合，其中，正运动学解算位姿信息作为卡尔曼滤波的量测信息，惯性传感器信息作为卡尔曼滤波的状态信息。本发明用于在线获取灵巧手手指末端高精度的位姿与姿态信息。该方法综合了惯性传感器独立自主、不易受外界干扰、短时间内精度高和关节端角度位置信息稳定的优点，通过卡尔曼滤波方法，实现惯性传感器解算位姿信息和正运动学解算位姿信息的数据融合，获得高精度的手指末端位置与姿态信息。
[0149]
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

技术特征：
1.一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，包括：在灵巧手每个手指末端布置一个惯性传感器；在线采集并存储灵巧手惯性传感器输出数据与各关节伺服系统角度信息；根据惯性传感器输出数据，利用捷联解算方法计算每个手指末端位置与姿态信息；根据各关节伺服系统角度信息，利用正运动学解算每个手指末端位置与姿态信息；利用卡尔曼滤波方法实现惯性传感器解算的位置与姿态信息和正运动学解算的位置与姿态信息的融合，获得最终各个手指末端位置与姿态信息。2.根据权利要求1所述的一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，在灵巧手每个手指末端布置惯性传感器时，传感器基准坐标系与其所在灵巧手手指的末端位姿坐标系平行，且安装在手指末端中心处。3.根据权利要求1所述的一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，在线采集并存储灵巧手惯性传感器输出数据与各关节伺服系统角度信息的方法为：分别采集并存储灵巧手各个关节伺服系统位置传感器的输出数据，各个惯性传感器输出的陀螺仪与加速度计数据。4.根据权利要求1所述的一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，根据惯性传感器输出数据，利用捷联解算方法计算每个手指末端位置与姿态信息，具体方法为：
①
利用四元数方法更新惯性传感器姿态矩阵初始四元数计算公式为：ψ、θ、γ分别为惯性传感器的航向角、俯仰角、横滚角；采用角增量法，根据t时刻的四元数q(t)计算t+1时刻的四元数q(t+1)，离散化四阶计算公式为：式中：式中：δθ
x
、δθ
y
、δθ
z
为姿态速率
在载体系的三个分量；表示载体系相对导航系的旋转角速度在载体系上的投影，所述载体系即为b系，导航系即为n系；得到四元数更新值q(t+1)后，进行姿态矩阵更新；
②
计算惯性传感器姿态惯性传感器的姿态计量方式和有效范围定义如下：航向角ψ为载体系y轴在导航系的投影与导航系y轴的夹角，从导航系y轴起算，逆时针为正，有效范围为[0
°
，360
°
]；俯仰角θ为载体系y轴与其在导航系的投影间的夹角，以惯性传感器抬头为正，有效范围为[-90
°
，90
°
]；横滚角γ为载体系x轴与其在导航系的投影间的夹角，以惯性传感器右倾为正，左倾为负，有效范围为[-180
°
，180
°
]；记为t，在姿态矩阵更新以后，由以下公式计算更新后的姿态角：t
23
为矩阵t的第二行第三列数据，t
13
为矩阵t的第一行第三列数据，t
33
为矩阵t的第三行第三列数据，t
21
为矩阵t的第二行第一列数据，t
22
为矩阵t的第二行第二列数据；
③
计算惯性传感器速度由下列速度方程进行惯性传感器速度的更新：由下列速度方程进行惯性传感器速度的更新：表示惯性传感器在n系下x、y、z轴的运动加速度，表示惯性传感器的加速度计在n系下输出的x、y、z轴的比力加速度信息；表示在x、y、z轴的分量，表示在x、y、z轴的分量；表示地球自转角速率ω
ie
在n系中的投影，表示n系相对于地球坐标系的角速率；表示惯性传感器在n系下x、y、z轴的速度；计算惯性传感器加速度则t
′
为采集周期；g为惯性传感器所处位置的重力加速度数值。
④
计算惯性传感器位置惯性传感器位置(λ
b
,l
b
,h
b
)通过初始位置信息(λ0,l0,h0)对速度进行积分获得，λ
b
为经度，l
b
为经度，h
b
为高度，λ0为初始经度，l0为初始经度，h0为初始高度；
t
k
第k个采集时刻，t
k+1
第k+1个采集时刻。5.根据权利要求4所述的一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，的计算公式为：式中：为惯性传感器陀螺仪输出角速率；表示n系相对惯性系的旋转角速度在b系上的投影，惯性系即为i系；有上的投影，惯性系即为i系；有表示地球自转角速率ω
ie
在n系中的投影，表示n系相对于地球坐标系的角速率，二者的计算式分别为：式中：v
e
、v
n
、v
u
为计算速度v在n系中的东向、北向和天向分量；r
m
和r
n
分别为沿子午圈和卯酉圈的主曲率半径。6.根据权利要求4所述的一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，在wgs-84全球大地坐标系下，载体所处位置的重力加速度数值g为：7.根据权利要求1所述的一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，根据各关节伺服系统角度信息，利用正运动学解算每个手指末端位置与姿态信息，实现方法如下：采用d-h法建立手指坐标系，其中，x0y0z0为手指定参考坐标系，其他为随动坐标系；各指节长度分别为a1、a2、a3、a4，各关节转角分别为θ1、θ2、θ3、θ4，侧摆轴与屈曲轴垂直，夹角用α表示，指尖在手指定参考坐标系中的位置坐标为(x，y，z)；为减少坐标变换，令关节1的坐标系与对应手指定参考坐标系重合；灵巧手手指正运动学变换矩阵0t
h
为：式中，s
i
′
＝sinθ
i
′
，c
i
′
＝cosθ
i
′
，s
i
′
j
′
＝sin(θ
i
′
+θ
j
′
)，c
i
′
j
′
＝cos(θ
i
′
+θ
j
′
)，s
i
′
j
′
k
′
＝sin
(θ
i
′
+θ
j
′
+θ
k
′
)，c
i
′
j
′
k
′
＝cos(θ
i
′
+θ
j
′
+θ
k
′
)，i
′
,j
′
,k
′
＝1,2,3,4；0t
h
第4列的前3个元素即为指尖在手指定参考坐标系中的位置坐标为(x，y，z)，因而有：x＝(a4c
234
+a3c
23
+a2c2+a1)c1y＝(a4c
234
+a3c
23
+a2c2+a1)s1z＝-(a4s
234
+a3s
23
+a2s2)手掌基坐标系固定，所以n系到手掌基坐标系的转换矩阵固定且已知；灵巧手构型确定，手掌基坐标系到每个手指的定参考坐标系的转换矩阵固定且已知；所以则正运动学解算获得的姿态旋转矩阵动学解算获得的姿态旋转矩阵为手指末端相对于跟关节的姿态变换矩阵；正运动学计算得到的手指末端姿态信息根据如下公式获得：t
″
23
为矩阵的第二行第三列数据，t
″
13
为矩阵的第一行第三列数据，t
″
33
为矩阵的第三行第三列数据，t
″
21
为矩阵的第二行第一列数据，t
″
22
为矩阵的第二行第二列数据；正运动学利用如下公式计算手指末端位置信息λ
b1
为经度，l
b1
为经度，h
b1
为高度，λ
base
为初始经度，l
base
为初始经度，h
base
为初始高度。8.根据权利要求1所述的一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，手指末端相对于跟关节的姿态变换矩阵为：9.根据权利要求1所述的一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，利用卡尔曼滤波方法实现惯性传感器解算的位置与姿态信息和正运动学解算的位置与姿态信息的融合，获得最终各个手指末端位置与姿态信息，具体实现方法如下：设卡尔曼滤波的状态方程状态量x选取手指末端惯性传感器的误差参数式中：ε
x
,ε
y
,ε
z
为载体系三个坐标轴上陀螺的常值漂移，为载体系三个坐标轴上加速度计的常值偏置；φ
x
,φ
y
,φ
z
为载体系三个坐标轴上的失准角误差，δv
x
,δv
y
,δv
z
为载体系三个坐标轴上的速度误差，δl,δλ,δh为载体系三个坐标轴上的位置误差；f为状态转移矩阵，q为随机驱动向量；
选取由正运动学解算得到的位置信息和惯性传感器解算得到的位置信息之间的误差、正运动学解算得到的姿态信息和惯性传感器解算得到的姿态信息之间的误差作为组合系统的量测量z，则组合系统的量测方程为：其中，为正运动学解算得到的姿态信息，为对应的手指末端惯性传感器解算得到的姿态信息；l
b1
,λ
b1
,h
b1
为正运动学解算得到的位置信息，l
b
,λ
b
,h
b
为对应的手指末端惯导传感器解算得到的位置信息,g代表正运动学计算的量测噪声,h
′
为量测方程；根据状态方程和量测方程，利用卡尔曼滤波算法获得最终各个手指末端位置与姿态信息。10.根据权利要求9所述的一种基于惯导网络的灵巧手末端位姿信息在线获取方法，其特征在于，i3×3为3
×
3的单位阵。

技术总结
一种基于惯导网络的灵巧手末端位姿信息在线获取方法，用于在线获取灵巧手手指末端高精度的位姿与姿态信息。首先采用惯导组网的形式，将五个惯性传感器分布在五个手指的末端，同时测量五个手指的位姿信息；然后利用捷联解算方法计算惯性传感器输出位置与姿态信息；再并充分利用各手指关节伺服系统的角度信息，通过正运动学解算获得一组末端位姿信息；最后利用卡尔曼滤波方法，实现惯性传感器信息和正运动学解算位姿信息的数据融合。本发明通过卡尔曼滤波方法，实现惯性传感器解算位姿信息和正运动学解算位姿信息的数据融合，获得高精度的手指末端位置与姿态信息。手指末端位置与姿态信息。手指末端位置与姿态信息。


技术研发人员：郭雅静 吕博瀚 杨帆 张俊宁 于志远 曾思
受保护的技术使用者：北京精密机电控制设备研究所
技术研发日：2023.04.19
技术公布日：2023/8/9