NMPC轨迹跟踪控制器推导及Simulink验证

NMPC轨迹跟踪控制器推导及Simulink验证

NMPC与MPC的不同特点

MPC(模型预测控制)：

1. 线性或线性化模型：MPC通常基于线性系统模型，或者将非线性系统模型在工作点附近进行线性化处理。
2. 优化问题：在每一个控制周期内，MPC解决一个线性规划或二次规划问题，以优化未来的控制行为。
3. 适用范围：适用于线性系统或者可以在操作点附近线性化的非线性系统。
4. 计算复杂度：相对较低，因为优化问题相对简单。

NMPC(非线性模型预测控制)：

1. 非线性模型：NMPC直接使用非线性系统模型，不进行线性化处理。
2. 优化问题：在每一个控制周期内，NMPC解决一个非线性规划问题，这通常比线性或二次规划问题更复杂。
3. 适用范围：适用于非线性系统，尤其是当线性化处理无法准确描述系统动态时。
4. 计算复杂度：相对较高，因为非线性优化问题通常更难求解。

NMPC轨迹跟踪控制器推导

一 系统模型建立

将非线性系统表述如下：
$$\dot{X}=f(X,u)$$

二 预测区间状态和变量推导

采用4阶Runge-Kutta方法对系统状态变量进行预测：

针对非线性微分方程$\dot{X}=f(X,u)$，以时间间隔$\Delta t$进行数值求解：

$$\{\begin{array}{rl}{k}_1& =f(X_t,u_t)\\ k_2& =f(X_t+k_1\frac{\Delta t}{2},u_t)\\ k_3& =f(X_t+k_2\frac{\Delta t}{2},u_t)\\ k_4& =f(X_t+k_3\Delta t,u_t)\\ X_{t+1}& =X_t+(k_1+2k_2+2k_3+k_4)\frac{\Delta t}{6}\end{array}$$

三 代价函数推导

定义代价函数为阶段成本和最终成本之和：
$$J={\int }_t^{t+N\Delta t}L(X(\tau ),u(\tau ))d\tau +V(X(t+N\Delta t))$$
其中，$N$为预测步长，$L、V$分别为阶段成本和最终成本函数。

定义阶段成本函数为：
$$L(X(\tau ),u(\tau ))=(X(\tau )-R_{ef}(\tau ){)}^{T}Q(X(\tau )-R_{ef}(\tau ))+u(\tau {)}^{T}Ru(\tau )$$
包含位置误差成本和控制输入成本两部分。

最终成本函数为：
$$V(X(t+N\Delta t))=X(t+N\Delta t{)}^{T}FX(t+N\Delta t)$$
为最终误差成本。

四 优化求解

通过3. 代价函数推导，采用内点优化方法使得代价$J$最小，进而进行控制，定义优化问题如下：
$$\begin{array}{rl}\underset{u_k}{\min }& J={\int }_t^{t+N\Delta t}L(X(\tau ),u(\tau ))d\tau +V(X(t+N\Delta t))\\ \text{subject to}& u_{min}\le u(\tau )\le u_{max}\end{array}$$
在MATLAB中可使用函数fmincon(fmincon函数介绍)求解：

根据MATLAB中quadprog要求，定义:
$$\{\begin{array}{rl}fun& =J\\ A& =[]\\ b& =[]\\ A_{eq}& =[]\\ b_{eq}& =[]\\ lb& =u_{min}\\ ub& =u_{max}\end{array}$$
调用 fmincon(fun,A,b,Aeq,beq,lb,ub) 即可求解代价函数$J$取最小值时，最优控制输入$u_k$。

基于MPC的倒立摆控制系统

采用这篇文章建立的倒立摆模型进行仿真测试。
在simulink中搭建控制仿真模型如下：

输入计算出的反馈增益$K$仿真结果如下：

相关资料

MATLAB代码和simulink模型点击这里

MPC控制器推导及Simulink验证点击这里

欢迎大家与我交流！！！

Reference：

1. 推导倒立摆模型并使用LQR进行位置跟踪控制：https://blog.csdn.net/weixin_55249340/article/details/140421091?spm=1001.2014.3001.5501
2. MATLAB fmincon函数：https://ww2.mathworks.cn/help/optim/ug/fmincon.html?searchHighlight=optim&s_tid=srchtitle_support_results_9_optim