轨迹规划 | 图解路径跟踪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 ( t ) = K p e ( t ) + K i ∫ t 0 t e ( τ ) d τ + K d e ˙ ( t ) u\left( t \right) =K_pe\left( t \right) +K_i\int_{t_0}^t{e\left( \tau \right) \mathrm{d}\tau}+K_d\dot{e}\left( t \right) u(t)=Kpe(t)+Ki∫t0te(τ)dτ+Kde˙(t)

其中 K p K_p Kp、 K i K_i Ki、 K d K_d Kd分别称为比例、积分与微分增益系数

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

u ( k ) = K p e ( k ) + K i ∑ i = 0 k e ( i ) Δ t + K d ( e ( k ) − e ( k − 1 ) ) / Δ t u\left( k \right) =K_pe\left( k \right) +K_i\sum_{i=0}^k{e\left( i \right) \varDelta t}+K_d{{\left( e\left( k \right) -e\left( k-1 \right) \right)}/{\varDelta t}} u(k)=Kpe(k)+Kii=0∑ke(i)Δt+Kd(e(k)−e(k−1))/Δt

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

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

计算增量式PID控制律

Δ u ( k ) = K p Δ e ( k ) + K i e ( k ) Δ t + K d ( Δ e ( k ) − Δ e ( k − 1 ) ) / Δ t \varDelta u\left( k \right) =K_p\varDelta e\left( k \right) +K_ie\left( k \right) \varDelta t+K_d{{\left( \varDelta e\left( k \right) -\varDelta e\left( k-1 \right) \right)}/{\varDelta t}} Δu(k)=KpΔe(k)+Kie(k)Δt+Kd(Δe(k)−Δe(k−1))/Δt

其中

Δ u ( k ) = u ( k ) − u ( k − 1 ) Δ e ( k ) = e ( k ) − e ( k − 1 ) \varDelta u\left( k \right) =u\left( k \right) -u\left( k-1 \right) \\ \varDelta e\left( k \right) =e\left( k \right) -e\left( k-1 \right) Δu(k)=u(k)−u(k−1)Δe(k)=e(k)−e(k−1)

2 基于PID的路径跟踪

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

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

e k = sin ⁡ ( θ k , d − θ k ) ⋅ d k e_k=\sin \left( \theta _{k,d}-\theta _k \right) \cdot d_k ek=sin(θk,d−θk)⋅dk

其中

θ k , d = a tan ⁡ ( y k , d − y k , x k , d − x k ) d k = ( x k , d − x k ) 2 + ( y k , d − y k ) 2 \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} θk,d=atan(yk,d−yk,xk,d−xk)dk=(xk,d−xk)2+(yk,d−yk)2

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

3 仿真实现

3.1 ROS C++实现

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

cpp 复制代码

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;
}

cpp 复制代码

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实现

主体控制流程如下：

python 复制代码

def plan(self):
    plan_idx = 0
    for _ in range(self.max_iter):
        # break until goal reached
        if 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 point
        while plan_idx < len(self.path):
            ...
    
        # calculate velocity command
        if 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 / 10
            u = np.array([[self.linearController(v_d)], [self.angularController(theta_d)]])

        # feed into robotic kinematic
        self.robot.kinematic(u, self.dt)
    
    return False, None

3.3 Matlab实现