自动驾驶——两轮差速机器人基于PID模型构建与跟踪控制（针对于ROS工程下的下位机后附linux下c++代码，Github源码链接）

关注

原创 2023-10-07 16:18:21 1.4w 阅读 AI 写同款· GEO 优化 ›

AGV移动机器人PID运动控制 PID算法是控制行业最经典、最简单、而又最能体现反馈控制思想的算法，PID算法利用反馈来检测偏差信号，并通过偏差信号来控制被控量。而控制器本身就是比例、积分、微分三个环节的加和。其功能框如图1所示：考虑在某个特定的时刻t，此时输入量为rin(t)，输出量为rout(t)，于是偏差就可计算为err(t)=rin(t)-rout(t)。于是PID的基本控制规律就可以表示为如下公式：其中Kp为比例带，TI为积分时间，TD为微分时间。阅读详情

上几次介绍了两轮差速机器人的数学模型参见我的博客差速转向机器人数学模型_Jack Ju的博客-CSDN博客_差速转向和只做了关于航向角的闭环PID控制两轮差速机器人基于PID控制下轨迹跟踪_Jack Ju的博客-CSDN博客_pid轨迹跟踪。

今天这篇文章前几篇会更深入、更具有工程性。

首先，有一个问题我一直有些模糊，最近考虑到工程实现，所以想了一下。ROS标准的Twist速度信息会向下位机发送一个速度V和角速度w，然后我们需要用v和w去控制两个电机。之前的博客已经推导过，利用运动学反解即可，有如下关系

$v_{r}=v_{c}+\frac{w_{c}l}{2}$

$v_{l}=v_{c}-\frac{w_{c}l}{2}$

至此，这个博客的核心就说完啦。

别小看这两个公式，vr和vl可以认为是控制电机转速的PWM大小，这样的话就有理有根据的把v和w转换成两个电机的转速信息，妙！

然后我们就要对距离进行比例控制有：

$v=k_{1} \times \sqrt{(x-x_goal)^{2}+(y-y_goal)^{2}}$

对航向角控制

$w=k_{1} \times (\theta -arctan(\frac{(y-y_goal)}{(x-x_goal)}))$

好了，这是我加入的两个闭环控制，显然，你会发现，越靠近目标点，速度越慢。仿真结果相对于上一次真的是好太多，设定终点时(10,10)，见下图所示,一点不差走到那里，之前的差不多(10.21,10.21.)是有偏差的。

两轮差速机器人基于PID模型构建与跟踪控制（针对于ROS工程下的下位机后附linux下c++代码）_Jack Ju的博客-CSDN博客_差速pid

具体的工程源码参见我的Github：GitHub - JackJu-HIT/TrajectoryControl: 用于无人船轨迹跟踪控制

 %%%第一个航向角pid控制器，输出w
    %%%计算当前与目标的朝向误差
        theta_goal=atan((y_goal-y)/(x_goal-x));
        theta_error=theta-theta_goal;
        error_sum=error_sum+theta_error;
        w=-k*(theta_error)+ki*error_sum+kd*(theta_error-pre_error);
        pre_error=theta_error;
 
   %%%%第二个距离的pid控制器，输出v     
        dist=((x-x_goal)^2+(y-y_goal)^2)^(1/2);
        v=k2*dist;
        
 %%%%控制输入，左电机和右侧电机。
 
 vr=v+w*L/2;%控制输入需要你去更改//
 vl=v-w*L/2;%控制输入需要你去更改
 
 
 
 %%%%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;

上述是我设计的两个PID控制器，用于追踪目标的，设定起点和终点，他会自己寻求一条路线。C++版完整代码请参见我的github：GitHub - JackJu-HIT/ImuGpsGuiding: 利用IMU和GPS从指定起点到指定终点，基于ROS框架。

但是对于高更加复杂的情况不知道是我没想到还是pid确实有局限性，下一次准备研究MPC控制器。

如果觉得文章还不错，可以关注下我的微信公众号： 20200710