目的
使用简单的“纯跟踪算法”实现无人车自动泊车或者位姿调整。在泊车或者工业场景,如果空间不够,那么车辆经常需要做一些大角度的转向或者倒车,例如叉车。
这些场景与一般的道路行驶场景可能有所区别,道路行驶一般只考虑前进方向的高速行驶,并且转向曲率不会太大。泊车场景恰好相反,曲率大、速度慢,而且伴随行驶方向的变化。
道路行驶下的跟踪已经被研究的比较深入了,那么道路行驶使用的跟踪算法还适用于倒车场景吗?本文我们来研究一下这个问题。
Reeds-Sheep曲线
假设无人车的运动路径是已知的,笔者使用 https://github.com/hbanzhaf/steering_functions 中提出的曲率连续的改进Reeds-Sheep曲线生成路径。
程序输出的路径是一系列离散的点,点之间的距离可以自定义,笔者选择每5毫米一个点,程序中设置DISCRETIZATION=0.005。
路径采用nav_msgs::Path消息发出。
纯跟踪算法
纯跟踪算法(Pure Pursuit)首先要指定一个被跟踪的目标点。
原始版本的纯跟踪算法只讨论了跟踪无人车前方的点,对于Reeds-Sheep曲线这种包含运动方向变化的曲线,无人车既需要前进也需要后退,但是想实现后退也非常简单。
笔者将被跟踪的目标点称为局部目标(local goal)无人车真正最终的静态目标点则称为全局目标(global goal)。
纯跟踪需要无人车的定位,仿真时假设这个定位信息由ROS中的/base_pose_ground_truth消息给出。局部目标的计算方式是,遍历路径,找到第一个离无人车≥ d l 的路径点。
d l 就是前视距离,d l 越小跟踪精度越高,但是越容易导致震荡。机器人在运动时,这个局部目标也会更新。
如果找到的局部目标落在了无人车的后方,此时意味着无人车需要后退,只需要将速度取负值即可,前轮转角不用变。
出现的问题
1.转折点
在仿真时出现了一些问题。首先,最困难的是对于尖点(cusp)怎么处理。因为很多情况下,Reeds-Sheep曲线都包含尖点,在尖点处车辆会改变运动方向。
如果使用纯跟踪算法跟踪这个路径,那么在尖点处会出现一个问题。因为纯跟踪算法总要指定一个跟踪点,这个跟踪点一般在车辆前方或者后方一定距离(d l )处。
在向尖点运动时,车辆不会正好处于尖点上,而是提前离开。下图中的d l = 0.2后面也采用这一数值。
图中的黄点是被跟踪的局部目标,红色点表示无人车后轮轴中心处的实时位置。
这就导致车辆没有完全位于路径上,进而导致后面的跟踪出现横向偏差(如下图所示),即使采用曲率连续的Reeds-Sheep曲线版本也没有用。
这是纯跟踪算法本身的问题吗?不是,纯跟踪算法完全可以跟得上,我们为了安全通常把输出角度给限幅了,如果解除限幅你就会发现纯跟踪算法完全可以准确的跟踪。
但是实际使用时我们又不可能解除限幅,所以怎么解决这个问题呢?
一种是直接增大一点Reeds-Sheep曲线的最小转向半径,令其略大于车辆的真实最小转向半径,笔者尝试增加了约10%,跟踪情况如下图。
另一种方法是增加尖点(cusp)部分的长度,这可以通过改变主程序(steering_functions_node.cpp)中的sigma_max_变量实现,sigma_max_越小,过渡部分越长,最好大于d l 试验发现取sigma_max_=0.5左右就可以。
控制指令如下图所示。
速度单独进行规划,然后叠加到路径上,如下图所示。
2.定位误差
前面的控制都假设定位是完美的,不存在定位误差。如果加入定位误差,纯跟踪算法的表现会怎么样呢?
我们用随机数来模拟定位误差,定位误差一般是正太分布的,因此用正态分布函数std::normal_distribution生成随机数,均值总是取0,标准差决定了误差的范围。
首先取小的标准差—— 1mm,无人车的表现如下图所示,无人车的跟踪效果比较好。
但是前轮转角的变化却非常剧烈,如下图所示。这还仅仅是1mm左右的误差,这在实际中是几乎不可能达到的。
标准差为1cm时的表现如下图所示,已经产生了明显的横向跟踪偏差。
此时前轮转角已经惨不忍睹了,如下图所示,这还是1cm左右的误差,实际中无人车的定位要达到1cm也是很困难的。
标准差为5cm时的表现如下图所示,这个误差是一般室外卫星定位的误差范围,也就是常见的误差,此时无人车彻底无法跟踪。
不仅前轮转角更疯狂了,而且由于横向偏差已经超过了前视距离d l ,局部目标已经出现在无人车侧面了,导致无人车完全无法跟踪了,如下图所示。
这说明纯跟踪算法对定位误差是极其敏感的,在实际应用时这是个非常严重的问题。
算法理解
为了易于理解纯跟踪算法,笔者用Mathematica设计了一个小程序,你可以用鼠标拖动目标点(绿色点),并观察前轮的转角,如下图。
目标点是纯跟踪算法中的核心概念,这个目标点是人为设计或者选择的。跟踪性能的好坏不仅取决于控制参数的选择,目标点的选择也起到重要的作用。
当目标点选取的不好时,例如距离无人车当前位置过近,则会出现控制量剧烈变化。
你也可以用鼠标拖动无人车的参考点,观察前轮的转角,如下图。从图中可以发现,在距离目标比较近时,纯跟踪算法的表现很糟糕,参考点位置有一点点改变都会导致前轮转角剧烈变化。
但是无人车的定位本身是必然存在偏差的,所以纯跟踪算法在前视距离短时稳定性并不好。
cuboid[center_: {0, 0}, dim_, radius_: 0] := Rectangle[center - dim/2, center + dim/2, RoundingRadius -> 0.01];
move2D[shape_, pose_] := Translate[Rotate[shape, pose[[3]], {0, 0}], pose[[1 ;; 2]]];
L = 1.64;
[Delta]max = 25 Degree ;
bicycle[pose_, [Delta]_] := {
rearWheel = cuboid[{0, 0}, {0.4, 0.1}, 0.1];
frontWheel = move2D[rearWheel, {L, 0, [Delta]}];
trunk = cuboid[{L/2, 0}, {L, 0.02}, 0.1];
move2D[{Blue, frontWheel, rearWheel, Black, trunk, Red, Circle[{L, 0}, 0.22, {0, [Delta]}]}, pose]
};
Manipulate[
pose = Flatten@{p, [Theta]};
dirvec = AngleVector[[Theta]];
vertvec = {-dirvec[[2]], dirvec[[1]]};
p1 = p + L*dirvec;
dl = Norm[goal - p];
[Alpha] = VectorAngle[goal - p, {1, 0}] - [Theta];
[Delta] = ArcTan[2*L*Sin[[Alpha]]/dl];
R = Abs[dl/2/Sin[[Alpha]]];
c = p + Sign[[Alpha]]*R*vertvec;
a1 = -VectorAngle[p - c, {1, 0}];
a2 = -VectorAngle[goal - c, {1, 0}];
Graphics[{bicycle[pose, [Delta]], Point[c], AbsoluteThickness[1],
Line[{p1, p1 + AngleVector[[Theta] + [Delta]]*0.3}], AbsoluteDashing[{6, 3}], Black, Line[{p, p1 + dirvec*0.3}], Gray, Line[{p, c}], Line[{c, goal}], Line[{goal, p}], Line[{c, p1}], Orange, Circle[c, R(*,{a1,a2}*)], AbsolutePointSize[8], White, Point[p], Red, Point[c], Darker@Green, Point[goal], Red, Text[Style[ "[Delta]=" <> ToString@Round[[Delta]*180/Pi, 0.01] <> "[Degree]", FontSize -> 16], p1 + dirvec*0.5], Text["!(*SubscriptBox[(d), (l)])=" <> ToString@Round[dl, 0.01], (p + goal)/2 + {0, 0.1}]},
ImageSize -> 600, PlotRange -> 1.5 {{-1.5, 1.5}, {-0.5, 1.5}},
Axes -> False], {{p, {0, 0}}, Locator, Appearance -> Graphics@Point[{0, 0}]}, {{goal, {0.16, 0.12}}, Locator, Appearance -> Graphics[{Green, Point[{0, 0}]}]}, {{[Theta], Pi/6}, 0, 2 Pi, 0.01}, TrackedSymbols :> True, Initialization :> {goal = {0.16, 0.12}}]
审核编辑 :李倩
全部0条评论
快来发表一下你的评论吧 !