基于MPC的移动机器人轨迹跟踪控制matlab例程

基于MPC的移动机器人轨迹跟踪控制matlab例程

github地址

https://github.com/zzy5510/MPC_control_robot

移动机器人建模

因为所有机器人均可转化为独轮车模型,因此本项目采用独轮车模型。
( x ˙ c y ˙ c θ ˙ c ) = ( cos ⁡ θ c 0 sin ⁡ θ c 0 0 1 ) ( v c ω c ) \left(\begin{array}{c}\dot{x}_{\mathrm{c}} \\ \dot{y}_{\mathrm{c}} \\ \dot{\theta}_{\mathrm{c}}\end{array}\right)=\left(\begin{array}{cc}\cos \theta_{\mathrm{c}} & 0 \\ \sin \theta_{\mathrm{c}} & 0 \\ 0 & 1\end{array}\right)\left(\begin{array}{c}v_{\mathrm{c}} \\ \omega_{\mathrm{c}}\end{array}\right) x˙cy˙cθ˙c=cosθcsinθc0001(vcωc)
vc为输入速度,wc为输入角速度。积分后离散,得到离散化的状态方程:
( x k + 1 y k + 1 φ k + 1 ) = ( x k + v k T s cos ⁡ φ k y k + v k T s sin ⁡ φ k φ k + w k T s ) \left(\begin{array}{c}x_{k+1} \\ y_{k+1} \\ \varphi_{k+1}\end{array}\right)=\left(\begin{array}{c}x_{k}+v_{k} T_{s} \cos \varphi_{k} \\ y_{k}+v_{k} T_{s} \sin \varphi_{k} \\ \varphi_{k}+w_{k} T_{s}\end{array}\right) xk+1yk+1φk+1=xk+vkTscosφkyk+vkTssinφkφk+wkTs
可以看出,该状态方程组是非线性的,角度和位置之间存在耦合。然而基于状态方程的MPC要求方程为线性,因此需要对该方程进行处理。线性化方法参考了文献《基于模型预测控制的自主移动机器人跟踪控制研究_周靖期》。
将方程绕 ( x r , u r ) \left(x_{\mathrm{r}}, u_{\mathrm{r}}\right) (xr,ur)展开,得:
x ˙ = f ( x r , u r ) + f x , r ( x − x r ) + f u , r ( u − u r ) \dot{x}=f\left(x_{r}, u_{r}\right)+f_{x, r}\left(x-x_{r}\right)+f_{u, r}\left(u-u_{r}\right) x˙=f(xr,ur)+fx,r(xxr)+fu,r(uur)
其中, x r x_{r} xr, y r y_{r} yr为待跟踪的轨迹位置。 f x , r f_{x, r} fx,r, f y , r f_{y,r} fy,r为f对x和y的偏导,即:
x ~ ˙ = f x , r x ~ + f u , r u ~ \dot{\tilde{x}}=f_{x, \mathrm{r}} \tilde{x}+f_{u, \mathrm{r}} \tilde{u} x~˙=fx,rx~+fu,ru~
离散化后,得:
x ~ ( k + 1 ) = A ( k ) x ~ ( k ) + B ( k ) u ~ ( k ) \tilde{x}(k+1)=A(k) \tilde{x}(k)+B(k) \tilde{u}(k) x~(k+1)=A(k)x~(k)+B(k)u~(k)
A ( k ) ≜ ( 1 0 − v r ( k ) sin ⁡ θ r ( k ) T 0 1 v r ( k ) cos ⁡ θ r ( k ) T 0 0 1 ) B ( k ) ≜ ( cos ⁡ θ r ( k ) T 0 sin ⁡ θ r ( k ) T 0 0   T ) A(k) \triangleq\left(\begin{array}{ccc} 1 & 0 & -v_{r}(k) \sin \theta_{r}(k) T \\ 0 & 1 & v_{r}(k) \cos \theta_{r}(k) T \\ 0 & 0 & 1 \end{array}\right) \quad B(k) \triangleq\left(\begin{array}{ccc} \cos \theta_{r}(k) T & 0 \\ \sin \theta_{r}(k) \mathrm{T} & 0 \\ 0 & \mathrm{~T} \end{array}\right) A(k)100010vr(k)sinθr(k)Tvr(k)cosθr(k)T1B(k)cosθr(k)Tsinθr(k)T000 T
离散化的方法参考https://wenku.baidu.com/view/ab37b916fbb069dc5022aaea998fcc22bdd14314.html

预测模型

