1. 寫在前面

最近在搞一個"大曲率彎道"場景的數據挖掘，里面有個邏輯是給定自車的定位坐標和車道線的坐標點，根據點到線段的距離，去找到自車所在的車道中心線。

然后發現這個計算其實在很多場景中都是可以用到的，所以就想通過一篇文章來整理下這塊的原理和代碼實戰，算是把學校學習的向量知識真正的應用到實戰中。

2. 原理

首先得知道一個點：點到線段最短距離的運算與點到直線的最短距離的運算二者之間存在一定的差別，即求點到線段最短距離時需要考慮參考點在沿線段方向的投影點是否在線段上，若在線段上才可采用點到直線距離公式。

三種情況：

1. 點在投射在線段上， 點到線段的距離 等效 點到直線的距離， 圖(a)
2. 點在線段外， 點到線段的距離為該點到最近短點的距離， 圖(b)(c )
3. 點在線段上， 點到線段的距離為0

參考這篇文章， 介紹3種方法：

3. 實戰

有了理論， 下面給出代碼實戰， 用python實現了兩版。

import numpy as np
import math# 向量之間直接運算
def distance_point_to_segment(point, segment_start, segment_end):"""計算點到線段的最短距離:param point: 點的坐標，形如(x, y):param segment_start: 線段起點坐標，形如(x, y):param segment_end: 線段終點坐標，形如(x, y):return: 最短距離"""segment_vec = segment_end - segment_startpoint_vec = point - segment_startprojection_length = np.dot(point_vec, segment_vec) / np.dot(segment_vec, segment_vec)if projection_length < 0:return np.linalg.norm(point_vec)elif projection_length > 1:return np.linalg.norm(point - segment_end)else:projection = segment_start + projection_length * segment_vecreturn np.linalg.norm(point - projection)# 如果用坐標的計算方式
def distance_point_to_line_segment(point, start, end):"""計算點到線段的距離:param point: 點 (x, y):param start: 線段起點 (x, y):param end: 線段終點 (x, y):return: 點到線段的距離"""px, py = pointsx, sy = startex, ey = end# 計算線段的長度line_length = math.sqrt((ex - sx) ** 2 + (ey - sy) ** 2)if line_length == 0:return math.sqrt((px - sx) ** 2 + (py - sy) ** 2)# 計算點到線段所在直線的投影比例dot_product = ((px - sx) * (ex - sx) + (py - sy) * (ey - sy)) / (line_length ** 2)if dot_product < 0:return math.sqrt((px - sx) ** 2 + (py - sy) ** 2)elif dot_product > 1:return math.sqrt((px - ex) ** 2 + (py - ey) ** 2)else:# 計算投影點的坐標proj_x = sx + dot_product * (ex - sx)proj_y = sy + dot_product * (ey - sy)return math.sqrt((px - proj_x) ** 2 + (py - proj_y) ** 2)# 三個點計算曲率
def get_lane_curve(lane, closest_end_point_idx):point_len = len(lane.geometry.points)point_1 = lane.geometry.points[closest_end_point_idx]point_0 = lane.geometry.points[min(closest_end_point_idx + 12, point_len - 2)]point_2 = lane.geometry.points[max(closest_end_point_idx - 8, 0)]cur_point = np.array((point_1.xyz.x, point_1.xyz.y))points = lane.geometry.points# 向后搜索30米的點(point_0)for i in range(closest_end_point_idx, point_len):next_point = np.array((points[i].xyz.x, points[i].xyz.y))if np.linalg.norm(next_point - cur_point) >= 15:point_0 = points[i]break# 向前搜索30米的點(point_2)for i in range(closest_end_point_idx, -1, -1):next_point = np.array((points[i].xyz.x, points[i].xyz.y))if np.linalg.norm(next_point - cur_point) >= 15:point_2 = points[i]breakpoint_0 = np.array((point_0.xyz.x, point_0.xyz.y))point_1 = np.array((point_1.xyz.x, point_1.xyz.y))point_2 = np.array((point_2.xyz.x, point_2.xyz.y))x0, y0 = point_0x1, y1 = point_1x2, y2 = point_2cross_product = (x1 - x0) * (y2 - y0) - (x2 - x0) * (y1 - y0)if cross_product == 0:curve_radius = float('inf')else:# 計算三邊長度ab = np.linalg.norm(point_1 - point_0)bc = np.linalg.norm(point_1 - point_2)ca = np.linalg.norm(point_0 - point_2)# 計算曲率半徑curve_radius = (ab * bc * ca) / (2 * abs(cross_product))return curve_radius

lua實現了一版

-- 計算點到線段的最短距離
function distance_point_to_segment(point, segment_start, segment_end)local segment_vec = vec_sub(segment_end, segment_start)local point_vec = vec_sub(point, segment_start)local segment_length_sq = vec_dot(segment_vec, segment_vec)if segment_length_sq == 0 thenreturn vec_norm(point_vec)end-- 限制在[0,1]區間local t = vec_dot(point_vec, segment_vec) / segment_length_sqt = math.max(0, math.min(1, t))local projection = vec_add(segment_start, vec_mul_scalar(segment_vec, t))return vec_norm(vec_sub(point, projection))
end

參考資料：

• 數學—點到線段的最短距離
• 可視化學習：利用向量計算點到線段的距離并展示
• 點到線段的距離
• 使用向量的方法計算點到直線的距離(有一些基礎知識)