路徑規劃與避障算法(二)---規劃層與定位層的接口定義及數據融合

接口概述

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相關代碼可以參見上一部分