MATLAB 轮式机器人轨迹跟踪仿真

两轮式移动机器人参考点位于机器人驱动轴中心模型分析：

控制目标是使机器人在二维平面中运动到指定位置，但由于轮式机器人不能进行平移运动，故其状态变量不能只取（x,y），应考虑车头朝向问题，所以小车的状态变量用向量表示为：
$$q=[x,y,\theta {]}^T$$
（x,y）为机器人当前位置，theta为机器人的车头方向，机器人只能沿着垂直与驱动轴的方向移动，故移动机器人的运动约束为：
$$\dot{x}\sin \theta -\dot{y}\cos \theta =0$$
将机器人驱动轴方向的运动力抵消掉
在这个约束条件下，移动机器人的运动学模型可以用
$$\dot{x}=v\cos \theta \dot{y}=v\sin \theta \dot{\theta }=\omega$$
其中，v和omega分别表示移动机器人的线性速度和角速度
假设参考轨迹是可行的，并由以下虚拟机器人生成
$${\dot{x}}_d=v_d\cos {\theta }_d{\dot{y}}_d=v_d\sin {\theta }_d{\dot{\theta }}_d={\omega }_d$$
$$q_d={\left[x_d,y_d,{\theta }_d\right]}^T$$
那么我们的控制目标是为移动机器人设计一个连续反馈控制律(v，ω)，同时解决稳定和跟踪问题，控制目标为：
$$\underset{t\to \infty }{\lim }\left(q(t)-q_d(t)\right)=0$$
参考文献：
Simultaneous Stabilization and Tracking of Nonholonomic Mobile Robots: A Lyapunov-Based Approach

两轮式移动机器人参考点位于机器人驱动轴中心前偏离L模型分析：

机器人的状态变量用向量表示为：
$$x=[x,y,\theta {]}^T$$
图中对于xp点有
$${\mathbf{x}}_p={\left[\begin{array}{l}x,y\end{array}\right]}^{\top }$$
对于偏移点S（x）有
$$s(\mathbf{x})={\mathbf{x}}_p+l\left[\begin{array}{c}\cos (\theta )\\ \sin (\theta )\end{array}\right]$$
这样S（x）点的运动将没有约束条件，机器人的运动学模型为：
$$\dot{\mathbf{x}}=\left[\begin{array}{cc}\cos (\theta )& 0\\ \sin (\theta )& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \omega \end{array}\right]$$
s(x)的导数为：
$$\dot{s}(\mathbf{x})={\dot{\mathbf{x}}}_p+l\dot{\theta }\left[\begin{array}{c}-\sin (\theta )\\ \cos (\theta )\end{array}\right]$$
得到：
$$\dot{s}(\mathbf{x})=R_l(\theta )\left[\begin{array}{l}v\\ \omega \end{array}\right]$$
其中
$$R_l(\theta )=\left[\begin{array}{cc}\cos (\theta )& -l\sin (\theta )\\ \sin (\theta )& l\cos (\theta )\end{array}\right]$$
$$R_l^{-1}(\theta )=\left[\begin{array}{cc}\cos (\theta )& \sin (\theta )\\ -\frac{1}{l}\sin (\theta )& \frac{1}{l}\cos (\theta )\end{array}\right]$$
最终有：
$$\left[\begin{array}{c}v\\ \omega \end{array}\right]=R_l^{-1}(\theta )\dot{s}(\mathbf{x})$$
这样就可以根据$\dot{s}(\mathbf{x})$求出机器人的线速度与角速度，进而对机器人进行控制
参考文献：
The Robotarium: Globally Impactful Opportunities, Challenges, and Lessons Learned in Remote-Access, Distributed Control of Multirobot Systems

MATLAB仿真

