路径规划与避障算法(二)---规划层与定位层的接口定义及数据融合

接口概述

1.规划层与定位层的数据交互主要体现在以下几个方面:

• 定位层提供车辆在map座标系下当前时刻的位置信息,角度信息
• 定位层提供车辆当前时刻的线速度信息,线性加速度信息
• 定位层提供车辆当前时刻的角速度信息,角加速度信息
• 定位层提供车辆上各个传感器,惯导,baselink以及map座标系之间的转换关系并以tf_tree的形式发布

2.定位层对外发布信息时,需要统一发布的数据所存在的座标系,通常选用的是东北天座标系,东北天座标系指的是右手座标系下:

• 以正东方向为X轴正半轴
• 以正北方向为Y轴正半轴
• 以向上方向为Z轴正半轴
• 以X正半轴逆时针转向Y轴正半轴角度为角度增加方向.

3.定位层的tf_tree是无人驾驶中很重要的一个组成部分,它描述了车身各个传感器,惯导与baselink,map之间的座标转换关系:

• 需要考虑各个传感器与baselink的转换关系,不然会造成障碍物位置的偏离

4.由于规划层同时与定位层以及感知层进行数据交互,定位层与感知层的信息发布时间往往是不一致的,这样会对规划层的避障规划结果造成比较大的影响.因此建议首先对激光雷达以及惯导在硬件上进行硬同步操作,同时在软件上利用message_filters对两层进行软同步.

代码详述

接口四元数转换欧拉角的方式:

message_filters::Subscriber<nav_msgs::Odometry> odom_sub(n, "/gps/odom", 1);//定义callback函数,这里是与感知信息结合做了一个软同步

void dwa_planner_node::localization (nav_msgs::Odometry odom_msg)
  odom_msg_ = odom_msg;
  tf::Quaternion q(
      odom_msg_.pose.pose.orientation.x,
      odom_msg_.pose.pose.orientation.y,
      odom_msg_.pose.pose.orientation.z,
      odom_msg_.pose.pose.orientation.w);
      tf::Matrix3x3 m(q);
      m.getRPY(roll, pitch, yaw);//将四元数转换为欧拉角的方式之一

or

 tf::getYaw(global_pose.getRotation())//将四元数转换为欧拉角的方式之二

---

tf_tree相关代码

  tf::TransformListener listener;
  static tf::StampedTransform transform_map_to_base;//from map to baselink
  
 try
  {
    
    listener.waitForTransform("/base_link","/map",ros::Time(0),ros::Duration(0.5));//from map to baselink
    listener.lookupTransform("base_link", "map", ros::Time(0), transform_map_to_base);
   
   catch (tf::TransformException& ex)
   {
     ROS_ERROR("%s", ex.what());
     ros::Duration(1.0).sleep();
   }
//主要直接用于座标系之间的转换

tf::TransformListener listener;
geometry_msgs::PointStamped odom_point;
  odom_point.header.stamp=ros::Time();
  odom_point.header.frame_id="/map";
  odom_point.point.x = float(wx);
  odom_point.point.y = float(wy);
  odom_point.point.z = 0;
  geometry_msgs::PointStamped base_point;
  try
  {
   listener.transformPoint("/base_link",odom_point,base_point);
  }//将点由map座标系转换到baselink座标系下
    catch (tf::TransformException& ex)
    {
      ROS_ERROR("%s", ex.what());
      ros::Duration(1.0).sleep();
    }
   wx = base_point.point.x;
   wy = base_point.point.y;

调试经验

• 调试过程中需要注意惯导给出的yaw角是什么座标系下的,是否需要座标转换,在此项目中用的是东北天座标系
• 关于tf_tree,由于baselink,imu,velodyne的座标系之间距离,角度差距有时会很大,因此利用tf_tree是十分必要的,tf_tree相关代码可以参见上一部分