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

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

【技术实现步骤摘要】
一种基于惯导网络的灵巧手末端位姿信息在线获取方法


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

技术介绍

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

技术实现思路

[0004]本专利技术的技术解决问题是：为实现灵巧手高精度实时反馈手指末端位置与姿态信息在线获取，从而实现多指手与物体之间的接触位置规划和抓取姿态规划，提出一种基于惯导网络的灵巧手末端位姿信息在线获取方法，该方法通过传感器信息融合，综合了惯性传感器独立自主、不易受外界干扰、短时间内精度高和关节端角度位置信息稳定的优点，为实现灵巧手多指操作运动控制奠定基础。
[0005]本专利技术的技术解决方案为：
[0006]一种基于惯导网络的灵巧手末端位姿信息在线获取方法，包括：/>[0007]在灵巧手每个手指末端布置一个惯性传感器；
[0008]在线采集并存储灵巧手惯性传感器输出数据与各关节伺服系统角度信息；
[0009]根据惯性传感器输出数据，利用捷联解算方法计算每个手指末端位置与姿态信息；
[0010]根据各关节伺服系统角度信息，利用正运动学解算每个手指末端位置与姿态信息；
[0011]利用卡尔曼滤波方法实现惯性传感器解算的位置与姿态信息和正运动学解算的位置与姿态信息的融合，获得最终各个手指末端位置与姿态信息。
[0012]优选的，在灵巧手每个手指末端布置惯性传感器时，传感器基准坐标系与其所在灵巧手手指的末端位姿坐标系平行，且安装在手指末端中心处。
[0013]优选的，在线采集并存储灵巧手惯性传感器输出数据与各关节伺服系统角度信息的方法为：
[0014]分别采集并存储灵巧手各个关节伺服系统位置传感器的输出数据，各个惯性传感
器输出的陀螺仪与加速度计数据。
[0015]优选的，根据惯性传感器输出数据，利用捷联解算方法计算每个手指末端位置与姿态信息，具体方法为：
[0016]①
利用四元数方法更新惯性传感器姿态矩阵
[0017]初始四元数计算公式为：
[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
,L
b
,H
b
)通过初始位置信息(λ0,L0,H0)对速度进行积分获得，λ
b
为经度，L
b
为经度，H
b
为高度，λ0为初始经度，L0为初始经度，H0为初始高度；
[0039][0040]t
k
第k个采集时刻，t
k+1
第k+1个采集时刻；
[0041]优选的，的计算公式为：
[0042][0043]式中：为惯性传感器陀螺仪输出角速率；表示n系相对惯性系的旋转角速度在b系上的投影，惯性系即为i系；有表示地球自转角速率ω
ie
在n系中的投影，表示n系相对于地球坐标系的角速率，二者的计算式分别为：
[0044][0045]式中：V
E
、V
N
、V
U
为计算速度V在n系中的东向、北向和天向分量；R
M
和R
N
分别为沿子午圈和卯酉圈的主曲率半径。
[0046]优选的，
[0047]在WGS
‑
84全球大地坐标系下，载体所处位置的重力加速度数值g为：
[0048][0049]优选的，根据各关节伺服系统角度信息，利用正运动学解算每个手指末端位置与姿态信息，实现方法如下：
[0050]采用D
‑
H法建立手指坐标系，其中，x0y0z0为手指定参考坐标本文档来自技高网...

【技术保护点】

【技术特征摘要】
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，各关节...

【专利技术属性】
技术研发人员：郭雅静，吕博瀚，杨帆，张俊宁，于志远，曾思，
申请(专利权)人：北京精密机电控制设备研究所，
类型：发明
国别省市：