在华北舵狗王：NLP非线性优化第一步Webots与CasAdi中我们完成了采用matlab下casadi库对优化算法的快速验证，在华北舵狗王：NLP非线性优化第二步CasAdi Matlab库导出与部署我们完成了将成型的优化算法转换为c程序并进一步编译为c++可以识别库，最终在Webots环境下调用实现了迁移应用，本文完成部署最终一步就是将优化程序部署到嵌入式平台和机器人内部完成在线的优化。这样顺便说下对于优化问题来说其实际完全是一个数学问题，因此在Matlab环境下验证是完全没问题的，不像控制方法需要完整的输入输出反馈，整机动力学测试才最终能验证算法的合理性。优化算法在某种程度上是可以脱离动力学仿真的，本文用到的库下载地址为：
链接：https://pan.baidu.com/s/1LjKmC9Ig74CC5UN5HdUVkQ
提取码：mh1t 1 将CasADi程序放入自己的代码中
类似在Webots下完成VS环境中C++代码的开发，首先需要在自己控制程序中完成对CasADi程序编译，这里介绍交叉编译的方法，为了不让程序调用PC机安装的CasADi程序，这里直接将原文件放置在项目目录下：
之后在Cmakelist文件中增加链接：
add_subdirectory(casadi lib/ EXCLUDE_FROM_ALL)
target_link_libraries(${PROJECT_NAME} casadi)编译如果没问题后可以增加casadi的头文件和命名空间看是否会报错，下面是简单测试代码：
#include <casadi/casadi.hpp>
using namespace casadi;
int test_casadi() {
cout << "casadi_test" << endl;
// This is another way to define a nonlinear solver. Opti is new
/*
* min x1^2 + x2^2 + x3^2
* s.t. 6*x1 + 3&x2 + 2*x3 - p0 = 0
* p2*x1 + x2 - x3 - 1 = 0
* x1, x2, x3 >= 0
*/
// Optimization variables
SX x = SX::sym("x", 3);
std::cout << "x:" << x << std::endl;
// Parameters
SX p = SX::sym("p", 2);
std::cout << "p:" << p << std::endl;
// Objective
SX f = x(0) * x(0) + x(1) * x(1) + x(2) * x(2);
std::cout << "f:" << f << std::endl;
// Constraints
SX g = vertcat(6 * x(0) + 3 * x(1) + 2 * x(2) - p(0), p(1) * x(0) + x(1) - x(2) - 1);
std::cout << "g:" << g << std::endl;
// Initial guess and bounds for the optimization variables
vector<double> x0 = { 0.15, 0.15, 0.00 };
vector<double> lbx = { 0.00, 0.00, 0.00 };
vector<double> ubx = { inf, inf, inf };
// Nonlinear bounds
vector<double> lbg = { 0.00, 0.00 };
vector<double> ubg = { 0.00, 0.00 };
// Original parameter values
vector<double> p0 = { 5.00, 1.00 };
// NLP
SXDict nlp = { { "x", x }, { "p", p }, { "f", f }, { "g", g } };
// Create NLP solver and buffers
Function solver = nlpsol("solver", "ipopt", nlp);
std::map<std::string, DM> arg, res;
// Solve the NLP
arg["lbx"] = lbx;
arg["ubx"] = ubx;
arg["lbg"] = lbg;
arg["ubg"] = ubg;
arg["x0"] = x0;
arg["p"] = p0;
res = solver(arg);
// Print the solution
cout << "--------------------------------" << endl;
cout << "Optimal solution for p = " << arg.at("p") << ":" << endl;
cout << setw(30) << "Objective: " << res.at("f") << endl;
cout << setw(30) << "Primal solution: " << res.at("x") << endl;
cout << setw(30) << "Dual solution (x): " << res.at("lam_x") << endl;
cout << setw(30) << "Dual solution (g): " << res.at("lam_g") << endl;
printf("NLP Init!\n");
return 0;
}编译无误后，此时程序仍然无法在目标机上运行，下面需要在目标机上安装对应的ipopt和CasADi。
2 在ARM上编译ipopt和CasADI
这里测试了之前我的OdroidC4主控，已经打过RT实时补丁，首先需要下网盘中文件其中，coinhsl-2015.06.23.zip：用于安装HSL第三方库，Ipopt-3.12.7.tgz：用于安装其他第三方库(Blas、Lapack、ASL、Mumps、Metis)，Ipopt-releases-3.12.7.tar.gz：用于安装Ipopt核心：
安装过程参考了：
https://www.jianshu.com/p/8d80ba6afcfd
Xavier(5):ubuntu18.04安装ipopt-3.12(附百度网盘链接及报错分析)_biter0088的博客-CSDN博客_ipopt安装
Ipopt安装【Ubuntu18.04】XAVIER（ARM架构）_爱吃的毛毛虫的博客-CSDN博客_ipopt
安装casadi_大宝啊啊啊啊的博客-CSDN博客_matlab安装casadi
由于这里面坑比较多下面介绍下我成功的流程：
（1）安装coinhsl
首先还是安装相关依赖：
sudo apt-get install libblas3 libblas-dev liblapack3 liblapack-dev gfortran build-essential cppad gcc g++
然后解压Ipopt-3.12.7.tgz删除原始ThirdParty中的HSL文件，将coinhsl中的文件复制进去：
在该目录下首先完成授权,选择ARM的编译方式：
chmod +x ./configure
sudo ./configure --prefix=/usr/local/ --build=arm-linux
sudo make
sudo make install（2）安装Blas
同理完成Blas的安装：
chmod +x ./configure
chmod +x ./get.Blas
./get.Blas
sudo ./configure --prefix=/usr/local/ --build=arm-linux
sudo make
sudo make install 注：如果出现install-sh: Permission denied这样的错误，就cd到报错的目录下使用chmod授权install-sh。
（3）安装ipopt
解压Ipopt-releases-3.12.7.tar.gz，在该目录下建立build文件夹同时授权configure文件，进入build文件后：
sudo ./configure --prefix=/usr/local/ --build=arm-linux
sudo make
sudo make install 如果无误则可以继续，在安装后需要增加对hsl库的软链接，否则casadi调用是找不到库，直接编译完在local下的库名称是：
Casadi需要的是：
则cd到该目录下输入：
sudo ln -s libcoinhsl.so.1.4.5 libhsl.so（4）编译CasADi
解压文件，在文件下建立build:
mkdir build
cd build
cmake -DWITH_IPOPT=true ..
make如果编译无误后则正常运行代码，优化问题将不再报错，另外如果还有问题可以尝试下手动复制相应编译后的结果：
cd Ipopt-releases-3.12.7/build
sudo cp -a include/* /usr/include/.
sudo cp -a lib/* /usr/lib/.3 CasADi在ODroid上的测试
如果基本代码测试无误后可以按照上面的步骤完成在目标机上优化求解环境的配置，下面就可以将之前matlab使用codegen导出的nlp.c文件在目标机上进行编译，在c文件同目录下输入：
gcc -fPIC -shared -O1 nlp.c -o nlp.so在等待3分钟左右后会完成编译，此后在代码中建立求解器时选择编译的库即完成库调用：
Function solver = nlpsol("solver", "ipopt", "/home/odroid/Corgi/Motion/nlp.so");//建立求解器 读取编译库 不需要在增加opt最终，我在目标机采用相同数据进行了测试，可以看到在Webots里计算总共求解需要5s在ODroidC4上需要8s左右，速度确实慢很多，当然ODroid毕竟也不是一个算力非常高的控制器：
Objective...............: 3.6142600826208081e+01 3.6142600826208081e+01
Dual infeasibility......: 2.3880201879963963e+01 2.3880201879963963e+01
Constraint violation....: 1.4999999596066885e-01 1.4999999596066885e-01
Complementarity.........: 1.0100956367650396e-09 1.0100956367650396e-09
Overall NLP error.......: 2.3880201879963963e+01 2.3880201879963963e+01
Number of objective function evaluations = 190
Number of objective gradient evaluations = 119
Number of equality constraint evaluations = 190
Number of inequality constraint evaluations = 190
Number of equality constraint Jacobian evaluations = 143
Number of inequality constraint Jacobian evaluations = 143
Number of Lagrangian Hessian evaluations = 141
Total CPU secs in IPOPT (w/o function evaluations) = 7.214
Total CPU secs in NLP function evaluations = 1.472
EXIT: Converged to a point of local infeasibility. Problem may be infeasible.
solver : t_proc (avg) t_wall (avg) n_eval
nlp_f | 58.27ms (306.67us) 58.79ms (309.41us) 190
nlp_g | 165.93ms (873.31us) 167.16ms (879.79us) 190
nlp_grad | 4.23ms ( 4.23ms) 4.25ms ( 4.25ms) 1
nlp_grad_f | 64.75ms (539.59us) 65.24ms (543.70us) 120
nlp_hess_l | 596.02ms ( 4.26ms) 600.15ms ( 4.29ms) 140
nlp_jac_g | 576.06ms ( 4.00ms) 580.29ms ( 4.03ms) 144
total | 8.78 s ( 8.78 s) 8.82 s ( 8.82 s) 1当然最后优化的结果会有一定差距，原因是PC中调用的求解器是：
This is Ipopt version 3.13.3, running with linear solver pardiso.而目标机上是：
This is Ipopt version 3.12.7, running with linear solver ma27.4 应用测试
至此我们已经完成了从matlab算法验证到嵌入式部署的过程，由于最终优化的轨迹结果一样这里我也没再样机上实际测试，因为之前已经采用过直接读取离线数据的效果，可以看到对于低算力的应用如果要实现运动中的规划确实没法采用在线的方式，更划算的还是建立各种行为的数据库，依据需求快速调用。
当然在线建立最大的好处是优化问题的初始值与实际匹配性好，这在做跟踪控制中也会更加准确，毕竟比较正常的方法是类似采用规划足端力前馈加反馈的形式来实现，这里也是一个非常大的问题，即在样机中出现规划的结果和实物对不上，从此的过程就非常类似强化学习的步骤主要是解决模型迁移的问题，这里有一个可参考的轨迹跟踪方案：
下面我在Webots中增加了对规划轨迹、实际轨迹和末端状态的可视化显示实现了一个简单类似Atlas跑酷运动视频中的规划（下一篇文章将实现更逼真的模仿），当然并不是BD真正采用的技术方案，但要复现腾讯MAX2机器人梅花桩跳跃的过程似乎是没太大问题的：
总结下采用本文的方法能比较方便的完成工程化的过程，之前开源的Quad-SDK在实现全局优化的时候似乎也是采用类似的方案，没有直接在C++中建立完整的优化问题，当然目前最大的问题还是对轨迹如何跟踪。
毕竟不像无人机，足式机器人在做飞行相跳跃的过程中对接触判断和步态相序切换时刻的不一样，最终的效果会差很多！当然目前的规划里面实际也没有考虑地形和落足点的问题，不过可以确认的是要实现Atlas那样多步落足和运动的规划，对控制器算力的要求超乎想象，同时在对算法上的优化与加速也是重中之重，之前对比了Ipopt3.10.1和现在采用的12.4版本速度慢了30%，相信商用的优化器将更加快，这也是我国最大的短板，目前没听说啥大规模非线性优化求解器是自主研制的！
评论（1）
您还未登录，请登录后发表或查看评论