一、需求:
在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;
}
评论(0)
您还未登录,请登录后发表或查看评论