《自动驾驶编程原理与应用》课件 第8章运动控制

1主讲：黄炜自动驾驶编程原理与应用2第八章运动控制知识点：了解主流运动控制方法；掌握自动驾驶系统运动控制功能包相关功能、实现原理和使用方法。重点：重点掌握纯跟踪算法原理和自动驾驶车辆纵向和横向控制方法，以及Autoware运动控制模块中ROS节点pure_pursuit等。难点：运动控制功能模块与示例节点源码解析。自动驾驶编程原理与应用3引言运动控制是指自动驾驶系统基于决策规划结果计算在车辆运动学和动力学约束下底盘执行机构的控制指令，通过这些指令控制车辆的加速、制动、转向等动作，实现相应的速度跟踪和路径跟踪行为。4引言自动驾驶车辆的运动控制主要包含车辆的纵向控制和横向控制，其中纵向控制指根据轨迹规划中的速度规划结果（期望速度）进行跟踪控制，横向控制指根据轨迹规划中的路径规划结果（期望路径）进行跟踪控制。车辆纵向控制车辆横向控制运动控制速度跟踪控制路径跟踪控制5引言对于车辆纵向控制而言，其速度跟踪控制主要基于期望速度与实际速度之间的误差计算得到的期望加速度/减速度，利用车辆逆纵向动力学模型将期望加速度/减速度转换为底盘执行机构（比如加速踏板执行机构）的控制期望，进而通过控制底盘执行机构使车辆的实际速度能够准确、快速、平顺地跟踪期望速度。对于车辆横向控制而言，其路径跟踪控制主要基于期望路径与当前位姿信息之间的误差，利用直接或间接的方法建立该误差与底盘执行机构（比如转向执行机构）之间的控制关系，进而通过控制底盘执行机构使车辆能够准确、快速、平顺地跟踪期望路径。6本章节目录8.1运动控制方法概述常见运动控制方法简介Autoware运动控制基本步骤8.2运动控制功能包纯跟踪控制方法节点pure_pursuit与源码解析78.1运动控制方法概述8常见运动控制方法简介对于速度跟踪控制而言，经典的PID算法是一种常见且简单有效的速度跟踪控制方法，通过比例、积分和微分三个环节实现对被控对象的闭环控制。速度跟踪控制算法8.1运动控制方法概述98.1运动控制方法概述常见运动控制方法简介对于路径跟踪控制而言，其控制方法通常分为基于几何的方法和基于模型的方法两类。路径跟踪控制算法基于几何的方法基于模型的方法路径跟踪控制纯跟踪方法Stanley方法滑模控制方法模型预测控制方法108.1运动控制方法概述常见运动控制方法简介基于几何的路径跟踪控制方法的特点是利用车辆与期望路径之间的几何关系得到路径跟踪问题的控制律，一般使用预瞄距离（Look-aheadDistance）来描述车辆路径跟踪的误差。常见的基于几何的路径跟踪控制方法有纯跟踪（PurePursuit）方法、Stanley方法等。与基于模型的方法相比，基于几何的路径跟踪控制方法较为简单和直接，不用考虑车辆的运动学模型和动力学模型，控制时使用的参数少，工程应用较为方便。然而，该方法忽略了车辆的运动学和动力学特性，可能会导致车辆在高速或大转向角度下跟踪性能下降，同时对复杂自动驾驶场景的适应性也相对较弱。路径跟踪控制算法118.1运动控制方法概述常见运动控制方法简介基于模型的路径跟踪控制方法更适合于复杂的实际自动驾驶场景。但是该方法考虑了车辆的运动学和动力学特性，使其复杂度和计算量大大增加。目前常见的基于模型的方法可再进一步细分为基于运动学模型的方法和基于动力学模型的方法。常见的路径跟踪控制算法有滑模控制（SlidingModeControl）、模型预测控制（ModelPredictiveControl）等。相较于其他控制算法，模型预测控制对于处理在运动学、动力学和执行机构等约束条件下的多输入多输出系统控制问题较为有优势。路径跟踪控制算法12Autoware运动控制基本步骤以运行官方示例数据为例，在终端输入以下命令开始运动控制：$cdautoware.ai$sourceinstall/setup.bash$roslaunchautoware_quickstart_examplesmy_motion_planning.launch8.1运动控制方法概述13Autoware运动控制基本步骤为了查看启动文件my_motion_planning.launch的文本内容，在终端输入以下命令，在官方示例autoware_quickstart_examples路径下找到工作目录“./launch/rosbag_demo”，然后在rosbag_demo文件夹中打开启动文件my_motion_planning.launch：$cdautoware.ai/src/autoware/documentation/autoware_quickstart_examples$cdlaunch/rosbag_demo$geditmy_motion_planning.launch8.1运动控制方法概述<launch><!--VehicleContorl--><includefile="$(findruntime_manager)/launch_files/vehicle_socket.launch"/><!--obstacle_avoid--><includefile="$(findwaypoint_planner)/launch/astar_avoid.launch"/><!--velocity_set--><includefile="$(findwaypoint_planner)/launch/velocity_set.launch"/><!--pure_pursuit--><nodepkg="rostopic"type="rostopic"name="config_waypoint_follower_rostopic"args="pub-l/config/waypoint_followerautoware_config_msgs/ConfigWaypointFollower'{header:auto,param_flag:1,velocity:5.0,lookahead_distance:4.0,lookahead_ratio:2.0,minimum_lookahead_distance:6.0,displacement_threshold:0.0,relative_angle_threshold:0.0}'"/><includefile="$(findwaypoint_follower)/launch/pure_pursuit.launch"/><!--twist_filter--><includefile="$(findwaypoint_follower)/launch/twist_filter.launch"/></launch>14my_motion_planning.launch8.1运动控制方法概述Autoware运动控制基本步骤启动文件通过导入“pure_pursuit.launch”等多个启动文件到当前文件实现运动控制过程中所需轨迹跟踪控制等相关功能的调用。158.2运动控制功能包16纯跟踪控制方法所建立的运动学模型主要是面向控制器使用，需要在较为准确地描述车辆运行学过程的基础上尽可能地简化，以减少控制算法的计算负担。因此，在车辆运动学建模时，要进行以下理想化假设：1)假设车辆在平坦的路面上行驶，忽略车辆垂向运动，即车辆仅在二维平面（地面）上运动，其沿z轴的位移以及绕y轴的俯仰角和绕x轴的侧倾角均为零。2)假设车辆为左右质量相同对称的刚体。3)假设悬架系统是刚性的，忽略悬架运动及其对耦合关系的影响。8.2运动控制功能包17纯跟踪控制方法所建立的运动学模型主要是面向控制器使用，需要在较为准确地描述车辆运行学过程的基础上尽可能地简化，以减少控制算法的计算负担。因此，在车辆运动学建模时，要进行以下理想化假设：4)假设车辆行驶过程中不产生相对滑动和滑移现象。5)不考虑轮胎的弹性形变与横向载荷转移。6)认为轮距相对于转弯半径可忽路不计，使用单轨模型来描述车辆运动。7)忽略左右转向轮之间存在的转向角度差。8.2运动控制功能包18纯跟踪控制方法基于上述假设可以简化得到三自由度的车辆运动学模型。8.2运动控制功能包模型重点关注车辆沿x轴和y轴的运动，以及绕z轴的转动。19

