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

作者：weixin_40725706 | 2024-04-14 22:37:13

踩

两轮差速机器人mpc控制

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

鞠春宇

于研究室

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

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/weixin_40725706/article/detail/424446