apollo小知识点——匹配点计算

hua衬衫

已于 2022-07-03 13:56:46 修改

阅读量1k

点赞数

分类专栏：概念文章标签：自动驾驶

于 2022-05-04 22:35:47 首次发布

本文链接：https://blog.youkuaiyun.com/q384515674/article/details/124578756

版权

概念专栏收录该内容

3 篇文章

订阅专栏

本文详细介绍了如何在自动驾驶场景下，根据车辆当前位置和参考路径（reference_line）找到匹配点(match_point)。匹配点定义为参考线上的一点，其切线与车辆到该点的连线垂直。首先，从参考路径中找到最近点A’，然后通过计算车辆位置A到A’切线的投影，确定匹配点的s值。文章提到了两种计算方法，一种是Apollo使用的简便方法，另一种可能涉及更复杂的三角运算。最后，根据s值进行一阶插值，获取match_point的完整信息，包括笛卡尔坐标、曲率和航向角，并可按需转换为Frenet坐标系下的坐标。该过程对于路径规划和决策至关重要。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

问题具体描述

求traj上当前位置在reference_line上的match_point(匹配点)

匹配点定义

位于reference_line上，且在该点处的切线与车辆当前位置和匹配点连成的线相垂直

步骤一、遍历reference_line，取与车辆当前位置点A最近的点A’
步骤二、取A’前后点A_f,A_b,，并将两点的连线看作A’点的切线(也可直接取A’点处的切线)
步骤三、计算车辆当前位置A点到A’切线的投影点的s值
方法1(apollo).计算AA’到A’切线的投影，记投影长度为Δs，s=s_A_f+Δs
方法2.计算AA_f到A’切线的投影，记投影长度为Δs，s=s_A’+Δs
[备注]：方法2的计算相对复杂，需要考虑A’切线与AA_f夹角是钝角还是锐角，这会影响到Δs的正负，而方法1巧妙的规避了这个问题
步骤四、根据s进行一阶插值，计算match_point的剩余信息，包括笛卡尔坐标系下的x，y，曲率以及航向角’'。
[备注]：航向角的计算需要注意角度的归一化(-pi,pi)
步骤五、根据需要，计算match_point的在frenet坐标系下的坐标
一般em_planner(public_road)需要的s,s_dot,s_ddot,L,L’,L’’
lattice_planner 需要 s,s_dot,s_ddot,L,L_dot,L_ddot
其中dot表示对时间t求导，’ 表示对frenet坐标轴S求导