【路径跟踪】纯跟踪PurePursuit算法个人总结|matlab|c++|Webots

未定义～

已于 2023-09-10 22:36:08 修改

阅读量287

点赞数

分类专栏：路径跟踪算法文章标签：算法 matlab c++

于 2023-09-05 12:28:38 首次发布

本文链接：https://blog.csdn.net/weixin_44519259/article/details/132686141

版权

路径跟踪算法专栏收录该内容

3 篇文章 0 订阅

订阅专栏

参考资料

1. PurePursuit算法

最近在做项目需要仿真一个前轮转向、驱动的自走剪叉式升降平台底盘，其中需要对路径跟踪，正好借此机会将之前了解过的路径跟踪类的知识应用一下。纯跟踪PurePursuit算法是比较早的基于几何法的一种路径跟踪方法，和Stanley跟踪算法属于同一类的基于几何的路径跟踪算法。Stanley算法同样也做了个人总结

1.1 算法思想

基于当前车辆后轮中心位置，在参考路径上向Ld(自定义)的距离，匹配一个预瞄点。假设车辆后轮中心点可以按照一定的转弯半径R行驶到该预瞄点，然后根据预瞄距离Ld，转弯半径R，车辆坐标系下预瞄点的航向角2α之间的几何关系，来确定前轮转角δ。

1.2 公式

这里不在阐述，参考的网上的数学公式推导
在这里插入图片描述
对其中公式进行理解：

其中曲率计算K：R的计算是利用弦长Ld与圆心角 $\alpha$ 关系，计算公式：

1.3 算法流程

输入：当前车辆位置（后轮中心的位置）；航向角 $\theta$ _yaw；速度v（注意这里用的是后轮的速度）；预瞄参考路径点处的航向角，即切线角；
根据当前机器人速度v、和目标速度target_v，PID控制器单纯P控制器计算加速度
根据预瞄距离Ld找参考路径点，先采取遍历找到最近的路径点，然后从最近的路径点开始，向参考路径后继续计算，遍历下一个路径点，然后累积n个路径点的总长度L_steps，当L_steps >= Ld时，则表示取到了所需要的预瞄路径点。如果直接找距离车辆当前坐标点的Ld距离的路径点会导致找不到路径点，因此采取累积路径长度的方法。
找到预瞄路径点后，计算前轮转角
然后更新机器人状态

2. matlab代码

2.1 车辆模型

四轮阿克曼转向底盘一般简化为双轮单车模型
在这里插入图片描述

2.2 参考路径生成

部分代码示意：

map_L = 4;
map_R = 5.3 / 2;
RefPos = zeros(50,2);
RefPos(:,1) = map_R*6;
RefPos(:,2) = linspace(0, 2*map_L - 0.5, 50);

theta = linspace(0,1*pi, 40);
x = map_R * 5 + map_R*cos(theta);
y = map_L*2 + map_R*sin(theta);
RefPos = [RefPos;[x' y']];

% 计算离散轨迹每个点的参考航向角
diff_x = diff(RefPos(:,1)) ;
diff_x(end+1) = diff_x(end);
diff_y = diff(RefPos(:,2)) ;
diff_y(end+1) = diff_y(end);
refHeading = atan2(diff_y , diff_x);
refHeading = mod((2*pi + refHeading), 2*pi);

在这里插入图片描述

2.3 初始参数设置

%% initialization相关参数定义
target_speed = 1;      % m/s
Kv =0.1;              % 前视距离系数
Kp = 0.8;              % 速度P控制器系数
Ld0 =0.01;               % Ld0是预瞄距离的下限值
dt = 0.02;              % 时间间隔，单位：s
L = 2.24;               % 车辆轴距，单位：m
% pos = RefPos(1,:)+1;%初始位置
pos = [5.3*3 0];%初始位置
v = 0;%初始速度
heading = pi/2;%初始航向角

2.4 速度跟随

这里只需简单计算，用一个P控制器即可

function [a] = PIDcontrol(target_v, current_v, Kp)              %计算加速度的函数
a = Kp * (target_v - current_v);
end

2.5 搜索预瞄路径点

