利用 IMU 估计人体关节轴向和位置 —— 论文推导

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)×j12g2(t)×j22=0(1)

两个通过铰链关节连接的刚性段,其陀螺仪测量的角速度在关节平面投影长度相等,利用该特性可在传感器方向未知时识别铰链关节轴,其原理涉及几何关系和运动学知识:

  1. 铰链关节的运动特性:铰链关节只允许两个刚性段在一个特定平面内相对转动,这个平面就是关节平面,而关节轴垂直于该平面。
    两个刚性段虽可在空间自由旋转和移动,但相对运动受限于这个铰链关节的约束。
  2. 角速度的几何关系:分别安装在两个刚性段上的陀螺仪,测量得到各自的角速度 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) 的差异仅在于关节角的变化速度以及随时间变化的旋转矩阵。
    因为它们围绕同一个铰链关节轴运动,所以在垂直于关节轴的平面(即关节平面)上的投影具有特殊性质。
  3. 投影长度相等的证明:设单位关节轴向量在第一段陀螺仪坐标系中为 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)×ji2 表示 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)×j12g2(t)×j22=0
  4. 识别关节轴的方法:当传感器方向未知时,在大量的陀螺仪测量数据中,搜索满足 ∥ 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)×j12g2(t)×j22=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)×ji2)=gi(t)×ji2(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 叉乘的模长公式是基于向量叉乘的几何意义和性质得到的。

  1. 向量叉乘的几何意义: 向量 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)×ji2,根据上述几何意义,就等于 ∣ 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)∣∣jisinθ=gi(t)sinθ,这里 θ \theta θ g i ( t ) g_{i}(t) gi(t) j i j_{i} ji 的夹角。
  2. 投影长度的表示: 由于 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)×ji2,实际上就是 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)×ji2)=gi(t)×ji2(gi(t)×ji)×gi(t) 的推导过程:

  1. 明确符号与基本定义
    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

  2. 对向量叉乘的模长函数求导
    h ( j i ) = ∥ g i ( t ) × j i ∥ 2 h(j_{i})=\left\| g_{i}(t) × j_{i}\right\| _{2} h(ji)=gi(t)×ji2,我们要计算 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)ji3gi3(t)ji2,gi3(t)ji1gi1(t)ji3,gi1(t)ji2gi2(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)=u2=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 )=2x 1,对 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}} ukh(ji)=u2uk,所以 ∂ h ( j i ) ∂ u = u ∥ u ∥ 2 \frac{\partial h(j_{i})}{\partial u}=\frac{u}{\left\|u\right\| _{2}} uh(ji)=u2u(这里 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)ji3gi3(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 jiu=gi(t)×(这里“ × \times ×”表示一种与叉乘相关的矩阵 - 向量运算结构,从结果上看等同于 g i ( t ) g_{i}(t) gi(t) 再次与 u u u 叉乘)。
  1. 应用链式法则
    根据链式法则 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)=uh(ji)jiu,将前面求得的结果代入:
    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)×ji2)=u2u(gi(t)×)=gi(t)×ji2(gi(t)×ji)×gi(t)

综上,得到了给定的梯度计算公式。


3. 铰接关节约束梯度的人工推导

曾经人工推导过如下. (AI 时代, 人工涂鸦看起来可能更温暖一点, 我们何去何从?)

[notes]Joint-1 [notes]Joint-2-page2

III. 球窝关节的约束

1. 球窝关节约束的原理

  1. 场景与前提
    考虑由球窝关节连接的两个连杆,如原文中图 2 所示。球窝关节具有三个自由度,这意味着两个相连刚体可在多个方向上相对转动。由于这种多自由度的特性,两个传感器测量到的角速度之间不存在普遍的固定关系。为了利用运动学约束进行相关研究,需要引入加速度计的读数。

  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. 方程(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)2a2(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. 方程(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}) Δiai(t)Γgi(oi). 因为 ∥ Δ i ∥ 2 = Δ i ⋅ Δ i \Vert \Delta_i \Vert_2 = \sqrt{\Delta_i\cdot \Delta_i} Δi2=Δ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(Δi2)=doidΔiΔi =2ΔiΔi 1doid(ΔiΔi)=2∥Δi21doid(Δ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))=Δi2Γ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)*/:(Γ2a~g)~g+Γ2a~dg$
