智能车跟随圆弧路径原理
1. 圆弧点生成
2. 圆弧点的公式
x=Rcosθy=Rsinθ(1)
\begin{aligned}
x = Rcos\theta \\
y = Rsin\theta \tag{1}
\end{aligned}
x=Rcosθy=Rsinθ(1)
通过对点的求导,可以得到点在世界坐标系下的速度关系
x˙=−Rsinθθ˙y˙=Rcosθθ˙(2)
\begin{aligned}
\dot x &= -Rsin\theta\dot\theta \\
\dot y &= Rcos\theta\dot\theta \\ \tag{2}
\end{aligned}
x˙y˙=−Rsinθθ˙=Rcosθθ˙(2)
通过**公式(1)(2)**得到每个时刻小车在X,YX, YX,Y轴的速度值,将速度传给麦克纳姆轮的运动学模型即可进
行圆弧运动,小车的角速度为 000。 本文麦克纳姆轮小车车头朝向跟机器人坐标系Y轴方向一致
思路流程
得到圆弧的点的坐标→求导→得到每个点的瞬时速度关系(x˙,y˙,0)→麦克纳姆轮运动学模型→四个电机得到圆弧的点的坐标\rightarrow求导\rightarrow得到每个点的瞬时速度关系(\dot x, \dot y, 0)\rightarrow麦克纳姆轮运动学模型\rightarrow四个电机得到圆弧的点的坐标→求导→得到每个点的瞬时速度关系(x˙,y˙,0)→麦克纳姆轮运动学模型→四个电机
通过上述流程得到,麦克纳姆轮成功走出圆的轨迹,但是车头方向是固定不变的。
接着,如何让小车走圆轨迹的时候,小车的车头跟圆的速度切线方向一致 或者 小车车头一直指向世界坐标系的原点(即圆弧的圆心) 呢
3. 圆弧路径跟随进阶
3.1 小车车头与圆的速度切线方向一致
此时的麦克纳姆轮小车可以简化成为两轮差速小车模型,有关两轮差速小车走圆的资料挺多的。也可以借鉴一下阿克曼构型的纯追踪算法如何跟踪圆弧轨迹。
接下来本文用另一种思路去解释,需要用到旋转变换的思想。 当然可以自己尝试对圆进行运动学分析,得到结果也会一样。
x′,y′表示的是机器人坐标系,x,y表示的是世界坐标系。x^{'}, y^{'}表示的是机器人坐标系, x, y 表示的是世界坐标系。x′,y′表示的是机器人坐标系,x,y表示的是世界坐标系。
当机器人车头方向与速度方向一致的时候
α=θ
\alpha = \theta
α=θ
第一种情况,因为机器人车头朝向是固定不变的,且机器人的坐标系方向与世界坐标系方向一致,因此瞬时速度在世界坐标系和机器人坐标系中都是一致(大小,方向)的。
当前情况,由于机器人坐标系与世界坐标系是存在角度α\alphaα,我们得到的公式(2)是世界坐标系下的瞬时速度,需要转换到机器人坐标系中,存在旋转矩阵RrwR_{rw}Rrw. w是世界坐标系(WorldCoordinate),r是机器人坐标系(RobotCoordinate)w是世界坐标系(World Coordinate),r是机器人坐标系(Robot Coordinate)w是世界坐标系(WorldCoordinate),r是机器人坐标系(RobotCoordinate).
通过旋转矩阵我们得知
Rab∗Rba=IRba=Rab−1=Rab⊺
R_{ab} * R_{ba} = I \\ R_{ba} = R^{-1}_{ab} = {R_{ab}}^\intercal
Rab∗Rba=IRba=Rab−1=Rab⊺
Rab=[cosα−sinαsinαcosα]
R_{ab} =\left[
\begin{matrix}
cos\alpha & -sin\alpha \\
sin\alpha & cos\alpha \\
\end{matrix}
\right]
Rab=[cosαsinα−sinαcosα]
因此
Rrw=[cosαsinα−sinαcosα]
R_{rw} =\left[
\begin{matrix}
cos\alpha & sin\alpha \\
-sin\alpha & cos\alpha \\
\end{matrix}
\right]
Rrw=[cosα−sinαsinαcosα]
把世界坐标系的瞬时速度通过旋转变换到机器人坐标系下,即可得到机器人需要运行的速度(x˙r,y˙r,θ˙r)(\dot x_r, \dot y_r, \dot \theta_r)(x˙r,y˙r,θ˙r)
Vr=RrwVw[x˙ry˙r]=[cosαsinα−sinαcosα]∗[x˙y˙]
V_r = R_{rw}V_w \\
\left[
\begin{matrix}
\dot x_r \\
\dot y_r \\
\end{matrix}
\right]
= \left[
\begin{matrix}
cos\alpha & sin\alpha \\
-sin\alpha & cos\alpha \\
\end{matrix}
\right] * \left[
\begin{matrix}
\dot x \\
\dot y \\
\end{matrix}
\right]
Vr=RrwVw[x˙ry˙r]=[cosα−sinαsinαcosα]∗[x˙y˙]
因为小车要走圆弧,因此角速度跟世界坐标系下的角速度一致,最后通过整理得到:
x˙r=−Rsin(θ−α)θ˙y˙r=Rcos(θ−α)θ˙θ˙r=θ˙
\begin{aligned}
\dot x_r &= -Rsin(\theta -\alpha)\dot \theta\\
\dot y_r &= Rcos(\theta-\alpha)\dot \theta\\
\dot \theta_r &= \dot\theta
\end{aligned}
x˙ry˙rθ˙r=−Rsin(θ−α)θ˙=Rcos(θ−α)θ˙=θ˙
最后将速度(x˙r,y˙r,θ˙r)(\dot x_r,\dot y_r,\dot \theta_r )(x˙r,y˙r,θ˙r)传递到麦卡纳姆轮运动学模型中,即可完成车头跟速度切向方向一致的绕圆弧运动。
同理,对于小车车头一直指向世界坐标系的原点(即圆弧的圆心)也是旋转变换。
因此当α和θ角度关系的不同,小车绕圆弧运动也不一样,如下图所示因此当\alpha和\theta角度关系的不同,小车绕圆弧运动也不一样,如下图所示因此当α和θ角度关系的不同,小车绕圆弧运动也不一样,如下图所示
4. 个人思考与反思
1.在得到机器人走圆弧轨迹的方程后,如何让机器人进行一定时间内的速度规划?也就是轨迹规划,如三次、五次、梯形、S型规划,可以在评论区留言,后续会更新轨迹规划章节。