遍历并累加路径长度，直到大于预瞄距离，输出最后的路径点索引值target_index

function  [lookaheadPoint,idx_target, Ld] = findLookaheadPoint(pos, v, RefPos, Kv, Ld0)

% 找到距离当前位置最近的一个参考轨迹点的序号
sizeOfRefPos = size(RefPos,1);%

for i = 1:sizeOfRefPos
    dist(i,1) = norm(RefPos(i,:) - pos);
end
[~,idx] = min(dist);
% 从该点开始向轨迹前方搜索，找到与预瞄距离最相近的一个轨迹点
L_steps = 0;           % 参考轨迹上几个相邻点的累计距离
Ld = Kv*v + Ld0;       % Ld0是预瞄距离的下限值；

while L_steps < Ld && idx < sizeOfRefPos%%重点看
    L_steps = L_steps + norm(RefPos(idx + 1,:) - RefPos(idx,:));
    idx = idx+1;
end

idx_target = idx; % 索引
lookaheadPoint = RefPos(idx,:);%某一个点的坐标
end

2.6 计算纯跟踪PurePursuit算法

function [delta,latError] = pure_pursuit_control(lookaheadPoint,idx,pos, heading, v,RefPos,refHeading, Kv, Ld0, L)
sizeOfRefPos = size(RefPos,1);%%=1223

if idx < sizeOfRefPos
    Point_temp = lookaheadPoint;
else
    Point_temp = RefPos(end,1:2);%从最后一行开始，从1开始取，取到第2个元素，也就是最后一个点的横纵坐标
end

alpha = atan2(Point_temp(2) - pos(2), Point_temp(1) - pos(1))  - heading;%
Ld = Kv*v + Ld0;
% 求位置、航向角的误差
x_error  = pos(1) - Point_temp(1);
y_error = pos(2) - Point_temp(2);
heading_r = refHeading(idx);
% 根据百度Apolo，计算横向误差 这里是当前坐标点到路径点切线的垂直距离
latError = y_error*cos(heading_r) - x_error*sin(heading_r);
% 前轮转角
delta = atan2(2*L*sin(alpha), Ld);
end

2.7 更新车辆状态

function [pos_now, yaw, v] = update(pos_last, yaw, v, ai, delta,dt,carL)   %更新车辆状态的函数
    pos_now(1) = pos_last(1) + v * cos(yaw) * dt;
    pos_now(2) = pos_last(2) + v * sin(yaw) * dt;
    yaw = yaw + v / carL * tan(delta) * dt;
    v = v + ai * dt;
end

2.8 主函数

i = 0;
while 100 > time 
   i = i+1; 
  ai = PIDcontrol(target_speed,v,Kp); %
  [Look_pos, target_index, ld(i,2)] = findLookaheadPoint(pos, v, RefPos, Kv, Ld0);                 %输出距离车辆当前最近的点的位置
  [delta,latError] = pure_pursuit_control(Look_pos,target_index,pos, heading, v,RefPos,refHeading, Kv, Ld0, L);
   path_look(i,:) = Look_pos;
   path_pos(i,:) = pos;
   path_latError(i,:) = [latError,time];
   ld(i,1) = time;
   path_v(i,1) = time;
   path_v(i,2) = v;   
   
  [pos, heading, v] = update(pos, heading, v, ai, delta,dt,L);   %更新车辆状态的函数
  time = time + dt;                          %时间过完一周期

    if abs(pos(1) - RefPos(end,1)) < 0.05 && abs(pos(2) - RefPos(end,2)) < 0.05
        break
    end
end

2.9 运行效果

左上角为结果图，红色为机器人路径，绿色为跟踪点路径，从右下角开始运动的，跟踪效果良好，图2是横向误差，图3是预瞄距离，图4是速度，中间一根奇怪的直线线是因为有没删除的（0，0）导致，忽略即可。

在这里插入图片描述

2.10 完整可运行matlab代码

% 纯跟踪算法
% 跟随效果与预瞄距离关系很大。就像人开车时眼睛往前看多远，看的太近了，到了弯道才慌忙打方向盘；看的太远了，又容易导致最近一段的轨迹跟随效果不好。
%% 轨迹点计算
clear all;
clc;
close all;

