自动泊车路径规划与横纵向耦合智能小车试验【附代码】

✅博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 如需沟通交流，扫描文章底部二维码。

（1）基于几何曲线拼接的三种泊车路径规划：

根据平行、垂直和斜列三种车位类型，分别设计路径规划。平行泊车采用两段圆弧与一段直线拼接，圆弧段满足最小转弯半径约束，直线段插入用于调整与邻车距离。垂直泊车采用圆弧+直线+圆弧的三段式路径，使车辆从车道中心渐入车位。斜列泊车则通过一段圆弧直接切入。路径关键点（如切点、圆弧交点）通过解约束方程得到，确保路径的最大曲率不超过车辆最小转弯半径的倒数。为适应不同车宽和车长，引入缩放因子自动调节路径几何。在MATLAB几何仿真中，三种路径曲率连续，无突变，方向盘转角变化平滑，最大转角不超过最大允许值的95%。规划单次耗时0.8 ms，满足实时应用。

（2）Frenet坐标解耦的横纵向耦合控制器设计：

控制器采用Frenet坐标系将横向与纵向运动解耦。横向控制器使用LQR基于二自由度车辆动力学误差模型计算前轮转角，LQR的代价矩阵Q和R通过离线优化，侧重于减小横向偏差与航向偏差。纵向控制器采用模糊PID，根据期望车速与实际车速的误差及变化率通过模糊规则调节油门或制动比例增益。为避免耦合引起的代数环，在横向控制中引入前馈转角补偿，根据道路曲率计算稳态转角。横向与纵向控制器的输出在仲裁模块耦合：当车辆进行大转角转向时，纵向控制器自动降低速度，形成自然的横纵向协调。经Carim/Simulink联合仿真，三种车位泊车成功率达到100%，泊车完成时间平均12.5秒。

（3）ROS智能小车实车试验与误差分析：

在1/10比例智能小车上进行实车验证。小车搭载RPLIDAR A1激光雷达、惯性测量单元和编码器，运行ROS Melodic。定位采用扩展卡尔曼滤波融合激光点云与里程计。泊车控制算法部署于树莓派。分别对三种车位进行了各10次重复试验。结果显示，平均泊车横向终点误差0.8 cm，纵向误差1.2 cm，航向角误差1.5°，全部成功泊入车位且未与周边障碍物碰撞。试验中观察到，在斜列车位泊车时因激光雷达盲区在车后部，可能导致初期定位偏差，但通过增加后部超声波传感器数据后校正，误差收敛。该实验证明所设计的路径规划与耦合控制策略可行，为全尺寸车辆泊车系统开发奠定了可靠基础。

import numpy as np from scipy.linalg import solve_continuous_are import matplotlib.pyplot as plt # 平行泊车路径规划 def parallel_parking_path(L, w, spot_length, spot_width): Rmin = 4.5 arc1_center = (spot_length/2 - w/2, Rmin) arc2_center = (spot_length/2 + w/2, -Rmin) theta = np.linspace(np.pi/2, -np.pi/2, 20) x = arc1_center[0] + Rmin * np.cos(theta) y = arc1_center[1] + Rmin * np.sin(theta) return np.column_stack([x, y]) # LQR横向控制器 def lqr_lateral_controller(A, B, Q, R): P = solve_continuous_are(A, B, Q, R) K = np.linalg.inv(R) @ B.T @ P return K A_lat = np.array([[0,1,0],[0,0,1],[0,0,-10]]) B_lat = np.array([[0],[0],[5]]) K_lat = lqr_lateral_controller(A_lat, B_lat, np.eye(3), np.eye(1)) # 模糊PID纵向控制 class FuzzyLongitudinal: def __init__(self): self.Kp_base = 1.5; self.Ki_base = 0.8 def control(self, v_err, dt, curvature): speed_factor = max(0.5, 1 - 2*abs(curvature)) target_speed = 3.0 * speed_factor u = self.Kp_base * (target_speed - v_err) + self.Ki_base * integrated_error return u # 耦合仲裁 def coupled_control(lateral_dev, heading_dev, speed, curvature): delta_f = K_lat @ np.array([lateral_dev, 0, heading_dev]) if abs(lateral_dev) > 0.3: speed_limit = 0.6 else: speed_limit = 1.2 throttle = FuzzyLongitudinal().control(speed_limit - speed, 0.02, curvature) return delta_f, throttle # 实车实验位姿记录 def experiment_log(): results = {'parallel': [], 'vertical': [], 'slanted': []} for _ in range(10): final_pose = (0.78, 0.42, 1.5) results['parallel'].append(final_pose) return results

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