CN106979780B - 一种无人车实时姿态测量方法 - Google Patents

Abstract

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

Description

一种无人车实时姿态测量方法

技术领域

本发明涉及无人车技术，特别是一种面向无人车的实时姿态测量方法。

背景技术

自20世纪60年代移动机器人诞生以来，研究人员一直梦想研究无人智能交通工具，作为智能交通系统的重要组成部分，无人车排除了人为不确定因素的影响，不仅可以提高驾驶安全性，而且可以解决交通拥堵，提高能源利用率，百度曾宣布开发复杂人工智能无人车，该产品是具备环境感知、规划和自平衡控制等复杂人工智能的无人自行车，主要集合了百度在人工智能、深度学习、大数据和云计算技术的成就，然而对技术细节没有任何披露。

陀螺是最常用的姿态传感器，但是高性能陀螺的体积大，价格高，限制了陀螺在某些场合的应用，在汽车工业需求推动系下，微机械陀螺获得了很大的发展，体积小，成本低，易于批量生产，扩展了微机械陀螺的应用，然而它的精度较低，并且陀螺漂移会在短时间内引起较大的姿态误差，因此，必须与其它姿态传感器，例如GPS、倾斜计、磁强计等组合使用才能给出长期稳定可靠的无人车姿态信息。

四元数估计算法虽然利用了在计算过程中利用了四元数进行计算，可以避免欧拉角方法带来的奇异性问题，同时在计算过程中也不需要进行三角计算，但是在第一步结算过程中，通过加速度计和磁强计的输入信息首先得到的是欧拉角，在进入卡尔曼滤波之前要转换为四元数，在这个转换过程中就会出现一些奇异性的问题，表现在仿真结果中就是四元数在过零点的时候会出现正负偏差的问题，四元数估计算法可以获得单次观测最优解，却无法利用系统动态信息和历史观测信息，不能通过动态滤波改善测量精度，也不能同时获得轨迹姿态速率等其他参数。

发明内容

本发明的目的在于提供一种无人车实时姿态测量方法，包括如下步骤：

(1)选定卡尔曼滤波器，并且由微型惯量测量系统采集获得原始数据；

(2)定义测量矢量，并且根据公式得到计算测量矢量；

(3)计算获得模型误差矢量；

(4)通过最小平方误差标准函数，估计姿态四元数；

(5)利用回归矩阵，将四元数进行旋转，计算与体坐标系中测量的加速度和地磁场相关的最优四元数，并把该最优四元数作为卡尔曼滤波器的测量值；

(6)设定约束条件，获得线性矩阵的降阶矩阵，从而获得无人车的姿态参量。

优选的，所述步骤(1)中的卡尔曼滤波器利用来自GSP、磁强计、加速度计和倾斜计的数据补偿陀螺偏差引起的姿态误差。

优选的，所述步骤(1)中的微型惯量测量系统通过内部微处理器的处理，输出姿态和航向信息。

优选的，所述内部微处理器功耗低，通过实时卡尔曼滤波提供准确的位置和速度信息，同时提供无漂移的三维加速度、三维转速度、三维地磁场、静态压力信息。

优选的，所述步骤(2)三轴正交加速度计的单位矢量为h，三轴正交磁力计的单位式量为b，定义测量矢量：

由和得到计算矢量：

其中E_m＝[0,0,0,1]为地球坐标系中加速度四元数形式的单位矢量；E_n＝[0,n₁,n₂,n₃]为当地地磁场的单位矢量。

优选的，所述步骤(3)的模型误差矢量为：

优选的，所述步骤(4)中估计姿态四元数为：所述标准函数通过高斯-牛顿迭代减小。

优选的，所述步骤(5)中回归矩阵为：S＝[X^TX]^-1

优选的，所述步骤(6)的约束条件为：式中：

并且，当旋转矢量接近零矢量的时候，q_r作为四元数旋转的增量接近单位四元数。

根据下文结合附图对本发明具体实施例的详细描述，本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。

