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

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

Stanley

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

前轮转角控制量:

δ = θ φ + θ y \large\delta =\theta _{\varphi }+\theta _{y } δ=θφ+θy

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

δ = θ φ + θ y = θ φ + a r c t a n e y d \large\delta =\theta _{\varphi }+\theta _{y }=\theta _{\varphi }+arctan\frac{e_{y}}{d} δ=θφ+θy=θφ+arctandey

横向误差变化率: e y ˙ = − v s i n θ y ≈ − v θ y ≈ − v t a n θ y = − v e y d \dot{e_{y}} =-vsin\theta _{y }\approx -v\theta _{y }\approx -vtan\theta _{y }=-v\frac{e_{y}}{d} ey˙=vsinθyvθyvtanθy=vdey
e y ˙ ≈ − v d e y \dot{e_{y}} \approx-\frac{v}{d}e_{y} ey˙dvey得:
e ( t ) = e ( 0 ) e − k t = e ( 0 ) e − v d t e(t)=e(0)e^{-kt }=e(0)e^{-\frac{v}{d}t } e(t)=e(0)ekt=e(0)edvt

1 Stanley控制器设计如下:

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

输入:当前车辆位置、航向角 ψ ψ ψ、速度 v v v、当前目标路点和离车辆前轴中心最近目标路径点的航向角 ψ t ψ_t ψt
计算横向误差 e y e_y ey
计算 δ = ψ t ​ − ψ + a r c t a n k e y v ​​ δ=ψ_t​−ψ+arctan\frac{ke_y}{v}​​ δ=ψtψ+arctanvkey​​
输出:前轮转角控制量 δ \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;
}

3 Pure pursuit与Stanley算法对比

相同点:基于对前轮转角进行控制来消除横向误差
不同点:
PurePursuit算法:预瞄点的选取,距离越短,控制精度越高,但可能会产生震荡;预瞄距离越长,控制效果趋于平滑,震荡减弱。
Stanley算法:控制效果取决于控制增益,缺少纯追踪的规律性,调试需要花一定的精力寻找合适的控制增益值

  • 27
    点赞
  • 27
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
Stanley算法也被称为Stanley-Stembridge算法,主要用于计算可逆写作有限生成排列的生长函数。以下是一个基本的Stanley算法的MATLAB程序示例: ```matlab function sf = stanley(n) % 输入一个正整数n,计算Stanley算法的生长函数 sf = sym(zeros(1,n)); % 创建一个符号变量数组来存储生长函数的值 % 初始化第一个排列(1) p = 1; % 对于每个排列长度m从1到n-1 for m = 1:n-1 % 计算长度为m的排列的生成标志函数 gf = genFlag(p, m); % 计算长度为m的排列的生成多项式 gm = sum(gf); % 更新生长函数数组 sf(m+1) = gm; % 生成下一个长度为m+1的排列 p = nextPerm(p, m); end % 打印生长函数数组 disp(sf); end function gf = genFlag(p, m) % 输入一个排列p和排列的长度m,计算排列的生成标志函数 n = length(p); gf = sym(ones(1,m)); for i = 1:m for j = 1:m if p(i) < p(j) gf(i) = gf(i) * (n - j + 1); end end end end function np = nextPerm(p, m) % 输入一个排列p和排列的长度m,生成下一个长度为m+1的排列 n = length(p); np = [p, m+1]; indexes = 1:n; for i = 1:m indexes(p(i)) = []; end np = [np, indexes]; end ``` 在此程序中,我们首先定义了一个函数`stanley`,其中处理Stanley算法的主要步骤。我们创建一个符号变量数组`sf`来存储生长函数的值,并初始化第一个排列[1]。然后,我们使用一个循环来计算每个长度为m的排列的生成多项式,并将其存储在生长函数数组中。最后,我们输出生长函数数组。 我们还定义了两个辅助函数`genFlag`和`nextPerm`。`genFlag`函数用于计算排列的生成标志函数,`nextPerm`函数用于生成下一个长度为m+1的排列。 请注意,这只是一个基本的Stanley算法程序示例,可能需要根据具体应用进行修改和扩展。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值