1 简介
官方文件：https://web.casadi.org/docs/
CasADi是一个开源的数值优化软件工具，而且尤其可以处理包含微分方程的优化问题，比如最优控制。
CasADi有C++，Python和MATLAB接口，使用这三者在性能上基本没有区别。但是Python API资料最丰富而且比MATLAB API稍微稳定一点。C++API是稳定的但是资料不丰富，而且缺少解释性语言的交互性。
2 安装
官方教程：https://github.com/casadi/casadi/wiki/InstallationLinux
首先需要安装可供CasADi调用的求解器，如非线性求解器IPOUT,QP求解器qpOASES等。
2.1 安装ipopt
博主需要用来求解NMPC，所以安装一个IPOPT。
安装教程：https://blog.csdn.net/asd22222984565/article/details/130794329
2.2 安装casadi
git clone https://github.com/casadi/casadi.git
cd casadi
mkdir build
cd build
cmake .. -DWITH_IPOPT=ON //用到别的求解器也这么写，加在后面，重新make
make
sudo make install
报错：
casadi/interfaces/ipopt/ipopt_nlp.hpp:29:10: fatal error: IpTNLP.hpp: No such file or directory
3 测试
可以用官方的demo测试一下。
先建一个文件夹，然后新建casadi_demo.cpp
#include <iostream>
#include <casadi/casadi.hpp>
using namespace casadi;
int main() {
auto opti = casadi::Opti();
auto x = opti.variable();
auto y = opti.variable();
auto z = opti.variable();
opti.minimize(x*x + 100*z*z);
opti.subject_to(z + (1-x) * (1-x) - y == 0);
opti.solver("ipopt");
auto sol = opti.solve();
std::cout << sol.value(x) << ":" << sol.value(y) << ":" << sol.value(z) << std::endl;
return 0;
}
新建CMakeLists.txt
cmake_minimum_required(VERSION 3.10.2)
# export CMAKE_PREFIX_PATH=../casadi_install/casadi/cmake/
find_package(casadi)
add_executable(casadi_demo casadi_demo.cpp)
target_link_libraries(casadi_demo casadi)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11")
install(TARGETS casadi_demo RUNTIME DESTINATION . LIBRARY DESTINATION .)
在文件夹目录下，
mkdir build && cd build
cmake ..
make
./casadi_demo
输出
******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
Ipopt is released as open source code under the Eclipse Public License (EPL).
For more information visit https://github.com/coin-or/Ipopt
******************************************************************************
This is Ipopt version 3.14.12, running with linear solver ma27.
Number of nonzeros in equality constraint Jacobian...: 3
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 2
Total number of variables............................: 3
variables with only lower bounds: 0
variables with lower and upper bounds: 0
variables with only upper bounds: 0
Total number of equality constraints.................: 1
Total number of inequality constraints...............: 0
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 0
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 0.0000000e+00 1.00e+00 0.00e+00 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 0.0000000e+00 0.00e+00 0.00e+00 -1.7 1.00e+00 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 1
(scaled) (unscaled)
Objective...............: 0.0000000000000000e+00 0.0000000000000000e+00
Dual infeasibility......: 0.0000000000000000e+00 0.0000000000000000e+00
Constraint violation....: 0.0000000000000000e+00 0.0000000000000000e+00
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 0.0000000000000000e+00 0.0000000000000000e+00
Overall NLP error.......: 0.0000000000000000e+00 0.0000000000000000e+00
Number of objective function evaluations = 2
Number of objective gradient evaluations = 2
Number of equality constraint evaluations = 2
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 2
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 1
Total seconds in IPOPT = 0.001
EXIT: Optimal Solution Found.
solver : t_proc (avg) t_wall (avg) n_eval
nlp_f | 67.00us ( 33.50us) 3.28us ( 1.64us) 2
nlp_g | 90.00us ( 45.00us) 6.64us ( 3.32us) 2
nlp_grad_f | 234.00us ( 78.00us) 18.79us ( 6.26us) 3
nlp_hess_l | 75.00us ( 75.00us) 5.38us ( 5.38us) 1
nlp_jac_g | 135.00us ( 45.00us) 10.55us ( 3.52us) 3
total | 13.92ms ( 13.92ms) 1.14ms ( 1.14ms) 1
0:1:0
还可以去这位大佬的博客看看，用他开源的程序试一试能不能求解。
https://zhuanlan.zhihu.com/p/391903468
4 基本语法
//四维向量
casadi::MX X = casadi::MX::sym("x", 4);
//切片
casadi::MX currentPosition = X(casadi::Slice(0,2));
5 基于casadi构建简单的最优控制问题
下面实现一下通过casadi构建一个质点直线运动的问题，可以给定边界值，然后通过运动学约束求解出运动的速度和加速度，运动学模型是简单的二阶积分模型，即位置的导数是速度，速度的导数是加速度。最优控制可以对状态和速度加约束，状态量为p,v，控制量是a，可以给一个上下限。
头文件point_motion.h
#ifndef SIMPLE_OPTIMAL_CONTROL_H
#define SIMPLE_OPTIMAL_CONTROL_H
#include <iostream>
#include <casadi/casadi.hpp>
#include <Eigen/Eigen>
using casadi::MX;
class PointMotion
{
private:
int N_;
double dt_;
MX X_, U_, T_;
casadi::Function dynamic_equation_;
std::unique_ptr<casadi::OptiSol> solution_;
public:
PointMotion();
~PointMotion();
casadi::Function setDynamicEquation();
MX RK4_discretisize(MX dt, MX x_v, MX u_v);
bool solve();
void getSolution(casadi::DM& state, casadi::DM& control, casadi::DM& tf);
};
#endif
point_motion.cpp
#include "simple_optimal_control/point_motion.h"
#include <vector>
using namespace casadi;
using namespace std;
PointMotion::PointMotion()
{
dynamic_equation_ = setDynamicEquation();
}
PointMotion::~PointMotion(){}
casadi::Function PointMotion::setDynamicEquation() {
MX x = MX::sym("x");
MX v = MX::sym("v");
MX a = MX::sym("a");
MX state = MX::vertcat({x, v});
MX rhs = MX::vertcat({v, a});
return Function("dynamic_equation", {state, a},{rhs});
}
//四阶龙哥库塔法离散化
MX PointMotion::RK4_discretisize(MX dt, MX x_v, MX u_v) {
MX result;
vector<MX> input(2);
input[0] = x_v;
input[1] = u_v;
MX k1 = dynamic_equation_(input)[0];
input[0] += dt / 2 * k1;
MX k2 = dynamic_equation_(input)[0];
input[0] = x_v + dt / 2 * k2;
MX k3 = dynamic_equation_(input)[0];
input[0] = x_v + dt * k3;
MX k4 = dynamic_equation_(input)[0];
result = x_v + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4);
return result;
}
bool PointMotion::solve() {
Opti opti = Opti();
Slice all;
int N_ = 100;
//优化变量 X_ U_ T_
X_ = opti.variable(2, N_ + 1);
MX pos = X_(0, all);
MX vel = X_(1, all);
U_ = opti.variable(1, N_);
MX acc = U_;
T_ = opti.variable();
opti.minimize(T_);
MX dt = T_ / N_;
//-------dynamic constraints----------
for (int i = 0; i < N_; ++i) {
MX X_next = RK4_discretisize(dt, X_(all, i), U_(all, i));
//一阶欧拉法
// vector<MX> input(2);
// input[0] = X_(all, i);
// input[1] = U_(all, i);
// MX X_next = dynamic_equation_(input)[0] * dt_ + X_(all, i);
opti.subject_to(X_next == X_(all, i + 1));
}
cout << "set dynamic constraints finish" << endl;
//v and a constrains
opti.subject_to(-1 <= vel <= 5);
opti.subject_to(-1 <= acc <= 1);
//boundary conditions
opti.subject_to(pos(0) == 0); //初始位置
opti.subject_to(pos(N_) == 10); //终止位置
opti.subject_to(vel(0) == 0); //初始速度
opti.subject_to(vel(N_) == 0); //终止速度
opti.subject_to(T_ >= 0);
//set initial value
opti.set_initial(pos, 0);
opti.set_initial(vel, 0);
opti.set_initial(T_, 2);
//开始求解
Dict solver_opts;
solver_opts["expand"] = true;
solver_opts["ipopt.max_iter"] = 100;
solver_opts["ipopt.print_level"] = 0;
solver_opts["print_time"] = 0;
solver_opts["ipopt.acceptable_tol"] = 1e-6;
solver_opts["ipopt.acceptable_obj_change_tol"] = 1e-6;
opti.solver("ipopt", solver_opts);
cout << "solve finish" << endl;
solution_ = std::make_unique<OptiSol>(opti.solve());
return true;
}
//获取最优解
void PointMotion::getSolution(casadi::DM& state, casadi::DM& control, casadi::DM& tf) {
state = solution_->value(X_);
control = solution_->value(U_);
tf = solution_->value(T_);
}
测试加画图代码
#include "simple_optimal_control/point_motion.h"
#include "ros/ros.h"
#include "simple_optimal_control/matplotlibcpp.h"
#include "visualization_msgs/Marker.h"
#include "nav_msgs/Path.h"
#include "geometry_msgs/PoseStamped.h"
using namespace std;
using namespace casadi;
//matplotlib-cpp 画速度和加速度图
namespace plt = matplotlibcpp;
double t_plot = 0;
vector<double> p_vec;
vector<double> v_vec;
vector<double> a_vec;
vector<double> time_vec;
int idx = 0;
//ros related
ros::Publisher spherePub;
ros::Publisher path_pub;
nav_msgs::Path path;
geometry_msgs::PoseStamped pose;
//rviz中可视化小球
void visualizeSphere(const Eigen::Vector3d ¢er,
const double &radius)
{
visualization_msgs::Marker sphereMarkers, sphereDeleter;
sphereMarkers.id = 0;
sphereMarkers.type = visualization_msgs::Marker::SPHERE_LIST;
sphereMarkers.header.stamp = ros::Time::now();
sphereMarkers.header.frame_id = "map";
sphereMarkers.pose.orientation.w = 1.00;
sphereMarkers.action = visualization_msgs::Marker::ADD;
sphereMarkers.ns = "spheres";
sphereMarkers.color.r = 1.00;
sphereMarkers.color.g = 0.00;
sphereMarkers.color.b = 0.00;
sphereMarkers.color.a = 1.00;
sphereMarkers.scale.x = radius * 2.0;
sphereMarkers.scale.y = radius * 2.0;
sphereMarkers.scale.z = radius * 2.0;
sphereDeleter = sphereMarkers;
sphereDeleter.action = visualization_msgs::Marker::DELETE;
geometry_msgs::Point point;
point.x = center(0);
point.y = center(1);
point.z = center(2);
sphereMarkers.points.push_back(point);
spherePub.publish(sphereDeleter);
spherePub.publish(sphereMarkers);
}
void vis_points(const ros::TimerEvent &e) {
visualizeSphere({p_vec[idx], 0, 0}, 0.2);
idx++;
//cout << idx << endl;
path_pub.publish(path);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "waypoints_publisher");
ros::NodeHandle nh;
spherePub = nh.advertise<visualization_msgs::Marker>("/spheres", 100);
path_pub = nh.advertise<nav_msgs::Path>("/path", 1);
path.header.frame_id = "map";
pose.pose.position.x = 0;
path.poses.push_back(pose);
pose.pose.position.x = 10;
path.poses.push_back(pose);
ros::Timer vis_point;
PointMotion point_motion; //调用那个类
DM sol_state, sol_control, sol_T; // 存解
int N = 100;
if (point_motion.solve()) { //求解
cout << "solve success" << endl;
point_motion.getSolution(sol_state, sol_control, sol_T);
}
cout << "state:\n" << sol_state << "\ncontrol:\n" << sol_control << "\nT:\n" << sol_T << "\n\n";
//画图 存储结果
double dt = static_cast<double>(sol_T / N);
for (int i = 0; i < N; ++i) {
p_vec.push_back(static_cast<double>(sol_state(0, i)));
v_vec.push_back(static_cast<double>(sol_state(1, i)));
a_vec.push_back(static_cast<double>(sol_control(i)));
time_vec.push_back(i * dt);
}
//定时器 每隔dt在rviz中可视化小球
vis_point = nh.createTimer(ros::Duration(dt), vis_points);
plt::subplot(2,1,1);
plt::title("motion speed");
plt::plot(time_vec, v_vec, "b-");
plt::subplot(2,1,2);
plt::title("accelerate");
plt::plot(time_vec, a_vec, "g-");
plt::show();
ros::spin();
return 0;
}
cmakelist
cmake_minimum_required(VERSION 3.0.2)
project(simple_optimal_control)
add_compile_options(-std=c++11)
find_package(casadi REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
)
catkin_package(
INCLUDE_DIRS include
# LIBRARIES simple_optimal_control
CATKIN_DEPENDS roscpp std_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
include_directories(/usr/local/include)
link_directories(/usr/local/lib)
add_executable(simple_optimal_control src/point_motion.cpp src/test.cpp)
target_link_libraries(simple_optimal_control
${catkin_LIBRARIES}
casadi
)
find_package(PythonLibs 2.7)
target_include_directories(simple_optimal_control PRIVATE ${PYTHON_INCLUDE_DIRS})
target_link_libraries(simple_optimal_control ${PYTHON_LIBRARIES})
文件夹结构如下，
可以建一个ros功能包试试。
运行结果如下，
演示效果见B站
https://www.bilibili.com/video/BV1Jk4y1F7vj/?spm_id_from=333.999.0.0&vd_source=62bfb7720b0b2f9941f7f34210ba6a18
扫一扫
5289
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
