機器人軌跡跟蹤控制—

機器人軌跡跟蹤控制——CLF-CBF-QP

本次使用MATLAB復現CLF-CBF-QP算法，以實現機器人軌跡跟蹤同時保證安全性能

模型

使用自行車模型來進行模擬機器人的移動動態，具體的模型推導參考車輛運動學模型-自行車模型

采用偏差變量
$\tilde{p} = p - p_{ref} \\ \tilde{u} = u - u_{ref}$
系統偏差模型寫成狀態方程形式為
$\dot{\tilde{p}} = A\tilde{p} + B\tilde{u} = \begin{bmatrix} 0 & 0 & -v\sin(\varphi) & \cos(\varphi) \\ 0 & 0 & v\cos(\varphi) & \sin(\varphi) \\ 0 & 0 & 0 & \frac{\tan(\delta)}{l} \\ 0 & 0 & 0 & 0 \end{bmatrix} \tilde{p} + \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 0 & \frac{v}{l\cos(\delta)^2} \\ 1 & 0 \end{bmatrix} \tilde{u}$
其中，矩陣 $\frac{\partial \tilde{f}(p, u)}{\partial p}$ ， $\frac{\partial \tilde{f}(p, u)}{\partial u}$

算法

算法使用控制障礙函數保證安全性，控制李雅普諾夫函數保證軌跡跟蹤，具體可以參考基于控制障礙函數(Control Barrier Function)的二次規劃（QP）控制
$\begin{equation} \nonumber \begin{aligned} & u^*(x)=\underset{u=(u,\delta)\in\mathbb{R}^m\times\mathbb{R}}{\arg\min}\frac{1}{2}u^TH(x)u + F^T(x)u \\ & s.t. \begin{cases} L_fV(x)+L_gV(x)u + c_3V(x)-\delta\leq0 \\ -L_fh(x)-L_gh(x)u-\alpha (h(x))\leq0 \end{cases} \end{aligned} \end{equation}$

CLF 選取

基于運動學偏差模型，選取CLF $V:R^n\rightarrow R$ 如下
$V(\tilde{p})=\frac{1}{2}\tilde{p}^TS\tilde{p}$
其中，矩陣 $S$ 為 LQR 線性二次調節器過程中產生的 Algebraic Riccaci Equation 等式
$A^TS + SA+Q - SBR^{-1}B^TS = 0$
的解，其中，矩陣 $A$ 、 $B$ 的取值見偏差模型狀態方程，滿足

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/news/900470.shtml
繁體地址，請注明出處：http://hk.pswp.cn/news/900470.shtml
英文地址，請注明出處：http://en.pswp.cn/news/900470.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！