系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理
文章目录
前言
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对使用nlopt优化库进行路径规划的后端轨迹优化–梯度下降的函数优化使得轨迹形变做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章
提示：以下是本篇文章正文内容
一、nlopt进行轨迹优化的目的
1.输入
要进行优化的控制点points、优化的时间间隔ts，优化项cost_function，最大的迭代次数max_num_id、最大的优化时间max_time_id、一开始不需要进行优化的控制点数量
.
2.输出
使用nlopt优化库进行优化，输出优化过后的控制点points
.
.
二、步骤一：设计相关的代价目标函数【用于计算代价cost和梯度grad】
1.子目标函数的设计
【用xy的几何运算搞出cost和grad】【防盗标记–盒子君hzj】
其中，平顺项，速度、加速度项都是利用均匀B样条的微分公式，求出速度控制点，加速度控制点，jerk控制点，从而构造与相应控制点相关的优化项。
梯度则是利用链式法则求偏导，用cost对影响这一cost的控制点求导，然后在循环中对cost及各控制点梯度进行累加（一个控制点会影响多个控制点的优化项）
.
.
（1）平滑度代价函数calcSmoothnessCost（）
核心原理：jerk加加速度函数平滑取极值，即是求加加速度的距离代价cost和梯度【防盗标记–盒子君hzj】
//设计平滑度代价函数
//核心原理：jerk平滑取极值，即是求加加速度的距离代价cost和梯度
void calcSmoothnessCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
//初始化代价值和梯度为0
cost = 0.0;
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
//遍历所有的控制点
Eigen::Vector3d jerk, temp_j;
for (int i = 0; i < q.size() - order_; i++)
{
/* 代价值 evaluate jerk */
jerk = q[i + 3] - 3 * q[i + 2] + 3 * q[i + 1] - q[i];
cost += jerk.squaredNorm();
temp_j = 2.0 * jerk;
/* 梯度值 jerk gradient */
gradient[i + 0] += -temp_j;
gradient[i + 1] += 3.0 * temp_j;
gradient[i + 2] += -3.0 * temp_j;
gradient[i + 3] += temp_j;
}
}
.
.
（2）远离障碍物代价优化calcDistanceCost（）
核心原理：直接用esdf地图数据来填充【若是TEB算法用代价地图的，实现可能不一样，但是原理是一样的】
//远离障碍物优化
//核心原理：直接用esdf地图数据来填充【若是TEB算法用代价地图的，实现可能不一样，但是原理是一样的】
void calcDistanceCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
// *********************** comment out for later use ***********************************
//初始化代价值和梯度为0
cost = 0.0;
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
double dist;
Eigen::Vector3d dist_grad, g_zero(0, 0, 0);
int end_idx = (cost_function_ & ENDPOINT) ? q.size() : q.size() - order_;
for (int i = order_; i < end_idx; i++) {
// edt_environment_->evaluateEDTWithGrad(q[i], -1.0, dist, dist_grad);
Eigen::Vector2f grad;
dist = collision_detection_->getDistWithGradBilinear(Eigen::Vector2f(q[i](0), q[i](1)), grad);
constexpr double max_range = 2.8;
if (dist < 0 || dist > max_range) {
dist_grad = Eigen::Vector3d(0.0, 0.0, 0.0);
dist = max_range;
} else {
dist_grad(0) = grad(0);
dist_grad(1) = grad(1);
dist_grad(2) = 0.0;
}
if (dist_grad.norm() > 1e-4) dist_grad.normalize();
if (dist < dist0_) {
cost += pow(dist - dist0_, 2);
gradient[i] += 2.0 * (dist - dist0_) * dist_grad;
}
}
}
.
.
（3）计算速度、加速度可行性代价函数calcFeasibilityCost（）
核心原理：计算飞机现在的速度和加速度有没有超过阈值，若超过计算对应的grad和cost(几何的方式算)
【防盗标记–盒子君hzj】
// *********************** comment out for later use ***********************************
//计算速度、加速度可行性代价函数
//核心原理：计算飞机现在的速度和加速度有没有超过阈值，若超过计算对应的grad和cost(几何的方式算)
void calcFeasibilityCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
//初始化代价值和梯度为0
cost = 0.0;
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
/* 定义相关中间变量 */
double ts, vm2, am2, ts_inv2, ts_inv4;
vm2 = max_vel_ * max_vel_;
am2 = max_acc_ * max_acc_;
ts = bspline_interval_;
ts_inv2 = 1 / ts / ts;
ts_inv4 = ts_inv2 * ts_inv2;
/* 速度可行性 velocity feasibility */
for (int i = 0; i < q.size() - 1; i++) {
Eigen::Vector3d vi = q[i + 1] - q[i]; //求速度（约去时间）
for (int j = 0; j < 3; j++) {
double vd = vi(j) * vi(j) * ts_inv2 - vm2; //判断按现在的速度和最大速度的差值来优化
if (vd > 0.0) {
cost += pow(vd, 2);
double temp_v = 4.0 * vd * ts_inv2;
gradient[i + 0](j) += -temp_v * vi(j);
gradient[i + 1](j) += temp_v * vi(j);
}
}
}
/*加速可行性 acceleration feasibility */
for (int i = 0; i < q.size() - 2; i++) {
Eigen::Vector3d ai = q[i + 2] - 2 * q[i + 1] + q[i]; //求加速度（约去时间）
for (int j = 0; j < 3; j++) {
double ad = ai(j) * ai(j) * ts_inv4 - am2; //判断按现在的加速度和最大速度的差值来优化
if (ad > 0.0) {
cost += pow(ad, 2);
double temp_a = 4.0 * ad * ts_inv4;
gradient[i + 0](j) += temp_a * ai(j);
gradient[i + 1](j) += -2 * temp_a * ai(j);
gradient[i + 2](j) += temp_a * ai(j);
}
}
}
}
.
.
（4）计算最后一个吻合程度代价函数
核心原理：终点非均值滤波后得到的终点和理想终点的差值，通过二范数计算距离 cost=xx + yy
【防盗标记–盒子君hzj】
//计算最后一个吻合程度代价函数
//核心原理：终点非均值滤波后得到的终点和理想终点的差值，通过二范数计算距离 cost=x*x + y*y
void calcEndpointCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
//初始化代价值和梯度为0
cost = 0.0;
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
//硬约束下的零成本与梯度
Eigen::Vector3d q_3, q_2, q_1, dq;
q_3 = q[q.size() - 3];
q_2 = q[q.size() - 2];
q_1 = q[q.size() - 1];
dq = 1 / 6.0 * (q_3 + 4 * q_2 + q_1) - end_pt_; //终点非均值滤波后得到的终点和理想终点的差值
cost += dq.squaredNorm(); //通过二范数计算距离 cost=x*x + y*y
gradient[q.size() - 3] += 2 * dq * (1 / 6.0);
gradient[q.size() - 2] += 2 * dq * (4 / 6.0);
gradient[q.size() - 1] += 2 * dq * (1 / 6.0);
}
.
.
（5）计算路径点贴合的代价函数
核心原理：非均值滤波后得到的路径点和理想路径点的差值，通过二范数计算距离 cost=xx + yy
【防盗标记–盒子君hzj】
//计算路径点贴合的代价函数
//核心原理：非均值滤波后得到的路径点和理想路径点的差值，通过二范数计算距离 cost=x*x + y*y
void calcWaypointsCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
//初始化代价值和梯度为0
cost = 0.0;
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
Eigen::Vector3d q1, q2, q3, dq;
// for (auto wp : waypoints_) {
for (int i = 0; i < waypoints_.size(); ++i) {
Eigen::Vector3d waypt = waypoints_[i];
int idx = waypt_idx_[i];
q1 = q[idx];
q2 = q[idx + 1];
q3 = q[idx + 2];
dq = 1 / 6.0 * (q1 + 4 * q2 + q3) - waypt; //非均值滤波后得到的路径点和理想路径点的差值
cost += dq.squaredNorm(); //通过二范数计算距离 cost=x*x + y*y
gradient[idx] += dq * (2.0 / 6.0); // 2*dq*(1/6)
gradient[idx + 1] += dq * (8.0 / 6.0); // 2*dq*(4/6)
gradient[idx + 2] += dq * (2.0 / 6.0);
}
}
（6）计算路径点引导的代价函数
核心原理：使用几何路径上的均匀采样点引导轨迹。对于每个要优化的控制点，在路径上为其指定一个引导点，并对它们之间的距离进行惩罚
//计算路径点引导的代价函数
//核心原理：使用几何路径上的均匀采样点引导轨迹。对于每个要优化的控制点，在路径上为其指定一个引导点，并对它们之间的距离进行惩罚
void calcGuideCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
//初始化代价值和梯度为0
cost = 0.0;
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
int end_idx = q.size() - order_;
for (int i = order_; i < end_idx; i++) {
Eigen::Vector3d gpt = guide_pts_[i - order_];
cost += (q[i] - gpt).squaredNorm();
gradient[i] += 2 * (q[i] - gpt);
}
}
2.总的目标函数-combine所有的子目标函数
核心原理
每个子目标函数都有对应的权重，把每个子子目标函数看成一个项，累加到一起就行了
并将其自变量从控制点转化为Nlopt优化的变量x
结合三种优化项的函数是combineCost()函数，函数有三个参数，第一个std::vector& x即是Nlopt优化的变量，应该与三个维度的控制点对应。第二个参数是std::vector& grad，即总的优化项关于每个优化变量的梯度信息。第三个参数是double& f_combine 。即结合后的Cost。
首先是给g_q赋值，g_q是用来计算每次优化循环三个优化项的控制点。值得注意的是，前 p b p_bp
b
个控制点和最后p b个控制点是不进行优化的，始终保持线性拟合得到控制点原值。中间的控制点则是因为每一次迭代优化后都不同，因此用x来赋值。这里的x是通过Nlopt的opt 对象在set_min_objective中进行初始化的，具体的大小在构造 Nlopt optimizer对象时就通过variable_num的大小确定了。而初始值则是在Nlopt求解时 .optimize函数中进行赋值。
接下来就是利用CalcSmoothneesCost, CalcDistanceCost, CalcFeasibilityCost 三个函数计算每一部分的cost和grad,并乘上权重后累加至grad 和 f_combine中，唯一需要注意的是grad和各部分优化项梯度之间维度上差了两个B样条次数
至此，我们已经构造完成了优化变量、目标函数、梯度信息
【防盗标记–盒子君hzj】
//计算并累加所有的代价及其梯度【根据权重值进行代价值累加】
//输出的是最小的代价f_combine和梯度grad
void combineCost(const std::vector<double>& x, std::vector<double>& grad,
double& f_combine) {
/*(1)将NLopt格式向量转换为控制点格式 */
//该解算器可以支持1D-3D B样条优化，但我们使用Vector3d存储每个控制点。对于1D情况，第二个和第三个元素为零，对于2D情况类似。
// std::cout << "individual control points:" << std::endl;
for (int i = 0; i < order_; i++) {
for (int j = 0; j < dim_; ++j) {
g_q_[i][j] = control_points_(i, j);
// std::cout << g_q_[i][j] << " ";
}
// std::cout << std::endl;
for (int j = dim_; j < 3; ++j) {
g_q_[i][j] = 0.0;
}
}
for (int i = 0; i < variable_num_ / dim_; i++) {
for (int j = 0; j < dim_; ++j) {
g_q_[i + order_][j] = x[dim_ * i + j];
}
for (int j = dim_; j < 3; ++j) {
g_q_[i + order_][j] = 0.0;
}
}
if (!(cost_function_ & ENDPOINT)) {
for (int i = 0; i < order_; i++) {
for (int j = 0; j < dim_; ++j) {
g_q_[order_ + variable_num_ / dim_ + i][j] =
control_points_(control_points_.rows() - order_ + i, j);
}
for (int j = dim_; j < 3; ++j) {
g_q_[order_ + variable_num_ / dim_ + i][j] = 0.0;
}
}
}
f_combine = 0.0;
grad.resize(variable_num_);
fill(grad.begin(), grad.end(), 0.0);
/*（2）计算代价f_combine及其梯度grad*/
//各项的代价值
double f_smoothness, f_distance, f_feasibility, f_endpoint, f_guide, f_waypoints, f_follow;
f_smoothness = f_distance = f_feasibility = f_endpoint = f_guide = f_waypoints = 0.0;
//平滑性（保证了路径没有突变）
if (cost_function_ & SMOOTHNESS) {
calcSmoothnessCost(g_q_, f_smoothness, g_smoothness_); //1）计算代价值，g_q_是路径点
f_combine += lambda1_ * f_smoothness; //2）根据权重值进行代价值累加
for (int i = 0; i < variable_num_ / dim_; i++) //3）计算梯度
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda1_ * g_smoothness_[i + order_](j);
}
//距离（用到了esdf地图，保证了原理障碍物）
if (cost_function_ & DISTANCE) {
calcDistanceCost(g_q_, f_distance, g_distance_); //1）计算代价值
f_combine += lambda2_ * f_distance; //2）根据权重值进行代价值累加
for (int i = 0; i < variable_num_ / dim_; i++) //3）计算梯度
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda2_ * g_distance_[i + order_](j);
}
//可行性（保证了路径的速度和加速是不会突变的）
if (cost_function_ & FEASIBILITY) {
calcFeasibilityCost(g_q_, f_feasibility, g_feasibility_);//1）计算代价值
f_combine += lambda3_ * f_feasibility; //2）根据权重值进行代价值累加
for (int i = 0; i < variable_num_ / dim_; i++) //3）计算梯度
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda3_ * g_feasibility_[i + order_](j);
}
//最后点（保证了优化出来的终点和原来的终点相距不太远）
if (cost_function_ & ENDPOINT) {
calcEndpointCost(g_q_, f_endpoint, g_endpoint_); //1）计算代价值
f_combine += lambda4_ * f_endpoint; //2）根据权重值进行代价值累加
for (int i = 0; i < variable_num_ / dim_; i++) //3）计算梯度
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda4_ * g_endpoint_[i + order_](j);
}
//引导 （保证啥？）
if (cost_function_ & GUIDE) {
calcGuideCost(g_q_, f_guide, g_guide_); //1）计算代价值
f_combine += lambda5_ * f_guide; //2）根据权重值进行代价值累加
for (int i = 0; i < variable_num_ / dim_; i++) //3）计算梯度
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda5_ * g_guide_[i + order_](j);
}
//路径点（保证了优化出来的路径和原来的路径相差不太远）
if (cost_function_ & WAYPOINTS) {
calcWaypointsCost(g_q_, f_waypoints, g_waypoints_); //1）计算代价值
f_combine += lambda7_ * f_waypoints; //2）根据权重值进行代价值累加
for (int i = 0; i < variable_num_ / dim_; i++) //3）计算梯度
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda7_ * g_waypoints_[i + order_](j);
}
// 若是3D的曲线，一开始的一些路径点忽略，使其梯度为零lock first few points
if (dim_ == 3) {
for (int i = 0; i < ignore_num_; i++) {
if (i < variable_num_ / dim_) {
for (int j = 0; j < dim_; j++) {
grad[dim_ * i + j] = 0;
}
}
}
// for (int i = 1; i <= 3; i++) {
// grad[variable_num_ - i] = 0.0;
// }
}
// stick to line
// if (dim_ == 3) {
// calcFollowCost(g_q_, f_follow, g_follow_);
// f_combine += lambda9_ * f_follow;
// for (int i = 0; i < variable_num_ / dim_; i++)
// for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda9_ * g_follow_[i + order_](j);
// }
/* print cost */
// if ((cost_function_ & WAYPOINTS) && iter_num_ % 10 == 0) {
// cout << iter_num_ << ", total: " << f_combine << ", acc: " << lambda8_ * f_view
// << ", waypt: " << lambda7_ * f_waypoints << endl;
// }
// if (optimization_phase_ == SECOND_PHASE) {
// << ", smooth: " << lambda1_ * f_smoothness
// << " , dist:" << lambda2_ * f_distance
// << ", fea: " << lambda3_ * f_feasibility << endl;
// << ", end: " << lambda4_ * f_endpoint
// << ", guide: " << lambda5_ * f_guide
// }
}
.
//设置代价函数类型[作为nlopt库的输入]
//返回的是grad，func_data给opt的API
double costFunction(const std::vector<double>& x, std::vector<double>& grad,
void* func_data) {
/*(1)计算并累加所有的代价及其梯度*/
BsplineOptimizer* opt = reinterpret_cast<BsplineOptimizer*>(func_data);
double cost; //代价值
opt->combineCost(x, grad, cost); //输入路径点x，返回代价值cost、还有梯度grad【nlopt内置实现了梯度下降，使得路径形变】
opt->iter_num_++;
/*（2）保存最小代价的结果到best_variable_ save the min cost result */
if (cost < opt->min_cost_) {
opt->min_cost_ = cost;
opt->best_variable_ = x;
}
return cost;
// /* （3）评价evaluation */
// ros::Time te1 = ros::Time::now();
// double time_now = (te1 - opt->time_start_).toSec();
// opt->vec_time_.push_back(time_now);
// if (opt->vec_cost_.size() == 0)
// {
// opt->vec_cost_.push_back(f_combine);
// }
// else if (opt->vec_cost_.back() > f_combine)
// {
// opt->vec_cost_.push_back(f_combine);
// }
// else
// {
// opt->vec_cost_.push_back(opt->vec_cost_.back());
// }
}
.
三、步骤二：设置nlopt优化器的相关的输入参数
1.设置优化器的参数
【防盗标记–盒子君hzj】
void setParam() {
//（1）设置优化器权重参数
lambda1_ = 0.2;//0.1;//10.0; // smooth
lambda2_ = 0.8;
lambda3_ = 100;//1;//0.00001; // feasibility
lambda4_ = 0.01;
lambda5_ = -1.0;
lambda6_ = -1.0;
lambda7_ = 0.1;//5.0; // waypoints
lambda8_ = -1.0;
lambda9_ = 0.1;
//（2）设置输入参数
dist0_ = 2.6;
max_vel_ = 9;//7;//6;
max_acc_ = 3;//2.0;//1.0;
visib_min_ = -1.0;
dlmin_ = -1.0;
wnl_ = -1.0;
max_iteration_num_[0] = 2;
max_iteration_num_[1] = 10000;//1000; //5;
max_iteration_num_[2] = 200;
max_iteration_num_[3] = 200;
max_iteration_time_[0] = 0.0001;
max_iteration_time_[1] = 0.07;
max_iteration_time_[2] = 0.01;
max_iteration_time_[3] = 0.003;
//（3）用枚举的方法给定优化算法的类型
algorithm1_ = 15;
algorithm2_ = 11;
order_ = 3; //阶数为3的B样条
}
2.设置路径控制点
void setControlPoints(const Eigen::MatrixXd& points) {
control_points_ = points;
dim_ = control_points_.cols();
}
3.设置优化周期
void setBsplineInterval(const double& ts) {
bspline_interval_ = ts; }
void setTerminateCond(const int& max_num_id, const int& max_time_id) {
max_num_id_ = max_num_id;
max_time_id_ = max_time_id;
}
4.设置使用什么优化项
void setCostFunction(const int& cost_code) {
cost_function_ = cost_code;
// print optimized cost function
string cost_str;
if (cost_function_ & SMOOTHNESS) cost_str += "smooth |";
if (cost_function_ & DISTANCE) cost_str += " dist |";
if (cost_function_ & FEASIBILITY) cost_str += " feasi |";
if (cost_function_ & ENDPOINT) cost_str += " endpt |";
if (cost_function_ & GUIDE) cost_str += " guide |";
if (cost_function_ & WAYPOINTS) cost_str += " waypt |";
// ROS_INFO_STREAM("cost func: " << cost_str);
}
5.设置路径点
【防盗标记–盒子君hzj】
void setGuidePath(const vector<Eigen::Vector3d>& guide_pt) {
guide_pts_ = guide_pt; }
void setWaypoints(const vector<Eigen::Vector3d>& waypts,
const vector<int>& waypt_idx) {
waypoints_ = waypts;
waypt_idx_ = waypt_idx;
}
.
.
四、步骤三：通过Nlopt对控制点进行非线性优化【使用了nlopt优化库】
核心原理
BsplineOptimizeTraj()函数，将优化的控制点，均匀B样条的时间间隔，cost Function包含的优化项，以及终止条件（最大优化次数及最长优化时间）都设置好以后，就利用BsplineOptimizer的optimize()函数进行优化。
BsplineOptimizer的optimize()函数首先根据pt_num确定各部分优化项梯度vector的大小，然后再根据是否有终值点约束确定优化变量的个数。
接下来就是实例化Nlopt::opt类对象 opt。并设定目标函数。设定最大优化次数与最长优化时间，设定目标函数的最小值（这三者都是设定终止条件）。接着，根据线性你和得到的控制点设置优化变量的初值，并设置每个优化变量的上下界（初始值±10）。
最后利用Nlopt::opt类对行opt的optimize函数进行迭代优化求解，在求解结束后，通过costFunction中保留的best_Variable对control_point进行赋值。
//核心思想：
//输入：要进行优化的控制点points、优化的时间间隔ts，优化项类型cost_function，最大的迭代次数max_num_id、最大的优化时间max_time_id、一开始不需要进行优化的控制点数量
//输出：使用nlopt优化库进行优化，输出优化过后的控制点points
Eigen::MatrixXd BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,
const int& cost_function, int max_num_id,
int max_time_id, int ignore_num) {
//（1）设置相关的输入参数
setControlPoints(points);
setBsplineInterval(ts);
setCostFunction(cost_function);
setTerminateCond(max_num_id, max_time_id);
ignore_num_ = ignore_num;
//（2）进行优化操作【使用了nlopt优化库】
optimize();
//（3）返回优化后的控制点control_points_
return this->control_points_;
}
//进行优化操作
void optimize()
{
/* （1）初始化优化器参数并计算优化变量的数目initialize solver */
iter_num_ = 0; //迭代的次数
min_cost_ = std::numeric_limits<double>::max(); //最小的代价
const int pt_num = control_points_.rows(); //控制点数量
//相关控制点的代价权重
g_q_.resize(pt_num);
g_smoothness_.resize(pt_num);
g_distance_.resize(pt_num);
g_feasibility_.resize(pt_num);
g_endpoint_.resize(pt_num);
g_waypoints_.resize(pt_num);
g_guide_.resize(pt_num);
g_follow_.resize(pt_num);
//计算优化变量的数目
if (cost_function_ & ENDPOINT) //启动结束位置优化
{
variable_num_ = dim_ * (pt_num - order_);
//结束位置用于硬约束
//计算轨迹最后的一个点【结束位置的约束优化的方法 --权重滑动滤波法】
end_pt_ = (1 / 6.0) *
(control_points_.row(pt_num - 3) + 4 * control_points_.row(pt_num - 2) +
control_points_.row(pt_num - 1));
}
else //不启动结束位置优化
{
variable_num_ = max(0, dim_ * (pt_num - 2 * order_)) ;
}
/* （2）使用NLopt库进行优化 do optimization using NLopt slover */
//1）使用nlopt库创建一个优化器opt，并进行相关API的设置【包括目标代价、约束条件（最大的迭代次数、最大的优化时间）】
nlopt::opt opt(nlopt::algorithm(isQuadratic() ? algorithm1_ : algorithm2_), variable_num_); //给定优化变量和使用哪种优化算法【二次优化？/成本函数优化？，不同的优化项使用不用类型的优化算法】
opt.set_min_objective(BsplineOptimizer::costFunction, this);//【核心】设置要使用的代价函数类型 //costFunction返回的是grad，func_data给opt的API
opt.set_maxeval(max_iteration_num_[max_num_id_]); //【约束条件】设置最大的迭代次数
opt.set_maxtime(max_iteration_time_[max_time_id_]); //【约束条件】设置最大的优化时间
opt.set_xtol_rel(1e-5); //【终止条件】停止时需要的条件，绝对容差
// opt.set_xtol_rel(1e-7);
//2）遍历所有的控制点，并把所有的控制点放入中间变量q中【后面就是对这个中间变量q进行优化的】
vector<double> q(variable_num_);
for (int i = order_; i < pt_num; ++i) //pt_num是控制点数量
{
if (!(cost_function_ & ENDPOINT) && i >= pt_num - order_) continue;
for (int j = 0; j < dim_; j++) //dim是B样条的阶数
{
q[dim_ * (i - order_) + j] = control_points_(i, j);
// std::cout << q[dim_ * (i - order_) + j] << " ";
}
// std::cout << std::endl;
}
//3）【约束条件】nlopt优化库设定优化变量上下边界
if (dim_ != 1)
{
vector<double> lb(variable_num_), ub(variable_num_);//下边界lb、上边界ub
const double bound = 10.0;
for (int i = 0; i < variable_num_; ++i) {
lb[i] = q[i] - bound;
ub[i] = q[i] + bound;
}
opt.set_lower_bounds(lb);
opt.set_upper_bounds(ub);
}
//4）开始优化nlopt的优化
try
{
double final_cost;
nlopt::result result = opt.optimize(q, final_cost);//开始优化nlopt的优化
}
catch (std::exception& e) {
// ROS_WARN("[Optimization]: nlopt exception");
cout << e.what() << endl;
}
//5)得到最优的控制点control_points_
for (int i = order_; i < control_points_.rows(); ++i) {
if (!(cost_function_ & ENDPOINT) && i >= pt_num - order_) continue;
for (int j = 0; j < dim_; j++) {
control_points_(i, j) = best_variable_[dim_ * (i - order_) + j];
}
}
// if (!(cost_function_ & GUIDE)) ROS_INFO_STREAM("iter num: " << iter_num_);
}
五、步骤四：获取优化出来的路径点
【防盗标记–盒子君hzj】
getControlPoints() {
return this->control_points_; }
总结
nlopt优化库怎么用？
看我另外一篇那个博客，在路径规划planning专栏~
还有下面几个好的链接：
https://blog.csdn.net/Travis_X/article/details/102960895
https://www.cnblogs.com/GuanghuiLiu/p/9967684.html
https://zhuanlan.zhihu.com/p/24350637
参考资料
上面使用的nlopt优化库还有优化思想流程，【防盗标记–盒子君hzj】我是直接从高飞实验室fast_planner工程里面提取出来的，你想要更多？去深钻fast_planner吧
当然，上面的流程是我总结出来的，依着我亏优化器使用步骤总结的，有什么错误可以私信我，相互学习~【防盗标记–盒子君hzj】