根据上述关系，我们最终要输出的为机器人的线速度与角速度，进而控制机器人跟踪期望轨迹，假定期望位置为
$$x_r,y_r$$
在x,y,$\theta$方向上的误差分别为：
$$e_x=x_r-x,e_y=y_r-y,e_{\theta }={\theta }_r-\theta$$
定义位姿跟踪误差：
$$q_e=\left[\begin{array}{c}{x}_e\\ y_e\\ {\theta }_e\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}{l}{x}_r-x\\ y_r-y\\ {\theta }_r-\theta \end{array}\right]$$
求导得到：
$$\begin{array}{rl}{\dot{x}}_e& =-\dot{\theta }\sin \theta \left(x_r-x\right)+\cos \theta \left({\dot{x}}_r-\dot{x}\right)+\dot{\theta }\cos \theta \left(y_r-y\right)+\sin \theta \left({\dot{y}}_r-\dot{y}\right)\\ & =\dot{\theta }\left[-\sin \theta \left(x_r-x\right)+\cos \theta \left(y_r-y\right)\right]-v+{\nu }_r\left(\cos {\theta }_r\cos \theta +\sin {\theta }_r\sin \theta \right)\\ & =\omega y_e-v+v_r\cos {\theta }_e\end{array}$$

$$\begin{array}{rl}{\dot{y}}_e& =-\dot{\theta }\cos \theta \left(x_r-x\right)-\sin \theta \left({\dot{x}}_r-\dot{x}\right)-\dot{\theta }\sin \theta \left(y_r-y\right)+\cos \theta \left({\dot{y}}_r-\dot{y}\right)\\ & =-\dot{\theta }\left[\cos \theta \left(x_r-x\right)+\sin \theta \left(y_r-y\right)\right]+{\nu }_r\left(-\cos {\theta }_r\sin \theta +\sin {\theta }_r\cos \theta \right)\\ & =-\omega x_e+v_r\sin {\theta }_e\end{array}$$
整理位姿误差微分方程为：
$$\left[\begin{array}{c}{\dot{x}}_e\\ {\dot{y}}_e\\ {\dot{\theta }}_e\end{array}\right]=\left[\begin{array}{c}\omega y_e-v+v_r\cos {\theta }_e\\ -\omega x_e+v_r\sin {\theta }_e\\ {\omega }_r-\omega \end{array}\right]$$
根据上述得到的运动学方程，自主导航车轨迹跟踪的问题转化为设计输入控制量$q={\left(\begin{array}{ll}v& w\end{array}\right)}^T$使闭环系统位姿误差$p_e$凡能够全局一致有界，并且当$t\to \infty$，系统在任意的初始跟踪误差下有${\lim }_{t\to \infty }\left[\left|x_e(t)\right|+\left|y_e(t)\right|+\left|{\theta }_e(t)\right|\right]=0$。

  根据自主导航车的位姿误差微分方程，可以选取函数如下：
$$\mathrm{V}=\frac{k_1}{2}\left(x_e^2+y_e^2\right)+2{\left(\sin \frac{{\theta }_e}{2}\right)}^2\ge 0$$
将其微分得：
$$\begin{array}{rl}\dot{\mathrm{V}}& =k_1\left(x_e{\dot{x}}_e+y_e{\dot{y}}_e\right)+2\sin \frac{{\theta }_e}{2}\cos \frac{{\theta }_e}{e}{\dot{\theta }}_e\\ & =k_1\left[x_e\left(v_r\cos {\theta }_e-v+y_e\omega \right)+y_e\left(v_r\sin {\theta }_e-x_e\omega \right)\right]+\left({\omega }_r-\omega \right)\sin {\theta }_e\\ & =k_1\left[x_e{v}_r\cos {\theta }_e-v{x}_e\right]+k_1{y}_c{v}_r\sin {\theta }_e+\left({\omega }_r-\omega \right)\sin {\theta }_e\end{array}$$
由此可得控制律为：
$$q=\left[\begin{array}{c}v\\ w\end{array}\right]=\left[\begin{array}{c}{v}_r\cos {\theta }_e+k_2{x}_e\\ w_r+k_1{v}_r{y}_e+k_3\sin {\theta }_e\end{array}\right]$$
其中，控制参数$k_2,k_3>0$。
（上述控制律的实验效果并不好，可能是参数数值选取的不恰当，可自行尝试，下面进行单纯比例控制实验）

单纯比例控制：

我们的目标是跟踪误差为0，即$e_x,e_y$为0

