第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）

核心职责：

1. 轨迹生成：将离散路径点转换为平滑连续轨迹
2. 碰撞检测：确保轨迹不与障碍物碰撞
3. 速度规划：为轨迹点分配合理的速度和加速度
4. 约束满足：满足车辆运动学和动力学约束

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::NodeOptions&options):Node("obstacle_avoidance_planner",options){// 订阅行为路径sub_path_=create_subscription<Path>("/planning/scenario_planning/lane_driving/behavior_planning/path",1,std::bind(&ObstacleAvoidancePlannerNode::onPath,this,_1));// 订阅障碍物sub_objects_=create_subscription<PredictedObjects>("/perception/object_recognition/objects",1,std::bind(&ObstacleAvoidancePlannerNode::onObjects,this,_1));// 发布优化后的轨迹pub_trajectory_=create_publisher<Trajectory>("/planning/scenario_planning/trajectory",1);// 初始化优化器optimizer_=std::make_shared<TrajectoryOptimizer>();}voidonPath(constPath::SharedPtr path_msg){// 1. 路径预处理autopreprocessed_path=preprocessPath(*path_msg);// 2. 生成初始轨迹autoinitial_trajectory=generateInitialTrajectory(preprocessed_path);// 3. 碰撞检测autocollision_points=detectCollisions(initial_trajectory,current_objects_);// 4. 轨迹优化autooptimized_trajectory=optimizer_->optimize(initial_trajectory,collision_points);// 5. 速度规划autofinal_trajectory=planVelocity(optimized_trajectory);// 6. 发布轨迹pub_trajectory_->publish(final_trajectory);}private:std::shared_ptr<TrajectoryOptimizer>optimizer_;PredictedObjects current_objects_;};}// namespace obstacle_avoidance_planner

2. 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_start

---

13.2 轨迹生成算法

13.2.1 样条曲线插值

样条曲线是最常用的轨迹平滑方法，能生成C2连续的平滑曲线。

三次样条插值：

std::vector<TrajectoryPoint>generateSplineTrajectory(conststd::vector<PathPoint>&path_points)const{// 1. 提取路径点坐标std::vector<double>x_points,y_points,s_points;doubles=0.0;for(size_t i=0;i<path_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(i>0){s+=calculateDistance(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::vector<TrajectoryPoint>trajectory;doubleds=0.1;// 采样间隔0.1米for(doubles_interp=0;s_interp<=s_points.back();s_interp+=ds){TrajectoryPoint point;point.pose.position.x=spline_x.interpolate(s_interp);point.pose.position.y=spline_y.interpolate(s_interp);// 计算航向角doubledx=spline_x.derivative(s_interp);doubledy=spline_y.derivative(s_interp);doubleyaw=std::atan2(dy,dx);point.pose.orientation=createQuaternionFromYaw(yaw);// 计算曲率doubleddx=spline_x.secondDerivative(s_interp);doubleddy=spline_y.secondDerivative(s_interp);doublecurvature=(dx*ddy-dy*ddx)/std::pow(dx*dx+dy*dy,1.5);trajectory.push_back(point);}returntrajectory;}

13.2.2 贝塞尔曲线生成

贝塞尔曲线适用于车道变换等需要明确控制点的场景。

TrajectoryPointevaluateBezier(conststd::vector<Point>&control_points,doublet)const{// 使用De Casteljau算法计算贝塞尔曲线点autopoints=control_points;intn=points.size()-1;for(intk=1;k<=n;++k){for(inti=0;i<=n-k;++i){points[i].x=(1-t)*points[i].x+t*points[i+1].x;points[i].y=(1-t)*points[i].y+t*points[i+1].y;}}TrajectoryPoint result;result.pose.position.x=points[0].x;result.pose.position.y=points[0].y;returnresult;}

13.2.3 优化方法

基于优化的轨迹生成可以同时考虑多个约束条件。

优化目标：

minimize: w1 * smoothness + w2 * deviation + w3 * collision_cost subject to: - curvature < max_curvature - lateral_offset < max_offset - collision_free

---

13.3 碰撞检测与避障

13.3.1 碰撞检测算法

boolcheckCollision(constTrajectoryPoint&point,constPredictedObject&object)const{// 1. 获取车辆轮廓autovehicle_polygon=getVehicleFootprint(point.pose);// 2. 获取障碍物轮廓（考虑预测位置）autoobject_polygon=getObjectFootprint(object,point.time_from_start);// 3. 膨胀安全边界autoinflated_vehicle=inflatePolygon(vehicle_polygon,safety_margin_);// 4. 检查多边形相交returnpolygonsIntersect(inflated_vehicle,object_polygon);}

13.3.2 安全距离计算

根据速度动态调整安全距离：

doublecalculateSafetyMargin(doublevelocity)const{// 基础安全距离doublebase_margin=1.0;// 1米// 速度相关的额外距离doublevelocity_margin=velocity*time_headway_;// 通常2秒// 最小和最大限制doubletotal_margin=base_margin+velocity_margin;returnstd::clamp(total_margin,min_margin_,max_margin_);}

13.3.3 动态避障策略

当检测到碰撞风险时，采取避障措施。

---

13.4 路径平滑优化

13.4.1 平滑算法

移动平均平滑：

std::vector<TrajectoryPoint>smoothPath(conststd::vector<TrajectoryPoint>&path,intwindow_size)const{std::vector<TrajectoryPoint>smoothed_path;inthalf_window=window_size/2;for(size_t i=0;i<path.size();++i){TrajectoryPoint smoothed_point=path[i];doublesum_x=0,sum_y=0;intcount=0;for(intj=-half_window;j<=half_window;++j){intidx=i+j;if(idx>=0&&idx<static_cast<int>(path.size())){sum_x+=path[idx].pose.position.x;sum_y+=path[idx].pose.position.y;count++;}}smoothed_point.pose.position.x=sum_x/count;smoothed_point.pose.position.y=sum_y/count;smoothed_path.push_back(smoothed_point);}returnsmoothed_path;}

13.4.2 曲率约束

确保轨迹曲率不超过车辆转向能力。

13.4.3 舒适性优化

限制加加速度（jerk）以提高乘坐舒适性。

---

13.5 速度规划

13.5.1 速度曲线生成

voidplanVelocityProfile(Trajectory&trajectory)const{// 1. 初始化速度为最大限速for(auto&point:trajectory.points){point.longitudinal_velocity_mps=speed_limit_;}// 2. 根据曲率限制速度for(auto&point:trajectory.points){doublecurvature=calculateCurvature(point);doublemax_speed_at_curvature=std::sqrt(max_lateral_accel_/std::abs(curvature));point.longitudinal_velocity_mps=std::min(point.longitudinal_velocity_mps,max_speed_at_curvature);}// 3. 向后传播减速约束for(inti=trajectory.points.size()-2;i>=0;--i){doubledistance=calculateDistance(trajectory.points[i],trajectory.points[i+1]);doublemax_speed_from_next=std::sqrt(trajectory.points[i+1].longitudinal_velocity_mps*trajectory.points[i+1].longitudinal_velocity_mps+2*max_deceleration_*distance);trajectory.points[i].longitudinal_velocity_mps=std::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(constPose&current_pose,doublecurrent_velocity)const{Trajectory emergency_traj;// 使用最大减速度doubledecel=max_emergency_deceleration_;// -5.0 m/s^2doublet=0;doublev=current_velocity;doubles=0;while(v>0){TrajectoryPoint point;point.pose=current_pose;point.pose.position.x+=s*std::cos(getYaw(current_pose));point.pose.position.y+=s*std::sin(getYaw(current_pose));point.longitudinal_velocity_mps=v;point.acceleration_mps2=decel;point.time_from_start=t;emergency_traj.points.push_back(point);// 更新状态t+=0.1;v+=decel*0.1;s+=v*0.1;}returnemergency_traj;}

13.7.2 最小停车距离计算

doublecalculateMinimumStoppingDistance(doublevelocity)const{// 反应时间距离doublereaction_distance=velocity*reaction_time_;// 制动距离doublebraking_distance=velocity*velocity/(2*max_deceleration_);returnreaction_distance+braking_distance;}

13.7.3 紧急避让规划

当紧急制动不足以避免碰撞时，尝试紧急避让。

---

参考资料

官方文档

• Autoware Documentation - Motion Planning

源码仓库

• obstacle_avoidance_planner
• path_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.