EGO-planner 论文笔记（一）
论文数学建模
避障排斥力
B-spline曲线的控制点集合Q。
初始化，一条不考虑碰撞的B-spline曲线 Φ \Phi Φ。
在可能碰撞的曲线段，从曲线 Φ \Phi Φ取点 Q i Q_i Qi，求 P h i Phi Phi在该点切线，过该点作切线的垂直平面，该平面与初始轨迹平面 Γ \Gamma Γ的交线 l l l, l l l与障碍物表面的交点即 p i , j p_{i,j} pi,j。注：下标j是指第j个障碍物。
定义从点 Q i Q_i Qi到点 p i , j p_{i,j} pi,j的单位向量 v i , j v_{i,j} vi,j。
d i , j = ( Q i − p i , j ) v i , j d_{i,j} =(\mathbf{Q}i-\mathbf{p}{i,j}) \mathbf{v}_{i,j} di,j=(Qi−pi,j)vi,j
当控制点在障碍物内部， d i , j d_{i,j} di,j为负数，在外部为正数？待证伪。
定义控制点 Q i \mathbf{Q}_i Qi的速度、加速度、加加速度，
V i = Q i + 1 − Q i δ t \mathbf{V}i=\frac{\mathbf{Q}{i+1}-\mathbf{Q}_i}{\delta{t}} Vi=δtQi+1−Qi
A i = V i + 1 − V i δ t \mathbf{A}i=\frac{\mathbf{V}{i+1}-\mathbf{V}_i}{\delta{t}} Ai=δtVi+1−Vi
J i = A i + 1 − A i δ t \mathbf{J}i=\frac{\mathbf{A}{i+1}-\mathbf{A}_i}{\delta{t}} Ji=δtAi+1−Ai
构建碰撞代价（惩罚项）最小化问题
min Q J = λ s J s + λ c J c + λ d J d \min_QJ=\lambda_sJ_s + \lambda_cJ_c + \lambda_dJ_d QminJ=λsJs+λcJc+λdJd
J s = ∑ i = 1 N c − 1 J_s = \sum ^{N_{c-1}}_{i=1} Js=i=1∑Nc−1
梯度法优化轨迹
设计B-spline的自由度是 p b p_b pb， N c N_c Nc个控制点，节点向量 { t 1 , t 2 , . . . , t M } \{t_1,t_2,...,t_M\} {t1,t2,...,tM}， t ∗ ∈ R t_*\in R t∗∈R， M = N_c + p_b ， ， ，t_\* 意义是时刻，且对各个 意义是时刻，且对各个 意义是时刻，且对各个m ， ， ，\\Delta t = t_{m+1} - t_m相等。
B-spline的性质：
- convex hull
- k阶导数是 p b − k p_b-k pb−k自由度的B-spline
微分平坦特性：
系统的所有状态变量和控制输入都可以用一组特定的输出变量（称为平坦输出）及其有限阶导数来代数表示（无需积分）。
公式(3)是代价总函数，包含平滑、避障、动力学可行三项代价。
优化算法
对比3种算法：Barzilai-Borwein、近似牛顿法、L-BFGS。L-BFGS在这问题上性能最好。
求解最优问题 m i n x ∈ R n f ( x ) min_{x\in \mathbb{R}^n}f(x) minx∈Rnf(x)主要思路：
按照实际计算顺序，
y k = ∇ f x k + 1 − ∇ f x k \mathbf{y}k = \nabla\mathbf f{x}{k+1} - \nabla\mathbf f{x}_{k} yk=∇fxk+1−∇fxk
s k = x k + 1 − x k \mathbf{s}k=x{k+1} - x_k sk=xk+1−xk
H k 0 = s k − 1 T y k − 1 y k − 1 T y k − 1 I \mathbf{H}^0_k=\frac{s_{k-1}^T y_{k-1}}{y_{k-1}^T y_{k-1}}\mathbf{I} Hk0=yk−1Tyk−1sk−1Tyk−1I
ρ k = ( y k T s k ) − 1 \rho_k = (y_k^Ts_k)^{-1} ρk=(ykTsk)−1
V k = I − ρ k y k s k T \mathbf{V}_k=\mathbf{I}-\rho_k\mathbf{y}_ks_k^T Vk=I−ρkykskT
H k + 1 = V k T H k V k + ρ k s k s k T \mathbf{H}_{k+1} = \mathbf{V}_k^T\mathbf{H}_k\mathbf{V}_k+\rho_k\mathbf{s}_k\mathbf{s}_k^T Hk+1=VkTHkVk+ρkskskT
x k + 1 = x k − α k H k ∇ f k x_{k+1} = x_k-\alpha_k\mathbf{H}_k\nabla\mathbf{f}_k xk+1=xk−αkHk∇fk
L-BFGS算法，应用参考链接
代码实现
基本框架是有限状态机，定时调用函数execFSMCallback()，设定目标点，状态从WAIT_TARGET改为SEQUENTIAL_START，调用planFromGlobalTraj。
以在rviz中设定目标点为例，ego_planner_node节点的EGOReplanFSM类型的对象，订阅 /move_base_simple/goal，在回调函数EGOReplanFSM::waypointCallback(const geometry_msgs::PoseStampedPtr &msg)，将目标点高度设为1，同时have_target_设为true，
cpp
void EGOReplanFSM::waypointCallback(const geometry_msgs::PoseStampedPtr &msg)
{
if (msg->pose.position.z < -0.1)
return;
cout << "Triggered!" << endl;
// trigger_ = true;
init_pt_ = odom_pos_;
Eigen::Vector3d end_wp(msg->pose.position.x, msg->pose.position.y, 1.0);
planNextWaypoint(end_wp);
}planNextWaypoint调用planGlobalTraj函数，先在起始、终止点间每隔4m插入点。
cpp
vector<Eigen::Vector3d> points;
points.push_back(start_pos);
points.push_back(end_pos);
// insert intermediate points if too far
vector<Eigen::Vector3d> inter_points;
const double dist_thresh = 4.0;
for (size_t i = 0; i < points.size() - 1; ++i)
{
inter_points.push_back(points.at(i));
double dist = (points.at(i + 1) - points.at(i)).norm();
if (dist > dist_thresh)
{
int id_num = floor(dist / dist_thresh) + 1;
for (int j = 1; j < id_num; ++j)
{
Eigen::Vector3d inter_pt =
points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num;
inter_points.push_back(inter_pt);
}
}
}
// 加终点
inter_points.push_back(points.back());再构建一个Eigen矩阵，存放系列点坐标。
c++
int pt_num = inter_points.size();
Eigen::MatrixXd pos(3, pt_num);
for (int i = 0; i < pt_num; ++i)
{
pos.col(i) = inter_points[i];
}估算除起点外，各个点从上一个点到该点的所用时长
cpp
Eigen::Vector3d zero(0, 0, 0);
Eigen::VectorXd time(pt_num - 1);
for (int i = 0; i < pt_num - 1; ++i)
{
time(i) = (pos.col(i + 1) - pos.col(i)).norm() / (pp_.max_vel_);
}
// 由于从零要加速，到第一个点的时长*2
time(0) *= 2.0;
time(time.rows() - 1) *= 2.0;