羊村懒王

这个屌丝很懒，什么也没留下！

热门标签

园区自动驾驶实车平台决策规划控制系统（一）——基于纯追踪算法的横向控制（C++实现）_纯跟踪算法c++实现

作者：羊村懒王 | 2024-06-13 02:39:43

踩

纯跟踪算法c++实现

总结一下自己这一年的项目经验，由于本人技术水平有限，写作内容难免有错，如果发现还请不吝赐教~

在项目中本人负责的是自动驾驶的决策规划控制算法部分，在研发过程中负责对接上层感知定位模块和下层线控系统，项目实现了在园区场景下的自动驾驶。实现的场景包括单障碍物换道避障，多障碍物换道避障，跟车行驶，停车等待行人等。

接下来准备把项目中用到的横向控制、纵向控制、换道轨迹规划和决策模型分别记录，并附上核心代码，本文是第一篇。先放几个测试场景的视频作为预告。

园区自动驾驶（一）——基于Pure Pursuit的横向控制

由于本文是针对园区的自动驾驶，车速较低，且循迹路径是一条固定的路线，测试路径如下图所示，图中：绿色路径是录制的离线路径，录制方法是手动开车时记录下组合惯导系统的实时定位，并保存；紫色的路径是实际行驶路径；蓝色长条不是车辆模型，长度也不是车身长度，蓝条后端是车辆当前位置（后轴），蓝条前端是纯追踪算法当前的目标点。

在讲纯追踪算法之前，先陈述下使用纯追踪算法对车辆运动学和动力学模型的简化。

车辆运动学和动力学模型简化

无人驾驶-控制-自行车模型_0->oo的博客-CSDN博客

二自由度车辆运动学模型和动力学模型这位大佬讲的很清楚，总结一下结论，简化包括：

不考虑车辆在Z轴方向的运动，只考虑XY水平面的运动，如下图所示；
左右侧车轮转角一致，这样可将左右侧轮胎合并为一个轮胎，以便于搭建单车模型，如图2所示；
车辆行驶速度变化缓慢，忽略前后轴载荷的转移；
车身及悬架系统是刚性的；

简化结论有：

1、一般条件下： $\large yaw=\varphi +\beta$ ，其中航向角为yaw，横摆角为 $\large \varphi$ ；

但低速条件下，认为车不会发生侧向滑动（漂移），因此质心侧偏角 β ≈ 0，可得：航向角yaw=横摆角 $\large \varphi$ 。

2、一般条件下，轮胎由于是胶体，会在压力下发生形变，导致轮胎侧偏；

但低速条件下，将轮胎视为完全刚体，不会发生变形，不考虑轮胎侧偏，且后轮一般不转向，因此后轮转角 $\large \delta _{r}\approx 0$ 。

bicycle模型

在以上简化假设都成立的情况下，才能将四轮akerman转向模型简化为二轮bicycle模型。

如上图所示，bicycle模型中，L是车的轴距，R是当前转向角下，车辆遵循的转弯半径，可以得到前轮转角δ 为：

$\delta =arctan(L/R)$

纯追踪算法推导

接下来就是老生常谈的推导了，图懒得用visio画了，手画快些。

将四轮akerman模型简化成bicycle模型后，从录制的离线路径中选取下一个要追踪的目标点G(gx,gy)，控制目标是让车辆的后轴中点S（组合惯导系统位于车辆后轴，因此定位得到的车辆当前位置其实是车辆后轴的位置）经过该目标点G。

目标点G，车辆位置S和转向圆圆心三个点构成了一个等腰三角形，根据正弦定理，可以推出以下三个公式：

其中R为转向圆半径； $\large l_{d}$ 是希望控制车辆到达的下一个位置，即 look-ahead distance（前视距离）：人类驾驶员开车时，眼睛看向道路前方的位置就是想开到的地方；纯追踪算法也一样，变成了在离线轨迹上寻找下一个期望到达的位置；夹角 α 为车身坐标系的x轴与车身坐标系原点与目标点的夹角，夹角 α 如下图所示。

1/R也就是这段圆弧的曲率，为：

将1/R带入到bicycle模型的前轮转角公式 $\delta =arctan(L/R)$ 中，可得：

将转角 δ 视作时间 t 的函数，可得：

上式就是纯跟踪算法的输出表达式，输出量为车辆前轮转角 δ 。

为了更好的理解影响纯跟踪算法精度的因素，定义车辆当前位姿 S 和目标点 G 的横向误差 $\large e_{l}$ ，可得夹角 α 正弦：

转角 δ 可以重写为：

由于轴距 L 是常数，横向误差 $\large e_{l}$ 是自变量，因此因变量 δ 的控制效果主要取决于前视距离 $\large l_{d}$ 。

通常来说， $\large l_{d}$ 被认为是车速的函数，需要根据车速传感器反馈的当前车速来实时更新当前的前视距离。

除了车速外，道路曲率也是影响循迹精度的关键。在弯道下，前视距离如果太大，可能导致循迹精度下降。

因此，本文将前视距离 $\large l_{d}$ 视作是车速和道路曲率的一次函数，即 $\large l_{d}=kv_{x}+l_{0}$ ，那么前轮的转角公式就变为：

其中， $\large l_{0}$ 为最小前视距离，本文的最小前视距离大小根据道路曲率设置：如果道路曲率太大，则减小 $\large l_{0}$ ，使追踪更加精确；如果道路曲率较小，则增大 $\large l_{0}$ ，使轨迹追踪更平滑。

