1.一种无人车实时姿态测量方法,其特征在于包括如下步骤:(1)选定卡尔曼滤波器,并且由微型惯量测量系统采集获得原始数据;
(2)定义测量矢量,并且根据公式得到计算测量矢量;
(3)计算获得模型误差矢量;
(4)通过最小平方误差标准函数,估计姿态四元数;
(5)利用回归矩阵,将四元数进行旋转,计算与体坐标系中测量的加速度和地磁场相关的最优四元数,并把该最优四元数作为卡尔曼滤波器的测量值;
(6)设定约束条件,获得线性矩阵的降阶矩阵,从而获得无人车的姿态参量。
2.根据权利要求1的一种无人车实时姿态测量方法,所述步骤(1)中的卡尔曼滤波器利用来自GSP、磁强计、加速度计和倾斜计的数据补偿陀螺偏差引起的姿态误差。
3.根据权利要求1的一种无人车实时姿态测量方法,所述步骤(1)中的微型惯量测量系统通过内部微处理器的处理,输出姿态和航向信息。
4.根据权利要求3的一种无人车实时姿态测量方法,所述内部微处理器功耗低,通过实时卡尔曼滤波提供准确的位置和速度信息,同时提供无漂移的三维加速度、三维转速度、三维地磁场、静态压力信息。
5.根据权利要求1的一种无人车实时姿态测量方法,步骤(2)三轴正交加速度计的单位矢量为h,三轴正交磁力计的单位式量为b,定义测量矢量:由 和 得到计算矢量:
其中Em=[0,0,0,1]为地球坐标系中加速度四元数形式的单位矢量;En=[0,n1,n2,n3]为当地地磁场的单位矢量。
6.根据权利要求1的一种无人车实时姿态测量方法,所述步骤(3)的模型误差矢量为:
7.根据权利要求1的一种无人车实时姿态测量方法,所述步骤(4)标示参考坐标系和体坐标系的关系如下:gb=T·gr,bb=T·br
8.根据权利要求1的一种无人车实时姿态测量方法,所述步骤(4)中估计姿态四元数为: 所述标准函数通过高斯-牛顿迭代减小。
9.根据权利要求1的一种无人车实时姿态测量方法,所述步骤(5)中回归矩阵为:S=[XTX]-1。
10.根据权利要求1的一种无人车实时姿态测量方法,所述步骤(6)的约束条件为:式中:
并且, 当旋转矢量接近零矢量的时候,qr作为四元数旋转的增量接近单位四元数。