作为无人机自主运动的入门路径规划算法，详细表述了路径规划的前端和后端，前端为路径搜索，后端未路径优化。具体参考代码。
一、kinodynamic a_star 路径搜索
这里参考的文章是Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments
总的可以概括为融合动力学约束的A*搜索，通过共轭梯度下降求局部最优。
解析kinodynamic_astar.cpp
Pathnode类(文章中motion Primitives)反向链表
class PathNode {
public:
/* -------------------- */
Eigen::Vector3i index; //voxel坐标
Eigen::Matrix<double, 6, 1> state; //基元状态,分别为xyz及其一阶导(vel)
double g_score, f_score; //代价函数、启发函数
Eigen::Vector3d input; //输入，分别为xyz上的二阶导(acc)
double duration; //输入持续时间
double time; // dyn
int time_idx;
PathNode* parent; //父节点
char node_state;
/* -------------------- */
PathNode() {
parent = NULL;
node_state = NOT_EXPAND;
}
~PathNode(){};
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
typedef PathNode* PathNodePtr;
主函数
/*
kinodynamic A*的主函数
parameter：
- start_pt：起点位置
- start_v: 起始速度
- start_a: 起始加速度
- end_pt: 终点位置
- end_v: 终点速度
- init： 初始化成功标志位
- dynamic：动静规划标志位
- time_start:起始时间
*/
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)
读取传入参数
//读取传入参数
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);
cur_node->f_score = lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal); //计算启发函数
cur_node->node_state = IN_OPEN_SET; //标记为OPEN
open_set_.push(cur_node); //将该点放入OPEN集，即论文中的C
use_node_num_ += 1; //扩展点数计数
启发函数
该文利用Pontryagins minimum principle计算从Xc到Xg轨迹的启发函数。笔者根据自身能力进行推导，具体参考A Computationally Efficient Motion Primitive for Quadrocopter Trajectory Generation及D. P. Bertsekas的《动态规划和最优控制》。
3D运动的cost function，Jk为投影到三轴上的加加速度
状态：
S
k
=
(
p
k
,
v
k
,
a
k
)
S_k=(p_k,v_k,a_k)
Sk=(pk,vk,ak)
状态方程：
S
˙
k
=
f
s
(
S
k
,
j
k
)
=
(
v
k
,
a
k
,
j
k
)
\dot{S}_k=f_s(S_k,j_k)=(v_k,a_k,j_k)
S˙k=fs(Sk,jk)=(vk,ak,jk)
构建Hamiltonian函数和协态方程：
求解costate的微分方程得
最佳输入轨迹J*(t)通过求最小化Hamiltonian函数(PMP原理)获得，即
j*(t)为加加速度的轨迹，
S
0
=
(
p
0
,
v
0
,
a
0
)
S_0=(p_0,v_0,a_0)
S0=(p0,v0,a0)通过积分可以获
a
∗
(
t
)
,
v
∗
(
t
)
,
p
∗
(
t
)
a^*(t),v^*(t),p*(t)
a∗(t),v∗(t),p∗(t)
目标状态
S
f
=
(
p
f
,
v
f
,
a
f
)
S_f=(p_f,v_f,a_f)
Sf=(pf,vf,af)，
解得：
FAST-Planner当中最小化加速度，故与上述最小化加加速度不同，文中给出的公式如下：
启发函数代码(即上述使J取最小的T)：
/*
计算启发函数
parameters：
- x1,x2:起点和终点状态
- optimal_time:达到最终点的最优时间
return:
- 代价值
*/
double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2,
double& optimal_time) {
const Vector3d dp = x2.head(3) - x1.head(3); //位置变化量
const Vector3d v0 = x1.segment(3, 3); //起点速度矩阵
const Vector3d v1 = x2.segment(3, 3); //终点速度矩阵
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_;
std::vector<double> ts = quartic(c5, c4, c3, c2, c1); //求解获得极值点的T
double v_max = max_vel_ * 0.5;
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;
//将根带回启发函数求最优Ts
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;
}
启发函数对T求导获得极值点或者鞍点对应的时间T，然后把多个根代回启发函数获得最优时间T。
搜索
循环搜索第一步判断当前节点是否满足停止搜索的要求
// 判断但前节点是否超出horizon或是离终点较近了
bool reach_horizon = (cur_node->state.head(3) - start_pt).norm() >= 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;
if (reach_horizon || near_end) {
terminate_node = cur_node; //判断其中一个达到了要求，将该节点设为终点
retrievePath(terminate_node);//从终点到起点的反向路径存储到path_nodes_
if (near_end) {
// Check whether shot traj exist
estimateHeuristic(cur_node->state, end_state, time_to_goal); //计算优化的最小时间
computeShotTraj(cur_node->state, end_state, time_to_goal); //带入优化的时间判断轨迹是否合理
if (init_search) ROS_ERROR("Shot in first search loop!");
}
}
if (reach_horizon)
if (near_end)
其中当满足终点条件时，要重新检查一下轨迹是否合理，即将轨迹带入地图，用到下面这个函数
//将计算得到的最优轨迹带入地图中检查轨迹是否安全，有没有发生碰撞，速度、加速度是否超过限制
bool KinodynamicAstar::computeShotTraj(Eigen::VectorXd state1, Eigen::VectorXd state2,
double time_to_goal)
当节点不满足终止条件，需要扩展节点，以当前节点为根，离散输入控制量，得到下一状态。
/* ---3）若当前节点没有抵达终点，就要进行节点扩张 ---------- */
//1、在open_list中删除节点，并在close_list中添加节点
open_set_.pop(); //弹出该节点
cur_node->node_state = IN_CLOSE_SET; //将该节点放入close节点当中
iter_num_ += 1;
//2、初始化状态传递
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; //输入控制量的持续时间
//3、判断节点没有被扩展过，把这个节点存下来
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);
}
//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) {
um = inputs[i];
double tau = durations[j];
stateTransit(cur_state, pro_state, um, tau); //状态传递，通过前向积分得到扩展节点的位置和速度
pro_t = cur_node->time + tau;
// Check inside map range
Eigen::Vector3d pro_pos = pro_state.head(3);
if (!edt_environment_->sdf_map_->isInBox(pro_pos)) {
if (init_search) std::cout << "box" << std::endl;
continue;
}
// 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 ||
!edt_environment_->sdf_map_->isInBox(pos)) {
is_occ = true;
break;
}
if (!optimistic_ && edt_environment_->sdf_map_->getOccupancy(pos) == SDFMap::UNKNOWN) {
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);
/* ----------5)在循环中对比扩展节点,进行节点剪枝---------- */
// 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];
// 首先判断当前临时扩展节点与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;
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;
}
}
// 如果不用剪枝的话，则首先判断当前临时扩展节点pro_node是否出现在open集中
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;
}
}
// 如果不是存在于open集的话，则可以直接将pro_node加入open集中
else {
cout << "error type in searching: " << pro_node->node_state << endl;
}
}
}
// init_search = false;
}
cout << "open set empty, no path!" << endl;
cout << "use node num: " << use_node_num_ << endl;
cout << "iter num: " << iter_num_ << endl;
return NO_PATH;
获得规划得到的路径点
kinodynamic A*的最后一步文章来源：https://www.toymoban.com/news/detail-861740.html
/* 作用：
完成路径搜索后，按照预设的时间分辨率delta_t，通过节点回溯和状态前向积分得到分辨率最高的路径点。
param：
delta_t:时间分辨率
return：
state_list=节点扩张的回溯轨迹+两点边界轨迹
*/
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;
}
第一部分结束，未完待续。。。文章来源地址https://www.toymoban.com/news/detail-861740.html
到了这里，关于Fast-Planner(一)前端详解kinodynamic A*的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
