本文已参与「新人创作礼」活动，一起开启掘金创作之路。
前言
考虑平面运动机器人，自由度有3个，分别是x , y , θ x,y,\theta x , y , θ v v v ω \omega ω
文章对控制算法原理进行简要介绍，最后有Matlab Simulink 实现
运动学模型
懒得画图，直接在网上找了个示意图。
运动学模型比较简单，直接给出结果
{ x ˙ = v cos ( θ ) y ˙ = v sin ( θ ) θ ˙ = ω \left \{\begin{array}{l}
\dot{x}=v\cos(\theta)\\
\dot{y}=v\sin(\theta)\\
\dot\theta=\omega
\end{array}\right. ⎩ ⎨ ⎧ x ˙ = v cos ( θ ) y ˙ = v sin ( θ ) θ ˙ = ω 误差模型
误差模型一般是定义在车体坐标系下，因此需要乘变换矩阵转化一下。
定义：全局坐标系下误差x e = x r − x , y e = y r − y , θ e = θ r − θ x_e=x_r-x,y_e=y_r-y,\theta_e=\theta_r-\theta x e = x r − x , y e = y r − y , θ e = θ r − θ e 1 , e 2 , e θ e_1,e_2,e_{\theta} e 1 , e 2 , e θ
[ e 1 e 2 e θ ] = [ cos θ sin θ 0 − sin θ cos θ 0 0 0 1 ] [ x e y e θ 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] ⎣ ⎡ e 1 e 2 e θ ⎦ ⎤ = ⎣ ⎡ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ⎦ ⎤ ⎣ ⎡ x e y e θ e ⎦ ⎤ 对其求导，整理为误差模型
{ e ˙ 1 = ω e 2 − v + v r cos e θ e ˙ 2 = − ω e 1 + v r sin 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 = ω e 2 − v + v r cos e θ e ˙ 2 = − ω e 1 + v r sin e θ 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 = [ e 1 e 2 e 3 ] T t → ∞ t \rightarrow \infty t → ∞ ∥ e ∥ = 0 \quad\|e\|=0 ∥ e ∥ = 0
反步法(Backstepping)设计控制律
误差模型为非线性模型，可以通过将其线性化的方式然后用线性化控制理论处理，但此类方法的鲁棒性不高。这里直接采用非线性控制器的设计方法-反步法。反步法的核心是基于Lyapunov稳定性定理，将复杂的系统分解为几个子系统，然后依次设计控制律使子系统稳定，进而保证整个系统稳定。
定义虚拟反馈变量e 1 ˉ \bar{e_1} e 1 ˉ e 1 ˉ = e 1 − k 1 e 2 ( ω 1 + ω 2 ) \bar{e_1}=e_1-k_1e_2(\frac{\omega}{\sqrt{1+\omega^2}}) e 1 ˉ = e 1 − k 1 e 2 ( 1 + ω 2 ω ) V y V_y V y V y = 0.5 e 2 2 V_y=0.5e_2^2 V y = 0.5 e 2 2 V y ˙ = e 2 ( − ω e 1 + v r sin e θ ) \dot{V_y}=e_{2}\left(-\omega e_{1}+v_{r} \sin e_{\theta}\right) V y ˙ = e 2 ( − ω e 1 + v r sin e θ ) e 1 → k 1 e 2 ( ω 1 + ω 2 ) e_1 \rightarrow k_1e_2(\frac{\omega}{\sqrt{1+\omega^2}}) e 1 → k 1 e 2 ( 1 + ω 2 ω ) e θ → 0 e_{\theta} \rightarrow 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 V y ˙ = − k 1 ω ( 1 + ω 2 ω ) e 2 2 ≤ 0 t → ∞ t \rightarrow \infty t → ∞ e 2 → 0 e_2 \rightarrow 0 e 2 → 0
因此下一步的目标则是，设计控制量v 和 ω v和\omega v 和 ω lim t → ∞ e 1 = k 1 e 2 ( ω 1 + ω 2 ) \lim _{t \rightarrow \infty} e_{1}=k_1e_2(\frac{\omega}{\sqrt{1+\omega^2}}) lim t → ∞ e 1 = k 1 e 2 ( 1 + ω 2 ω ) lim t → ∞ e ˉ 1 = 0 \lim _{t \rightarrow \infty} \bar{e}_{1}=0 lim t → ∞ e ˉ 1 = 0 lim t → ∞ e θ = 0 \lim _{t \rightarrow \infty} e_{\theta}=0 lim t → ∞ 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} \bar{e}_{1}^{2}+\frac{1}{2} e_{2}^{2}+\frac{2}{k_{2}}\left(1-\cos \frac{e_{\theta}}{4}\right) V = 2 1 e ˉ 1 2 + 2 1 e 2 2 + k 2 2 ( 1 − cos 4 e θ )
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 ) ) \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 ˙ = e 1 ˉ e 1 ˉ ˙ + e 2 e ˙ 2 + k 2 1 sin ( 2 θ e ) θ ˙ e = e 1 ˉ ⋅ [ − v + v r cos θ e − ( 1 + ω 2 ) 2 3 k 1 ω ˙ e 2 − 1 + ω 2 k 1 ω ( − ω e 1 + v r sin θ e ) ] − k 1 e 2 2 1 + ω 2 ω 2 + k 2 1 sin ( 2 θ e ) ( ω r − ω + 2 k 3 e 2 v r cos ( 2 θ e ) ) 这里化简需要用到一些三角函数代换，比如sin e θ = 4 sin e θ 4 cos e θ 4 cos e θ 2 \sin e_{\theta}=4 \sin \frac{e_{\theta}}{4} \cos \frac{e_{\theta}}{4} \cos \frac{e_{\theta}}{2} sin e θ = 4 sin 4 e θ cos 4 e θ cos 2 e θ
为保证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\{\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 = ω = v r cos e θ + k 1 ( 1 + ω 2 ω ) ω e 1 − k 1 v r ( 1 + ω 2 ω ) sin e θ − ( 1 + ω 2 ) 2 3 k 1 ω ˙ e 2 + k 2 ( e 1 − k 1 ( 1 + ω 2 ω ) e 2 ) ω r + 2 k 3 e 2 v r cos 2 e θ + k 4 sin 2 e θ 其中,k 1. k 2. k 3. k 4 k1.k2.k3.k4 k 1. k 2. k 3. k 4
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; k 1 = 0.1 ; k 2 = 50 ; k 3 = 150 ; k 4 = 10 ; x e , y e , θ e x_e,y_e,\theta_e x e , y e , θ e 1 m 1m 1 m 1 m / s 1m/s 1 m / s
总结
关于反步法里虚拟变量为什么要设置成e 1 − k 1 e 2 ( ω / 1 + ω 2 ) e_1-k_1e_2(\omega/\sqrt{1+\omega^2}) e 1 − k 1 e 2 ( ω / 1 + ω 2 ) − k 2 e 2 -k_2e_2 − k 2 e 2 V ˙ ( e 2 ) < = 0 \dot{V}(e2)<=0 V ˙ ( e 2 ) <= 0 ω / 1 + ω 2 \omega/\sqrt{1+\omega^2} ω / 1 + ω 2
参考文献
英文好的建议直接看英文的，写的更清楚简洁。
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.
