匹配点算法和投影
理论方面可以参考这篇博客讲的十分细致:如何理解Apollo中的横向误差和纵向误差
参考线作为后序规划和控制得基础,必须要生成的足够快。提高速度方法可以:
- 减少规划频率,规划算法每100ms执行一次,控制算法每10ms执行一次
- 充分利用上一周期的规划结果
找匹配点
找到离散轨迹规划点中与车辆真实位置(x,y)最近的点,在Apollo中称之为match_point匹配点。将车辆投影到规划轨迹最近的位置的点称之为投影点。匹配点并不等于投影点,因为匹配点属于离散点,而实际投影是对轨迹做垂线找的最近点。但是可以通过匹配点来估算出投影点。只有当规划的离散轨迹点足够密集的时候,才可以近似将匹配点当作投影点。
由匹配点计算出投影点:
假设:匹配点----> 投影的曲率不变 -----》 匹配------>投影的轨迹近似用圆弧代替。
首先计算出匹配点的切向方向单位向量和垂线方向法向量:
τ
⃗
m
=
(
cos
θ
m
,
sin
θ
m
)
n
⃗
m
=
(
−
sin
θ
m
,
cos
θ
m
)
\vec{\tau}_m\,\,=\,\,\left( \cos \theta _m, \sin \theta _m \right) \\ \vec{n}_m\,\,=\,\,\left( -\sin \theta _m, \cos \theta _m \right)
τm=(cosθm,sinθm)nm=(−sinθm,cosθm)
其中
τ
\tau
τ是切向方向向量,
n
n
n是法向向量。接着还要计算出匹配点指向车辆真实距离(x, y)的矢量:
d
_
v
=
(
x
⃗
−
x
⃗
m
,
y
⃗
−
y
⃗
m
)
d\_v\ =\ \left( \vec{x}\,\,-\,\,\vec{x}_m, \vec{y}\,\,-\,\,\vec{y}_m \right)
d_v = (x−xm,y−ym)
通过上述已知量可以得到投影点方向的s速度:
s
˙
=
d
v
∗
τ
⃗
\dot{s}\,\,=\,\,d_v*\vec{\tau}
s˙=dv∗τ, 由此用下方公式计算出投影点的x,y 坐标:
(
x
r
,
y
r
)
=
(
x
m
,
y
m
)
+
s
˙
⋅
τ
\left( x_r, y_r \right) \,\,=\,\,\left( x_m, y_m \right) \,\,+\,\,\dot{s}\,\,\cdot \,\,\tau
(xr,yr)=(xm,ym)+s˙⋅τ
接着计算投影点的航向角度:
θ
r
=
θ
m
+
k
m
⋅
s
˙
\theta _r\,\,=\,\,\theta _m\,\,+\,\,k_m\,\,\cdot \,\,\dot{s}
θr=θm+km⋅s˙
之前假设曲率不变,因此:
k
r
=
k
m
k_r\,\,=\,\,k_m
kr=km
如何快速找到匹配点,将整个路径点进行遍历找最近点太时间。因此可以用上一个周期的匹配点作为起点做遍历,一旦有
l
i
+
1
>
l
i
l_{i+1} > l_i
li+1>li , 立刻退出遍历,
l
i
l_i
li的点作为匹配点。同时,还要考虑到遍历的方向,因为车会向前走和向后走。
但是这个方法只适用于上一个匹配点附近只有一个极小值点,随便按照100ms规划周期,即使车速很快,车也走不了太远,因此通常上一个规划周期的匹配点附近应该都只有一个极小值点。但是为了保险起见,可以设计一个变量increase count表示 l l l连续新增的次数,当连续新增n次以后,如果还是新增,说明附近只有一个极小值点。
实际代码中,需要区分是否是第一次运行,如果是第一次运行就需要从头开始遍历找匹配点,否则就从上一个规划周期作为起点开始遍历。
i = -1 # 提前定义一个变量用于遍历曲线上的点
input_xy_length = len(xy_list)
frenet_path_length = len(frenet_path_node_list)
match_point_index_list = np.zeros(input_xy_length, dtype="int32")
project_node_list = [] # 最终长度和input_xy_length应该相同
if is_first_run is True:
for index_xy in range(input_xy_length): # 为每一个点寻找匹配点
x, y = xy_list[index_xy]
start_index = 0
# 用increase_count记录distance连续增大的次数,避免多个局部最小值的干扰
increase_count = 0
min_distance = float("inf")
# 确定匹配点
for i in range(start_index, frenet_path_length):
frenet_node_x, frenet_node_y, _, _ = frenet_path_node_list[i]
# 计算(x,y) 与 (frenet_node_x, frenet_node_y) 之间的距离
distance = math.sqrt((frenet_node_x - x) ** 2 + (frenet_node_y - y) ** 2)
if distance < min_distance:
min_distance = distance # 保留最小值
match_point_index_list[index_xy] = i
increase_count = 0
else:
increase_count += 1
if increase_count >= 50: # 向后50个点还没找到的话,说明当前最小值点及时最优的
# 第一次运行阈值较大,是为了保证起始点匹配的精确性
break
# 通过匹配点确定投影点
# xy_list中取出第一个(x,y)的匹配点目标索引
match_point_index = match_point_index_list[0]
x_m, y_m, theta_m, k_m = frenet_path_node_list[match_point_index] # 获取匹配点的信息
d_v = np.array([x - x_m, y - y_m]) # 由匹配点指向(x,y)的矢量。
# tou是指匹配点位置的切向方向向量。
tou_v = np.array([np.cos(theta_m), np.sin(theta_m)])
# 投影点方向的s速度
ds = np.dot(d_v, tou_v)
r_m_v = np.array([x_m, y_m]) # 参考点坐标
# 根据公式计算投影点的位置信息
# 计算投影点坐标,匹配点切向方向的单位向量*d_s
x_r, y_r = r_m_v + ds * tou_v
theta_r = theta_m + k_m * ds # 计算投影点在frenet曲线上切线与X轴的夹角
k_r = k_m # 投影点在frenet曲线处的曲率
# 将结果打包放入缓存区
project_node_list.append((x_r, y_r, theta_r, k_r))
如果不是第一次运算的话,需要计算遍历方向:
start_index = pre_match_index
# 用increase_count记录distance连续增大的次数,避免多个局部最小值的干扰
increase_count = 0
# 上个周期匹配点坐标
pre_match_point_xy = [frenet_path_node_list[start_index][0], frenet_path_node_list[start_index][1]]
pre_match_point_theta_m = frenet_path_node_list[start_index][2]
# 上个匹配点在曲线上的切向向量 , 用来判断方向
pre_match_point_direction = np.array([np.cos(pre_match_point_theta_m), np.sin(pre_match_point_theta_m)])
# 计算上个匹配点指向当前(x, y)的向量
pre_match_to_xy_v = np.array([x - pre_match_point_xy[0], y - pre_match_point_xy[1]])
# 计算pre_match_to_xy_v在pre_match_point_direction上的投影,用于判断遍历方向
# 大于零,说明往前走,小于零就是后退了。
flag = np.dot(pre_match_to_xy_v, pre_match_point_direction) # 大于零正反向遍历,反之,反方向遍历
判断完方向就是正常找匹配点的步骤:
min_distance = float("inf")
if flag > 0:
for i in range(start_index, frenet_path_length):
frenet_node_x, frenet_node_y, _, _ = frenet_path_node_list[i]
# 计算(x,y) 与 (frenet_node_x, frenet_node_y) 之间的距离
distance = math.sqrt((frenet_node_x - x) ** 2 + (frenet_node_y - y) ** 2)
if distance < min_distance:
min_distance = distance # 保留最小值
match_point_index_list[index_xy] = i
increase_count = 0
else:
increase_count += 1
if increase_count >= 5: # 为了加快速度,这里阈值为5,第一个周期是不同的,向后5个点还没找到的话,说明当前最小值点及时最优的
break
else:
for i in range(start_index, -1, -1):
frenet_node_x, frenet_node_y, _, _ = frenet_path_node_list[i]
# 计算(x,y) 与 (frenet_node_x, frenet_node_y) 之间的距离
distance = math.sqrt((frenet_node_x - x) ** 2 + (frenet_node_y - y) ** 2)
if distance < min_distance:
min_distance = distance # 保留最小值
match_point_index_list[index_xy] = i
increase_count = 0
else:
increase_count += 1
if increase_count >= 5:
break
最后是通过匹配点来计算出投影点的(x,y)和 θ r \theta_r θr以及k。
# 通过匹配点确定投影点
match_point_index = match_point_index_list[0]
x_m, y_m, theta_m, k_m = frenet_path_node_list[match_point_index]
d_v = np.array([x - x_m, y - y_m])
tou_v = np.array([np.cos(theta_m), np.sin(theta_m)])
ds = np.dot(d_v, tou_v)
r_m_v = np.array([x_m, y_m])
# 根据公式计算投影点的坐标信息
x_r, y_r = r_m_v + ds * tou_v
theta_r = theta_m + k_m * ds
k_r = k_m
# 将结果打包放入缓存区
project_node_list.append((x_r, y_r, theta_r, k_r))