Path Tracking

這一部分利用極坐標下的線性控制器完成路徑跟蹤，主要思路就是把路徑拆開，變成p2p問題，即從當前姿態(tài)到目標姿態(tài)。

極坐標下的線性控制器

設速度\(v\)，角速度\(\omega\)，則有兩輪差速機器人在慣性坐標系下的模型

\[ \begin{bmatrix} \dot x\\ \dot y\\ \dot \theta \end{bmatrix}= \begin{bmatrix} cos \theta & 0\\ sin \theta & 0\\ 0 & 1 \end{bmatrix} \begin{bmatrix} v\\ \omega \end{bmatrix} \]

如圖轉換到誤差極坐標系下

\(\rho=\sqrt{\Delta x^2+\Delta y^2}\)
\(\alpha=-\theta+arctan2(\Delta y, \Delta x)\)
\(\beta=-\theta-\alpha\)

$$ \begin{bmatrix} \dot \rho\\ \dot \alpha\\ \dot \beta \end{bmatrix}= \begin{bmatrix} -cos \alpha & 0\\ \frac{sin \alpha}{\rho} & -1\\ -\frac{sin \alpha}{\rho} & 0 \end{bmatrix} \begin{bmatrix} v\\ \omega \end{bmatrix} $$

設計線性控制器
\(v = k_{\rho}\rho\)
$\omega = k_{\alpha}\alpha + k_{\beta}\beta $

近似化以后

\[ \begin{bmatrix} \dot \rho\\ \dot \alpha\\ \dot \beta \end{bmatrix}= \begin{bmatrix} -k_{\rho} & 0 & 0\\ 0 & -(k_{\alpha}-k_{\rho}) & k_{\beta}\\ 0 & -k_{\rho} & 0 \end{bmatrix} \begin{bmatrix} \rho \\ \alpha \\ \beta \end{bmatrix} \]

求出特征多項式\((\lambda+k_{\rho})[\lambda^2+\lambda(k_{\alpha}-k_{\rho})-k_{\rho}k_{\beta}]\)
從而得到收斂條件
\(k_{\rho}\ge 0, k_{\beta}\le 0, k_{\alpha}-k_{\rho}\ge 0\)

實際上，靜態(tài)的參數(shù)容易使機器人陷入局部極值，我們做了動態(tài)調參。當\(\rho\)比較大時，我們希望\(\alpha\)影響更大，反之希望\(\beta\)影響更大，同時希望不出現(xiàn)突變，因此設定了一個閾值\(th\)，將控制器改為
\[ \omega= \begin{cases} (\frac{\rho}{th})^2 k_{\alpha}\alpha + k_{\beta}\beta & \rho < th\\ k_{\alpha}\alpha + (\frac{th}{\rho})^2 k_{\beta}\beta & \text{else} \end{cases}\]

轉載于:https://www.cnblogs.com/7391-KID/p/10903404.html

總結

以上是生活随笔為你收集整理的移动机器人建图与导航代码实现——3.Path Tracking的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內容還不錯，歡迎將生活随笔推薦給好友。