系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理
文章目录
前言
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对Kinodynamic A*算法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章
提示：以下是本篇文章正文内容
运动学/动力学Kinodynamic A*算法在高飞的fast_planner中使用过，他的源码而是开源的
好像印象之中，高飞在深蓝学院的运动规划课程中，稍微也是对Kinodynamic A*算法有讲述
这个Kinodynamic A*算法进行路径探索不仅仅会考虑节点位置还会考虑其节点的速度和加速度
我跑一下，后面更~立个flag date:11.24
date:11.26我来实现我的flag了
.
.
.
一、前端kinodynamic A*算法动力学路径搜索的功能
用于实现全局规划 ，主要调用函数为 “search()”，在已知地图的前提下，给定起点和终点状态(位置和速度)实现混合A搜索；如果搜索成功，返回一系列path_nodes_节点【防盗标记–盒子君hzj】
前端 kinodynamic a_star 路径搜索（在控制空间进行motion primitive）以及基于帕特利亚金最小值原理的两点边界值问题求解（解决控制空间稀疏采样无法抵达目标点的问题-在状态空间进行motion primitive）【防盗标记–盒子君hzj】
.
.
二、步骤一：进行实时采样,离散的获得一些轨迹点（节点point_set，即创建open_list）以及起始点的速度与加速度
【getSamples（）】
1.功能
KinodynamicAstar进行实时采样,离散的获得一些轨迹点（节点point_set，即创建open_list）以及起始点的速度与加速度open_list
.
.
2.代码实现
//功能:
// KinodynamicAstar进行采样,离散的获得一些轨迹点以及起始点的速度与加速度,得到节点point_set，即open_list
void KinodynamicAstar::getSamples(
double& ts, vector<Eigen::Vector3d>& point_set,
std::vector<Eigen::Vector3d>& start_end_derivatives) {
/* ---------- 【计算最后的轨迹时间T_sum】final trajectory time ---------- */
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 << "final time:" << T_sum << endl;
/* ---------- 【采样过程初始化】init for sampling ---------- */
int K = floor(T_sum / ts);
ts = T_sum / static_cast<double>(K + 1);
// cout << "K:" << K << ", ts:" << ts << endl;
bool sample_shot_traj = is_shot_succ_; //复位获取的轨迹点标志位
// Eigen::VectorXd sx(K + 2), sy(K + 2), sz(K + 2);
// int sample_num = 0;
node = path_nodes_.back();
//设置终点的速度、终点加速度
Eigen::Vector3d end_vel, end_acc;
double t;
if (sample_shot_traj) {
//若能采样到轨迹点，使用运动约束计算终点的速度、终点加速度
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 = node->duration;
end_vel = node->state.tail(3);
end_acc = node->input;
}
for (double ti = T_sum; ti > -1e-5; ti -= ts) {
/* ---------- 【开始采样轨迹点】---------- */
if (sample_shot_traj) {
//若采样到轨迹点sample shot traj，把轨迹点存入point_set
Eigen::Vector3d coord;
Eigen::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);
}
// sx(sample_num) = coord(0), sy(sample_num) = coord(1), sz(sample_num) =
// coord(2);
// ++sample_num;
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 {
//若没有采样得到轨迹点，进行状态传递，继续采样sample search traj
Eigen::Matrix<double, 6, 1> x0 = node->parent->state;
Eigen::Matrix<double, 6, 1> xt;
Eigen::Vector3d ut = node->input;
stateTransit(x0, xt, ut, t); //【终点】状态传递
// sx(sample_num) = xt(0), sy(sample_num) = xt(1), sz(sample_num) = xt(2);
// ++sample_num;
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;
}
}
}
/* ---------- return samples ---------- */
// samples.col(K + 2) = start_vel_;
// samples.col(K + 3) = end_vel_;
// samples.col(K + 4) = node->input;
reverse(point_set.begin(), point_set.end());
start_end_derivatives.push_back(start_vel_);
start_end_derivatives.push_back(end_vel);
start_end_derivatives.push_back(node->input); // start acc
start_end_derivatives.push_back(end_acc); // end acc
}
.
.
三：步骤二：设置算法搜索参数setParam()
void KinodynamicAstar::setParam() {
max_tau_ = 1.0; // 最大时间 s
init_max_tau_ = 0.8; // 初始化时间 s
max_vel_ = 6.0; // 最大的速度 m/s
max_acc_ = 2.0; // 最大的加速度 m/s^2
w_time_ = 10.0; // 花费时间的权重
horizon_ = 50.0; // 搜索范围，在这里基本上是无用的，所以我们必须将它设置为一个大值
lambda_heu_ = 5.0;
resolution_ = 0.5; // 距离分辨率 m
time_resolution_ = 0.8; // 时间分辨率 s
margin_ = 0.2; // 边界
allocate_num_ = 100000; // 分配数量,设置了100000个节点到path_node_pool
check_num_ = 10; // 检查数量
}
nh.param("search/max_tau", max_tau_, -1.0); //如果考虑对时间维度进行划分才设置，这里未设置
nh.param("search/init_max_tau", init_max_tau_, -1.0);
nh.param("search/max_vel", max_vel_, -1.0); // 速度限制
nh.param("search/max_acc", max_acc_, -1.0); // 加速度限制
nh.param("search/w_time", w_time_, -1.0); //
nh.param("search/horizon", horizon_, -1.0); //限制全局规划的距离，保证实时性
nh.param("search/resolution_astar", resolution_, -1.0); //空间分辨率
nh.param("search/time_resolution", time_resolution_, -1.0); // 时间维度分辨率
nh.param("search/lambda_heu", lambda_heu_, -1.0); // 启发函数权重
nh.param("search/margin", margin_, -1.0); //检测碰撞
nh.param("search/allocate_num", allocate_num_, -1); //最大节点数目
nh.param("search/check_num", check_num_, -1); //对中间状态安全检查
.
.
.
四、步骤三：整体的搜索过程search()–包括进行节点扩张-剪枝
1.整体搜索的逻辑流程
手撕示例图
步骤详细解释
（1）步骤1:自己额外加的规划条件约束
1）记录一下规划时间，规划超时退出
2）据业务要求的不进行路径规划，放了一个启动plan标志位实现【防盗标记–盒子君hzj】
（2）步骤2：进行采样,离散的获得一些轨迹点以及起始点的速度与加速度,得到节点point_set，即创建open_list
getSamples（）【基于控制空间采样的过程，我在后面分析】
（3）步骤3:将起始点及目标点的三维位置转化至栅格地图的index. 并计算第一个扩展点的Heuristic cost
【计算启发代价H函数的原理，我在后面分析】【防盗标记–盒子君hzj】
（4）步骤4：迭代循环搜索扩张节点，扩张节点的同时不断进行两点边界只的直达曲线计算
1）从open_list优先级队列中取出f(n) = g(n) + h(n)代价值最小的节点
2）【重点】判断当前节点是否超出horizon或是离终点较近了，并计算一条直达曲线，检查这条曲线上是否存在。若存在，则搜索完成，返回路径点即可
【computeShotTraj（）计算直达曲线的原理，，我在后面分析】
3）【重点】若当前节点没有抵达终点，就要进行节点扩张-剪枝【重点!!!】
1、在open_list中删除节点，并在close_list中添加节点
2、初始化状态传递
3、判断节点是已经被扩展过
4、状态传递循环迭代，检查速度约束，检查碰撞，都通过的话，就计算当前节点的g_score以及f_score，并且对重复的扩展节点进行剪枝
判断节点若在不在close_list中
判断节点最大速度
判断节点不在同样的网格中
判断节点是否发生碰撞
状态传递，通过前向积分得到扩展节点的位置和速度stateTransit（）【防盗标记–盒子君hzj】
计算当前节点的真实的代价G值
计算启发式代价H值
在循环中对比扩展节点,进行节点剪枝
【状态传递stateTransit（）、计算当前节点的真实的代价G值、计算启发式代价H值、节点剪枝这四个过程比较重要，后面详细分析】
4）open_list已经遍历完成了，没有搜索到路径 open set empty, no path
（4）步骤4：获取规划得到的路径点getKinoTraj（）
2.代码实现
核心原理：
search函数的参数包含起始点以及重点的位置，速度，加速度。以及两个标志位init 和dynamic，还有一个搜索起始时间
int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v,
Eigen::Vector3d start_a, Eigen::Vector3d end_pt,
Eigen::Vector3d end_v, bool init, bool dynamic,
double time_start) {
//【自己加的】记录一下规划时间
using Time = apollo::cyber::Time;
decideMapType(start_pt, end_pt); //判断使用什么地图类型
double search_starttime = Time::MonoTime().ToSecond();
//【自己加的】业务要求的不进行路径规划，放了一个标志位实现
if (not_to_plan_) {
return NO_PATH;
}
/* ---------- （1）将起始点及目标点的三维位置转化至栅格地图的index. 并计算第一个扩展点的Heuristic cost ---------- */
{
start_vel_ = start_v;
start_acc_ = start_a;
//设置第一个节点
PathNodePtr cur_node = path_node_pool_[0];
cur_node->parent = NULL;
cur_node->state.head(3) = start_pt;
cur_node->state.tail(3) = start_v;
cur_node->index = posToIndex(start_pt);
cur_node->g_score = 0.0;
//设置最后一个节点
Eigen::VectorXd end_state(6);
Eigen::Vector3i end_index;
double time_to_goal;
end_state.head(3) = end_pt;
end_state.tail(3) = end_v;
end_index = posToIndex(end_pt);
//【核心】计算启发代价值f
cur_node->f_score =
lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal);
cur_node->node_state = IN_OPEN_SET;
//把起点放入open_list和expanded_nodes_
open_set_.push(cur_node);
use_node_num_ += 1;
expanded_nodes_.insert(cur_node->index, cur_node);
PathNodePtr neighbor = NULL; //邻节点
PathNodePtr terminate_node = NULL; //终止节点
bool init_search = init; //判断hybrid A*有没有初始化成功
int tolerance1 = ceil(2 / resolution_);
int tolerance2 = ceil( 2.5 * end_v.norm() * 0.4 / resolution_);
int tolerance = 2; // 2 grids //std::max(tolerance1, tolerance2);
}
/* ---------- （2）搜索扩张节点迭代循环 ---------- */
while (!open_set_.empty()) {
//在open_list中寻找节点，直到open_list为空
//【自己加的】规划超时退出
//计算规划到现在所耗的时间，规划到现在所耗的时间 = 现在的时刻 - 开始规划的时刻
double elapsed_time = Time::MonoTime().ToSecond() - search_starttime;
if (elapsed_time > 0.2) {
cout << "Searched for too long, abort!!!" << endl;
return NO_PATH;
}
/* --- 1）从open_list优先级队列中取出f(n) = g(n) + h(n)代价值最小的节点 get lowest f_score node ---------- */
cur_node = open_set_.top();//取出栈顶节点，栈顶节点就是代价值最小的节点
/* --- 2）判断当前节点是否超出horizon或是离终点较近了，并计算一条直达曲线，检查这条曲线上是否存在。若存在，则搜索完成，返回路径点即可 ---------- */
//判断是否距离终点比较近了
bool near_end = abs(cur_node->index(0) - end_index(0)) <= tolerance &&
abs(cur_node->index(1) - end_index(1)) <= tolerance &&
abs(cur_node->index(2) - end_index(2)) <= tolerance;
//判断当前节点有没有碰撞（通过地图）
bool has_collide = collide(cur_node->state.head(3), end_pt);
//判断当前角度是否正确
bool right_angle;
{
Eigen::Vector3d cur_v = cur_node->state.tail(3);
double angle = std::acos(cur_v.dot(end_v) / (cur_v.norm() * end_v.norm()));
if (angle < 45.0 / 180.0 * M_PI) {
right_angle = true;
} else {
right_angle = false;
}
if (end_v.norm() < 1.5) {
right_angle = true;
}
}
//若距离终点比较近了，并且没有发生碰撞，角度还是正确的，则计算一条直达曲线，检查这条曲线上是否存在。若存在，则搜索完成，返回路径点
//后来我们直接认为就到达终点了，不用这么严谨麻烦
if (near_end && !has_collide && right_angle) {
terminate_node = cur_node;
retrievePath(terminate_node);
has_path_ = true;
cout << "Kino AStar near end" << endl;
cout << "use node num" << endl;
cout << "iter num" << endl;
/* 计算一条直达曲线one shot trajectory */
//计算最优控制时间time_to_goal
// estimateHeuristic(cur_node->state, end_state, time_to_goal);
//计算并验证该轨迹是安全的，即不发生碰撞，速度、加速度不超限。
//computeShotTraj(cur_node->state, end_state, time_to_goal);
if (terminate_node->parent == NULL) {
// TODO : fix the problem that the first point is already near end
// std::cout << " failed to shoot for the moon\n";
cout << "Failed to shoot for the moon" << endl;
return NO_PATH;
} else {
return REACH_END;
}
}
/* ---3）若当前节点没有抵达终点，就要进行节点扩张 ---------- */
//以上代码在当前节点的基础上，根据对输入、时间的离散进行扩展得到临时节点tmp
//首先判断节点是否已经被扩展过，即是否与当前节点在同一个节点
//检查速度约束，检查碰撞，都通过的话，就计算当前节点的g_score以及f_score.
//其中的state transit函数即通过前向积分得到扩展节点的位置和速度。接下来，就要进行节点剪枝
//1、在open_list中删除节点，并在close_list中添加节点
open_set_.pop();
cur_node->node_state = IN_CLOSE_SET;
iter_num_ += 1;
//2、初始化状态传递
double res = 1 / 2.0, time_res = 1 / 1.0, time_res_init = 1 / 8.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;
// //3、判断节点是已经被扩展过，弹出节点
// if (init_search) {
// inputs.push_back(start_acc_);
// for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_;
// tau += time_res_init * init_max_tau_)
// durations.push_back(tau);
// } 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);
// }
// 3、判断节点没有被扩展过，把这个节点存下来
for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += 0.5) {
for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += 0.5) {
// for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ *
// res)
{
um << ax, ay, 0; // az;
inputs.push_back(um);
}
}
}
// for (double tau = time_res * max_tau_; tau <= max_tau_;
// tau += time_res * max_tau_) { // actually there is only one single tau
// durations.push_back(tau);
// }
durations.push_back(max_tau_);
//4、状态传递循环迭代，检查速度约束，检查碰撞，都通过的话，就计算当前节点的g_score以及f_score.并且对重复的扩展节点进行剪枝
// 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) {
init_search = false;
um = inputs[i];
double tau = durations[j];
stateTransit(cur_state, pro_state, um, tau);//状态传递，通过前向积分得到扩展节点的位置和速度
pro_t = cur_node->time + tau;
/* ---------- check if in free space ---------- */
//【自己加的】增加地图范围inside map range
if (pro_state(0) <= origin_(0) || pro_state(0) >= map_size_3d_(0) ||
pro_state(1) <= origin_(1) || pro_state(1) >= map_size_3d_(1) ||
pro_state(2) <= origin_(2) || pro_state(2) >= map_size_3d_(2)) {
// std::cout << "outside map" << endl;
// std::cout << "pro_state: " << pro_state(0) << " " << pro_state(1)
// << " " << pro_state(2) << std::endl; std::cout << "origin_: " <<
// origin_(0) << " " << origin_(1) << " " << origin_(2) << std::endl;
// std::cout << "map_size_3d_: " << map_size_3d_(0) << " " <<
// map_size_3d_(1) << " " << map_size_3d_(2) << std::endl;
continue;
}
// 判断节点若在不在close_list中
Eigen::Vector3i pro_id = posToIndex(pro_state.head(3));
int pro_t_id = timeToIndex(pro_t);
PathNodePtr pro_node = expanded_nodes_.find(pro_id);
if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET) {
// cout << "in closeset" << endl;
continue;
}
// 判断节点最大速度
// 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_) {
// // cout << "vel infeasible" << endl;
// continue;
// }
Eigen::Vector3d pro_v = pro_state.tail(3);
if (pro_v.norm() > max_vel_) {
// cout << "vel infeasible" << endl;
continue;
}
// 判断节点不在同样的网格中 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)) {
cout << "Same voxel" << endl;
continue;
}
//判断节点是否发生碰撞 collision free
Eigen::Vector3d pos;
Eigen::Matrix<double, 6, 1> xt;
bool is_occ = false;
for (int k = 1; k <= check_num_; ++k) {
double dt =
tau * static_cast<double>(k) / static_cast<double>(check_num_);
stateTransit(cur_state, xt, um, dt);//状态传递，通过前向积分得到扩展节点的位置和速度
pos = xt.head(3);
double dist_no_inter = collision_detection_->minObstacleDistance(pos);
if (dist_no_inter <= SAFE_DISTANCE_) {
is_occ = true;
break;
}
if (pointHasGlobalCollision(pos)) {
is_occ = true;
break;
}
}
if (is_occ) {
// cout << "collision" << endl;
continue;
}
//计算代价值 compute cost
double time_to_goal, tmp_g_score, tmp_f_score;
//计算当前节点的真实的代价G值
tmp_g_score = (um.squaredNorm() + w_time_) * tau + cur_node->g_score;
//计算启发式代价H值
tmp_f_score = tmp_g_score +
1.0 * lambda_heu_ *
estimateHeuristic(pro_state, end_state, time_to_goal);
/* ----------5)在循环中对比扩展节点,进行节点剪枝---------- */
// 首先判断当前临时扩展节点与current node的其他临时扩展节点是否在同一个voxel中，如果是的话，就要进行剪枝。
// 如果不用剪枝的话，则首先判断当前临时扩展节点pro_node是否出现在open集中，
// 如果不是存在于open集的话，则可以直接将pro_node加入open集中。
// 如果是存在于open集但还未扩展的话，则比较当前临时扩展节点与对应VOXEL节点的fscore,若更小，则更新voxel中的节为当前临时扩展节点。
// 需要进行说明的是，在Fast planner的实现中，
// open集是通过两个数据结构实现的，
// 一个队列用来存储，弹出open集中的节点。
// 另一个哈希表NodeHashtable 用来查询节点是否已经存在于open集中。
// 而判断一个节点是否存在于close set中，则是通过Nodehashtable 与nodestate来决定的，
// 如果nodeState 是 InCloseSet, 且存在于NodeHashtable, 则说明该节点已经被扩展过了，存在于close set中。
bool prune = false; //剪枝标志位
for (int j = 0; j < tmp_expand_nodes.size(); ++j) {
PathNodePtr expand_node = tmp_expand_nodes[j];
// 首先判断当前临时扩展节点与current node的其他临时扩展节点是否在同一个voxel中，如果是的话就是扩展的节点多余了，就要进行剪枝。
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;
}
break;
}
}
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;
open_set_.push(pro_node);
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;
}
}
// 如果不用剪枝的话，则首先判断当前临时扩展节点pro_node是否出现在open集中
else if (pro_node->node_state == IN_OPEN_SET) {
// 如果是存在于open集但还未扩展的话，则比较当前临时扩展节点与对应VOXEL节点的fscore,若更小，则更新voxel中的节为当前临时扩展节点。
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;
}
}
// 如果不是存在于open集的话，则可以直接将pro_node加入open集中
else {
cout << "error type in searching: " << pro_node->node_state << endl;
}
}
/* ---------- ---------- */
}
}
/* ----------5）open_list已经遍历完成了，没有搜索到路径 open set empty, no path ---------- */
// cout << "open set empty, no path!" << endl;
// cout << "use node num: " << use_node_num_ << endl;
// cout << "iter num: " << iter_num_ << endl;
// if(iter_num_ == 1) {
// int a;
// std::cin >> a;
// std::cout << a;
// }
return NO_PATH;
}
.
.
.
五、步骤四：获取规划得到的路径点getKinoTraj（）
1、功能
【调用获取规划得到的路径点】
完成路径搜索后，按照预设的时间分辨率delta_t， 通过节点回溯和状态前向积分得到分辨率更高的路径点。 如果最后的shot trajectory存在的话，则还要加上最后一段shot trajectory(即通过computeshottraj)算出来得到的【防盗标记–盒子君hzj】
最终轨迹 = 前面节点扩张的回溯轨迹+后面两点边界只轨迹
当然，我们前后没有启动shot trajectory【防盗标记–盒子君hzj】
2、代码实现
//输入预设时间分辨率，获取对应的一段动力学轨迹state_list
std::vector<Eigen::Vector3d> KinodynamicAstar::getKinoTraj(double delta_t) {
std::vector<Vector3d> state_list;
/* ---------- 【获取搜索到的轨迹state_list】get traj of searching ---------- */
PathNodePtr node = path_nodes_.back();
Eigen::Matrix<double, 6, 1> x0, xt;
while (node->parent != NULL) {
Eigen::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());
/* ---------- 【获取一小段轨迹state_list】get traj of one shot ---------- */
if (is_shot_succ_) {
Eigen::Vector3d coord;
Eigen::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;
}
六、关键函数分析【重点】
（1）计算启发代价值H estimateHeuristic（）
1、原理
estimateHeuristic（）主要原理是利用庞特里亚金原理解决两点边值问题，得到最优解后用最优解的控制代价作为启发函数。
2、代码实现
首先通过设置启发函数对时间求导等于0，得到启发函数关于时间T的四次方程，再通过求解该四次方程，得到一系列实根，通过比较这些实根所对应的cost大小，得到最优时间。
这里需要注意，关于时间的一元四次方程是通过费拉里方法求解的，需要嵌套一个元三次方程进行求解，也就是代码中应的cubic（）函数。【防盗标记–盒子君hzj】
//算法原理：对应文章中的III.B小节
//主要原理:
//利用庞特里亚金原理解决两点边值问题，得到最优解后用最优解的控制代价作为启发函数。
//具体步骤：
// 首先通过设置启发函数对时间求导等于0，得到启发函数关于时间T的四次方程，再通过求解该四次方程，
// 得到一系列实根，通过比较这些实根所对应的cost大小，得到最优时间。
//input ：起点x1、终点x2
//return :optimal_time最优控制时间、启发值代价
double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1,
Eigen::VectorXd x2,
double& optimal_time) {
const Eigen::Vector3d dp = x2.head(3) - x1.head(3);//两点间的距离
const Eigen::Vector3d v0 = x1.segment(3, 3); //起点速度
const Eigen::Vector3d v1 = x2.segment(3, 3); //终点速度
//(1)首先通过设置启发函数对时间求导等于0，得到启发函数关于时间T的四次方程
//dot()两个数组的点积
double c1 = -36 * dp.dot(dp);
double c2 = 24 * (v0 + v1).dot(dp);
double c3 = -4 * (v0.dot(v0) + v0.dot(v1) + v1.dot(v1));
double c4 = 0;
double c5 = w_time_;
//(2)求解一元四次方程，得到一系列实根，通过比较这些实根所对应的cost大小，得到最优时间。
//关于时间的一元四次方程是通过费拉里方法求解的，需要嵌套一个元三次方程进行求解，也就是代码中应的cubic（）函数。
//一元四次方程的求解过程参见wikipedia中的费拉里方法：https://zh.wikipedia.org/wiki/%E5%9B%9B%E6%AC%A1%E6%96%B9%E7%A8%8B
//一元三次方程的求见过程参见wikipedia中的求根系数法。需要加以说明的是，代码中的判别式大于0及等于0的情况利用了求根公式，判别式小于0的情况则是使用了三角函数解法
//https://zh.wikipedia.org/wiki/%E4%B8%89%E6%AC%A1%E6%96%B9%E7%A8%8B
std::vector<double> ts = quartic(c5, c4, c3, c2, c1);
double v_max = max_vel_;
double t_bar = (x1.head(3) - x2.head(3)).lpNorm<Infinity>() / v_max;
ts.push_back(t_bar);
double cost = 100000000;
double t_d = t_bar;
//比较这些实根所对应的cost大小，得到最优时间
for (auto t : ts) {
if (t < t_bar) continue;
double c = -c1 / (3 * t * t * t) - c2 / (2 * t * t) - c3 / t + w_time_ * t;
if (c < cost) {
cost = c;
t_d = t;
}
}
optimal_time = t_d;
return 1.0 * (1 + tie_breaker_) * cost;
}
//求解一元四次方程
//ax^4 + bx^3 + cx^2 + dx + e = 0
std::vector<double> KinodynamicAstar::quartic(double a, double b, double c,
double d, double e) {
std::vector<double> dts;
double a3 = b / a;
double a2 = c / a;
double a1 = d / a;
double a0 = e / a;
//求解一元三次方程
std::vector<double> ys =
cubic(1, -a2, a1 * a3 - 4 * a0, 4 * a2 * a0 - a1 * a1 - a3 * a3 * a0);
double y1 = ys.front();
double r = a3 * a3 / 4 - a2 + y1;
if (r < 0) return dts;
double R = sqrt(r);
double D, E;
if (R != 0) {
D = sqrt(0.75 * a3 * a3 - R * R - 2 * a2 +
0.25 * (4 * a3 * a2 - 8 * a1 - a3 * a3 * a3) / R);
E = sqrt(0.75 * a3 * a3 - R * R - 2 * a2 -
0.25 * (4 * a3 * a2 - 8 * a1 - a3 * a3 * a3) / R);
} else {
D = sqrt(0.75 * a3 * a3 - 2 * a2 + 2 * sqrt(y1 * y1 - 4 * a0));
E = sqrt(0.75 * a3 * a3 - 2 * a2 - 2 * sqrt(y1 * y1 - 4 * a0));
}
if (!std::isnan(D)) {
dts.push_back(-a3 / 4 + R / 2 + D / 2);
dts.push_back(-a3 / 4 + R / 2 - D / 2);
}
if (!std::isnan(E)) {
dts.push_back(-a3 / 4 - R / 2 + E / 2);
dts.push_back(-a3 / 4 - R / 2 - E / 2);
}
return dts;
}
//求解一元三次方程
//ax^3 + bx^2 + cx + d = 0
std::vector<double> KinodynamicAstar::cubic(double a, double b, double c,
double d) {
std::vector<double> dts;
double a2 = b / a;
double a1 = c / a;
double a0 = d / a;
double Q = (3 * a1 - a2 * a2) / 9;
double R = (9 * a1 * a2 - 27 * a0 - 2 * a2 * a2 * a2) / 54;
double D = Q * Q * Q + R * R;
if (D > 0) {
double S = std::cbrt(R + sqrt(D));
double T = std::cbrt(R - sqrt(D));
dts.push_back(-a2 / 3 + (S + T));
return dts;
} else if (D == 0) {
double S = std::cbrt(R);
dts.push_back(-a2 / 3 + S + S);
dts.push_back(-a2 / 3 - S);
return dts;
} else {
double theta = acos(R / sqrt(-Q * Q * Q));
dts.push_back(2 * sqrt(-Q) * cos(theta / 3) - a2 / 3);
dts.push_back(2 * sqrt(-Q) * cos((theta + 2 * M_PI) / 3) - a2 / 3);
dts.push_back(2 * sqrt(-Q) * cos((theta + 4 * M_PI) / 3) - a2 / 3);
return dts;
}
}
.
.
（2）计算当前节点的真实的代价G值
tmp_g_score = (um.squaredNorm() + w_time_) * tau + cur_node->g_score;
.
.
（3）计算一条直达曲线computeShotTraj（）
1.原理
计算出起始节点的启发项后，就基本进入搜索循环【防盗标记–盒子君hzj】，第一步是判断当前节点是否超出horizon或是离终点较近了，并计算一条直达曲线，检查这条曲线上是否存在。若存在，则搜索完成，返回路径点即可
利用庞特里亚金原理解一个两点边值问题。因为最优控制时间已经在estimateHeuristic中计算过了，所以这里只要引入该时间进行多项式计算即可。这部分的目的是为了验证该轨迹是安全的，即不发生碰撞，速度、加速度不超限
五次多项式优化就是mini_snap，这里本质就是通过多项式计算五次多项式两点边界问题，方法有很多，这里利用庞特里亚金原理解一个两点边值问题，其他方法看我另外体格博客
2.代码实现
//【重要函数】利用庞特里亚金原理解一个两点边值问题。
//因为最优控制时间time_to_goal已经在estimateHeuristic中计算过了
//所以这里只要引入该时间进行多项式计算即可
//目的：计算并验证该轨迹是安全的，即不发生碰撞，速度、加速度不超限。
//主要步骤：多项式计算
bool KinodynamicAstar::computeShotTraj(Eigen::VectorXd state1,
Eigen::VectorXd state2,
double time_to_goal) {
/* ---------- 获取系数get coefficient ---------- */
const Eigen::Vector3d p0 = state1.head(3);
const Eigen::Vector3d dp = state2.head(3) - p0;
const Eigen::Vector3d v0 = state1.segment(3, 3);
const Eigen::Vector3d v1 = state2.segment(3, 3);
const Eigen::Vector3d dv = v1 - v0;
double t_d = time_to_goal;
MatrixXd coef(3, 4);
end_vel_ = v1;
Eigen::Vector3d a =
1.0 / 6.0 *
(-12.0 / (t_d * t_d * t_d) * (dp - v0 * t_d) + 6 / (t_d * t_d) * dv);
Eigen::Vector3d b =
0.5 * (6.0 / (t_d * t_d) * (dp - v0 * t_d) - 2 / t_d * dv);
Eigen::Vector3d c = v0;
Eigen::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;
Eigen::Vector3d coord, vel, acc;
Eigen::VectorXd poly1d, t, polyv, polya;
Eigen::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 = Eigen::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_) {
if (collision_detection_->minObstacleDistance(coord) <=
margin_) {
return false;
}
if (pointHasGlobalCollision(coord)) {
return false;
}
}
coef_shot_ = coef;
t_shot_ = t_d;
is_shot_succ_ = true;
return true;
}
（4）节点扩张、节点剪枝流程
这个过程就是A*搜索的套路，如步骤三所示
1.节点扩张
根据对输入、时间的离散进行扩展得到tmp临时节点，首先判断节点是否已经被扩展过，是否与当前节点在同一个节点，检查速度约束，检查碰撞，都通过的话，就计算当前节点的g_score以及f_score. 其中的state transit函数即通过前向积分得到扩展节点的位置和速度
.
2.节点剪枝
首先判断当前临时扩展节点与current node的其他临时扩展节点是否在同一个voxel中，如果是的话，就要进行剪枝。要判断当前临时扩展节点的fscore是否比同一个voxel的对比fscore小，如果是的话，则更新这一Voxel节点为当前临时扩展节点。【防盗标记–盒子君hzj】
如果不剪枝的话，则首先判断当前临时扩展节点pro_node是否出现在open集中，如果是不是的话，则可以直接将pro_node加入open集中。【防盗标记–盒子君hzj】如果存在于open集但还未扩展的话，则比较当前临时扩展节点与对应VOXEL节点的fscore,若更小，则更新voxel中的节点。
需要进行说明的是，在Fast planner的实现中，open集是通过两个数据结构实现的，一个队列用来存储，弹出open集中的节点。另一个哈希表NodeHashtable 用来查询节点是否已经存在于open集中。而判断一个节点是否存在于close set中，【防盗标记–盒子君hzj】则是通过Nodehashtable 与nodestate来决定的，如果nodeState 是 InCloseSet, 且存在于NodeHashtable, 则说明该节点已经被扩展过了，存在于close set中
总结
啃这个Kinodynamic A_star算法，建议先啃一啃高飞fast_planner那篇论文，把理论打好，如果还是比较迷，就先看Dijksta、再看A_star、再看hybrid A_star 三个的流程，因为往上资料比较多，再看这个Kinodynamic A_star算法，流程已经很相像了。如果。。。不没有如果了~【防盗标记–盒子君hzj】
参考资料
网站
https://blog.csdn.net/weixin_42284263/article/details/119204964
论文
Trajectory Replanning for Quadrotors Using Kinodynamic Search and Elastic Optimization
Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight
Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths