CasADi mpc

CasADi是一个功能强大的非线性优化框架，非常适合与ROS2结合实现MPC。以下是详细的集成指南：

CasADi 安装

1. 安装 CasADi

方法一：pip安装（推荐）

pip install casadi

方法二：源码安装

# 安装依赖
sudo apt install g++ gcc gfortran cmake git# 克隆源码
git clone https://github.com/casadi/casadi.git
cd casadi
mkdir build && cd build
cmake ..
make -j$(nproc)
sudo make install

2. 创建ROS2 + CasADi MPC包

# 创建工作空间
mkdir -p ~/casadi_mpc_ws/src
cd ~/casadi_mpc_ws/src# 创建ROS2包
ros2 pkg create casadi_mpc --build-type ament_cmake --dependencies rclcpp std_msgs geometry_msgs nav_msgs sensor_msgs

基本MPC节点示例

包结构：

casadi_mpc/
├── CMakeLists.txt
├── package.xml
├── include/casadi_mpc/
└── src/├── mpc_node.cpp└── mpc_controller.cpp

1. package.xml

xml

<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypelocation="http://www.ros.org/schema/package_format3.xsd"?> <package format="3"> <name>casadi_mpc</name> <version>0.0.0</version> <description>MPC controller using CasADi in ROS2</description> <maintainer email="[e-mail]">Your Name</maintainer> <license>Apache-2.0</license> <buildtool_depend>ament_cmake</buildtool_depend> <depend>rclcpp</depend> <depend>std_msgs</depend> <depend>geometry_msgs</depend> <depend>nav_msgs</depend> <depend>sensor_msgs</depend> <build_depend>eigen</build_depend> <exec_depend>eigen</exec_depend> </package>

2. CMakeLists.txt

cmake

cmake_minimum_required(VERSION 3.8) project(casadi_mpc) # 默认使用 C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() # 查找依赖 find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(sensor_msgs REQUIRED) # 查找 Python 和 CasADi find_package(Python3 COMPONENTS Interpreter Development REQUIRED) execute_process( COMMAND ${Python3_EXECUTABLE} -c "import casadi; print(casadi.__file__)" OUTPUT_VARIABLE CASADI_PATH OUTPUT_STRIP_TRAILING_WHITESPACE ) get_filename_component(CASADI_DIR ${CASADI_PATH} DIRECTORY) # 包含目录 include_directories( include ${CASADI_DIR} ${CASADI_DIR}/casadi ) # 创建可执行文件 add_executable(mpc_node src/mpc_node.cpp src/mpc_controller.cpp) ament_target_dependencies(mpc_node rclcpp std_msgs geometry_msgs nav_msgs sensor_msgs) # 安装 install(TARGETS mpc_node DESTINATION lib/${PROJECT_NAME}) # 导出依赖 ament_export_dependencies(rclcpp std_msgs geometry_msgs) ament_package()

3. MPC控制器头文件 (include/casadi_mpc/mpc_controller.hpp)

#pragma once #include <casadi/casadi.hpp> #include <vector> #include <memory> class MPCController { public: MPCController(); void setupMPC(); std::vector<double> solve(const std::vector<double>& current_state, const std::vector<double>& reference); private: casadi::Function solver_; casadi::Dict opts_; casadi::Dict solver_options_; int N_; // 预测步长 double dt_; // 时间步长 };

4. MPC控制器实现 (src/mpc_controller.cpp)

