Autoware 笔记 No. 5——基于GNSS的定位
阅读原文时间:2020年11月08日阅读:12

1. 前言

在之前的笔记No.2 中,我们直接采用ndt_matching的方法实现定位,但需要在打开rviz中,通过2D Pose Estimate指定初始位置。加入GNSS后,可以帮助ndt_matching找到初始位置,同时如果ndt_matching在运动过程中匹配失败,GNSS可以帮助重定位。

在/ndt_matching算法中,有两个条件使用GNSS重定位:

(1) 如果设置ndt_matching的config中设置了GNSS,而非Initial Pos,那么ndt_matching会在程序初始运行中采用GNSS作为初值;

(2) 在车辆运行过程中,当topic /ndt_stat中的score值大于600(与地图匹配程度,值越大匹配的越差),同样会使用GNSS重定位。

下面介绍干货

2. GNSS读入数据配置

我们采用某品牌的GNSS设备,通过串口读入信息,所以首先在Autoware中设置为串口读入,如下图:

首先配置GNSS下的Serial GNSS的config,我们的串口GNSS设备的设备名称为/dev/ttyS0,波特率为115200,设置成功后,点击OK保存,然后点击Serial GNSS选项,这时会提示错误:

ERROR: cannot launch node of type [nmea_navsat_driver/nmea_topic_serial_reader]: nmea_navsat_driver
ROS path []=/opt/ros/kinetic/share/ros
ROS path []=/home/a/autoware/ros/src
ROS path []=/home/a/catkin_ws/src
ROS path []=/opt/ros/kinetic/share
ERROR: cannot launch node of type [nmea_navsat_driver/nmea_topic_driver]: nmea_navsat_driver
ROS path []=/opt/ros/kinetic/share/ros
ROS path []=/home/a/autoware/ros/src
ROS path []=/home/a/catkin_ws/src
ROS path []=/opt/ros/kinetic/share

这时由于Autoware中的串口驱动没有装,根据autoware/ros/src/sensing/drivers/gnss/packages/nmea_navsat/scripts/README.md文件,需要下载驱动,运行

$ sudo apt-get install ros-kinetic-nmea-navsat-driver

如果不能更新,也可以从源码下载,然后在autoware的工作空间下安装,源码的下载地址为:https://github.com/ros-drivers/nmea_navsat_driver.git,下载后切换到适合的kinetic分支即可运行。

这时在勾选Serial GNSS选项,通过查看topic /nmea_sentence,如果有输出,且在sentence中有内容,即GNSS设备数据读取成功。/nmea_sentence的数据内容:

---
header:
seq:
stamp:
secs:
nsecs:
frame_id: "/gps"
sentence: "$×××××,0,7847.600,360.000,1.663,0.720,0.0000000,0.0000000,0.00,0.000,0.000,0.000,0.000,0,0,0C*0C"

$*****代表nmea的标准,主流的标准有$GPGGA、$GPGSA、$GPGSV、$GPRMC、$GPVTG、$GPGLL。而我们采用的传感器不是这些标准,所以需要自己写nmea的数据解析。

不同的GNSS设备,sentence的中内容的标准会有不同。

如果想了解nmea,请参考博客:https://blog.csdn.net/jickjiang/article/details/79086202

3. 数据解析

Autoware的数据解析程序为nmea2tfpose包,由于我们采用的设备不是标准协议,但我们的协议和$GPGGA很接近,所以我们采用在$GPGGA的基础上做了改进nmea2tfpose_core.cpp。

我们的修改思路为:

(1) 首先讲基站的经纬度作为原点;

(2) 采用$GPGGA的方式,将经纬度坐标系映射到以基站为原点的平面坐标系;

(3) 通过tf变换,将基站为原点的坐标系的转换到地图坐标系。

我们没有修改源码,而是将nmea2tfpose包拿出来修改,编译,然后单独发布。

nmea2tfpose包最终发布的topic为/gnss_pose。查看/gnss_pose与/ndt_pose发布的数据是否基本一致,如果基本一致,那么成功。这里我公布出部分源码,供大家参考(nmea2tfpose_core.cpp):

// 设置基站经纬度
geo_.set_origin(longitude_, latitude_);

if (nmea.at() == "$×××××")  //heading pitch roll  
{  
  // 直接读取gnss中的roll, pitch, yaw  
  position\_time\_ = stod(nmea.at());  
  roll\_ = stod(nmea.at()) \* M\_PI / .;  
  pitch\_ = - \* stod(nmea.at()) \* M\_PI / .;  
  yaw\_ = -stod(nmea.at()) \* M\_PI / . + M\_PI\*;

  //ROS\_INFO("roll:%f, pitch:%f, yaw:%f.", roll\_,pitch\_,yaw\_);

  // 根据GPGGA计算从经纬度到局部坐标系的映射  
  double lat = stod(nmea.at());  
  double lon = stod(nmea.at());  
  double h = stod(nmea.at());  
  geo\_.set\_llh\_nmea\_degrees(lat, lon, h);  
}

// 基站为原点坐标系到地图坐标系的变换
tf::Pose tf_pose;
tf::poseMsgToTF(pose.pose, tf_pose);
tf::Quaternion q;
q.setRPY(, , 3.2277); // 基站为原点坐标系与地图坐标系夹角
tf::Transform tf_trans(q, tf::Vector3(-28.11, 10.79, )); // 基站为原点坐标系与地图坐标系原点的差
tf_pose = tf_trans.inverse() * tf_pose;
tf::poseTFToMsg(tf_pose, pose.pose);

4. 结果

上面的代码完成后,可以按照Autoware 笔记4的内容进行设置,这时无论车停在轨迹上的任何位置,可以直接找到位置,无需2D Pose Estimate指定初始位置。

手机扫一扫

移动阅读更方便

阿里云服务器
腾讯云服务器
七牛云服务器

你可能感兴趣的文章