8.2运动控制功能包20

8.2运动控制功能包21

8.2运动控制功能包

22

8.2运动控制功能包23节点pure_pursuit与源码解析双击打开功能包waypoint_follower目录下的CMakeLists.txt文件。如图所示，节点pure_pursuit的源文件是功能包waypoint_follower目录下的“nodes/pure_pursuit/pure_pursuit_node.cpp”和“nodes/pure_pursuit/pure_pursuit.cpp”等。其中，主函数在pure_pursuit_node.cpp中。8.2运动控制功能包……add_executable(pure_pursuitnodes/pure_pursuit/pure_pursuit_node.cppnodes/pure_pursuit/pure_pursuit.cppnodes/pure_pursuit/pure_pursuit_core.cppnodes/pure_pursuit/pure_pursuit_viz.cpp)……CMakeLists.txt24节点pure_pursuit与源码解析（1）主函数流程解析在VSCode软件中打开文件夹autoware.ai下的代码空间src，然后在侧边栏中依次点击文件夹autoware下的子文件夹common找到功能包waypoint_follower，选择节点文件夹nodes中的节点pure_pursuit，最后双击打开源文件pure_pursuit_node.cpp和pure_pursuit.cpp。8.2运动控制功能包25节点pure_pursuit与源码解析（1）主函数流程解析主函数位于源文件pure_pursuit_node.cpp中，其主要通过使用waypoint_follower::PurePursuitNode对象的run函数实现纯跟踪算法的运动控制功能。#include<ros/ros.h>#include"pure_pursuit_core.h"intmain(intargc,char**argv){ros::init(argc,argv,"pure_pursuit");waypoint_follower::PurePursuitNodeppn;ppn.run();return0;}pure_pursuit_node.cpp文本中主函数8.2运动控制功能包26节点pure_pursuit与源码解析（1）主函数流程解析waypoint_follower::PurePursuitNode构造函数首先对数据成员进行初始化，包括预瞄距离系数lookahead_distance_ratio、最小预瞄距离minimum_lookahead_distance等。……namespacewaypoint_follower{……{initForROS();node_status_publisher_ptr_=std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh_,private_nh_);node_status_publisher_ptr_->ENABLE();pp_.setLinearInterpolationParameter(is_linear_interpolation_);}……}pure_pursuit_core.cpp文本中waypoint_follower::PurePursuitNode构造函数8.2运动控制功能包27节点pure_pursuit与源码解析（1）主函数流程解析然后，创建一个智能指针node_status_publisher_ptr_，并对节点句柄nh_和private_nh_分配动态内存空间并初始化，以便于系统内存的使用和释放。……namespacewaypoint_follower{……{initForROS();node_status_publisher_ptr_=std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh_,private_nh_);node_status_publisher_ptr_->ENABLE();pp_.setLinearInterpolationParameter(is_linear_interpolation_);}……}pure_pursuit_core.cpp文本中waypoint_follower::PurePursuitNode构造函数8.2运动控制功能包28节点pure_pursuit与源码解析（1）主函数流程解析接着，调用ENABLE()函数对整个系统的运行状态进行检查。……namespacewaypoint_follower{……{initForROS();node_status_publisher_ptr_=std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh_,private_nh_);node_status_publisher_ptr_->ENABLE();pp_.setLinearInterpolationParameter(is_linear_interpolation_);}……}pure_pursuit_core.cpp文本中waypoint_follower::PurePursuitNode构造函数8.2运动控制功能包29节点pure_pursuit与源码解析（1）主函数流程解析最后，调用pp_对象的setLinearInterpolationParameter函数设置线性插值参数，其is_linear_interpolation_表示是否启用线性插值。……namespacewaypoint_follower{……{initForROS();node_status_publisher_ptr_=std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh_,private_nh_);node_status_publisher_ptr_->ENABLE();pp_.setLinearInterpolationParameter(is_linear_interpolation_);}……}pure_pursuit_core.cpp文本中waypoint_follower::PurePursuitNode构造函数8.2运动控制功能包30节点pure_pursuit与源码解析（1）主函数流程解析PurePursuitNode::initForROS函数实现逻辑与前面章节中解析的节点主函数的实现逻辑相似，PurePursuitNode::initForROS函数的流程同样包括了参数获取、话题发布和订阅。8.2运动控制功能包31节点pure_pursuit与源码解析（1）主函数流程解析参数获取……voidPurePursuitNode::initForROS(){//rosparametersettingsprivate_nh_.param("is_linear_interpolation",is_linear_interpolation_,bool(true));private_nh_.param("publishes_for_steering_robot",publishes_for_steering_robot_,bool(false));nh_.param("vehicle_info/wheel_base",wheel_base_,double(2.7));……}……pure_pursuit_core.cpp文本中PurePursuitNode::initForROS函数的参数获取部分通过直接调用ros::param函数来访问参数服务器上的参数，并将其赋值给相应的变量，包括is_linear_interpolation_、publishes_for_steering_robot_、wheel_base_等。如果上述变量在参数服务器中没有找到对应的参数，则自动设置为默认值。以变量is_linear_interpolation为例，将参数服务器中获取的参数is_linear_interpolation的值赋值给变量is_linear_interpolation_；如果参数服务器中提供没有相应的参数，则变量数值默认设置为true。此外，变量publishes_for_steering_robot_默认设置为false，变量wheel_base_的数值默认设置为2.7（m）。8.2运动控制功能包……voidPurePursuitNode::initForROS(){……//setupsubscribersub1_=nh_.subscribe("final_waypoints",10,&PurePursuitNode::callbackFromWayPoints,this);sub2_=nh_.subscribe("current_pose",10,&PurePursuitNode::callbackFromCurrentPose,this);sub3_=nh_.subscribe("config/waypoint_follower",10,&PurePursuitNode::callbackFromConfig,this);sub4_=nh_.subscribe("current_velocity",10,&PurePursuitNode::callbackFromCurrentVelocity,this);//setuppublisherpub1_=nh_.advertise<geometry_msgs::TwistStamped>("twist_raw",10);pub2_=nh_.advertise<autoware_msgs::ControlCommandStamped>("ctrl_cmd",10);pub11_=nh_.advertise<visualization_msgs::Marker>("next_waypoint_mark",0);pub12_=nh_.advertise<visualization_msgs::Marker>("next_target_mark",0);pub13_=nh_.advertise<visualization_msgs::Marker>("search_circle_mark",0);pub14_=nh_.advertise<visualization_msgs::Marker>("line_point_mark",0);//debugtoolpub15_=nh_.advertise<visualization_msgs::Marker>("trajectory_circle_mark",0);pub16_=nh_.advertise<std_msgs::Float32>("angular_gravity",0);pub17_=nh_.advertise<std_msgs::Float32>("deviation_of_current_position",0);}节点pure_pursuit与源码解析（1）主函数流程解析pure_pursuit_core.cpp文本中PurePursuitNode::initForROS函数的话题发布和订阅部分发布和订阅通过查找源文件pure_pursuit_core.cpp中PurePursuitNode::initForROS函数发布和订阅的话题，可以快速确认节点pure_pursuit的输出和输入信息。328.2运动控制功能包33节点pure_pursuit与源码解析（1）主函数流程解析在PurePursuitNode::initForROS函数中发布的话题有“twist_raw”、“ctrl_cmd”、“next_waypoint_mark”、“next_target_mark”、“search_circle_mark”等，而订阅的话题则有“final_waypoints”、“current_pose”、“config/waypoint_follower”、“current_velocity”等。其中，订阅话题时会通过回调函数（如callbackFromWayPoints函数、callbackFromCurrentPose函数、callbackFromConfig函数、callbackFromCurrentVelocity函数等）处理接收到的话题信息。发布和订阅8.2运动控制功能包34节点pure_pursuit与源码解析（2）关键子函数解析run函数是pure_pursuit_node.cpp的功能主体，以下就run函数相关功能实现关键部分进行解析。run函数8.2运动控制功能包#include<ros/ros.h>#include"pure_pursuit_core.h"intmain(intargc,char**argv){ros::init(argc,argv,"pure_pursuit");waypoint_follower::PurePursuitNodeppn;ppn.run();return0;}pure_pursuit_node.cpp文本中主函数35节点pure_pursuit与源码解析（2）关键子函数解析……voidPurePursuitNode::run(){ROS_INFO_STREAM("purepursuitstart");ros::Rateloop_rate(LOOP_RATE_);while(ros::ok()){ros::spinOnce();if(!is_pose_set_||!is_waypoint_set_||!is_velocity_set_||!is_config_set_){ROS_WARN("Necessarytopicsarenotsubscribedyet...");loop_rate.sleep();continue;}pp_.setLookaheadDistance(computeLookaheadDistance());pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_);doublekappa=0;boolcan_get_curvature=pp_.canGetCurvature(&kappa);

