移动机器人领域相关C++库的综述

C++开源库的介绍，包括Eigen, Sophus,  NLopt, OOQP, Mosek, OSQP, OpenCV，Ipopt, GLPK, OMPL, SBPL, MoveIt，Ceres, g2o, PCL.

 

Ceres和g2o(General Graphic Optimization)作为SLAM常用的两个优化库。 

一. Eigen

处理矩阵，向量，数值分析等线性代数的C++库.

 

 

二. Sophus

Sophus是关于李群李代数相关的C++库，在Eigen基础上开发，支持SO(3)，SE(3)，SO(2)，SE(2)，以及相似变换Sim(3).

群：群(Group)是一种集合加上一种运算的代数结构，以A表示集合，“·”表示运算，则群一般写作 G(A, ·)，满足四个性质：凤（封闭性）姐（结合性）咬（幺元性）你（可逆性）

李群李代数：李群是指拥有(连续)光滑性质的群，常用的也就是旋转矩阵群，变换矩阵群（旋转+平移）.

表达旋转的方式有旋转向量，旋转矩阵，四元数，欧拉角等方式，但关于为什么还要采用李群李代数表达机器人旋转，这篇博客写的比较好https://blog.csdn.net/qq_45010210/article/details/126692609，简言之就是，在估计机器人位姿时需要对表示旋转的量求导，变换矩阵不满足加法封闭性，变换矩阵加变换矩阵不等于变换矩阵，而李群可以作对数映射转化为李代数，就是三维向量，满足加法封闭，便于求导。

 

 

假设我们对空间一点p进行旋转，得到Rp，现在我们要计算旋转之后的点关于旋转矩阵的导数：

 

由于R不满足加法，所以转换成李代数进行求导：

  上式的结果用BCH近似得到，由于Jl的存在，增加计算的复杂度，所以可以使用扰动模型（左乘）来进行简化。左扰动模型的结果为：

 

 

 李群BCH公式：

 

1. 安装：

 

git clone https://github.com/strasdat/Sophus.git

cd Sophus

git checkout a621ff

mkdir build

cd build

cmake ..

make

sudo make install

如果make时提示错误

Sophus/sophus/so2.cpp:33:26:error: lvalue required as left operand of assignment

unit_complex_.real() = 1.;

改为unit_complex_.real(1.);

安装完成

 2. 使用

SO(3)，SE(3)，so(3)，se(3)的相互转换关系

 CMakeLists.txt文件如下：

