[发明专利]载体的姿态数据的确定方法和装置

权利要求书

1.一种载体的姿态数据的确定方法，其特征在于，包括：

获取载体的惯性传感器输出的传感器数据；

使用在李群上的扩展卡尔曼滤波器对所述传感器数据进行融合，得到所述载体的姿态数据，其中，所述扩展卡尔曼滤波器的状态变量和观测变量都是由旋转变换矩阵表示；

其中，所述获取载体的惯性传感器输出的传感器数据包括：获取所述载体的惯性传感器第k次输出的第k次传感器数据，其中，k为大于1的自然数；

所述传感器数据包括陀螺仪输出的角速度数据，扩展卡尔曼滤波器的加速度计输出的加速度数据，和/或扩展卡尔曼滤波器的磁场计输出的磁场强度数据；其中，所述使用在李群上的扩展卡尔曼滤波器对所述传感器数据进行融合，得到所述载体的姿态数据，包括：根据所述载体的第k-1次姿态数据、所述陀螺仪第k次输出的角速度数据和时间差获得所述第k次姿态数据的预估数据，其中，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔，所述第k-1次姿态数据是由所述扩展卡尔曼滤波器对所述惯性传感器输出的第k-1次传感器数据进行融合得到的所述载体的姿态数据；根据所述陀螺仪第k次输出的角速度数据、所述时间差、所述扩展卡尔曼滤波器的预测模型中第k-1次的状态变量X_k-1的协方差、和所述扩展卡尔曼滤波器的预测模型的第k-1次的预设过程噪声协方差获得第一中间变量；根据所述加速度计第k次输出的加速度数据和/或所述磁场计第k次输出的磁场强度数据对所述预估数据进行校正，得到所述载体的第k次姿态数据；

根据所述加速度计第k次输出的加速度数据对所述预估数据进行校正，得到所述载体的第k次姿态数据包括：根据所述第一中间变量、所述第k次姿态数据的预估数据、世界坐标系下重力加速度向量和预设加速度计测量噪声协方差获得所述第k次姿态数据；根据所述第一中间变量、所述第k次姿态数据的预估数据、所述世界坐标系下重力加速度向量和所述预设加速度计测量噪声协方差获得所述扩展卡尔曼滤波器的预测模型中第k次的状态变量的协方差；

所述旋转变换矩阵属于李群；所述扩展卡尔曼滤波器的状态变量为传感器坐标系到世界坐标系的旋转变换矩阵；所述扩展卡尔曼滤波器的观测变量包括所述扩展卡尔曼滤波器的加速度计观测变量和所述扩展卡尔曼滤波器的磁场计观测变量。

2.根据权利要求1所述方法，其特征在于，所述扩展卡尔曼滤波器的加速度计观测变量用于表示所述传感器坐标系下重力加速度向量到所述世界坐标系下重力加速度向量的旋转；所述扩展卡尔曼滤波器的磁场计观测变量用于表示所述传感器坐标系下地磁指北向量到所述世界坐标系下地磁指北向量的旋转。

3.根据权利要求1所述方法，其特征在于，所述惯性传感器包括陀螺仪，其中，在所述扩展卡尔曼滤波器的预测模型为匀角速度模型的情况下，对于所述陀螺仪第k次输出的角速度数据，所述预测模型用于：根据所述扩展卡尔曼滤波器第k-1次的状态变量、所述陀螺仪第k次输出的角速度数据和时间差获得该扩展卡尔曼滤波器第k次的状态变量，所述时间差为所述陀螺仪第k次输出角速度数据与所述陀螺仪第k-1次输出角速度数据之间的时间间隔。

4.根据权利要求1所述方法，其特征在于，所述惯性传感器包括加速度计，所述扩展卡尔曼滤波器的观测变量包括所述扩展卡尔曼滤波器的加速度计观测变量，其中，在所述加速度计观测变量用于表示传感器坐标系下重力加速度向量到世界坐标系下重力加速度向量的旋转的情况下，所述传感器坐标系下重力加速度向量包括所述加速度计第k次输出的加速度数据，所述加速度计观测变量与所述扩展卡尔曼滤波器第k次的状态变量通过加速度计测量噪声建立关联关系，所述加速度计测量噪声为均值为0，方差为第一方差值的高斯白噪声。

5.根据权利要求1所述方法，其特征在于，所述惯性传感器包括磁场计，所述扩展卡尔曼滤波器的观测变量包括所述扩展卡尔曼滤波器的磁场计观测变量，其中，在所述扩展卡尔曼滤波器的磁场计观测变量用于表示为传感器坐标系下地磁指北向量到世界坐标系下地磁指北向量的旋转的情况下，所述传感器坐标系下地磁指北向量包括所述磁场计第k次输出的磁场强度数据，所述磁场计观测变量与所述扩展卡尔曼滤波器第k次的状态变量通过磁场计测量噪声建立关联关系，所述磁场计测量噪声为均值为0，方差为第二方差值的高斯白噪声。