map_L = 4;
map_R = 5.3 / 2;
RefPos = zeros(50,2);
RefPos(:,1) = map_R*6;
RefPos(:,2) = linspace(0, 2*map_L - 0.5, 50);

theta = linspace(0,1*pi, 40);
x = map_R * 5 + map_R*cos(theta);
y = map_L*2 + map_R*sin(theta);
RefPos = [RefPos;[x' y']];

x = linspace(map_R*4, map_R*4, 30);
y = linspace(map_L*2 - 0.5, map_L + 0.5, 30);
RefPos = [RefPos;[x' y']];

theta = linspace(0,-1*pi, 40);
x = map_R*3 + map_R*cos(theta);
y = map_L + map_R*sin(theta);
RefPos = [RefPos;[x' y']];

x = linspace(map_R * 2, map_R * 2, 30);
y = linspace(map_L+0.5, map_L*2 - 0.5, 30);
RefPos = [RefPos;[x' y']];

theta = linspace(0,1*pi, 40);
x = map_R + map_R*cos(theta);
y = map_L*2 + map_R*sin(theta);
RefPos = [RefPos;[x' y']];

x = linspace(0, 0, 30);
y = linspace(map_L*2-0.5, 3, 30);
RefPos = [RefPos;[x' y']];
% 计算离散轨迹每个点的参考航向角
diff_x = diff(RefPos(:,1)) ;
diff_x(end+1) = diff_x(end);
diff_y = diff(RefPos(:,2)) ;
diff_y(end+1) = diff_y(end);
refHeading = atan2(diff_y , diff_x);
refHeading = mod((2*pi + refHeading), 2*pi);
%% initialization相关参数定义
target_speed = 1;      % m/s
Kv =0.1;              % 前视距离系数
Kp = 0.8;              % 速度P控制器系数
Ld0 =0.01;               % Ld0是预瞄距离的下限值
dt = 0.02;              % 时间间隔，单位：s
L = 2.24;               % 车辆轴距，单位：m
% pos = RefPos(1,:)+1;%初始位置
pos = [5.3*3 0];%初始位置
v = 0;%初始速度
heading = pi/2;%初始航向角
%% 主计算函数
figure(1);
time = 0;
path_look = zeros(10000,2);
path_pos = zeros(10000,2);
path_latError = zeros(10000,2);
ld = zeros(8000,2);
path_v = zeros(8000,2);
i = 0;
while 100 > time 
   i = i+1; 
  ai = PIDcontrol(target_speed,v,Kp); %
  [Look_pos, target_index, ld(i,2)] = findLookaheadPoint(pos, v, RefPos, Kv, Ld0);                 %输出距离车辆当前最近的点的位置
  [delta,latError] = pure_pursuit_control(Look_pos,target_index,pos, heading, v,RefPos,refHeading, Kv, Ld0, L);
   path_look(i,:) = Look_pos;
   path_pos(i,:) = pos;
   path_latError(i,:) = [latError,time];
   ld(i,1) = time;
   path_v(i,1) = time;
   path_v(i,2) = v;   
   
  [pos, heading, v] = update(pos, heading, v, ai, delta,dt,L);   %更新车辆状态的函数
  time = time + dt;                          %时间过完一周期

    if abs(pos(1) - RefPos(end,1)) < 0.05 && abs(pos(2) - RefPos(end,2)) < 0.05
        break
    end
end
%% 画图

subplot(2,2,1)
hold on;
plot(path_look(:,1), path_look(:,2), 'go', 'LineWidth', 5);
axis equal;
plot(path_pos(:, 1), path_pos(:, 2), 'r-', 'LineWidth', 1);
legend('look_pos','pos');
hold on;
title("路径图");

subplot(2,2,2)
plot(path_latError(:,2), path_latError(:,1));
title("横向误差");
subplot(2,2,3)
plot(ld(:,1), ld(:,2));
title("预瞄距离");
subplot(2,2,4)
plot(path_v(:,1), path_v(:,2));
title("速度");
%% 
function [pos_now, yaw, v] = update(pos_last, yaw, v, ai, delta,dt,carL)   %更新车辆状态的函数
    pos_now(1) = pos_last(1) + v * cos(yaw) * dt;
    pos_now(2) = pos_last(2) + v * sin(yaw) * dt;
    yaw = yaw + v / carL * tan(delta) * dt;
    v = v + ai * dt;
