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

Jack Ju

已于 2023-10-07 16:13:58 修改

阅读量2.4w

点赞数 18

分类专栏：自动驾驶算法自动驾驶核心算法与代码文章标签：移动机器人

于 2020-07-09 22:46:03 首次发布

本文链接：https://blog.csdn.net/qq_40464599/article/details/107239274

版权

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

38 篇文章 33 订阅 ¥39.90 ¥99.00

订阅专栏

超级会员免费看

自动驾驶算法

76 篇文章 55 订阅

订阅专栏

本文探讨了自动驾驶中两轮差速机器人使用PID控制进行轨迹跟踪的方法。仅采用航向角控制，未涉及速度控制。通过Matlab建立模型并进行仿真验证，展示了不同起点和航向角下的轨迹效果。核心代码实现包括PID控制器的设计，目前仅使用比例环节。未来计划优化并转换为C++代码，应用于Linux平台。

摘要由CSDN通过智能技术生成

机器人轨迹跟踪一般有两种方式，一是基于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

鞠春宇

于研究室

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

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