智能车跟随圆弧路径原理

智能车跟随圆弧路径原理

1. 圆弧点生成

圆弧点P在世界坐标系的位置关系

2. 圆弧点的公式

$$\begin{array}{r}x=Rcos\theta \\ y=Rsin\theta \end{array}\text{\hspace{1em}(1)}$$
通过对点的求导，可以得到点在世界坐标系下的速度关系
$$\begin{array}{rl}\dot{x}& =-Rsin\theta \dot{\theta }\\ \dot{y}& =Rcos\theta \dot{\theta }\end{array}\text{\hspace{1em}(2)}$$
通过**公式(1)(2)**得到每个时刻小车在$X,Y$轴的速度值，将速度传给麦克纳姆轮的运动学模型即可进
行圆弧运动，小车的角速度为 $0$。 本文麦克纳姆轮小车车头朝向跟机器人坐标系Y轴方向一致

思路流程
$得到圆弧的点的坐标\to 求导\to 得到每个点的瞬时速度关系(\dot{x},\dot{y},0)\to 麦克纳姆轮运动学模型\to 四个电机$

通过上述流程得到，麦克纳姆轮成功走出圆的轨迹，但是车头方向是固定不变的。
接着，如何让小车走圆轨迹的时候，小车的车头跟圆的速度切线方向一致 或者 小车车头一直指向世界坐标系的原点(即圆弧的圆心) 呢

3. 圆弧路径跟随进阶

3.1 小车车头与圆的速度切线方向一致

此时的麦克纳姆轮小车可以简化成为两轮差速小车模型，有关两轮差速小车走圆的资料挺多的。也可以借鉴一下阿克曼构型的纯追踪算法如何跟踪圆弧轨迹。

接下来本文用另一种思路去解释，需要用到旋转变换的思想。 当然可以自己尝试对圆进行运动学分析，得到结果也会一样。

$x^{{}^{\prime }},y^{{}^{\prime }}表示的是机器人坐标系，x,y表示的是世界坐标系。$
当机器人车头方向与速度方向一致的时候
$$\alpha =\theta$$

第一种情况，因为机器人车头朝向是固定不变的，且机器人的坐标系方向与世界坐标系方向一致，因此瞬时速度在世界坐标系和机器人坐标系中都是一致(大小，方向)的。
当前情况，由于机器人坐标系与世界坐标系是存在角度$\alpha$，我们得到的公式(2)是世界坐标系下的瞬时速度，需要转换到机器人坐标系中，存在旋转矩阵$R_{rw}$. $w是世界坐标系(WorldCoordinate),r是机器人坐标系(RobotCoordinate)$.
通过旋转矩阵我们得知
$$\begin{gathered} R_{ab}\ast R_{ba}=I \\ {R}_{ba}=R_{ab}^{-1}={R_{ab}}^{⊺} \end{gathered}$$

$$R_{ab}=\left[\begin{array}{cc}cos\alpha & -sin\alpha \\ sin\alpha & cos\alpha \end{array}\right]$$
因此
$$R_{rw}=\left[\begin{array}{cc}cos\alpha & sin\alpha \\ -sin\alpha & cos\alpha \end{array}\right]$$
把世界坐标系的瞬时速度通过旋转变换到机器人坐标系下，即可得到机器人需要运行的速度$({\dot{x}}_r,{\dot{y}}_r,{\dot{\theta }}_r)$

$$\begin{gathered} V_r=R_{rw}{V}_w \\ \left[\begin{array}{c}{\dot{x}}_r\\ {\dot{y}}_r\end{array}\right]=\left[\begin{array}{cc}cos\alpha & sin\alpha \\ -sin\alpha & cos\alpha \end{array}\right]\ast \left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right] \end{gathered}$$
因为小车要走圆弧，因此角速度跟世界坐标系下的角速度一致，最后通过整理得到：
$$\begin{array}{rl}{\dot{x}}_r& =-Rsin(\theta -\alpha )\dot{\theta }\\ {\dot{y}}_r& =Rcos(\theta -\alpha )\dot{\theta }\\ {\dot{\theta }}_r& =\dot{\theta }\end{array}$$
最后将速度$({\dot{x}}_r,{\dot{y}}_r,{\dot{\theta }}_r)$传递到麦卡纳姆轮运动学模型中，即可完成车头跟速度切向方向一致的绕圆弧运动。

同理，对于小车车头一直指向世界坐标系的原点(即圆弧的圆心)也是旋转变换。
$因此当\alpha 和\theta 角度关系的不同，小车绕圆弧运动也不一样,如下图所示$

4. 个人思考与反思

1.在得到机器人走圆弧轨迹的方程后，如何让机器人进行一定时间内的速度规划？也就是轨迹规划，如三次、五次、梯形、S型规划，可以在评论区留言，后续会更新轨迹规划章节。