自动驾驶-导航定位

目录

1.简介

2.IMU 定位原理

2.1 加速度计

2.2 磁力计

2.3 陀螺仪

3.GPS 定位原理

3.1 直接GPS定位

3.2 差分GPS定位

4.轮速差速航迹推算

4.1 差速器

4.2 航迹推算

5.四元数与姿态解算

5.1 欧拉角

5.2 四元数

6.多重定位融合

---

1.简介

自动驾驶中，一般用到的导航定位技术包括，惯性导航以及GPS定位导航，惯性导航一般用到IMU，此外为了进一步确保定位的可信度，还会采集轮速，进行航迹推演，最终实现多重定位的融合。

2.IMU 定位原理

IMU为惯性测量单元，一般其内部集成了 加速度计+磁力计+陀螺仪，下面分别简要介绍各个传感器的原理。

2.1 加速度计

如图所示，原理非常简单，就是牛顿方程，F = ma;

那么分解到三个坐标系为：Fx = m ax    Fy = m ay  Fz = m ax.

2.2 磁力计

一般采用北方向为y轴，因此磁力计可以测量出当前的航向相对y轴的夹角。

2.3 陀螺仪

陀螺仪的原理，即为采用科里奥利力，首先形象说明一下场景，一物体，在转动的圆盘上（圆盘无摩擦），假如我们站在圆盘外面看，则 物体从A到B点运动。

好，现在我们换个角度，假设这个时候，你站在圆盘上，从A直视B点，这个时候，你会发现屋里沿着曲线，从A到B'运动。

所以科里奥利力是由于你选择了一个旋转坐标系，引入的一个假想力。

下面我们进一步推导科里奥利力

首先圆盘以w角速度旋转，物体相对地面的速度为v，下面我们转变思路，换成圆盘当我们的坐标系。

那么相当于物体相对圆盘向相反方向转动。

此时物体相对圆盘的速度为 Vref  = v + w r （矢量和）  对其求导可进一步推导

   

-->

-->

-->

-->

-->

-->

F’是转动系看物体受到的力，F是静止系看物体受到的力，F1就是离心力，F2就是科里奥利力。

进而得到：

F2 = -2m (w * v)。

因此通过陀螺仪可以测量得到三个轴上的角速度，通过积分得到三个轴上的角度。

3.GPS 定位原理

3.1 直接GPS定位

如图所示，三颗定位卫星就可以获取一个测量物体的位置，具体公式为：

进而可以求出车辆当前的定位坐标(x,y,z)

3.2 差分GPS定位

差分GPS，为了提供测量精度，有一个附近的基准基站，去和车辆测量相同卫星位置，进而把对应的测量误差传给车辆，以提供测量精度。

4.轮速差速航迹推算

4.1 差速器

如图我们可以看到，车辆都配备差速器，在进行车辆转弯时，转弯的内轮和外轮的转速是不一样的，根据内外轮车速不一致，进而可以进一步推算出当前的车辆状态。

4.2 航迹推算

从图中可以清晰看到几何关系 θ = θ1 = θ2

角速度为：

车速为平均车速

由此可得转弯半径 

因此通过轮速差速，可以获得当前的车速-当前的转弯半径-当前的角速度，进而获取整个车行驶过程的航迹。

5.四元数与姿态解算

5.1 欧拉角

一般IMU出来的数据为四元数（四元数的好处在于其计算更加方便），那么通过四元数，如何解算出当前的姿态呢？

首先说明欧拉角

pitch，俯仰角，对应绕Z轴旋转角

Yaw，偏航角，对应绕Y轴旋转角

Roll，翻滚角，对应绕X轴旋转角

下一步进一步说明在三维空间中的旋转，即分别绕x轴，y轴，z轴旋转，得到旋转矩阵。

5.2 四元数

所谓的四元数，就是找到一根旋转轴，只通过绕这根轴旋转一次就可以得到目标方向。

所即为，空间中的任意旋转，可以通过选择一个三维坐标系，绕这个三维坐标系的某个轴旋转一个角度得到，记为：

[,x,y,z] = [q0,q1,q2,q3]

举例来说，单位向量[x,y,z]旋转得到的四元数为：

[cos/2,x*sin/2,y*sin/2,z*sin/2]

进而求旋转矩阵可得

6.多重定位融合