路径跟踪算法---PID实现

---

前言

在获取机器人路径以后，下一步任务就是控制机器人沿着给定的路径进行运动，常用的控制算法有PID控制、纯路径跟踪算法（PurePursuit）、线性二次型调节器（LQR）、模型预测控制（MPC）等，本文首先介绍如何使用PID算法路径跟踪。

---

一、PID控制

1. PID简介

PID算法是一种经典、简单、高效的动态速度调节方式，Kp为比例系数，Ki积分系数，Kd为微分系数。
$$U(t)=K_p\ast e(t)+K_i{\int }_0^{t}e(t)d(t)+K_{d}d(e_t)/dt$$
通常依据控制器输出与执行机构的对应关系，将基本数字PID算法分为位置式PID和增量式PID两种，参考博客：空虚法师
a.Kpa.err + a.Kia.integral + a.Kd*(a.err-a.err_last);
位置式PID：
$$K_p\ast e(k)+K_i\ast \sum _{i=0}e(i)+K_d\ast (e(k)-e(k-1))$$
e(k)：误差 = 目标值 - 当前值
$\sum e(i)$：误差累加
e(k) - e(k-1)： 本次误差- 上次误差
增量式PID：
$$K_p\ast (e(k)-e(k-1))+K_i\ast e(k)+K_d(e(k)-2e(k-1)+e(k-2))$$

e(k) - e(k-1)：本次误差- 上次误差
e(k)：误差
e(k) - 2e(k-1) + e(k-2)：本次误差- 2*上次误差 + 上上次误差

增量式PID不需要误差累加，之和最近几次的误差有关，位置式PID算法需要误差累加，容易产生较大的累加误差。

顺便简单复习下PID的调速，可以忽略

1.1 P（比例）调节：

比例调节是一个循环过程，以移动速度为例，不断的修正目标速度和当前速度的误差，使得速度波动越来越小，最终达到稳态速度，但不会达到目标速度，产生稳态误差。
$$k_p\ast (目标速度-当前速度)=PWM（能够达成的稳态速度）$$
举例：假定Kp为2，目标速度为1m/s，当前机器静止，速度为0，最大速度为2m/s；加速过程：若无周期限制和测量值，则经过P调节，速度可达到2m/s，并一直按照2m/s的速度运动，由于存在周期限制和测量器件，在速度到达某值时会进入下一个周期。
  第1次：Kp*(1-0) = 2, 机器加速，进入第二个周期，速度到达1.2m/s
  第2次：Kp*(1-1.2) <0 ，机器减速趋于0，进入第三个周期，速度为0.6m/s
  第3次：Kp*(1-0.6) = 0.8,加速，进入第四个周期；
  。。。
比例环节可以使得速度尽快的到达目标速度；最终，速度会在某个值波动。

1.2 PI 调节:

$$K_p\ast (目标速度-当前速度)+K_i\ast (er{r}_1+er{r}_2+...+er{r}_n)$$
消除稳态误差，每次执行P调节时，会不断累加计算I运算的结果，存在超调，且会震荡
$$K_i\ast (er{r}_1+er{r}_2+...+er{r}_n)$$
接上例：每个周期存在速度误差，目标速度-当前速度
                     Ki * (-0.2 + 0.4 + …)
  第k次：此时速度达到0.9m/s, 积分环节计算：Ki*(k-1次的误差 + 0.1)
  第K+1次：速度达到0.92m/s, 积分环节计算： Ki*(k-1次的误差 + 0.1+ 0.08)
  第K+2次：速度达到0.96m/s, 积分环节计算：Ki*(k-1次的误差 + 0.1+ 0.08 + 0.04)
   。。。
在加速过程，速度不断增大，会超过目标速度的值；同时一直起作用，引起系统震荡。

1.3 PID 调节:

每个周期执行完比例环节、积分环节后、再累加微分环节。接近目标速度时，微分环节可减少积分环节的增益，减小震荡。
$$K_d\ast (本次误差-上次误差)$$

接上例：
  第K+1次：本次误差0.08，上次误差0.1; 微分环节计算结果：Kd*(0.08 - 0.1) <0;
  第K+2次：本次误差0.04，上次误差0.08; 微分环节计算结果：Kd*(0.04 - 0.08) <0;
   。。。
微分环节 阻止偏差的变化，根据偏差的变化趋势（变化速度）进行控制。偏差变化的越快，微分控制器的输出就越大，并能在偏差值变大之前进行修正。微分作用的引入， 将有助于减小超调量， 克服振荡， 使系统趋于稳定。

2. 算法实现

本文使用gazebo+ros仿真实现PID算法对路径进行跟踪实验。需要接收机器人位置信息、路径信息以及速度信息，机器人位置信息。平面上机器人运动一般需要提供线速度和角速度，线速度采用增量式pid进行计算，通过给定期望线速度给增量式pid函数，获取线速度；角速度采用位置式pid计算 (只为演示说明，具体使用哪种pid算法可自行调试，参考：于晓沐)

同样地，需要接收期望的机器人路径信息，可参考之前的博客

2.1 pid_path_follow.h

