参考文章:Faster Planner——Kinodynamic Astar详解-CSDN博客
1. KinodynamicAstar 类
用于实现一个考虑动力学约束的A*搜索算法来规划路径
1.1 KinodynamicAstar::search
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->g_score = 0.0; // 实际代价
// 初始化终点信息
Eigen::VectorXd end_state(6);
...
// 设置终点状态和索引
end_state.head(3) = end_pt;
...
// 计算起点节点的启发函数代价值并加入open列表
cur_node->f_score = lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal);
cur_node->node_state = IN_OPEN_SET;
open_set_.push(cur_node);
use_node_num_ += 1;
// 动态环境初始化
if (dynamic)
{
...
}
...
// 主搜索循环
while (!open_set_.empty())
{
cur_node = open_set_.top();
...
// 检查是否达到终点或达到搜索范围的边界
bool reach_horizon = (cur_node->state.head(3) - start_pt).norm() >= horizon_;
...
// 如果达到终点或搜索范围边界,则终止搜索
if (reach_horizon || near_end)
{
...
}
...
// 检查是否存在直接连接的路径
if (near_end)
...
// 从open列表移除当前节点,并标记
open_set_.pop();
...
// 生成可能的输入加速度向量和时间组合,拓展当前节点
for (int i = 0; i < inputs.size(); ++i)
for (int j = 0; j < durations.size(); ++j)
{
...
// 检查扩展节点的有效性(),并更新代价和启发函数
...
}
...
}
...
}
主要步骤:
1.初始化起点节点:设置起点位置、速度、加速度等信息,并计算启发函数值将其加入open_set;
2.主搜索循环 while (!open_set_.empty()) :
a.对每个open_set 中的节点,检查其是否到达预设范围(horizon)或接近终点;
b.如果到达终点或预设范围,检索路径并返回成功;
c.否则,生成可能的输入(加速度)和持续时间,拓展当前节点;
d.对于每个可能的输入和持续时间,计算下一个状态并检查其有效性(速度限制、障碍物碰撞),并更新代价和启发函数;
e.将有效的拓展节点添加到open_set中。
3.路径找到后的处理:如果找到路径或满足终止条件,调用retrievePath
函数从终点回溯到起点,构建完整路径。
辅助函数:
- stateTransit:根据输入加速度和时间,将当前状态转换到下一状态。
- estimateHeuristic:估计启发式成本。
- computeShotTraj:计算从当前节点到终点的直接路径是否可行。
1.2 KinodynamicAstar::setParam
void KinodynamicAstar::setParam(ros::NodeHandle& nh)
{
nh.param("search/max_tau", max_tau_, -1.0);
...
}
设置参数 :从ROS节点句柄 (nh
) 中获取参数并设置类成员变量。
1.3 KinodynamicAstar::retrievePath
void KinodynamicAstar::retrievePath(PathNodePtr end_node)
{
...
}
路径回溯检索 :
- 从终点节点开始,沿着父节点链回溯,直到起点节点。
- 将路径节点存储在
path_nodes_
中,并将其顺序反转,以得到从起点到终点的路径
1.4 KinodynamicAstar::estimateHeuristic
double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2, double& optimal_time)
{
// x1 、x2 起始和目标状态,包括位置和速度
const Vector3d dp = x2.head(3) - x1.head(3); // x1和x2间位置差异
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_;
// 求解五次多项式,得到Th
std::vector<double> ts = quartic(c5, c4, c3, c2, c1);
// 求最短估计时间
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;
// 循环计算多项式的解 计算对应代价,找到最小代价和时间
for (auto t : ts)
{
...
}
optimal_time = t_d;
return 1.0 * (1 + tie_breaker_) * cost;
}
启发函数:计算当前状态到目标状态的启发函数值。
根据论文中求得的最优启发代价函数 ,将系数项
、
带入整理得:
令其对 求偏导:
式中各项分别对应多项式系数 c1-c5,c5 为时间权重。代码中设 ,求解该四次多项式的根。求解四次多项式的根时,调用 quartic() 函数,该函数使用费拉里方法来求解四次多项式的根。在求解三次多项式时,代码使用两种方法,当判别式大于0及等于0的情况利用求根公式,判别式小于0的情况使用三角函数解法。
1.5 KinodynamicAstar::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.6 KinodynamicAstar::cubic
vector<double> KinodynamicAstar::cubic(double a, double b, double c, double d)
{
...
}
求解三次多项式
1.7 KinodynamicAstar::quartic
vector<double> KinodynamicAstar::quartic(double a, double b, double c, double d, double e)
{
...
}
quartic
函数用来求解五次多项式的根,得到时间的候选值:
1.8 KinodynamicAstar::init()
void KinodynamicAstar::init()
{
/* ---------- map params ---------- */
this->inv_resolution_ = 1.0 / resolution_;
inv_time_resolution_ = 1.0 / time_resolution_;
edt_environment_->sdf_map_->getRegion(origin_, map_size_3d_);
cout << "origin_: " << origin_.transpose() << endl;
cout << "map size: " << map_size_3d_.transpose() << endl;
/* ---------- pre-allocated node ---------- */
path_node_pool_.resize(allocate_num_);
for (int i = 0; i < allocate_num_; i++)
{
path_node_pool_[i] = new PathNode;
}
phi_ = Eigen::MatrixXd::Identity(6, 6);
use_node_num_ = 0;
iter_num_ = 0;
}
- 功能: 初始化Astar对象,设置地图参数并预分配节点池。
- 实现: 获取地图的区域信息,初始化节点池。
1.9 KinodynamicAstar::setEnvironment
void KinodynamicAstar::setEnvironment(const EDTEnvironment::Ptr& env)
{
this->edt_environment_ = env;
}
- 功能: 设置环境评估对象。
- 实现: 将传入的环境评估对象赋值给成员变量
edt_environment_
。
1.10 KinodynamicAstar::reset()
void KinodynamicAstar::reset()
{
expanded_nodes_.clear();
path_nodes_.clear();
std::priority_queue<PathNodePtr, std::vector<PathNodePtr>, NodeComparator> empty_queue;
open_set_.swap(empty_queue);
for (int i = 0; i < use_node_num_; i++)
{
PathNodePtr node = path_node_pool_[i];
node->parent = NULL;
node->node_state = NOT_EXPAND;
}
use_node_num_ = 0;
iter_num_ = 0;
is_shot_succ_ = false;
has_path_ = false;
}
- 功能: 重置Astar对象的状态,以便进行新的搜索。
- 实现: 清空已扩展节点集合、路径节点集合,重置优先级队列和节点池的状态。
1.11 KinodynamicAstar::getKinoTraj
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;
}
1.12 KinodynamicAstar::getSamples
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);
}
- 功能:从路径中生成样本点。
- 步骤:
- 遍历路径中的每一个节点对,计算每一段路径的中间点。
- 将每段路径上的点加入
point_set
中,将起点和终点的速度信息加入start_end_derivatives
中
- 意义:从路径中生成一系列样本点。
- 作用:
- 对路径中的每一段进行离散化,计算并存储路径上的点。
- 将路径的起点和终点的速度信息存储到
start_end_derivatives
中。
1.13 KinodynamicAstar::getVisitedNodes()
std::vector<PathNodePtr> KinodynamicAstar::getVisitedNodes()
{
vector<PathNodePtr> visited;
visited.assign(path_node_pool_.begin(), path_node_pool_.begin() + use_node_num_ - 1);
return visited;
}
1.14 其他辅助函数
// 将位置转换为索引。
Eigen::Vector3i KinodynamicAstar::posToIndex(Eigen::Vector3d pt)
{
Vector3i idx = ((pt - origin_) * inv_resolution_).array().floor().cast<int>();
// idx << floor((pt(0) - origin_(0)) * inv_resolution_), floor((pt(1) -
// origin_(1)) * inv_resolution_),
// floor((pt(2) - origin_(2)) * inv_resolution_);
return idx;
}
// 将时间转换为索引
int KinodynamicAstar::timeToIndex(double time)
{
int idx = floor((time - time_origin_) * inv_time_resolution_);
return idx;
}
//意义:根据当前状态、输入加速度和持续时间计算下一个状态。
//作用:通过动力学方程更新状态信息,用于生成下一个节点的状态。
void KinodynamicAstar::stateTransit(Eigen::Matrix<double, 6, 1>& state0, Eigen::Matrix<double, 6, 1>& state1,
Eigen::Vector3d um, double tau)
{
for (int i = 0; i < 3; ++i)
phi_(i, i + 3) = tau;
Eigen::Matrix<double, 6, 1> integral;
integral.head(3) = 0.5 * pow(tau, 2) * um;
integral.tail(3) = tau * um;
state1 = phi_ * state0 + integral;
}
} // namespace fast_planner