基于NLopt库的非线性优化实例

1. 目标函数

在编写目标函数时，若是不便写出显示表达式，可以分步骤推导出目标函数。t是自变量数组，grad是目标函数对自变量的梯度数组（利用无导数接口时，在函数体内可以不用写出grad的表达式），my_func_data可以传入目标函数中需要用到的一些参数，是一个指向结构体的指针。

double myfunc(unsigned n, const double *t,double *grad, void *my_func_data)
{
    my_function_data * function_data_ptr = (my_function_data *) my_func_data;
    Eigen::Vector3d _P0,_Pf,_V0,_Vf,delta_P,delta_V;
    Eigen::RowVector3d temp1,temp2;
    double J;
    _P0 = function_data_ptr->_P0;
    _Pf = function_data_ptr->_Pf;
    _V0 = function_data_ptr->_V0;
    _Vf = function_data_ptr->_Vf;

    double alpha_1,alpha_2,alpha_3,beta_1,beta_2,beta_3;
    delta_P = _Pf - _V0 * t[0]- _P0;
    delta_V = _Vf - _V0;
    //alpha_1
    temp1(0) = -12.0/pow(t[0],3);
    temp1(1) = 0;
    temp1(2) = 0;

    temp2(0) = 6.0/(t[0]*t[0]);
    temp2(1) = 0;
    temp2(2) = 0;

    alpha_1 =  (temp1 * delta_P + temp2 * delta_V)(0);

    //alpha_2
    temp1(0) = 0;
    temp1(1) = -12.0/pow(t[0],3);
    temp1(2) = 0;

    temp2(0) = 0;
    temp2(1) = 6.0/(t[0]*t[0]);
    temp2(2) = 0;

    alpha_2 =  (temp1 * delta_P + temp2 * delta_V)(0);

    //alpha_3
    temp1(0) = 0;
    temp1(1) = 0;
    temp1(2) = -12.0/pow(t[0],3);

    temp2(0) = 0;
    temp2(1) = 0;
    temp2(2) = 6.0/(t[0]*t[0]);

    alpha_3 =  (temp1 * delta_P + temp2 * delta_V)(0);


    //beta_1
    temp1(0) = 6.0/(t[0]*t[0]);
    temp1(1) = 0;
    temp1(2) = 0;

    temp2(0) = -2.0/t[0];
    temp2(1) = 0;
    temp2(2) = 0;

    beta_1 =  (temp1 * delta_P + temp2 * delta_V)(0);

    //beta_2
    temp1(0) = 0;
    temp1(1) = 6.0/(t[0]*t[0]);
    temp1(2) = 0;

    temp2(0) = 0;
    temp2(1) = -2.0/t[0];
    temp2(2) = 0;

    beta_2 =  (temp1 * delta_P + temp2 * delta_V)(0);

    //beta_3
    temp1(0) = 0;
    temp1(1) = 0;
    temp1(2) = 6.0/(t[0]*t[0]);

    temp2(0) = 0;
    temp2(1) = 0;
    temp2(2) = -2.0/t[0];

    beta_3 =  (temp1 * delta_P + temp2 * delta_V)(0);

    J = t[0] + (alpha_1 * alpha_1 * pow(t[0],3))/3.0 + alpha_1 * beta_1 * t[0] * t[0] + beta_1 * beta_1 *t[0]
                       + (alpha_2 * alpha_2 * pow(t[0],3))/3.0 + alpha_2 * beta_2 * t[0] * t[0] + beta_2 * beta_2 *t[0]
                       + (alpha_3 * alpha_3 * pow(t[0],3))/3.0 + alpha_3 * beta_3 * t[0] * t[0] + beta_3 * beta_3 *t[0];
    return J;
}

2. 参数结构体

参数结构体打包了在目标函数中需要用到的全部参数。

typedef struct {
    Eigen::Vector3d _P0,_Pf,_V0,_Vf;
} my_function_data;

3. 问题求解

这里以求解无人机起点到终点的最优控制问题为例，给出了求解最优解的过程。

double Homeworktool::OptimalBVP(Eigen::Vector3d _start_position,Eigen::Vector3d _start_velocity,Eigen::Vector3d _target_position)
{
    double optimal_cost = 100000; 
    //将到达终点的速度置为0
    Eigen::Vector3d _target_velocity;
    _target_velocity(0) = 0;
    _target_velocity(1) = 0;
    _target_velocity(2) = 0;

    //构建优化器
    nlopt::opt opter(nlopt::LN_COBYLA, 1);
    //设置优化器中用到的数据
    my_function_data position_and_velocity;
    position_and_velocity._P0 = _start_position;
    position_and_velocity._Pf = _target_position;
    position_and_velocity._V0 = _start_velocity;
    position_and_velocity._Vf = _target_velocity;
    my_function_data* position_and_velocity_ptr;
    position_and_velocity_ptr = &position_and_velocity;
    //设置时间t的下限
    std::vector<double> lb(1);
    lb[0] = 0;
    opter.set_lower_bounds(lb);
    opter.set_min_objective(myfunc, position_and_velocity_ptr);
    //设置终止条件
    opter.set_xtol_rel(1e-4);;
    //设置初始条件
    std::vector<double> t(1);
    t[0] = 5;
    //求解最小值
    double minf = optimal_cost;
    //输出结果
    try{
        nlopt::result result = opter.optimize(t, minf);
        std::cout << "found minimum at f(" << t[0] << ") = "
            << std::setprecision(10) << minf << std::endl;
    }
    catch(std::exception &e) {
        std::cout << "nlopt failed: " << e.what() << std::endl;
    }

    optimal_cost = minf;
    return optimal_cost;
}

4. CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(grid_path_searcher)

find_package(catkin REQUIRED COMPONENTS 
    roscpp 
    std_msgs
    nav_msgs
    visualization_msgs 
)

find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(NLopt REQUIRED)

set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
set(Ceres_INCLUDE_DIRS ${CERES_INCLUDE_DIR})
set(NLopt_INCLUDE_DIRS ${NLOPT_INCLUDE_DIR})

catkin_package(
  INCLUDE_DIRS include
)

include_directories(
    include 
    SYSTEM 
    third_party
    ${catkin_INCLUDE_DIRS} 
    ${Eigen3_INCLUDE_DIRS} 
    ${PCL_INCLUDE_DIRS}
    ${NLOPT_INCLUDE_DIR}
)

set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} -O3 -Wall") # -Wextra -Werror

add_executable( demo_node 
    src/demo_node.cpp
    src/hw_tool.cpp)

target_link_libraries(demo_node 
    ${catkin_LIBRARIES}
    ${PCL_LIBRARIES}
    ${NLOPT_LIBRARIES}
)

add_executable ( random_complex 
    src/random_complex_generator.cpp )

target_link_libraries( random_complex
    ${catkin_LIBRARIES}
    ${PCL_LIBRARIES}
)