[发明专利]一种基于函数迭代积分的刚体姿态解算方法

说明书

本发明提供了一种基于函数迭代积分的刚体姿态解算方法，包括如下步骤：步骤1、根据时间区间上的陀螺测量值，拟合出角速度的多项式函数；步骤2、利用角速度的多项式拟合函数以及罗德里格向量积分方程，迭代计算罗德里格向量；步骤3、根据罗德里格向量的迭代结果，以四元数的形式给出时间区间上的姿态变化。

技术领域

本发明涉及惯性导航、机器人等技术领域，具体而言，涉及一种基于函数迭代积分的刚体姿态解算方法。

背景技术

三维空间刚体运动的计算或估计是物理、机器人、导航制导、机械、计算机视觉等众多领域中的核心问题。与速度、位置等平移运动不同，姿态不能被直接测量，只能通过角速度积分或向量匹配等间接方式获得。角速度积分方式的姿态解算是完全自主的，不需要外部信息辅助，因此在很多(如卫星导航系统不能发挥作用的)应用场合备受青睐。然而，陀螺输出的角速度难免包含误差，造成积分后的姿态误差会无限增大。事实上，大家普遍认为：即使角速度是精确的，由于转动不可交换性的存在，我们仍然不能精确计算姿态。转动交换性是指转动的不同顺序将导致不同的姿态。因此，在姿态计算时必须采用近似的方法。

惯性导航领域的主流姿态解算方法通常采用四元数描述当前姿态、旋转向量描述姿态增量。在一般性的姿态运动下，旋转向量的微分与角速度的关系比较复杂，必须进行相当地简化才能应用近似方法来处理。主流姿态解算方法通常使用连续采集的多个角速度或角增量测量(又称‘子样’)来近似计算旋转向量，因此只能在最后一个子样到来时才能开始姿态解算。如果我们只关心姿态的解算，这不是个大问题；但是，当后续计算步骤需要将姿态作为输入时，情况就大不一样了，比如在惯性导航系统中，速度和位置的计算需要用到姿态的解算结果。另外，所有的主流姿态解算方法几乎都是以某种特殊运动(如圆锥运动)为优化指标设计的，在其他运动情形下不具有最优性。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于函数迭代积分的刚体姿态解算方法。本方法基于函数迭代积分的技术，利用罗德里格(Rodrigues)向量，实现从角速度解析重建姿态。罗德里格向量的微分方程形式比旋转向量简单，且允许采用多项式函数的迭代积分实现精确的姿态重构。

根据本发明提供的基于函数迭代积分的刚体姿态解算方法，包括如下步骤：

步骤1、根据时间区间上的陀螺测量值，拟合出角速度的多项式函数；

步骤2、利用角速度的多项式拟合函数以及罗德里格向量积分方程，迭代计算罗德里格向量；

步骤3、根据罗德里格向量的迭代结果，以四元数的形式给出时间区间上的姿态变化。

优选的，所述陀螺测量值包括角速度或者角速度增量。

优选的，步骤1包括：

对于t_k时刻的N个角速度测量值角速度函数采用不超过N-1阶的多项式进行拟合；或者，角速度函数采用切比雪夫多项式为基进行拟合。

优选的，步骤1包括：

对于t_k时刻的N个角增量值角速度函数采用不超过N-1阶的多项式进行拟合；或者，角速度函数采用切比雪夫多项式为基进行拟合。

优选的，步骤2包括：

将角速度的多项式拟合函数代入罗德里格向量积分方程进行迭代计算，直到满足收敛条件或达到事先设定的最大迭代次数。

与现有技术相比，本发明具有如下的有益效果：

1)提出了一种新的姿态解算方法，并能够拓展应用于一般的刚体运动；2)如果角速度是精确的，新的姿态解算方法将严格收敛于精确姿态；3)从角速度重构得到的罗德里格向量具有解析多项式形式，因此可以给出相应时间区间内的所有姿态结果；4)新姿态解算方法的设计不依赖于任何特殊的运动形式假设。

附图说明