当前栏目
机器人运动学轨迹跟踪控制(Matlab实现)
前前言
每到临近毕业季的时候，这篇文章的关注就会突然增多。很开心能跟大家分享、讨论、共同进步；但也有很多伸手党问我要源文件，这里统一答复：没有。一是确实由于时间比较长，源文件找不到了；二是我用到的大部分代码(除了文中的target模块代码)都贴了出来，没必要再整个源文件，想复现的话照着做就一定能复现。所以请不要再问我能不能分享源文件了，当然别的问题可以一起交流讨论~
前言
考虑平面运动机器人，自由度有3个，分别是 x , y , θ x,y, heta x,y,θ，控制量为机器人的线速度 v v v和横摆角速度 ω omega ω，希望实现机器人跟踪目标轨迹。
文章对控制算法原理进行简要介绍，最后有Matlab Simulink 实现
运动学模型
懒得画图，直接在网上找了个示意图。
运动学模型比较简单，直接给出结果
{ x ˙ = v cos ( θ ) y ˙ = v sin ( θ ) θ ˙ = ω left {egin{array}{l} dot{x}=vcos( heta)\ dot{y}=vsin( heta)\ dot heta=omega end{array} ight. ⎩⎨⎧x˙=vcos(θ)y˙=vsin(θ)θ˙=ω
误差模型
误差模型一般是定义在车体坐标系下，因此需要乘变换矩阵转化一下。
定义：全局坐标系下误差
x
e
=
x
r
−
x
,
y
e
=
y
r
−
y
,
θ
e
=
θ
r
−
θ
x_e=x_r-x,y_e=y_r-y, heta_e= heta_r- heta
xe=xr−x,ye=yr−y,θe=θr−θ；局部坐标系误差
e
1
,
e
2
,
e
θ
e_1,e_2,e_{ heta}
e1,e2,eθ。
其关系可表达为：
[
e
1
e
2
e
θ
]
=
[
cos
θ
sin
θ
0
−
sin
θ
cos
θ
0
0
0
1
]
[
x
e
y
e
θ
e
]
left[egin{array}{c} e_1 \ e_2 \ e_{ heta} end{array}
ight]=left[egin{array}{ccc} cos heta & sin heta & 0 \ -sin heta & cos heta & 0 \ 0 & 0 & 1 end{array}
ight]left[egin{array}{c} x_e \ y_e \ heta_e end{array}
ight]
⎣⎡e1e2eθ⎦⎤=⎣⎡cosθ−sinθ0sinθcosθ0001⎦⎤⎣⎡xeyeθe⎦⎤
对其求导，整理为误差模型
{
e
˙
1
=
ω
e
2
−
v
+
v
r
cos
e
θ
e
˙
2
=
−
ω
e
1
+
v
r
sin
e
θ
e
˙
θ
=
ω
r
−
ω
left{egin{array}{l} dot{e}_{1}=omega e_{2}-v+v_{r} cos e_{ heta} \ dot{e}_{2}=-omega e_{1}+v_{r} sin e_{ heta} \ dot{e}_{ heta}=omega_{r}-omega end{array}
ight.
⎩⎨⎧e˙1=ωe2−v+vrcoseθe˙2=−ωe1+vrsineθe˙θ=ωr−ω
对移动机器人的运动学模型而言，实现轨迹跟踪控制便是寻找合适的线速
度
v
v
v 和角速度
ω
omega
ω ，并将
v
v
v 和
ω
omega
ω 作为控制输入，使得系统的位姿误差
e
=
[
e
1
e
2
e
3
]
T
e=[e_1~ e_2 ~e_3]^T
e=[e1 e2 e3]T能够保持有界，且当
t
→
∞
t
ightarrow infty
t→∞ 时 ,
∥
e
∥
=
0
quad|e|=0
∥e∥=0恒成立
反步法(Backstepping)设计控制律
误差模型为非线性模型，可以通过将其线性化的方式然后用线性化控制理论处理，但此类方法的鲁棒性不高。这里直接采用非线性控制器的设计方法-反步法。反步法的核心是基于Lyapunov稳定性定理，将复杂的系统分解为几个子系统，然后依次设计控制律使子系统稳定，进而保证整个系统稳定。
定义虚拟反馈变量
e
1
ˉ
ar{e_1}
e1ˉ
e
1
ˉ
=
e
1
−
k
1
e
2
(
ω
1
+
ω
2
)
ar{e_1}=e_1-k_1e_2(frac{omega}{sqrt{1+omega^2}})
e1ˉ=e1−k1e2(1+ω2ω)
选取Lyapunov函数
V
y
V_y
Vy
V
y
=
0.5
e
2
2
V_y=0.5e_2^2
Vy=0.5e22
其导数为：
V
y
˙
=
e
2
(
−
ω
e
1
+
v
r
sin
e
θ
)
dot{V_y}=e_{2}left(-omega e_{1}+v_{r} sin e_{ heta}
ight)
Vy˙=e2(−ωe1+vrsineθ)
当
e
1
→
k
1
e
2
(
ω
1
+
ω
2
)
e_1
ightarrow k_1e_2(frac{omega}{sqrt{1+omega^2}})
e1→k1e2(1+ω2ω)且
e
θ
→
0
e_{ heta}
ightarrow 0
eθ→0时
V
y
˙
=
−
k
1
ω
(
ω
1
+
ω
2
)
e
2
2
≤
0
dot{V_y}=-k_{1} omega (frac{omega}{sqrt{1+omega^2}}) e_{2}^{2} leq 0
Vy˙=−k1ω(1+ω2ω)e22≤0
由Lyapunov定理可得，
t
→
∞
t
ightarrow infty
t→∞时，
e
2
→
0
e_2
ightarrow 0
e2→0(PS:这里的证明不太严谨，更加严谨的证明请参阅参考文献)。
因此下一步的目标则是，设计控制量 v 和 ω v和omega v和ω, 使得 lim t → ∞ e 1 = k 1 e 2 ( ω 1 + ω 2 ) lim _{t ightarrow infty} e_{1}=k_1e_2(frac{omega}{sqrt{1+omega^2}}) limt→∞e1=k1e2(1+ω2ω) 即 lim t → ∞ e ˉ 1 = 0 lim _{t ightarrow infty} ar{e}_{1}=0 limt→∞eˉ1=0 且 lim t → ∞ e θ = 0 lim _{t ightarrow infty} e_{ heta}=0 limt→∞eθ=0。
选取系统Lyapunov函数：
V
=
1
2
e
ˉ
1
2
+
1
2
e
2
2
+
2
k
2
(
1
−
cos
e
θ
4
)
V=frac{1}{2} ar{e}_{1}^{2}+frac{1}{2} e_{2}^{2}+frac{2}{k_{2}}left(1-cos frac{e_{ heta}}{4}
ight)
V=21eˉ12+21e22+k22(1−cos4eθ)
对其求导，并化简有：
V
˙
=
e
1
ˉ
e
1
ˉ
˙
+
e
2
e
˙
2
+
1
k
2
sin
(
θ
e
2
)
θ
˙
e
=
e
1
ˉ
⋅
[
−
v
+
v
r
cos
θ
e
−
k
1
ω
˙
e
2
(
1
+
ω
2
)
3
2
−
k
1
ω
1
+
ω
2
(
−
ω
e
1
+
v
r
sin
θ
e
)
]
−
k
1
e
2
2
ω
2
1
+
ω
2
+
1
k
2
sin
(
θ
e
2
)
(
ω
r
−
ω
+
2
k
3
e
2
v
r
cos
(
θ
e
2
)
)
egin{aligned} &dot{V}=ar{e_1} dot{ar{e_1}}+e_{2} dot{e}_{2}+frac{1}{k_{2}} sin left(frac{ heta_{e}}{2}
ight) dot{ heta}_{e} \ &=ar{e_1} cdotleft[-v+v_{r} cos heta_{e}-frac{k_{1} dot{omega} e_{2}}{left(1+omega^{2}
ight)^{frac{3}{2}}}
ight. left.-frac{k_{1} omega}{sqrt{1+omega^{2}}}left(-omega e_{1}+v_{r} sin heta_{e}
ight)
ight]-k_{1} e_{2}^{2} frac{omega^{2}}{sqrt{1+omega^{2}}} \ &+frac{1}{k_{2}} sin left(frac{ heta_{e}}{2}
ight)left(omega_{r}-omega+2 k_{3} e_{2} v_{r} cos left(frac{ heta_{e}}{2}
ight)
ight) 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))
这里化简需要用到一些三角函数代换，比如 sin e θ = 4 sin e θ 4 cos e θ 4 cos e θ 2 sin e_{ heta}=4 sin frac{e_{ heta}}{4} cos frac{e_{ heta}}{4} cos frac{e_{ heta}}{2} sineθ=4sin4eθcos4eθcos2eθ。
为保证
V
˙
dot{V}
V˙负定，因此取控制律为
{
v
=
v
r
cos
e
θ
+
k
1
(
ω
1
+
ω
2
)
ω
e
1
−
k
1
v
r
(
ω
1
+
ω
2
)
sin
e
θ
−
k
1
ω
˙
e
2
(
1
+
ω
2
)
3
2
+
k
2
(
e
1
−
k
1
(
ω
1
+
ω
2
)
e
2
)
ω
=
ω
r
+
2
k
3
e
2
v
r
cos
e
θ
2
+
k
4
sin
e
θ
2
left{egin{aligned} v=& v_{r} cos e_{ heta}+k_{1}left(frac{omega}{sqrt{1+omega^{2}}}
ight) omega e_{1}-k_{1} v_{r}left(frac{omega}{sqrt{1+omega^{2}}}
ight) sin e_{ heta} \ &-frac{k_{1} dot{omega} e_{2}}{left(1+omega^{2}
ight)^{frac{3}{2}}}+k_{2}left(e_{1}-k_{1}left(frac{omega}{sqrt{1+omega^{2}}}
ight) e_{2}
ight) \ omega=& omega_{r}+2 k_{3} e_{2} v_{r} cos frac{e_{ heta}}{2}+k_{4} sin frac{e_{ heta}}{2} end{aligned}
ight.
⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧v=ω=vrcoseθ+k1(1+ω2ω)ωe1−k1vr(1+ω2ω)sineθ−(1+ω2)23k1ω˙e2+k2(e1−k1(1+ω2ω)e2)ωr+2k3e2vrcos2eθ+k4sin2eθ
其中,
k
1.
k
2.
k
3.
k
4
k1.k2.k3.k4
k1.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)>5
v=sign(v)*5;
end
if abs(omega)>5
omega=sign(omega)*5;
end
y = [v omega];
%y =[1 0];
仿真结果
几个参数需要自己调，这里的仿真结果用到的参数为
k
1
=
0.1
;
k
2
=
50
;
k
3
=
150
;
k
4
=
10
;
k1=0.1;k2=50;k3=150;k4=10;
k1=0.1;k2=50;k3=150;k4=10;。
图例从上至下依次是
x
e
,
y
e
,
θ
e
x_e,y_e, heta_e
xe,ye,θe。小车的初始位置为(0,0,0);目标点的初始位置为(1,0,0)，跟踪半径为
1
m
1m
1m的圆形轨迹，线速度为
1
m
/
s
1m/s
1m/s。
总结
关于反步法里虚拟变量为什么要设置成 e 1 − k 1 e 2 ( ω / 1 + ω 2 ) e_1-k_1e_2(omega/sqrt{1+omega^2}) e1−k1e2(ω/1+ω2)，我个人的理解原因是：引入 − k 2 e 2 -k_2e_2 −k2e2项可以使得 V ˙ ( e 2 ) < = 0 dot{V}(e2)<=0 V˙(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.
相关文章
- 前端面试 【JavaScript】— typeof 是否能正确判断类型？
- 前端面试 【JavaScript】— instanceof 能否判断基本数据类型？
- 前端面试 【JavaScript】— 能不能手动实现一下 instanceof 的功能？
- 前端面试 【JavaScript】— Object.is和=== 有什么区别？
- 前端面试 【JavaScript】— JS中类型转换有哪几种？
- 前端面试 【JavaScript】— == 和 ===有什么区别？
- 前端面试 【JavaScript】— 对象转原始类型是根据什么流程运行的？
- JavaScript 的 parseInt() 函数
- javascript实现两个数字进行组合
- JS监听键盘按键
- 大前端开发中的路由管理之五：Flutter篇
- Javascript的DOM操作
- 在Vue项目中使用WebSocket技术
- 新手向：前端程序员必学基本技能——调试JS代码
- React 毁了 Web 开发！
- 「JS 逆向百例」cnki 学术翻译 AES 加密分析
- 商标注册域名后缀用什么？商标和域名有哪些区别？
- 网站建设流程是怎样的？需要看重哪些细节？
- 网站域名商标注册流程是什么？网站域名商标有什么用？
- 如何建设一个实用性强的网站 网站上线后如何运营