end
%%
function [a] = PIDcontrol(target_v, current_v, Kp)              %计算加速度的函数
a = Kp * (target_v - current_v);
end
%% 计算控制量，也就是前轮转角
function [delta,latError] = pure_pursuit_control(lookaheadPoint,idx,pos, heading, v,RefPos,refHeading, Kv, Ld0, L)
sizeOfRefPos = size(RefPos,1);%%=1223

if idx < sizeOfRefPos
    Point_temp = lookaheadPoint;
else
    Point_temp = RefPos(end,1:2);%从最后一行开始，从1开始取，取到第2个元素，也就是最后一个点的横纵坐标
end

alpha = atan2(Point_temp(2) - pos(2), Point_temp(1) - pos(1))  - heading;%
Ld = Kv*v + Ld0;
% 求位置、航向角的误差
x_error  = pos(1) - Point_temp(1);
y_error = pos(2) - Point_temp(2);
heading_r = refHeading(idx);
% 根据百度Apolo，计算横向误差
latError = y_error*cos(heading_r) - x_error*sin(heading_r);
% 前轮转角
delta = atan2(2*L*sin(alpha), Ld);
end
%%
function  [lookaheadPoint,idx_target, Ld] = findLookaheadPoint(pos, v, RefPos, Kv, Ld0)

% 找到距离当前位置最近的一个参考轨迹点的序号
sizeOfRefPos = size(RefPos,1);%

for i = 1:sizeOfRefPos
    dist(i,1) = norm(RefPos(i,:) - pos);
end
[~,idx] = min(dist);
% 从该点开始向轨迹前方搜索，找到与预瞄距离最相近的一个轨迹点
L_steps = 0;           % 参考轨迹上几个相邻点的累计距离
Ld = Kv*v + Ld0;       % Ld0是预瞄距离的下限值；

while L_steps < Ld && idx < sizeOfRefPos%%重点看
    L_steps = L_steps + norm(RefPos(idx + 1,:) - RefPos(idx,:));
    idx = idx+1;
end

idx_target = idx; % 索引
lookaheadPoint = RefPos(idx,:);%某一个点的坐标
end

3 移植到Webots仿真 | c++

3.1 主要部分改成C++代码

从matlab移植到webots里仿真，以下是算法部分c++代码供参考。

#include <vector>
#include <cmath>
#include <algorithm>
#include <iostream>