附图说明

后文将参照附图以示例性而非限制性的方式详细描述本发明的一些具体实施例。附图中相同的附图标记标示了相同或类似的部件或部分。本领域技术人员应该理解，这些附图未必是按比例绘制的。本发明的目标及特征考虑到如下结合附图的描述将更加明显，附图中：

图1为根据本发明实施例的方法步骤流程图。

具体实施方式

高斯-牛顿方法将所有的旋转通过四元数表达，而不是常用的欧拉角，应用四元数可以减少所需的矢量计算，减少三角函数计算时的计算量，同时可以避免用欧拉角表示姿态时的奇异性。

姿态就是联系动坐标系和参考坐标系角位置的参数，欧拉角和四元数是常用的表达转动的两种方式。四元数因为可以避免欧拉角的奇异问题而应用的更加广泛。四元数姿态表达式是描述姿态的最小非奇异参数集，是一种四参数的表达式，表示从一个坐标系向另一个坐标系变换，可以围绕某一单位矢量的单次旋转实现。选择参考坐标系为NED坐标系，定义坐标系如下：

N北方X

E东方Y

D垂直Z

旋转(-π,π]

θ提升[-π/2,π/2]

ψ偏移(-π,π]

体坐标系和地球坐标系的旋转矩阵(欧拉角)，四元数与欧拉角的对应关系都是本领域公知的技术。

结合附图1如下详细说明一种无人车实时姿态测量方法，包括如下步骤：

(1)选定卡尔曼滤波器，并且由微型惯量测量系统采集获得原始数据，卡尔曼滤波器利用来自GSP、磁强计、加速度计和倾斜计的数据补偿陀螺偏差引起的姿态误差，微型惯量测量系统通过内部微处理器的处理，输出姿态和航向信息，内部微处理器功耗低，通过实时卡尔曼滤波提供准确的位置和速度信息，同时提供无漂移的三维加速度、三维转速度、三维地磁场、静态压力信息。

(2)定义测量矢量，并且根据公式得到计算测量矢量，将三轴正交加速度计的单位矢量为h，三轴正交磁力计的单位式量为b，定义测量矢量：由和得到计算矢量：其中E_m＝[0,0,0,1]为地球坐标系中加速度四元数形式的单位矢量；E_n＝[0,n₁,n₂,n₃]为当地地磁场的单位矢量；

(3)计算获得模型误差矢量，模型误差矢量为：

(4)通过最小平方误差标准函数，估计姿态四元数为：所述标准函数通过高斯-牛顿迭代减小；

(5)利用回归矩阵S＝[X^TX]^-1(5)，将四元数进行旋转，计算与体坐标系中测量的加速度和地磁场相关的最优四元数，并把该最优四元数作为卡尔曼滤波器的测量值；

(6)设定约束条件为：式中： 并且，由上述3*3的公式可以得到更加简单的方程和更加深入地理解，其中：

当旋转矢量接近零矢量的时候，q_r作为四元数旋转的增量接近单位四元数，因此

获得线性矩阵的降阶矩阵，从而获得无人车的姿态参量。

将该检测方法进行系统仿真与分析，初始时刻，传感器静止，此时可以测量当地的地磁参量，并且获得陀螺仪，加速度计，磁力计等主要传感器的角度表现情况，包括全尺度，现行度，偏置稳定性，噪声密度和带宽。在Matlab下进行实时仿真，该高斯-牛顿迭代方法能够完全追踪运动轨迹，大大降低四元数估级标准差和陀螺漂移估计误差标准差。

虽然本发明已经参考特定的说明性实施例进行了描述，但是不会受到这些实施例的限定而仅仅受到附加权利要求的限定。本领域技术人员应当理解可以在不偏离本发明的保护范围和精神的情况下对本发明的实施例能够进行改动和修改。

Claims (1)

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)设定约束条件，获得线性矩阵的降阶矩阵，从而获得无人车的姿态参量。