目的

使用簡單的“純跟蹤算法”實現無人車自動泊車或者位姿調整。在泊車或者工業場景，如果空間不夠，那么車輛經常需要做一些大角度的轉向或者倒車，例如叉車。

這些場景與一般的道路行駛場景可能有所區別，道路行駛一般只考慮前進方向的高速行駛，并且轉向曲率不會太大。泊車場景恰好相反，曲率大、速度慢，而且伴隨行駛方向的變化。

道路行駛下的跟蹤已經被研究的比較深入了，那么道路行駛使用的跟蹤算法還適用于倒車場景嗎？本文我們來研究一下這個問題。

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}}]

審核編輯 ：李倩