[人工智能]PID算法在ROS仿真环境中的理论及应用（ROS_F1TENTH）

一、需求：

在F1TENTH 仿真环境simulator中，使用PID算法实现无人车沿墙走wall_following
源码链接：https://github.com/Grizi-ju/ros_program/blob/main/wall_following_pid.cpp
（另外两个是python版本的）
B站讲解：https://www.bilibili.com/video/BV14F411v7Jw?spm_id_from=333.999.0.0
C++
ROS
阿克曼车

二、实现效果：

三、实现过程

1、搭建仿真环境
2、复制粘贴源码
3、编译并运行

1、搭建仿真环境

按照F1TENTH官网教程搭建仿真环境：https://f1tenth.readthedocs.io/en/stable/going_forward/simulator/sim_install.html

按照以上步骤可正常启动仿真环境，接下来写PID节点：

2、复制粘贴源码

源码链接：https://github.com/Grizi-ju/ros_program/blob/main/wall_following_pid.cpp
在同一工作空间下新创建一个功能包ros_code来放你自己的代码，将wall_following_pid.cpp放入src文件夹中，并在cmakelist中添加为可执行文件

3、编译并运行

启动仿真环境：roslaunch f1tenth_simulator simulator.launch
启动PID节点：rosrun ros_code wall_following_pid
运行成功，无人车在沿墙走！

四、算法理论

PID理论：
就一个公式：
kp是比例系数
kd是微分系数，是对误差进行微分。误差error的微分是 (err2 - err1)
ki是积分系数，是对误差的积分（累加），也就是误差的无限和。作用是减小静态情况下的误差，让受控物理量尽可能接近目标值。

最终让你想控制的量，等于这个公式就行，就这么简单。

wall_following官方理论：
https://linklab-uva.github.io/autonomousracing/assets/files/assgn4-print.pdf
代码中的公式、变量等都是参考该文档完成的
通过PID算法控制车始终与墙保持一定距离，沿着墙走，这个距离可根据这个公式得到，因为运动过程会产生误差，要把误差尽量消除掉，所以就要通过上面公式处理。

五、算法代码

注释已经很清楚了

/*
    Created by Ju Yuting, for bilibili tutorials , ID:小巨同学zz
    https://linklab-uva.github.io/autonomousracing/assets/files/assgn4-print.pdf
*/
#include <ros/ros.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include "math.h"

#define KP 1.00
#define KD 0.001  
#define KI 0.005
#define DESIRED_DISTANCE_RIGHT 1.0  //AB
#define DESIRED_DISTANCE_LEFT 1.0
#define LOOK_AHEAD_DIS 1.0
#define PI 3.1415927

class SubscribeAndPublish {
public:
    SubscribeAndPublish() {
        //1、发布与订阅的话题
        drive_pub = nh.advertise<ackermann_msgs::AckermannDriveStamped>("/drive", 1000);
        scan_sub = nh.subscribe("/scan", 1000, &SubscribeAndPublish::callback, this);
    }

    void callback(const sensor_msgs::LaserScan& lidar_info) {

        //2、获取激光雷达测量距离 getRange
        unsigned int b_index = (unsigned int)(floor((90.0 / 180.0 * PI - lidar_info.angle_min) / lidar_info.angle_increment));  //dist = data.ranges[int(index)]
        double b_angle = 90.0 / 180.0 * PI;    //两条射线之间的角度   
        double a_angle = 45.0 / 180.0 * PI; 
        unsigned int a_index;
        if (lidar_info.angle_min > 45.0 / 180.0 * PI) {
            a_angle = lidar_info.angle_min;
            a_index = 0;
        } else {
            a_index = (unsigned int)(floor((45.0 / 180.0 * PI - lidar_info.angle_min) / lidar_info.angle_increment));
        }            
        double a_range = 0.0;   
        double b_range = 0.0;
        if (!std::isinf(lidar_info.ranges[a_index]) && !std::isnan(lidar_info.ranges[a_index])) {   //isinf()是否为无穷大   ranges：转一周的测量数据一共360个，每度是一个
            a_range = lidar_info.ranges[a_index];   //得到a的长度
        } else {
            a_range = 100.0;
        }
        if (!std::isinf(lidar_info.ranges[b_index]) && !std::isnan(lidar_info.ranges[b_index])) {
            b_range = lidar_info.ranges[b_index];   //得到b的长度
        } else {
            b_range = 100.0;
        }
     
        //3、计算公式，参考pdf  
        //在车的右边得到两条射线a、b来确定车到右墙的距离AB和相对于AB的方向
        double alpha = atan((a_range * cos(b_angle - a_angle) - b_range) / (a_range * sin(b_angle - a_angle)));   //b_angle - a_angle为a、b夹角THETA
        double AB = b_range * cos(alpha);    //实际离右墙距离
        double projected_dis = AB + LOOK_AHEAD_DIS * sin(alpha);     //Due to the forward motion of the car and a finite delay in execution of the control and perception nodes; 
                                                                     //we instead, virtually project the car forward a certain distance from its current position.  
                                                                     //LOOK_AHEAD_DIS = AC = 1   projected_dis = CD
        error = DESIRED_DISTANCE_RIGHT - projected_dis;   //求出误差
        ROS_INFO("projected_dis = %f",projected_dis);
        ROS_INFO("error = %f",error);
        ROS_INFO("del_time = %f\n",del_time);
        SubscribeAndPublish::pid_control();
    }

        //4、PID控制器
    void pid_control() {
        ackermann_msgs::AckermannDriveStamped ackermann_drive_result;
        double tmoment = ros::Time::now().toSec();
        del_time = tmoment - prev_tmoment;   //当前时刻-上一个时刻 = 间隔时刻
        integral += prev_error * del_time;   //对误差积分，也就是误差的无限和  积分=积分+累计误差
        ackermann_drive_result.drive.steering_angle = -(KP * error + KD * (error - prev_error) / del_time + KI * integral);
        prev_tmoment = tmoment;    //时间的迭代

        //不同情况下的速度调整，转弯时速度降低，直行时速度加快
        if (abs(ackermann_drive_result.drive.steering_angle) > 20.0 / 180.0 * PI) {
            ackermann_drive_result.drive.speed = 2.0;
            //speed = 1.3;
        } else if (abs(ackermann_drive_result.drive.steering_angle) > 10.0 / 180.0 * PI) {
            ackermann_drive_result.drive.speed = 3.0;
            //speed = 2.0;
        } else {
            ackermann_drive_result.drive.speed = 5.0;
            //speed = 3.0;
        }
        drive_pub.publish(ackermann_drive_result);
    }

private:
    ros::NodeHandle nh;
    ros::Publisher drive_pub;
    ros::Subscriber scan_sub;
    double prev_error = 0.0;    //前一个误差
    double prev_tmoment = ros::Time::now().toSec();    //当前时间，单位秒s
    double error = 0.0;
    double integral = 0.0;
    double speed = 0.0;
    double del_time = 0.0;

}; 

int main(int argc, char** argv) {

    ros::init(argc, argv, "wall_following_pid");
    SubscribeAndPublish SAPObject;
    ros::spin();
    return 0;
}