[发明专利]一种无人车实时姿态测量方法

摘要

本发明提供了一种无人车实时姿态测量方法，包括如下步骤：(1)选定卡尔曼滤波器，并且由微型惯量测量系统采集获得原始数据；(2)定义测量矢量，并且根据公式得到计算测量矢量；(3)计算获得模型误差矢量；(4)通过最小平方误差标准函数，估计姿态四元数；(5)利用回归矩阵，将四元数进行旋转，计算与体坐标系中测量的加速度和地磁场相关的最优四元数，并把该最优四元数作为卡尔曼滤波器的测量值；(6)设定约束条件，获得线性矩阵的降阶矩阵，从而获得无人车的姿态参量。

主权项

1.一种无人车实时姿态测量方法，其特征在于包括如下步骤：(1)选定卡尔曼滤波器，并且由微型惯量测量系统采集获得原始数据，所述卡尔曼滤波器利用来自GPS、磁强计、加速度计和倾斜计的数据补偿陀螺偏差引起的姿态误差，所述微型惯量测量系统通过内部微处理器的处理，输出姿态和航向信息，所述内部微处理器功耗低，通过实时卡尔曼滤波提供准确的位置和速度信息，同时提供无漂移的三维加速度、三维转速度、三维地磁场、静态压力信息；(2)定义测量矢量，并且根据公式得到计算测量矢量，具体包括：三轴正交加速度计的单位矢量为h，三轴正交磁力计的单位式量为b，定义测量矢量：由和得到计算矢量：其中E_m＝[0,0,0,1]为地球坐标系中加速度四元数形式的单位矢量；E_n＝[0,n₁,n₂,n₃]为当地地磁场的单位矢量；(3)计算获得模型误差矢量，模型误差矢量为：(4)通过最小平方误差标准函数，估计姿态四元数为：所述最小平方误差标准函数通过高斯‑牛顿迭代减小；(5)利用回归矩阵，将四元数进行旋转，计算与体坐标系中测量的加速度和地磁场相关的最优四元数，并把该最优四元数作为卡尔曼滤波器的测量值，所述回归矩阵为：S＝[X^TX]^‑1；(6)设定约束条件，获得线性矩阵的降阶矩阵，从而获得无人车的姿态参量。