Title: 利用 IMU 估计人体关节轴向和位置 —— “Joint axis and position estimation from inertial measurement data by exploiting kinematic constraints” —— 论文推导
文章目录
- I. 论文回顾
- II. 铰接关节的约束
- 1. 铰接关节约束的原理
- 2. 铰接关节约束的梯度
- 3. 铰接关节约束梯度的人工推导
- III. 球窝关节的约束
- 1. 球窝关节约束的原理
- 2. 球窝关节约束的人工解读
- 3. 球窝关节约束梯度的符号计算
- IV. 算法实现
- V. 小结
本篇博客中, 如在引用段落中的是 AI 生成的内容. 此处主要用的是豆包. 因提示 “服务器繁忙,请稍后再试”, 没有用 Deepseek.
在正常段落中的是由博主真人输出的.
I. 论文回顾
论文 Joint axis and position estimation from inertial measurement data by exploiting kinematic constraints 由 Thomas Seel 等人撰写。文章提出利用关节运动学约束从惯性测量数据中估计关节轴和关节位置的新方法,经仿真和实验验证有效,可应用于机器人、车辆等领域, 尤其是外骨骼控制、人体运动跟踪等场景。
研究背景:惯性测量单元(IMU)在人体运动分析等领域应用广泛,但使用中存在两个问题:传感器偏差导致位置和角度估计漂移;需知道传感器坐标系相对关节轴或安装段的方向信息。现有解决方法存在局限性,因此需要能准确估计关节位置和轴的方法。
利用运动学约束
- 铰链关节约束:两个通过铰链关节连接的刚性段,其陀螺仪测量的角速度在关节平面投影长度相等,利用该特性可在传感器方向未知时识别铰链关节轴。通过计算相关梯度,使用高斯 - 牛顿算法可搜索满足条件的关节轴向量。此外,还需判断估计的关节轴向量是否对齐。
- 球窝关节约束:对于球窝关节,结合加速度计读数,可利用关节中心加速度相等的约束来估计关节中心到传感器坐标系原点的偏移向量,同样通过计算梯度并使用高斯 - 牛顿算法进行估计。
建模与仿真:开发三连杆运动学仿真模型,模拟传感器测量。对铰链关节轴和球窝关节偏移向量估计算法进行实现,采用特定参数化和更新过程。仿真结果表明,算法在不同运动、采样率和噪声幅度下表现良好,估计值能快速收敛到真实值附近。
实验结果:将算法应用于基于 IMU 的步态分析实验数据,实验中使用无线运动追踪器,让受试者进行腿部和脚部环绕运动及行走,记录传感器数据。结果显示,算法能在不到二十次迭代中正确识别膝关节轴和踝关节位置,虽存在一定误差,但最小二乘法能有效处理这些不准确性。
结论与展望:提出的最小二乘法能有效估计关节轴和位置,无需传感器安装知识和积分运算,对采样率要求低。该算法可应用于机器人操纵器、链接车辆等领域,未来将关注角度相关关节轴和位置的识别等扩展研究。
II. 铰接关节的约束
1. 铰接关节约束的原理
∥ g 1 ( t ) × j 1 ∥ 2 − ∥ g 2 ( t ) × j 2 ∥ 2 = 0 (1) \left\| g_{1}(t) × j_{1}\right\| _{2}-\left\| g_{2}(t) × j_{2}\right\| _{2}=0 \tag{1} ∥g1(t)×j1∥2−∥g2(t)×j2∥2=0(1)
两个通过铰链关节连接的刚性段,其陀螺仪测量的角速度在关节平面投影长度相等,利用该特性可在传感器方向未知时识别铰链关节轴,其原理涉及几何关系和运动学知识:
- 铰链关节的运动特性:铰链关节只允许两个刚性段在一个特定平面内相对转动,这个平面就是关节平面,而关节轴垂直于该平面。
两个刚性段虽可在空间自由旋转和移动,但相对运动受限于这个铰链关节的约束。- 角速度的几何关系:分别安装在两个刚性段上的陀螺仪,测量得到各自的角速度 g 1 ( t ) g_{1}(t) g1(t) 和 g 2 ( t ) g_{2}(t) g2(t)。
从几何角度看, g 1 ( t ) g_{1}(t) g1(t) 和 g 2 ( t ) g_{2}(t) g2(t) 的差异仅在于关节角的变化速度以及随时间变化的旋转矩阵。
因为它们围绕同一个铰链关节轴运动,所以在垂直于关节轴的平面(即关节平面)上的投影具有特殊性质。- 投影长度相等的证明:设单位关节轴向量在第一段陀螺仪坐标系中为 j 1 j_{1} j1,在第二段陀螺仪坐标系中为 j 2 j_{2} j2 。
根据向量叉乘的几何意义,向量 g i ( t ) g_{i}(t) gi(t) 与 j i j_{i} ji 叉乘的结果,其模长 ∥ g i ( t ) × j i ∥ 2 \left\| g_{i}(t) × j_{i}\right\| _{2} ∥gi(t)×ji∥2 表示 g i ( t ) g_{i}(t) gi(t) 在垂直于 j i j_{i} ji 方向(即关节平面)上的投影长度。
由于两个刚性段围绕同一关节轴运动,在任意时刻 t t t, g 1 ( t ) g_{1}(t) g1(t) 和 g 2 ( t ) g_{2}(t) g2(t) 在关节平面的投影长度必然相等,即 ∥ g 1 ( t ) × j 1 ∥ 2 − ∥ g 2 ( t ) × j 2 ∥ 2 = 0 \left\| g_{1}(t) × j_{1}\right\| _{2}-\left\| g_{2}(t) × j_{2}\right\| _{2}=0 ∥g1(t)×j1∥2−∥g2(t)×j2∥2=0。- 识别关节轴的方法:当传感器方向未知时,在大量的陀螺仪测量数据中,搜索满足 ∥ g 1 ( t ) × j 1 ∥ 2 − ∥ g 2 ( t ) × j 2 ∥ 2 = 0 \left\| g_{1}(t) × j_{1}\right\| _{2}-\left\| g_{2}(t) × j_{2}\right\| _{2}=0 ∥g1(t)×j1∥2−∥g2(t)×j2∥2=0 的向量 j 1 ^ \hat{j_{1}} j1^和 j 2 ^ \hat{j_{2}} j2^ ,就能找到可能的关节轴方向。
通过计算相关梯度(如 d ( ∥ g i ( t ) × j i ∥ 2 ) d j i = ( g i ( t ) × j i ) × g i ( t ) ∥ g i ( t ) × j i ∥ 2 \frac{d\left(\left\| g_{i}(t) × j_{i}\right\| _{2}\right)}{d j_{i}}=\frac{\left(g_{i}(t) × j_{i}\right) × g_{i}(t)}{\left\| g_{i}(t) × j_{i}\right\| _{2}} djid(∥gi(t)×ji∥2)=∥gi(t)×ji∥2(gi(t)×ji)×gi(t) , i = 1 , 2 i = 1, 2 i=1,2),并利用高斯-牛顿算法等优化方法,可在最小二乘意义下找到最符合条件的 j 1 ^ \hat{j_{1}} j1^ 和 j 2 ^ \hat{j_{2}} j2^ ,从而实现对铰链关节轴的识别。
在推导方程 (1) 时,向量 g i ( t ) g_{i}(t) gi(t) 与 j i j_{i} ji 叉乘的模长公式是基于向量叉乘的几何意义和性质得到的。
- 向量叉乘的几何意义: 向量 a ⃗ \vec{a} a 与 b ⃗ \vec{b} b 叉乘 a ⃗ × b ⃗ \vec{a}\times\vec{b} a×b,其结果是一个向量,该向量的模长 ∣ a ⃗ × b ⃗ ∣ \vert\vec{a}\times\vec{b}\vert ∣a×b∣ 等于 ∣ a ⃗ ∣ ∣ b ⃗ ∣ sin θ \vert\vec{a}\vert\vert\vec{b}\vert\sin\theta ∣a∣∣b∣sinθ,其中 θ \theta θ 是 a ⃗ \vec{a} a 与 b ⃗ \vec{b} b 之间的夹角。
在当前问题中,对于向量 g i ( t ) g_{i}(t) gi(t) 与单位关节轴向量 j i j_{i} ji( ∣ j i ∣ = 1 \vert j_{i}\vert = 1 ∣ji∣=1 ),它们叉乘的模长 ∥ g i ( t ) × j i ∥ 2 \left\| g_{i}(t) × j_{i}\right\| _{2} ∥gi(t)×ji∥2,根据上述几何意义,就等于 ∣ g i ( t ) ∣ ∣ j i ∣ sin θ = ∣ g i ( t ) ∣ sin θ \vert g_{i}(t)\vert\vert j_{i}\vert\sin\theta = \vert g_{i}(t)\vert\sin\theta ∣gi(t)∣∣ji∣sinθ=∣gi(t)∣sinθ,这里 θ \theta θ 是 g i ( t ) g_{i}(t) gi(t) 与 j i j_{i} ji 的夹角。- 投影长度的表示: 由于 j i j_{i} ji 垂直于关节平面, g i ( t ) g_{i}(t) gi(t) 与 j i j_{i} ji 叉乘的模长 ∥ g i ( t ) × j i ∥ 2 \left\| g_{i}(t) × j_{i}\right\| _{2} ∥gi(t)×ji∥2,实际上就是 g i ( t ) g_{i}(t) gi(t) 在垂直于 j i j_{i} ji 方向(即关节平面)上的投影长度。
从几何直观上理解,在以 j i j_{i} ji 为法向量的关节平面上, g i ( t ) g_{i}(t) gi(t) 在该平面的投影长度可以通过上述叉乘模长公式来表示,这是由向量运算的几何性质所决定的。
图形表示:
2. 铰接关节约束的梯度
以下是对梯度公式 d ( ∥ g i ( t ) × j i ∥ 2 ) d j i = ( g i ( t ) × j i ) × g i ( t ) ∥ g i ( t ) × j i ∥ 2 \frac{d\left(\left\| g_{i}(t) × j_{i}\right\| _{2}\right)}{d j_{i}}=\frac{\left(g_{i}(t) × j_{i}\right) × g_{i}(t)}{\left\| g_{i}(t) × j_{i}\right\| _{2}} djid(∥gi(t)×ji∥2)=∥gi(t)×ji∥2(gi(t)×ji)×gi(t) 的推导过程:
明确符号与基本定义:
设 g i ( t ) g_{i}(t) gi(t) 和 j i j_{i} ji 为三维向量, × \times ×表示向量的叉乘运算, ∥ ⋅ ∥ 2 \left\| \cdot \right\| _{2} ∥⋅∥2表示向量的2 - 范数(即欧几里得范数)。对于向量 a ⃗ = ( a 1 , a 2 , a 3 ) \vec{a}=(a_1,a_2,a_3) a=(a1,a2,a3),其 2 - 范数 ∥ a ⃗ ∥ 2 = a 1 2 + a 2 2 + a 3 2 \left\|\vec{a}\right\| _{2}=\sqrt{a_1^2 + a_2^2 + a_3^2} ∥a∥2=a12+a22+a32。对向量叉乘的模长函数求导:
令 h ( j i ) = ∥ g i ( t ) × j i ∥ 2 h(j_{i})=\left\| g_{i}(t) × j_{i}\right\| _{2} h(ji)=∥gi(t)×ji∥2,我们要计算 d h ( j i ) d j i \frac{dh(j_{i})}{d j_{i}} djidh(ji)。
根据向量叉乘的定义,若 g i ( t ) = ( g i 1 ( t ) , g i 2 ( t ) , g i 3 ( t ) ) g_{i}(t)=(g_{i1}(t),g_{i2}(t),g_{i3}(t)) gi(t)=(gi1(t),gi2(t),gi3(t)), j i = ( j i 1 , j i 2 , j i 3 ) j_{i}=(j_{i1},j_{i2},j_{i3}) ji=(ji1,ji2,ji3),则 g i ( t ) × j i = ( g i 2 ( t ) j i 3 − g i 3 ( t ) j i 2 , g i 3 ( t ) j i 1 − g i 1 ( t ) j i 3 , g i 1 ( t ) j i 2 − g i 2 ( t ) j i 1 ) g_{i}(t) × j_{i}=(g_{i2}(t)j_{i3}-g_{i3}(t)j_{i2},g_{i3}(t)j_{i1}-g_{i1}(t)j_{i3},g_{i1}(t)j_{i2}-g_{i2}(t)j_{i1}) gi(t)×ji=(gi2(t)ji3−gi3(t)ji2,gi3(t)ji1−gi1(t)ji3,gi1(t)ji2−gi2(t)ji1)。
设 u = g i ( t ) × j i u = g_{i}(t) × j_{i} u=gi(t)×ji,那么 h ( j i ) = ∥ u ∥ 2 = u 1 2 + u 2 2 + u 3 2 h(j_{i})=\left\|u\right\| _{2}=\sqrt{u_1^2 + u_2^2 + u_3^2} h(ji)=∥u∥2=u12+u22+u32。
根据复合函数求导法则,我们先对 h ( j i ) h(j_{i}) h(ji) 关于 u u u 求导,再乘以 u u u 关于 j i j_{i} ji 的导数。
- 对 h ( j i ) = u 1 2 + u 2 2 + u 3 2 h(j_{i})=\sqrt{u_1^2 + u_2^2 + u_3^2} h(ji)=u12+u22+u32 关于 u u u 求导:
根据求导公式 ( x ) ′ = 1 2 x (\sqrt{x})^\prime=\frac{1}{2\sqrt{x}} (x)′=2x1,对 h ( j i ) h(j_{i}) h(ji) 关于 u k u_k uk( k = 1 , 2 , 3 k = 1,2,3 k=1,2,3)求偏导数可得:
∂ h ( j i ) ∂ u k = u k ∥ u ∥ 2 \frac{\partial h(j_{i})}{\partial u_k}=\frac{u_k}{\left\|u\right\| _{2}} ∂uk∂h(ji)=∥u∥2uk,所以 ∂ h ( j i ) ∂ u = u ∥ u ∥ 2 \frac{\partial h(j_{i})}{\partial u}=\frac{u}{\left\|u\right\| _{2}} ∂u∂h(ji)=∥u∥2u(这里 u u u 是向量形式)。- 对 u = g i ( t ) × j i u = g_{i}(t) × j_{i} u=gi(t)×ji 关于 j i j_{i} ji 求导:
我们知道 u k u_k uk 是关于 j i 1 , j i 2 , j i 3 j_{i1},j_{i2},j_{i3} ji1,ji2,ji3 的线性函数。以 u 1 = g i 2 ( t ) j i 3 − g i 3 ( t ) j i 2 u_1 = g_{i2}(t)j_{i3}-g_{i3}(t)j_{i2} u1=gi2(t)ji3−gi3(t)ji2 为例,对 u 1 u_1 u1 关于 j i 1 j_{i1} ji1 求偏导数为 0 0 0,对 u 1 u_1 u1 关于 j i 2 j_{i2} ji2 求偏导数为 − g i 3 ( t ) -g_{i3}(t) −gi3(t),对 u 1 u_1 u1关于 j i 3 j_{i3} ji3 求偏导数为 g i 2 ( t ) g_{i2}(t) gi2(t)。
同理可得 u 2 u_2 u2 和 u 3 u_3 u3 关于 j i 1 , j i 2 , j i 3 j_{i1},j_{i2},j_{i3} ji1,ji2,ji3 的偏导数。
经过整理可以发现, ∂ u ∂ j i = g i ( t ) × \frac{\partial u}{\partial j_{i}}=g_{i}(t)\times ∂ji∂u=gi(t)×(这里“ × \times ×”表示一种与叉乘相关的矩阵 - 向量运算结构,从结果上看等同于 g i ( t ) g_{i}(t) gi(t) 再次与 u u u 叉乘)。
- 应用链式法则:
根据链式法则 d h ( j i ) d j i = ∂ h ( j i ) ∂ u ⋅ ∂ u ∂ j i \frac{dh(j_{i})}{d j_{i}}=\frac{\partial h(j_{i})}{\partial u}\cdot\frac{\partial u}{\partial j_{i}} djidh(ji)=∂u∂h(ji)⋅∂ji∂u,将前面求得的结果代入:
d ( ∥ g i ( t ) × j i ∥ 2 ) d j i = u ∥ u ∥ 2 ⋅ ( g i ( t ) × ) = ( g i ( t ) × j i ) × g i ( t ) ∥ g i ( t ) × j i ∥ 2 \frac{d\left(\left\| g_{i}(t) × j_{i}\right\| _{2}\right)}{d j_{i}}=\frac{u}{\left\|u\right\| _{2}}\cdot(g_{i}(t)\times)=\frac{\left(g_{i}(t) × j_{i}\right) × g_{i}(t)}{\left\| g_{i}(t) × j_{i}\right\| _{2}} djid(∥gi(t)×ji∥2)=∥u∥2u⋅(gi(t)×)=∥gi(t)×ji∥2(gi(t)×ji)×gi(t) 。综上,得到了给定的梯度计算公式。
3. 铰接关节约束梯度的人工推导
曾经人工推导过如下. (AI 时代, 人工涂鸦看起来可能更温暖一点, 我们何去何从?)
III. 球窝关节的约束
1. 球窝关节约束的原理
场景与前提:
考虑由球窝关节连接的两个连杆,如原文中图 2 所示。球窝关节具有三个自由度,这意味着两个相连刚体可在多个方向上相对转动。由于这种多自由度的特性,两个传感器测量到的角速度之间不存在普遍的固定关系。为了利用运动学约束进行相关研究,需要引入加速度计的读数。变量定义:
设两个传感器(分别位于两个连杆上)的加速度分别为 a 1 ( t ) a_{1}(t) a1(t) 和 a 2 ( t ) a_{2}(t) a2(t) , o 1 o_{1} o1 和 o 2 o_{2} o2 分别是从关节中心到第一和第二传感器坐标系原点的偏移向量。
定义 Γ g i ( o i ) \Gamma_{g_{i}}(o_{i}) Γgi(oi): Γ g i ( o i ) = g i ( t ) × ( g i ( t ) × o i ) + g ˙ i ( t ) × o i \Gamma_{g_{i}}(o_{i}) = g_{i}(t)\times(g_{i}(t)\times o_{i})+\dot{g}_{i}(t)\times o_{i} Γgi(oi)=gi(t)×(gi(t)×oi)+g˙i(t)×oi , i = 1 , 2 i = 1,2 i=1,2。
Γ g i ( o i ) \Gamma_{g_{i}}(o_{i}) Γgi(oi) 表示由于绕关节中心旋转而产生的(径向和切向)加速度。
它综合考虑了角速度 g i ( t ) g_{i}(t) gi(t) 及其变化率 g ˙ i ( t ) \dot{g}_{i}(t) g˙i(t) 与偏移向量 o i o_{i} oi 的关系。
通过向量叉乘运算,体现了旋转运动对加速度的贡献。方程(3)解释:
方程 ∥ a 1 ( t ) − Γ g 1 ( o 1 ) ∥ 2 − ∥ a 2 ( t ) − Γ g 2 ( o 2 ) ∥ 2 = 0 ∀ t \left\| a_{1}(t)-\Gamma_{g_{1}}(o_{1})\right\| _{2}-\left\| a_{2}(t)-\Gamma_{g_{2}}(o_{2})\right\| _{2}=0 \ \forall t ∥a1(t)−Γg1(o1)∥2−∥a2(t)−Γg2(o2)∥2=0 ∀t ,其含义是 ( a 1 ( t ) − Γ g 1 ( o 1 ) ) (a_{1}(t)-\Gamma_{g_{1}}(o_{1})) (a1(t)−Γg1(o1)) 表示在第一个局部坐标系下关节中心的加速度, ( a 2 ( t ) − Γ g 2 ( o 2 ) ) (a_{2}(t)-\Gamma_{g_{2}}(o_{2})) (a2(t)−Γg2(o2))表示在第二个局部坐标系下关节中心的加速度。
因为球窝关节连接的两个连杆在关节中心处的加速度,在不考虑坐标系旋转差异时应相等(这里忽略了旋转矩阵的影响,只考虑加速度的大小关系),所以两者加速度的欧几里得范数(即 ∥ a 1 ( t ) − Γ g 1 ( o 1 ) ∥ 2 \left\| a_{1}(t)-\Gamma_{g_{1}}(o_{1})\right\| _{2} ∥a1(t)−Γg1(o1)∥2 和 ∥ a 2 ( t ) − Γ g 2 ( o 2 ) ∥ 2 \left\| a_{2}(t)-\Gamma_{g_{2}}(o_{2})\right\| _{2} ∥a2(t)−Γg2(o2)∥2 )之差为0。
这个方程在任何时刻 t t t都成立,基于此,可以利用大量的数据集 ( a 1 ( t k ) , a 2 ( t k ) , g 1 ( t k ) , g 2 ( t k ) ) k = 1 N (a_{1}(t_{k}), a_{2}(t_{k}), g_{1}(t_{k}), g_{2}(t_{k}))_{k = 1}^{N} (a1(tk),a2(tk),g1(tk),g2(tk))k=1N 来估计 o 1 o_{1} o1 和 o 2 o_{2} o2。方程(4)解释:
方程 d ( ∥ a i ( t ) − Γ g i ( o i ) ∥ 2 ) d o i = Γ g i T ( a i ( t ) − Γ g i ( o i ) ) ∥ a i ( t ) − Γ g i ( o i ) ∥ 2 \frac{d\left(\left\| a_{i}(t)-\Gamma_{g_{i}}(o_{i})\right\| _{2}\right)}{d o_{i}}=\frac{\Gamma_{g_{i}}^{T}(a_{i}(t)-\Gamma_{g_{i}}(o_{i}))}{\left\| a_{i}(t)-\Gamma_{g_{i}}(o_{i})\right\| _{2}} doid(∥ai(t)−Γgi(oi)∥2)=∥ai(t)−Γgi(oi)∥2ΓgiT(ai(t)−Γgi(oi)) , i = 1 , 2 i = 1,2 i=1,2 ,这是关于 ∥ a i ( t ) − Γ g i ( o i ) ∥ 2 \left\| a_{i}(t)-\Gamma_{g_{i}}(o_{i})\right\| _{2} ∥ai(t)−Γgi(oi)∥2 对 o i o_{i} oi 的梯度公式。
其中 Γ g i T ( o i ) = ( o i × g i ( t ) ) × g i ( t ) + o i × g ˙ i ( t ) \Gamma _{g_{i}}^{T}(o_{i})=(o_{i}\times g_{i}(t))\times g_{i}(t)+o_{i}\times \dot {g}_{i}(t) ΓgiT(oi)=(oi×gi(t))×gi(t)+oi×g˙i(t) ,它是 Γ g i \Gamma_{g_{i}} Γgi 矩阵乘法表示形式的转置。
这个梯度公式在优化算法中非常重要,用于计算当 o i o_{i} oi 发生微小变化时, ∥ a i ( t ) − Γ g i ( o i ) ∥ 2 \left\| a_{i}(t)-\Gamma_{g_{i}}(o_{i})\right\| _{2} ∥ai(t)−Γgi(oi)∥2 的变化率,进而通过迭代优化方法来求解 o 1 o_{1} o1 和 o 2 o_{2} o2 ,以满足方程(3)的约束条件,实现从测量数据中准确估计关节中心到传感器坐标系原点的偏移向量。
2. 球窝关节约束的人工解读
绕关节中心旋转而产生的(径向和切向)加速度的人工推导:
关节中心加速度的人工推导如下.
说明:
为了推导方便,我们构建的全局坐标系. 而论文中都是基于瞬时固定的 IMU 局部坐标系来推导. 事实上, 瞬时固定的局部坐标系看作为了参考坐标系, 作用类似于全局坐标系. 瞬时固定的局部坐标系并不是固定在 IMU 上的随动局部坐标系, 而是 IMU 运动瞬时在外部惯性空间中与 IMU 随动坐标系瞬时重合的外部参考坐标系.
基于瞬时固定的 IMU 局部坐标系与我们引入的全局坐标系相差一个旋转矩阵. 相应得到的速度和加速度关系也相差一个旋转矩阵.
3. 球窝关节约束梯度的符号计算
d ( ∥ a i ( t ) − Γ g i ( o i ) ∥ 2 ) d o i = Γ g i T ( a i ( t ) − Γ g i ( o i ) ) ∥ a i ( t ) − Γ g i ( o i ) ∥ 2 (4) \frac{d\left(\left\| a_{i}(t)-\Gamma_{g_{i}}(o_{i})\right\| _{2}\right)}{d o_{i}}=\frac{\Gamma_{g_{i}}^{T}(a_{i}(t)-\Gamma_{g_{i}}(o_{i}))}{\left\| a_{i}(t)-\Gamma_{g_{i}}(o_{i})\right\| _{2}} \tag{4} doid(∥ai(t)−Γgi(oi)∥2)=∥ai(t)−Γgi(oi)∥2ΓgiT(ai(t)−Γgi(oi))(4)
简写 Δ i ≜ a i ( t ) − Γ g i ( o i ) \Delta_i \triangleq a_{i}(t)-\Gamma_{g_{i}}(o_{i}) Δi≜ai(t)−Γgi(oi). 因为 ∥ Δ i ∥ 2 = Δ i ⋅ Δ i \Vert \Delta_i \Vert_2 = \sqrt{\Delta_i\cdot \Delta_i} ∥Δi∥2=Δi⋅Δi, 可知式 (4) 左手边为
d ( ∥ Δ i ∥ 2 ) d o i = d Δ i ⋅ Δ i d o i = 1 2 Δ i ⋅ Δ i d ( Δ i ⋅ Δ i ) d o i = 1 2 ∥ Δ i ∥ 2 d ( Δ i ⋅ Δ i ) d o i (4-a) \frac{d (\Vert \Delta_i \Vert_2 )}{d o_i} = \frac{d \sqrt{\Delta_i \cdot \Delta_i}}{d o_i} = \frac{1}{2\sqrt{\Delta_i \cdot \Delta_i}} \frac{d(\Delta_i \cdot \Delta_i)}{d o_i} = \frac{1}{2\Vert{\Delta_i }\Vert_2} \frac{d(\Delta_i \cdot \Delta_i)}{d o_i} \tag{4-a} doid(∥Δi∥2)=doidΔi⋅Δi=2Δi⋅Δi1doid(Δi⋅Δi)=2∥Δi∥21doid(Δi⋅Δi)(4-a)
式 (4) 右手边可以简写为
Γ g i T ( a i ( t ) − Γ g i ( o i ) ) ∥ a i ( t ) − Γ g i ( o i ) ∥ 2 = Γ g i T ( Δ i ) ∥ Δ i ∥ 2 (4-b) \frac{\Gamma_{g_{i}}^{T}(a_{i}(t)-\Gamma_{g_{i}}(o_{i}))}{\left\| a_{i}(t)-\Gamma_{g_{i}}(o_{i})\right\| _{2}} = \frac{\Gamma_{g_{i}}^{T}(\Delta_i)}{\left\| \Delta_i \right\| _{2}} \tag{4-b} ∥ai(t)−Γgi(oi)∥2ΓgiT(ai(t)−Γgi(oi))=∥Δi∥2ΓgiT(Δi)(4-b)
所以要证明式 (4) 成立, 只要验证
1 2 d ( Δ i ⋅ Δ i ) d o i = Γ g i T ( Δ i ) (4-c) \frac{1}{2} \frac{d(\Delta_i \cdot \Delta_i)}{d o_i} = {\Gamma_{g_{i}}^{T}(\Delta_i)} \tag{4-c} 21doid(Δi⋅Δi)=ΓgiT(Δi)(4-c)
成立即可.
下面的推导计算比较复杂, 我们借用 Maxima 符号计算来推导. (不过结果和论文中相差一个负号)
/*Thomas Seel, etc. Joint Axis and Position Estimation from Inertial Measurement Data by Exploiting Kinematic Constraints*/
/*B. Constraints induced by sphernoidal joints*/
/* Derivation of Equation (4)*/
/*a---acceleration of IMU*/
/*o---offset vestor from the joint center to the origin of the IMU frame*/
/*g---angular velocity of the gyroscope(IMU)*/
/*dg---dg(t)/dt*/
/*Γ---the (radial and tangential) acceleration due to rotation around the joint center(spheroidal joint)*/
/*Gnorm---function to be derivated at left hand of equation (4). Derivatives with respect to vectors*/
/*DGnormX---X-component of the left hand of equation (4). Derivatives with respect to vectors*/
/*DGnormY---Y-component of the left hand of equation (4)*/
/*DGnormZ---Z-component of the left hand of equation (4)*/a:[a_x,a_y,a_z];
o:[o_x,o_y,o_z];
g:[g_x,g_y,g_z];
dg:[dg_x,dg_y,dg_z];
load("vect");
express(g~o);
Γ:(g~(g~o)+dg~o);
Γ2:express(Γ);
Γ2a:(a-Γ2);Gnorm: Γ2a.Γ2a$
DGnormX: expand(diff(Gnorm,o_x)/2);
/*numerator of the right hand of equation (4)*/
TΓ:(Γ2a~g)~g+Γ2a~dg$
TΓ2:express(TΓ)$/*X-component of the numerator of the right hand of equation (4)*/
expand(TΓ2[1]);DGnormY:expand(diff(Gnorm,o_y)/2);
/*Y-component of the numerator of the right hand of equation (4)*/
expand(TΓ2[2]);DGnormZ:expand(diff(Gnorm,o_z)/2);
/*Z-component of the numerator of the right hand of equation (4)*/
expand(TΓ2[3]);
IV. 算法实现
初始化:
设定输入数据,即获取 N N N 个数据集 ( g 1 ( t k ) , g 2 ( t k ) ) k = 1 N (g_{1}(t_{k}), g_{2}(t_{k}))_{k = 1}^{N} (g1(tk),g2(tk))k=1N,其中 N ≫ 4 N \gg 4 N≫4 。这些数据代表在不同时刻 t k t_{k} tk 从两个传感器获取的与关节运动相关的信息。
初始化变量 x x x,在估计球窝关节偏移向量时, x x x 为 o 1 o_{1} o1 和 o 2 o_{2} o2 的拼接。这里 o 1 o_{1} o1 和 o 2 o_{2} o2 分别是从关节中心到第一和第二传感器坐标系原点的偏移向量。
(铰接关节的 x x x 由公式 (5) 第一式定义)关节轴估计:
利用公式(5),基于当前的 x x x 值计算关节轴估计值 j 1 ^ \hat{j_{1}} j1^ 和 j 2 ^ \hat{j_{2}} j2^。
公式(5)建立了 x x x 与 j 1 ^ \hat{j_{1}} j1^、 j 2 ^ \hat{j_{2}} j2^ 之间的数学关系,通过将 x x x 的各个分量代入公式(5),按照规定的数学运算顺序进行计算,得出 j 1 ^ \hat{j_{1}} j1^ 和 j 2 ^ \hat{j_{2}} j2^ 的值。
同时,为保证估计的合理性,将关节轴估计值限制为单位长度,使得估计问题转化为四维。计算误差与梯度:
[球窝关节] 定义误差函数 e ( t ) ≜ ∥ a 1 ( t ) − Γ g 1 ( o 1 ) ∥ 2 − ∥ a 2 ( t ) − Γ g 2 ( o 2 ) ∥ 2 e(t) \triangleq \left\|a_{1}(t) - \Gamma_{g_{1}}(o_{1})\right\|_{2} - \left\|a_{2}(t) - \Gamma_{g_{2}}(o_{2})\right\|_{2} e(t)≜∥a1(t)−Γg1(o1)∥2−∥a2(t)−Γg2(o2)∥2 。其中 a 1 ( t ) a_{1}(t) a1(t) 和 a 2 ( t ) a_{2}(t) a2(t) 分别是两个传感器在时刻 t t t 的加速度测量值, Γ g 1 ( o 1 ) \Gamma_{g_{1}}(o_{1}) Γg1(o1) 和 Γ g 2 ( o 2 ) \Gamma_{g_{2}}(o_{2}) Γg2(o2) 是与关节旋转相关的加速度项,通过此误差函数衡量当前估计的偏移向量 o 1 o_{1} o1 和 o 2 o_{2} o2 的准确性。
[铰接关节] 定义误差函数 e ( k ) ≜ ∥ j 1 ^ × g 1 ( t k ) ∥ 2 − ∥ j 2 ^ × g 2 ( t k ) ∥ 2 e(k) \triangleq \left\|\hat{j_{1}} × g_{1}(t_{k})\right\|_{2}-\left\|\hat{j_{2}} × g_{2}(t_{k})\right\|_{2} e(k)≜ j1^×g1(tk) 2− j2^×g2(tk) 2,即两个范数的差值。如果关节轴估计准确,那么对于每个时刻 t k t_{k} tk, ∥ j 1 ^ × g 1 ( t k ) ∥ 2 \left\|\hat{j_{1}} × g_{1}(t_{k})\right\|_{2} j1^×g1(tk) 2 和 ∥ j 2 ^ × g 2 ( t k ) ∥ 2 \left\|\hat{j_{2}} × g_{2}(t_{k})\right\|_{2} j2^×g2(tk) 2 的值应该接近,此时 e ( k ) e(k) e(k) 接近零。通过分析误差向量 e e e 的值,可以评估关节轴估计的准确性,并进一步用于优化估计过程,例如在迭代算法中,根据误差向量 e e e 来调整 j 1 ^ \hat{j_{1}} j1^ 和 j 2 ^ \hat{j_{2}} j2^ 的值,使得误差逐渐减小,从而得到更准确的关节轴估计。
根据公式(4)计算误差 e e e 关于 x x x 的梯度 d e d x \frac{d e}{d x} dxde 。公式(4)给出了关于误差对 x x x 的导数形式,在计算过程中,需根据 x x x 的具体形式(即 o 1 o_{1} o1 和 o 2 o_{2} o2 的拼接)以及误差函数 e e e 的表达式,按照公式(4)的规则进行详细的数学推导和计算,得到梯度值。更新变量 x x x:
使用公式 x = x − p i n v ( d e d x ) e x = x - pinv(\frac{d e}{d x}) e x=x−pinv(dxde)e 更新变量 x x x 。这里 p i n v ( d e d x ) pinv(\frac{d e}{d x}) pinv(dxde) 表示梯度 d e d x \frac{d e}{d x} dxde 的伪逆,通过该公式,让 x x x 沿着负梯度方向(乘以误差 e e e)进行更新,步长由伪逆矩阵 p i n v ( d e d x ) pinv(\frac{d e}{d x}) pinv(dxde) 决定,使得误差 e e e 逐渐减小。循环与终止条件:
重复步骤 2 至 4,直到满足特定的终止条件。终止条件可以设定为误差 e e e 小于某个预设的阈值,表明算法已经收敛到一个足够准确的结果;或者达到一定的迭代次数,防止算法无限循环。噪声处理:
在计算过程中,由于 g 1 ( t ) g_{1}(t) g1(t) 和 g 2 ( t ) g_{2}(t) g2(t) 的时间导数对计算结果至关重要,且数据可能存在噪声干扰,采用一种抗噪声的非因果低通滤波器与简单的差商近似相结合的方法。
非因果低通滤波器用于去除数据中的高频噪声,使数据更加平滑;简单的差商近似则用于从离散的测量数据中估计 g 1 ( t ) g_{1}(t) g1(t) 和 g 2 ( t ) g_{2}(t) g2(t) 的时间导数,从而保证在噪声环境下算法仍能准确运行。
通过以上算法实现步骤,能够有效地估计关节轴及球窝关节的偏移向量,为后续相关问题的解决提供可靠的数据支持。
V. 小结
以上推导了利用 IMU 估计人体关节轴向与位置的数学原理. 该算法可用于外骨骼控制、人体运动跟踪等场景.
写这边博文 AI 帮了很大忙, 提高了效率. 但我在这篇博文中的作用却降低了吧?
版权声明:本文为博主原创文章,遵循 CC 4.0 BY 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/woyaomaishu2/article/details/145503633
本文作者:wzf@robotics_notes