Path Tracking

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

極坐標(biāo)下的線性控制器

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

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

如圖轉(zhuǎn)換到誤差極坐標(biāo)系下

\(\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} $$

設(shè)計(jì)線性控制器
\(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} \]

求出特征多項(xiàng)式\((\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\)

實(shí)際上，靜態(tài)的參數(shù)容易使機(jī)器人陷入局部極值，我們做了動(dòng)態(tài)調(diào)參。當(dāng)\(\rho\)比較大時(shí)，我們希望\(\alpha\)影響更大，反之希望\(\beta\)影響更大，同時(shí)希望不出現(xiàn)突變，因此設(shè)定了一個(gè)閾值\(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}\]

轉(zhuǎn)載于:https://www.cnblogs.com/7391-KID/p/10903404.html

總結(jié)

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

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