CN111887856A - 一种基于惯性传感器的抗位置窜动关节角度解算实时方法 - Google Patents

Abstract

本发明公开了一种基于惯性传感器的抗位置窜动关节角度解算实时方法。步骤1：惯性传感器设置在待测关节两端的肢体上，并开始测量和读取数据；步骤2：将惯性传感器采集到的加速度以及角速度数据作为输入，建立坐标；步骤3：惯性传感器固连坐标系中的关节轴线向量的坐标j₁，j₂上面，积分得到关节角度q_w；步骤4：惯性传感器测得的加速度a₁，a₂的前两项分量投影到关节轴线向量的前两项上，求解两个投影的夹角q_a，步骤5：关节角度q_w与两个投影的夹角q_a融合；步骤6：将不同滑动窗口解算的关节轴线向量对比，继续进行解算或更新关节轴线向量坐标。本发明其目的是解决应用惯性传感器估算人体关节角度过程中传感器相对于肢体发生窜动所导致的测量误差问题。

Description

一种基于惯性传感器的抗位置窜动关节角度解算实时方法

技术领域

本发明属于可穿戴机器人技术领域；具体涉及一种基于惯性传感器的抗位置窜动关节角度解算实时方法。

背景技术

目前可穿戴机器人技术，康复医学与生物力学的发展使得实时解算人体关节角度日趋成为各技术领域的关键问题之一。目前常用的关节角度解算技术为基于高速摄像头的光学动捕以及基于外骨骼等刚性可穿戴设备的机械动捕。光学动捕由于其繁重的设备和较长的计算时间使得其在实时应用中难以实现，而机械动捕由于关节轴线不对齐的原因精度难以保证。惯性传感器由于其便于穿戴、体积小以及已于与其他设备兼容等特点，适于实时解算人体关节角度。目前存在的基于惯性传感器的人体动作捕捉技术在解算关节角度方面存在缺陷，即精度严重依赖角度估算前的矫正动作的实施，并且在传感器相对肢体发生窜动后，除重新实施矫正动作外无法矫正窜动产生的测量错误。而重新实施矫正动作阻碍的该项技术的实时应用。

发明内容

本发明提供一种基于惯性传感器的抗位置窜动关节角度解算实时方法，根据设置在人肢体上的惯性传感器测量的加速度与角速度的信号来对人体关节进行角度解算，同时通过基于阈值的方法，判断惯性传感器相对于肢体的传动，并对误差作出补偿。其目的是解决应用惯性传感器估算人体关节角度过程中传感器相对于肢体发生窜动所导致的测量误差问题。

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

一种基于惯性传感器的抗位置窜动关节角度解算实时方法，所述解算实时方法包括以下步骤：

步骤1：将惯性传感器设置在待测关节两端的肢体上，并开始测量和读取数据；

步骤2：将步骤1中通过1000到3000个采样点的滑动窗口截取的惯性传感器采集到的加速度以及角速度数据作为输入，将建立的关节生理学运动约束作为代价函数，利用数值优化算法得到关节轴线向量在肢体上惯性传感器固连坐标系中的坐标；

步骤3：将关节两端肢体上的惯性传感器测得的角速度投影到，表示在惯性传感器固连坐标系中的关节轴线向量的坐标j₁，j₂上面，积分得到关节角度q_w；

步骤4：将关节两端肢体上的惯性传感器测得的加速度α₁，α₂的前两项分量投影到关节轴线向量的前两项上，求解两个投影的夹角q_a，即

步骤5：将步骤3中的关节角度q_w与步骤4中的两个投影的夹角q_a通过互补滤波对上述两个关节角度求加权平均；

步骤6：将不同滑动窗口解算的关节轴线向量对比，将其差值的二范数与阈值对比，若不超过阈值，则判断为传感器未发生窜动，继续进行上述解算；若超过阈值，则判断传感器发生窜动，停止角度解算，重启关节轴线向量求解，更新关节轴线向量坐标。

进一步的，所述步骤1具体为，获取置于大腿、小腿上的IMU传感器在实验对象保持正常行走时的数据，将测得的加速度、角速度信息通过无相位滞后的低通巴特沃斯滤波器，截止频率分别为8hz和10hz。

进一步的，所述步骤2具体为，每个滑动窗口的间隔为500到1000个采样点，由于角速度只在关节轴线外的旋转上数量相等，关节生理学运动约束表达为：

||ω₁×j₁||-||ω₂×j₂||＝0

ω₁，ω₂为大腿、小腿上惯性传感器测得的角速度信息，j₁，j₂为关节轴线向量在大腿、小腿上惯性传感器固连坐标系中表达的坐标，将j₁，j₂的坐标用球坐标形式描述(j_i＝[cos(θ_i)cos(u_i)，cos(θ_i)sin(u_i)，sin(θ_i)]，i＝1，2)，利用该运动约束作为代价函数，以θ_i，u_i为变量，利用高斯-牛顿算法求解上述问题。

