中核工建设集团有限公司网站,网站建设管理中se是什么意思,小程序seo帝搜软件sem880官网,中国建设网官方网站平台本次使用MATLAB复现CLF-CBF-QP算法，以实现机器人轨迹跟踪同时保证安全性能
模型
使用自行车模型来进行模拟机器人的移动动态，具体的模型推导参考车辆运动学模型-自行车模型
采用偏差变量 p ~ = p − p r e f u ~ = u − u r e f \tilde{p} = p - p_{ref} \\ \tilde{u} = …本次使用MATLAB复现CLF-CBF-QP算法，以实现机器人轨迹跟踪同时保证安全性能
模型
使用自行车模型来进行模拟机器人的移动动态，具体的模型推导参考车辆运动学模型-自行车模型
采用偏差变量 p ~ = p − p r e f u ~ = u − u r e f \tilde{p} = p - p_{ref} \\ \tilde{u} = u - u_{ref} p~=p−prefu~=u−uref 系统偏差模型写成状态方程形式为 p ~ ˙ = A p ~ + B u ~ = [ 0 0 − v sin ( φ ) cos ( φ ) 0 0 v cos ( φ ) sin ( φ ) 0 0 0 tan ( δ ) l 0 0 0 0 ] p ~ + [ 0 0 0 0 0 v l cos ( δ ) 2 1 0 ] u ~ \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} p~˙=Ap~+Bu~= 00000000−vsin(φ)vcos(φ)00cos(φ)sin(φ)ltan(δ)0 p~+ 000100lcos(δ)2v0 u~ 其中，矩阵 A = ∂ f ~ ( p , u ) ∂ p A = \frac{\partial \tilde{f}(p, u)}{\partial p} A=∂p∂f~(p,u)， B = ∂ f ~ ( p , u ) ∂ u B = \frac{\partial \tilde{f}(p, u)}{\partial u} B=∂u∂f~(p,u)
算法
算法使用控制障碍函数保证安全性，控制李雅普诺夫函数保证轨迹跟踪，具体可以参考基于控制障碍函数(Control Barrier Function)的二次规划（QP）控制 u ∗ ( x ) = arg min u = ( u , δ ) ∈ R m × R 1 2 u T H ( x ) u + F T ( x ) u s . t . { L f V ( x ) + L g V ( x ) u + c 3 V ( x ) − δ ≤ 0 − L f h ( x ) − L g h ( x ) u − α ( h ( x ) ) ≤ 0 \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} u∗(x)=u=(u,δ)∈Rm×Rargmin21uTH(x)u+FT(x)us.t.{LfV(x)+LgV(x)u+c3V(x)−δ≤0−Lfh(x)−Lgh(x)u−α(h(x))≤0
CLF 选取
基于运动学偏差模型，选取CLF V : R n → R V:R^n\rightarrow R V:Rn→R 如下 V ( p ~ ) = 1 2 p ~ T S p ~ V(\tilde{p})=\frac{1}{2}\tilde{p}^TS\tilde{p} V(p~)=21p~TSp~ 其中，矩阵 S S S为 LQR 线性二次调节器过程中产生的 Algebraic Riccaci Equation 等式 A T S + S A + Q − S B R − 1 B T S = 0 A^TS + SA+Q - SBR^{-1}B^TS = 0 ATS+SA+Q−SBR−1BTS=0 的解，其中，矩阵 A A A、 B B B的取值见偏差模型状态方程，满足 V ˙ = d d t p ~ T S p ~ = − ( p ~ T Q p ~ +
