自动驾驶—两轮差速机器人基于PID控制下轨迹跟踪

柏贤

墨衍会员 · AI 创作全网分发码龄9年

关注

原创 2023-10-07 16:13:58 2.7w 阅读 AI 写同款· GEO 优化 ›

自动驾驶核心算法与代码同时被 2 个专栏收录

¥39.90 ¥99.00

42篇文章

订阅专栏

超级会员免费看

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

因为是在Matlab做的实验验证，首先你需要一个建立一个两轮差速模型，详情见我上一次的机器人差速模型博客：差速转向机器人数学模型_Jack Ju的博客-CSDN博客_差速转向；

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

$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：GitHub - 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

鞠春宇

于研究室

———————————————————————————————————————————

如果觉得写得还不错，就关注一下我的微信公众号，最新动态会分享在此。