进一步的，所述步骤3具体为，通过步骤2得到关节轴线向量j₁，j₂坐标后，可通过角速度积分得到关节角度，由于肢体的角速度在关节轴线方向的差值即为关节的旋转角速度，因此关节轴线方向的角速度差值在时间上的积分即为：

q_w＝|∫(ω₁·j₁-ω₂·j₂)dt。

进一步的，所述步骤4具体为，由于步骤3中角速度积分放大了测量噪声，步骤3求得的角度漂移较为严重，根据加速度a₁，a₂的前两项分量在关节轴线向量上的投影，可根据加速度倾角求解关节角度，计算如下：

进一步的，所述步骤5具体为，由于用于求解两个投影的夹角q_a的加速度混合了重力加速度、运动产生的加速度和噪声，其结果精度较差，因此针对两个投影的夹角q_a和关节角度q_w的不同特点通过互补滤波将二者融合，计算如下：

q＝γ·q_a+(1-γ)·q_w。

进一步的，所述步骤6中，若各滑动窗口求得的关节轴线向量j_i的差的模超过阈值，则判断传感器发生窜动，重启关节轴向估计，更新窜动后的关节轴线向量；若未超过阈值，则判断为未发生窜动，继续解算关节角度。

本发明的有益效果是：

由于避免了角度解算前矫正动作的实施，本发明利用阈值法对比不同滑动窗口间关节轴线向量的差值，从而在角度实时解算过程中实现了传感器位置窜动的探测与矫正，提高了惯性传感器在实时关节角度解算方面的实用性和鲁棒性。

附图说明

图1本发明的方法流程图。

图2本发明的滑动窗口示意图。

图3本发明的关节生理学运动约束简化模型示意图。

具体实施方式

下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种基于惯性传感器的抗位置窜动关节角度解算实时方法，所述解算实时方法包括以下步骤：

步骤1：将惯性传感器MPU9250设置在待测关节两端的肢体上，并开始测量和读取数据；

步骤2：将步骤1中通过1000到3000个采样点的滑动窗口截取的惯性传感器采集到的加速度以及角速度数据作为输入，将建立的关节生理学运动约束作为代价函数，利用数值优化算法得到关节轴线向量在肢体上惯性传感器固连坐标系中的坐标；

步骤3：将关节两端肢体上的惯性传感器测得的角速度投影到(即将角速度与关节轴线向量点乘)，表示在惯性传感器固连坐标系中的关节轴线向量的坐标j₁，j₂上面，积分得到关节角度q_w；

步骤4：将关节两端肢体上的惯性传感器测得的加速度a₁，a₂的前两项分量投影到关节轴线向量的前两项上，求解两个投影的夹角q_a，即

步骤5：将步骤3中的关节角度q_w与步骤4中的两个投影的夹角q_a通过互补滤波对上述两个关节角度求加权平均；

步骤6：将不同滑动窗口解算的关节轴线向量对比，将其差值的二范数与阈值对比，若不超过阈值，则判断为传感器未发生窜动，继续进行上述解算；若超过阈值，则判断传感器发生窜动，停止角度解算，重启关节轴线向量求解，更新关节轴线向量坐标。

进一步的，所述步骤1具体为，获取置于大腿、小腿上的IMU传感器在实验对象保持正常行走时的数据。将测得的加速度、角速度信息通过无相位滞后的低通巴特沃斯滤波器，截止频率分别为8hz和10hz。

进一步的，所述步骤2具体为，每个滑动窗口的间隔为500到1000个采样点，由图3所示，由于角速度只在关节轴线外的旋转上数量相等，关节生理学运动约束表达为：

||ω₁×j₁||-||ω₂×j₂||＝0

ω₁，ω₂为大腿、小腿上惯性传感器测得的角速度信息，j₁，j₂为关节轴线向量在大腿、小腿上惯性传感器固连坐标系中表达的坐标，将j₁，j₂的坐标用球坐标形式描述(j_i＝[cos(θ_i)cos(u_i)，cos(θ_i)sin(u_i)，sin(θ_i)]，i＝1，2)，利用该运动约束作为代价函数，以θ_i，u_i为变量，利用高斯-牛顿算法求解上述问题(上述问题即关节生理学运动约束表达)。

进一步的，所述步骤3具体为，通过步骤2得到关节轴线向量j₁，j₂坐标后，可通过角速度积分得到关节角度。由于肢体的角速度在关节轴线方向的差值即为关节的旋转角速度，因此关节轴线方向的角速度差值在时间上的积分即为：

q_w＝|∫(ω₁·j₁-ω₂·j₂)dt。

进一步的，所述步骤4具体为，由于步骤3中角速度积分放大了测量噪声，步骤3求得的角度漂移较为严重，根据加速度a₁，a₂的前两项分量在关节轴线向量上的投影，可根据加速度倾角求解关节角度，计算如下：

