在此，总结一下自己半年的项目经验，本人第一次写博客，写作内容难免有错。技术水平有限，仅能实现简单的自动驾驶避障、跟车等功能，望大家多多指教~
在项目中本人负责的是自动驾驶的决策规划控制算法部分，在研发过程中负责对接上层感知定位模块和下层线控系统，项目实现了在园区场景下的自动驾驶。实现的场景包括单障碍物换道避障，多障碍物换道避障，跟车行驶，停车等待行人等。
接下来会分享以下的几部分内容：横纵向控制部分和决策规划部分。以下是实车测试视频和仿真示意图：
1、横纵向控制
基于本项目的低速园区场景，横纵向控制分别采用纯跟踪和PID算法进行。
1.1纯跟踪算法
图中(gx,gy)是要追踪的路点，需要控制车辆的后轴中心点经过要追踪的点，ld表示车辆当前位置(即后轴位置)到目标路点的距离， 表示目前车身朝向与目标路点之间的角度，由正弦定理可以推导出如下转换式
上式也可表达为：
K是计算出来的圆弧的曲率，车辆轴距用L表示，则前轮的转角可表示为：
结合以上两式可得纯追踪控制算法控制量表达式：
上式就是纯跟踪算法的输出表达式，输出量为车辆前轮转角 δ 。
为了更好的理解影响纯跟踪算法精度的因素，定义车辆当前位姿 S 和目标点 G 的横向误差 可得 α正弦:
转角 δ 可以重写为：
由于轴距 L 是常数，横向误差 是自变量，因此因变量 δ 的控制效果主要取决于前视距离
。本文将前视距离
视作是车速和道路曲率的一次函数，即
，那么前轮的转角公式就变为：
其中， 为最小前视距离，本文的最小前视距离大小根据道路曲率设置：如果道路曲率太大，则减小
，使追踪更加精确；如果道路曲率较小，则增大
，使轨迹追踪更平滑。
以下为项目中纯跟踪算法 部分代码：
首先，根据实车的参数定义相关变量：
#define K (1.0) // 前视距离系数
#define L_VEHICLE (2.85) // 车身长度 (前轮-后轮长度，m)
double lfc = 1.0; //前视距离其中，前视距离在根据道路曲率进行定义，当曲率大于0.03时（弯道），lfc = 1.5m，其他情况lfc = 2.0，代码如下：
//通过曲率求解 curvature_msg表示接受到的曲率
void pure_pursuit::curvature_callback(const common_msgs::processed_object_array &det_arr){
if(det_arr.lane_curve >= 0.03 )
{
lfc = 1.0;
vehcleCtr.speed = 1.5;
}
else
{
lfc = 1.5;
vehcleCtr.speed = 2.5;
}
}订阅位置信息和曲率信息
//接受信息定位
vehicle_location = node.subscribe("/geometry_pose", 2, &pure_pursuit::location_update, (pure_pursuit *) this);
//接收曲率
curve_sub = node.subscribe("/obstacles_info_in_lane", 1, &pure_pursuit::curvature_callback, (pure_pursuit *) this);接收定位传来的当前位姿，包括世界坐标系坐标和四元数，用ros的tf::Quaternion库将四元数转为 roll，pitch 和 yaw，得到航向角yaw：
//用PoseStamped更新位姿
void pure_pursuit::location_update(const geometry_msgs::PoseStamped::ConstPtr locat) {
vehicleState.x = locat->pose.position.x;
vehicleState.y = locat->pose.position.y;
//表示位姿是否更新，默认为false
vehLocUpdatRdy = true;
//state.z = locat->z;
tf::Quaternion quat(locat->pose.orientation.x, locat->pose.orientation.y, locat->pose.orientation.z,
locat->pose.orientation.w);
// 用tf::Quaternion库将四元数转为 roll pitch 和 yaw
double roll, pitch, yaw;
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
vehicleState.yaw = yaw;
}
计算前视距离路径上点的个数：
//计算前视距离路径上点的个数
int pure_pursuit::get_goal_index(const State &s, const vector <Point> &path) {
vector<double> d;
for (int i = 0; i < path.size(); i++)//轨迹点计数
d.push_back(pow((s.x - path[i].x), 2) + pow((s.y - path[i].y), 2));//距离计算
int index = 0;
double d_min = d[0];
int dVecLen = d.size();
for (int i = 0; i < dVecLen; i++) {
if (d_min > d[i]){
d_min = d[i];
//当前车辆距离起点位置2-5m时，将纯跟踪跟踪的点初始化为0，实现环形道路绕圈的功能
if((pow((s.x - path[0].x), 4) + pow((s.y - path[0].y), 2)) < 25 && i > dVecLen / 2){
index = 0;
i = 0;
}
else{
index = i;
}
}
}
//ROS_INFO("DEBUG 1 : index = %d",index);
ROS_INFO_THROTTLE(1, "DEBUG : index = %d, d_min = %.2f", index, d_min);
double l = 0;
double lf = K * s.speed + lfc;
double dx_, dy_;
while (l < lf && index < path.size()) {
dx_ = path[index + 1].x - path[index].x;
dy_ = path[index + 1].y - path[index].y;
l += sqrt(dx_ * dx_ + dy_ * dy_);
index++;
}
return index;
}纯跟踪核心代码：
//根据目标进行转角控制
double pure_pursuit::pure_pursuit_control(const State &s, const vector <Point> &path, int *lastIndex) {
int index = get_goal_index(s, path); // 搜索目标点，返回目标点的标签
// 用上一个循环的目标点判断是否是在向前走
if (index <= *lastIndex) {
index = *lastIndex;
}
Point goal;
//防止index溢出
if (index < path.size()) {
goal = path[index];
} else {
index = path.size() - 1;
goal = path[index];
}
// 车身坐标系的x轴和目标点与车身坐标系原点连线的夹角
double alpha = atan2(goal.y - s.y, goal.x - s.x) - s.yaw;
if (s.speed < 0)
alpha = M_PI - alpha;
double lf = K * s.speed + L0; // 根据车速和道路曲率设置前视距离
// delta 即为纯跟踪算法的最终输出
double delta = atan2((2.0 * L_VEHICLE * sin(alpha)) / lf, 1.0);
*lastIndex = index; // 为下一个循环更新上一个目标点
return delta;
}跟踪效果如图：
平均误差：6.09cm 最大误差： 21.78cm 最小误差：0cm
1.2 PID纵向控制
pid的算法原理就不讲了，不清楚的可以先看文末参考链接。直接上项目中PID的部分代码。
首先构造一个PID_position的类以及一些需要调用的函数：
class PID_position
{
private:
float kp; //比例系数
float ki; //积分系数
float kd; //微分系数
float target; //目标值
float actual; //实际值
float e; //误差
float e_pre; //上一次误差
float integral; //积分项
float integral_limit; //积分项限制
public:
PID_position();
~PID_position(){};
PID_position(float p, float i, float d); //构造函数
float pid_control(float tar, float act); //执行PID控制
void set_pid_param(float kp, float ki, float kd); //设置pid参数
void pid_show(); //显示PID控制器的内部参数
void set_integral_limit(float limit_value); //设置积分项的限值
void reset(); //重置内部的值
};设置参数，其中重要的就是kp、ki、kd三个参数：
priv_node.param("pid_controller_kp", config_.pid_controller_kp_, double(3.0));
priv_node.param("pid_controller_ki", config_.pid_controller_ki_, double(0.05));
priv_node.param("pid_controller_kd", config_.pid_controller_kd_, double(0.01));PID核心代码：
float PID_position::pid_control(float tar, float act)
{
float u;
target = tar;
actual = act;
e = target - actual;
integral += e;
if ((ki * integral) > integral_limit)
{
integral = integral_limit / ki; //最大积分项限制
}
// else if (integral < (-1 * integral_limit))
// {
// /* code */
// integral = -1 * integral_limit;
// }
u = kp * e + ki * integral + kd * (e - e_pre);
e_pre = e;
return u;
}最终控制效果（太久没上车了，之前录制的速度时间图）：
本文设置的控制车速为2m/s，可以看出，效果其实一般，但是受益于低速的工况，项目中是能用的。还有很多点可以进行优化：比如说制作油门刹车表定标、调整kp、ki、kd参数、对车辆反馈的实时车速进行滤波等。
2、决策规划
这里就将决策规划在一起讲解。本文采用的是基于有限状态机的决策和基于贝塞尔曲线进行局部路径规划。
参考了BOSS无人车决策框架，减去了原版的部分内容。主要状态分为：匀速行驶、跟车行驶、停车等待、道路选择和变道行驶。
部分代码如下：
根据实际场景定义全局变量（道路、车辆自身参数、跟车参数等）：
float d = 1.5; //d为车和车道线间的距离
float l = 2.5; //l为车道线宽度
float car_width = 1.70; //车轮距
float car_length = 3.0; //车轴距
float downLine = -d; //下车道线
float upLine = 2*l-d; //上车道线
float middleLine = l-d; //中间车道线
float safeArea = 10; //人与车的安全距离
float walkRoad = 3;
float FollowArea = 20; //跟车距离范围：（safeArea，FollowArea）
float StopArea = 5.0;
float uniform_speed = 2;
float follow_speed = uniform_speed;
bool flag_1_2 = true;
bool flag_2_1= true;
bool flag_2= true;
bool flag3 = true;
bool obstacle_flag_1 = true;订阅地图模块发出的全局路径信息、障碍信息（Frenet坐标）、GPS位置信息
//障碍物信息
sub_obstacles = node.subscribe("/obstacles_info_in_lane", 1, &fsm_decisionmaking::obstacles_callback, (fsm_decisionmaking *) this);
//GPS
sub_gps = node.subscribe("/geometry_pose", 1, &fsm_decisionmaking::gps_callback, (fsm_decisionmaking *) this);
//全局路径信息
sub_p5_path = node.subscribe("/map_path", 1, &fsm_decisionmaking::p5_path_callback, (fsm_decisionmaking *) this);停车、匀速行驶、道路选择状态：
//停车等待状态
void fsm_decisionmaking::event_StopAndWait()
{
vehcleCtr_target.speed = 0;
vehcleCtr_target.manFlag = true;
vehcleCtr_target.header.stamp = ros::Time::now();
cout<<endl<<"STOP AND WAIT"<<endl<<endl;
}
//匀速行驶状态
void fsm_decisionmaking::event_ConstantSpeed()
{
vehcleCtr_target.speed = uniform_speed;
vehcleCtr_target.manFlag = true;
vehcleCtr_target.header.stamp = ros::Time::now();
}
//道路选择状态
int fsm_decisionmaking::event_LaneSelector(){
int Lane_now=0;
LaneVehicleIn(offlinepath_1,offlinepath_2,GPS_vehicle.x,GPS_vehicle.y,Lane_now);
cout<<"lane_now_换道 = "<<Lane_now<<endl;
int LaneSelect=0;
//如果车在1，2上没障碍物，就选2
if((Lane_now==1)&&(Lane_2_Obstacle<1))
LaneSelect=2;
//如果车在2，1上没障碍物，就选1
else if((Lane_now==2)&&(Lane_1_Obstacle<1))
LaneSelect=1;
else if(Lane_now==0){LaneSelect=0;}
else{
event_StopAndWait();
}
cout<<"Lane_Select = "<<LaneSelect<<endl;
return LaneSelect;
}变道行驶工况，基于Frenut坐标下障碍物的位置坐标和经过边缘膨胀的障碍物大小等信息进行决策。
//障碍物边框向外膨胀0.5m作为安全距离预留
float obs_L = boxarray->objects[i].frenet_distance+(boxarray->objects[i].width/2)+1.5;//驾驶员视角下障碍物检测框左边缘点，向左膨胀0.5m
float obs_R = boxarray->objects[i].frenet_distance-(boxarray->objects[i].width/2)-1.5;//驾驶员视角下障碍物检测框右边缘点，向右膨胀0.5m
float obs_L_upLine = abs(obs_L-upLine);//obs_L点到上车道线的距离
float obs_R_downLine = abs(obs_R-downLine);//obs_R到上下道线的距离
float obs_L_middleLine = abs(obs_L-middleLine);//obs_L点到中间车道线
float obs_R_middleLine = abs(obs_R-middleLine);//obs_R到中间车道线//道路选择状态
int LaneSelect = event_LaneSelector();
//cout<<"LaneSelect="<<LaneSelect<<" Lane_now="<<Lane_now<<endl;
//设可通行宽度为2m
if(obs_R_downLine>=2.4 && flag_2)//从障碍物右边绕行
{
//对绕行轨迹进行局部规划
//LaneSelect=2; //(定义为在一车道上的2路径)
//选定P5点frenet坐标下的d坐标为obs_R与downLine的中点
p_5.y = obs_R-(obs_R_downLine/2);
p_5.x = boxarray->objects[i].frenet_length;
P5_pub.publish(p_5);
// }
}
else if(obs_L_upLine>=2.75 && flag_2)//从障碍物左边绕行,p5落在左车道上
{
//选定P5点frenet坐标下的d坐标为obs_L向左偏移1.5m
p_5.y = obs_L+1.5;
p_5.x = boxarray->objects[i].frenet_length;
P5_pub.publish(p_5);
flag_2 = false;
}
cout<<"p5.frenet_length = "<<p_5.x<< " "<<"p5.frenet_dis = "<<p_5.y<<endl;
if((Lane_now == 1) &&(LaneSelect == 2)&&(Lane_2_Obstacle == 0)&&(obstacle_x <= 15)&&(flag_1_2))
{
//选取换道路径终点
double *P_5 = get_ClosestPoint(offlinepath_2,GPS_obstacle_x,GPS_obstacle_y);
//换道行驶状态
flag_1_2= false;
event_LaneChange(P_5,LaneSelect,Lane_now);
}
else if((Lane_now==2)&&(LaneSelect==1)&&(Lane_1_Obstacle == 0)&&(obstacle_x <= 15)&& flag3){
//回到原车道只需考虑原车道有无障碍物，在前面执行了.
double *P_5 = get_ClosestPoint(offlinepath_1,GPS_obstacle_x,GPS_obstacle_y);
flag3= false;
event_LaneChange(P_5,LaneSelect,Lane_now);
}
else{
//正在换道
}跟车工况
if( (obstacle_speed >= 100) && ((obstacle_y >= -l/2) && (obstacle_y <= l/2)) && obstacle_x <= 30)
{
cout << "===============进入动态障碍物的决策==============" << endl;
if (obstacle_x > 20){
cout << "===============动态障碍物在20米以外,忽略==============" << endl;
continue;
}
//跟车距离
else if(obstacle_x >= safeArea && obstacle_x <= FollowArea){
// obstacle_flag_1 = false;//检测到动态障碍物并进入跟车距离将flag置为false
//前车速度比较快
if(obstacle_speed > uniform_speed){
// if(follow_speed <= 2){follow_speed += 0.1;} //加速
// cout << "----前方车速大于2-->本车加速到2为止----" << endl;
follow_speed = uniform_speed;
}
else if(obstacle_speed < uniform_speed){
if(follow_speed >obstacle_speed){follow_speed -= 0.1;}
else if(follow_speed < obstacle_speed){follow_speed += 0.1;}
else{follow_speed = follow_speed;}
cout << "----前方车速小于uniform_speed-->跟车----" << endl;
}
}
else if(obstacle_x < safeArea && obstacle_x > 0){
if(obstacle_x > StopArea){
if(follow_speed > 1.0){
follow_speed -= 0.2;
cout << "----距前方小于安全距离-->本车减速----" << endl;
}
}
else if (obstacle_x < StopArea){
event_StopAndWait();
cout<<"----停车距离-->本车减速停车----"<<endl;
}
}其中，局部路径规划基于5阶贝塞尔曲线设计，贝塞尔曲线原理请参考文末链接
基于五阶贝塞尔曲线进行弯道局部路径规划。根据检测到的障碍物位置，给定点P0,P1,…,Pn，则n次贝塞尔曲线如下：
选取6个控制点，分别为下图中P0-P5，其中P0P1P2在原路径上，P3P4P5在换道路径上。
geometry_msgs::Point32 p_0, p_1, p1, p_2, p2, p_3, p3, p_4, p4, p_5;
p_0.x = GPS_vehicle.x;
p_0.y = GPS_vehicle.y;
p_5.x = P_5[0];
p_5.y = P_5[1];
p2.x = 0.5*(p_0.x+p_5.x);
// p_2.y = get_ClosestY(p_2.x,p_0,Lane_now)-GPS_vehicle.y;
p_2 = get_Point(p2.x,p_0,Lane_now);
cout << "Lane_now:" << Lane_now <<endl;
p1.x = 0.5*(p_0.x+p_2.x);
//p_1.y = get_ClosestY(p_1.x,p_0,Lane_now)-GPS_vehicle.y;
p_1 = get_Point(p1.x,p_0,Lane_now);
//p_1.y = (p_2.y+p_0.y-GPS_vehicle.y) / 2;
p3.x = p_2.x;
p_3 = get_Point(p3.x,p_5,LaneSelect);
p4.x = 0.5*(p_3.x+p_5.x);
p_4 = get_Point(p4.x,p_5,LaneSelect);
P5坐标为 换道路径上 与障碍物中心距离最近的点，由getClosestPoint()得到。
//获取最近的点
double* fsm_decisionmaking::get_ClosestPoint(vector <Point> &outofflinepath,double GPS_obstacle_x,double GPS_obstacle_y){
double d_temp=10000;
static double P_5[2];
int d_shortest_number = 0;
for(int i=0;i<outofflinepath.size();i++)
{
if(d_temp>(pow((GPS_obstacle_x - outofflinepath[i].x), 2) + pow((GPS_obstacle_y - outofflinepath[i].y), 2)))
{
d_shortest_number=i;
d_temp=(pow((GPS_obstacle_x - outofflinepath[i].x), 2) +pow((GPS_obstacle_y - outofflinepath[i].y), 2));
}
}
P_5[0]=outofflinepath[d_shortest_number].x;
P_5[1]=outofflinepath[d_shortest_number].y;
return P_5;
}P2P3的x坐标为P0和P5的中点，P1P4的x坐标分别为P0P2和P3P5的中点。
用getClosestY()计算P1P2P3P4在已知x坐标下的y坐标。
这样就得到了P0-P5六个控制点的坐标。
以P0为起点，P5为终点，规划一条贝塞尔曲线换道路径，中间一共取50个路径点。将规划好的换道轨迹分别发送给plan_convert（路径拼接）进行路径替换，发送给path_visualization（可视化）进行可视化。
for(int j=0;j<51;j++)
{
float k = j*0.02;
Curve_path.header.stamp = ros::Time::now();
CurveVision_path.header.stamp = ros::Time::now();
Curve_scatter.pose.position.x = pow((1-k),5)*p_0.x + 5*pow((1-k),4)*k*p_1.x + 10*pow((1-k),3)*pow(k,2)*p_2.x +
10*pow( (1-k),2)*pow(k,3)*p_3.x + 5*(1-k)*pow(k,4)*p_4.x +pow(k,5)*p_5.x +GPS_vehicle.x;
Curve_scatter.pose.position.y = pow((1-k),5)*p_0.y + 5*pow((1-k),4)*k*p_1.y + 10*pow((1-k),3)*pow(k,2)*p_2.y +
10*pow((1-k),2)*pow(k,3)*p_3.y + 5*(1-k)*pow(k,4)*p_4.y +pow(k,5)*p_5.y +GPS_vehicle.y;
Curve_path.poses.push_back(Curve_scatter);
Curve_path.lane_number = LaneSelect;
// cout<<"LaneSelect = "<<Curve_path.lane_number<<endl;
//转换为相对坐标
double firstPoint_x = 693221.656967812683433294;
double firstPoint_y = 2550764.669960263650864363;
Curve_scatter.pose.position.x = Curve_scatter.pose.position.x - firstPoint_x;
Curve_scatter.pose.position.y = Curve_scatter.pose.position.y - firstPoint_y;
//UTM坐标进行可视化
CurveVision_path.poses.push_back(Curve_scatter);
//cout<<"Curve_scatter_first.x = "<<Curve_scatter.pose.position.x<<" Curve_scatter_first.y = "<<Curve_scatter.pose.position.y<<endl;
}
总结
这篇blog仅用来记录近半年的工作，本项目还要感谢我的师兄在前期完成了很多工作。本项目用的都是些基础的算法，可以优化的地方很多，比如规划的不是轨迹（没有时间戳）、只有匀速和停车等工况，缺少速度规划、跟车工况很粗糙、缺少碰撞检测等等。
参考博客
园区自动驾驶实车平台决策规划控制系统（一）——基于纯追踪算法的横向控制（C++实现）-CSDN博客
PID参数解析+调参经验笔记（经验法）_pid调参_Xuan-ZY的博客-CSDN博客
基于C++的PID控制器_pid c++_Alexon Xu的博客-CSDN博客
基于贝塞尔曲线的变道轨迹规划_基于贝塞尔曲线的轨迹规划_逐风的小黄的博客-CSDN博客
常用规划算法解析4-凸优化（bezier曲线）篇 – 知乎
文章出处登录后可见！
已经登录？立即刷新
