文章目录

前前言前言运动学模型误差模型反步法(Backstepping)设计控制律Matlab 实现仿真结果总结参考文献

---

前前言

每到临近毕业季的时候，这篇文章的关注就会突然增多。很开心能跟大家分享、讨论、共同进步；但也有很多伸手党问我要源文件，这里统一答复：没有。一是确实由于时间比较长，源文件找不到了；二是我用到的大部分代码(除了文中的target模块代码)都贴了出来，没必要再整个源文件，想复现的话照着做就一定能复现。所以请不要再问我能不能分享源文件了，当然别的问题可以一起交流讨论~

前言

考虑平面运动机器人，自由度有3个，分别是x,y,θx,y,\thetax,y,θ，控制量为机器人的线速度vvv和横摆角速度ω\omegaω，希望实现机器人跟踪目标轨迹。

文章对控制算法原理进行简要介绍，最后有Matlab Simulink 实现

运动学模型

懒得画图，直接在网上找了个示意图。

运动学模型比较简单，直接给出结果

{x˙=vcos⁡(θ)y˙=vsin⁡(θ)θ˙=ω\left \{\begin{array}{l} \dot{x}=v\cos(\theta)\\ \dot{y}=v\sin(\theta)\\ \dot\theta=\omega \end{array}\right. ⎩⎨⎧​x˙=vcos(θ)y˙​=vsin(θ)θ˙=ω​

误差模型

误差模型一般是定义在车体坐标系下，因此需要乘变换矩阵转化一下。

定义：全局坐标系下误差xe=xr−x,ye=yr−y,θe=θr−θx_e=x_r-x,y_e=y_r-y,\theta_e=\theta_r-\thetaxe​=xr​−x,ye​=yr​−y,θe​=θr​−θ；局部坐标系误差e1,e2,eθe_1,e_2,e_{\theta}e1​,e2​,eθ​。

其关系可表达为：

[e1e2eθ]=[cos⁡θsin⁡θ0−sin⁡θcos⁡θ0001][xeyeθe]\left[\begin{array}{c} e_1 \\ e_2 \\ e_{\theta} \end{array}\right]=\left[\begin{array}{ccc} \cos \theta & \sin \theta & 0 \\ -\sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{c} x_e \\ y_e \\ \theta_e \end{array}\right] ⎣⎡​e1​e2​eθ​​⎦⎤​=⎣⎡​cosθ−sinθ0​sinθcosθ0​001​⎦⎤​⎣⎡​xe​ye​θe​​⎦⎤​

对其求导，整理为误差模型

{e˙1=ωe2−v+vrcos⁡eθe˙2=−ωe1+vrsin⁡eθe˙θ=ωr−ω\left\{\begin{array}{l} \dot{e}_{1}=\omega e_{2}-v+v_{r} \cos e_{\theta} \\ \dot{e}_{2}=-\omega e_{1}+v_{r} \sin e_{\theta} \\ \dot{e}_{\theta}=\omega_{r}-\omega \end{array}\right. ⎩⎨⎧​e˙1​=ωe2​−v+vr​coseθ​e˙2​=−ωe1​+vr​sineθ​e˙θ​=ωr​−ω​

对移动机器人的运动学模型而言，实现轨迹跟踪控制便是寻找合适的线速

度vvv 和角速度ω\omegaω ，并将vvv 和ω\omegaω 作为控制输入，使得系统的位姿误差e=[e1e2e3]Te=[e_1~ e_2 ~e_3]^Te=[e1​e2​e3​]T能够保持有界，且当t→∞t \rightarrow \inftyt→∞ 时 , ∥e∥=0\quad\|e\|=0∥e∥=0恒成立

反步法(Backstepping)设计控制律

误差模型为非线性模型，可以通过将其线性化的方式然后用线性化控制理论处理，但此类方法的鲁棒性不高。这里直接采用非线性控制器的设计方法-反步法。反步法的核心是基于Lyapunov稳定性定理，将复杂的系统分解为几个子系统，然后依次设计控制律使子系统稳定，进而保证整个系统稳定。

定义虚拟反馈变量e1ˉ\bar{e_1}e1​ˉ​

e1ˉ=e1−k1e2(ω1+ω2)\bar{e_1}=e_1-k_1e_2(\frac{\omega}{\sqrt{1+\omega^2}})e1​ˉ​=e1​−k1​e2​(1+ω2​ω​)

选取Lyapunov函数VyV_yVy​

Vy=0.5e22V_y=0.5e_2^2Vy​=0.5e22​

其导数为：

Vy˙=e2(−ωe1+vrsin⁡eθ)\dot{V_y}=e_{2}\left(-\omega e_{1}+v_{r} \sin e_{\theta}\right)Vy​˙​=e2​(−ωe1​+vr​sineθ​)

当e1→k1e2(ω1+ω2)e_1 \rightarrow k_1e_2(\frac{\omega}{\sqrt{1+\omega^2}})e1​→k1​e2​(1+ω2​ω​)且eθ→0e_{\theta} \rightarrow 0eθ​→0时

Vy˙=−k1ω(ω1+ω2)e22≤0\dot{V_y}=-k_{1} \omega (\frac{\omega}{\sqrt{1+\omega^2}}) e_{2}^{2} \leq 0Vy​˙​=−k1​ω(1+ω2​ω​)e22​≤0

由Lyapunov定理可得，t→∞t \rightarrow \inftyt→∞时，e2→0e_2 \rightarrow 0e2​→0(PS:这里的证明不太严谨，更加严谨的证明请参阅参考文献)。

因此下一步的目标则是，设计控制量v和ωv和\omegav和ω, 使得 lim⁡t→∞e1=k1e2(ω1+ω2)\lim _{t \rightarrow \infty} e_{1}=k_1e_2(\frac{\omega}{\sqrt{1+\omega^2}})limt→∞​e1​=k1​e2​(1+ω2​ω​) 即 lim⁡t→∞eˉ1=0\lim _{t \rightarrow \infty} \bar{e}_{1}=0limt→∞​eˉ1​=0 且 lim⁡t→∞eθ=0\lim _{t \rightarrow \infty} e_{\theta}=0limt→∞​eθ​=0。

选取系统Lyapunov函数：

V=12eˉ12+12e22+2k2(1−cos⁡eθ4)V=\frac{1}{2} \bar{e}_{1}^{2}+\frac{1}{2} e_{2}^{2}+\frac{2}{k_{2}}\left(1-\cos \frac{e_{\theta}}{4}\right)V=21​eˉ12​+21​e22​+k2​2​(1−cos4eθ​​)

对其求导，并化简有：

V˙=e1ˉe1ˉ˙+e2e˙2+1k2sin⁡(θe2)θ˙e=e1ˉ⋅[−v+vrcos⁡θe−k1ω˙e2(1+ω2)32−k1ω1+ω2(−ωe1+vrsin⁡θe)]−k1e22ω21+ω2+1k2sin⁡(θe2)(ωr−ω+2k3e2vrcos⁡(θe2))\begin{aligned} &\dot{V}=\bar{e_1} \dot{\bar{e_1}}+e_{2} \dot{e}_{2}+\frac{1}{k_{2}} \sin \left(\frac{\theta_{e}}{2}\right) \dot{\theta}_{e} \\ &=\bar{e_1} \cdot\left[-v+v_{r} \cos \theta_{e}-\frac{k_{1} \dot{\omega} e_{2}}{\left(1+\omega^{2}\right)^{\frac{3}{2}}}\right. \left.-\frac{k_{1} \omega}{\sqrt{1+\omega^{2}}}\left(-\omega e_{1}+v_{r} \sin \theta_{e}\right)\right]-k_{1} e_{2}^{2} \frac{\omega^{2}}{\sqrt{1+\omega^{2}}} \\ &+\frac{1}{k_{2}} \sin \left(\frac{\theta_{e}}{2}\right)\left(\omega_{r}-\omega+2 k_{3} e_{2} v_{r} \cos \left(\frac{\theta_{e}}{2}\right)\right) \end{aligned} ​V˙=e1​ˉ​e1​ˉ​˙​+e2​e˙2​+k2​1​sin(2θe​​)θ˙e​=e1​ˉ​⋅[−v+vr​cosθe​−(1+ω2)23​k1​ω˙e2​​−1+ω2​k1​ω​(−ωe1​+vr​sinθe​)]−k1​e22​1+ω2​ω2​+k2​1​sin(2θe​​)(ωr​−ω+2k3​e2​vr​cos(2θe​​))​

这里化简需要用到一些三角函数代换，比如sin⁡eθ=4sin⁡eθ4cos⁡eθ4cos⁡eθ2\sin e_{\theta}=4 \sin \frac{e_{\theta}}{4} \cos \frac{e_{\theta}}{4} \cos \frac{e_{\theta}}{2}sineθ​=4sin4eθ​​cos4eθ​​cos2eθ​​。

为保证V˙\dot{V}V˙负定，因此取控制律为

{v=vrcos⁡eθ+k1(ω1+ω2)ωe1−k1vr(ω1+ω2)sin⁡eθ−k1ω˙e2(1+ω2)32+k2(e1−k1(ω1+ω2)e2)ω=ωr+2k3e2vrcos⁡eθ2+k4sin⁡eθ2\left\{\begin{aligned} v=& v_{r} \cos e_{\theta}+k_{1}\left(\frac{\omega}{\sqrt{1+\omega^{2}}}\right) \omega e_{1}-k_{1} v_{r}\left(\frac{\omega}{\sqrt{1+\omega^{2}}}\right) \sin e_{\theta} \\ &-\frac{k_{1} \dot{\omega} e_{2}}{\left(1+\omega^{2}\right)^{\frac{3}{2}}}+k_{2}\left(e_{1}-k_{1}\left(\frac{\omega}{\sqrt{1+\omega^{2}}}\right) e_{2}\right) \\ \omega=& \omega_{r}+2 k_{3} e_{2} v_{r} \cos \frac{e_{\theta}}{2}+k_{4} \sin \frac{e_{\theta}}{2} \end{aligned}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧​v=ω=​vr​coseθ​+k1​(1+ω2​ω​)ωe1​−k1​vr​(1+ω2​ω​)sineθ​−(1+ω2)23​k1​ω˙e2​​+k2​(e1​−k1​(1+ω2​ω​)e2​)ωr​+2k3​e2​vr​cos2eθ​​+k4​sin2eθ​​​

其中,k1.k2.k3.k4k1.k2.k3.k4k1.k2.k3.k4均为正常数。

Matlab 实现

在Simuklink 依据车体运动学模型建立车体运动子系统，依据上面的控制律设计控制器。

target给出目标位姿，controller给出控制量。

function y = controller(u)%u分别为全局坐标系下车体位姿误差和车体横摆角%y为车体线速度和角速度%目标速度vr=1;wr=1;%将世界坐标系下的误差转化至车体坐标下e1=cos(u(4))*u(1)+sin(u(4))*u(2);e2=-sin(u(4))*u(1)+cos(u(4))*u(2);etheta=u(3);%反步法设计控制律% k1=1;k2=20;k3=20;k4=2;% e1_bar=e1-k1*e2;% omega=k2*e2*vr*cos(etheta/2)*cos(etheta/4)+k4*sin(etheta/4)+wr;% v=vr*cos(etheta)+k3*e1_bar;%反步法设计控制律2% k1=0.01;k2=16;k3=4;k4=16;% omega=k2*e2*vr*cos(etheta/2)*cos(etheta/4)+k4*sin(etheta/4)+wr;% e1_bar=e1-k1*atan(omega)*e2;% vrdot=0;% wrdot=0;% omega_dot=k2*((-omega*e1+vr*sin(etheta))*vr+e2*vrdot)*cos(etheta/2)...%*cos(etheta/4)-(k2*e2*vr*sin(etheta/4)*(0.5+0.75*cos(etheta/2))-0.25*k4*cos(etheta/4))...%*(wr-omega)+wrdot;% v=vr*cos(etheta)-k1*e2/(1+omega^2)*omega_dot-k1*atan(omega)*(-omega*e1+vr*sin(etheta))+k3*e1_bar;%反步法设计控制律3k1=0.1;k2=50;k3=150;k4=10;omega=2*k3*e2*vr*cos(etheta/2)+k4*sin(etheta/2)+wr;%e1_bar=e1-k1*(omega/(1+omega^2)^0.5)*e2;vrdot=0;wrdot=0;omega_dot=2*k3*((-omega*e1+vr*sin(etheta))*vr+e2*vrdot)*cos(etheta/2)-k3*e2*vr*sin(etheta/2)*(wr-omega)...+0.5*k4*cos(etheta/2)*(wr-omega)+wrdot;v=vr*cos(etheta)+k1*omega*e1*(omega/(1+omega^2)^0.5)-k1*vr*sin(etheta)*(omega/(1+omega^2)^0.5)...-k1*omega_dot*e2/(1+omega^2)^1.5+k2*(e1-k1*e2*(omega/(1+omega^2)^0.5));%限制输出if abs(v)>5v=sign(v)*5;endif abs(omega)>5omega=sign(omega)*5;endy = [v omega];%y =[1 0];

仿真结果

几个参数需要自己调，这里的仿真结果用到的参数为k1=0.1;k2=50;k3=150;k4=10;k1=0.1;k2=50;k3=150;k4=10;k1=0.1;k2=50;k3=150;k4=10;。

图例从上至下依次是xe,ye,θex_e,y_e,\theta_exe​,ye​,θe​。小车的初始位置为(0,0,0);目标点的初始位置为(1,0,0)，跟踪半径为1m1m1m的圆形轨迹，线速度为1m/s1m/s1m/s。

总结

关于反步法里虚拟变量为什么要设置成e1−k1e2(ω/1+ω2)e_1-k_1e_2(\omega/\sqrt{1+\omega^2})e1​−k1​e2​(ω/1+ω2​)，我个人的理解原因是：引入−k2e2-k_2e_2−k2​e2​项可以使得V˙(e2)<=0\dot{V}(e2)<=0V˙(e2)<=0，说白了就是通过试凑使其变为负定，引入ω/1+ω2\omega/\sqrt{1+\omega^2}ω/1+ω2​是为了更快的收敛，同时增加稳定性，当然可以选用其他形式，也可以不选，设置为1,不过控制效果没有上文中的好。（大家有什么其他的想法欢迎一起讨论）

参考文献

英文好的建议直接看英文的，写的更清楚简洁。

Hao, Y., Wang, J., Chepinskiy, S. A., Krasnov, A. J., & Liu, S. (). Backstepping based trajectory tracking control for a four-wheel mobile robot with differential-drive steering. 36th Chinese Control Conference (CCC). doi:10.23919/chicc..8028131

路少康. 轮式移动机器人轨迹跟踪控制[D].西安电子科技大学,.

如果觉得《机器人运动学轨迹跟踪控制(Matlab实现)》对你有帮助，请点赞、收藏，并留下你的观点哦！