两轮差速机器人基于PID控制下轨迹跟踪

     机器人轨迹跟踪一般有两种方式,一是基于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,你也可以修改起点坐标)两轮差速机器人基于PID控制下轨迹跟踪_第1张图片

两轮差速机器人基于PID控制下轨迹跟踪_第2张图片

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

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

%%%%两轮差速履带车数学模型建立%%%%%%
    %%参数设置
 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

                                                                                                                                                                             鞠春宇

                                                                                                                                                                             于研究室

你可能感兴趣的:(自动驾驶算法,移动机器人)