机器人运动学轨迹跟踪控制(Matlab实现)
文章目录
- 前前言
- 前言
- 运动学模型
- 误差模型
- 反步法(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] ⎣⎡e1e2eθ⎦⎤=⎣⎡cosθ−sinθ0sinθcosθ0001⎦⎤⎣⎡xeyeθe⎦⎤
对其求导,整理为误差模型
{e˙1=ωe2−v+vrcoseθe˙2=−ωe1+vrsineθ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+vrcoseθe˙2=−ωe1+vrsineθ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−k1e2(1+ω2ω)
选取Lyapunov函数VyV_yVy
Vy=0.5e22V_y=0.5e_2^2Vy=0.5e22
其导数为:
Vy˙=e2(−ωe1+vrsineθ)\dot{V_y}=e_{2}\left(-\omega e_{1}+v_{r} \sin e_{\theta}\right)Vy˙=e2(−ωe1+vrsineθ)
当e1→k1e2(ω1+ω2)e_1 \rightarrow k_1e_2(\frac{\omega}{\sqrt{1+\omega^2}})e1→k1e2(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和ω, 使得 limt→∞e1=k1e2(ω1+ω2)\lim _{t \rightarrow \infty} e_{1}=k_1e_2(\frac{\omega}{\sqrt{1+\omega^2}})limt→∞e1=k1e2(1+ω2ω) 即 limt→∞eˉ1=0\lim _{t \rightarrow \infty} \bar{e}_{1}=0limt→∞eˉ1=0 且 limt→∞eθ=0\lim _{t \rightarrow \infty} e_{\theta}=0limt→∞eθ=0。
选取系统Lyapunov函数:
V=12eˉ12+12e22+2k2(1−coseθ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=21eˉ12+21e22+k22(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ˉ˙+e2e˙2+k21sin(2θe)θ˙e=e1ˉ⋅[−v+vrcosθe−(1+ω2)23k1ω˙e2−1+ω2k1ω(−ωe1+vrsinθe)]−k1e221+ω2ω2+k21sin(2θe)(ωr−ω+2k3e2vrcos(2θe))
这里化简需要用到一些三角函数代换,比如sineθ=4sineθ4coseθ4coseθ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=vrcoseθ+k1(ω1+ω2)ωe1−k1vr(ω1+ω2)sineθ−k1ω˙e2(1+ω2)32+k2(e1−k1(ω1+ω2)e2)ω=ωr+2k3e2vrcoseθ2+k4sineθ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=ω=vrcoseθ+k1(1+ω2ω)ωe1−k1vr(1+ω2ω)sineθ−(1+ω2)23k1ω˙e2+k2(e1−k1(1+ω2ω)e2)ωr+2k3e2vrcos2eθ+k4sin2eθ
其中,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;%反步法设计控制律3
k1=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;
end
if 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−k1e2(ω/1+ω2),我个人的理解原因是:引入−k2e2-k_2e_2−k2e2项可以使得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. (2017). Backstepping based trajectory tracking control for a four-wheel mobile robot with differential-drive steering. 2017 36th Chinese Control Conference (CCC). doi:10.23919/chicc.2017.8028131
路少康. 轮式移动机器人轨迹跟踪控制[D].西安电子科技大学,2020.
机器人运动学轨迹跟踪控制(Matlab实现)相关推荐
- 空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂(双臂)轨迹跟踪控制matlab仿真程序
空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂(双臂)轨迹跟踪控制matlab仿真程序,含空间机器人动力学模型,PD控制程序,带仿真结果,可供二次开发学习 ID:672006146 ...
- 基于MPC的移动机器人轨迹跟踪控制qpOASES例程
参考了 一个模型预测控制(MPC)的简单实现 https://www.cnblogs.com/zhjblogs/p/13880682.html 与 基于MPC的移动机器人轨迹跟踪控制matlab例程 ...
- 多自由度机械臂运动学正-逆解|空间轨迹规划控制|MATLAB仿真+实际机器调试
多自由度机械臂运动学正-逆解|空间轨迹规划控制|MATLAB仿真+实际机器调试 ) DH建模法可以参考这个博客: 还有<机器人>这本书,一定要理论实践相结合,理解后可以用几何法建模也可以用 ...
- 在Matlab下编程实现二维与三维的航迹跟踪控制、路径跟踪控制和轨迹跟踪控制,实现编队集群控制与避障控制
在Matlab下编程实现二维与三维的航迹跟踪控制.路径跟踪控制和轨迹跟踪控制,实现编队集群控制与避障控制. 研究对象有空中无人机.地面机器人.水面无人艇.水下机器人以及多智能体等. ID:321006 ...
- 基于运动学模型的轨迹跟踪控制
章四 基于运动学模型的轨迹跟踪控制 MPC(4)基于运动学模型的轨迹跟踪控制器设计 无人驾驶车辆模型预测控制(龚建伟)第四章 基于运动学模型的轨迹跟踪控制(仿真部分) 无人车辆在惯性坐标系中,车辆必须 ...
- 【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制
如果对Carsim的基础使用还不了解,可以参考:[Carsim Simulink自动驾驶仿真]基于MPC的速度控制 如果对MPC算法原理不清楚,可以参考:如何理解MPC模型预测控制理论 项目介绍: 教 ...
- 使用webots的MPC的移动机器人轨迹跟踪控制
上一篇文章中使用MPC对机器人的一个方向自由度进行了控制, 基于MPC的移动机器人轨迹跟踪控制qpOASES例程 现在使用速度与角速度对机器人进行平面运动控制. 所以机器人的控制量为U=[v ;w], ...
- 无人驾驶车辆路径规划及轨迹跟踪控制学习笔记(2)
目录 汇总 学习笔记 汇总 在关键交通场景中,轨迹规划和轨迹跟踪控制是自动驾驶车辆避免碰撞的两个关键.它不仅需要系统功能,而且需要强大的实时性. 我们集成了自动驾驶汽车的轨迹规划器和跟踪控制器,通过轨 ...
- 无人驾驶车辆规划+轨迹跟踪控制学习笔记(1)
综述 自动驾驶汽车有一个很有前途的未来,它可以使运输时间变得轻松,并使驾驶员能够参与其他活动,从而改变世界各地的日常生活.它们有可能大大减少由驾驶员的过失造成的撞车事故,包括驾驶积极.补偿过度.经验不 ...
- 机器人运动学轨迹规划(三)----S型曲线规划算法
写在前面:机器人运动学轨迹规划(二)里介绍了T型曲线规划算法,本文主要介绍S型速度曲线算法.同T型速度曲线相比,S形曲线更加平滑,避免了T形曲线在速度拐点引起的电机和drive train的冲击,但是 ...
最新文章
- winform中获取指定文件夹下的所有图片
- 使用pipeline的函数
- Android性能优化典范 - 第6季
- html不用点击自动执行,页面自动执行(加载)js的几种方法
- 后端技术:Java代码优秀案例,一定对你有提升!
- vCenter 升级错误 VCSServiceManager 1603
- javascript实现图片轮播_第2章 第9节 JavaScript(四)
- 超简单!基于Python搭建个人“云盘”
- mysql数据库简单语句
- Yeelink初步体验
- 阻塞IO, 非阻塞IO, 同步IO,异步IO
- 让getElementsByName适应IE和firefox
- 如何设置dedecms自定义表单必填项?
- python连接hive kerberos_数据库开发实战教程:使用Python连接Kerberos的Presto
- 【学习笔记】深入理解js原型和闭包(15)——闭包
- SilverLight自定义集合控件中的集合项数据绑定问题
- 用java实现新浪爬虫,代码完整剖析(仅针对当前SinaSignOn有效)
- UNCODE 与 ANSI 编码互相转换
- photoshop更改图片DPI方法
- 了解一下这几款实用的小众软件,相信你会有意想不到的收获