本文的前提是规划器已经为轮式移动机器人(WMR)规划出了满意的路径,倘若建立了路径与时间确切的对应关系,则称之为轨迹。本文重点讨论如何实现对于确定轨迹的跟踪问题。
机器人坐标系
在平面上,该机器人底盘总的维数是3个:2个为平面中的位置;1个为沿垂直轴的方向的转动,它与平面正交。把机器人的底盘看作刚体,忽略机器人和它的轮子间内在的关联和自由度。随动轮仅在运动过程中起支撑作用,其在运动学模型中的影响可忽略不计。假设WMR的左右轮均为刚体,只能沿着车轮滚动的方向前进,无侧向滑动。为了确定机器人在平面中的位置,如下图所示,建立了平面全局参考坐标系和机器人局部参考坐标系之间的关系。
将平面上任意一点选为原点 O O O,相互正交的 X X X轴和 Y Y Y轴建立全局参考坐标系。为了确定机器人的位置,选择机器人两轮连轴的轴心 o o o 作为它的位置参考点。基于 { x , y } \{x, y\} {
x,y}定义机器人底盘上相对于 o o o 的两个轴,其中, y y y轴位于两轮连线上并指向左轮, x x x轴与 y y y轴正交,从而定义了机器人的局部参考坐标系。在全局参考坐标系上, O O O的位置由坐标 x x x和 y y y确定,全局和局部参考坐标系之间的角度差由机器人坐标系的 x x x轴与全局坐标系 X X X轴的夹角 θ \theta θ给定。因此可以将机器人的位姿描述为具有这3个元素的矢量。
ξ 1 = [ x y θ ] \xi_1 = \begin{bmatrix} x \\ y\\ \theta \end{bmatrix} ξ1=⎣⎡xyθ⎦⎤
双轮差速移动机器人运动学模型
全局坐标系和机器人坐标系之间的变换关系
为了根据分量的移动描述机器人的移动,就需要构造全局参考坐标系和机器人局部参考坐标系的运动映射。
先定义一些接下来会用到的量:
- R R R 移动机器人轮子的半径;
- w l , w r w_l, w_r wl,wr 分别是左右轮的角速度;
- v l , v r v_l, v_r vl,vr 分别是左右轮的线速度;
- w 0 , v 0 w_0, v_0 w0,v0 分别是质心 o o o的角速度和线速度;
- l l l 两轮的轴距
现在考虑在极短时间 Δ t \Delta t Δt内,机器人位姿的变化:在 Δ t \Delta t Δt时间内,可以认为机器人朝 x x x轴方向匀速运动,匀速转动。因此,
全局坐标系下,横坐标改变量为
Δ x = v ( t ) Δ t cos θ \Delta x = v(t)\Delta t\cos\theta Δx=v(t)Δtcosθ
全局坐标系下,纵坐标改变量为
Δ y = v ( t ) Δ t sin θ \Delta y = v(t)\Delta t\sin\theta Δy=v(t)Δtsinθ
θ \theta θ的改变量为
Δ θ = w ( t ) Δ t \Delta\theta = w(t)\Delta t Δθ=w(t)Δt
写成矩阵形式:
[ Δ x Δ y Δ θ ] = [ v ( t ) Δ t cos θ v ( t ) Δ t sin θ w ( t ) Δ t ] = [ cos θ 0 sin θ 0 0 1 ] [ v ( t ) w ( t ) ] Δ t \begin{bmatrix} \Delta x \\ \Delta y \\ \Delta\theta\end{bmatrix} = \begin{bmatrix} v(t)\Delta t\cos\theta \\ v(t)\Delta t \sin\theta \\ w(t)\Delta t\end{bmatrix}= \begin{bmatrix} \cos\theta & 0 \\ \sin\theta & 0\\ 0 & 1\end{bmatrix} \begin{bmatrix} v(t) \\ w(t)\end{bmatrix}\Delta t ⎣⎡ΔxΔyΔθ⎦⎤=⎣⎡v(t)Δtcosθv(t)Δtsinθw(t)Δt⎦⎤=⎣⎡cosθsinθ0001⎦⎤[v(t)w(t)]Δt
于是
[ Δ x Δ t Δ y Δ t Δ θ Δ t ] = [ cos θ 0 sin θ 0 0 1 ] [ v ( t ) w ( t ) ] \begin{bmatrix} \frac{\Delta x}{\Delta t} \\ \frac{\Delta y}{\Delta t} \\ \frac{\Delta\theta}{\Delta t} \end{bmatrix}= \begin{bmatrix} \cos\theta & 0 \\ \sin\theta & 0 \\ 0 & 1\end{bmatrix} \begin{bmatrix} v(t) \\ w(t)\end{bmatrix} ⎣⎡ΔtΔxΔtΔyΔtΔθ⎦⎤=⎣⎡cosθsinθ0001⎦⎤[v(t)w(