概要
0.基于匹配点求解投影点的位姿信息步骤
1.首先对规划轨迹点结构内容的了解
主要是PathPoint（路经点的位姿），TrajectoryPoint结构体内容。
syntax = "proto2"; // Defined Point types that are commonly used in PnC (Planning and Control) // modules. package apollo.common; message SLPoint { optional double s = 1; optional double l = 2; } message FrenetFramePoint { optional double s = 1; optional double l = 2; optional double dl = 3; optional double ddl = 4; } message SpeedPoint { optional double s = 1; optional double t = 2; // speed (m/s) optional double v = 3; // acceleration (m/s^2) optional double a = 4; // jerk (m/s^3) optional double da = 5; } message PathPoint { // coordinates optional double x = 1; optional double y = 2; optional double z = 3; // direction on the x-y plane optional double theta = 4; // curvature on the x-y planning optional double kappa = 5; // accumulated distance from beginning of the path optional double s = 6; // derivative of kappa w.r.t s. optional double dkappa = 7; // derivative of derivative of kappa w.r.t s. optional double ddkappa = 8; // The lane ID where the path point is on optional string lane_id = 9; // derivative of x and y w.r.t parametric parameter t in CosThetareferenceline optional double x_derivative = 10; optional double y_derivative = 11; } message Path { optional string name = 1; repeated PathPoint path_point = 2; } message TrajectoryPoint { // path point optional PathPoint path_point = 1; // linear velocity optional double v = 2; // in [m/s] // linear acceleration optional double a = 3; // relative time from beginning of the trajectory optional double relative_time = 4; // longitudinal jerk optional double da = 5; // The angle between vehicle front wheel and vehicle longitudinal axis optional double steer = 6; // Gaussian probability information optional GaussianInfo gaussian_info = 7; }相对路经：modules/common_msgs/basic_msgs/pnc_point.proto
2. 计算匹配点主要涉及的函数：
1) 函数名: MatchToPath(const std::vector< PathPoint >& reference_line, const double x, const double y)
函数位置: modules/common/math/path_matcher.cc
函数作用: 在参考线上计算匹配点
2) 函数名: FindProjectionPoint(const PathPoint& p0,const PathPoint& p1, const double x,const double y)
函数位置: modules/common/math/path_matcher.cc
函数作用: 从自车当前点做投影到参考线上,找到在参考线上投影的精确位置
3) 函数名: InterpolateUsingLinearApproximation(const PathPoint &p0, const PathPoint &p1,const double s)
函数位置: modules/common/math/linear_interpolation.cc
函数作用: 使用线性插值的办法来计算投影得到的点在参考线上的x,y,heading,k等一系列值
4) 函数名: slerp(const double a0, const double t0, const double a1, const double t1, const double t)
函数位置: modules/common/math/linear_interpolation.cc
函数作用: 核心还是用的线性插值,不过对角度做了处理,保证在[-pi,pi)之间
5) 函数名: lerp(const T &x0, const double t0, const T &x1, const double t1, const double t)
函数位置: modules/common/math/linear_interpolation.h
函数作用: 线性差值
6) 函数名: NormalizeAngle(const double angle)
函数位置: modules/common/math/math_utils.cc
函数作用: 对角度处理使得在[-pi,pi)之间
函数调用架构流程
函数模块化解析
1.lattice_planner.cc中代码部分
// 在参考线上计算匹配点, 输入参数:参考线,起点x,起点y, 相当于说在参考线上进行遍历找到一点,该点离我当前轨迹上车辆所在位置点的距离是最短的,即frenet公式推导中的L最短 // 2. compute the matched point of the init planning point on the reference // line. PathPoint matched_point = PathMatcher::MatchToPath( *ptr_reference_line, planning_init_point.path_point().x(), planning_init_point.path_point().y());2. 函数-MatchToPath
// 在参考线上计算匹配点 PathPoint PathMatcher::MatchToPath(const std::vector<PathPoint>& reference_line, const double x, const double y) { CHECK_GT(reference_line.size(), 0); // 定义了一个内部的函数,用于计算距离 auto func_distance_square = [](const PathPoint& point, const double x, const double y) { double dx = point.x() - x; double dy = point.y() - y; return dx * dx + dy * dy; }; // 首先计算了一下参考线的起点到traj上当前车辆位置的距离 double distance_min = func_distance_square(reference_line.front(), x, y); std::size_t index_min = 0; // 不仅要知道大小,还要知道索引 // 从index为1的点开始(因为index=0的点前面刚计算过),依次计算和车辆位置的距离 for (std::size_t i = 1; i < reference_line.size(); ++i) { double distance_temp = func_distance_square(reference_line[i], x, y); // 找到参考线上距离自车当前位置最近的点的index if (distance_temp < distance_min) { distance_min = distance_temp; index_min = i; } } // index_min == 0,意味着最近的点是referenceLine的起点,不等于0就 index_min - 1, std::size_t index_start = (index_min == 0) ? index_min : index_min - 1; // 判断index_min+1的值是否等于参考线的size,否的话 index_end=index_min+1 std::size_t index_end = (index_min + 1 == reference_line.size()) ? index_min : index_min + 1; // 到这里找下来相当于在referenceLine上找了三个点,即参考线上距离车辆位置最近的点,以及这个点 // 的前后各一个点,为什么要在前后各找一个点呢?因为他要做车辆当前的点 在referenceLine上的投影, // 这里会把index_start和index_end连起来,当作一条线(方向start指向end),供我们做投影,公式为: // 向量A(index_start -> traj上的车辆当前位置) 点乘 向量B(index_start -> index_end) if (index_start == index_end) { return reference_line[index_start]; } // 做投影,向量A(index_start -> traj上的车辆当前位置) 点乘 向量B(index_start -> index_end) return FindProjectionPoint(reference_line[index_start], reference_line[index_end], x, y); }3.函数-FindProjectionPoint
为什么需要在邻近点附近向后找一个点index_end和向邻近点前面找一个点index_start?
主要便于计算出向量start->vehicle_position 在 向量start->end 上的投影，从而便于计算出邻近点到投影点的位姿信息。
PathPoint PathMatcher::FindProjectionPoint(const PathPoint& p0, const PathPoint& p1, const double x, const double y) { // 为什么投影用的 向量start->vehicle_position 在 向量start->end 上的投影而不用 // 向量index_min->vehicle_position 在 向量start->end 上的投影呢? //主要原因：保证求得的投影距离始终是一个正值，向量start->vehicle_position 在 向量start->end 作点积，只有夹脚是锐角，点积后的结果才是正值；如果向量index_min->vehicle_position 在 向量start->end 上点积，则需要考虑，vehicle_position在index_min之前，或者之后二者向量的夹角是锐角还是钝角，增加计算复杂度，使用前者可始终保证夹角是锐角。 // 详见参考:https://www.bilibili.com/video/BV1rM4y1g71p?spm_id_from=333.999.0.0 //计算车辆当前点与邻近点前一个点的向量的分量 double v0x = x - p0.x(); double v0y = y - p0.y(); //计算车辆邻近点后一个点与邻近点前一个点的向量的分量 double v1x = p1.x() - p0.x(); double v1y = p1.y() - p0.y(); //计算车辆邻近点后一个点与邻近点前一个点的向量的模 double v1_norm = std::sqrt(v1x * v1x + v1y * v1y); //计算车辆邻近点后一个点与邻近点前一个点的向量的点积 double dot = v0x * v1x + v0y * v1y; // v0 · v1 = ||v0||*||v1||*cos(theta) theta为v0和v1的夹角，此夹角未知，但向量vo和v1已知。 // (v0 · v1) / ||v1|| = ||v0||*cos(theta) // ||v0||*cos(theta) 即为v0在v1上的投影的大小 double delta_s = dot / v1_norm; // 这个函数主要使用线性插值的办法来计算投影得到的点在参考线上的坐标,heading等一系列值； //p0.s(),为从参考线的起点开始到p0这个点的距离,p0.s() + delta_s就是v0在参考线上的投影点到参考线起点的距离 return InterpolateUsingLinearApproximation(p0, p1, p0.s() + delta_s); }4.函数-InterpolateUsingLinearApproximation
PathPoint InterpolateUsingLinearApproximation(const PathPoint &p0, const PathPoint &p1, const double s) { double s0 = p0.s(); double s1 = p1.s(); PathPoint path_point; //关于s0和s1以及s可以参考概述中 0的图 s0=p0_s,s1=p1_s,s=d_s+p0_s double weight = (s - s0) / (s1 - s0); // 计算投影点的权重，也即投影比 // 计算投影点位姿信息公式: x = p0.x + weight * (p1.x - p0.x), 下面他用的是合并同类项之后的,都一样 //其中p0.x为index_start的坐标信息 double x = (1 - weight) * p0.x() + weight * p1.x(); double y = (1 - weight) * p0.y() + weight * p1.y(); // 核心还是用的线性插值,不过对角度做了处理,保证在[-pi,pi)之间 double theta = slerp(p0.theta(), p0.s(), p1.theta(), p1.s(), s); double kappa = (1 - weight) * p0.kappa() + weight * p1.kappa(); double dkappa = (1 - weight) * p0.dkappa() + weight * p1.dkappa(); double ddkappa = (1 - weight) * p0.ddkappa() + weight * p1.ddkappa(); //把计算得到的投影点位姿信息写入到路经点的结构体中 path_point.set_x(x); path_point.set_y(y); path_point.set_theta(theta); path_point.set_kappa(kappa); path_point.set_dkappa(dkappa); path_point.set_ddkappa(ddkappa); path_point.set_s(s); return path_point; }5. 函数-slerp，对航向角进行归一化处理，保证在[-pi,pi)
double slerp(const double a0, const double t0, const double a1, const double t1, const double t) { // start和end两个点距离太近 if (std::abs(t1 - t0) <= kMathEpsilon) { ADEBUG << "input time difference is too small"; return NormalizeAngle(a0); } // 对两个heading角处理使得在[-pi,pi)之间 const double a0_n = NormalizeAngle(a0); const double a1_n = NormalizeAngle(a1); // 得到delta(heading) double d = a1_n - a0_n; if (d > M_PI) { d = d - 2 * M_PI; } else if (d < -M_PI) { d = d + 2 * M_PI; } const double r = (t - t0) / (t1 - t0); const double a = a0_n + d * r; return NormalizeAngle(a); }6.函数-NormalizeAngle
double NormalizeAngle(const double angle) { // 这一步取余的操作可以保证结果在(-2pi,2pi)范围内 double a = std::fmod(angle + M_PI, 2.0 * M_PI); // 这个保证结果在(0,2pi)范围内 if (a < 0.0) { a += (2.0 * M_PI); } // 这个保证结果在(-pi,pi)范围内 return a - M_PI; }
技术细节
1.关于邻近点的前一个点和后一个点的选取，一般前一个点也即index_start = 0;后一个点end_start=1;根据这两个点，做投影，找到投影点，并根据投影比计算出投影点的位姿信息，作为规划的参考点信息。
2.计算出的参考点的航向角，记得做角度归一化处理，避免出现大角度,导致计算异常，控制在[-pi,pi）
3.计算投影点的长度（投影比）要用邻近点前一个点index_start->vehicle_position连线的向向量在index_start->index_end连线的投影，而非邻近点->vehicle_position连线的向向量在index_start->index_end连线的投影！
4.控制最终使用的不是匹配点，而是投影点。
补充：关于匹配点和投影点的关系
车辆横向误差微分方程(look：车辆横向误差微分方程的推导)：
如何选择投影点：
上层轨迹规划模块给控制模块的轨迹为离散点轨迹，而不是连续的。
匹配点的寻找与定义：
控制使用的是基于匹配点计算后的投影点的位姿信息！！！
详细参考博主：百度阿波罗（APOLLO）自动驾驶项目中的横向误差与纵向误差的计算方法该怎么理解？ - 知乎
小结
参考博客：
https://blog.csdn.net/weixin_44558122/article/details/124585202
866
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
