ego(7)---为Astar路径点计算交点，避障基准点与排斥方向

在 ego-planner 中，在进行了之前的初始轨迹采样，Astar 绕障轨迹，障碍物边界的二次计算后，会有两组点，一组是二次计算后的障碍物边界轨迹点，一组是 Astar 绕障轨迹点，(可以查看之前的 ego 文章)。

初始轨迹采样：https://blog.csdn.net/a_xiaoning/article/details/151158675?spm=1001.2014.3001.5501

Astar绕障碍：https://blog.csdn.net/a_xiaoning/article/details/151722705?spm=1001.2014.3001.5501

障碍物边界二次计算：https://blog.csdn.net/a_xiaoning/article/details/151800448?spm=1001.2014.3001.5501

为了给避障留有足够的空间，ego 的做法是会将 Astar 的路径控制点进一步推离障碍物。

计算交点，避障基准点与排斥方向的步骤

障碍段轨迹点初始标记

首先，进行障碍段轨迹点的初始标记，这个障碍段是指在上一步进行了障碍物边界点扩张后的点，在 ego 中，存储在 final_segment_ids 中，对应上示意图中的绿色点：

      // final_segment_ids 为当前障碍物段的所有可调控制点  cps_是所有轨迹控制点  在初始时，会将所有可调控制点标记为 falsefor (int j = final_segment_ids[i].first; j <= final_segment_ids[i].second; ++j)cps_.flag_temp[j] = false;

计算交点，避障基准点与扩张方向

1. 首先计算当前轨迹点的切线方向，也就是向量上一个轨迹点->下一个轨迹点，记作 ctrl_pts_law：

// 计算控制点 Qj 的法向方向:Q(j-1) -> Q(j+1) 的垂直方向
Eigen::Vector3d ctrl_pts_law(cps_.points.col(j + 1) - cps_.points.col(j - 1)), intersection_point;

2. 从 Astar 路径的中点开始搜索垂直于向量 ctrl_pts_law 的法线向量，这里使用了向量点积的概念。

可以回顾一下点积的概念，结合一下 ego 的场景，两个向量点积就是一个向量在另一个向量的投影，当点积 > 0 时，说明两个向量同向，点积 < 0 时，说明两个向量反向。

那么在 ego 中，就利用这个性质，来在 Astar 的路径点中，寻找与当前控制点的切线向量垂直的控制点(也就是点积异号的时候)：

// 从 Astar 路径中点开始搜索
int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation
// 计算 Astar 路径与初始 B 样条控制点方向的点积，用以确定寻找 Astar 路径点的方向
// 点积的意义表示 Astar 路径上某点与控制点控制点Qj组成的向量 在 ctrl_pts_law 上的投影，如果 val > 0 则表示与 ctrl_pts_law 同向，否则反向
double val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law), last_val = val;// 沿 Astar 路径搜索，找到与 ctrl_pts_law 方向的交点(即障碍物基准点方向)
while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size())
{last_Astar_id = Astar_id;if (val >= 0)--Astar_id;else++Astar_id;val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law);// 检测到交点（点积符号变化，说明跨越ctrl_pts_law方向）if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed{

3. 在找到 Astar 对应的交点标号后，再在两个交点之间进行插值计算准确的交点坐标：

  // 检测到交点（点积符号变化，说明跨越ctrl_pts_law方向）if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed{// 线性插值计算精确交点坐标（intersection_point）intersection_point =a_star_pathes[i][Astar_id] +((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) *(ctrl_pts_law.dot(cps_.points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t);//cout << "i=" << i << " j=" << j << " Astar_id=" << Astar_id << " last_Astar_id=" << last_Astar_id << " intersection_point = " << intersection_point.transpose() << endl;got_intersection_id = j;  // 标记找到交点的控制点break;}

4. 基于交点坐标计算障碍物基准点与排斥方向。

障碍物基准点：就是在控制点与交点连线上，离障碍物最近的点，在寻找时，使用障碍物网格地图的分辨率在连线上逐次寻找即可

排斥方向：向量 控制点->交点

// 基于交点计算障碍物基准点与排斥方向
if (got_intersection_id >= 0)
{cps_.flag_temp[j] = true;   // 标记该控制点已分配约束double length = (intersection_point - cps_.points.col(j)).norm(); // 控制点与交点的距离if (length > 1e-5){// 在控制点与交点的连线上寻找离障碍物最近的安全点 根据网格地图的分辨率来逐次移动for (double a = length; a >= 0.0; a -= grid_map_->getResolution()){// 在控制点与交点的连线上寻找离障碍物最近的安全点occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));if (occ || a < grid_map_->getResolution()){// 若安全点在障碍物内，则回退一个栅格分辨率if (occ)a += grid_map_->getResolution();// 基准点：避开障碍物的最近安全点cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));// 排斥方向：从控制点指向基准点（归一化）cps_.direction[j].push_back((intersection_point - cps_.points.col(j)).normalized());break;}}}
}

5.循环计算在障碍物段内所有轨迹控制点对应的障碍物基准点与排斥方向

计算不出来的点直接分配

为剩下计算不出来交点，障碍物基准点，排斥方向的控制点分配基准点与排斥方向。这个逻辑也比较简单，直接向前或向后传递即可：

//step 3
// 为未直接分配约束的控制点“传递”约束（确保区间内所有控制点有排斥信息）
if (got_intersection_id >= 0)
{
// 向后传递（j从got_intersection_id+1到区间上界）
for (int j = got_intersection_id + 1; j <= final_segment_ids[i].second; ++j)if (!cps_.flag_temp[j]){// 继承前一个控制点的基准点和排斥方向cps_.base_point[j].push_back(cps_.base_point[j - 1].back());cps_.direction[j].push_back(cps_.direction[j - 1].back());}
// 向前传递（j从got_intersection_id-1到区间下界）
for (int j = got_intersection_id - 1; j >= final_segment_ids[i].first; --j)if (!cps_.flag_temp[j]){// 继承后一个控制点的基准点和排斥方向cps_.base_point[j].push_back(cps_.base_point[j + 1].back());cps_.direction[j].push_back(cps_.direction[j + 1].back());}
}
else
{
// Just ignore, it does not matter ^_^.
// ROS_ERROR("Failed to generate direction! segment_id=%d", i);
}