定义两个PID类，分别为LinearPID线速度PID类和AngularPID角速度PID类，用于声明PID相关的参数，包含误差、上次误差、上上次误差、设定速度、实际速度、积分值、以及Kp, Ki, Kd。

    class LinearPID{
    public:
        LinearPID()
        {
            linear_err = 0.0;
            linear_err_last = 0.0;
            linear_err_l_last = 0.0;
            actual_linear_vel = 0.0;
        }
        double linear_err;   //误差
        double linear_err_last; //上次误差
        double linear_err_l_last; //上上次误差
        double Kp,Ki,Kd;
        double actual_linear_vel;  //实际速度
        double set_vel = 0.0;  //设定速度
    };
  
    class AngularPID{
    public:
        AngularPID()
        {
            angular_err = 0.0;
            angualr_err_last = 0.0;
            integral = 0.0;
            actual_angular_vel = 0.0;
        }
        double angular_err ;  //误差
        double angualr_err_last ;  //上次误差
        double integral ;   //误差累计
        double Kp, Ki, Kd;
        double actual_angular_vel ; //实际速度
        double set_angualr_vel = 0.0; //设定速度
    };

再定义一个Control类，用于pid算法的实现，包含机器人位置函数、路径接收函数、速度接收函数、PID计算速度函数等。通过获取机器人当前位置与目标路径点位置，计算机器人的线速度和角速度，并设定定时器进行更新计算。

   class Control{
    public:
        Control();
    private:
        void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& amcl_pose);
        void robotPathCallBack(const nav_msgs::PathConstPtr& path_msg);
        void robotVelCallBack(const geometry_msgs::Twist::ConstPtr& vel_msg);
        
        void calculate_vel();
        void update_point();
        double calculate_angular();                     
        
        double linearPidControl(double goal_vel, LinearPID &linear);
        double angularPidControl(double angular_vel, AngularPID &angular);

        void controlLoopCB(const ros::TimerEvent& );

        inline double distance(double x1, double y1, double x2, double y2)
        {
            return std::sqrt((x1-x2)* (x1-x2) + (y1 - y2) * (y1 -y2));
        }
    private:
        ros::NodeHandle nh_;
        ros::Publisher cmd_vel_pub_;
        ros::Publisher path_end_point_;
        ros::Subscriber amcl_pose_sub_;
        ros::Subscriber path_sub_;
        ros::Subscriber cmd_vel_sub_;
        
        ros::Timer timer_;
        double t_ ;
        int point_i_, controller_freq_;
        geometry_msgs::Pose current_pose_, goal_pose_, end_pose_;
        nav_msgs::Path nav_path_;
        geometry_msgs::Twist cmd_vel_;
        LinearPID linear_vel_;
        AngularPID angular_vel_;
        bool recv_path_flag_ , nav_flag_;
        double want_vel_;
    };

2.1 pid_path_follow.cpp

pid_path_follow.h的函数实现，首先在构造函数中 初始化变量，使用参数服务器来加载参数，便于更好的调试更改pid参数以及期望速度。将机器人的第一个目标位置设定为机器人当前位置，或者直接设置为(0,0)。path_sub_()为期望路径接收函数，接收期望的机器人运动路径，便于机器人进行路径跟踪。
话题"/path_pub"，是路径发布节点的话题名；"/ackermann_steering_controller/cmd_vel"是速度话题。注意对应自己的

 Control::Control(){

        recv_path_flag_ = true;
        nav_flag_ = false;
        point_i_ = 0;  //路径点变量

        ros::NodeHandle private_node("~");
        private_node.param<int>("controller_freq",controller_freq_,100);
        private_node.param<double>("linear_Kp",linear_vel_.Kp,0.0001);
        private_node.param<double>("linear_Ki",linear_vel_.Ki,0.2);
        private_node.param<double>("linear_Kd",linear_vel_.Kd,0.3);

        private_node.param<double>("Angular_Kp",angular_vel_.Kp, 0.02);
        private_node.param<double>("Angular_Ki",angular_vel_.Ki,0.2);
        private_node.param<double>("Angular_Kd",angular_vel_.Kd,0.002);

        private_node.param<double>("t",t_,5);
        private_node.param<double>("want_vel",want_vel_,0.3);
        
        goal_pose_.position.x = 0.0;
        goal_pose_.position.y = 0.0;

        amcl_pose_sub_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/amcl_pose",10,&Control::robotPoseCallBack,this);
        cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/ackermann_steering_controller/cmd_vel",1);
        path_sub_ = nh_.subscribe<nav_msgs::Path>("/path_pub",1,&Control::robotPathCallBack,this);

        timer_ = nh_.createTimer(ros::Duration((1.0)/controller_freq_),&Control::controlLoopCB,this);
    }

机器人位置接收函数实现：使用amcl发布的定位信息获取机器人当前位置

    void Control::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& amcl_pose)
    {
        current_pose_.position.x = amcl_pose->pose.pose.position.x;
        current_pose_.position.y = amcl_pose->pose.pose.position.y;
        current_pose_.orientation = amcl_pose->pose.pose.orientation;
    }