基于C++的纯追踪算法实车实现核心代码

本实现方法在ros环境下实现，纯追踪算法只用来控制转向角度，速度控制将在下一篇纵向控制中介绍。

首先根据实车型号定义各项参数。


#define  K          (1.5)       // 前视距离系数
#define  L_VEHICLE  (2.9)       // 车的轴距
int L0 = 2;    //最小前视距离，根据道路曲率变化

本文将初始最小前视距离设为2 m，前视距离系数设为1.5 ，AionLX 车的轴距为2.9m。

定义车辆状态类，在bicycle模型中，位姿只考虑车辆当前世界坐标系下坐标（x，y）,车辆偏航角yaw，以及车辆的速度speed：


struct State{
  double x = 0;          // m
  double y = 0;          // m
  double yaw = 0;        // degree
  double speed = 0;      // m/s
};
State vehicleState;

订阅话题，接受定位和传感器节点发布的消息，更新车辆状态：


        vehicle_location = node.subscribe("/geometry_pose", 2, &pure_pursuit::location_update, (pure_pursuit *) this);
        veh_speed = node.subscribe("/vehicle_response", 2, &pure_pursuit::vehReportCallback,  (pure_pursuit *) this);

接收定位传来的当前位姿，包括世界坐标系坐标和四元数，用ros的tf::Quaternion库将四元数转为 roll，pitch 和 yaw，得到航向角yaw：


//用PoseStamped更新位姿(x,y,yaw)
    void pure_pursuit::location_update(const geometry_msgs::PoseStamped::ConstPtr locat) {
        vehicleState.x = locat->pose.position.x;
        vehicleState.y = locat->pose.position.y;
 
        //根据当前位置更新最小前视距离L0，其中boundary是直道和弯道的分界点
        if(vehicleState.x>boundary.x&&vehicleState.y>boundary.y){
            L0=1;   //最小前视距离默认为2，弯道上变为1
        }
 
        //用vehLocUpdatRdy表示位姿是否更新，默认为false
        vehLocUpdatRdy = true;
 
        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;
    }

接受传感器传来的当前车速，更新当前车速：


//用传感器回调函数更新当前车速
void pure_pursuit::vehReportCallback(const common_msgs::veh_response::ConstPtr &veh_report_msg)
    {
        vehicleState.speed = static_cast<double>(veh_report_msg->veh_spd);
    }

纯追踪横向控制核心代码：


//根据目标进行转角控制
    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;
    }

定义函数用于搜索最近的目标点：


//index为跟踪目标
    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];
                index = i;
            }
        }
 
        double l = 0;
        double lf = K * s.speed + L0;
        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;
    }

发送转角给线控系统执行：


    bool pure_pursuit::track_follow_process(const vector <Point> &pathVet) {
        bool isjobDone = false;
        
        //判断路径是否走完
        if (goalIndex < rearIndex) {
 
            //获取纯跟踪输出的前轮转角
            delta = pure_pursuit_control(vehicleState, pathVet, &goalIndex);
 
            wheelAngle = delta * 180 / M_PI;
            wheelAngle = -1 * wheelAngle;  //前轮转角为弧度制，逆时针为正，AionLX车的顺时针转向为正，逆时针为负，刚好相反
 
            //AionLX前轮转角最大为40°
            if (wheelAngle > 40.0) wheelAngle = 40.0;
            if (wheelAngle < -40.0) wheelAngle = -40.0;
 
        }
        
        //如果走完，则急停
        if (goalIndex >= rearIndex)
            isjobDone = true;
        
        //发布前轮转角
        std_msgs::Float32 tempAngleSet;
        tempAngleSet.data = wheelAngle;
        strAngle_pub.publish(tempAngleSet);
 
        return isjobDone;
    }

以上就是纯追踪算法的核心代码，经实车实验，平均误差小于10cm。如下左图，由于设置的最大前轮转角不够大，且前视距离设置较大，所以在急剧转角处出现了转向不足的情况；将最大前轮转角改为40°，前视距离改小后，如下右图，在大弯道也能准确循迹。

小结

纯追踪算法是一种几何路径跟踪算法，很适合新手上手使用。优点是很大地简化了车辆运动学和动力学模型，输入输出明确，在低速下能获得较好地控制效果；缺点是简化了太多的车辆运动学和动力学模型，所以只要速度和加速度提高，运动学动力学简化条件不再成立，就无法保证横向控制精度。

本项目由于是在园区内行驶，车速不会高于10km/h，所以纯追踪算法能够达到横向控制需求。但目前选择最佳前视距离的方法并不一定，本文将 $\large l_{d}$ 视作纵向速度和路径曲率相关的一次函数，大的前视距离可以使控制更平滑，但在大转角处会转向不足；前视距离太小又会带来控制的震荡(前轮转角变动幅度很大)。因此，影响横向控制精度地前视距离 $\large l_{d}$ 的选取仍有待优化。

参考博客

无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪_AdamShan的博客-CSDN博客自动驾驶（七十一）---------Pure Pursuit轨迹追踪_一实相印的博客-CSDN博客_自动驾驶轨迹跟踪

无人驾驶-控制-阿克曼模型_0->oo的博客-CSDN博客_阿克曼模型

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/羊村懒王/article/detail/710572