未备案个人网站 如何挣钱,wordpress如何修改布局,南昌做网站哪个好,网页设计发展前景分析纯跟踪(Pure Pursuit)路径跟踪算法研究#xff08;2#xff09;
下午进行了简单的公式推导#xff0c;理论推导部分是没有问题的 下面的博客提供了在实车上用 GPS 实现纯跟踪控制的一些思路和注意点 Pure Pursuit#xff08;纯追踪算法#xff09;ROS实践 并不急于在实车…纯跟踪(Pure Pursuit)路径跟踪算法研究2
下午进行了简单的公式推导理论推导部分是没有问题的 下面的博客提供了在实车上用 GPS 实现纯跟踪控制的一些思路和注意点 Pure Pursuit纯追踪算法ROS实践 并不急于在实车上实现第一步实现 CarSim 于 Simulink 的联合仿真
下面几篇博客主要是用Python模拟读着稍微有点困难 无人驾驶汽车系统入门十八——使用pure pursuit实现无人车轨迹追踪 无人驾驶运动控制----pure pursuit算法实践和理解 自主跟随学习笔记一使用纯跟踪pure pursuit算法跟随目标 下面的博客介绍了纯跟踪的原理以及在开源框架 Autoare 中的实现 自动驾驶Motion Planning—Pure Pursuit算法原理及实现 Pure Pursuit算法的基本思想是参考人类驾驶的行为通过计算车辆当前位置到预瞄点goal point的曲率使车辆沿着经过预瞄点的圆弧行驶从而实现轨迹跟踪(如图1)。因此该算法的核心在于通过设计合理的预瞄距离从而计算出轨迹跟踪的控制曲率 Pure Pursuit本质上是一个基于横向误差控制的比例控制器
预瞄点(goal point)的选择
如何从路径点中选择预瞄点并且保证预瞄点在行驶方向的前方而不是反方向还不清楚 预瞄距离(Ld)的选择 Pure Pursuit的效果强依赖于预瞄距离的选择可根据车速动态的调整预瞄距离从而增强系统的控制鲁棒性
下面的代码是基于上面的 Python 代码用 MATLAB 实现了下但代码并不完整 自动驾驶—基于运动学模型约束的纯跟踪控制算法 Pure Pursiut 纯跟踪算法是一种基于运动学约束的算法对该算法的学习可以更好地理解车辆运动学模型。根据上一文档的讲解我们可以建立后轮速度、前轮偏角和车辆大地坐标系下的位置、航向角的关系那么我们在忽略轮胎力学、执行机构特性等情况下就能根据后轮速度和前轮偏角任意控制车辆的速度、位置。本文只讨论横向控制假设后轮速度恒定也就是只控制前轮偏角 首先也应当实现当车辆后轮速度恒定即车辆纵向速度恒定下的纯跟踪控制
纯跟踪算法提出“预瞄距离”的概念根据预瞄距离寻找目标轨迹中符合条件的目标路径点判断逻辑就是寻找目标轨迹上哪个点和当前车辆位置图中的当前后轮位置的相对距离等于预瞄距离可以以当前位置为圆心预瞄距离为半径画圆寻找与目标轨迹的交点则该点就是当前时刻的目标点图中的目标后轮位置。控制目标则是计算多大的前轮偏角可以使当前后轮位置运动到目标后轮位置 恒定后轮速度恒定前轮偏角那么后轮的运动轨迹将是一个圆即车辆绕着转动中心做圆周运动如下图所示 基于这个现象可以认为在每个运行周期内速度恒定前轮偏角恒定车辆绕转动中心做圆周运动。那么控制目标就可以简化为两点①是怎样的圆周运动轨迹可以将后轮从当前位置运动至目标位置②如何控制前轮偏角实现这个圆周运动 下面的博客并没有给出具体的 Simulink 实现但对预瞄控制的解释还是很生动的 Pure Pursuit(纯跟踪算法)与Simulink实现 如上图所示A点是车辆后轴中心也是图中坐标系的原点图中这种坐标系我们一般称之为车辆坐标系B点是车辆要达到的目标点预瞄点L是AB之间的距离预瞄距离O点是假设存在的圆心使得车辆从位置A以转弯半径r可以达到位置B。圆心位置O的坐标与转弯半径r就是计算目标。此图颇有遗憾因为AO线段看上去比BO线段长但二者应该是相等的都是转弯半径
纯跟踪控制算法就是一种**基于几何关系计算车辆转弯半径**的方法 纯跟踪算法核心计算部分是非常简洁的不过实际应用时还有两块工作要做1从期望轨迹中找预瞄点B方法是从期望轨迹上找距离A点等于预瞄距离L的点。2A点与B点坐标可能都是基于某个全局坐标系所以需要将B点坐标从全局坐标系转换至车辆坐标系上。这两部分计算是使用纯跟踪算法的准备工作 上面所说的两个问题确实是比较关键的问题
全局坐标系到车辆坐标系的变换或许可以参考下面的代码 自动驾驶Motion Planning—Pure Pursuit算法原理及实现 float32_t PurePursuit::compute_points_distance_squared(const TrajectoryPoint point1,const TrajectoryPoint point2)
{using autoware::common::types::float64_t;const float64_t diff_x point1.pose.position.x - point2.pose.position.x;const float64_t diff_y point1.pose.position.y - point2.pose.position.y;return static_castfloat32_t((diff_x * diff_x) (diff_y * diff_y));
}std::pairfloat32_t, float32_t PurePursuit::compute_relative_xy_offset(const TrajectoryPoint current,const TrajectoryPoint target) const
{const auto diff_x target.pose.position.x - current.pose.position.x;const auto diff_y target.pose.position.y - current.pose.position.y;const auto yaw ::motion::motion_common::to_angle(current.pose.orientation);const auto cos_pose std::cos(yaw);const auto sin_pose std::sin(yaw);const auto relative_x static_castfloat32_t((cos_pose * diff_x) (sin_pose * diff_y));const auto relative_y static_castfloat32_t((-sin_pose * diff_x) (cos_pose * diff_y));const std::pairfloat32_t, float32_t relative_xy(relative_x, relative_y);return relative_xy;
}float32_t PurePursuit::compute_steering_rad(const TrajectoryPoint current_point)
{// Compute the steering angle by arctan(curvature * wheel_distance)// link: https://www.ri.cmu.edu/pub_files/2009/2/// Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdfconst float32_t denominator compute_points_distance_squared(current_point, m_target_point);const float32_t numerator compute_relative_xy_offset(current_point, m_target_point).second;constexpr float32_t epsilon 0.0001F;// equivalent to (2 * y) / (distance * distance) (2 * sin(th)) / distanceconst float32_t curvature (denominator epsilon) ? ((2.0F * numerator) / denominator) : 0.0F;const float32_t steering_angle_rad atanf(curvature * m_config.get_distance_front_rear_wheel());return steering_angle_rad;
}车辆当前位置和预瞄点间连线与车辆轴线(前进方向)夹角可以参考下面的公式 《基于纯追踪算法和樽海鞘优化算法的无人驾驶路径跟踪算法研究》 纯跟踪算法的轨迹跟随效果与参数预瞄距离关系很大。就像人开车时眼睛往前看多远看的太近了到了弯道才慌忙打方向盘看的太远了又容易导致最近一段的轨迹跟随效果不好
最后谈一下纯跟踪算法的硬伤纯跟踪算法可以缩小车辆与期望轨迹的位置偏差但是对角度偏差束手无策因为数学原理上根本就没有考虑角度偏差
下面的博客用 MATLAB 跟踪了圆形轨迹并且测试了大预瞄距离和小预瞄距离的情况 路径跟踪之Pure Pursuit控制算法 减小前视距离仿真结果如下图所示可以看出跟踪结果出现了一定程度的震荡这是因为前视距离减小后近似P控制器的比例系数增加从而造成动态响应出现超调和震荡 同时讨论了朝向角 90度和朝向角 90度的情况
如果预瞄点如下图所示那么大于90度车辆后轮的运行轨迹应该是大圆绕了一个大圈是在向后跟踪此时的前轮转角计算公式仍然不变 等于90度的情况车辆后轮的运行轨迹应该是右上那个半圆 通过以上分析不难发现三种情况下的前轮转角计算公式均适用同样“近似P控制器”这个结论在上述三种情况下也都成立 下面的方法简要分析了 Pure Pursuit、Stanley、MPC 这几种轨迹跟踪控制方法可以作为后面继续研究的开展 横向自动控制方法Purepursuit, Stanley, MPC对比 下面的博客给出了 Pure Pursuit 的仿真实现可以参考学习下实测可以跑 PurePursuit纯追踪算法MATLAB程序个人总结 Pure Pursuit纯跟踪算法Python/Matlab算法实现 %% 纯跟踪算法跟随效果与预瞄距离关系很大。就像人开车时眼睛往前看多远看的太近了到了弯道才慌忙打方向盘看的太远了又容易导致最近一段的轨迹跟随效果不好。%% initialization
k 0.1; % look forward gain 前向预测距离所用增益
Kp 1.0 ; % speed propotional gain
Lfc 1; % 基础预瞄距离
dt 0.1; % [s]
L 2.9 ; % [m] wheel base of vehicle
cx 0:0.1:50; %每0.1长度取一次预瞄点的x值for i 1:length(cx) %全局路径c(y)生成 路径初始化cy(i) cos(cx(i)/5)*cx(i)/4;
endi 1;
target_speed 10/3.6; %设定速度 如果实际自动驾驶这个就是设定要到达的速度%附加目前这里没有规划如果有规划就在过弯的时候给出横向加速度所以车身方向速度就会减小。
T 500;
lastIndex length(cx); %返回cx的向量长度 有501个预瞄点x值 有501个序号
x 0; y 4; yaw 0; v 0; %车辆初始位置
time 0;
Ld k * v Lfc; %初始速度为0时的预瞄距离LdLfcfigure(1);
while T time target_index calc_target_index(x,y,cx,cy); %输出距离车辆当前最近的点的位置ai PIDcontrol(target_speed,v,Kp); %Ld k * v Lfc ;delta pure_pursuit_control(x,y,yaw,v,cx,cy,target_index,k,Lfc,L,Ld); %前轮要转的角度[x,y,yaw,v] update(x,y,yaw,v, ai, delta,dt,L); %更新车辆状态time time dt; %时间过完一周期
% pause(0.1)plot(cx,cy,b,x,y,r-*) %将预瞄点位置用蓝色表示当前位置用红色表示drawnowhold onendfunction [x, y, yaw, v] update(x, y, yaw, v, ai, delta,dt,L) %更新车辆状态的函数x x v * cos(yaw) * dt;y y v * sin(yaw) * dt;yaw yaw v / L * tan(delta) * dt;v v ai * dt;
endfunction [a] PIDcontrol(target_v, current_v, Kp) %计算加速度的函数
a Kp * (target_v - current_v);
endfunction [delta] pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Ld) %纯追踪控制器前轮转角方程的函数tx cx(ind); %距离车辆当前最近的点的位置对应的cx值 ind是距离车辆当前最近的点的位置ty cy(ind); %距离车辆当前最近的点的位置对应的cy值alpha atan((ty-y)/(tx-x))-yaw; %该处定义向左转为alphabeta-Fai所以向右转就输出-alphaLd k * v Lfc ; % 预瞄距离Ld与车速成线性关系 delta atan(2*L * sin(alpha)/Ld) ; %前轮转角
endfunction [ind] calc_target_index(x,y, cx,cy) %计算找到距车辆当前位置最近点的序号 输出最近点的函数
N length(cx); %N 501
Distance zeros(N,1); % 501行 1列的矩阵for i 1:N
Distance(i) sqrt((cx(i)-x)^2 (cy(i)-y)^2); %每一个预瞄点到初始位置的距离
end[~, location] min(Distance); %找出最近的距离对应的位置
ind location;
ind ind 5
end实际上 MATLAB 本身也提供了 Pure Pursuit 工具箱 MATLAB Pure Pursuit Controller GitHub 上也有相关的基于 Gazebo 的仿真 https://github.com/NeXTzhao/planning
