轨迹规划 | 图解路径跟踪PID算法(附ROS C++/Python/Matlab仿真)

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 PID控制基本原理

PID控制是一种常用的经典控制算法，其应用背景广泛，例如

• 工业自动化控制：温度控制、压力控制、流量控制、液位控制等过程控制系统多采用PID闭环，可以帮助维持系统参数在设定值附近，以提高生产过程的稳定性和效率；
• 机械工程：PID算法可用于实现精确的运动控制，包括控制位置、速度和力。这包括机器人控制、电机控制、汽车巡航控制等；
• 农业自动化：PID算法可用于控制温室环境，包括温度、湿度和光照，以促进植物的生长和提高农业生产；
• …

PID代表比例(Proportional)、积分(Integral)和微分(Derivative)，它通过根据误差信号的大小和变化率来调整控制器的输出，以使系统的输出尽可能接近期望值，其控制框图如下所示

连续型PID控制律如下

$$u\left(t\right)=K_{p}e\left(t\right)+K_i{\int }_{t_0}^{t}e\left(\tau \right)\mathrm{d}\tau +K_d\dot{e}\left(t\right)$$

其中$K_p$、$K_i$、$K_d$分别称为比例、积分与微分增益系数

位置式离散型PID控制律如下

$$u\left(k\right)=K_{p}e\left(k\right)+K_i\sum _{i=0}^{k}e\left(i\right)\Delta t+K_d\left(e\left(k\right)-e\left(k-1\right)\right)/\Delta t$$

由于位置式PID算法需要计算累计偏差，占用存储单元，可以通过

$$u\left(k\right)-u\left(k-1\right)$$

计算增量式PID控制律

$$\Delta u\left(k\right)=K_p\Delta e\left(k\right)+K_{i}e\left(k\right)\Delta t+K_d\left(\Delta e\left(k\right)-\Delta e\left(k-1\right)\right)/\Delta t$$

其中

$$\begin{gathered} \Delta u\left(k\right)=u\left(k\right)-u\left(k-1\right) \\ \Delta e\left(k\right)=e\left(k\right)-e\left(k-1\right) \end{gathered}$$

2 基于PID的路径跟踪

在基于PID的局部路径规划中，希望机器人能快速跟踪上预设的轨迹，设误差量为$e_k$。$e_k$可以根据实际的控制目标进行选择，例如线速度误差、角速度误差、轨迹跟踪误差等

以轨迹跟踪误差为例，如图所示，根据几何关系可得

$$e_k=\sin \left({\theta }_{k,d}-{\theta }_k\right)\cdot d_k$$

其中

$$\begin{gathered} {\theta }_{k,d}=\mathrm{a}\tan \left(y_{k,d}-y_k,x_{k,d}-x_k\right) \\ {d}_k=\sqrt{{\left(x_{k,d}-x_k\right)}^2+{\left(y_{k,d}-y_k\right)}^2} \end{gathered}$$

接着以该误差作为反馈测量值通过PID控制器生成控制量，机器人基于控制量和运动学模型运动，循环往复直到机器人完成控制目标

3 仿真实现

3.1 ROS C++实现

核心的线速度PID控制和角速度PID控制代码如下

double PIDPlanner::LinearPIDController(nav_msgs::Odometry& base_odometry, double b_x_d, double b_y_d)
{double v = std::hypot(base_odometry.twist.twist.linear.x, base_odometry.twist.twist.linear.y);double v_d = std::hypot(b_x_d, b_y_d) / d_t_;if (std::fabs(v_d) > max_v_)v_d = std::copysign(max_v_, v_d);double e_v = v_d - v;i_v_ += e_v * d_t_;double d_v = (e_v - e_v_) / d_t_;e_v_ = e_v;double v_inc = k_v_p_ * e_v + k_v_i_ * i_v_ + k_v_d_ * d_v;if (std::fabs(v_inc) > max_v_inc_)v_inc = std::copysign(max_v_inc_, v_inc);double v_cmd = v + v_inc;if (std::fabs(v_cmd) > max_v_)v_cmd = std::copysign(max_v_, v_cmd);else if (std::fabs(v_cmd) < min_v_)v_cmd = std::copysign(min_v_, v_cmd);return v_cmd;
}

double PIDPlanner::AngularPIDController(nav_msgs::Odometry& base_odometry, double e_theta)
{regularizeAngle(e_theta);double w_d = e_theta / d_t_;if (std::fabs(w_d) > max_w_)w_d = std::copysign(max_w_, w_d);double w = base_odometry.twist.twist.angular.z;double e_w = w_d - w;i_w_ += e_w * d_t_;double d_w = (e_w - e_w_) / d_t_;e_w_ = e_w;double w_inc = k_w_p_ * e_w + k_w_i_ * i_w_ + k_w_d_ * d_w;if (std::fabs(w_inc) > max_w_inc_)w_inc = std::copysign(max_w_inc_, w_inc);double w_cmd = w + w_inc;if (std::fabs(w_cmd) > max_w_)w_cmd = std::copysign(max_w_, w_cmd);else if (std::fabs(w_cmd) < min_w_)w_cmd = std::copysign(min_w_, w_cmd);return w_cmd;
}

3.2 Python实现

主体控制流程如下：

def plan(self):plan_idx = 0for _ in range(self.max_iter):# break until goal reachedif math.hypot(self.robot.px - self.goal[0], self.robot.py - self.goal[1]) < self.p_precision:return True, self.robot.history_pose# find next tracking pointwhile plan_idx < len(self.path):...# calculate velocity commandif math.hypot(self.robot.px - self.goal[0], self.robot.py - self.goal[1]) < self.p_precision:if abs(self.robot.theta - self.goal[2]) < self.o_precision:u = np.array([[0], [0]])else:u = np.array([[0], [self.angularController(self.goal[2])]])elif abs(theta_d - self.robot.theta) > np.pi / 2:u = np.array([[0], [self.angularController(theta_d)]])else:v_d = math.hypot(b_x_d, b_y_d) / self.dt / 10u = np.array([[self.linearController(v_d)], [self.angularController(theta_d)]])# feed into robotic kinematicself.robot.kinematic(u, self.dt)return False, None

3.3 Matlab实现

核心的线速度PID控制和角速度PID控制代码如下

function [v, e_v_, i_v_] = linearController(robot, b_x_d, b_y_d, dt, e_v_, i_v_)v_d = norm([b_x_d, b_y_d]) / dt / 10;e_v = v_d - robot.v;i_v_ = i_v_ + e_v * dt;d_v = (e_v - e_v_) / dt;e_v_ = e_v;k_v_p = 1.00;k_v_i = 0.00;k_v_d = 0.00;v_inc = k_v_p * e_v_ + k_v_i * i_v_ + k_v_d * d_v;v = robot.v + v_inc;
end

function [w, e_w_, i_w_] = angularController(robot, theta_d, dt, e_w_, i_w_)e_theta = theta_d - robot.theta;if (e_theta > pi)e_theta = e_theta - 2 * pi;elseif (e_theta < -pi)e_theta = e_theta + 2 * pi;endw_d = e_theta / dt / 10;e_w = w_d - robot.w;i_w_ = i_w_ + e_w * dt;d_w = (e_w - e_w_) / dt;e_w_ = e_w;k_w_p = 1.00;k_w_i = 0.00;k_w_d = 0.01;w_inc = k_w_p * e_w_ + k_w_i * i_w_ + k_w_d * d_w;w = robot.w + w_inc;
end

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