Skip to main content

机器人运动学-AGV轮系

单舵轮AGV轮系​

结构演示​

轮系结构​

如下图所示，本文研究的单舵轮 AGV 的结构包括1个主动轮、2个定向轮和1个万向轮，其中主动轮为舵轮，有驱动和转向的功能。万向轮主要起支撑作用，提高AGV承载能力和稳定性，对运动学模型没有影响。定义AGV前面两个从动轮中心连线的中点OO作为 AGV 的参考点，O−X−YO-X-Y 为小车局部坐标系。

逆运动学​

在全局坐标系OXYOXY中，有

[x˙y˙θ˙]=[cos⁡θ0sin⁡θ001]⋅[vω](1)\left[\begin{array}{c} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{array}\right]=\left[\begin{array}{cc} \cos \theta & 0 \\ \sin \theta & 0 \\ 0 & 1 \end{array}\right] \cdot\left[\begin{array}{l} v \\ \omega \end{array}\right] \tag{1}

式中： vv 和 ωω（定义逆时针旋转为正）分别为 AGV 在参考点OO处的线速度和角速度。

根据图中的几何关系可得:

{l1=L/sin⁡δl2=L/tan⁡δ+d(2)\left\{\begin{array}{l} l_{1}=L / \sin \delta \\ l_{2}=L / \tan \delta+d \end{array}\right. \tag{2}

式中 LL 为 AGV 前轮轴心线与后轮轴心线的间距； dd 为舵轮横向偏距；δδ 为舵轮偏转角，定义逆时针为正。 参考点处的线速度 vv 与角速度 ωω 表达式分别为：

{v=ω⋅l2ω=vd/l1(3)\left\{\begin{array}{l} v=\omega \cdot l_{2} \\ \omega=v_{d} / l_{1} \end{array}\right. \tag{3}

式中 vdv_d 为舵轮驱动速度。

将式(2)代入式(3)可得:

{v=vd(cos⁡δ+dsin⁡δL)ω=vdsin⁡δL(4)\left\{\begin{array}{l} v=v_{d}\left(\cos \delta+\frac{d \sin \delta}{L}\right) \\ \omega=\frac{v_{d} \sin \delta}{L} \end{array}\right. \tag{4}

结合式(1)和式(4)可得，单舵轮 AGV 运动学模型为：

{x˙=vdcos⁡θ(cos⁡δ+dsin⁡δL)y˙=vdsin⁡θ(cos⁡δ+dsin⁡δL)θ˙=vdsin⁡δL(5)\left\{\begin{array}{l} \dot{x}=v_{d} \cos \theta\left(\cos \delta+\frac{d \sin \delta}{L}\right) \\ \dot{y}=v_{d} \sin \theta\left(\cos \delta+\frac{d \sin \delta}{L}\right) \\ \dot{\theta}=\frac{v_{d} \sin \delta}{L} \end{array}\right. \tag{5}

正运动学​

未完待续

待续...

双舵轮AGV轮系​

结构演示​

轮系结构​

双舵轮AGV机器人解算，速度瞬心位于OO点，LL为O1O_1至O2O_2的长度。

1111

正运动学​

输入量

[vr0αrvf0αf]\left[ \begin{matrix} {v_{r0}}\\ {\alpha _r}\\ {v_{f0}}\\ {\alpha _f} \end{matrix} \right]

正解待解量

[v0ω0α0]\left[ \begin{matrix} {v_0}\\ {\omega _0}\\ {\alpha _0}\\ \end{matrix} \right]

1求解ω0\omega _0​

根据三角形△O1OO2\triangle{O_1}O{O_2}的角度关系

∠O1OO2+π2−αr+π2−αf=π\angle {O_1}O{O_2}{\rm{ + }}{\pi \over 2} - {\alpha _r} + {\pi \over 2} - {\alpha _f} = \pi

得出

∠O1OO2=αr+αf\angle {O_1}O{O_2}{\rm{ = }}{\alpha _r} + {\alpha _f}

根据正弦定理

Lsin⁡∠O1OO2=Rfsin⁡(π2−αr){L \over {\sin \angle {O_1}O{O_2}}} = {{{R_f}} \over {\sin ({\pi \over 2} - {\alpha _r})}}

因此

Rf=L⋅cos⁡αrsin⁡(αr+αf){R_f} = {{L \cdot \cos {\alpha _r}} \over {\sin ({\alpha _r} + {\alpha _f})}}

根据速度瞬心原理

ω0=vf0Rf=vf0⋅sin⁡(αr+αf)L⋅cos⁡αr=vf0⋅(sin⁡αr⋅cos⁡αf+cos⁡αr⋅sin⁡αf)L⋅cos⁡αr{\omega _0}{\rm{ = }}{{{v_{f0}}} \over {{R_f}}} = {{{v_{f0}} \cdot \sin ({\alpha _r} + {\alpha _f})} \over {L \cdot \cos {\alpha _r}}}{\rm{ = }}{{{v_{f0}} \cdot (\sin {\alpha _r} \cdot \cos {\alpha _f} + \cos {\alpha _r} \cdot \sin {\alpha _f})} \over {L \cdot \cos {\alpha _r}}}

2求解v0v_0​

在三角形△O0OO2\triangle{O_0}O{O_2}中，根据余弦定理

cos⁡(π2−αf)=Rf2+L24−r22⋅Rf⋅L2\cos ({\pi \over 2} - {\alpha _f}) = {{{R_f}^2 + {{{L^2}} \over 4} - {r^2}} \over {2 \cdot {R_f} \cdot {L \over 2}}}

解得

r=Rf2+L24−Rf⋅L⋅sin⁡αf=L⋅cos⁡2αrsin⁡2(αr+αf)+14−cos⁡αr⋅sin⁡αfsin⁡(αr+αf)=L2sin⁡(αr+αf)4cos⁡2αr+sin⁡2(αr+αf)−4cos⁡αr⋅sin⁡αf⋅sin⁡(αr+αf)\begin{array}{l}r = \sqrt {{R_f}^2 + \frac{{{L^2}}}{4} - {R_f} \cdot L \cdot \sin {\alpha _f}} \\{\rm{ = }}L \cdot \sqrt {\frac{{{{\cos }^2}{\alpha _r}}}{{{{\sin }^2}({\alpha _r} + {\alpha _f})}} + \frac{1}{4} - \frac{{\cos {\alpha _r} \cdot \sin {\alpha _f}}}{{\sin ({\alpha _r} + {\alpha _f})}}} \\ = \frac{L}{{2\sin ({\alpha _r} + {\alpha _f})}}\sqrt {4{{\cos }^2}{\alpha _r} + {{\sin }^2}({\alpha _r} + {\alpha _f}) - 4\cos {\alpha _r} \cdot \sin {\alpha _f} \cdot \sin ({\alpha _r} + {\alpha _f})} \end{array}

进而

v0=ω0⋅r=vf0⋅sin⁡(αr+αf)L⋅cos⁡αr⋅L2sin⁡(αr+αf)4cos⁡2αr+sin⁡2(αr+αf)−4cos⁡αr⋅sin⁡αf⋅sin⁡(αr+αf)=vf0⋅4cos⁡2αr+sin⁡2(αr+αf)−4cos⁡αr⋅sin⁡αf⋅sin⁡(αr+αf)2cos⁡αr\begin{array}{l}{v_0} = {\omega _0} \cdot r\\ = \frac{{{v_{f0}} \cdot \sin ({\alpha _r} + {\alpha _f})}}{{L \cdot \cos {\alpha _r}}} \cdot \frac{L}{{2\sin ({\alpha _r} + {\alpha _f})}}\sqrt {4{{\cos }^2}{\alpha _r} + {{\sin }^2}({\alpha _r} + {\alpha _f}) - 4\cos {\alpha _r} \cdot \sin {\alpha _f} \cdot \sin ({\alpha _r} + {\alpha _f})} \\{\rm{ = }}\frac{{{v_{f0}} \cdot \sqrt {4{{\cos }^2}{\alpha _r} + {{\sin }^2}({\alpha _r} + {\alpha _f}) - 4\cos {\alpha _r} \cdot \sin {\alpha _f} \cdot \sin ({\alpha _r} + {\alpha _f})} }}{{2\cos {\alpha _r}}}\end{array}

3求解α0\alpha _0​

在三角形△O0OO2\triangle{O_0}O{O_2}中，根据正弦定理

rsin⁡(π2−αf)=Rfsin⁡(π2+α0)rcos⁡αf=Rfcos⁡α0\begin{array}{l}\frac{r}{{\sin (\frac{\pi }{2} - {\alpha _f})}} = \frac{{{R_f}}}{{\sin (\frac{\pi }{2} + {\alpha _0})}}\\\frac{r}{{\cos {\alpha _f}}} = \frac{{{R_f}}}{{\cos {\alpha _0}}}\end{array}

即

α0=arccos(Rf⋅cos⁡αfr)=arccos(L⋅cos⁡αr⋅cos⁡αfsin⁡(αr+αf)L2sin⁡(αr+αf)4cos⁡2αr+sin⁡2(αr+αf)−4cos⁡αr⋅sin⁡αf⋅sin⁡(αr+αf))=arccos(2cos⁡αr⋅cos⁡αf4cos⁡2αr+sin⁡2(αr+αf)−4cos⁡αr⋅sin⁡αf⋅sin⁡(αr+αf))\begin{array}{l}{\alpha _0}{\rm{ = arccos(}}\frac{{{R_f} \cdot \cos {\alpha _f}}}{r}{\rm{)}}\\{\rm{ = arccos(}}\frac{{\frac{{L \cdot \cos {\alpha _r} \cdot \cos {\alpha _f}}}{{\sin ({\alpha _r} + {\alpha _f})}}}}{{\frac{L}{{2\sin ({\alpha _r} + {\alpha _f})}}\sqrt {4{{\cos }^2}{\alpha _r} + {{\sin }^2}({\alpha _r} + {\alpha _f}) - 4\cos {\alpha _r} \cdot \sin {\alpha _f} \cdot \sin ({\alpha _r} + {\alpha _f})} }}{\rm{)}}\\{\rm{ = arccos(}}\frac{{2\cos {\alpha _r} \cdot \cos {\alpha _f}}}{{\sqrt {4{{\cos }^2}{\alpha _r} + {{\sin }^2}({\alpha _r} + {\alpha _f}) - 4\cos {\alpha _r} \cdot \sin {\alpha _f} \cdot \sin ({\alpha _r} + {\alpha _f})} }}{\rm{)}}\end{array}

逆运动学​

输入量

[v0ω0α0]\left[ {\begin{array}{ccccccccccccccc}{{v_0}}\\{{\omega _0}}\\{{\alpha _0}}\end{array}} \right]

逆解待解量

[vr0αrvf0αf]\left[ {\begin{array}{ccccccccccccccc}{{v_{r0}}}\\{{\alpha _r}}\\{{v_{f0}}}\\{{\alpha _f}}\end{array}} \right]

1求解vr0v_{r0}​

由线速度和角速度的关系可知

r=v0ω0r = \frac{{{v_0}}}{{{\omega _0}}}

在三角形△O0OO1\triangle{O_0}O{O_1}中，根据余弦定理

cos⁡(π2−α0)=r2+L24−Rr22⋅r⋅L2\cos (\frac{\pi }{2} - {\alpha _0}) = \frac{{{r^2} + \frac{{{L^2}}}{4} - {R_r}^2}}{{2 \cdot r \cdot \frac{L}{2}}}

则

Rr=r2+L24−rL⋅sin⁡α0=v02ω02+L24−v0⋅Lω0⋅sin⁡α0{R_r} = \sqrt {{r^2} + \frac{{{L^2}}}{4} - rL \cdot \sin {\alpha _0}} = \sqrt {\frac{{{v_0}^2}}{{{\omega _0}^2}} + \frac{{{L^2}}}{4} - \frac{{{v_0} \cdot L}}{{{\omega _0}}} \cdot \sin {\alpha _0}}

此时

vr0=ω0⋅Rr=ω0⋅v02ω02+L24−v0⋅Lω0⋅sin⁡α0=v02+L24ω02−v0⋅L⋅ω0⋅sin⁡α0{v_{r0}} = {\omega _0} \cdot {R_r} = {\omega _0} \cdot \sqrt {\frac{{{v_0}^2}}{{{\omega _0}^2}} + \frac{{{L^2}}}{4} - \frac{{{v_0} \cdot L}}{{{\omega _0}}} \cdot \sin {\alpha _0}} = \sqrt {{v_0}^2 + \frac{{{L^2}}}{4}{\omega _0}^2 - {v_0} \cdot L \cdot {\omega _0} \cdot \sin {\alpha _0}}

2求解αr\alpha _r​

在三角形△O0OO1\triangle{O_0}O{O_1}中，根据正弦定理

rsin⁡(π2−αr)=Rrsin⁡(π2−α0)\frac{r}{{\sin (\frac{\pi }{2} - {\alpha _r})}} = \frac{{{R_r}}}{{\sin (\frac{\pi }{2} - {\alpha _0})}}

则

αr=arccos⁡(r⋅cos⁡α0Rr)=arccos⁡(v0ω0⋅cos⁡α0v02ω02+L24−v0⋅Lω0⋅sin⁡α0)=arccos⁡(v0⋅cos⁡α0v02+L24ω02−v0⋅L⋅ω0⋅sin⁡α0)\begin{array}{l}{\alpha _r} = \arccos (\frac{{r \cdot \cos {\alpha _0}}}{{{R_r}}}) = \arccos (\frac{{\frac{{{v_0}}}{{{\omega _0}}} \cdot \cos {\alpha _0}}}{{\sqrt {\frac{{{v_0}^2}}{{{\omega _0}^2}} + \frac{{{L^2}}}{4} - \frac{{{v_0} \cdot L}}{{{\omega _0}}} \cdot \sin {\alpha _0}} }})\\ = \arccos (\frac{{{v_0} \cdot \cos {\alpha _0}}}{{\sqrt {{v_0}^2 + \frac{{{L^2}}}{4}{\omega _0}^2 - {v_0} \cdot L \cdot {\omega _0} \cdot \sin {\alpha _0}} }})\end{array}

3求解vf0v_{f0}​

在三角形△O0OO2\triangle{O_0}O{O_2}中，根据余弦定理

cos⁡(π2+α0)=r2+L24−Rf22⋅r⋅L2\cos (\frac{\pi }{2} + {\alpha _0}) = \frac{{{r^2} + \frac{{{L^2}}}{4} - {R_f}^2}}{{2 \cdot r \cdot \frac{L}{2}}}

则

Rf=r2+L24+rL⋅sin⁡α0=v02ω02+L24+v0⋅Lω0⋅sin⁡α0{R_f} = \sqrt {{r^2} + \frac{{{L^2}}}{4} + rL \cdot \sin {\alpha _0}} = \sqrt {\frac{{{v_0}^2}}{{{\omega _0}^2}} + \frac{{{L^2}}}{4} + \frac{{{v_0} \cdot L}}{{{\omega _0}}} \cdot \sin {\alpha _0}}

此时

vf0=ω0⋅Rf=ω0⋅v02ω02+L24+v0⋅Lω0⋅sin⁡α0=v02+L24ω02+v0⋅L⋅ω0⋅sin⁡α0{v_{f0}} = {\omega _0} \cdot {R_f} = {\omega _0} \cdot \sqrt {\frac{{{v_0}^2}}{{{\omega _0}^2}} + \frac{{{L^2}}}{4} + \frac{{{v_0} \cdot L}}{{{\omega _0}}} \cdot \sin {\alpha _0}} = \sqrt {{v_0}^2 + \frac{{{L^2}}}{4}{\omega _0}^2 + {v_0} \cdot L \cdot {\omega _0} \cdot \sin {\alpha _0}}

4求解αf{\alpha _f}​

在三角形△O1OO2\triangle{O_1}O{O_2}中，根据正弦定理

rsin⁡(π2−αf)=Rfsin⁡(π2+α0)\frac{r}{{\sin (\frac{\pi }{2} - {\alpha _f})}} = \frac{{{R_f}}}{{\sin (\frac{\pi }{2} + {\alpha _0})}}

则

αf=arccos⁡(r⋅cos⁡α0Rf)=arccos⁡(v0ω0⋅cos⁡α0v02ω02+L24+v0⋅Lω0⋅sin⁡α0)=arccos⁡(v0⋅cos⁡α0v02+L24ω02+v0⋅L⋅ω0⋅sin⁡α0)\begin{array}{l}{\alpha _f} = \arccos (\frac{{r \cdot \cos {\alpha _0}}}{{{R_f}}}) = \arccos (\frac{{\frac{{{v_0}}}{{{\omega _0}}} \cdot \cos {\alpha _0}}}{{\sqrt {\frac{{{v_0}^2}}{{{\omega _0}^2}} + \frac{{{L^2}}}{4} + \frac{{{v_0} \cdot L}}{{{\omega _0}}} \cdot \sin {\alpha _0}} }})\\ = \arccos (\frac{{{v_0} \cdot \cos {\alpha _0}}}{{\sqrt {{v_0}^2 + \frac{{{L^2}}}{4}{\omega _0}^2 + {v_0} \cdot L \cdot {\omega _0} \cdot \sin {\alpha _0}} }})\end{array}