预测模型的过程如下:
将预测过程中的所有量结合成向量:
x ˉ ( k + 1 ) ≜ ( x ~ ( k + 1 ∣ k ) x ~ ( k + 2 ∣ k ) ⋮ x ~ ( k + N ∣ k ) ) u ˉ ( k ) ≜ ( u ~ ( k ∣ k ) u ~ ( k + 1 ∣ k ) ⋮ u ~ ( k + N − 1 ∣ k ) ) \bar{x}(k+1) \triangleq\left(\begin{array}{c} \tilde{x}(k+1 \mid k) \\ \tilde{x}(k+2 \mid k) \\ \vdots \\ \tilde{x}(k+N \mid k) \end{array}\right) \quad \bar{u}(k) \triangleq\left(\begin{array}{c} \tilde{u}(k \mid k) \\ \tilde{u}(k+1 \mid k) \\ \vdots \\ \tilde{u}(k+N-1 \mid k) \end{array}\right) xˉ(k+1)x~(k+1k)x~(k+2k)x~(k+Nk)uˉ(k)u~(kk)u~(k+1k)u~(k+N1k)
x ˉ ( k + 1 ) = A ˉ ( k ) x ~ ( k ∣ k ) + B ˉ ( k ) u ˉ ( k ) \bar{x}(k+1)=\bar{A}(k) \tilde{x}(k \mid k)+\bar{B}(k) \bar{u}(k) xˉ(k+1)=Aˉ(k)x~(kk)+Bˉ(k)uˉ(k)
其中,
A ˉ ( k ) ≜ ( A ( k ∣ k ) A ( k ∣ k ) A ( k + 1 ∣ k ) ⋮ A ( k ∣ k ) A ( k + 1 ∣ k ) . . . A ( k + P − 1 ∣ k ) ) \bar{A}(k) \triangleq\left(\begin{array}{c} A(k \mid k) \\ A(k \mid k) A(k+1 \mid k) \\ \vdots \\ \\A(k \mid k) A(k+1 \mid k)...A(k+P-1\mid k) \end{array}\right) Aˉ(k)A(kk)A(kk)A(k+1k)A(kk)A(k+1k)...A(k+P1k)
B ˉ ( k ) ≜ ( B ( k ∣ k ) 0 ⋯ 0 A ( k + 1 ∣ k ) B ( k ∣ k ) B ( k + 1 ∣ k ) ⋯ 0 ⋮ ⋱ ⋮ A ( k + 1 ∣ k ) . . . A ( k + P − 1 ∣ k ) B ( k ∣ k ) A ( k + 2 ∣ k ) . . . A ( k + P − 1 ∣ k ) B ( k + 1 ∣ k ) ⋯ B ( k + N − 1 ∣ k ) ) \bar{B}(k) \triangleq\left(\begin{array}{cccc} B(k \mid k) & 0 & \cdots & 0 \\ A(k+1 \mid k) B(k \mid k) & B(k+1 \mid k) & \cdots & 0 \\ \vdots & & \ddots & \vdots \\ \\A(k+1 \mid k)...A(k+P-1\mid k) B(k \mid k) & A(k+2 \mid k)...A(k+P-1\mid k) B(k+1 \mid k) & \cdots & B(k+N-1 \mid k) \end{array}\right) Bˉ(k)B(kk)A(k+1k)B(kk)A(k+1k)...A(k+P1k)B(kk)0B(k+1k)A(k+2k)...A(k+P1k)B(k+1k)00B(k+N1k)
这里将预测时域和控制时域设为同一个值P。

反馈校正

将模型预测的结果与车辆的实际位置做对比,得到误差值e。该值之后会用在滚动优化上。

滚动优化

待优化的二次型多项式为:
ϕ ( k ) = 1 2 u ˉ T ( k ) H ( k ) u ˉ ( k ) + f T ( k ) u ˉ ( k ) + a ( k ) \phi(k)=\frac{1}{2} \bar{u}^{\mathrm{T}}(k) H(k) \bar{u}(k)+f^{\mathrm{T}}(k) \bar{u}(k)+a(k) ϕ(k)=21uˉT(k)H(k)uˉ(k)+fT(k)uˉ(k)+a(k)
其中,
H ( k ) ≜ 2 ( B ˉ T ( k ) Q ˉ B ˉ ( k ) + R ˉ ) f ( k ) ≜ 2 B ˉ T ( k ) Q ˉ A ˉ ( k ) x ~ ( k ∣ k ) a ( k ) ≜ x ~ T ( k ∣ k ) A ˉ T ( k ) Q ˉ A ˉ ( k ) x ~ ( k ∣ k ) \begin{array}{l} H(k) \triangleq 2\left(\bar{B}^{\mathrm{T}}(k) \bar{Q} \bar{B}(k)+\bar{R}\right) \\ f(k) \triangleq 2 \bar{B}^{\mathrm{T}}(k) \bar{Q} \bar{A}(k) \tilde{x}(k \mid k) \\ a(k) \triangleq \tilde{x}^{\mathrm{T}}(k \mid k) \bar{A}^{\mathrm{T}}(k) \bar{Q} \bar{A}(k) \tilde{x}(k \mid k) \end{array} H(k)2(BˉT(k)QˉBˉ(k)+Rˉ)f(k)2BˉT(k)QˉAˉ(k)x~(kk)a(k)x~T(kk)AˉT(k)QˉAˉ(k)x~(kk)

a与待求解的u无关。得到一个标准的QP问题,可以用matlab内置函数直接求解,也可以手动求解。

你可能感兴趣的:(机器人)