状态空间模型
对于两轮差动平面机器人，运动学模型\(\dot{X} = f(X, u)\)：
\[
\begin{array}{l}
\dot{x}=v \cos \theta \\
\dot{y}=v \sin \theta \\
\dot{\theta}=w
\end{array}
\]
写成矩阵形式：
\[
\left[\begin{array}{l}
\dot{x} \\
\dot{y} \\
\dot{\theta}
\end{array}\right]=\left[\begin{array}{cc}
\cos \theta & 0 \\
\sin \theta & 0 \\
0 & 1
\end{array}\right]\left[\begin{array}{c}
v \\
\omega
\end{array}\right]
\]
对机器人参考点，同样满足上式：
\[
\dot{X}_r = f(X_r, u_r)
\]
上式为非线性方程，不利于优化求解。对上式进行一阶泰勒展开，线性化得到：
\[
\begin{aligned}
\dot{\mathbf{x}}=f\left(\mathbf{x}_{r}, \mathbf{u}_{r}\right) &+\left.\frac{\partial f(\mathbf{x}, \mathbf{u})}{\partial \mathbf{x}}\right|_{\mathbf{x}=\mathbf{x}_{r} \atop \mathbf{u}=\mathbf{u}_{r}}\left(\mathbf{x}-\mathbf{x}_{r}\right)\\
&+\left.\frac{\partial f(\mathbf{x}, \mathbf{u})}{\partial \mathbf{u}}\right|_{\mathbf{x}=\mathbf{x}_{r} \atop \mathbf{u}=\mathbf{u}_{r}}\left(\mathbf{u}-\mathbf{u}_{r}\right)
\end{aligned}
\]
其中\(x_r, y_r,\theta_r,v_r,\omega_r\)为机器人线性化点。
两式相减，得：
\[
\dot{\tilde{\mathbf{x}}}=f_{\mathbf{x}, r} \tilde{\mathbf{x}}+f_{\mathbf{u}, r} \tilde{\mathbf{u}}
\]
其中\(\tilde{\mathbf{x}} = \mathbf{x} -\mathbf{x}_r \quad \tilde{\mathbf{u}} = \mathbf{u} -\mathbf{u}_r\)。
即：
\[
\left[\begin{array}{l}
\dot{x} - \dot{x}_r \\
\dot{y} - \dot{y}_r \\
\dot{\theta} - \dot{\theta}_r
\end{array}\right]=
\left[\begin{array}{ccc}
0 & 0 & -v_{r} \sin \theta_{r} \\
0 & 0 & v_{r} \cos \theta_{r} \\
0 & 0 & 0
\end{array}\right]\left[\begin{array}{c}
x-x_{r} \\
y-y_{r} \\
\theta-\theta_{r}
\end{array}\right]+\left[\begin{array}{cc}
\cos \theta_{r} & 0 \\
\sin \theta_{r} & 0 \\
0 & 1
\end{array}\right]\left[\begin{array}{c}
v-v_{r} \\
w-w_{r}
\end{array}\right]
\]
对上式进行一阶差分离散化，得到状态方程：
\[
\frac{\tilde{\mathbf{x}_{(k+1)}} - \tilde{\mathbf{x}_{(k)}}}{T}=f_{\mathbf{x}, r} \tilde{\mathbf{x}}+f_{\mathbf{u}, r} \tilde{\mathbf{u}}
\]
即：
\[
\tilde{\mathbf{x}}(k+1)=\mathbf{A}(k) \tilde{\mathbf{x}}(k)+\mathbf{B}(k) \tilde{\mathbf{u}}(k)
\]
可以看到，上式为机器人状态空间模型，其中：
\[
\begin{array}{l}
\mathbf{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] \\
\mathbf{B}(k) \triangleq\left[\begin{array}{cc}
\cos \theta_{r}(k) T & 0 \\
\sin \theta_{r}(k) T & 0 \\
0 & T
\end{array}\right]
\end{array}
\]
模型预测控制
将上时进行三次预测，得到：
\[
\begin{aligned}
\tilde{\mathbf{x}}(1)&=\mathbf{A}(0) \tilde{\mathbf{x}}(0)+\mathbf{B}(0) \tilde{\mathbf{u}}(0) \\
\tilde{\mathbf{x}}(2)&=\mathbf{A}(1) \tilde{\mathbf{x}}(1)+\mathbf{B}(1) \tilde{\mathbf{u}}(1) \\
&=\mathbf{A}(1)\mathbf{A}(0) \tilde{\mathbf{x}}(0) + \mathbf{A}(1)\mathbf{B}(0) \tilde{\mathbf{u}}(0) + \mathbf{B}(1) \tilde{\mathbf{u}}(1) \\
\tilde{\mathbf{x}}(3)&=\mathbf{A}(2) \tilde{\mathbf{x}}(2)+\mathbf{B}(2) \tilde{\mathbf{u}}(2) \\
&=\mathbf{A}(2)\mathbf{A}(1)\mathbf{A}(0) \tilde{\mathbf{x}}(0) + \mathbf{A}(2)\mathbf{A}(1)\mathbf{B}(0) \mathbf{A}(2)\tilde{\mathbf{u}}(0) + \mathbf{A}(2)\mathbf{B}(1) \tilde{\mathbf{u}}(1) + \mathbf{B}(2) \tilde{\mathbf{u}}(2) \\
\end{aligned}
\]
自此可得到通式：
\[
\overline{\mathbf{x}}(k+1)=\overline{\mathbf{A}}(k) \tilde{\mathbf{x}}(k \mid k)+\overline{\mathbf{B}}(k) \overline{\mathbf{u}}(k)
\]
其中\(a(m|n)\)表示在\(n\)处的\(m\)次预测：
\[
\overline{\mathbf{x}}(k+1) \triangleq\left[\begin{array}{c}
\tilde{\mathbf{x}}(k+1 \mid k) \\
\tilde{\mathbf{x}}(k+2 \mid k) \\
\vdots \\
\tilde{\mathbf{x}}(k+N \mid k)
\end{array}\right] \quad \overline{\mathbf{u}}(k) \triangleq\left[\begin{array}{c}
\tilde{\mathbf{u}}(k \mid k) \\
\tilde{\mathbf{u}}(k+1 \mid k) \\
\vdots \\
\tilde{\mathbf{u}}(k+N-1 \mid k)
\end{array}\right] \\
\overline{\mathbf{A}}(k) \triangleq\left[\begin{array}{c}
\mathbf{A}(k \mid k) \\
\mathbf{A}(k \mid k) \mathbf{A}(k+1 \mid k) \\
\vdots \\
\alpha(k, 0)
\end{array}\right] \\
\overline{\mathbf{B}}(k) = \left[\begin{array}{cccc}
\mathbf{B}(k \mid k) & \mathbf{0} & \cdots & \mathbf{0} \\
\mathbf{A}(k+1 \mid k) \mathbf{B}(k \mid k) & \mathbf{B}(k+1 \mid k) & \cdots & \mathbf{0} \\
\vdots & \vdots & \ddots & \vdots \\
\alpha(k, 1) \mathbf{B}(k \mid k) & \alpha(k, 2) \mathbf{B}(k+1 \mid k) & \cdots & \mathbf{B}(k+N-1 \mid k)
\end{array}\right] \\
\alpha(k, j) \triangleq \prod_{i=j}^{N-1} \mathbf{A}(k+i \mid k)
\]
考虑轨迹跟踪的例子：
为了能够良好控制机器沿着轨迹运行，我们需要定义一个指标函数，用于描述控制效果，很容易想到，机器运行轨迹与参考轨迹越接近越好，并且机器运行越平稳越好。因此定义目标函数：
\[
\begin{aligned}
\Phi(k)&=\sum_{j=1}^{N} \tilde{\mathbf{x}}^{T}(k+j \mid k) \mathbf{Q} \tilde{\mathbf{x}}(k+j \mid k)\\
&+\tilde{\mathbf{u}}^{T}(k+j-1 \mid k) \mathbf{R} \tilde{\mathbf{u}}(k+j-1 \mid k),
\end{aligned}
\]
其中N是prediction horizon,\(Q\geq 0,R > 0\)是权重矩阵。
则上式可以写成：
\[
\Phi(k)=\overline{\mathbf{x}}^{T}(k+1) \overline{\mathbf{Q}} \overline{\mathbf{x}}(k+1)+\overline{\mathbf{u}}^{T}(k) \overline{\mathbf{R}} \overline{\mathbf{u}}(k)
\]
其中：
\[
\overline{\mathbf{Q}} \triangleq\left[\begin{array}{cccc}
\mathbf{Q} & 0 & \cdots & 0 \\
0 & \mathbf{Q} & \cdots & 0 \\
\vdots & \vdots & \ddots & \vdots \\
0 & 0 & \cdots & \mathbf{Q}
\end{array}\right] \quad \overline{\mathbf{R}} \triangleq\left[\begin{array}{cccc}
\mathbf{R} & 0 & \cdots & 0 \\
0 & \mathbf{R} & \cdots & 0 \\
\vdots & \vdots & \ddots & \vdots \\
0 & 0 & \cdots & \mathbf{R}
\end{array}\right]
\]
带入目标函数得：
\[
\Phi(k)=\frac{1}{2} \overline{\mathbf{u}}^{T}(k) \mathbf{H}(k) \overline{\mathbf{u}}(k)+\mathbf{f}^{T}(k) \overline{\mathbf{u}}(k)+\mathbf{d}(k)
\]
其中：
\[
\begin{aligned}
\mathbf{H}(k) & \triangleq 2\left(\overline{\mathbf{B}}(k)^{T}(k) \overline{\mathbf{Q}} \overline{\mathbf{B}}(k)+\overline{\mathbf{R}}\right) \\
\mathbf{f}(k) & \triangleq 2 \overline{\mathbf{B}}^{T}(k) \overline{\mathbf{Q}} \overline{\mathbf{A}}(k) \tilde{\mathbf{x}}(k \mid k) \\
\mathbf{d}(k) & \triangleq \tilde{\mathbf{x}}^{T}(k \mid k) \overline{\mathbf{A}}^{T}(k) \overline{\mathbf{Q}} \overline{\mathbf{A}}(k) \tilde{\mathbf{x}}(k \mid k)
\end{aligned}
\]
可以看到\(d(k)\)与优化变量不相关，可以舍去。则上式为一个带有约束的二次规划问题（Quadratic Programming）。可以求得全局最优解。
核心代码如下：
bool mpc(Eigen::VectorXd& uout, Eigen::VectorXd& predict, const Eigen::VectorXd& x, const Eigen::VectorXd& u, const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
const Eigen::VectorXd& ref, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, const Eigen::VectorXd& lower, const Eigen::VectorXd& upper, int horizon)
{
int x_dim = B.rows();
int u_dim = B.cols();
int horizon_u = horizon * u_dim;
int horizon_x = horizon * x_dim;
bool result = true;
OSQPSettings *settings = reinterpret_cast<OSQPSettings *>(c_malloc(sizeof(OSQPSettings)));
osqp_set_default_settings(settings);
settings->polish = true;
settings->scaled_termination = true;
//settings->warm_start = true;
settings->verbose = true;
//settings->max_iter = 100;
settings->eps_abs = std::numeric_limits<float>::epsilon();
OSQPWorkspace *workspace = nullptr;
OSQPData *data = reinterpret_cast<OSQPData *>(c_malloc(sizeof(OSQPData)));
std::vector<c_float> p_data; std::vector<c_int> p_indices; std::vector<c_int> p_indptr;
std::vector<c_float> a_data; std::vector<c_int> a_indices; std::vector<c_int> a_indptr;
do{
Eigen::MatrixXd barA = Eigen::MatrixXd::Zero(horizon_x, x_dim);
Eigen::MatrixXd barB = Eigen::MatrixXd::Zero(horizon_x, horizon_u);
Eigen::MatrixXd barQ = Eigen::MatrixXd::Identity(horizon_x, horizon_x);
Eigen::MatrixXd barR = Eigen::MatrixXd::Identity(horizon_u, horizon_u);
Eigen::VectorXd l = Eigen::VectorXd::Zero(horizon_u);
Eigen::VectorXd u = Eigen::VectorXd::Zero(horizon_u);
for(int pos = 0; pos < horizon; pos ++){
if(!pos){
barA.block(0, 0, x_dim, x_dim) = A;
continue;
}
barA.block(x_dim * pos, 0, x_dim, x_dim) = barA.block(x_dim * (pos - 1), 0, x_dim, x_dim) * A;
}
Eigen::Matrix3d baroa = Eigen::Matrix3d::Identity();
for(int pos = 0; pos < horizon; pos ++){
// barB
for(int cols = 0; cols < pos; cols ++){
barB.block(pos * x_dim, cols * u_dim, x_dim, u_dim) = barA.block((pos - cols - 1) * x_dim, 0, x_dim, x_dim) * B;
}
barB.block(pos * x_dim, pos * u_dim, x_dim, u_dim) = B;
// barQ barR
barQ.block(pos * x_dim, pos * x_dim, x_dim, x_dim) = Q;
barR.block(pos * u_dim, pos * u_dim, u_dim, u_dim) = R;
l.segment(pos * u_dim, u_dim) = lower;
u.segment(pos * u_dim, u_dim) = upper;
}
Eigen::MatrixXd p = 2. * (barB.transpose() * barQ * barB + barR);
Eigen::VectorXd q = 2. * barB.transpose() * barQ * barA * (x - ref.segment(0, x_dim));
{
int index = 0;
for(int col = 0; col < horizon_u; col ++){
a_indptr.push_back(index);
for(int row = 0; row < horizon_u; row ++){
if(col != row){continue;}
a_data.push_back(1.);
a_indices.push_back(row);
index ++;
}
}
a_indptr.push_back(index);
}
{
int index = 0;
for(int col = 0; col < horizon_u; col ++){
p_indptr.push_back(index);
for(int row = 0; row <= col; row ++){
p_data.push_back(p(row, col));
p_indices.push_back(row);
index ++;
}
}
p_indptr.push_back(index);
}
data->n = horizon_u;
data->m = horizon_u;
data->A = csc_matrix(horizon_u, horizon_u, a_data.size(), a_data.data(), a_indices.data(), a_indptr.data());
data->P = csc_matrix(horizon_u, horizon_u, p_data.size(), p_data.data(), p_indices.data(), p_indptr.data());
data->q = q.data();
data->l = l.data();
data->u = u.data();
osqp_setup(&workspace, data, settings);
osqp_solve(workspace);
auto status = workspace->info->status_val;
if(status < 0){
result = false;
break;
}
uout.resize(horizon_u);
for(int pos = 0; pos < horizon_u; pos+= u_dim){
for(int id = 0; id < u_dim; id ++){
uout(id + pos) = workspace->solution->x[id + pos];
}
}
predict = barA * (x - ref.segment(0, x_dim)) + barB * uout;// + ref.segment(0, x_dim);
for(int pos = 0; pos < horizon; pos++){
predict.segment(pos * x_dim, x_dim) = ref.segment(0, x_dim) + predict.segment(pos * x_dim, x_dim);
}
for(int pos = 0; pos < horizon_u; pos+= u_dim){
uout.segment(pos, u_dim) = ref.segment(x_dim, u_dim) + uout.segment(pos, u_dim);
}
}while(false);
if(workspace){osqp_cleanup(workspace);}
if(data && data->A){c_free(data->A);}
if(data && data->P){c_free(data->P);}
if(data){c_free(data);}
if(settings){c_free(settings);}
return result;
}
void mpc(const Eigen::Vector3d& x, const Eigen::Vector2d& u, const Eigen::Matrix<double, 5, 1>& ref, Eigen::Vector2d& uresult, Eigen::VectorXd& predict, float dt)
{
Eigen::MatrixXd Q = Eigen::Matrix3d::Identity(); Q.diagonal() << 1., 1., 0.5;
Eigen::MatrixXd R = Eigen::Matrix2d::Identity(); R.diagonal() << 0.1, 0.1;
Eigen::VectorXd ulower = Eigen::VectorXd::Zero(2);ulower << -0.5, -1.5;
Eigen::VectorXd uupper = Eigen::VectorXd::Zero(2);uupper << 0.5, 1.5;
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3, 3);
Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, 2);
A << 1., 0., (-ref[3] * std::sin(ref[2]) * dt),
0., 1., (ref[3] * std::cos(ref[2]) * dt),
0., 0., 1.;
B << std::cos(ref[2]) * dt, 0,
std::sin(ref[2]) * dt, 0.,
0., dt;
Eigen::VectorXd control;
mpc(control, predict, x, u, A, B, ref, Q, R, ulower, uupper, 20);
uresult[0] = control(0);
uresult[1] = control(1);
}
Reference:
[1] httpcse.lab.imtlucca.it~bemporadmpc_course.html [2] https://github.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi [3] https://www.youtube.com/playlist?list=PLK8squHT_Uzej3UCUHjtOtm5X7pMFSgAL [4] https://github.com/robotology/osqp-eigen [5] model predictive control of a mobile robot using linearization [6] Point Stabilization of Mobile Robots with Nonlinear Model Predictive Control [7] <无人驾驶车辆模型预测控制> [8] https://blog.csdn.net/weixin_42143018/article/details/102868432