机器人轨迹跟踪一般有两种方式，一是基于PID，二是基于MPC模型预测控制方法。本文主要介绍了PID控制，这里我只采用了航向角的控制，对速度没有控制，因为，我认为，小车只要能到达目标点即可，复杂的我后续再分析分析以及还有在无人驾驶中比较有名的·MPC控制。    

因为是在Matlab做的实验验证， 首先你需要一个建立一个两轮差速模型，详情见我上一次的机器人差速模型博客：https://blog.csdn.net/qq_40464599/article/details/107176589；

   然后有状态空间的表达式如下：

x(t+1)=x(t)+v_{c}cos\theta \times T

y(t+1)=y(t)+v_{c}sin\theta \times T

\theta (t+1)=theta (t)+w_{c}\times T

   有了被控对象的数学模型，我们就开始写代码了，我的Matlab源码参见我的github：https://github.com/JackJu-HIT/TrajectoryControl

      首先看一下我的仿真结果，x = 2，y = 1，theta = pi / 6，终点只是限制x = 10，y = 10; x = 2，y = 1，theta = pi / 2，终点只是限制x = 10，y = 10; 可以得到两个不同的轨迹图（只是修改了航向角theta，你也可以修改起点坐标）

 然后我们开始分析要源码和工程思路了。

    这里是初始化的设置，起始位置和航向角设置。

%%%%两轮差速履带车数学模型建立%%%%%%
    %%参数设置
 L=4;%两个轮子间的距离
 T=0.1;%采样时间
 x=2;%初始化位置x坐标
 y=1;%初始化位置y坐标
 theta=pi/2;%初始化位置theta坐标
 
 x_goal=10;%终点位置坐标
 y_goal=10;%终点位置坐标

   这部分就是核心了，有数学模型还有PID控制，这里的我只用了比例控制，比例取得10.

while((x-x_goal)^2+(y-y_goal)^2 >0.01&&(x-x_goal<=0)) 
%%%%%%%%%这一段设置跟踪器，跟踪一段直线%%%%%%
%%%计算当前与目标的朝向误差
theta_goal=atan((y_goal-y)/(x_goal-x));
theta_error=theta-theta_goal;
u=-k*(theta_error);
 
 
 %%%%控制输入，左电机和右侧电机。
 vr=4+u;%控制输入需要你去更改//4代表着你想让车走多快，我这里未考虑到。
 vl=4;%控制输入需要你去更改
 
 
 
 %%%%process model
 
 %%%运动模型
 v=(vl+vr)/2;%中心速度
 w=(vr-vl)/L;
 x=x+v*cos(theta)*T;
 y=y+v*sin(theta)*T;
 theta=theta+w*T;
 x_push=[x_push;x];
 y_push=[y_push;y];
 theta_push=[theta_push;theta];
end

     这里的PID控制器，我想多数几句，就是航向是否向目标走去，如果向左偏，控制器就向右调节，总之处于一个动态平衡下。到时候加入积分微分环节看看，主要我这个数学模型太有好了，参数选的太好了吧，系统居然没有超调量，哈哈哈，就没用其他环节啦。

       后面继续优化好了，就写成C++代码，毕竟还要在Linux上用。

                                                                                                                                                                            20200709

                                                                                                                                                                             鞠春宇

                                                                                                                                                                             于研究室