基于机械角度的航位推算定位

1 前言

基于机械角度的航位推算定位的定位原理是在给定移动机器人初始位姿条件下，根据移动机器人左右两侧轮子的机械角度在采样周期内的变化，计算出车轮移动的距离以及方向角的变化，从而实时推算出移动机器人的位姿[1]。
电机的机械角度指电机转子的实际空间几何角度，即用物理工具（如圆规、编码器）可测量的角度。无论电机极对数多少，转子旋转一圈对应的机械角度始终为360°，而在机器人的定位中，将转子旋转一圈的角度量程定位[-180°, 180°]。

2 模型建立

在建模过程中，将割草机器人按照两轮差速驱动方式进行建模。设割草机器人车轮半径为$r$，采样周期$\Delta T$机械角度的变化量为$\Delta {\theta }_m$，则$\Delta T$时间内车轮移动的距离$\Delta d$为：
$$\Delta d=\frac{\pi r}{180}\Delta {\theta }_m$$
如图1所示，设割草机器人从初始位姿$P_0=(x_0,y_0,{\theta }_0)$开始做逆时针方向的圆弧运动，经过$\Delta T$时间后到达位姿点$P_1=(x_0+\Delta x,y_0+\Delta y,{\theta }_0+\Delta \theta )$。其中，$(x_0,y_0)$为割草机器人初始位置，${\theta }_0$为初始方位角，即割草机器人坐标系$X_R$轴正方向与全局坐标系$X_W$轴正方向的夹角，$b$为左右车轮轮距，$R$为 割草机器人运动圆弧的半径，$\Delta S_R$、$\Delta S_L$、$\Delta S$分别表示左右轮和中心点在$\Delta T$时间内运动的距离，$\Delta x$、$\Delta y$、$\Delta \theta$（单位：rad）分别表示割草机器人在$X_W$方向和$Y_W$方向上的位移和方位角增加量。

假设在运动过程中，车轮没有发生打滑及碰撞现象，则一下方程组成立：
$$\{\begin{array}{l}\Delta S_R=(R+b/2)\Delta \theta \\ \Delta S_L=(R-b/2)\Delta \theta \\ \Delta S=R\Delta \theta \end{array}$$
解上述方程组可得，
$$\{\begin{array}{l}\Delta \theta =(\Delta S_R-\Delta S_L)/b\\ \Delta S=(\Delta S_R+\Delta S_L)/2\end{array}$$
根据上述方程，结合弦长公式，可最终计算出$X_W$与$Y_W$方向上的位移增加量，
$$\{\begin{array}{l}\Delta x=\Delta S[\sin (\frac{\Delta \theta }{2})/\frac{\Delta \theta }{2}]\cos (\theta +\frac{\Delta \theta }{2})\\ \Delta y=\Delta S[\sin (\frac{\Delta \theta }{2})/\frac{\Delta \theta }{2}]\sin (\theta +\frac{\Delta \theta }{2})\end{array}$$
当里程计的采样时间$\Delta T$比较短时，即机器人在一个周期内的转角变化$\Delta \theta$非常小。对于很小的$x$，有$sin(x)\approx x$，令$x=\frac{\Delta \theta }{2}$，则有，
$$sin(\frac{\Delta \theta }{2})\approx \frac{\Delta \theta }{2}$$
则上述方程组可以简化为：
$$\{\begin{array}{l}\Delta x=\Delta S\cos (\theta +\frac{\Delta \theta }{2})\\ \Delta y=\Delta S\sin (\theta +\frac{\Delta \theta }{2})\end{array}$$
最终得到基于里程计的割草机器人圆弧离散运动模型：
$$\{\begin{array}{l}{x}_{k+1}=x_k+\Delta S\cos ({\theta }_k+\frac{\Delta \theta }{2})\\ y_{k+1}=y_k+\Delta S\sin ({\theta }_k+\frac{\Delta \theta }{2})\\ {\theta }_{k+1}={\theta }_k+\Delta \theta \end{array}$$
当不考虑割草机器人方位角变化时，即$\Delta \theta \to 0$，可以得到直线离散运动模型：
$$\{\begin{array}{l}{x}_{k+1}=x_k+\Delta S\cos ({\theta }_k)\\ y_{k+1}=y_k+\Delta S\sin ({\theta }_k)\\ {\theta }_{k+1}={\theta }_k\end{array}$$
其中，$\Delta S=\frac{\Delta S_{R,k}+\Delta S_{L,k}}{2}$，$\Delta \theta =\frac{\Delta S_{R,k}-\Delta S_{L,k}}{b}$，$\Delta S_{R,k}和\Delta S_{L,k}$分别表示从$k$到$k+1$时刻，左右两侧车轮行驶的距离。
基于传统里程计的航位推算定位方法左右轮行驶的距离是根据光电编码器来计算得到的，其测量和计算误差会随着时间和距离无限增长，最终会导致割草机器人位姿估计值越来越偏离真实值。
基于机械角度的航位推算定位左右轮行驶的距离是根据电机机械角度来计算得到的，而由于机械角度代表的是电机转子的实际空间几何角度，因此没有累计误差，所以该方法在理论上是能够达到割草机器人长距离、高精度定位的要求，而且不依赖外部信息就能够实现割草机器人的自主定位和里程标定。

3 参考文献

[1]吴显.基于多传感器信息融合的移动机器人定位方法研究[D].北京交通大学,2016.