TΓ2:express()$

/*X-component of the numerator of the right hand of equation (4)*/
expand(2[1]);

DGnormY:expand(diff(Gnorm,o_y)/2);
/*Y-component of the numerator of the right hand of equation (4)*/
expand(2[2]);

DGnormZ:expand(diff(Gnorm,o_z)/2);
/*Z-component of the numerator of the right hand of equation (4)*/
expand(2[3]);
eq4_1 eq4-2

IV. 算法实现

  1. 初始化
    设定输入数据,即获取 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 N4 。这些数据代表在不同时刻 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) 第一式定义)

  2. 关节轴估计
    利用公式(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^ 的值。
    同时,为保证估计的合理性,将关节轴估计值限制为单位长度,使得估计问题转化为四维。

  3. 计算误差与梯度
    [球窝关节] 定义误差函数 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)2a2(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)的规则进行详细的数学推导和计算,得到梯度值。

  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=xpinv(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 逐渐减小。

  5. 循环与终止条件
    重复步骤 2 至 4,直到满足特定的终止条件。终止条件可以设定为误差 e e e 小于某个预设的阈值,表明算法已经收敛到一个足够准确的结果;或者达到一定的迭代次数,防止算法无限循环。

  6. 噪声处理
    在计算过程中,由于 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

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/965983.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

oracle ORA-27054报错处理

现象 在oracle执行expdp,rman备份,xtts的时候,由于没有足够的本地空间,只能使用到NFS的文件系统但有时候会出现如下报错 ORA-27054: NFS file system where the file is created or resides is not mounted with correct options根据提示信…

python模拟键盘输入(可视化界操作面)

因为受到一些限制,无法在输入框进行文本的复制粘贴,这时我们便需要模拟键盘输入一些文本内容,话不多说,直接上干货(文末附成品工具,需要自取,操作简单无脑,工具功能:将粘…

k8s部署go-fastdfs

前置环境:已部署k8s集群,ip地址为 192.168.10.1~192.168.10.5,总共5台机器。 1. 创建provisioner制备器(如果已存在,则不需要) 制备器的具体部署方式可参考我的上一篇文章: k8s部署rabbitmq-CSDN博客文章浏览阅读254次,点赞3次,收藏5次。k8s部署rabbitmqhttps://blo…

DeepSeek在FPGA/IC开发中的创新应用与未来潜力

随着人工智能技术的飞速发展,以DeepSeek为代表的大语言模型(LLM)正在逐步渗透到传统硬件开发领域。在FPGA(现场可编程门阵列)和IC(集成电路)开发这一技术密集型行业中,DeepSeek凭借其…

(一)DeepSeek大模型安装部署-Ollama安装

大模型deepseek安装部署 (一)、安装ollama curl -fsSL https://ollama.com/install.sh | sh sudo systemctl start ollama sudo systemctl enable ollama sudo systemctl status ollama(二)、安装ollama遇到网络问题,请手动下载 ollama-linux-amd64.tgz curl -L …

基于Flask的汽车质量投诉可视化分析系统的设计与实现

【FLask】基于Flask的汽车质量投诉可视化分析系统的设计与实现(完整系统源码开发笔记详细部署教程)✅ 目录 一、项目简介二、项目界面展示三、项目视频展示 一、项目简介 随着汽车市场的不断扩大和消费者维权意识的增强,汽车质量投诉问题日益…

计算机网络 应用层 笔记1(C/S模型,P2P模型,FTP协议)

应用层概述: 功能: 常见协议 应用层与其他层的关系 网络应用模型 C/S模型: 优点 缺点 P2P模型: 优点 缺点 DNS系统: 基本功能 系统架构 域名空间: DNS 服务器 根服务器: 顶级域…

Android studio 创建aar包给Unity使用

1、aar 是什么? 和 Jar有什么区别 aar 和 jar包 都是压缩包,可以使用压缩软件打开 jar包 用于封装 Java 类及其相关资源 aar 文件是专门为 Android 平台设计的 ,可以包含Android的专有内容,比如AndroidManifest.xml 文件 &#…

MySQL--loaddata infile、outfile into及mysqldump高效导入导出数据_mysql load outfile

【学习背景】 在日常工作和学习MySQL时,经常涉及到MySQL数据的导入和导出,分享几种常用又方便的方式: (1)MySQL命令行source命令 (3)语法into outfile和load data infile (3&#xf…

基于LMStudio本地部署DeepSeek R1

DeepSeek R1 DeepSeek R1是由DeepSeek团队开发的一款高性能AI推理模型,其开源版本包括完整的DeepSeek R1 671B权重,以及基于其蒸馏出的多个小型模型。 DeepSeek R1通过蒸馏技术将推理模式迁移到更小的模型中,显著提升了这些模型的推理能力。…

#渗透测试#批量漏洞挖掘#Splunk Enterprise for Windows 任意文件读取漏洞( CVE-2024-36991)

免责声明 本教程仅为合法的教学目的而准备,严禁用于任何形式的违法犯罪活动及其他商业行为,在使用本教程前,您应确保该行为符合当地的法律法规,继续阅读即表示您需自行承担所有操作的后果,如有异议,请立即停…

读书笔记--分布式架构的异步化和缓存技术原理及应用场景

本篇是在上一篇的基础上,主要对分布式应用架构下的异步化机制和缓存技术进行学习,主要记录和思考如下,供大家学习参考。大家知道原来传统的单一WAR应用中,由于所有数据都在同一个数据库中,因此事务问题一般借助数据库事…

【提示词工程】探索大语言模型的参数设置:优化提示词交互的技巧

在与大语言模型(Large Language Model, LLM)进行交互时,提示词的设计和参数设置直接影响生成内容的质量和效果。无论是通过 API 调用还是直接使用模型,掌握模型的参数配置方法都至关重要。本文将为您详细解析常见的参数设置及其应用场景,帮助您更高效地利用大语言模型。 …

(七)QT——消息事件机制&绘图&文件

目录 前言 消息事件机制 (Event System) 绘图 (Graphics & Drawing) 绘图设备 Qt 提供的主要绘图设备 Qt 主要绘图设备的特点 各个绘图设备的详细介绍 文件处理 (File Handling) 总结 前言 QT 是一个非常强大的图形用户界面(GUI)开发框架&…

ChatGPT提问技巧:行业热门应用提示词案例-文案写作

ChatGPT 作为强大的 AI 语言模型,已经成为文案写作的得力助手。但要让它写出真正符合你需求的文案,关键在于如何与它“沟通”,也就是如何设计提示词(Prompt)。以下是一些实用的提示词案例,帮助你解锁 ChatG…

C++服务端开发注意事项总结

文章目录 一、架构设计1. 选择合适的网络框架2. 确定并发模型3. 模块化设计 二、性能优化1. 优化内存管理2. 减少锁的使用3. 优化网络通信 三、安全性1. 输入验证2. 使用安全的通信协议3. 防止拒绝服务攻击(DoS) 四、可维护性1. 日志记录2. 代码注释3. 单…

idea中git的简单使用

提交,推送直接合并 合到哪个分支就到先切到哪个分支

Kubernetes 中 BGP 与二层网络的较量:究竟孰轻孰重?

如果你曾搭建过Kubernetes集群,就会知道网络配置是一个很容易让人深陷其中的领域。在负载均衡器、服务通告和IP管理之间,你要同时应对许多变动的因素。对于许多配置而言,使用二层(L2)网络就完全能满足需求。但边界网关协议(BGP)—— 支撑互联网运行的技术 —— 也逐渐出…

LSSVM最小二乘支持向量机多变量多步光伏功率预测(Matlab)

代码下载:LSSVM最小二乘支持向量机多变量多步光伏功率预测(Matlab) LSSVM最小二乘支持向量机多变量多步光伏功率预测 一、引言 1.1、研究背景与意义 随着全球能源危机和环境问题的日益严重,可再生能源的开发利用成为了世界各国…

docker容器运行时忘了加自动重启命令了,之后如何添加自动重启命令,使其随开机自动重启

要让已有的Docker容器在系统重启后自动启动,可以通过以下步骤设置其重启策略: 步骤 1:查找容器名称或ID docker ps -a找到目标容器的ID或名称。 步骤 2:更新容器的重启策略 使用 docker update 命令直接修改容器的重启策略&am…