CN107339987B - 一种基于函数迭代积分的刚体姿态解算方法 - Google Patents

Abstract

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

Description

一种基于函数迭代积分的刚体姿态解算方法

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

优选的，步骤1包括：

对于t_k时刻的N个角速度测量值

角速度函数采用不超过N-1阶的多项式进行拟合；或者，角速度函数采用切比雪夫多项式为基进行拟合。

优选的，步骤1包括：

对于t_k时刻的N个角增量值

角速度函数采用不超过N-1阶的多项式进行拟合；或者，角速度函数采用切比雪夫多项式为基进行拟合。

优选的，步骤2包括：

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

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

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

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明提供的基于函数迭代积分的刚体姿态解算方法的流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

结合如图1所示，本发明提供的基于函数迭代积分的刚体姿态解算方法具体包括：

1)根据时间区间[0t]上的陀螺测量值，拟合出角速度的多项式函数；

陀螺的测量值一般有两种形式：角速度或角速度增量。下面分别讨论：

对于t_k时刻的N个角速度测量值

角速度函数可以采用n阶(不超过N-1)的多项式进行拟合，即

其中，系数c_i通过求解如下方程来确定

即

当n<N-1时,

表示最小二乘意义下的矩阵逆,

下同。其中，T为均匀采样时间间隔。

类似地,对于t_k时刻的N个角增量值

角速度函数可以采用n阶(不超过N-1)的多项式进行拟合。角增量值与角速度的关系为

即

当拟合多项式的阶数较大时，

或

可能会面临数值计算的稳定性问题。为了提高计算的鲁棒性，角速度的多项式函数(1)也可以使用切比雪夫(Chebyshev)多项式为基进行拟合。

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

罗德里格向量g的迭代积分方程如下

初始值g₀＝0。将角速度的多项式拟合函数(1)代入上式，迭代计算，直到满足收敛条件或达到事先设定的最大迭代次数。

3)根据罗德里格向量的迭代结果，给出以时间区间开始时刻为参考的姿态四元数。

上述时间区间长度t与子样个数N和均匀采样时间间隔T的关系为t＝N×T。对于更大时间区间上的姿态解算，可将其划分为若干个小时间区间，依次计算实现。

原则上，如果能接受一定程度的精度损失，本发明提出的基于函数迭代积分的姿态解算方法也适用于其他三维姿态参数，如旋转向量，此时需要对步骤2)和步骤3)做相应的调整如下：

2)利用角速度的多项式拟合函数以及旋转向量积分方程，迭代计算旋转向量；

旋转向量g的迭代积分方程如下

初始值g₀＝0。将角速度的多项式拟合函数(1)代入上式，迭代计算，直到满足收敛条件或达到事先设定的最大迭代次数。

3)根据旋转向量的迭代结果，给出以时间区间开始时刻为参考的姿态四元数。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (2)

1.一种基于函数迭代积分的刚体姿态解算方法，其特征在于，包括如下步骤：

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

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

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

步骤1包括：

所述陀螺测量值包括角速度测量值或角速度增量值；

对于t_k时刻的N个角速度测量值

角速度函数采用不超过N-1阶的多项式进行拟合得到角速度多项式函数，角速度多项式函数采用切比雪夫多项式为基进行拟合；或者，

对于t_k时刻的N个角增量值

角速度函数采用不超过N-1阶的多项式进行拟合得到角速度多项式函数，角速度多项式函数采用切比雪夫多项式为基进行拟合；

步骤2包括：

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

2.根据权利要求1所述的基于函数迭代积分的刚体姿态解算方法，其特征在于，所述陀螺测量值包括角速度或者角速度增量。

Images