使用MPU6050计算方向盘角度

我给你们作了榜样、叫你们照着我向你们所作的去作。

——【约翰福音13:15】

1.前言

前段时间接到一个项目需求：使用现有的陀螺仪MPU6050实现计算当前车辆的方向盘角度。

2.需求分析

MPU6050可获取三轴角速度和三轴加速度，并通过算法可以获得横滚角、俯仰角、航向角。

方向盘角度旋转会同步引起车轮的角度变化，方向盘旋转角度与车轮角度是一个已知量，可从实际车辆设计中获得，本次需求的车辆中方向盘旋转角度与前车轮的角度关系为：

左转时，方向盘角度范围 0~610°，车轮角度0~28°；

右转时，方向盘角度范围0~720°，车轮角度0~37°。

车辆在行驶时，若方向盘有角度，则一定在某个圆上绕圈，通过查找资料发现，其实每一个轮胎都在各自的圆上绕圈，并且最关键的是 这些圆的圆心在同一个点上，如下图(网上找的)：

 按照上图，我们尝试分析，车辆在行驶过程中左转的转弯角度；左前轮和左后轮并圆心组成一个直角三角形，并且很容易看出，转弯角度(左前轮与前进方向的夹角)就是圆心角。

3.相关公式

根据上面的分析，我们可以从角度这个关键点去列出所有相关公式：

1. A =  610°/28°     //!<方向盘角度与车轮角度的关系，左转比例因子A
2. S = V * t             //!<圆弧长S = 车速V * 时间t
3. L = 2π * r           //!<圆周长L = 2π * 半径r
4. D = |d|                //!<角速度D = 角速度初始值d的绝对值
5. α = D * t             //!<转弯角度α = 角速度D *时间t
6. α/360° = S/L      //!<转弯角度α / 圆周角度360° = 圆弧长S / 圆周长L
7. B/r = sinθ          //!<轴距B / 半径r = sin(车轮角度θ)
8. P = A *θ             //!<方向盘旋转角度P = 比例因子A * 车轮角度θ

所以 左转时，P = A * arcsin(2π*|d|*B/(V*360°)）

同样右转时也可以列出公式，所以要求方向盘角度需要的输入参数为：

比例因子A、角速度D、轴距B、车速V

其中比例因子A为实际车辆的固定值，角速度D可以从MPU6050中获取，轴距B就是车前轮圆心和车后轮圆心的距离，也为实际车辆的固定值，车速V可以实时从整车CAN中获取。

最后使用算法实现以上公式，便可求出当前方向盘角度。

4.结论

将以上算法应用到实际车辆后，实际测试结果均可基本满足测量需求。不过也存在一个缺点，就是当车速为零时，无法计算当前方向盘角度。另外现在新生产的车辆一般都会安装带有角度测量的方向盘，所以以上解决方案可用在后装车辆上。

本篇博文仅作记录。