publishTwistStamped(can_get_curvature,kappa);publishControlCommandStamped(can_get_curvature,kappa);node_status_publisher_ptr_->NODE_ACTIVATE();node_status_publisher_ptr_->CHECK_RATE("/topic/rate/vehicle/slow",8,5,1,"topicvehicle_cmdpublishratelow.");……}}……pure_pursuit_core.cpp文本中PurePursuitNode::run函数通过标志位（is_pose_set_、is_waypoint_set_、is_velocity_set_、is_config_set_）判断节点所需的话题消息输入是否正常接收，当判断语句!is_pose_set_||!is_waypoint_set_||!is_velocity_set_||!is_config_set_为真时，表示未正常接收节点所需的话题消息，则输出提示“Necessarytopicsarenotsubscribedyet...”并在此处等待。run函数8.2运动控制功能包36节点pure_pursuit与源码解析（2）关键子函数解析……voidPurePursuitNode::run(){ROS_INFO_STREAM("purepursuitstart");ros::Rateloop_rate(LOOP_RATE_);while(ros::ok()){ros::spinOnce();if(!is_pose_set_||!is_waypoint_set_||!is_velocity_set_||!is_config_set_){ROS_WARN("Necessarytopicsarenotsubscribedyet...");loop_rate.sleep();continue;}pp_.setLookaheadDistance(computeLookaheadDistance());pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_);doublekappa=0;boolcan_get_curvature=pp_.canGetCurvature(&kappa);

publishTwistStamped(can_get_curvature,kappa);publishControlCommandStamped(can_get_curvature,kappa);node_status_publisher_ptr_->NODE_ACTIVATE();node_status_publisher_ptr_->CHECK_RATE("/topic/rate/vehicle/slow",8,5,1,"topicvehicle_cmdpublishratelow.");……}}……pure_pursuit_core.cpp文本中PurePursuitNode::run函数调用setLookaheadDistance函数对预瞄距离lookahead_distance_进行设置，其值通过computeLookaheadDistance函数计算得到。同时，调用setMinimumLookaheadDistance函数对最小预瞄距离minimum_lookahead_distance_进行设置。run函数8.2运动控制功能包37节点pure_pursuit与源码解析（2）关键子函数解析……voidPurePursuitNode::run(){ROS_INFO_STREAM("purepursuitstart");ros::Rateloop_rate(LOOP_RATE_);while(ros::ok()){ros::spinOnce();if(!is_pose_set_||!is_waypoint_set_||!is_velocity_set_||!is_config_set_){ROS_WARN("Necessarytopicsarenotsubscribedyet...");loop_rate.sleep();continue;}pp_.setLookaheadDistance(computeLookaheadDistance());pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_);doublekappa=0;boolcan_get_curvature=pp_.canGetCurvature(&kappa);

publishTwistStamped(can_get_curvature,kappa);publishControlCommandStamped(can_get_curvature,kappa);node_status_publisher_ptr_->NODE_ACTIVATE();node_status_publisher_ptr_->CHECK_RATE("/topic/rate/vehicle/slow",8,5,1,"topicvehicle_cmdpublishratelow.");……}}……pure_pursuit_core.cpp文本中PurePursuitNode::run函数调用canGetCurvature函数判断是否能得到连接当前车辆后轴参考点与期望轨迹上目标点的圆弧，并计算出圆弧曲率。run函数8.2运动控制功能包38节点pure_pursuit与源码解析（2）关键子函数解析……voidPurePursuitNode::run(){ROS_INFO_STREAM("purepursuitstart");ros::Rateloop_rate(LOOP_RATE_);while(ros::ok()){ros::spinOnce();if(!is_pose_set_||!is_waypoint_set_||!is_velocity_set_||!is_config_set_){ROS_WARN("Necessarytopicsarenotsubscribedyet...");loop_rate.sleep();continue;}pp_.setLookaheadDistance(computeLookaheadDistance());pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_);doublekappa=0;boolcan_get_curvature=pp_.canGetCurvature(&kappa);

publishTwistStamped(can_get_curvature,kappa);publishControlCommandStamped(can_get_curvature,kappa);node_status_publisher_ptr_->NODE_ACTIVATE();node_status_publisher_ptr_->CHECK_RATE("/topic/rate/vehicle/slow",8,5,1,"topicvehicle_cmdpublishratelow.");……}}……pure_pursuit_core.cpp文本中PurePursuitNode::run函数run函数8.2运动控制功能包

