目的

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

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

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

Reeds-Sheep曲線

假設無人車的運動路徑是已知的，筆者使用 https://github.com/hbanzhaf/steering_functions 中提出的曲率連續(xù)的改進Reeds-Sheep曲線生成路徑。

程序輸出的路徑是一系列離散的點，點之間的距離可以自定義，筆者選擇每5毫米一個點，程序中設置DISCRETIZATION=0.005。

路徑采用nav_msgs::Path消息發(fā)出。

純跟蹤算法

純跟蹤算法（Pure Pursuit）首先要指定一個被跟蹤的目標點。

原始版本的純跟蹤算法只討論了跟蹤無人車前方的點，對于Reeds-Sheep曲線這種包含運動方向變化的曲線，無人車既需要前進也需要后退，但是想實現(xiàn)后退也非常簡單。

筆者將被跟蹤的目標點稱為局部目標（local goal）無人車真正最終的靜態(tài)目標點則稱為全局目標（global goal）。

純跟蹤需要無人車的定位，仿真時假設這個定位信息由ROS中的/base_pose_ground_truth消息給出。局部目標的計算方式是，遍歷路徑，找到第一個離無人車≥ d l 的路徑點。

d l 就是前視距離，d l 越小跟蹤精度越高，但是越容易導致震蕩。機器人在運動時，這個局部目標也會更新。

如果找到的局部目標落在了無人車的后方，此時意味著無人車需要后退，只需要將速度取負值即可，前輪轉(zhuǎn)角不用變。

出現(xiàn)的問題

1.轉(zhuǎn)折點

在仿真時出現(xiàn)了一些問題。首先，最困難的是對于尖點（cusp）怎么處理。因為很多情況下，Reeds-Sheep曲線都包含尖點，在尖點處車輛會改變運動方向。

如果使用純跟蹤算法跟蹤這個路徑，那么在尖點處會出現(xiàn)一個問題。因為純跟蹤算法總要指定一個跟蹤點，這個跟蹤點一般在車輛前方或者后方一定距離（d l ）處。

在向尖點運動時，車輛不會正好處于尖點上，而是提前離開。下圖中的d l = 0.2后面也采用這一數(shù)值。

圖中的黃點是被跟蹤的局部目標，紅色點表示無人車后輪軸中心處的實時位置。

這就導致車輛沒有完全位于路徑上，進而導致后面的跟蹤出現(xiàn)橫向偏差（如下圖所示），即使采用曲率連續(xù)的Reeds-Sheep曲線版本也沒有用。

這是純跟蹤算法本身的問題嗎？不是，純跟蹤算法完全可以跟得上，我們?yōu)榱税踩ǔ０演敵鼋嵌冉o限幅了，如果解除限幅你就會發(fā)現(xiàn)純跟蹤算法完全可以準確的跟蹤。

但是實際使用時我們又不可能解除限幅，所以怎么解決這個問題呢？

一種是直接增大一點Reeds-Sheep曲線的最小轉(zhuǎn)向半徑，令其略大于車輛的真實最小轉(zhuǎn)向半徑，筆者嘗試增加了約10%，跟蹤情況如下圖。

另一種方法是增加尖點（cusp）部分的長度，這可以通過改變主程序（steering_functions_node.cpp）中的sigma_max_變量實現(xiàn)，sigma_max_越小，過渡部分越長，最好大于d l 試驗發(fā)現(xiàn)取sigma_max_=0.5左右就可以。

控制指令如下圖所示。

速度單獨進行規(guī)劃，然后疊加到路徑上，如下圖所示。

2.定位誤差

前面的控制都假設定位是完美的，不存在定位誤差。如果加入定位誤差，純跟蹤算法的表現(xiàn)會怎么樣呢？

我們用隨機數(shù)來模擬定位誤差，定位誤差一般是正太分布的，因此用正態(tài)分布函數(shù)std::normal_distribution生成隨機數(shù)，均值總是取0，標準差決定了誤差的范圍。

首先取小的標準差—— 1mm，無人車的表現(xiàn)如下圖所示，無人車的跟蹤效果比較好。

但是前輪轉(zhuǎn)角的變化卻非常劇烈，如下圖所示。這還僅僅是1mm左右的誤差，這在實際中是幾乎不可能達到的。

標準差為1cm時的表現(xiàn)如下圖所示，已經(jīng)產(chǎn)生了明顯的橫向跟蹤偏差。

此時前輪轉(zhuǎn)角已經(jīng)慘不忍睹了，如下圖所示，這還是1cm左右的誤差，實際中無人車的定位要達到1cm也是很困難的。

標準差為5cm時的表現(xiàn)如下圖所示，這個誤差是一般室外衛(wèi)星定位的誤差范圍，也就是常見的誤差，此時無人車徹底無法跟蹤。

不僅前輪轉(zhuǎn)角更瘋狂了，而且由于橫向偏差已經(jīng)超過了前視距離d l ，局部目標已經(jīng)出現(xiàn)在無人車側(cè)面了，導致無人車完全無法跟蹤了，如下圖所示。

這說明純跟蹤算法對定位誤差是極其敏感的，在實際應用時這是個非常嚴重的問題。

算法理解

為了易于理解純跟蹤算法，筆者用Mathematica設計了一個小程序，你可以用鼠標拖動目標點（綠色點），并觀察前輪的轉(zhuǎn)角，如下圖。

目標點是純跟蹤算法中的核心概念，這個目標點是人為設計或者選擇的。跟蹤性能的好壞不僅取決于控制參數(shù)的選擇，目標點的選擇也起到重要的作用。

當目標點選取的不好時，例如距離無人車當前位置過近，則會出現(xiàn)控制量劇烈變化。

你也可以用鼠標拖動無人車的參考點，觀察前輪的轉(zhuǎn)角，如下圖。從圖中可以發(fā)現(xiàn)，在距離目標比較近時，純跟蹤算法的表現(xiàn)很糟糕，參考點位置有一點點改變都會導致前輪轉(zhuǎn)角劇烈變化。

但是無人車的定位本身是必然存在偏差的，所以純跟蹤算法在前視距離短時穩(wěn)定性并不好。

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

版權(quán)聲明：本文為CSDN博主「robinvista」的原創(chuàng)文章，遵循CC 4.0 BY-SA版權(quán)協(xié)議，轉(zhuǎn)載請附上原文出處鏈接及本聲明。

原文鏈接：

https://blog.csdn.net/robinvista/article/details/118538616