Pure Pursuit 純跟蹤控制算法

阿克曼轉向阿克曼結構前輪來控制轉向

假設車沿着R的半徑在旋轉，車身前輪到後輪軸距L，因爲車在旋轉
重點理解這句話
所以前輪的方向始終是該圓周的切線，遵循曲率

所以得到前輪的轉角應該爲

$tan(\delta)=\frac{L}{R}$

路徑跟蹤算法

以車輪後中心爲點，需要該機器經過該目標點gx gy,
ld:當前點到要跟隨目標點的距離
$\alpha$:當前的方向和到目標點的方向差
三角公式可得一下函數
$\frac{ld}{sin(2\alpha)}=\frac{R}{sin(90-\alpha)}$
簡化一下如下
$R=\frac{ld}{2sin(\alpha)}$

兩輪差速路徑跟隨組合公式

假如是兩輪機器人，沒有軸距 即可得到機器人路徑跟隨的算法

$R=\frac{ld}{2sin(\alpha)}$
$R=\frac{v}{w}$
兩者結合，消除R
==>
$\frac{v}{w}=\frac{ld}{2sin(\alpha)}$
再結合DWA算法 就可以仿真出來一組基於當前ld ,的vw 集合，最後選擇比較好的路徑算法

阿克曼路徑角度輸出

結合這兩個公式 組合推算當前控制角度
$tan(\delta)=\frac{L}{R}$

$R=\frac{ld}{2sin(\alpha)}$
兩個公式 結合消除R ==>
ld:當前點到要跟隨目標點的距離，該值是根據當前點和目標點的位置，計算得出
$\alpha$:當前的方向和到目標點的方向差， 該值是根據當前航向角和目標點的位置，計算得出
L:阿克曼車型軸間距，前輪中心到後輪中心的距離
$tan(\delta)=\frac{2Lsin(\alpha)}{ld}$

在具體的實際情況中，ld 即目標跟隨點的位置是根據弧度在發生變化的
可以用
$l=kv(t)$

具體代碼測試

https://github.com/AGV-IIT-KGP/PythonRobotics/tree/master/PathTracking/pure_pursuit

參考
https://zhuanlan.zhihu.com/p/48117381