39节点pure_pursuit与源码解析（2）关键子函数解析computeLookaheadDistance函数8.2运动控制功能包computeLookaheadDistance函数根据param_flag_的数值判断选择的模式（Dialog模式还是Waypoint模式），当判断语句param_flag_==enumToInteger(Mode::dialog)为真时，表示选择的是Dialog模式，则直接返回一个固定数值的预瞄距离const_lookahead_distance_；……doublePurePursuitNode::computeLookaheadDistance()const{if(param_flag_==enumToInteger(Mode::dialog))returnconst_lookahead_distance_;doublemaximum_lookahead_distance=current_linear_velocity_*10;doubleld=current_linear_velocity_*lookahead_distance_ratio_;returnld<minimum_lookahead_distance_?minimum_lookahead_distance_:ld>maximum_lookahead_distance?maximum_lookahead_distance:ld;}……pure_pursuit_core.cpp文本中computeLookaheadDistance函数40节点pure_pursuit与源码解析（2）关键子函数解析8.2运动控制功能包否则，将最大预瞄距离maximum_lookahead_distance设置为当前车辆速度current_linear_velocity的10倍，同时将预瞄距离ld的参考值设置为当前车辆速度current_linear_velocity乘以比例系数lookahead_distance_ratio_。……doublePurePursuitNode::computeLookaheadDistance()const{if(param_flag_==enumToInteger(Mode::dialog))returnconst_lookahead_distance_;doublemaximum_lookahead_distance=current_linear_velocity_*10;doubleld=current_linear_velocity_*lookahead_distance_ratio_;returnld<minimum_lookahead_distance_?minimum_lookahead_distance_:ld>maximum_lookahead_distance?maximum_lookahead_distance:ld;}……pure_pursuit_core.cpp文本中computeLookaheadDistance函数computeLookaheadDistance函数41节点pure_pursuit与源码解析（2）关键子函数解析8.2运动控制功能包最后将最小预瞄距离minimum_lookahead_distance_、预瞄距离ld、最大预瞄距离maximum_lookahead_distance三者的中间值返回作为预瞄距离。……doublePurePursuitNode::computeLookaheadDistance()const{if(param_flag_==enumToInteger(Mode::dialog))returnconst_lookahead_distance_;doublemaximum_lookahead_distance=current_linear_velocity_*10;doubleld=current_linear_velocity_*lookahead_distance_ratio_;returnld<minimum_lookahead_distance_?minimum_lookahead_distance_:ld>maximum_lookahead_distance?maximum_lookahead_distance:ld;}……pure_pursuit_core.cpp文本中computeLookaheadDistance函数computeLookaheadDistance函数42节点pure_pursuit与源码解析（2）关键子函数解析……boolPurePursuit::canGetCurvature(double*output_kappa){//searchnextwaypointgetNextWaypoint();if(next_waypoint_number_==-1){ROS_INFO("lostnextwaypoint");returnfalse;}//checkwhethercurvatureisvalidornotboolis_valid_curve=false;for(constauto&el:current_waypoints_){if(getPlaneDistance(el.pose.pose.position,current_pose_.position)>minimum_lookahead_distance_){is_valid_curve=true;break;}}if(!is_valid_curve){returnfalse;}……}……pure_pursuit.cpp文本中PurePursuit::canGetCurvature函数Part1canGetCurvature函数8.2运动控制功能包使用PurePursuit::getNextWaypoint函数获取期望轨迹中下一个需要跟踪的目标点，判断是否丢失目标点，当判断语句next_waypoint_number_==-1为真时，表示丢失目标点，返回false。43节点pure_pursuit与源码解析（2）关键子函数解析……boolPurePursuit::canGetCurvature(double*output_kappa){//searchnextwaypointgetNextWaypoint();if(next_waypoint_number_==-1){ROS_INFO("lostnextwaypoint");returnfalse;}//checkwhethercurvatureisvalidornotboolis_valid_curve=false;for(constauto&el:current_waypoints_){if(getPlaneDistance(el.pose.pose.position,current_pose_.position)>minimum_lookahead_distance_){is_valid_curve=true;break;}}if(!is_valid_curve){returnfalse;}……}……pure_pursuit.cpp文本中PurePursuit::canGetCurvature函数Part18.2运动控制功能包使用for循环语句遍历轨迹点序列中的每一点，调用getPlaneDistance函数计算轨迹点序列中的每一点到当前车辆后轴参考点的距离，接着判断其是否大于最小预瞄距离，当判断语句getPlaneDistance(el.pose.pose.position,current_pose_.position)>minimum_lookahead_distance_为真时，表示计算的距离大于最小预瞄距离进而判断圆弧有效，则对标志位is_valid_curve赋值true，否则返回false。canGetCurvature函数44节点pure_pursuit与源码解析（2）关键子函数解析……boolPurePursuit::canGetCurvature(double*output_kappa){……//ifis_linear_interpolation_isfalseornextwaypointisfirstorlastif(!is_linear_interpolation_||next_waypoint_number_==0||next_waypoint_number_==(static_cast<int>(current_waypoints_.size()-1))){next_target_position_=current_waypoints_.at(next_waypoint_number_).pose.pose.position;*output_kappa=calcCurvature(next_target_position_);returntrue;}//linearinterpolationandcalculateangularvelocityboolinterpolation=interpolateNextTarget(next_waypoint_number_,&next_target_position_);if(!interpolation){ROS_INFO_STREAM("losttarget!");returnfalse;}*output_kappa=calcCurvature(next_target_position_);returntrue;}……pure_pursuit.cpp文本中PurePursuit::canGetCurvature函数Part28.2运动控制功能包判断是否存在不能线性插值或者目标点是第一个点或者最后一个点的情况，当判断语句!is_linear_interpolation_||next_waypoint_number_==0||next_waypoint_number_==(static_cast<int>(current_waypoints_.size()-1))为真时，表示存在前述情况，则直接使用当前轨迹点作为目标点，并调用PurePursuit::calcCurvature函数计算圆弧曲率，同时返回true。canGetCurvature函数45节点pure_pursuit与源码解析（2）关键子函数解析……boolPurePursuit::canGetCurvature(double*output_kappa){……//ifis_linear_interpolation_isfalseornextwaypointisfirstorlastif(!is_linear_interpolation_||next_waypoint_number_==0||next_waypoint_number_==(static_cast<int>(current_waypoints_.size()-1))){next_target_position_=current_waypoints_.at(next_waypoint_number_).pose.pose.position;*output_kappa=calcCurvature(next_target_position_);returntrue;}//linearinterpolationandcalculateangularvelocityboolinterpolation=interpolateNextTarget(next_waypoint_number_,&next_target_position_);if(!interpolation){ROS_INFO_STREAM("losttarget!");returnfalse;}*output_kappa=calcCurvature(next_target_position_);returntrue;}……pure_pursuit.cpp文本中PurePursuit::canGetCurvature函数Part28.2运动控制功能包调用PurePursuit::interpolateNextTarget函数进行线性插值处理，如果插值失败，则返回false。canGetCurvature函数46节点pure_pursuit与源码解析（2）关键子函数解析……voidPurePursuit::getNextWaypoint(){intpath_size=static_cast<int>(current_waypoints_.size());if(path_size==0){next_waypoint_number_=-1;return;}for(inti=0;i<path_size;i++){if(i==(path_size-1)){ROS_INFO("searchwaypointisthelast");next_waypoint_number_=i;return;}if(getPlaneDistance(current_waypoints_.at(i).pose.pose.position,current_pose_.position)>lookahead_distance_){next_waypoint_number_=i;return;}}next_waypoint_number_=-1;return;}……pure_pursuit.cpp文本中PurePursuit::getNextWaypoint函数getNextWaypoint函数8.2运动控制功能包getNextWaypoint函数主要用于获取期望轨迹中下一个需要跟踪的目标点。47节点pure_pursuit与源码解析（2）关键子函数解析……voidPurePursuit::getNextWaypoint(){intpath_size=static_cast<int>(current_waypoints_.size());if(path_size==0){next_waypoint_number_=-1;return;}for(inti=0;i<path_size;i++){if(i==(path_size-1)){ROS_INFO("searchwaypointisthelast");next_waypoint_number_=i;return;}if(getPlaneDistance(current_waypoints_.at(i).pose.pose.position,current_pose_.position)>lookahead_distance_){next_waypoint_number_=i;return;}}next_waypoint_number_=-1;return;}……pure_pursuit.cpp文本中PurePursuit::getNextWaypoint函数8.2运动控制功能包获取当前序列轨迹点个数并将其赋值给path_size。其次，判断path_size的值是否为0，当判断语句path_size==0为真时，表示当前序列轨迹点个数为0，则将next_waypoint_number_设置为-1（表示丢失目标点）并返回。getNextWaypoint函数48节点pure_pursuit与源码解析（2）关键子函数解析……voidPurePursuit::getNextWaypoint(){intpath_size=static_cast<int>(current_waypoints_.size());if(path_size==0){next_waypoint_number_=-1;return;}for(inti=0;i<path_size;i++){if(i==(path_size-1)){ROS_INFO("searchwaypointisthelast");next_waypoint_number_=i;return;}if(getPlaneDistance(current_waypoints_.at(i).pose.pose.position,current_pose_.position)>lookahead_distance_){next_waypoint_number_=i;return;}}next_waypoint_number_=-1;return;}……pure_pursuit.cpp文本中PurePursuit::getNextWaypoint函数8.2运动控制功能包使用for循环语句遍历轨迹点序列中的每一点，在每次循环中判断是否是为最后一个点，当判断语句i==(path_size-1)为真时，表示无论预瞄距离数值如何都将选择最后一个点作为下一个需要跟踪的目标点，则将下标赋值给next_waypoint_number_；getNextWaypoint函数49节点pure_pursuit与源码解析（2）关键子函数解析……voidPurePursuit::getNextWaypoint(){intpath_size=static_cast<int>(current_waypoints_.size());if(path_size==0){next_waypoint_number_=-1;return;}for(inti=0;i<path_size;i++){if(i==(path_size-1)){ROS_INFO("searchwaypointisthelast");next_waypoint_number_=i;return;}if(getPlaneDistance(current_waypoints_.at(i).pose.pose.position,current_pose_.position)>lookahead_distance_){next_waypoint_number_=i;return;}}next_waypoint_number_=-1;return;}……pure_pursuit.cpp文本中PurePursuit::getNextWaypoint函数8.2运动控制功能包接着调用getPlaneDistance函数计算轨迹点序列中的每一点到当前车辆后轴参考点的距离，并判断其是否大于预瞄距离，当判断语句getPlaneDistance(current_waypoints_.at(i).pose.pose.position,current_pose_.position)>lookahead_distance_)为真时，则将下标赋值给next_waypoint_number_作为下一个需要跟踪的目标点。getNextWaypoint函数50节点pure_pursuit与源码解析（2）关键子函数解析……doublegetPlaneDistance(geometry_msgs::Pointtarget1,geometry_msgs::Pointtarget2){tf::Vector3v1=point2vector(target1);v1.setZ(0);tf::Vector3v2=point2vector(target2);v2.setZ(0);returntf::tfDistance(v1,v2);}……libwaypoint_follower.cpp文本中getPlaneDistance函数8.2运动控制功能包getPlaneDistance函数对应的源文件为“~/autoware.ai/src/autoware/common/waypoint_follower/lib/libwaypoint_follower.cpp”。getPlaneDistance函数主要用于计算轨迹点序列中的每一点到当前车辆后轴参考点的距离。首先，使用point2vector函数将轨迹点的数据类型由geometry_msgs::Point转换为tf::Vector3类型，以便于后续使用TF库相关数学运算功能。然后，将两个向量的Z方向的分量设置为0，相当于将三维点投影到XY二维平面。最后，使用TF库的tfDistance函数计算两个二维向量的欧几里得距离。getPlaneDistance函数51

……doublePurePursuit::calcCurvature(geometry_msgs::Pointtarget)const{doublekappa;doubledenominator=pow(getPlaneDistance(target,current_pose_.position),2);doublenumerator=2*calcRelativeCoordinate(target,current_pose_).y;if(denominator!=0)kappa=numerator/denominator;else{if(numerator>0)kappa=KAPPA_MIN_;elsekappa=-KAPPA_MIN_;}ROS_INFO("kappa:%lf",kappa);returnkappa;}……pure_pursuit.cpp文本中PurePursuit::calcCurvature函数calcCurvature函数8.2运动控制功能包52节点pure_pursuit与源码解析（2）关键子函数解析……doublePurePursuit::calcCurvature(geometry_msgs::Pointtarget)const{doublekappa;doubledenominator=pow(getPlaneDistance(target,current_pose_.position),2);doublenumerator=2*calcRelativeCoordinate(target,current_pose_).y;if(denominator!=0)kappa=numerator/denominator;else{if(numerator>0)kappa=KAPPA_MIN_;elsekappa=-KAPPA_MIN_;}ROS_INFO("kappa:%lf",kappa);returnkappa;}……pure_pursuit.cpp文本中PurePursuit::calcCurvature函数8.2运动控制功能包计算公式的分母denominator，其为当前车辆后轴参考点到目标点的距离的平方。其次，计算公式的分子numerator，其为目标点在车辆坐标系中沿y方向的距离的2倍，其过程需要调用calcRelativeCoordinate函数进行坐标转换。calcCurvature函数53节点pure_pursuit与源码解析（2）关键子函数解析……doublePurePursuit::calcCurvature(geometry_msgs::Pointtarget)const{doublekappa;doubledenominator=pow(getPlaneDistance(target,current_pose_.position),2);doublenumerator=2*calcRelativeCoordinate(target,current_pose_).y;if(denominator!=0)kappa=numerator/denominator;else{if(numerator>0)kappa=KAPPA_MIN_;elsekappa=-KAPPA_MIN_;}ROS_INFO("kappa:%lf",kappa);returnkappa;}……pure_pursuit.cpp文本中PurePursuit::calcCurvature函数8.2运动控制功能包根据分子numerator和分母denominator的值计算圆弧曲率kappa，其过程需要判断分母是否为0，当判断语句denominator!=0不为真时，则根据分子符号情况返回相应最小曲率KAPPA_MIN_。calcCurvature函数54节点pure_pursuit与源码解析（2）关键子函数解析……boolPurePursuit::interpolateNextTarget(intnext_waypoint,geometry_msgs::Point*next_target)const{constexprdoubleERROR=pow(10,-5);intpath_size=static_cast<int>(current_waypoints_.size());if(next_waypoint==path_size-1){*next_target=current_waypoints_.at(next_waypoint).pose.pose.position;returntrue;}doublesearch_radius=lookahead_distance_;