机器人路径接收函数实现：只需要接收一次信息即可，同时将路径终点保存，以便控制机器人路径跟踪结束后停止。

    void Control::robotPathCallBack(const nav_msgs::PathConstPtr& path_msg)
    {
        if(recv_path_flag_)
        {
            nav_path_.poses = path_msg->poses;
            recv_path_flag_ = false;
            nav_flag_ = true;
            end_pose_.position.x = nav_path_.poses[nav_path_.poses.size()-1].pose.position.x;
            end_pose_.position.y = nav_path_.poses[nav_path_.poses.size()-1].pose.position.y;
        }
    }

机器人实际速度接收函数：通过获取实际速度，便于pid计算期望速度与实际速度的误差，下发控制速度。

    void Control::robotVelCallBack(const geometry_msgs::Twist::ConstPtr& vel_msg)
    {
        this->linear_vel_.actual_linear_vel = vel_msg->linear.x;
        this->angular_vel_.actual_angular_vel = vel_msg->angular.z;
    }

线速度计算实现：增量式PID计算线速度

    double Control::linearPidControl(double goal_vel, LinearPID &linear)
    {
        linear.set_vel = goal_vel;
        linear.linear_err = linear.set_vel - linear.actual_linear_vel;  //误差计算
        double increment = linear.Kp* (linear.linear_err) + linear.Ki * linear.linear_err 
                            + linear.Kd * (linear.linear_err - 2 * linear.linear_err_last + linear.linear_err_l_last);  //PID计算

        linear.actual_linear_vel += increment ; //速度增量累计
        linear.linear_err_l_last = linear.linear_err_last; //误差更新
        linear.linear_err_last = linear.linear_err;
        return linear.actual_linear_vel;
    }

角速度计算实现：位置式PID计算角速度

    double Control::angularPidControl(double angular_vel, AngularPID &angular)
    {
        angular.set_angualr_vel = angular_vel;
        angular.angular_err = angular.set_angualr_vel - angular.actual_angular_vel;  //误差计算
        angular.integral+= angular.angular_err;  //积分
        double voltage = angular.Kp*angular.angular_err + angular.Ki* angular.integral + angular.Kd*(angular.angular_err - angular.angualr_err_last);  //PID实现
        angular.angualr_err_last = angular.angular_err;  //误差更新
        angular.actual_angular_vel = voltage * 1.0;
        return angular.actual_angular_vel;
    }

机器人转向角计算：

    double Control::calculate_angular()
    {
        //目标点 在机器人坐标系下的向量
        double corrd_x1 = goal_pose_.position.x - current_pose_.position.x;
        double corrd_y1 = goal_pose_.position.y - current_pose_.position.y;

        tf::Quaternion quat;
        tf::quaternionMsgToTF(current_pose_.orientation,quat);
        double roll, pitch, yaw;
        tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);
        //机器人朝向向量
        double corrd_x2 = std::cos(yaw);
        double corrd_y2 = std::sin(yaw);
        //机器人的转向角
        double Dot_Product = corrd_x1 * corrd_x2 + corrd_y1 * corrd_y2; //点乘
        double temp = Dot_Product / std::sqrt(pow(corrd_x1,2)+pow(corrd_y1,2)); //夹角余铉
        double alpha = std::acos(fminl(fmaxl(temp,-1.0),1.0));  //计算夹角
        //叉乘判断方向，正右转 ，负左转
        double Product = corrd_x1 * corrd_y2 - corrd_x2 * corrd_y1; //叉乘正负决定转向
        if(Product > 0) alpha = -alpha;
        return alpha;
    }

更新跟踪的路径点：机器人当前位置和目标位置（路径点）距离小于某个阈值视为到达，即进行下一个路径点更新

    void Control::update_point()
    {
        if(nav_flag_)
        {
            nav_flag_ = true;
            goal_pose_.position.x = nav_path_.poses[point_i_].pose.position.x;
            goal_pose_.position.y = nav_path_.poses[point_i_].pose.position.y;
            double dis = distance(current_pose_.position.x, current_pose_.position.y, goal_pose_.position.x, goal_pose_.position.y);
            if(dis < 0.2)
                point_i_ ++;
        }
    }

计算下发速度：

    void Control::calculate_vel()
    {
        update_point();
        cmd_vel_.linear.x = linearPidControl(want_vel_,linear_vel_);
        double w = calculate_angular() * t_;
        cmd_vel_.angular.z = angularPidControl(w, angular_vel_);
        double dis = distance(current_pose_.position.x, current_pose_.position.y,end_pose_.position.x,end_pose_.position.y);
        //当机器人到达终点后停止，零速度下发。
        if(dis < 0.1 && point_i_ > 10)
        {
            cmd_vel_.linear.x = 0;
            cmd_vel_.angular.z = 0;
        }
    }

定时器更新：调用速度计算函数，并发布速度

    void Control::controlLoopCB(const ros::TimerEvent&)
    {
        calculate_vel();
        cmd_vel_pub_.publish(cmd_vel_);
    }

over~~~,完整代码：pid_path_tracking
倍速效果如下：（仓库中已添加循环跟踪参数，实现循环运动）