4、MPC开发环境搭建：Ubuntu系统配置、ROS2安装、Eigen库、OSQP求解器、CasADi框架

说实话，做MPC控制器部署，最怕的不是算法调参，而是环境搭到一半卡住了。我见过太多同事，花了两天装ROS2，结果因为源没换对，编译报错直接劝退。今天咱们就把这套环境一次搞定。

4.1 Ubuntu系统配置——打好地基

我个人习惯用Ubuntu 22.04 LTS。为什么？因为ROS2 Humble正好支持它，而且Eigen和OSQP的预编译包都很全。你想想看，要是用20.04，后面装CasADi还得自己编译，多折腾。

第一步：换源

别用官方源，慢得让人抓狂。我一般用清华源或者中科大源。直接改 /etc/apt/sources.list：

sudo sed -i 's@//.*archive.ubuntu.com@//mirrors.tuna.tsinghua.edu.cn@g' /etc/apt/sources.list
sudo apt update && sudo apt upgrade -y

第二步：装基础工具

嗯，这里要注意，有些包是后面编译ROS2 workspace必须的，提前装好省得后面报错。

sudo apt install -y \
    build-essential \
    cmake \
    git \
    wget \
    python3-pip \
    python3-venv \
    libeigen3-dev \
    liblapack-dev \
    libblas-dev

4.2 ROS2安装——别踩这个坑

ROS2的安装文档其实挺全的，但有个坑我踩过——一定要先设locale。否则后面rosdep会报奇怪的编码错误。

sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8

然后添加ROS2的apt源：

sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt update && sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update

装桌面版（带rviz2和demo）：

sudo apt install ros-humble-desktop -y

验证安装：

source /opt/ros/humble/setup.bash
ros2 run demo_nodes_cpp talker

如果看到“Publishing: 'Hello World'”，说明ROS2跑起来了。

4.3 Eigen库——线性代数引擎

Eigen是MPC里最常用的线性代数库。说白了，你写MPC的QP问题，状态矩阵、控制矩阵、代价矩阵，全得靠它。

Ubuntu下直接apt装：

sudo apt install libeigen3-dev

装完头文件在 /usr/include/eigen3/。我习惯在CMakeLists.txt里这样引用：

find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})

一个小例子： 用Eigen定义MPC的预测矩阵

#include <Eigen/Dense>
using namespace Eigen;

int main() {
    // 状态维度 n=4, 控制维度 m=2, 预测步长 N=10
    const int n = 4, m = 2, N = 10;
    MatrixXd A(n, n);  // 系统矩阵
    MatrixXd B(n, m);  // 输入矩阵
    
    // 构建预测矩阵（简化版）
    MatrixXd Phi(n*N, n);
    MatrixXd Gamma(n*N, m*N);
    
    // 填充Phi和Gamma...
    // 这里省略具体填充逻辑，后面章节会详细讲
    
    return 0;
}

4.4 OSQP求解器——QP问题的利器

OSQP是专门解凸二次规划（QP）的求解器。MPC的核心就是每步解一个QP问题，所以OSQP是咱们的“发动机”。

安装方式： 我推荐从源码编译，这样能控制优化选项。

git clone --recursive https://github.com/osqp/osqp.git
cd osqp
mkdir build && cd build
cmake -G "Unix Makefiles" ..
cmake --build .
sudo cmake --build . --target install

装完测试一下：

#include "osqp.h"

int main() {
    // 定义一个简单的QP问题
    // min 0.5 x^T P x + q^T x
    // s.t. l <= A x <= u
    
    c_float P_x[3] = {4.0, 1.0, 2.0};  // 上三角
    c_int P_nnz = 3;
    c_int P_i[3] = {0, 0, 1};
    c_int P_p[3] = {0, 1, 3};
    
    // ... 设置其他参数
    // 具体用法后面章节会展开
    
    return 0;
}

4.5 CasADi框架——符号计算与自动微分

CasADi是MPC领域的神器。它支持符号计算、自动微分，还能直接生成C代码。说白了，你写一个MPC的优化问题，它能帮你自动求导、生成求解器。

安装方式： 我建议用Python接口，开发效率高。C++接口适合部署。

pip install casadi

验证安装：

import casadi as ca

# 定义符号变量
x = ca.SX.sym('x', 2)  # 2维状态
u = ca.SX.sym('u', 1)  # 1维控制

# 定义系统动态
x_next = ca.vertcat(
    x[0] + 0.1 * (x[1] + u),
    x[1] + 0.1 * (-x[0] + u)
)

# 自动求导
J = ca.jacobian(x_next, x)
print(J)

CasADi + OSQP 的典型用法：

import casadi as ca
import osqp
import numpy as np

# 用CasADi构建MPC问题
N = 10  # 预测步长
nx = 4  # 状态维度
nu = 2  # 控制维度

# 定义优化变量
X = ca.SX.sym('X', nx, N+1)
U = ca.SX.sym('U', nu, N)

# 构建代价函数和约束
cost = 0
g = []
for k in range(N):
    cost += ca.mtimes([X[:,k].T, Q, X[:,k]]) + ca.mtimes([U[:,k].T, R, U[:,k]])
    g.append(X[:,k+1] - (A @ X[:,k] + B @ U[:,k]))

# 转换为QP形式，交给OSQP求解
# 具体转换过程后面章节会详细讲

4.6 环境验证——跑一个完整的MPC例子

最后，咱们把整个环境串起来，跑一个简单的MPC例子。这样能确认所有组件都正常工作。

# 1. 检查Eigen
g++ -I/usr/include/eigen3 -o test_eigen test_eigen.cpp && ./test_eigen

# 2. 检查OSQP
g++ -losqp -o test_osqp test_osqp.cpp && ./test_osqp

# 3. 检查CasADi
python3 -c "import casadi; print(casadi.__version__)"

# 4. 检查ROS2
ros2 run demo_nodes_cpp listener &  # 开一个终端
ros2 run demo_nodes_cpp talker      # 开另一个终端

如果以上都能跑通，恭喜你，MPC开发环境搭建完毕。后面咱们就可以开始写真正的MPC控制器了。