cmake_minimum_required(VERSION 3.10)
project(test)
find_package(Sophus REQUIRED)
find_package(Eigen3 REQUIRED)
include_directories(include   ${Sophus_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
add_executable(test test.cpp)
target_link_libraries(test ${Sophus_LIBRARIES} ${EIGEN3_LIBRARIES})

头文件：

#include<Eigen/Core>
#include<sophus/so3.h>
#include<sophus/se3.h>

常用函数：Sophus::SO3::exp(V1);SO3_V1.log();Sophus::SO3::hat(so3_V1);Sophus::SO3::vee(M_so3_V1);

用的时候可以查阅：https://blog.csdn.net/u011092188/article/details/77833022

 

三. NLopt

开源的非线性优化库，官网：https://nlopt.readthedocs.io/en/latest/

1. 安装：

下载完，

cd  nlopt-2.7.1   

mkdir build

cmake ..

make

sudo make install

 

   2. 使用

 

相比官方教程，需要在CMaikeists.txt中增加  set(NLopt_LIBRARIES /usr/local/lib/libnlopt.so.0.11.1 ) 
否则会报下面的错：

/usr/bin/ld: CMakeFiles/test.dir/test.cpp.o: in function `main':
test.cpp:(.text+0x2be): undefined reference to `nlopt_create'
/usr/bin/ld: test.cpp:(.text+0x2d5): undefined reference to `nlopt_set_lower_bounds'
/usr/bin/ld: test.cpp:(.text+0x2ed): undefined reference to `nlopt_set_min_objective'
/usr/bin/ld: test.cpp:(.text+0x30e): undefined reference to `nlopt_add_inequality_constraint'
/usr/bin/ld: test.cpp:(.text+0x32f): undefined reference to `nlopt_add_equality_constraint'
/usr/bin/ld: test.cpp:(.text+0x344): undefined reference to `nlopt_set_xtol_rel'
/usr/bin/ld: test.cpp:(.text+0x35b): undefined reference to `nlopt_optimize'
/usr/bin/ld: test.cpp:(.text+0x39c): undefined reference to `nlopt_destroy'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/test.dir/build.make:103：test] 错误 1
make[1]: *** [CMakeFiles/Makefile2:76：CMakeFiles/test.dir/all] 错误 2
make: *** [Makefile:84：all] 错误 2

 

CMakeLists.txt如下：

 

cmake_minimum_required(VERSION 3.10)
project(test)
find_package(NLopt REQUIRED)
set(NLopt_LIBRARIES /usr/local/lib/libnlopt.so.0.11.1 )  //动态连接库位置需要指定，否则容易报错；也可以是libnlopt.so.0，具体按照自己安装的版本
include_directories(include ${NLopt_INCLUDE_DIRS})
add_executable(test test.cpp)
target_link_libraries(test  ${NLopt_LIBRARIES} )

2.1案例

 

           

 

 

代码结构：

ls文件夹：

build    CMakeLists.txt  include(/test.h)    test.cpp 

CMakeLists.txt 已经给出，test.cpp如下：
     

#pragma once
#include"test.h"
using namespace std;
using namespace nlopt;
/*
 * main.c
 *
 *  Created on: Oct 9, 2018
 *      Author: lgh
 */
#define INF (1.0/0.0)
int i=0;

//目标函数；
double utility(unsigned n, const double *x, double *grad, void *data)
{
    if(grad){
      grad[0]=2*x[0];
      grad[1]=2*x[1];
      grad[2]=1.0;
      grad[3]=2*x[3];
    }
    printf("迭代次数 i= %d, x[0]=%f, x[1]= %f，x[2]= %f，x[3]= %f，f(x1,x2,x3,x4)=%f\n",i++,x[0],x[1],x[2],x[3],x[0]*x[0]+x[1]*x[1]+x[2]+x[3]*x[3]+10);
  return ( x[0]*x[0]+x[1]*x[1]+x[2]+x[3]*x[3]+10 );
}

//等式限制条件；
double constraint(unsigned n, const double *x, double *grad, void *data)
{
    if(grad){
          grad[0]= 1.0;
          grad[1]= 1.0;
          grad[2]= 1.0;
          grad[3]= 1.0;
    }

  return (x[0]+x[1]+x[2]+x[3]);
}

//不等式限制条件；
double inconstraint(unsigned n, const double *x, double *grad, void *data)
{
    if(grad){
     grad[0]= -2*x[0];
     grad[1]= -2*x[1];
    }
  return (-x[0]*x[0]-x[1]*x[1]-100);
}


int main(int argc, char const *argv[])
{
  double tol=1e-8;
  double lb[4]={-INF,-INF,-INF,-INF};          //x1、x2的下边界；
  double ub[4]={INF,INF,INF,INF};
  double x[4]={1, 1, 1, 1};      //给x1、x2赋予初始值；
  double f_max;

  nlopt_opt opter=nlopt_create( NLOPT_LD_SLSQP, 4);

  //设置自变量下限；
  nlopt_set_lower_bounds(opter, lb);

  // 目标函数；
  nlopt_set_min_objective(opter, utility, NULL);

  // 不等式约束；
  nlopt_add_inequality_constraint(opter, inconstraint, NULL, tol);

  // 等式约束；
  nlopt_add_equality_constraint(opter, constraint, NULL, tol);

  // 停止时需要的条件；
  nlopt_set_xtol_rel(opter, tol);

  // 开始优化；
  nlopt_result result=nlopt_optimize(opter, x, &f_max);

  if (result)
  {
    printf("目标函数最大值=%g, x=(%g,%g)\n", f_max, x[0], x[1], x[2], x[3]);
  }

  //free
  nlopt_destroy(opter);
  return 0;
}

 

test.h如下：

 

#pragma once
#include<iostream>
#include <vector>
#include <string>#include <iomanip>
#include <nlopt.hpp>
#include <stdio.h>
#include <math.h>

 

运行结果：

更多的案例参考：https://www.cnblogs.com/GuanghuiLiu/p/9967684.html

运动规划案例：https://github.com/HKUST-Aerial-Robotics/grad_traj_optimization

求解器类型：

     GN_DIRECT = 0,     GN_DIRECT_L,     GN_DIRECT_L_RAND,     GN_DIRECT_NOSCAL,     GN_DIRECT_L_NOSCAL,     GN_DIRECT_L_RAND_NOSCAL,     GN_ORIG_DIRECT,     GN_ORIG_DIRECT_L,     GD_STOGO,     GD_STOGO_RAND,
     LD_LBFGS_NOCEDAL,     LD_LBFGS,     LN_PRAXIS,     LD_VAR1,     LD_VAR2,     LD_TNEWTON,     LD_TNEWTON_RESTART,     LD_TNEWTON_PRECOND,     LD_TNEWTON_PRECOND_RESTART,
     GN_CRS2_LM,     GN_MLSL,     GD_MLSL,     GN_MLSL_LDS,     GD_MLSL_LDS,
     LD_MMA,     LN_COBYLA,     LN_NEWUOA,     LN_NEWUOA_BOUND,     LN_NELDERMEAD,     LN_SBPLX,     LN_AUGLAG,
     LD_AUGLAG,     LN_AUGLAG_EQ,     LD_AUGLAG_EQ,     LN_BOBYQA,     GN_ISRES,     AUGLAG,     AUGLAG_EQ,     G_MLSL,     G_MLSL_LDS,     LD_SLSQP,     LD_CCSAQ,     GN_ESCH,

G/L代表的就是 全局（global）或者局部（local）优化，N/D代表的就是 不需导数 或者 需要导数 的优化.

例如 LN_COBYLA 就是用的 COBYLA 算法 ，然后该算法用于局部（L）无需导数（N）的优化

 

四. OOQP

OOQP是一个基于原对偶内点方法的面向对象C++程序包，用于求解凸二次规划问题.

1.  安装：

OOQP的安装之前需要安装完成BLAS和MA27/MA57

  BLAS安装命令：

       sudo apt-get install libblas-dev

       安装位置：

       /usr/lib/x86_64-linux-gnu/libblas.so    /usr/lib/x86_64-linux-gnu/libblas.a 

 

  MA27/MA57安装：

 

 

2.  使用

 

 

五. Mosek

 

MOSEK是一款数学优化求解器，也是公认的求解二次规划、二阶锥规划和半正定规划问题最快的求解器之一.

 

 

 

六. OSQP

 

OSQP求解形式如下的凸二次规划问题.

 目标函数是二次的，约束是线性的.

1.  安装：

安装 qsqp-eigen

cmeke ..

make

sudo make install

2.  使用： 

https://zhuanlan.zhihu.com/p/376122376

（1）创建求解器Solver的实例

　　OsqpEigen::Solver solver;

（2）设置Setting参数

 

（3）设置Data参数

 

（4）初始化求解器Solver

（5）求解优化问题

（6）提取最优解

 

七. OpenCV

 

posted @ 2023-06-22 16:03  Derek_dhb  阅读(460)  评论(0)  编辑  收藏  举报