
在最小二乘法一章節中我給出了基于matlab仿真下的手推最小二乘解方法,最終采用廣義法能将任意多組資料求最小二乘解轉換為求取固定矩陣元素平均值的形式進而避免資料增長帶來的計算量增大:
https://zhuanlan.zhihu.com/p/267156837zhuanlan.zhihu.com
為實作程式設計應用可以将求解算法單獨寫入matlab檔案的函數中再使用coder将其轉換為c或c++代碼。
最小二乘法的理論知識實際為我們後續基于QP優化或LQR,MPC等最優化控制算法的應用提供了基礎的理論支撐,在我的基于QP優化的四足機器人矢量平面足底力優化文章:
https://zhuanlan.zhihu.com/p/264814614zhuanlan.zhihu.com
中已經給出介紹了如何基于MATLAB自帶優化庫建構一個帶錐限制的力配置設定優化問題,但該函數是無法自帶轉換為代碼的,
想在如Webots或嵌入式控制器上進行驗證測試需要采用專業的非線性優化庫。
目前有許多非線性優化算法庫如
MIT Mini Cheetha開源獵豹機器人中就使用了qpOASES,而在Python環境下能使用Pip指令友善地安裝Scipy或cvxopt 等優化庫,而在SLAM中g2o或Ceres Solver等能求解大規模優化問題的高性能非線性優化庫更被廣泛應用。
由于我目前開展的8自由度并聯四足機器人仿真在Webots中搭建,
基于我知乎中三通道解耦等文章已經實作了穩定的對角步态移動,但對如Bound或Gallop等非對角腿着地情況其力配置設定的數值求解方程難以列出,同時由于8自由度轉向采用差速形式,足底打滑會嚴重影響位姿控制穩定,需要進一步引入摩擦圓錐限制,是以為在Webots中驗證QP力配置設定的可行性,下面給出這幾天在Windows環境下配置非線性優化和矩陣運算庫踩過的坑。
(1)建立Webots下的Visual Studio工程
在之前的仿真中由于我使用自己STM32單片機的Moco 12舵狗代碼為基礎,是以直接采用Webots建立C工程程式設計,但為使用Eigen等數學庫需要轉換到C++架構下,
是以首先建立在系統中安裝VSCoder,原因是後續許多Windows下的cmake或gcc中間件在打開對應檔案後其會自動彈出對于插件進行安裝。下面首先在Webots中建立C++工程,并再安裝VS IDE(原因是可以直接使用VS完成許多庫的路徑快速準确配置,否則如選用Webots自帶的gcc編譯器,則需要自己修改makefile來連結需要的庫,這将十分麻煩畢竟我們主要是為了研究算法),這裡我安裝的是
VS2017專業版(秘鑰KBJFW-NXHK6-W4WJM-CRMQB-G3CDH)。
(2)下載下傳配置Eigen庫
在完成VS的安裝後打開Webots建立的.sln工程,下面首先安裝矩陣運算所需要的Eigen庫,從官網下載下傳(http://eigen.tuxfamily.org/index.php?title=Main_Page),
當然我在本文最後會貼上所用到的軟體資料。由于Eigen庫均在頭檔案中是以,可以其解壓直接放置在C槽根目錄下,通過在項目屬性中配置包含目錄可将其快速加入工程中:
之後可以在Webots項目主檔案中編輯如下測試代碼,點選
資料總管中顯示所有檔案檢視是否包含Eigen目錄驗證:
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
void test_eigen(){
Eigen::MatrixXd m = MatrixXd::Random(3, 3); //随機生成3*3的double型矩陣
m = (m + MatrixXd::Constant(3, 3, 1.2)) * 50; //MatrixXd::Constant(3,3,1.2)表示生成3*3的double型矩陣,該矩陣所有元素均為1.2
cout << "m =" << endl << m << endl;
VectorXd v(3); // 定義v為3*1的double型向量
v << 1, 2, 3; // 向量指派
cout << "v =" << endl << v << endl;
cout << "m * v =" << endl << m * v << endl;
}
編譯無誤後,在Webots仿真環境中點選運作,檢視Eigen矩陣運算結果:
m =
10.1251 90.8741 45.0291
66.3585 68.5009 99.5962
29.3304 57.9873 92.284
v =
1
2
3
m * v =
326.961
502.149
422.157
(3)編譯qpOASES
為實作後續在Webots中進行優化仿真和MPC控制仿真,這裡給出非線性優化庫的配置方法,為實作與MIT相關代碼的對應是以我仍然使用qpOASES,qpOASES是一個開源c++項目,是一個可結構開發的有效集求解器可用于解決具有一下标準形式的二次型優化問題:
,首次編譯時我遇到的問題是Window沒安裝該toolchain使得在qpOASES檔案目錄下make出錯,在我安裝
MinGW後編譯仍然無果,
是以我嘗試成功的方法是首先使用cmake自動産生對于的makefile之後再進行編譯。
當然這也是使用Vscode的好處,
當你打開其對應的CMakeLists檔案後Vscode會自動檢查是否安裝,并自動安裝cmake插件,在安裝完成後CD到對于目錄下建立build檔案夾在其中輸入
cmake ..則會自動産生如下配置檔案:
之後可以繼續使用make指令産生相應的靜态庫,但由于我GNU配置錯誤無法執行,
是以我安裝了VS2017打開其cmake後輸出的.sln工程,使用VS2017完成對項目的生成結果如下:
cmake後的工程中有許多example例子可以直接删除僅保留需要的qpOASES相關庫,
之後如編譯報出工具集142故障可以在屬性中修改為VS版本的工具集:
編譯後在将lib檔案夾下産生對應的靜态庫:
(4)調用測試qpOASES非線性優化庫
在生成所需優化庫lib後,我們重新回到Webots生成的VS工程中在屬性和庫目錄下包含對應的頭檔案路徑,
并通過在項目浏覽序列槽右鍵添加qpOASES.lib:
之後再在代碼中增加對應頭檔案和如下測試代碼驗證QP優化庫是否工作正常:
#include <qpOASES.hpp>
using namespace qpOASES;
int testQP()
{
/* Setup data of first QP. */
real_t H[2 * 2] = { 1.0, 0.0, 0.0, 0.5 };
real_t A[1 * 2] = { 1.0, 1.0 };
real_t g[2] = { 1.5, 1.0 };
real_t lb[2] = { 0.5, -2.0 };
real_t ub[2] = { 5.0, 2.0 };
real_t lbA[1] = { -1.0 };
real_t ubA[1] = { 2.0 };
/* Setup data of second QP. */
real_t g_new[2] = { 1.0, 1.5 };
real_t lb_new[2] = { 0.0, -1.0 };
real_t ub_new[2] = { 5.0, -0.5 };
real_t lbA_new[1] = { -2.0 };
real_t ubA_new[1] = { 1.0 };
/* Setting up QProblem object. */
QProblem example(2, 1);
/* Solve first QP. */
int_t nWSR = 10;
example.init(H, g, A, lb, ub, lbA, ubA, nWSR);
/* Solve second QP. */
nWSR = 10;
example.hotstart(g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR);
/* Get and print solution of second QP. */
real_t xOpt[2];
example.getPrimalSolution(xOpt);
printf("n xOpt = [ %e, %e ]; objVal = %enn",
xOpt[0], xOpt[1], example.getObjVal());
return 0;
}
編譯後傳回Webots軟體中點選運作,則在控制台中列印如下字元驗證庫已經安裝運作正常:
以上就是我嘗試在Webots中建立c++工程并配置Eigen矩陣運算庫與QP非線性優化庫的大緻流程,相關編譯後的檔案與庫函數可在此下載下傳:
連結:https://pan.baidu.com/s/1hzzo3B6Rl7r3XyMz6DRRdQ
提取碼:qqvu
則基于上述配置好的環境我們可以對之前提到的最小二乘法或者QP足力優化使用Eigen或qpOASEs庫來分别實作 ,當然在後續文章中會具體給出相關代碼實作方法。