Fast-Planner代码阅读-1. Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight
发布时间
阅读量：
阅读量
文章目录
-
引言
- 一种动态优化A星算法（混合A星算法在前端阶段实现路径动态优化）
- 基于启发式的路径计算：引入动态权重因子以平衡全局最优与局部避障能力
- 规划关键轨迹段：通过分段插值法生成连续可导的目标运动轨迹
- 节点扩展操作：结合障碍物环境构建动态障碍物栅格模型
- 冗余节点剪枝：基于移动速度限制和障碍物距离阈值实现智能删减无效搜索空间
- 生成最终路径信息并输出结果集
-
二、B样条曲线设置
-
2.1 均匀B样条的设置
- 2.2 计算B样条函数值
- 2.3 通过扣除前端路径拟合的方法获得控制点
- 2.4 非均匀B样条的一阶及其二阶微分计算
- 2.5 进行可达性验证
- 2.6 总结部分
-
三、B样条优化
-
- 3.1 优化项计算
- 3.2 优化项结合
- 3.3 Nlopt优化
- 3.4 小结
-
四. 规划系统运行逻辑
-
- 4.1 plan_mannager
- 4.2 kino_replan_fsm.cpp
- 4.3 流程总结
-
前言
参考文献：B. Zhou, F. Gao, L. Wang, C. Liu and S. Shen, “用于生成鲁棒性高、高效的一条四旋翼无人机轨迹的自动化方法”，在IEEE Robotics and Automation Letters期刊于2019年10月发表。
代码地址：https://github.com/HKUST-Aerial-Robotics/Fast-Planner
考虑到无人机运动规划的重要性以及其在该领域中的关键作用地位，我们选择了这篇文章的代码部分进行深入研究分析。此外，文章涉及的核心技术和关键技术体系构成非常完善的技术体系框架。
涵盖了前端使用的 kinodynamic a_star 路径规划方法（通过生成 运动素元 在 控制空间 实现路径探索），此外还采用基于Pattulli最小值原理的两点边值问题求解方法（该方法通过生成 运动素元 在 状态空间 实现路径探索以克服稀疏采样的限制）。
后端部分首先采用 均匀B样条曲线模型 对前端生成的路径数据进行建模。随后基于构建的距离场图（ SDF 实现方案）分析轨迹的非时间积分属性。在这一过程中分别考虑了物体距离限制方面以及速度与加速度超出限制范围的情况进行了软约束处理。
经过优化后得到的轨迹，
采用 非均匀B样条曲线 进行表达；
接着对超出速度上限和超出加速度上限的所有曲线段
进行时间节点的迭代调整；
经过这些调整后，
在所有这些曲线段中确保其
速度与加速度均满足约束条件。
一. kinodynamic a_star（前端hybrid A_star动力学路径搜索）
该系统涵盖了从Fast planner到路径规划、路径搜索直至kinodynamic A*算法的完整流程。路径搜索的核心功能体现在kinodynamicAstar类中的search方法中。我们需要从该搜索算法的核心模块入手，并详细解析其关键组件：
该函数参数涉及起点以及关键位置，并包含速度和加速度；此外还带有标记位init和dynamic；另有一个搜索启动时间；在上图代码中主要实现了将起始点及其目标点的空间坐标转换为栅格地图中的索引值；并计算了第一个扩展节点的成本评估值。
1.1启发函数的计算
基于庞特里亚金理论的估计型启发式函数 estimateHeuristic（）首次被提出，在文章第III.B节中详细阐述了其设计思路与理论基础。该函数的核心理论基础在于通过求解最优控制问题获得最优解，并以最优解所对应的控制成本作为启发式函数的基础。
具体对应代码如下：
首先采用启发函数对时间变量进行求导运算，并令其等于零；由此获得了一个关于时间变量T的四次多项式方程；进而对该四次多项式方程展开求解；从而得到若干个实数范围内的有效解；计算每个实数解对应的成本值；其中涉及将该一元四次方程转化为费拉里方法下的标准形式；并在此过程中嵌套一个三次多项式方程用于辅助计算；在编程实现时，则需调用自定义开发的一个cubic（）函数来处理这一子问题。
一元四次方程的求解过程可参考wikipedia中的费拉里方法： https://zh.wikipedia.org/wiki/四次方程
一元三次方程的推导过程可参考维基百科上的"四次方程"解法。需要注意的是，在代码实现中使用了当判别式大于零和等于零时的求根公式；而当判别式小于零时，则采用了三角函数解法：
https://zh.wikipedia.org/wiki/三次方程
1.2 Compute shot Traj
确定起始节点的启发项之后
在当前阶段，我们遇到了一个关键函数ComputeShotTraj。通过庞特里亚金最小值原理来解决这个两点边值问题。由于在之前的阶段已经确定了最优控制时间，在此阶段只需将该时间代入多项式计算即可完成后续步骤。其主要目标是确保所设计的轨迹满足以下条件：避免与任何障碍物发生碰撞，并且在运行过程中保持合理的速度和加速度水平。
bool KinodynamicAstar::computeShotTraj(Eigen::VectorXd state1, Eigen::VectorXd state2, double time_to_goal)
{
/* ---------- get coefficient ---------- */
const Vector3d p0 = state1.head(3);
const Vector3d dp = state2.head(3) - p0;
const Vector3d v0 = state1.segment(3, 3);
const Vector3d v1 = state2.segment(3, 3);
const Vector3d dv = v1 - v0;
double t_d = time_to_goal;
MatrixXd coef(3, 4);
end_vel_ = v1;
Vector3d a = 1.0 / 6.0 * (-12.0 / (t_d * t_d * t_d) * (dp - v0 * t_d) + 6 / (t_d * t_d) * dv);
Vector3d b = 0.5 * (6.0 / (t_d * t_d) * (dp - v0 * t_d) - 2 / t_d * dv);
Vector3d c = v0;
Vector3d d = p0;
// 1/6 * alpha * t^3 + 1/2 * beta * t^2 + v0
// a*t^3 + b*t^2 + v0*t + p0
coef.col(3) = a, coef.col(2) = b, coef.col(1) = c, coef.col(0) = d;
Vector3d coord, vel, acc;
VectorXd poly1d, t, polyv, polya;
Vector3i index;
Eigen::MatrixXd Tm(4, 4);
Tm << 0, 1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 3, 0, 0, 0, 0;
/* ---------- forward checking of trajectory ---------- */
double t_delta = t_d / 10;
for (double time = t_delta; time <= t_d; time += t_delta)
{
t = VectorXd::Zero(4);
for (int j = 0; j < 4; j++)
t(j) = pow(time, j);
for (int dim = 0; dim < 3; dim++)
{
poly1d = coef.row(dim);
coord(dim) = poly1d.dot(t);
vel(dim) = (Tm * poly1d).dot(t);
acc(dim) = (Tm * Tm * poly1d).dot(t);
if (fabs(vel(dim)) > max_vel_ || fabs(acc(dim)) > max_acc_)
{
// cout << "vel:" << vel(dim) << ", acc:" << acc(dim) << endl;
// return false;
}
}
if (coord(0) < origin_(0) || coord(0) >= map_size_3d_(0) || coord(1) < origin_(1) || coord(1) >= map_size_3d_(1) ||
coord(2) < origin_(2) || coord(2) >= map_size_3d_(2))
{
return false;
}
// if (edt_environment_->evaluateCoarseEDT(coord, -1.0) <= margin_) {
// return false;
// }
if (edt_environment_->sdf_map_->getInflateOccupancy(coord) == 1)
{
return false;
}
}
coef_shot_ = coef;
t_shot_ = t_d;
is_shot_succ_ = true;
return true;
}若当前节点没有抵达终点的可能，就要进行节点扩张
1.3 节点扩张
open_set_.pop();
cur_node->node_state = IN_CLOSE_SET;
iter_num_ += 1;
double res = 1 / 2.0, time_res = 1 / 1.0, time_res_init = 1 / 20.0;
Eigen::Matrix<double, 6, 1> cur_state = cur_node->state;
Eigen::Matrix<double, 6, 1> pro_state;
vector<PathNodePtr> tmp_expand_nodes;
Eigen::Vector3d um;
double pro_t;
vector<Eigen::Vector3d> inputs;
vector<double> durations;
if (init_search)
{
inputs.push_back(start_acc_);
for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_ + 1e-3;
tau += time_res_init * init_max_tau_)
durations.push_back(tau);
init_search = false;
}
else
{
for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += max_acc_ * res)
for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += max_acc_ * res)
for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ * res)
{
um << ax, ay, az;
inputs.push_back(um);
}
for (double tau = time_res * max_tau_; tau <= max_tau_; tau += time_res * max_tau_)
durations.push_back(tau);
}
// cout << "cur state:" << cur_state.head(3).transpose() << endl;
for (int i = 0; i < inputs.size(); ++i)
for (int j = 0; j < durations.size(); ++j)
{
um = inputs[i];
double tau = durations[j];
stateTransit(cur_state, pro_state, um, tau);
pro_t = cur_node->time + tau;
Eigen::Vector3d pro_pos = pro_state.head(3);
// Check if in close set
Eigen::Vector3i pro_id = posToIndex(pro_pos);
int pro_t_id = timeToIndex(pro_t);
PathNodePtr pro_node = dynamic ? expanded_nodes_.find(pro_id, pro_t_id) : expanded_nodes_.find(pro_id);
if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET)
{
if (init_search)
std::cout << "close" << std::endl;
continue;
}
// Check maximal velocity
Eigen::Vector3d pro_v = pro_state.tail(3);
if (fabs(pro_v(0)) > max_vel_ || fabs(pro_v(1)) > max_vel_ || fabs(pro_v(2)) > max_vel_)
{
if (init_search)
std::cout << "vel" << std::endl;
continue;
}
// Check not in the same voxel
Eigen::Vector3i diff = pro_id - cur_node->index;
int diff_time = pro_t_id - cur_node->time_idx;
if (diff.norm() == 0 && ((!dynamic) || diff_time == 0))
{
if (init_search)
std::cout << "same" << std::endl;
continue;
}
// Check safety
Eigen::Vector3d pos;
Eigen::Matrix<double, 6, 1> xt;
bool is_occ = false;
for (int k = 1; k <= check_num_; ++k)
{
double dt = tau * double(k) / double(check_num_);
stateTransit(cur_state, xt, um, dt);
pos = xt.head(3);
if (edt_environment_->sdf_map_->getInflateOccupancy(pos) == 1 )
{
is_occ = true;
break;
}
}
if (is_occ)
{
if (init_search)
std::cout << "safe" << std::endl;
continue;
}
double time_to_goal, tmp_g_score, tmp_f_score;
tmp_g_score = (um.squaredNorm() + w_time_) * tau + cur_node->g_score;
tmp_f_score = tmp_g_score + lambda_heu_ * estimateHeuristic(pro_state, end_state, time_to_goal);基于当前节点，在输入与时间被离散化处理后生成tmp临时点。首先检查该点是否已被扩展过，并确认其与当前处理的点是否相同；接着验证速度限制条件以及潜在碰撞情况均满足条件时，则计算出该点的目标函数值g_score和启发式函数值f_score。其中状态转移函数是通过前向积分方法确定新生成点的状态信息（包括位置和速度）。随后执行路径剪裁优化步骤。
1.4 节点剪枝
// Compare nodes expanded from the same parent
bool prune = false;
for (int j = 0; j < tmp_expand_nodes.size(); ++j)
{
PathNodePtr expand_node = tmp_expand_nodes[j];
if ((pro_id - expand_node->index).norm() == 0 && ((!dynamic) || pro_t_id == expand_node->time_idx))
{
prune = true;
if (tmp_f_score < expand_node->f_score)
{
expand_node->f_score = tmp_f_score;
expand_node->g_score = tmp_g_score;
expand_node->state = pro_state;
expand_node->input = um;
expand_node->duration = tau;
if (dynamic)
expand_node->time = cur_node->time + tau;
}
break;
}
}
// This node end up in a voxel different from others
if (!prune)
{
if (pro_node == NULL)
{
pro_node = path_node_pool_[use_node_num_];
pro_node->index = pro_id;
pro_node->state = pro_state;
pro_node->f_score = tmp_f_score;
pro_node->g_score = tmp_g_score;
pro_node->input = um;
pro_node->duration = tau;
pro_node->parent = cur_node;
pro_node->node_state = IN_OPEN_SET;
if (dynamic)
{
pro_node->time = cur_node->time + tau;
pro_node->time_idx = timeToIndex(pro_node->time);
}
open_set_.push(pro_node);
if (dynamic)
expanded_nodes_.insert(pro_id, pro_node->time, pro_node);
else
expanded_nodes_.insert(pro_id, pro_node);
tmp_expand_nodes.push_back(pro_node);
use_node_num_ += 1;
if (use_node_num_ == allocate_num_)
{
cout << "run out of memory." << endl;
return NO_PATH;
}
}
else if (pro_node->node_state == IN_OPEN_SET)
{
if (tmp_g_score < pro_node->g_score)
{
// pro_node->index = pro_id;
pro_node->state = pro_state;
pro_node->f_score = tmp_f_score;
pro_node->g_score = tmp_g_score;
pro_node->input = um;
pro_node->duration = tau;
pro_node->parent = cur_node;
if (dynamic)
pro_node->time = cur_node->time + tau;
}
}
else
{
cout << "error type in searching: " << pro_node->node_state << endl;
}
}首先需要判断当前临时扩展节点与current node的其他临时扩展节点是否存在在同一voxel中。如果存在，则执行剪枝操作以优化搜索空间。接着需要判断当前临时扩展节点的fscore值与其所在voxel区域内的对比fscore值相比是否有优势。如果其fscore值小于对比值，则将该Voxel节点更新为当前临时扩展节点以提高搜索效率。
假设不进行剪枝操作，则首先要判断当前临时生成的pro_node是否已经存在于开放集合中。如果该pro_node尚未出现在开放集合中，则可以直接将其加入到开放集合中。反之，在开放集合中存在但尚未被扩展过的前提下，则需比较当前生成的临时扩展节点与其对应的VOXEL节点的f值；如果当前临时生成的pro_node具有较小的f值，则应更新相应VOXEL节点的信息。
在Fast planner的具体实现中，open集合采用两种数据结构来完成功能：一种是队列用于存储并弹出open集合中的节点；另一种是哈希表NodeHashtable用于快速查询节点是否已包含在开放集合中。对于判断一个节点是否属于close集合，则需要结合NodeHashtable与nodestate来进行评估：如果一个节点的状态为InCloseSet状态且存在于NodeHashtable中，则说明该节点已经被扩展过并包含在close集合中。
1.5 返回kinopath与 getsamples
getKinoTraj这一函数的主要功能是，在完成路径规划后基于设定的时间间隔delta_t，通过节点回溯过程和状态前向积分方法生成更高分辨率的路径点序列。如果最终生成的shot轨迹数据存在的话，则需补充最后一段时间内的shot轨迹数据（即通过调用computeshottraj函数计算得出）。
std::vector<Eigen::Vector3d> KinodynamicAstar::getKinoTraj(double delta_t)
{
vector<Vector3d> state_list;
/* ---------- get traj of searching ---------- */
PathNodePtr node = path_nodes_.back();
Matrix<double, 6, 1> x0, xt;
while (node->parent != NULL)
{
Vector3d ut = node->input;
double duration = node->duration;
x0 = node->parent->state;
for (double t = duration; t >= -1e-5; t -= delta_t)
{
stateTransit(x0, xt, ut, t);
state_list.push_back(xt.head(3));
}
node = node->parent;
}
reverse(state_list.begin(), state_list.end());
/* ---------- get traj of one shot ---------- */
if (is_shot_succ_)
{
Vector3d coord;
VectorXd poly1d, time(4);
for (double t = delta_t; t <= t_shot_; t += delta_t)
{
for (int j = 0; j < 4; j++)
time(j) = pow(t, j);
for (int dim = 0; dim < 3; dim++)
{
poly1d = coef_shot_.row(dim);
coord(dim) = poly1d.dot(time);
}
state_list.push_back(coord);
}
}
return state_list;
}该函数则用于以离散形式获取一些采样点序列以及起始点处的速度和加速度值。
void KinodynamicAstar::getSamples(double& ts, vector<Eigen::Vector3d>& point_set,
vector<Eigen::Vector3d>& start_end_derivatives)
{
/* ---------- path duration ---------- */
double T_sum = 0.0;
if (is_shot_succ_)
T_sum += t_shot_;
PathNodePtr node = path_nodes_.back();
while (node->parent != NULL)
{
T_sum += node->duration;
node = node->parent;
}
// cout << "duration:" << T_sum << endl;
// Calculate boundary vel and acc
Eigen::Vector3d end_vel, end_acc;
double t;
if (is_shot_succ_)
{
t = t_shot_;
end_vel = end_vel_;
for (int dim = 0; dim < 3; ++dim)
{
Vector4d coe = coef_shot_.row(dim);
end_acc(dim) = 2 * coe(2) + 6 * coe(3) * t_shot_;
}
}
else
{
t = path_nodes_.back()->duration;
end_vel = node->state.tail(3);
end_acc = path_nodes_.back()->input;
}
// Get point samples
int seg_num = floor(T_sum / ts);
seg_num = max(8, seg_num);
ts = T_sum / double(seg_num);
bool sample_shot_traj = is_shot_succ_;
node = path_nodes_.back();
for (double ti = T_sum; ti > -1e-5; ti -= ts)
{
if (sample_shot_traj)
{
// samples on shot traj
Vector3d coord;
Vector4d poly1d, time;
for (int j = 0; j < 4; j++)
time(j) = pow(t, j);
for (int dim = 0; dim < 3; dim++)
{
poly1d = coef_shot_.row(dim);
coord(dim) = poly1d.dot(time);
}
point_set.push_back(coord);
t -= ts;
/* end of segment */
if (t < -1e-5)
{
sample_shot_traj = false;
if (node->parent != NULL)
t += node->duration;
}
}
else
{
// samples on searched traj
Eigen::Matrix<double, 6, 1> x0 = node->parent->state;
Eigen::Matrix<double, 6, 1> xt;
Vector3d ut = node->input;
stateTransit(x0, xt, ut, t);
point_set.push_back(xt.head(3));
t -= ts;
// cout << "t: " << t << ", t acc: " << T_accumulate << endl;
if (t < -1e-5 && node->parent->parent != NULL)
{
node = node->parent;
t += node->duration;
}
}
}
reverse(point_set.begin(), point_set.end());
// calculate start acc
Eigen::Vector3d start_acc;
if (path_nodes_.back()->parent == NULL)
{
// no searched traj, calculate by shot traj
start_acc = 2 * coef_shot_.col(2);
}
else
{
// input of searched traj
start_acc = node->input;
}
start_end_derivatives.push_back(start_vel_);
start_end_derivatives.push_back(end_vel);
start_end_derivatives.push_back(start_acc);
start_end_derivatives.push_back(end_acc);
}至此为止\text{Kinodynamic\_astar}的所有关键函数已完成分析。基于搜索过程即可获取一条具备一定动力学约束的安全避障路径。
二、B样条曲线设置
首先重温一下B样条曲线的几个基本性质
一个具有N+1个控制点的次数为P_b的B样条曲线，则该曲线共有P_b+N+1个时间节点（即时间节点序列t_0,t_1,\dots,t_M）。同样地，在设计一条次数为P_b且包含M个时间节点的B样条曲线时，则相应的控制点数目应确定为：M-P_b+1。
每个控制点P_i的时间影响区间是t_i到t_{i+p_b}之间。
该B样条的主要区间段t_i到t_{i+1}主要由位于第$i-p_b到第i-2位置上的p_b+2个控制点所决定。
2.1 均匀B样条设置
获得初始路径后，在此基础上进行B样条优化。均匀B样条用于对轨迹的平滑性、安全性以及速度和加速度进行优化。在non_uniform_bspline.cpp文件中，默认情况下定义了一个简单的三次贝塞尔曲线拟合方法；然而，在非均匀贝塞尔曲线实现时，则需要自定义拟合参数以满足特定需求。为了实现非均匀贝塞尔曲线拟合功能，请调用setNonUniformBspline()方法；该方法允许用户自定义拟合参数并指定拟合精度；
void NonUniformBspline::setUniformBspline(const Eigen::MatrixXd& points, const int& order,
const double& interval) {
control_points_ = points;
p_ = order;
interval_ = interval;
n_ = points.rows() - 1;
m_ = n_ + p_ + 1;
u_ = Eigen::VectorXd::Zero(m_ + 1);
for (int i = 0; i <= m_; ++i) {
if (i <= p_) {
u_(i) = double(-p_ + i) * interval_;
} else if (i > p_ && i <= m_ - p_) {
u_(i) = u_(i - 1) + interval_;
} else if (i > m_ - p_) {
u_(i) = u_(i - 1) + interval_;
}
}
}2.2 B样条函数值计算
给定一个时间\mu，如何确定该点的坐标值？常规方法是应用Cox-DeBoor公式来计算整个B样条函数。然而，在evaluateDeBoor()这一函数中，则采用了递归策略来实现这一过程。如图所示，请参考wikipedia页面：https://en.wikipedia.org/wiki/De_Boor's_algorithm
其中k表示x所处的时间区间，并满足x \in [t_k, t_{k+1}]
假设在此轨迹相关区域中初始设置有P+1个0阶次的'控制点'；经由双重循环计算得到第K个P阶次的'控制点'；此值即为此B样条基函数所对应的函数值。\ 内部循环采用递推公式计算出更高阶次的一组共P+1个新'控制点'；外部循环则逐步提升整个计算模型的空间分辨率。
Eigen::VectorXd NonUniformBspline::evaluateDeBoor(const double& u) {
double ub = min(max(u_(p_), u), u_(m_ - p_));
// determine which [ui,ui+1] lay in
int k = p_;
while (true) {
if (u_(k + 1) >= ub) break;
++k;
}
/* deBoor's alg */
vector<Eigen::VectorXd> d;
for (int i = 0; i <= p_; ++i) {
d.push_back(control_points_.row(k - p_ + i));
// cout << d[i].transpose() << endl;
}
for (int r = 1; r <= p_; ++r) {
for (int i = p_; i >= r; --i) {
double alpha = (ub - u_[i + k - p_]) / (u_[i + 1 + k - r] - u_[i + k - p_]);
// cout << "alpha: " << alpha << endl;
d[i] = (1 - alpha) * d[i - 1] + alpha * d[i];
}
}
return d[p_];
}同样的情况下,该函数仅仅得到区间[t_p,t_{m-p}]内的B样条函数值。
2.3 控制点获得-前端路径拟合
在前面两个小节中，我们已经能够在已知关键点的情况下求取任意时间点对应的轨迹值。那么如何获取这些关键点呢？在Fast-Planner算法中，算法通过将前端hybrid A*寻找到的初始路径进行拟合处理来确定这些关键点。
void NonUniformBspline::parameterizeToBspline(const double& ts, const vector<Eigen::Vector3d>& point_set,
const vector<Eigen::Vector3d>& start_end_derivative,
Eigen::MatrixXd& ctrl_pts) {
if (ts <= 0) {
cout << "[B-spline]:time step error." << endl;
return;
}
if (point_set.size() < 2) {
cout << "[B-spline]:point set have only " << point_set.size() << " points." << endl;
return;
}
if (start_end_derivative.size() != 4) {
cout << "[B-spline]:derivatives error." << endl;
}
int K = point_set.size();
// write A
Eigen::Vector3d prow(3), vrow(3), arow(3);
prow << 1, 4, 1;
vrow << -1, 0, 1;
arow << 1, -2, 1;
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2);
for (int i = 0; i < K; ++i) A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();
A.block(K, 0, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 2, 0, 1, 3) = (1 / ts / ts) * arow.transpose();
A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();
// cout << "A:\n" << A << endl;
// A.block(0, 0, K, K + 2) = (1 / 6.0) * A.block(0, 0, K, K + 2);
// A.block(K, 0, 2, K + 2) = (1 / 2.0 / ts) * A.block(K, 0, 2, K + 2);
// A.row(K + 4) = (1 / ts / ts) * A.row(K + 4);
// A.row(K + 5) = (1 / ts / ts) * A.row(K + 5);
// write b
Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4);
for (int i = 0; i < K; ++i) {
bx(i) = point_set[i](0);
by(i) = point_set[i](1);
bz(i) = point_set[i](2);
}
for (int i = 0; i < 4; ++i) {
bx(K + i) = start_end_derivative[i](0);
by(K + i) = start_end_derivative[i](1);
bz(K + i) = start_end_derivative[i](2);
}
// solve Ax = b
Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);
// convert to control pts
ctrl_pts.resize(K + 2, 3);
ctrl_pts.col(0) = px;
ctrl_pts.col(1) = py;
ctrl_pts.col(2) = pz;
// cout << "[B-spline]: parameterization ok." << endl;
}尽管该研究论文在计算单个点位于B-样条曲线上的值时采用了德博尔（DeBoor）算法，在基于均匀B-样条模型实现前端轨迹拟合的过程中，则采用了基于矩阵运算的方法来描述该曲线族的具体形状特征。参考文献[K. Qin]详细阐述了这一技术方案的基本原理与实现细节。
首先假设离散轨迹点总数为K个，则形成连续的轨迹段数为K−1段。基于B样条的基本性质可知，在这种情况下这些曲线的定义域区间为[u_3, u_{4+k−2}]。因此总knot的数量为：
M = (k - 1) + 6 = K + 5
其中总控制点数量等于knot数量减去三次方程系数的影响因素，
即：
M - 6 = K - 1
因此最终确定了总共需要设置(K + 2)个控制点用于构建三次B样条基函数。
如以下公式所示，对于B样条曲线上定义在t_m,t_{m+1}上的一小段曲线，其被[p_{m-p_b},p_m]这四个控制点所决定。其中s(t)=\frac{t-t_m}{\Delta t}。 M_{p_b}是四维常数矩阵，具体参见以上论文链接。
\begin{aligned} &\mathbf{p}(s(t))=\mathbf{s}(t)^{\top} \mathbf{M}_{p_{b}+1} \mathbf{q}_{m} \\ &\mathbf{s}(t)=\left[\begin{array}{lllll} 1 & s(t) & s^{2}(t) & \cdots & s^{p_{b}}(t) \end{array}\right]^{\top} & \\ &\mathbf{q}_{m}=\left[\begin{array}{lllll} \mathbf{Q}_{m-p_{b}} & \mathbf{Q}_{m-p_{b}} \mid+1 & \mathbf{Q}_{m-p_{b}+2} & \cdots & \mathbf{Q}_{m} \end{array}\right]^{\top} \end{aligned}
利用矩阵运算手段，从而获得第一个路径点对应的位置约束：
x_1 =\frac{1}{6}(Q_2-Q_0)
类推可得其余K-1个路径点的位置约束。
在处理速度和加速度约束时，在不影响系统性能的前提下（在不影响系统性能的前提下），这些关系可以通过代码实现（在不影响系统性能的前提下）。值得注意的是（值得注意的是），其中s(t)是一个关于时间t的函数，并包含一个常数项\frac{1}{\Delta t}；因此，在计算一阶导数时需乘以该常数项\frac{1}{\Delta t}；类似地，在计算二阶导数时则需乘以相应的系数\frac{1}{\Delta t^2}。
基于K+2个控制点建立K+4个等式约束（其中包含位置约束\mathbf{K}项、速度约束\mathbf{2}项以及加速度约束\mathbf{2}项），通过线性拟合法求解方程组即可确定相应的拟合控制点坐标
2.4 非均匀B样条一阶及二阶微分
在获得了具有相同时间节点的一阶均匀B样条曲线之后，在此基础上我们希望对该曲线的速度与加速度参数进行动态验证以便后续调整该曲线的时间节点设置。因此我们需要计算出其对应于非均匀参数下的速度与加速度控制顶点。其中直接采用该曲线原有顶点的原因在于一阶均匀B-样条可视为其特例
Eigen::MatrixXd NonUniformBspline::getDerivativeControlPoints() {
// The derivative of a b-spline is also a b-spline, its order become p_-1
// control point Qi = p_*(Pi+1-Pi)/(ui+p_+1-ui+1)
Eigen::MatrixXd ctp = Eigen::MatrixXd::Zero(control_points_.rows() - 1, control_points_.cols());
for (int i = 0; i < ctp.rows(); ++i) {
ctp.row(i) =
p_ * (control_points_.row(i + 1) - control_points_.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
}
return ctp;
}
NonUniformBspline NonUniformBspline::getDerivative() {
Eigen::MatrixXd ctp = getDerivativeControlPoints();
NonUniformBspline derivative(ctp, p_ - 1, interval_);
/* cut the first and last knot */
Eigen::VectorXd knot(u_.rows() - 2);
knot = u_.segment(1, u_.rows() - 2);
derivative.setKnot(knot);
return derivative;
}在代码中采用递归方法计算速度和加速度，在B样条中其一阶导数的次数降为当前多项式次数减一且具有控制点数量减一的B样条曲线因此其相应的Knot向量降为当前多项式次数减二通过上图中的公式计算出一级导数控制点后接着定义一个新的NonUniformBspline对象并将新的控制点序列指定的多项式次数以及调整后的Knot向量赋值给它
2.5 可达性检查
bool NonUniformBspline::checkFeasibility(bool show) {
bool fea = true;
// SETY << "[Bspline]: total points size: " << control_points_.rows() << endl;
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();
/* check vel feasibility and insert points */
double max_vel = -1.0;
for (int i = 0; i < P.rows() - 1; ++i) {
Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 ||
fabs(vel(2)) > limit_vel_ + 1e-4) {
if (show) cout << "[Check]: Infeasible vel " << i << " :" << vel.transpose() << endl;
fea = false;
for (int j = 0; j < dimension; ++j) {
max_vel = max(max_vel, fabs(vel(j)));
}
}
}
/* acc feasibility */
double max_acc = -1.0;
for (int i = 0; i < P.rows() - 2; ++i) {
Eigen::VectorXd acc = p_ * (p_ - 1) *
((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
(P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
(u_(i + p_ + 1) - u_(i + 2));
if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 ||
fabs(acc(2)) > limit_acc_ + 1e-4) {
if (show) cout << "[Check]: Infeasible acc " << i << " :" << acc.transpose() << endl;
fea = false;
for (int j = 0; j < dimension; ++j) {
max_acc = max(max_acc, fabs(acc(j)));
}
}
}
double ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));
return fea;
}
double NonUniformBspline::checkRatio() {
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();
// find max vel
double max_vel = -1.0;
for (int i = 0; i < P.rows() - 1; ++i) {
Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
for (int j = 0; j < dimension; ++j) {
max_vel = max(max_vel, fabs(vel(j)));
}
}
// find max acc
double max_acc = -1.0;
for (int i = 0; i < P.rows() - 2; ++i) {
Eigen::VectorXd acc = p_ * (p_ - 1) *
((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
(P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
(u_(i + p_ + 1) - u_(i + 2));
for (int j = 0; j < dimension; ++j) {
max_acc = max(max_acc, fabs(acc(j)));
}
}
double ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));
ROS_ERROR_COND(ratio > 2.0, "max vel: %lf, max acc: %lf.", max_vel, max_acc);
return ratio;
}
bool NonUniformBspline::reallocateTime(bool show) {
// SETY << "[Bspline]: total points size: " << control_points_.rows() << endl;
// cout << "origin knots:\n" << u_.transpose() << endl;
bool fea = true;
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();
double max_vel, max_acc;
/* check vel feasibility and insert points */
for (int i = 0; i < P.rows() - 1; ++i) {
Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 ||
fabs(vel(2)) > limit_vel_ + 1e-4) {
fea = false;
if (show) cout << "[Realloc]: Infeasible vel " << i << " :" << vel.transpose() << endl;
max_vel = -1.0;
for (int j = 0; j < dimension; ++j) {
max_vel = max(max_vel, fabs(vel(j)));
}
double ratio = max_vel / limit_vel_ + 1e-4;
if (ratio > limit_ratio_) ratio = limit_ratio_;
double time_ori = u_(i + p_ + 1) - u_(i + 1);
double time_new = ratio * time_ori;
double delta_t = time_new - time_ori;
double t_inc = delta_t / double(p_);
for (int j = i + 2; j <= i + p_ + 1; ++j) {
u_(j) += double(j - i - 1) * t_inc;
if (j <= 5 && j >= 1) {
// cout << "vel j: " << j << endl;
}
}
for (int j = i + p_ + 2; j < u_.rows(); ++j) {
u_(j) += delta_t;
}
}
}
/* acc feasibility */
for (int i = 0; i < P.rows() - 2; ++i) {
Eigen::VectorXd acc = p_ * (p_ - 1) *
((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
(P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
(u_(i + p_ + 1) - u_(i + 2));
if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 ||
fabs(acc(2)) > limit_acc_ + 1e-4) {
fea = false;
if (show) cout << "[Realloc]: Infeasible acc " << i << " :" << acc.transpose() << endl;
max_acc = -1.0;
for (int j = 0; j < dimension; ++j) {
max_acc = max(max_acc, fabs(acc(j)));
}
double ratio = sqrt(max_acc / limit_acc_) + 1e-4;
if (ratio > limit_ratio_) ratio = limit_ratio_;
// cout << "ratio: " << ratio << endl;
double time_ori = u_(i + p_ + 1) - u_(i + 2);
double time_new = ratio * time_ori;
double delta_t = time_new - time_ori;
double t_inc = delta_t / double(p_ - 1);
if (i == 1 || i == 2) {
// cout << "acc i: " << i << endl;
for (int j = 2; j <= 5; ++j) {
u_(j) += double(j - 1) * t_inc;
}
for (int j = 6; j < u_.rows(); ++j) {
u_(j) += 4.0 * t_inc;
}
} else {
for (int j = i + 3; j <= i + p_ + 1; ++j) {
u_(j) += double(j - i - 2) * t_inc;
if (j <= 5 && j >= 1) {
// cout << "acc j: " << j << endl;
}
}
for (int j = i + p_ + 2; j < u_.rows(); ++j) {
u_(j) += delta_t;
}
}
}
}
return fea;
}
void NonUniformBspline::lengthenTime(const double& ratio) {
int num1 = 5;
int num2 = getKnot().rows() - 1 - 5;
double delta_t = (ratio - 1.0) * (u_(num2) - u_(num1));
double t_inc = delta_t / double(num2 - num1);
for (int i = num1 + 1; i <= num2; ++i) u_(i) += double(i - num1) * t_inc;
for (int i = num2 + 1; i < u_.rows(); ++i) u_(i) += delta_t;
}这部分的三个函数checkFeasibility()、checkRatio()和reallocationTime()均遵循相同的逻辑。它们均基于以下两个公式来判断各控制点的速度和加速度是否超出限制，并计算出相应的调整幅度。
该函数reallocateTime用于根据当前控制点是否超限以及调整表比例来实现时间重新分配。对于涉及当前控制点i的时间区间进行时间调整，则需关注区间[t_i,t_{i+p_b +1}]。需要注意的是，在论文中讨论的3次B样条曲线中，这里的p_b即为当前B样条的次数：如果是速度，则为3-1=2；如果是加速度，则为3-2=1。后续的时间节点则直接添加总扩张时间即可。
值得我们注意的是，在代码实现中存在一个尚待理清的关键环节：即加速度超限时间调整过程中的i=1阶段及i=2阶段处理方式
if (i == 1 || i == 2) {
// cout << "acc i: " << i << endl;
for (int j = 2; j <= 5; ++j) {
u_(j) += double(j - 1) * t_inc;
}
for (int j = 6; j < u_.rows(); ++j) {
u_(j) += 4.0 * t_inc;
}2.6 小结
到此为止，在前端动力学搜索的基础上我们已经确定了一条初步路径 并且成功地运用了均匀B样条模型对其进行精确拟合 得到了初始控制点序列 在此基础上 我们深入研究了非均匀B样条的特点 并掌握了如何利用这一特性来评估原始匀 匀 B 样 条 曲 线 的 速 度 和 加 速 度 参数 在此过程中 我们不断对迭代时间进行了相应的 调整 然而 目前我们的工作仍存在一些待完善之处 具体来说 则是如何通过运用 均匀 B 样 条 技术实现轨迹平滑性 安全性以及运行速度与加速度的有效优化
三、B样条优化
3.1 优化项计算
基于均匀B样条方法进行优化时,首要关注点是三个优化维度及其对应的梯度描述.这三项包括jerk平滑性相关的指标,障碍物距离相关指标以及与速度和加速度限制相关的参数.
其中，在处理平滑性问题时，“平顺项”指的是速度和加速度相关的部分。这些参数（速度和加速度）都是通过均匀B样条的微分方程来确定的。通过计算每个参数对应的控制点位置来构建相应的优化目标。梯度则由链式法则计算得出，在每次迭代时会累加到各个相关参数上。需要注意的是，在这个过程中每个参数的变化都会影响多个优化目标。
在处理运动规划问题时需要特别注意的是，在优化速度和加速度的过程中以及计算梯度时都考虑了时间步长\Delta t。然而，在处理jerk相关的部分时省略了时间步长\Delta t-这可能与文中所述的后续纯几何构造法有关。这使得后续的时间调整更为便捷。相比之下，在实现平滑性方面所采用的方式与原文中的弹性带方法有所不同。
针对障碍物项的具体处理方法，在本研究中采用了基于SDF（偏移函数）的地图模型来计算控制点到最近障碍物的距离，并对其梯度进行求解。具体实施细节将在后续章节中进行详细阐述。
void BsplineOptimizer::calcSmoothnessCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
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;
}
}
void BsplineOptimizer::calcDistanceCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
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);
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;
}
}
}
void BsplineOptimizer::calcFeasibilityCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
cost = 0.0;
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
/* abbreviation */
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);
}
}
}
}3.2 优化项结合
由于KinoReplan仅关注Smoothness、distance以及可行性这三个优化因子，在处理其他优化项时暂不展开讨论。接下来探讨如何将这些关键因子进行整合，并将当前的自变量设置（即控制点）转化为Nlopt优化中的变量x。
void BsplineOptimizer::combineCost(const std::vector<double>& x, std::vector<double>& grad,
double& f_combine) {
/* convert the NLopt format vector to control points. */
// This solver can support 1D-3D B-spline optimization, but we use Vector3d to store each control point
// For 1D case, the second and third elements are zero, and similar for the 2D case.
for (int i = 0; i < order_; i++) {
for (int j = 0; j < dim_; ++j) {
g_q_[i][j] = control_points_(i, j);
}
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);
/* evaluate costs and their gradient */
double f_smoothness, f_distance, f_feasibility, f_endpoint, f_guide, f_waypoints;
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_);
f_combine += lambda1_ * f_smoothness;
for (int i = 0; i < variable_num_ / dim_; i++)
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda1_ * g_smoothness_[i + order_](j);
}
if (cost_function_ & DISTANCE) {
calcDistanceCost(g_q_, f_distance, g_distance_);
f_combine += lambda2_ * f_distance;
for (int i = 0; i < variable_num_ / dim_; i++)
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_);
f_combine += lambda3_ * f_feasibility;
for (int i = 0; i < variable_num_ / dim_; i++)
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_);
f_combine += lambda4_ * f_endpoint;
for (int i = 0; i < variable_num_ / dim_; i++)
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_);
f_combine += lambda5_ * f_guide;
for (int i = 0; i < variable_num_ / dim_; i++)
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_);
f_combine += lambda7_ * f_waypoints;
for (int i = 0; i < variable_num_ / dim_; i++)
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda7_ * g_waypoints_[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
// }
}
double BsplineOptimizer::costFunction(const std::vector<double>& x, std::vector<double>& grad,
void* func_data) {
BsplineOptimizer* opt = reinterpret_cast<BsplineOptimizer*>(func_data);
double cost;
opt->combineCost(x, grad, cost);
opt->iter_num_++;
/* save the min cost result */
if (cost < opt->min_cost_) {
opt->min_cost_ = cost;
opt->best_variable_ = x;
}
return cost;
// /* 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());
// }
}该函数用于整合三种优化项。
其中第一个参数std::vector& x表示Nlopt优化的变量，
应与三个维度的控制点一一对应。
第二个参数是std::vector& grad表示每个变量
的相关梯度信息，
第三个参数为double& f_combine即综合后的成本值。
首先给g_q赋值，并将其定义为用于计算每次优化循环中三个优化项的控制点参数。需要注意的是，在这些控制点序列中，默认情况下前p_b个以及最后p_b个位置上的控制点不会进行任何优化处理，它们始终维持线性拟合所得出的原始控制点数值不变。而位于这两个端部之外的中间位置上的控制点，则由于在每次迭代过程中也会发生变化，在这里我们统一用x来进行赋值操作。其中x是通过将Nlopt优化器对象设置为其最小目标函数的过程来初始化并确定其大小参数的具体数值的；而这个初始值则是在随后调用Nlopt求解器中的.optimize方法时自动进行赋值设定的。
接下来将运用CalcSmoothneesCost、CalcDistanceCost以及CalcFeasibilityCost这三个计算模块来分别运算每一部分的cost和grad，并通过乘以对应的权重系数将结果累加到grad和f_combine中。需要注意的是，在维度匹配方面存在一定的差异：具体来说，在B样条次数上相差了两次。
最后一步是搭建满足Nlopt要求的目标函数模块，并使用combinecost计算总成本的同时支持传递梯度信息。
3.3 Nlopt优化
至此完成之后的状态是完成了优化变量的搭建、目标函数的构建以及梯度信息的配置工作，在这个阶段之后我们就可以顺利地进入下一步骤即利用Nlopt软件对控制点实施非线性优化过程
Eigen::MatrixXd BsplineOptimizer::BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,
const int& cost_function, int max_num_id,
int max_time_id) {
setControlPoints(points);
setBsplineInterval(ts);
setCostFunction(cost_function);
setTerminateCond(max_num_id, max_time_id);
optimize();
return this->control_points_;
}
void BsplineOptimizer::optimize() {
/* 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);
if (cost_function_ & ENDPOINT) {
variable_num_ = dim_ * (pt_num - order_);
// end position used for hard constraint
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_)) ;
}
/* do optimization using NLopt slover */
nlopt::opt opt(nlopt::algorithm(isQuadratic() ? algorithm1_ : algorithm2_), variable_num_);
opt.set_min_objective(BsplineOptimizer::costFunction, this);
opt.set_maxeval(max_iteration_num_[max_num_id_]);
opt.set_maxtime(max_iteration_time_[max_time_id_]);
opt.set_xtol_rel(1e-5);
vector<double> q(variable_num_);
for (int i = order_; i < pt_num; ++i) {
if (!(cost_function_ & ENDPOINT) && i >= pt_num - order_) continue;
for (int j = 0; j < dim_; j++) {
q[dim_ * (i - order_) + j] = control_points_(i, j);
}
}
if (dim_ != 1) {
vector<double> lb(variable_num_), ub(variable_num_);
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);
}
try {
// cout << fixed << setprecision(7);
// vec_time_.clear();
// vec_cost_.clear();
// time_start_ = ros::Time::now();
double final_cost;
nlopt::result result = opt.optimize(q, final_cost);
/* retrieve the optimization result */
// cout << "Min cost:" << min_cost_ << endl;
} catch (std::exception& e) {
ROS_WARN("[Optimization]: nlopt exception");
cout << e.what() << endl;
}
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_);
}首先是对BsplineOptimizeTraj()函数进行了设置，在其中设置了两个主要方面的内容：一是确定了需要被优化的控制点位置；二是明确了均匀分布的B样条节点间距；三是综合考虑了成本函数中所包含的各项关键指标；四是在算法运行过程中设置了合理的终止条件组合（包括最大迭代次数和最长运行时间）。之后在完成了所有必要的参数配置后，并调用了BslineOptimizer类中的optimize方法来进行参数求解工作
在BsplineOptimizer的optimize()函数中，在计算各部分优化项时会依据pt_num来设定梯度向量的相关参数；同时，在是否存在终止点约束的情况下，则会决定优化变量的具体数量。
随后将对Nlopt::opt类的对象 opt 进行实例化，并设置目标函数。指定最大迭代次数以及最长运行时间，并确定目标函数的最小取值（这些参数共同构成了算法终止的标准）。随后基于线性搜索的结果确定各变量初始值，并规定各变量的变化范围（初始值加减10%）。
最后采用Nlopt::opt类中的该优化函数进行迭代优化计算，在优化完成后通过该costFunction保存的最佳变量更新控制点坐标
3.4 小结
至此为止,我们已经掌握如何由前端程序识别通向目标的一条路径,进而利用均匀B样条曲线精确拟合轨迹并提取关键点,随后通过Nlopt算法优化控制点位置,最后运用非均匀B样条曲线调整优化后的运动轨迹的时间参数。整篇文章的主要算法已经彻底分析完成。接下来,我们将围绕规划流程从头至尾展开详细梳理整个规划过程。
四. 规划系统运行逻辑
4.1 plan_mannager
首先聚焦于包 plan_manage 中的两个关键头文件：plan_manager.h 和 planner_manager.cpp。这两个核心头文件主要负责实现一种高效的 FastPlannerManager 类，在规划流程中扮演核心角色。
#ifndef _PLANNER_MANAGER_H_
#define _PLANNER_MANAGER_H_
#include <bspline_opt/bspline_optimizer.h>
#include <bspline/non_uniform_bspline.h>
#include <path_searching/astar.h>
#include <path_searching/kinodynamic_astar.h>
#include <path_searching/topo_prm.h>
#include <plan_env/edt_environment.h>
#include <plan_manage/plan_container.hpp>
#include <ros/ros.h>
namespace fast_planner {
// Fast Planner Manager
// Key algorithms of mapping and planning are called
class FastPlannerManager {
// SECTION stable
public:
FastPlannerManager();
~FastPlannerManager();
/* main planning interface */
bool kinodynamicReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc,
Eigen::Vector3d end_pt, Eigen::Vector3d end_vel);
bool planGlobalTraj(const Eigen::Vector3d& start_pos);
bool topoReplan(bool collide);
void planYaw(const Eigen::Vector3d& start_yaw);
void initPlanModules(ros::NodeHandle& nh);
void setGlobalWaypoints(vector<Eigen::Vector3d>& waypoints);
bool checkTrajCollision(double& distance);
PlanParameters pp_;
LocalTrajData local_data_;
GlobalTrajData global_data_;
MidPlanData plan_data_;
EDTEnvironment::Ptr edt_environment_;
private:
/* main planning algorithms & modules */
SDFMap::Ptr sdf_map_;
unique_ptr<Astar> geo_path_finder_;
unique_ptr<KinodynamicAstar> kino_path_finder_;
unique_ptr<TopologyPRM> topo_prm_;
vector<BsplineOptimizer::Ptr> bspline_optimizers_;
void updateTrajInfo();
// topology guided optimization
void findCollisionRange(vector<Eigen::Vector3d>& colli_start, vector<Eigen::Vector3d>& colli_end,
vector<Eigen::Vector3d>& start_pts, vector<Eigen::Vector3d>& end_pts);
void optimizeTopoBspline(double start_t, double duration, vector<Eigen::Vector3d> guide_path,
int traj_id);
Eigen::MatrixXd reparamLocalTraj(double start_t, double& dt, double& duration);
Eigen::MatrixXd reparamLocalTraj(double start_t, double duration, int seg_num, double& dt);
void selectBestTraj(NonUniformBspline& traj);
void refineTraj(NonUniformBspline& best_traj, double& time_inc);
void reparamBspline(NonUniformBspline& bspline, double ratio, Eigen::MatrixXd& ctrl_pts, double& dt,
double& time_inc);
// heading planning
void calcNextYaw(const double& last_yaw, double& yaw);
// !SECTION stable
// SECTION developing
public:
typedef unique_ptr<FastPlannerManager> Ptr;
// !SECTION
};
} // namespace fast_planner
#endifFastPlannerManager主要由一系列与plan相关的接口函数组成。此外，还包括了来自plan_container.hpp中定义的数据结构和相关类。接着包括了SDF地图的智能指针、前端路径寻找类以及后端B样条优化相关的智能指针。针对 topology-guided optimization的相关内容暂不做讨论。最后，在整个系统中使用了一个FastPlannerManager类的智能指针 Ptr。
// #include <fstream>
#include <plan_manage/planner_manager.h>
#include <thread>
namespace fast_planner {
// SECTION interfaces for setup and query
FastPlannerManager::FastPlannerManager() {}
FastPlannerManager::~FastPlannerManager() { std::cout << "des manager" << std::endl; }
void FastPlannerManager::initPlanModules(ros::NodeHandle& nh) {
/* read algorithm parameters */
nh.param("manager/max_vel", pp_.max_vel_, -1.0);
nh.param("manager/max_acc", pp_.max_acc_, -1.0);
nh.param("manager/max_jerk", pp_.max_jerk_, -1.0);
nh.param("manager/dynamic_environment", pp_.dynamic_, -1);
nh.param("manager/clearance_threshold", pp_.clearance_, -1.0);
nh.param("manager/local_segment_length", pp_.local_traj_len_, -1.0);
nh.param("manager/control_points_distance", pp_.ctrl_pt_dist, -1.0);
bool use_geometric_path, use_kinodynamic_path, use_topo_path, use_optimization, use_active_perception;
nh.param("manager/use_geometric_path", use_geometric_path, false);
nh.param("manager/use_kinodynamic_path", use_kinodynamic_path, false);
nh.param("manager/use_topo_path", use_topo_path, false);
nh.param("manager/use_optimization", use_optimization, false);
local_data_.traj_id_ = 0;
sdf_map_.reset(new SDFMap);
sdf_map_->initMap(nh);
edt_environment_.reset(new EDTEnvironment);
edt_environment_->setMap(sdf_map_);
if (use_geometric_path) {
geo_path_finder_.reset(new Astar);
geo_path_finder_->setParam(nh);
geo_path_finder_->setEnvironment(edt_environment_);
geo_path_finder_->init();
}
if (use_kinodynamic_path) {
kino_path_finder_.reset(new KinodynamicAstar);
kino_path_finder_->setParam(nh);
kino_path_finder_->setEnvironment(edt_environment_);
kino_path_finder_->init();
}
if (use_optimization) {
bspline_optimizers_.resize(10);
for (int i = 0; i < 10; ++i) {
bspline_optimizers_[i].reset(new BsplineOptimizer);
bspline_optimizers_[i]->setParam(nh);
bspline_optimizers_[i]->setEnvironment(edt_environment_);
}
}
if (use_topo_path) {
topo_prm_.reset(new TopologyPRM);
topo_prm_->setEnvironment(edt_environment_);
topo_prm_->init(nh);
}
}
void FastPlannerManager::setGlobalWaypoints(vector<Eigen::Vector3d>& waypoints) {
plan_data_.global_waypoints_ = waypoints;
}
bool FastPlannerManager::checkTrajCollision(double& distance) {
double t_now = (ros::Time::now() - local_data_.start_time_).toSec();
double tm, tmp;
local_data_.position_traj_.getTimeSpan(tm, tmp);
Eigen::Vector3d cur_pt = local_data_.position_traj_.evaluateDeBoor(tm + t_now);
double radius = 0.0;
Eigen::Vector3d fut_pt;
double fut_t = 0.02;
while (radius < 6.0 && t_now + fut_t < local_data_.duration_) {
fut_pt = local_data_.position_traj_.evaluateDeBoor(tm + t_now + fut_t);
double dist = edt_environment_->evaluateCoarseEDT(fut_pt, -1.0);
if (dist < 0.1) {
distance = radius;
return false;
}
radius = (fut_pt - cur_pt).norm();
fut_t += 0.02;
}
return true;
}
// !SECTION
// SECTION kinodynamic replanning
bool FastPlannerManager::kinodynamicReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,
Eigen::Vector3d start_acc, Eigen::Vector3d end_pt,
Eigen::Vector3d end_vel) {
std::cout << "[kino replan]: -----------------------" << std::endl;
cout << "start: " << start_pt.transpose() << ", " << start_vel.transpose() << ", "
<< start_acc.transpose() << "\ngoal:" << end_pt.transpose() << ", " << end_vel.transpose()
<< endl;
if ((start_pt - end_pt).norm() < 0.2) {
cout << "Close goal" << endl;
return false;
}
ros::Time t1, t2;
local_data_.start_time_ = ros::Time::now();
double t_search = 0.0, t_opt = 0.0, t_adjust = 0.0;
Eigen::Vector3d init_pos = start_pt;
Eigen::Vector3d init_vel = start_vel;
Eigen::Vector3d init_acc = start_acc;
// kinodynamic path searching
t1 = ros::Time::now();
kino_path_finder_->reset();
int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, true);
if (status == KinodynamicAstar::NO_PATH) {
cout << "[kino replan]: kinodynamic search fail!" << endl;
// retry searching with discontinuous initial state
kino_path_finder_->reset();
status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, false);
if (status == KinodynamicAstar::NO_PATH) {
cout << "[kino replan]: Can't find path." << endl;
return false;
} else {
cout << "[kino replan]: retry search success." << endl;
}
} else {
cout << "[kino replan]: kinodynamic search success." << endl;
}
plan_data_.kino_path_ = kino_path_finder_->getKinoTraj(0.01);
t_search = (ros::Time::now() - t1).toSec();
// parameterize the path to bspline
double ts = pp_.ctrl_pt_dist / pp_.max_vel_;
vector<Eigen::Vector3d> point_set, start_end_derivatives;
kino_path_finder_->getSamples(ts, point_set, start_end_derivatives);
Eigen::MatrixXd ctrl_pts;
NonUniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts);
NonUniformBspline init(ctrl_pts, 3, ts);
// bspline trajectory optimization
t1 = ros::Time::now();
int cost_function = BsplineOptimizer::NORMAL_PHASE;
if (status != KinodynamicAstar::REACH_END) {
cost_function |= BsplineOptimizer::ENDPOINT;
}
ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, ts, cost_function, 1, 1);
t_opt = (ros::Time::now() - t1).toSec();
// iterative time adjustment
t1 = ros::Time::now();
NonUniformBspline pos = NonUniformBspline(ctrl_pts, 3, ts);
double to = pos.getTimeSum();
pos.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_);
bool feasible = pos.checkFeasibility(false);
int iter_num = 0;
while (!feasible && ros::ok()) {
feasible = pos.reallocateTime();
if (++iter_num >= 3) break;
}
// pos.checkFeasibility(true);
// cout << "[Main]: iter num: " << iter_num << endl;
double tn = pos.getTimeSum();
cout << "[kino replan]: Reallocate ratio: " << tn / to << endl;
if (tn / to > 3.0) ROS_ERROR("reallocate error.");
t_adjust = (ros::Time::now() - t1).toSec();
// save planned results
local_data_.position_traj_ = pos;
double t_total = t_search + t_opt + t_adjust;
cout << "[kino replan]: time: " << t_total << ", search: " << t_search << ", optimize: " << t_opt
<< ", adjust time:" << t_adjust << endl;
pp_.time_search_ = t_search;
pp_.time_optimize_ = t_opt;
pp_.time_adjust_ = t_adjust;
updateTrajInfo();
return true;
}该函数的主要任务是对kino_path_finder和Bspline_optimizers进行初始化配置，并且除了配置ROS nh节点间的通信参数外
该FastPlannerManager::checkTrajCollision函数利用Local_data中属于非均匀B样条类的位置_traj属性中的evaluateDeBoor方法来计算局部轨迹上各个离散点处的值。同时,它调用SDF类中的评估粗等距函数来计算各位置到障碍物的距离。当发现任何一个位置与障碍物的距离小于设定阈值时,则返回当前局部轨迹从起始点到碰撞发生位置的最大安全距离(即最远可执行的安全距离)。
最核心的FastPlannerManager::kinodynamicReplan()函数一旦设置好了起止点状态就可以启动后续的操作流程。该功能将通过kino_path_finder的搜索功能来进行路径探索。值得注意的是该系统在初始阶段会采用start_acc作为初始输入值来进行查找如果此时未找到对应的结果系统将转而采用离散化输入的方式再次搜索若依然无法找到结果则整个路径探索将无法成功完成
通过系统性地探索与分析，在二维设计软件中成功寻找到一条优化路径后，在曲线拟合模块中借助NonUniformBspline::parameterizeToBspline()函数对该路径进行均匀B样条曲线拟合，并最终获得相应的控制顶点
随后将执行均匀B样条优化过程。需要注意的是，在当前轨迹仅到达感知域之外而未抵达目标点的情况下，默认的目标函数应添加ENDPOINT优化项。此时的最优化变量应包括最终确定的p_b个控制点。然而，在路径寻觅阶段已处于REACH_END状态时，则无需考虑EDNPOINT因素了。因为经过拟合后的前p_b个控制点足以满足位置约束条件了
通过非均匀B样条类实现轨迹的时间校准，并将校准后的轨迹赋值至local_data.position_traj字段中；同时调用updateTrajInfo函数更新local_data中的相关数据
4.2 kino_replan_fsm.cpp
最后一个极为重要的C++源文件是kino_replan_fsm.cpp。其中定义了一个名为KinoReplanFSM类的对象这一类包含了多种参数、标志位以及规划数据，并集成了一套基于ROS的操作系统组件。最核心的部分是FastPlannerManager::Ptr对象 plan_manager及其相关的回调函数和初始化操作。
#ifndef _KINO_REPLAN_FSM_H_
#define _KINO_REPLAN_FSM_H_
#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <vector>
#include <visualization_msgs/Marker.h>
#include <bspline_opt/bspline_optimizer.h>
#include <path_searching/kinodynamic_astar.h>
#include <plan_env/edt_environment.h>
#include <plan_env/obj_predictor.h>
#include <plan_env/sdf_map.h>
#include <plan_manage/Bspline.h>
#include <plan_manage/planner_manager.h>
#include <traj_utils/planning_visualization.h>
using std::vector;
namespace fast_planner {
class Test {
private:
/* data */
int test_;
std::vector<int> test_vec_;
ros::NodeHandle nh_;
public:
Test(const int& v) {
test_ = v;
}
Test(ros::NodeHandle& node) {
nh_ = node;
}
~Test() {
}
void print() {
std::cout << "test: " << test_ << std::endl;
}
};
class KinoReplanFSM {
private:
/* ---------- flag ---------- */
enum FSM_EXEC_STATE { INIT, WAIT_TARGET, GEN_NEW_TRAJ, REPLAN_TRAJ, EXEC_TRAJ, REPLAN_NEW };
enum TARGET_TYPE { MANUAL_TARGET = 1, PRESET_TARGET = 2, REFENCE_PATH = 3 };
/* planning utils */
FastPlannerManager::Ptr planner_manager_;
PlanningVisualization::Ptr visualization_;
/* parameters */
int target_type_; // 1 mannual select, 2 hard code
double no_replan_thresh_, replan_thresh_;
double waypoints_[50][3];
int waypoint_num_;
/* planning data */
bool trigger_, have_target_, have_odom_;
FSM_EXEC_STATE exec_state_;
Eigen::Vector3d odom_pos_, odom_vel_; // odometry state
Eigen::Quaterniond odom_orient_;
Eigen::Vector3d start_pt_, start_vel_, start_acc_, start_yaw_; // start state
Eigen::Vector3d end_pt_, end_vel_; // target state
int current_wp_;
/* ROS utils */
ros::NodeHandle node_;
ros::Timer exec_timer_, safety_timer_, vis_timer_, test_something_timer_;
ros::Subscriber waypoint_sub_, odom_sub_;
ros::Publisher replan_pub_, new_pub_, bspline_pub_;
/* helper functions */
bool callKinodynamicReplan(); // front-end and back-end method
bool callTopologicalTraj(int step); // topo path guided gradient-based
// optimization; 1: new, 2: replan
void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call);
void printFSMExecState();
/* ROS functions */
void execFSMCallback(const ros::TimerEvent& e);
void checkCollisionCallback(const ros::TimerEvent& e);
void waypointCallback(const nav_msgs::PathConstPtr& msg);
void odometryCallback(const nav_msgs::OdometryConstPtr& msg);
public:
KinoReplanFSM(/* args */) {
}
~KinoReplanFSM() {
}
void init(ros::NodeHandle& nh);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace fast_planner
#endif我们首先关注KinoReplanFSM::init()函数这一核心模块，在其执行过程中首先要完成的是配置一系列必要的参数设置。随后对该规划管理类中的智能指针对象planner_manager_进行初始化。接着我们定义了一系列用于实现功能所需的ROS组件包括一个定时器节点一个订阅节点以及一个发布节点等基础构建模块来支持后续的操作流程。具体来说在这些组件及其相关回调机制被成功调用的过程中实现了整个规划过程
void KinoReplanFSM::init(ros::NodeHandle& nh) {
current_wp_ = 0;
exec_state_ = FSM_EXEC_STATE::INIT;
have_target_ = false;
have_odom_ = false;
/* fsm param */
nh.param("fsm/flight_type", target_type_, -1);
nh.param("fsm/thresh_replan", replan_thresh_, -1.0);
nh.param("fsm/thresh_no_replan", no_replan_thresh_, -1.0);
nh.param("fsm/waypoint_num", waypoint_num_, -1);
for (int i = 0; i < waypoint_num_; i++) {
nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0);
nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0);
nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0);
}
/* initialize main modules */
planner_manager_.reset(new FastPlannerManager);
planner_manager_->initPlanModules(nh);
visualization_.reset(new PlanningVisualization(nh));
/* callback */
exec_timer_ = nh.createTimer(ros::Duration(0.01), &KinoReplanFSM::execFSMCallback, this);
safety_timer_ = nh.createTimer(ros::Duration(0.05), &KinoReplanFSM::checkCollisionCallback, this);
waypoint_sub_ =
nh.subscribe("/waypoint_generator/waypoints", 1, &KinoReplanFSM::waypointCallback, this);
odom_sub_ = nh.subscribe("/odom_world", 1, &KinoReplanFSM::odometryCallback, this);
replan_pub_ = nh.advertise<std_msgs::Empty>("/planning/replan", 10);
new_pub_ = nh.advertise<std_msgs::Empty>("/planning/new", 10);
bspline_pub_ = nh.advertise<plan_manage::Bspline>("/planning/bspline", 10);
}为了深入分析系统的定时任务管理机制，在本节中我们将详细探讨两个核心组件及其对应的回调机制。其中第一个主要组件是KinoReplanFSM::execFSMCallback()实例，在设计中特意将其配置为每隔0.01秒自动执行一次的动作。随后，在系统监控模块中会定期输出运行状态信息以供后续调试参考。接着，在动态响应阶段会根据当前系统的 exec_state_变量值切换相应的处理逻辑以实现更加灵活的任务调度策略。
如果state是INIT，则检测odometry和plan的相关trigger是否已被触发。其中have_odom和trigger这两个bool型变量分别由KinoReplanFSM::odometryCallback()与KinoReplanFSM::waypointCallback()这两个消息订阅引发的动作中被修改。一旦上述条件均满足后，则将current state更新为WAIT_TARGET并跳出当前循环。
如果当前状态为WAIT_TARGET，则检查是否存在target。如果存在，则将状态更新为GEN_NEW_TRAJ，并跳出当前循环。
void KinoReplanFSM::execFSMCallback(const ros::TimerEvent& e) {
static int fsm_num = 0;
fsm_num++;
if (fsm_num == 100) {
printFSMExecState();
if (!have_odom_) cout << "no odom." << endl;
if (!trigger_) cout << "wait for goal." << endl;
fsm_num = 0;
}
switch (exec_state_) {
case INIT: {
if (!have_odom_) {
return;
}
if (!trigger_) {
return;
}
changeFSMExecState(WAIT_TARGET, "FSM");
break;
}
case WAIT_TARGET: {
if (!have_target_)
return;
else {
changeFSMExecState(GEN_NEW_TRAJ, "FSM");
}
break;
}
case GEN_NEW_TRAJ: {
start_pt_ = odom_pos_;
start_vel_ = odom_vel_;
start_acc_.setZero();
Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
start_yaw_(0) = atan2(rot_x(1), rot_x(0));
start_yaw_(1) = start_yaw_(2) = 0.0;
bool success = callKinodynamicReplan();
if (success) {
changeFSMExecState(EXEC_TRAJ, "FSM");
} else {
// have_target_ = false;
// changeFSMExecState(WAIT_TARGET, "FSM");
changeFSMExecState(GEN_NEW_TRAJ, "FSM");
}
break;
}
case EXEC_TRAJ: {
/* determine if need to replan */
LocalTrajData* info = &planner_manager_->local_data_;
ros::Time time_now = ros::Time::now();
double t_cur = (time_now - info->start_time_).toSec();
t_cur = min(info->duration_, t_cur);
Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);
/* && (end_pt_ - pos).norm() < 0.5 */
if (t_cur > info->duration_ - 1e-2) {
have_target_ = false;
changeFSMExecState(WAIT_TARGET, "FSM");
return;
} else if ((end_pt_ - pos).norm() < no_replan_thresh_) {
// cout << "near end" << endl;
return;
} else if ((info->start_pos_ - pos).norm() < replan_thresh_) {
// cout << "near start" << endl;
return;
} else {
changeFSMExecState(REPLAN_TRAJ, "FSM");
}
break;
}
case REPLAN_TRAJ: {
LocalTrajData* info = &planner_manager_->local_data_;
ros::Time time_now = ros::Time::now();
double t_cur = (time_now - info->start_time_).toSec();
start_pt_ = info->position_traj_.evaluateDeBoorT(t_cur);
start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur);
start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur);
start_yaw_(0) = info->yaw_traj_.evaluateDeBoorT(t_cur)[0];
start_yaw_(1) = info->yawdot_traj_.evaluateDeBoorT(t_cur)[0];
start_yaw_(2) = info->yawdotdot_traj_.evaluateDeBoorT(t_cur)[0];
std_msgs::Empty replan_msg;
replan_pub_.publish(replan_msg);
bool success = callKinodynamicReplan();
if (success) {
changeFSMExecState(EXEC_TRAJ, "FSM");
} else {
changeFSMExecState(GEN_NEW_TRAJ, "FSM");
}
break;
}
}
}当执行状态标记为GEN_NEW_TRAJ时，则会触发callKinodynamicReplan()函数以执行路径规划。在成功的情况下，则会将执行状态更新至EXEC_TRAJ；反之，在失败的情况下，则会维持执行状态为GEN_NEW_TRAJ。让我们深入探讨callKinodynamicReplan()函数的行为机制。当路径规划任务取得成功时，则会通过Bspline_pub发布对应的B样条轨迹数据；若未能成功，则返回False值。
bool KinoReplanFSM::callKinodynamicReplan() {
bool plan_success =
planner_manager_->kinodynamicReplan(start_pt_, start_vel_, start_acc_, end_pt_, end_vel_);
if (plan_success) {
planner_manager_->planYaw(start_yaw_);
auto info = &planner_manager_->local_data_;
/* publish traj */
plan_manage::Bspline bspline;
bspline.order = 3;
bspline.start_time = info->start_time_;
bspline.traj_id = info->traj_id_;
Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
for (int i = 0; i < pos_pts.rows(); ++i) {
geometry_msgs::Point pt;
pt.x = pos_pts(i, 0);
pt.y = pos_pts(i, 1);
pt.z = pos_pts(i, 2);
bspline.pos_pts.push_back(pt);
}
Eigen::VectorXd knots = info->position_traj_.getKnot();
for (int i = 0; i < knots.rows(); ++i) {
bspline.knots.push_back(knots(i));
}
Eigen::MatrixXd yaw_pts = info->yaw_traj_.getControlPoint();
for (int i = 0; i < yaw_pts.rows(); ++i) {
double yaw = yaw_pts(i, 0);
bspline.yaw_pts.push_back(yaw);
}
bspline.yaw_dt = info->yaw_traj_.getInterval();
bspline_pub_.publish(bspline);
/* visulization */
auto plan_data = &planner_manager_->plan_data_;
visualization_->drawGeometricPath(plan_data->kino_path_, 0.075, Eigen::Vector4d(1, 1, 0, 0.4));
visualization_->drawBspline(info->position_traj_, 0.1, Eigen::Vector4d(1.0, 0, 0.0, 1), true, 0.2,
Eigen::Vector4d(1, 0, 0, 1));
return true;
} else {
cout << "generate new traj fail." << endl;
return false;
}
}当状态为EXEC_TRAJ时
当当前状态被标记为REPLAN_TRAJ时，则通过调用kinodynamic re planning算法基于当前位置、速度和当前状态信息进行路径重新规划。若规划成功，则设置为EXEC_TRAJ；否则则设置为GEN_NEW_TRAJ.
让我们深入理解KinoReplanFSM::checkCollisionCallback()的功能。该执行器回调函数的主要负责是评估目标点周围的障碍物情况以及规划轨迹的安全性。当遇到障碍物时，该函数会系统性地在目标点周围使用离散的半径和角度进行扫描。如果 scan 发现存在阻碍，则会立即切换状态并进入 REPLAN_TRAJ 阶段以寻找新的安全路径。若环境无阻碍且当前处于 EX Trajectory 状态，则调用 planner_manager 中的 checkTrajCollision 函数进行轨迹验证。通过这一机制，在确保路径有效的同时也能保证系统的安全运行。
void KinoReplanFSM::checkCollisionCallback(const ros::TimerEvent& e) {
LocalTrajData* info = &planner_manager_->local_data_;
if (have_target_) {
auto edt_env = planner_manager_->edt_environment_;
double dist = planner_manager_->pp_.dynamic_ ?
edt_env->evaluateCoarseEDT(end_pt_, /* time to program start + */ info->duration_) :
edt_env->evaluateCoarseEDT(end_pt_, -1.0);
if (dist <= 0.3) {
/* try to find a max distance goal around */
bool new_goal = false;
const double dr = 0.5, dtheta = 30, dz = 0.3;
double new_x, new_y, new_z, max_dist = -1.0;
Eigen::Vector3d goal;
for (double r = dr; r <= 5 * dr + 1e-3; r += dr) {
for (double theta = -90; theta <= 270; theta += dtheta) {
for (double nz = 1 * dz; nz >= -1 * dz; nz -= dz) {
new_x = end_pt_(0) + r * cos(theta / 57.3);
new_y = end_pt_(1) + r * sin(theta / 57.3);
new_z = end_pt_(2) + nz;
Eigen::Vector3d new_pt(new_x, new_y, new_z);
dist = planner_manager_->pp_.dynamic_ ?
edt_env->evaluateCoarseEDT(new_pt, /* time to program start+ */ info->duration_) :
edt_env->evaluateCoarseEDT(new_pt, -1.0);
if (dist > max_dist) {
/* reset end_pt_ */
goal(0) = new_x;
goal(1) = new_y;
goal(2) = new_z;
max_dist = dist;
}
}
}
}
if (max_dist > 0.3) {
cout << "change goal, replan." << endl;
end_pt_ = goal;
have_target_ = true;
end_vel_.setZero();
if (exec_state_ == EXEC_TRAJ) {
changeFSMExecState(REPLAN_TRAJ, "SAFETY");
}
visualization_->drawGoal(end_pt_, 0.3, Eigen::Vector4d(1, 0, 0, 1.0));
} else {
// have_target_ = false;
// cout << "Goal near collision, stop." << endl;
// changeFSMExecState(WAIT_TARGET, "SAFETY");
cout << "goal near collision, keep retry" << endl;
changeFSMExecState(REPLAN_TRAJ, "FSM");
std_msgs::Empty emt;
replan_pub_.publish(emt);
}
}
}
/* ---------- check trajectory ---------- */
if (exec_state_ == FSM_EXEC_STATE::EXEC_TRAJ) {
double dist;
bool safe = planner_manager_->checkTrajCollision(dist);
if (!safe) {
// cout << "current traj in collision." << endl;
ROS_WARN("current traj in collision.");
changeFSMExecState(REPLAN_TRAJ, "SAFETY");
}
}
}4.3 流程总结
至此为止，该算法的动态规划重排机制已经完成。接下来我们将具体阐述整个系统的运行流程。
- Fast_planner_node.cpp -> 为该系统注册一个ROS节点并创建一个kinoReplanFSM对象用于状态机管理，在初始化后启动ROS服务循环处理任务。
- kino_replan_fsm.cpp -> 实时获取无人机的位置数据以及目标点坐标。通过定时回调函数execFsmCallback动态获取当前运行状态并根据 exec_state参数值判断是否处于等待目标点阶段或执行运动轨迹阶段等状态；同时通过定时回调函数checkCollisionCallback实时监控飞行器与障碍物之间的碰撞风险；所有路径规划操作均调用CallKinodynamicReplan接口完成轨迹生成与优化计算工作；这些操作主要依赖于planner_manager中的kinodynamicReplan算法实现。
- planner_manager.cpp -> 在kinodynamicReplan算法中首先通过Path-finding包中的算法搜索到一条可行路径；接着使用Bspline包中的NonUniformBspline类对路径进行参数化处理得到平滑曲线；随后采用BsplineOptimization包中的均匀B样条优化方法获得平滑度更高的轨迹；整个过程分别对应了Path-finding包、Bspline包以及Bspline-Optimization包的主要功能实现。
到此为止，关于这篇文章的代码已经被别人详细分析完毕。由于笔者的能力有限，在这里可能存在不足之处，请各位予以谅解并提出批评指正。
全部评论 (0)
还没有任何评论哟~