CN106979779A

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

Info

Publication number: CN106979779A
Application number: CN201710361133.5A
Authority: CN
Inventors: 不公告发明人
Original assignee: Shenzhen Jing Zhou Technology Co Ltd
Current assignee: Shenzhen Jing Zhou Technology Co Ltd
Priority date: 2017-05-22
Filing date: 2017-05-22
Publication date: 2017-07-25
Also published as: WO2018214226A1

Abstract

本发明提供了一种无人车实时姿态测量方法，包括如下步骤：(1)选定卡尔曼滤波器，并且由微型惯量测量系统采集获得原始数据；(2)选择参考坐标系为NED坐标系，并忽略磁北极和真北之间的差别；(3)当载体在参考坐标系中静止，并且体坐标系和参考坐标系重合时，加速度计和磁强计测量重力加速度和地磁场在NED坐标系中的垂直分量；(4)当载体处于任意姿态时，假定加速度计和磁强计的输出，并且标示参考坐标系和体坐标系的关系；(5)综合以上信息获得联系方程组，通过姿态角与四元数的方程组，计算得到四元数。该方法可准确获得无人车的实时姿态。

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

(2)选择参考坐标系为NED坐标系，并忽略磁北极和真北之间的差别；

(3)当载体在参考坐标系中静止，并且体坐标系和参考坐标系重合时，加速度计和磁强计测量重力加速度和地磁场在NED坐标系中的垂直分量；

(4)当载体处于任意姿态时，假定加速度计和磁强计的输出，并且标示参考坐标系和体坐标系的关系；

(5)综合以上信息获得联系方程组，通过姿态角与四元数的方程组，计算得到四元数。

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

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

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

优选的，所述步骤(3)中加速度计和磁强计的测量输出为：

g_r＝[0,0,g]，b_r＝[b_n,0,b_d]，式中，g是重力加速度，b_n是地磁场在NED坐标系中的北向分量，b_d是地磁场在NED坐标系中的垂直分量。

优选的，所述步骤(4)中加速度计和磁强计的测量输出为：

g_b＝[g_x,g_y,g_z]，b_b＝[b_x,b_y,b_z]。

优选的，所述步骤(4)标示参考坐标系和体坐标系的关系如下：

g_b＝T·g_r，b_b＝T·b_r

优选的，所述步骤(5)获得的方程组为：

优选的，所述步骤(5)计算获得的四元数分别为：

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

附图说明

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

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

具体实施方式

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

N北方X

E东方Y

D垂直Z

旋转(-π,π]

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

ψ偏移(-π,π]

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

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

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

(3)当载体在参考坐标系中静止，并且体坐标系和参考坐标系重合时，加速度计和磁强计测量重力加速度和地磁场在NED坐标系中的垂直分量，加速度计和磁强计的测量输出为：g_r＝[0,0,g](1)，b_r＝[b_n,0,b_d](2)，

式中，g是重力加速度，b_n是地磁场在NED坐标系中的北向分量，b_d是地磁场在NED坐标系中的垂直分量；

(4)当载体处于任意姿态时，假定加速度计和磁强计的输出，并且标示参考坐标系和体坐标系的关系，加速度计和磁强计的测量输出为：

g_b＝[g_x,g_y,g_z](3)，b_b＝[b_x,b_y,b_z] (4)，

标示参考坐标系和体坐标系的关系如下：

g_b＝T·g_r(5)，b_b＝T·b_r (6)；

(5)综合以上信息获得联系方程组，通过姿态角与四元数的方程组，计算得到四元数，获得的方程组为：

计算获得的四元数分别为：

将该检测方法进行系统仿真与分析，初始时刻，传感器静止，此时可以测量当地的地磁参量，并且获得陀螺仪，加速度计，磁力计等主要传感器的角度表现情况，包括全尺度，现行度，偏置稳定性，噪声密度和带宽。在Matlab下进行实时仿真，该方法能够追踪运动轨迹，四元数在零点附近会出现一些正负偏差，产生奇异性问题，但不影响目标的最终姿态确定。

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

Claims

1.一种无人车实时姿态测量方法，其特征在于包括如下步骤：

2.根据权利要求1的一种无人车实时姿态测量方法，所述步骤(1)中的卡尔曼滤波器利用来自GSP、磁强计、加速度计和倾斜计的数据补偿陀螺偏差引起的姿态误差。

3.根据权利要求1的一种无人车实时姿态测量方法，所述步骤(1)中的微型惯量测量系统通过内部微处理器的处理，输出姿态和航向信息。

4.根据权利要求3的一种无人车实时姿态测量方法，所述内部微处理器功耗低，通过实时卡尔曼滤波提供准确的位置和速度信息，同时提供无漂移的三维加速度、三维转速度、三维地磁场、静态压力信息。

5.根据权利要求1的一种无人车实时姿态测量方法，所述步骤(3)中加速度计和磁强计的测量输出为：

6.根据权利要求1的一种无人车实时姿态测量方法，所述步骤(4)中加速度计和磁强计的测量输出为：

g_b＝[g_x,g_y,g_z]，b_b＝[b_x,b_y,b_z]。

7.根据权利要求1的一种无人车实时姿态测量方法，所述步骤(4)标示参考坐标系和体坐标系的关系如下：

g_b＝T·g_r，b_b＝T·b_r。

8.根据权利要求1的一种无人车实时姿态测量方法，所述步骤(5)获得的方程组为：

\{\begin{matrix} g_{x} = 2 g (q_{1} q_{3} - q_{0} q_{2}) \\ g_{y} = 2 g (q_{2} q_{3} + q_{0} q_{1}) \\ g_{z} = g (q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}) \\ 2 b_{x} (q_{1} q_{2} + q_{0} q_{3}) + b_{y} (q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2}) + 2 b_{x} (q_{2} q_{3} - q_{0} q_{1}) = 0 \end{matrix} .

9.根据权利要求1的一种无人车实时姿态测量方法，所述步骤(5)计算获得的四元数分别为：