自动驾驶相关的传感器和坐标系

### 自动驾驶中的Frenet坐标系 #### 使用场景 在自动驾驶领域，Frenet坐标系被广泛应用于路径规划和车辆控制。这种坐标系统能够有效地描述沿道路中心线行驶的车辆位置和姿态，尤其适用于复杂多变的道路环境。相比于传统的笛卡尔坐标系，在处理弯曲路段时，Frenet坐标系可以简化计算过程并提高精度[^1]。 对于动态障碍物避让以及实时调整行车路线而言，基于Frenet框架下的局部路径重规划算法具有显著优势。它允许系统快速响应周围环境变化，并保持较高的安全性和舒适度水平。 #### 实现方法 为了将实际道路上某一点从全球定位系统（GPS）获取到的经纬度数据转换成对应的Frenet坐标表示形式，需经历如下几个关键步骤： ##### 1. 获取参考路径 选取一条代表当前车道的理想化平滑曲线作为参考路径，该路径由一系列离散点组成，这些点可以通过高精地图获得或者通过传感器感知前方路况来构建。 ##### 2. 计算最近距离投影点 找到给定位置P(x,y)最接近于所选参考路径上的那个特定点Q(s,d)，其中s表示沿着路径测量的距离参数；d则衡量横向偏移量即垂直方向偏离程度。此操作可通过最小二乘法或其他优化技术完成求解。 ##### 3. 转换至Frenet空间 一旦确定了上述两个重要特征值s与d之后，则可利用相应变换矩阵把原始直角坐标映射为新的表达方式[(s, d)]。具体来说就是先求得切向量T和平面内单位正交基底{e₁,e₂}，再据此建立联系两者之间的关系式子[^2]。 ```python import numpy as np def cartesian_to_frenet(px, py, path_x, path_y): """ Convert Cartesian coordinates to Frenet frame. Parameters: px (float): Point's X coordinate in global frame. py (float): Point's Y coordinate in global frame. path_x (list of float): List containing reference path points' X coordinates. path_y (list of float): List containing reference path points' Y coordinates. Returns: tuple: A pair representing the point’s position along and across the road respectively. """ min_dist_sq = float('inf') closest_idx = None for i in range(len(path_x)): dist_sq = (px - path_x[i])**2 + (py - path_y[i])**2 if dist_sq < min_dist_sq: min_dist_sq = dist_sq closest_idx = i s = sum([np.sqrt((path_x[j]-path_x[j-1])**2+(path_y[j]-path_y[j-1])**2) for j in range(1,closest_idx+1)]) dx = px - path_x[closest_idx] dy = py - path_y[closest_idx] heading_angle = np.arctan2(path_y[closest_idx+1]-path_y[closest_idx], path_x[closest_idx+1]-path_x[closest_idx]) trans_matrix = [[np.cos(-heading_angle), -np.sin(-heading_angle)], [np.sin(-heading_angle), np.cos(-heading_angle)]] local_coords = np.dot(trans_matrix,[dx,dy]) return (s,local_coords[0]) # Return longitudinal distance 's', lateral offset 'd' ``` 尽管如此，值得注意的是，当涉及到非常规形状或急转弯处时，由于存在较大的误差累积风险，因此可能需要额外考虑其他因素来进行补偿修正措施[^3]。