【路径跟踪】纯跟踪PurePursuit算法个人总结|matlab|c++|Webots
参考资料
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后,就好了,需要注意。以上只是个人总结,如个人理解有误,见谅。