carsim/trucksim 自动泊车场景 45度平行车位自动泊车场景 45度平行车位
停车场里歪着停的45度平行车位简直是新手司机的噩梦——方向盘该往哪边打？回正时机怎么判断？但这对自动驾驶系统来说却是必须攻克的标准化场景。今天咱们就手搓一套基于Carsim的自动泊车方案，用代码教车自己钻进去。
先看场景建模：45度斜车位宽度2.8米（比车宽多60cm），前后车距3.5米。别小看这个斜角，传统垂直泊车的路径规划直接歇菜。咱们祭出Hybrid A*算法搞路径搜索：
def hybrid_a_star(start, goal): motion_primitives = [ (2.0, 0), # 直行2米 (1.5, 30), # 左转30度行驶 (1.5, -30) # 右转30度行驶 ] open_set = PriorityQueue() open_set.put(start) while not open_set.empty(): current = open_set.get() if distance(current, goal) < 0.2: return reconstruct_path(current) for dx, steer in motion_primitives: next_node = apply_steering(current, dx, steer) if collision_check(next_node): continue cost = current.cost + dx + abs(steer)*0.1 # 转向惩罚项 if next_node not in visited: visited.add(next_node) open_set.put(next_node)这段代码的精髓在运动基元设计——用三个基础动作组合出复杂路径。转向惩罚项（abs(steer)*0.1）能避免车辆像醉汉一样来回扭方向。碰撞检测函数里需要处理车体包络线计算，特别是当车辆斜向移动时的外摆量。
路径有了，接下来是控制层。传统PID在低速大转角场景容易振荡，改用Stanley控制器：
class StanleyController: def __init__(self, k=0.3): self.k = k # 转向增益 def update(self, path, current_pose, velocity): nearest_idx = self.find_nearest_point(path, current_pose) # 计算横向误差 dx = path[nearest_idx].x - current_pose.x dy = path[nearest_idx].y - current_pose.y cross_track_error = dx * sin(current_pose.yaw) - dy * cos(current_pose.yaw) # 航向角误差 yaw_error = path[nearest_idx].yaw - current_pose.yaw yaw_error = atan2(sin(yaw_error), cos(yaw_error)) # 归一化到[-pi, pi] # Stanley公式 steer = yaw_error + atan2(self.k * cross_track_error, velocity + 1e-5) return np.clip(steer, -30, 30) # 限制最大转角这里的速度项处理很关键——低速时放大横向误差的影响，防止停车瞬间方向失控。实测中发现当车速低于0.5m/s时，需要额外增加积分项来消除静态误差。
carsim/trucksim 自动泊车场景 45度平行车位自动泊车场景 45度平行车位
在Carsim中搭建测试场景时，注意设置地面摩擦系数为0.8-1.0（模拟沥青路面），方向盘传动比设为16:1。建议用S-Function接口连接控制算法，采样时间不要超过50ms。
跑仿真时常见翻车现场：车辆在入库最后阶段车尾扫到旁车。这时候得检查路径曲率是否连续，或者在控制层添加预测模块：
def predict_pose(current_pose, steer, velocity, dt=0.1): beta = np.arctan(0.5 * np.tan(np.radians(steer))) # 简化自行车模型 dx = velocity * np.cos(current_pose.yaw + beta) * dt dy = velocity * np.sin(current_pose.yaw + beta) * dt dyaw = (velocity / 2.8) * np.tan(np.radians(steer)) * dt # 轴距2.8米 return Pose(current_pose.x+dx, current_pose.y+dy, current_pose.yaw+dyaw)这个预测函数虽然简化，但足够在0.5秒内预判车尾位置。结合实时碰撞检测，能在距离旁车30cm时触发紧急制动。
调参时记住：转向增益与车速成反比，在2m/s时设为0.3，1m/s时调到0.5效果更佳。最后验收标准可不是能停进去就行——全程方向变化次数不超过5次，最大横向加速度必须小于0.3g，否则乘客得吐在车里。