进一步的，所述步骤5具体为，由于用于求解两个投影的夹角q_a的加速度混合了重力加速度、运动产生的加速度和噪声，其结果精度较差，因此针对两个投影的夹角q_a和关节角度q_w的不同特点通过互补滤波将二者融合，计算如下：

q＝γ·q_a+(1-γ)·q_w。

进一步的，所述步骤6中，若各滑动窗口求得的关节轴线向量j_i的差的模超过阈值，则判断传感器发生窜动，重启关节轴向估计，更新窜动后的关节轴线向量；若未超过阈值，则判断为未发生窜动，继续解算关节角度。

Claims (7)

1.一种基于惯性传感器的抗位置窜动关节角度解算实时方法，其特征在于，所述解算实时方法包括以下步骤：

步骤1：将惯性传感器设置在待测关节两端的肢体上，并开始测量和读取数据；

步骤2：将步骤1中通过1000到3000个采样点的滑动窗口截取的惯性传感器采集到的加速度以及角速度数据作为输入，将建立的关节生理学运动约束作为代价函数，利用数值优化算法得到关节轴线向量在肢体上惯性传感器固连坐标系中的坐标；

步骤3：将关节两端肢体上的惯性传感器测得的角速度投影到，表示在惯性传感器固连坐标系中的关节轴线向量的坐标j₁，j₂上面，积分得到关节角度q_w；

步骤4：将关节两端肢体上的惯性传感器测得的加速度a₁，a₂的前两项分量投影到关节轴线向量的前两项上，求解两个投影的夹角q_a，即

步骤5：将步骤3中的关节角度q_w与步骤4中的两个投影的夹角q_a通过互补滤波对上述两个关节角度求加权平均；

步骤6：将不同滑动窗口解算的关节轴线向量对比，将其差值的二范数与阈值对比，若不超过阈值，则判断为传感器未发生窜动，继续进行上述解算；若超过阈值，则判断传感器发生窜动，停止角度解算，重启关节轴线向量求解，更新关节轴线向量坐标。

2.根据权利要求1所述一种基于惯性传感器的抗位置窜动关节角度解算实时方法，其特征在于，所述步骤1具体为，获取置于大腿、小腿上的IMU传感器在实验对象保持正常行走时的数据，将测得的加速度、角速度信息通过无相位滞后的低通巴特沃斯滤波器，截止频率分别为8hz和10hz。

3.根据权利要求1所述一种基于惯性传感器的抗位置窜动关节角度解算实时方法，其特征在于，所述步骤2具体为，每个滑动窗口的间隔为500到1000个采样点，由于角速度只在关节轴线外的旋转上数量相等，关节生理学运动约束表达为：

||ω₁×j₁||-||ω₂×j₂||＝0

ω₁，ω₂为大腿、小腿上惯性传感器测得的角速度信息，j₁，j₂为关节轴线向量在大腿、小腿上惯性传感器固连坐标系中表达的坐标，将j₁，j₂的坐标用球坐标形式描述(J_i＝[cos(θ_i)cos(u_i)，cos(θ_i)sin(u_i)，sin(θ_i)]，i＝1，2)，利用该运动约束作为代价函数，以θ_i，u_i为变量，利用高斯-牛顿算法求解上述问题。

4.根据权利要求1所述一种基于惯性传感器的抗位置窜动关节角度解算实时方法，其特征在于，所述步骤3具体为，通过步骤2得到关节轴线向量j₁，j₂坐标后，可通过角速度积分得到关节角度，由于肢体的角速度在关节轴线方向的差值即为关节的旋转角速度，因此关节轴线方向的角速度差值在时间上的积分即为：

q_w＝∫(ω₁·j₁-ω₂·j₂)dt。

5.根据权利要求1所述一种基于惯性传感器的抗位置窜动关节角度解算实时方法，其特征在于，所述步骤4具体为，由于步骤3中角速度积分放大了测量噪声，步骤3求得的角度漂移较为严重，根据加速度a₁，a₂的前两项分量在关节轴线向量上的投影，可根据加速度倾角求解关节角度，计算如下：

6.根据权利要求1所述一种基于惯性传感器的抗位置窜动关节角度解算实时方法，其特征在于，所述步骤5具体为，由于用于求解两个投影的夹角q_a的加速度混合了重力加速度、运动产生的加速度和噪声，其结果精度较差，因此针对两个投影的夹角q_a和关节角度q_w的不同特点通过互补滤波将二者融合，计算如下：

q＝γ·q_a+(1-γ)·q_w。

7.根据权利要求1所述一种基于惯性传感器的抗位置窜动关节角度解算实时方法，其特征在于，所述步骤6中，若各滑动窗口求得的关节轴线向量j_i的差的模超过阈值，则判断传感器发生窜动，重启关节轴向估计，更新窜动后的关节轴线向量；若未超过阈值，则判断为未发生窜动，继续解算关节角度。

Images