第13章-运动规划与轨迹生成

第13章 运动规划与轨迹生成免责声明本文档为学术研究与技术学习目的而编写基于Autoware开源项目Apache 2.0许可证的源码分析。文档内容力求准确但不保证完全无误仅供参考。读者在实际应用时应以官方文档和源码为准。本文档不涉及任何商业用途所有代码示例均来自开源项目。如有侵权请联系删除。摘要运动规划Motion Planning是Autoware自动驾驶系统规划模块的最底层负责将行为决策转换为车辆可执行的平滑轨迹。本章深入解析运动规划模块的架构设计重点介绍轨迹生成算法样条曲线、优化方法、碰撞检测与避障机制、路径平滑优化技术、速度规划方法、特殊场景的规划策略以及紧急停车规划。运动规划直接影响自动驾驶的舒适性、安全性和效率是实现高质量自动驾驶体验的关键环节。目录13.1 运动规划架构13.1.1 运动规划在规划层级中的定位13.1.2 核心模块组成13.1.3 数据流与接口13.2 轨迹生成算法13.2.1 样条曲线插值13.2.2 贝塞尔曲线生成13.2.3 优化方法13.3 碰撞检测与避障13.3.1 碰撞检测算法13.3.2 安全距离计算13.3.3 动态避障策略13.4 路径平滑优化13.4.1 平滑算法13.4.2 曲率约束13.4.3 舒适性优化13.5 速度规划13.5.1 速度曲线生成13.5.2 加速度约束13.5.3 时间最优规划13.6 特殊场景规划13.6.1 狭窄空间规划13.6.2 倒车规划13.6.3 复杂路口规划13.7 紧急停车规划13.7.1 紧急制动轨迹13.7.2 最小停车距离计算13.7.3 紧急避让规划参考资料13.1 运动规划架构13.1.1 运动规划在规划层级中的定位运动规划是规划系统的执行层负责生成精确的、可执行的车辆轨迹。规划层级中的位置任务规划Mission Planning ↓ 全局路径Route 行为规划Behavior Planning ↓ 行为路径Path with Lane ID 运动规划Motion Planning—— 精确轨迹生成 ↓ 轨迹Trajectory 车辆控制Vehicle Control核心职责轨迹生成将离散路径点转换为平滑连续轨迹碰撞检测确保轨迹不与障碍物碰撞速度规划为轨迹点分配合理的速度和加速度约束满足满足车辆运动学和动力学约束13.1.2 核心模块组成源码路径:universe/autoware_universe/planning/motion_planning/主要包含以下模块1. Obstacle Avoidance Planner障碍物避障规划器// obstacle_avoidance_planner/src/node.cppnamespaceobstacle_avoidance_planner{classObstacleAvoidancePlannerNode:publicrclcpp::Node{public:ObstacleAvoidancePlannerNode(constrclcpp::NodeOptionsoptions):Node(obstacle_avoidance_planner,options){// 订阅行为路径sub_path_create_subscriptionPath(/planning/scenario_planning/lane_driving/behavior_planning/path,1,std::bind(ObstacleAvoidancePlannerNode::onPath,this,_1));// 订阅障碍物sub_objects_create_subscriptionPredictedObjects(/perception/object_recognition/objects,1,std::bind(ObstacleAvoidancePlannerNode::onObjects,this,_1));// 发布优化后的轨迹pub_trajectory_create_publisherTrajectory(/planning/scenario_planning/trajectory,1);// 初始化优化器optimizer_std::make_sharedTrajectoryOptimizer();}voidonPath(constPath::SharedPtr path_msg){// 1. 路径预处理autopreprocessed_pathpreprocessPath(*path_msg);// 2. 生成初始轨迹autoinitial_trajectorygenerateInitialTrajectory(preprocessed_path);// 3. 碰撞检测autocollision_pointsdetectCollisions(initial_trajectory,current_objects_);// 4. 轨迹优化autooptimized_trajectoryoptimizer_-optimize(initial_trajectory,collision_points);// 5. 速度规划autofinal_trajectoryplanVelocity(optimized_trajectory);// 6. 发布轨迹pub_trajectory_-publish(final_trajectory);}private:std::shared_ptrTrajectoryOptimizeroptimizer_;PredictedObjects current_objects_;};}// namespace obstacle_avoidance_planner2. Path Smoother路径平滑器使用样条曲线或优化方法平滑路径。3. Velocity Smoother速度平滑器确保速度曲线平滑且满足加速度约束。13.1.3 数据流与接口输入接口订阅Topic:/planning/scenario_planning/lane_driving/behavior_planning/path(Path) - 行为路径/perception/object_recognition/objects(PredictedObjects) - 动态障碍物/localization/kinematic_state(Odometry) - 车辆状态发布Topic:/planning/scenario_planning/trajectory(Trajectory) - 最终轨迹Trajectory消息格式# autoware_planning_msgs/msg/Trajectory.msgstd_msgs/Header header TrajectoryPoint[]points---# TrajectoryPoint.msggeometry_msgs/Pose pose float64 longitudinal_velocity_mps float64 lateral_velocity_mps float64 acceleration_mps2 float64 heading_rate_rps float64 time_from_start13.2 轨迹生成算法13.2.1 样条曲线插值样条曲线是最常用的轨迹平滑方法能生成C2连续的平滑曲线。三次样条插值std::vectorTrajectoryPointgenerateSplineTrajectory(conststd::vectorPathPointpath_points)const{// 1. 提取路径点坐标std::vectordoublex_points,y_points,s_points;doubles0.0;for(size_t i0;ipath_points.size();i){x_points.push_back(path_points[i].pose.position.x);y_points.push_back(path_points[i].pose.position.y);s_points.push_back(s);if(i0){scalculateDistance(path_points[i-1].pose.position,path_points[i].pose.position);}}// 2. 构建样条曲线SplineInterpolatorspline_x(s_points,x_points);SplineInterpolatorspline_y(s_points,y_points);// 3. 在样条曲线上重采样std::vectorTrajectoryPointtrajectory;doubleds0.1;// 采样间隔0.1米for(doubles_interp0;s_interps_points.back();s_interpds){TrajectoryPoint point;point.pose.position.xspline_x.interpolate(s_interp);point.pose.position.yspline_y.interpolate(s_interp);// 计算航向角doubledxspline_x.derivative(s_interp);doubledyspline_y.derivative(s_interp);doubleyawstd::atan2(dy,dx);point.pose.orientationcreateQuaternionFromYaw(yaw);// 计算曲率doubleddxspline_x.secondDerivative(s_interp);doubleddyspline_y.secondDerivative(s_interp);doublecurvature(dx*ddy-dy*ddx)/std::pow(dx*dxdy*dy,1.5);trajectory.push_back(point);}returntrajectory;}13.2.2 贝塞尔曲线生成贝塞尔曲线适用于车道变换等需要明确控制点的场景。TrajectoryPointevaluateBezier(conststd::vectorPointcontrol_points,doublet)const{// 使用De Casteljau算法计算贝塞尔曲线点autopointscontrol_points;intnpoints.size()-1;for(intk1;kn;k){for(inti0;in-k;i){points[i].x(1-t)*points[i].xt*points[i1].x;points[i].y(1-t)*points[i].yt*points[i1].y;}}TrajectoryPoint result;result.pose.position.xpoints[0].x;result.pose.position.ypoints[0].y;returnresult;}13.2.3 优化方法基于优化的轨迹生成可以同时考虑多个约束条件。优化目标minimize: w1 * smoothness w2 * deviation w3 * collision_cost subject to: - curvature max_curvature - lateral_offset max_offset - collision_free13.3 碰撞检测与避障13.3.1 碰撞检测算法boolcheckCollision(constTrajectoryPointpoint,constPredictedObjectobject)const{// 1. 获取车辆轮廓autovehicle_polygongetVehicleFootprint(point.pose);// 2. 获取障碍物轮廓考虑预测位置autoobject_polygongetObjectFootprint(object,point.time_from_start);// 3. 膨胀安全边界autoinflated_vehicleinflatePolygon(vehicle_polygon,safety_margin_);// 4. 检查多边形相交returnpolygonsIntersect(inflated_vehicle,object_polygon);}13.3.2 安全距离计算根据速度动态调整安全距离doublecalculateSafetyMargin(doublevelocity)const{// 基础安全距离doublebase_margin1.0;// 1米// 速度相关的额外距离doublevelocity_marginvelocity*time_headway_;// 通常2秒// 最小和最大限制doubletotal_marginbase_marginvelocity_margin;returnstd::clamp(total_margin,min_margin_,max_margin_);}13.3.3 动态避障策略当检测到碰撞风险时采取避障措施。13.4 路径平滑优化13.4.1 平滑算法移动平均平滑std::vectorTrajectoryPointsmoothPath(conststd::vectorTrajectoryPointpath,intwindow_size)const{std::vectorTrajectoryPointsmoothed_path;inthalf_windowwindow_size/2;for(size_t i0;ipath.size();i){TrajectoryPoint smoothed_pointpath[i];doublesum_x0,sum_y0;intcount0;for(intj-half_window;jhalf_window;j){intidxij;if(idx0idxstatic_castint(path.size())){sum_xpath[idx].pose.position.x;sum_ypath[idx].pose.position.y;count;}}smoothed_point.pose.position.xsum_x/count;smoothed_point.pose.position.ysum_y/count;smoothed_path.push_back(smoothed_point);}returnsmoothed_path;}13.4.2 曲率约束确保轨迹曲率不超过车辆转向能力。13.4.3 舒适性优化限制加加速度jerk以提高乘坐舒适性。13.5 速度规划13.5.1 速度曲线生成voidplanVelocityProfile(Trajectorytrajectory)const{// 1. 初始化速度为最大限速for(autopoint:trajectory.points){point.longitudinal_velocity_mpsspeed_limit_;}// 2. 根据曲率限制速度for(autopoint:trajectory.points){doublecurvaturecalculateCurvature(point);doublemax_speed_at_curvaturestd::sqrt(max_lateral_accel_/std::abs(curvature));point.longitudinal_velocity_mpsstd::min(point.longitudinal_velocity_mps,max_speed_at_curvature);}// 3. 向后传播减速约束for(intitrajectory.points.size()-2;i0;--i){doubledistancecalculateDistance(trajectory.points[i],trajectory.points[i1]);doublemax_speed_from_nextstd::sqrt(trajectory.points[i1].longitudinal_velocity_mps*trajectory.points[i1].longitudinal_velocity_mps2*max_deceleration_*distance);trajectory.points[i].longitudinal_velocity_mpsstd::min(trajectory.points[i].longitudinal_velocity_mps,max_speed_from_next);}}13.5.2 加速度约束限制加速度和减速度在舒适范围内。13.5.3 时间最优规划在满足约束的前提下尽可能缩短行驶时间。13.6 特殊场景规划13.6.1 狭窄空间规划在狭窄空间中需要更精细的规划。13.6.2 倒车规划倒车时的运动学模型与前进不同。13.6.3 复杂路口规划路口规划需要考虑多个转向选择。13.7 紧急停车规划13.7.1 紧急制动轨迹TrajectorygenerateEmergencyBrakingTrajectory(constPosecurrent_pose,doublecurrent_velocity)const{Trajectory emergency_traj;// 使用最大减速度doubledecelmax_emergency_deceleration_;// -5.0 m/s^2doublet0;doublevcurrent_velocity;doubles0;while(v0){TrajectoryPoint point;point.posecurrent_pose;point.pose.position.xs*std::cos(getYaw(current_pose));point.pose.position.ys*std::sin(getYaw(current_pose));point.longitudinal_velocity_mpsv;point.acceleration_mps2decel;point.time_from_startt;emergency_traj.points.push_back(point);// 更新状态t0.1;vdecel*0.1;sv*0.1;}returnemergency_traj;}13.7.2 最小停车距离计算doublecalculateMinimumStoppingDistance(doublevelocity)const{// 反应时间距离doublereaction_distancevelocity*reaction_time_;// 制动距离doublebraking_distancevelocity*velocity/(2*max_deceleration_);returnreaction_distancebraking_distance;}13.7.3 紧急避让规划当紧急制动不足以避免碰撞时尝试紧急避让。参考资料官方文档Autoware Documentation - Motion Planning源码仓库obstacle_avoidance_plannerpath_smoother学术论文Dolgov, D. et al. (2010). “Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments”. IJRR.Werling, M. et al. (2010). “Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame”. ICRA.