#include "casadi_mpc/mpc_controller.hpp" #include <iostream> MPCController::MPCController() : N_(10), dt_(0.1) { setupMPC(); } void MPCController::setupMPC() { using namespace casadi; // 状态变量: [x, y, theta, v] MX x = MX::sym("x"); MX y = MX::sym("y"); MX theta = MX::sym("theta"); MX v = MX::sym("v"); MX state = vertcat(x, y, theta, v); int nx = state.size1(); // 控制输入: [acceleration, steering] MX a = MX::sym("a"); MX delta = MX::sym("delta"); MX control = vertcat(a, delta); int nu = control.size1(); // 系统动力学 (简化的车辆模型) MX xdot = v * cos(theta); MX ydot = v * sin(theta); MX thetadot = v * tan(delta) / 2.5; // L = 2.5m wheelbase MX vdot = a; MX rhs = vertcat(xdot, ydot, thetadot, vdot); MXDict ode = {{"x", state}, {"p", control}, {"ode", rhs}}; // 创建积分器 Function integrator = integrator("integrator", "cvodes", ode, {{"tf", dt_}}); // MPC问题定义 MX U = MX::sym("U", nu, N_); MX X = MX::sym("X", nx, N_+1); MX P = MX::sym("P", nx + nx); // 参数: [current_state, reference] // 初始条件 std::vector<MX> obj_terms, g_terms; MX obj = 0; // 初始状态约束 g_terms.push_back(X(Slice(), 0) - P(Slice(0, nx))); // 构建目标函数和约束 for (int k = 0; k < N_; ++k) { // 状态跟踪代价 MX state_error = X(Slice(), k) - P(Slice(nx, 2*nx)); obj += 10*dot(state_error, state_error); // 控制代价 obj += 0.1*dot(U(Slice(), k), U(Slice(), k)); // 动力学约束 MXDict res = integrator({{"x0", X(Slice(), k)}, {"p", U(Slice(), k)}}); g_terms.push_back(X(Slice(), k+1) - res["xf"]); } // 终端代价 MX terminal_error = X(Slice(), N_) - P(Slice(nx, 2*nx)); obj += 100*dot(terminal_error, terminal_error); // 控制约束 MX U_flat = reshape(U, nu*N_, 1); MX X_flat = reshape(X, nx*(N_+1), 1); // NLP问题 MXDict nlp = { {"x", vertcat(X_flat, U_flat)}, {"f", obj}, {"g", vertcat(g_terms)}, {"p", P} }; // 求解器选项 opts_["ipopt"] = Dict{ {"print_level", 0}, {"max_iter", 100}, {"tol", 1e-6} }; // 创建求解器 solver_ = nlpsol("solver", "ipopt", nlp, opts_); } std::vector<double> MPCController::solve(const std::vector<double>& current_state, const std::vector<double>& reference) { using namespace casadi; // 构建参数向量 std::vector<double> p; p.insert(p.end(), current_state.begin(), current_state.end()); p.insert(p.end(), reference.begin(), reference.end()); // 初始猜测 std::vector<double> x0(4*(N_+1) + 2*N_, 0.0); // 边界 std::vector<double> lbg(4*(N_+1), 0.0); std::vector<double> ubg(4*(N_+1), 0.0); // 求解 DMDict arg = { {"x0", x0}, {"p", p}, {"lbg", lbg}, {"ubg", ubg} }; DMDict res = solver_(arg); // 提取第一个控制输入 std::vector<double> result(2); std::vector<double> full_solution = std::vector<double>(res.at("x")); result[0] = full_solution[4*(N_+1)]; // 加速度 result[1] = full_solution[4*(N_+1)+1]; // 转向角 return result; }

5. ROS2节点 (src/mpc_node.cpp)

#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "casadi_mpc/mpc_controller.hpp"class MPCNode : public rclcpp::Node {
public:MPCNode() : Node("mpc_node") {// 订阅者odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>("/odom", 10, std::bind(&MPCNode::odomCallback, this, std::placeholders::_1));goal_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/goal_pose", 10, std::bind(&MPCNode::goalCallback, this, std::placeholders::_1));// 发布者cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);// 控制器mpc_controller_ = std::make_unique<MPCController>();// 控制定时器timer_ = this->create_wall_timer(std::chrono::milliseconds(100),std::bind(&MPCNode::controlLoop, this));RCLCPP_INFO(this->get_logger(), "MPC Node started");}private:void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {current_odom_ = *msg;}void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {goal_pose_ = msg->pose;}void controlLoop() {if (!current_odom_ || !goal_pose_) {return;}// 提取当前状态std::vector<double> current_state = {current_odom_->pose.pose.position.x,current_odom_->pose.pose.position.y,getYawFromQuaternion(current_odom_->pose.pose.orientation),std::hypot(current_odom_->twist.twist.linear.x, current_odom_->twist.twist.linear.y)};// 目标状态std::vector<double> reference = {goal_pose_->position.x,goal_pose_->position.y,getYawFromQuaternion(goal_pose_->orientation),0.0  // 目标速度};// 求解MPCauto control = mpc_controller_->solve(current_state, reference);// 发布控制命令auto cmd_msg = geometry_msgs::msg::Twist();cmd_msg.linear.x = control[0];  // 加速度cmd_msg.angular.z = control[1]; // 转向角cmd_pub_->publish(cmd_msg);}double getYawFromQuaternion(const geometry_msgs::msg::Quaternion& quat) {double siny_cosp = 2.0 * (quat.w * quat.z + quat.x * quat.y);double cosy_cosp = 1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z);return std::atan2(siny_cosp, cosy_cosp);}rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;rclcpp::TimerBase::SharedPtr timer_;std::optional<nav_msgs::msg::Odometry> current_odom_;std::optional<geometry_msgs::msg::Pose> goal_pose_;std::unique_ptr<MPCController> mpc_controller_;
};int main(int argc, char** argv) {rclcpp::init(argc, argv);auto node = std::make_shared<MPCNode>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

编译和运行

cd ~/casadi_mpc_ws
colcon build --packages-select casadi_mpc
source install/setup.bash
ros2 run casadi_mpc mpc_node

关键特性

1. 非线性MPC: 使用CasADi处理非线性动力学

2. 实时优化: 使用IPOPT作为求解器

3. ROS2集成: 完整的ROS2节点结构

4. 可扩展: 易于修改模型和约束

这个实现提供了一个完整的ROS2 + CasADi MPC框架，您可以根据具体应用修改动力学模型、约束条件和目标函数。