轨迹英文

机器⼈运动学轨迹跟踪控制（Matlab实现）

⽂章⽬录

前⾔

考虑平⾯运动机器⼈，⾃由度有3个，分别是，控制量为机器⼈的线速度和横摆⾓速度，希望实现机器⼈跟踪⽬标轨迹。

⽂章对控制算法原理进⾏简要介绍，最后有MatlabSimulink实现

运动学模型

懒得画图，直接在⽹上找了个⽰意图。

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

误差模型

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

x,y,θvω

⎩

⎨

⎧

=vcos(θ)x˙

=vsin(θ)

y˙

=ωθ

˙

定义：全局坐标系下误差；局部坐标系误差。

其关系可表达为：

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

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

度和⾓速度，并将和作为控制输⼊，使得系统的位姿误差能够保持有界，且当时,恒成⽴

反步法(Backstepping)设计控制律

误差模型为⾮线性模型，可以通过将其线性化的⽅式然后⽤线性化控制理论处理，但此类⽅法的鲁棒性不⾼。这⾥直接采⽤⾮线性控制器的

设计⽅法-反步法。反步法的核⼼是基于Lyapunov稳定性定理，将复杂的系统分解为⼏个⼦系统，然后依次设计控制律使⼦系统稳定，进⽽

保证整个系统稳定。

定义虚拟反馈变量

选取Lyapunov函数

其导数为：

当且时

由Lyapunov定理可得，时，(PS:这⾥的证明不太严谨，更加严谨的证明请参阅参考⽂献)。

因此下⼀步的⽬标则是，设计控制量,使得即且。

x=ex−rx,y=ey−ry,θ=eθ−rθe,e,e12θ

=⎣

⎡e1

e2

eθ⎦

⎤

⎣

⎡cosθ

−sinθ

0

sinθ

cosθ

0

0

0

1⎦

⎤

⎣

⎡xe

ye

θe⎦

⎤

⎩

⎨

⎧=ωe−v+vcoe˙12rθ

=−ωe+vsinee˙21rθ

=ω−ωe˙θr

vωvωe=[eee]123

Tt→∞∥e∥=0

e1ˉ

=e1ˉ

e−1ke()12

1+ω2

ω

Vy

V=y0.5e2

2

=

Vy˙

e−ωe+vsine2(1rθ)

e→1ke()12

1+ω2

ω

e→θ0

=

Vy˙

−kω()e≤1

1+ω2

ω

2

2

0

t→∞e→20

v和ωlime=t→∞1ke()12

1+ω2

ω

lim=t→∞eˉ10lime=t→∞θ0

选取系统Lyapunov函数：

对其求导，并化简有：

这⾥化简需要⽤到⼀些三⾓函数代换，⽐如。

为保证负定，因此取控制律为

其中,均为正常数。

Matlab实现

V=+

2

1

eˉ1

2

e+

2

1

2

2

1−cosk2

2

(

4eθ)

=+e+sinV

˙

e1ˉ

e1ˉ˙

2e˙2

k2

1(

2θe)θ˙

e

=⋅−v+vcosθ−−−ωe+vsinθ−kee1ˉ[re1+ω(2)2

3

ke1ω˙2

1+ω2

kω1

(1re)]12

21+ω2

ω2

+sinω−ω+2kevcosk2

1(

2θe)(r32r(

2θe))

sine=θ4sincoscos4

eθ

4

eθ

2

eθ

V

˙

⎩⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎧v=

ω=

vco+kωe−kvsinerθ1(

1+ω2

ω)11r(1+ω2

ω)θ

−+ke−ke

1+ω(2)2

3

ke1ω˙2

2(11(

1+ω2

ω)2)

ω+2kevcos+ksinr32r2

eθ

42

eθ

k1.k2.k3.k4

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

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

functiony=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));

%限制输出

ifabs(v)>5

v=sign(v)*5;

end

ifabs(omega)>5

omega=sign(omega)*5;

end

y=[vomega];

%y=[10];

仿真结果

⼏个参数需要⾃⼰调，这⾥的仿真结果⽤到的参数为。

图例从上⾄下依次是。⼩车的初始位置为(0,0,0);⽬标点的初始位置为(1,0,0)，跟踪半径为的圆形轨迹，线速度为。

k1=0.1;k2=50;k3=150;k4=10;

x,y,θeee1m1m/s

总结

关于反步法⾥虚拟变量为什么要设置成，我个⼈的理解原因是：引⼊项可以使得，说⽩了就

是通过试凑使其变为负定，引⼊是为了更快的收敛，同时增加稳定性，当然可以选⽤其他形式，也可以不选，设置为1,不过控

制效果没有上⽂中的好。（⼤家有什么其他的想法欢迎⼀起讨论）

参考⽂献

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

Hao,Y.,Wang,J.,Chepinskiy,S.A.,Krasnov,A.J.,&Liu,S.(2017).Backsteppingbadtrajectorytrackingcontrolfora

four-wheelmobilerobotwithdifferential-drivesteering.201736thChineControlConference(CCC).

doi:10.23919/chicc.2017.8028131

路少康.轮式移动机器⼈轨迹跟踪控制[D].西安电⼦科技⼤学,2020.

e−1ke(ω/)12

1+ω2−ke22(e2)

˙

=0

ω/

1+ω2