我们设
$${\dot{e}}_x=-K{e}_x,{\dot{e}}_y=-K{e}_y$$
这个系统的平衡点就为（0，0），相当对进行了比例控制，而针对有偏移参考点，可将模型看成一阶积分器，这样可将输入描述为：
$$\dot{x}=u_x,\dot{y}=u_y$$
下面进行编程实现：
1，求出期望位置与当前位置的偏差
2，将偏差输入到控制器，根据变换关系$\left[\begin{array}{c}v\\ \omega \end{array}\right]=R_l^{-1}(\theta )\dot{s}(\mathbf{x})$求出线速度与角速度
3，跟据$x=x+\delta x,y=y+\delta y,\theta =\theta +\delta \theta ，$求出下一时刻的位置及车头朝向
4，进行循环求解跟踪
跟踪效果如下：

附代码：

clear all;clc;

%% define constant
l=0.2;
R=1;
K=0.1;
delta_T=0.5;

%% define initialization
Pos=[0,12,3/2*pi]; %X,Y,Theta
T=linspace(0,2*pi,200);
desire_Pos=[16*power(sin(T),3);13*cos(T)-5*cos(2*T)-2*cos(3*T)-cos(4*T)];

%%  Graphics
f3=figure;
xlabel('x (m)')
ylabel('y (m)')
grid on

%%  robot dimensions
A.R_w = 1/2; % robot width/2
A.R_l=  2/2;   % robot length/2
A.a1 = [-A.R_l -A.R_w]';
A.b1 = [A.R_l -A.R_w/2]';
A.b2 = [A.R_l A.R_w/2]';
A.c = [-A.R_l A.R_w]';
A.P = [A.a1 A.b1 A.b2 A.c];

A.Rot = [ cos(Pos(3)) -sin(Pos(3)); sin(Pos(3)) cos(Pos(3))]*A.P; %rotated car
A.Prot_trasl = A.Rot + [ ones(1,4)*Pos(1); ones(1,4)*Pos(2)]; % add offset of car's center

A.P_robot=patch(A.P(1,:),A.P(2,:),'b');
A.P_robot.XData=A.Prot_trasl(1,:)';
A.P_robot.YData=A.Prot_trasl(2,:)';


%% define controller


%% draw picture
h= animatedline('color','r','LineStyle','--');
h_car= animatedline('color','b');
h_car_model = animatedline('Marker','o','MaximumNumPoints',1);     %只显示1个新的点
axis([-20 20 -20 15])

for k = 1:length(T)
    addpoints(h_car,Pos(1),Pos(2));
    addpoints(h_car_model,Pos(1),Pos(2));
    
    e_x=Pos(1)-desire_Pos(1,k);
    e_y=Pos(2)-desire_Pos(2,k);
    u_x=-K*e_x;
    u_y=-K*e_y;
    A_mat=[cos(Pos(3)) -l*sin(Pos(3));sin(Pos(3)) l*cos(Pos(3))];
    P=[cos(Pos(3)) 0;sin(Pos(3)) 0;0 1];
    U_mat=[u_x,u_y]';
    v_w_mat=A_mat\U_mat;
    Pos(1)=Pos(1)+v_w_mat(1)*cos(Pos(3))-v_w_mat(2)*l*sin(Pos(3));
    Pos(2)=Pos(2)+v_w_mat(1)*sin(Pos(3))+v_w_mat(2)*l*cos(Pos(3));
    
    Pos(3)=Pos(3)+v_w_mat(2)*delta_T;
    addpoints(h,desire_Pos(1,k),desire_Pos(2,k));
    
    A.Rot = [cos(Pos(3)) -sin(Pos(3)); sin(Pos(3)) cos(Pos(3))]*A.P; %rotated car
    A.Prot_trasl = A.Rot + [ones(1,4)*Pos(1); ones(1,4)*Pos(2)]; % add offset of car's center
    A.P_robot.XData=A.Prot_trasl(1,:)';
    A.P_robot.YData=A.Prot_trasl(2,:)';
    frame=getframe(gcf);
    im = frame2im(frame); 
    [imind,cm] = rgb2ind(im,256);
    if k==1
         imwrite(imind,cm,'experiment.gif','gif', 'LoopCount',inf,'DelayTime',0.000001);
    end
    if rem(k,2)==0
         imwrite(imind,cm,'experiment.gif','gif','WriteMode','append','DelayTime',0.000001);
    end
    drawnow
end

能力有限仅供参考,欢迎提出宝贵意见！