自动驾驶
目录
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.多重定位融合
自动驾驶
目录
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.多重定位融合