路径跟踪算法Stanley 实现 c++

Big David

于 2024-02-17 14:38:14 发布

阅读量1.3k

点赞数 27

分类专栏：决策规划控制文章标签： c++ 自动驾驶算法 matplotlib Stanley

本文链接：https://blog.csdn.net/BigDavid123/article/details/136138266

版权

决策规划控制专栏收录该内容

48 篇文章 39 订阅

订阅专栏

参考博客：【自动驾驶】Stanley（前轮反馈）实现轨迹跟踪 | python实现 | c++实现

Stanley

前轮反馈控制(Front wheel feedback),又称Stanley控制。
核心思想：基于前轮中心的路径跟踪偏差量对方向盘转向控制量进行计算(Pure Pursuit是基于后轮中心)。
在这里插入图片描述

前轮转角控制量：

$\large\delta =\theta _{\varphi }+\theta _{y }$

由航向误差引起的转角 $\theta _{\varphi }$ ，即当前车身方向与参考轨迹最近的点 $P_{1}$ 的切线方向的夹角。
由横向误差引起的转角 $\theta _{y }$ ，即前轮中心到参考轨迹最近点 $P_{1}$ 的横向距离 $e_{y}$

$\large\delta =\theta _{\varphi }+\theta _{y }=\theta _{\varphi }+arctan\frac{e_{y}}{d}$

横向误差变化率： $\dot{e_{y}} =-vsin\theta _{y }\approx -v\theta _{y }\approx -vtan\theta _{y }=-v\frac{e_{y}}{d}$
$\dot{e_{y}} \approx-\frac{v}{d}e_{y}$ 得：
$e(t)=e(0)e^{-kt }=e(0)e^{-\frac{v}{d}t }$

1 Stanley控制器设计如下：

$\large\delta(k)=\theta _{e}(k)+arctan\frac{ke(k)}{v(k)}$
其中， $\theta _{e}(k)$ 为k时刻的航向角偏差，e(k)为横向跟踪误差，k为需要调节的参数，v为无人车当前速度。

输入：当前车辆位置、航向角 $ψ$ 、速度 $v$ 、当前目标路点和离车辆前轴中心最近目标路径点的航向角 $ψ_t$
计算横向误差 $e_y$
计算 $δ=ψ_t−ψ+arctan\frac{ke_y}{v}$
输出：前轮转角控制量 $\delta$

2 Stanley控制器代码

stanley.h

#pragma once
#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
using namespace std;

#define PI 3.1415926

class Stanley {
private:
    double k;
public:
    Stanley(double k);
    double calTargetIndex(vector<double> robot_state, vector<vector<double>> ref_path);
    double normalizeAngle(double angle);
    vector<double> stanleyControl(vector<double> robot_state, vector<vector<double>> ref_path);
};

stanley.cpp

#include "Stanley.h"

Stanley::Stanley(double k)
{
    this->k = k;
}

// 搜索目标邻近路点
// robot_state 当前机器人位置 ref_path 参考轨迹(数组)
double Stanley::calTargetIndex(vector<double> robot_state, vector<vector<double>> ref_path)
{
    vector<double> dists;
    for (vector<double> xy : ref_path)
    {
        double dist = sqrt(pow(xy[0] - robot_state[0], 2) + pow(xy[1] - robot_state[1], 2));
        dists.push_back(dist);
    }
    return min_element(dists.begin(), dists.end()) - dists.begin();
}

// 角度归一化到[-PI, PI]
double Stanley::normalizeAngle(double angle)
{
    while (angle > PI)
    {
        angle -= 2.0 * PI;
    }
    while (angle < -PI)
    {
        angle += 2.0 * PI;
    }
    return angle;
}

// stanley 控制
// robot_state：x,y,yaw,v ref_path：x,y,theta
// return 控制量 + 目标点索引
vector<double> Stanley::stanleyControl(vector<double> robot_state, vector<vector<double>> ref_path)
{
    double current_target_index = calTargetIndex(robot_state, ref_path);
    vector<double> current_ref_point;

    if (current_target_index >= ref_path.size())
    {
        current_target_index = ref_path.size() - 1;
        current_ref_point = ref_path[ref_path.size() - 1];
    } else {
        current_ref_point = ref_path[current_target_index];
    }
    double e_y;
    double psi_t = current_ref_point[2];

    if ((robot_state[0] - current_ref_point[0]) * psi_t - (robot_state[1] - current_ref_point[1]) > 0)
    {
        e_y = sqrt(pow(current_ref_point[0]-robot_state[0],2)+pow(current_ref_point[1]-robot_state[1],2));
    }
    else 
    {
        e_y = -sqrt(pow(current_ref_point[0]-robot_state[0],2)+pow(current_ref_point[1]-robot_state[1],2));
    }
    double psi = robot_state[2];
    double v = robot_state[3];
    double theta_e = psi_t - psi;
    double delta_e = atan2(k * e_y, v);
    double delta = normalizeAngle(delta_e + theta_e);
    return {delta, current_target_index};
}

main.cpp

#include "Stanley.h"
#include "matplotlibcpp.h"
#include "KinematicModel.h"
namespace plt = matplotlibcpp;

#define PI 3.1415926

int main()
{
    double x0 = 0.0, y0 = 1.0, psi = 0.5, v = 2, L = 2, dt = 0.1;
    double k = 1;
    vector<vector<double>> ref_path(1000, vector<double>(3));
    // 生成参考轨迹
    vector<double> ref_x, ref_y;
    for (int i = 0; i < 1000; i++)
    {
        ref_path[i][0] = 0.1 * i;
        ref_path[i][1] = 2 * sin(ref_path[i][0]);
        for (int i = 0; i < 999; i++) {
            ref_path[i][2]= atan2((ref_path[i+1][1]-ref_path[i][1]),(ref_path[i+1][0]-ref_path[i][0]));
        }
        ref_x.push_back(ref_path[i][0]);
        ref_y.push_back(ref_path[i][1]);
    }

    KinematicModel model(x0, y0, psi, v, L, dt);
    vector<double> x_, y_;
    vector<double> robot_state(4);
    Stanley stanley(k);
    for (int i = 0; i < 600; i++) {
        plt::clf();
        robot_state = model.getState();
        vector<double> delta_index = stanley.stanleyControl(robot_state, ref_path);
        model.updateState(0, delta_index[0]);
        x_.push_back(model.x);
        y_.push_back(model.y);

        plt::plot(ref_x, ref_y, "b--");
        plt::plot(x_, y_, "r");
        plt::grid(true);
        plt::ylim(-5, 5);
        plt::pause(0.01);
        if (delta_index[1] >= ref_path.size() - 1) break;
    }

    const char* filename = "./stanley.png";
    plt::save(filename);
    plt::show();
    return 0;
}