Nav2导航中的阿克曼计算

非汽车专业，有误之处烦请留言指正。

1 基本概念

前轮转向的阿克曼小车如图：

忽略左右轮的转角差，简化为自行车模型。如图：

可看到：

• 当前轮内转角为$\alpha$时，前轮运动的旋转中心在前轮中垂线上，而后轮不能转向，所以后轮运动旋转中心就在后轮轴线上。因此小车的运动旋转中心O是图中两条蓝色线的交点。
• 根据几何关系可以知道$\theta$ =$\alpha$
• 根据小车的前后轴距L，可以算出O点与内侧后轮中心距离$R=\frac{L}{tan(\alpha )}$
• 后面在做简化到点的时候，会用两后轮的中心C点（为什么我也不知道，原来我用的是矩形中心，但小车供应商给的驱动用的是C点）。C点与旋转中心O的距离是$R+\frac{W}{2}$，其中W是后轴两轮中心距（车底盘宽度）。

2 问题的提出

阿克曼车型在Nav2导航中需要用到的计算包括：

2.1 根据反馈信息估算当前位置，发布odom

• 需要根据上一次计算周期的坐标（x,y,yaw），结合小车当前的前进线速度（$v_x$）和前轮方向转角（$\alpha$），估算小车当前的位置。
• 上位机程序一般可以读到小车底层控制传上来的速度$v_x$(或根据车轮电机编码器信号可求得）和内转角$\alpha$(或根据转向电机编码器信号可求得）。
• 这时就把小车简化为一个质点（图中C点），它绕着O点在旋转，线速度就是小车前进速度（$v_x$），旋转角速度是线速度除以旋转半径$\omega =\frac{v_x}{R+\frac{W}{2}}=\frac{v_x}{\frac{L}{tan(\alpha )}+\frac{W}{2}}$。
• 计算方法需要用到数值积分，如下图所示，该点的在map坐标系下的初始坐标（即上一次计算周期的坐标：x，y，yaw）记为（$x_0,y_0,{\beta }_0$）,在细分到很微小的时间段$d_t$后，坐标变为
  $$x_1=x_0+d_x=x_0+v_x\times d_t\times cos({\beta }_0)$$
  $$y_1=y_0+d_y=y_0+v_x\times d_t\times sin({\beta }_0)$$
  
  而C点是绕着O点旋转的(如下图），所以yaw( 即 $\beta$)也在变化:
  
  $${\beta }_1={\beta }_0+d_{\beta }={\beta }_0+\omega \times d_t={\beta }_0+\frac{v_x}{R+\frac{W}{2}}\times d_t={\beta }_0+\frac{v_x}{\frac{L}{tan(\alpha )}+\frac{W}{2}}\times d_t$$
  因为$\beta$一直在变化，所以没法由上一时间点的坐标，直接公式求出经过计算周期$\Delta t$后的当前坐标，必须把 $\Delta t$细分成微小的时间段$d_t$，逐步累计（数值积分）得到当前计算时刻的坐标。（后注：其实就是纯旋转，用不着积分，😃）

本项目供应商采用的ascent库的数值积分算子RK4，把计算周期细分成100份，积分得到。

计算结果通过话题发布，作为小车的odom数据，即从开始以来，小车走过了多少路程，或者说以开始起点为坐标原点的当前坐标值 。而nav2的slam功能（一般是amcl）负责处理odom与地图的相互定位关系。（关于nav2的定位和坐标转换要另开一文来整理）

2.2 根据速度控制命令，计算小车速度和内转角

• 导航软件Nav2经过一些列规划控制计算后，由主题cmd_vel发布一个twist型的消息作为控制命令输出，该类型消息包含线速度$v_x$和角速度$\omega$。
• 那就需要把这个命令换算成小车前进的速度（$v_x$）和前轮转角（$\alpha$），然后通过某种方式（取决于小车硬件架构，本文项目是上位机通过CANBus发送速度和转向角给小车控制器就可以了）去驱动小车前进和转向。
• 从前面公式反推可得：
  $$\omega =\frac{v_x}{\frac{L}{tan(\alpha )}+\frac{W}{2}}\Rightarrow$$
  $$\alpha =arctan（\frac{L}{\frac{v_x}{\omega }-\frac{W}{2}}）$$

注意

• 所有角度都是弧度值，如果需要显示便于理解可以乘以180/M_PI。
• 小车发布的odom里面除了坐标还有速度（twist型，线速度+角速度）。