检测网站死链,网站推广页面设计,手机之家官网首页,网站开发 架构Title: 利用 IMU 估计人体关节轴向和位置 —— “Joint axis and position estimation from inertial measurement data by exploiting kinematic constraints” —— 论文推导 文章目录 I. 论文回顾II. 铰接关节的约束1. 铰接关节约束的原理2. 铰接关节约束的梯度3. 铰接关节约…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∥20(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∥20。识别关节轴的方法当传感器方向未知时在大量的陀螺仪测量数据中搜索满足 ∥ 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∥20 的向量 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 i1,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 ∥2a12a22a32 。 对向量叉乘的模长函数求导 令 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} ugi(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∥2u12u22u32 。 根据复合函数求导法则我们先对 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)u12u22u32 关于 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 k1,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} ugi(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} u1gi2(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∂ugi(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 i1,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)∥20 ∀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))k1N 来估计 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 i1,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⋅Δi 1doid(Δ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))k1N其中 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 xx−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 本文作者wzfrobotics_notes