vector<vector<float>> ref_pos = {{15.9000000000000, 0}, {15.9000000000000, 0.163265306122449};
vector<float> ref_yaw = {1.57079632679490, 1.57079632679490};
float target_speed = 1; // m / s
float Kv = 2;           // % 前视距离系数 best 0.5
float Kp = 0.8;         // % 速度P控制器系数
float Ld0 = 0.05;       // % Ld0是预瞄距离的下限值
float L = 2.24;         // % 车辆轴距，单位：m
float path_latError;
vector<float> look_pos;
float out_ld;

float PID_vel_control(float target_v, float current_v, float kp)
{
    return kp * (target_v - current_v);
}

float pure_pursuit_control(int idx, vector<float> pos, float yaw, float v, vector<vector<float>> ref_pos, vector<float> ref_yaw, float Kv, float Ld0, float L)
{
    int sizeOfRefPos = ref_pos.size();
    vector<float> Point_temp;

    if (idx < sizeOfRefPos)
        Point_temp = ref_pos[idx];
    else
    {
        Point_temp = ref_pos[sizeOfRefPos];
        idx = sizeOfRefPos;
    }

    float alpha = atan2(Point_temp[1] - pos[1], Point_temp[0] - pos[0]) - yaw;
    float Ld = Kv * v + Ld0;
    // % 求位置、航向角的误差
    float x_error = pos[0] - Point_temp[0];
    float y_error = pos[1] - Point_temp[1];
    yaw = ref_yaw[idx];
    // % 根据百度Apolo，计算横向误差
    float latError = y_error * cos(ref_yaw[idx]) - x_error * sin(ref_yaw[idx]);
    path_latError = latError;
    // % 前轮转角
    float delta = atan2(2 * L * sin(alpha), Ld);
    printf("转角计算结果: 夹角alpha: %3.1f, 预瞄距离：%3.2f, 输出前轮转角：%3.1f, x_error: %4.3f, y_error: %4.3f\n", Rad2Deg(alpha), Ld, Rad2Deg(delta), x_error, y_error);
    return delta;
}

int findLookaheadPoint(vector<float> pos, float v, vector<vector<float>> RefPos, float Kv, float Ld0)
{
    //    % 找到距离当前位置最近的一个参考轨迹点的序号
    int sizeOfRefPos = RefPos.size();
    vector<float> dist;
    for (int i = 0; i < sizeOfRefPos; i++)
        dist.push_back(hypot(RefPos[i][0] - pos[0], RefPos[i][1] - pos[1]));

    int idx = min_element(dist.begin(), dist.end()) - dist.begin();
    // % 从该点开始向轨迹前方搜索，找到与预瞄距离最相近的一个轨迹点
    float L_steps = 0;
    // % 参考轨迹上几个相邻点的累计距离
    float Ld = Kv * v + Ld0;
    out_ld = Ld;
    // % Ld0是预瞄距离的下限值
    while (L_steps < Ld && idx < sizeOfRefPos)
    { //  % % 重点看
        L_steps = L_steps + hypot(RefPos[idx + 1][0] - RefPos[idx][0], RefPos[idx + 1][1] - RefPos[idx][1]);
        idx = idx + 1;
    }
    return idx;
}

float run(float time, float &v, float yaw, vector<float> pos, int dt)
{
    float ai = PID_vel_control(target_speed, v, Kp);
    int target_index = findLookaheadPoint(pos, v, ref_pos, Kv, Ld0); // % 输出距离车辆当前最近的点的位置
    float delta = pure_pursuit_control(target_index, pos, yaw, v, ref_pos, ref_yaw, Kv, Ld0, L);

    vector<vector<float>> path_look, path_pos;
    path_look.push_back(ref_pos[target_index]);
    path_pos.push_back(pos);

    v = v + ai * dt * 0.001;
    cout << "v:" << v << " ai: " << ai << endl;
    delta = Limit(delta, Deg2Rad(77.5), Deg2Rad(-77.5));
    look_pos = ref_pos[target_index];
    return delta;
}

3.2 仿真结果

在webots仿真环境中实验路径跟踪请添加图片描述

跟踪效果：良好的图：
在这里插入图片描述
效果不好的图：

在这里插入图片描述

数据分析：左边是前轮转角，右边是横向误差
在这里插入图片描述

4. 总结

纯跟踪PurePursuit算法是基于几何方法的，跟踪的是后轴中心，适用于低速、路径连续或不连续均可的环境中，算法实施简单，效果良好。在实验过程中发现由于跟踪后轴中心，导致车身航向角与路径点同时跟踪，跟踪后轮中心更符合实际车辆情况。
该算法中核心应该就是预瞄距离点的计算，然后仿真的时候，用matlab的系数一开始失败了，但是勇于修改控制系数Kv后，就好了，需要注意。以上只是个人总结，如个人理解有误，见谅。

未定义～

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
1
评论
【路径跟踪】纯跟踪PurePursuit算法个人总结|matlab|c++|Webots

纯跟踪PurePursuit算法是基于几何方法的，跟踪的是后轴中心，适用于低速、路径连续或不连续均可的环境中，算法实施简单，效果良好。在实验过程中发现由于跟踪后轴中心，导致车身航向角与路径点同时跟踪，跟踪后轮中心更符合实际车辆情况。该算法中核心应该就是预瞄距离点的计算，然后仿真的时候，用matlab的系数一开始失败了，但是勇于修改控制系数Kv后，就好了，需要注意。以上只是个人总结，如个人理解有误，见谅。
复制链接

扫一扫