[发明专利]使用软约束和惩罚函数初始化惯性传感器

摘要

一种用于惯性传感器的方法，其在无需传感器以静止或者在公知的定位或取向下开始的情况下估计开始取向和速度。初始化使用编码为软约束集合的运动模式，这些软约束预期在初始化时段期间近似成立。惩罚度量被限定以便测量计算的运动轨迹与软约束的偏差。用于惯性传感器的运动微分方程以初始条件作为变量进行求解；最小化惩罚度量的初始条件用作对于传感器的实际初始条件的估计。用于具体应用的软约束和惩罚度量基于该应用所期望的运动模式类型进行选择。说明性案例包括初始化期间具有相对较小的运动的应用以及初始化期间具有近似周期性运动的应用。

主权项

1.一种用于确定惯性传感器的初始取向Q₀和初始速度v₀的估计的方法，包括步骤：在包括感兴趣活动的捕获时间间隔[t_i，t_f]上收集来自惯性传感器的样本数据，所述样本数据包括加速度计数据s(t)和速率陀螺仪数据ω(t)；在所述捕获时间间隔内选择初始化间隔限定对于所述初始化间隔期间的所述惯性传感器的位置r(t)和速度v(t)的软约束集合{C_i}，其中所述软约束集合{C_i}的每个软约束C_i包括所述初始化间隔[t₀，t_n]期间所述位置r(t)的轨迹的组合集合以及所述初始化间隔[t₀，t_n]期间所述速度v(t)的轨迹，其中所述软约束表示所述初始化间隔期间所述感兴趣活动的典型位置和速度轨迹模式的近似；限定惩罚度量m_j(r(t)，v(t))以便量化计算的所述位置r(t)和所述速度v(t)的轨迹与所述软约束{C_i}的偏差；以及当所述初始取向和初始速度用作在整个所述初始化间隔上借助于耦合的微分方程将所述样本数据整合到取向、位置和速度中的初始条件时，计算所述初始化间隔开始处所述惯性传感器的初始取向Q₀＝Q(t₀)和初始速度v₀＝v(t₀)的估计以便最小化所述惩罚度量，这些耦合的微分方程设置：取向对取向的导数乘以线性算子，其将矢量映射到所述速率陀螺仪数据与所述矢量的矢量叉积；速度对取向的导数乘以所述加速度数据加上重力矢量；以及位置对速度的导数。