基于激光雷达实现三边定位算法开发记录（八）——坐标转换

22年暑假实习过程中第一个开发任务，基于激光雷达实现三边定位功能

开发平台为ubuntu 18.04 + ros melodic

日期：2022.7.29

本次实现：在匹配别出目标反光柱并获取目标反光柱的坐标后，需要将反光柱坐标从当前的激光坐标系转换到全局坐标系下，以便后面三边定位算法算出来的小车坐标是全局坐标系中的坐标。

本次算法原理来源于文献：基于激光雷达的AGV路标定位技术研究

（一）原理

以上文献截图

（二）代码编写

由于坐标转换关系需要用到当前小车的位置和位姿，所以需要先将激光雷达装回车上，在main函数中加入读取/odom信息的命令，注意应该在激光前面

  ros::Subscriber subOdom = nh.subscribe("/odom", 10, OdomCB);

将里程计信息传入里程计信息处理函数，通过全局变量传出当前的x，y，yaw

struct Outline {
  float Angle_x;
  float Angle_y;
  float Angle_yaw;
};

Outline odom_angle;    

//处理里程计信息
void OdomCB(const nav_msgs::Odometry::ConstPtr &msg){
    double x, y, z;
    double roll, pitch, yaw;
    x = msg->pose.pose.position.x;
    y = msg->pose.pose.position.y;
    z = msg->pose.pose.position.z;
    tf::Quaternion quat;                                     //定义一个四元数
    tf::quaternionMsgToTF(msg->pose.pose.orientation, quat); //取出方向存储于四元数
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
    
    odom_angle.Angle_x = x;
    odom_angle.Angle_y = y;
    odom_angle.Angle_yaw = yaw;
}

 修改反光柱圆心数据文件读写函数，在写文件时将反光柱坐标转化为全局坐标保存

//反光柱圆心数据文件读写
void fileOperation(std::string fileaddres,const std::vector< Point > point, std::vector< std::pair< float, float > > &result, bool fileoperation_state){
    if(fileoperation_state){                                 //将数据以[[x,y][x,y][x,y]]格式保存到文件                                    
      std::ofstream outfile(fileaddres,std::ios::trunc );    //CircleData.txt里面的内容会被清空,重新开始写
      if (!outfile.is_open())                                //检测是否打开，打不开请检查文件地址
      {
      std::cout << "cannot open file" << std::endl;
      }
      outfile << "[";
      for (long i = 0; i< point.size(); i++) { 

        //转换为全局坐标保存
        float r_x = odom_angle.Angle_x+(WHEELBASE*cos(odom_angle.Angle_yaw))+(point[i].x*cos(odom_angle.Angle_yaw))-(point[i].y*sin(odom_angle.Angle_yaw)); 
        float r_y = odom_angle.Angle_y+(WHEELBASE*sin(odom_angle.Angle_yaw))+(point[i].x*sin(odom_angle.Angle_yaw))+(point[i].y*cos(odom_angle.Angle_yaw));        
               
        outfile << "[" << r_x << "," << r_y << "]" ;
        
      }
      outfile << "]";
      std::cout << "Data transfer succeeded" << std::endl;
      outfile.close();
    }else{                                                                     //读取文件数据
      std::string xy;
      std::string  error_return;
      std::ifstream infile(fileaddres); 

      if (!infile.is_open())                                       //检测是否打开，打不开请检查文件地址
      {
      std::cout << "cannot open file" << std::endl;
      }
      infile >> xy;
      if(parseVVF(xy, result, error_return)){   //将数据处理成(x,y)形式
        std::cout << error_return << std::endl;
      }else{
        std::cout << error_return << std::endl;
      }
      infile.close();
    }
}

修改处理激光信息函数，在上次读取到匹配上的反光柱当前坐标的基础上，将该坐标转换为全局坐标

//处理激光信息
void scan_CB(sensor_msgs::LaserScan scan) {                                            
  std::vector< Point > circle;                                                                                //当前反光柱数据
  std::vector< std::pair< float, float > > preservation_coordinates;   //保存的反光柱坐标集合
  std::vector< std::pair< float, float > > matching_coordinates;          //匹配的反光柱坐标集合

  std::string fileaddress = "/home/lanhai/lanhai_ws-DTEST/src/HAL/trilateration/CircleData.txt";   //反光柱坐标数据文件地址


  bool fileoperation_state = false;//true;      //文件读写标志为，true为写，false为读

  for (int i = 0; i < 5; i++) {
    circle = findCircle(scan);                                                                                                                       //获取各个反光柱圆心
  }
  fileOperation(fileaddress,circle,preservation_coordinates,fileoperation_state);        //对获取的反光柱数据进行读写文件处理
  if(!fileoperation_state){
  reflectorMatching(circle,preservation_coordinates,matching_coordinates);     //读文件状态下 保存的数据与当前的数据进行匹配                       
  for(int i=0;i<matching_coordinates.size();i++){
    //将匹配出来的反光住坐标转换为全局坐标
    float r_x = odom_angle.Angle_x+(WHEELBASE*cos(odom_angle.Angle_yaw))+(matching_coordinates[i].first*cos(odom_angle.Angle_yaw))-(matching_coordinates[i].second*sin(odom_angle.Angle_yaw)); 
    float r_y = odom_angle.Angle_y+(WHEELBASE*sin(odom_angle.Angle_yaw))+(matching_coordinates[i].first*sin(odom_angle.Angle_yaw))+(matching_coordinates[i].second*cos(odom_angle.Angle_yaw));  

    matching_coordinates[i].first = r_x;
    matching_coordinates[i].second = r_y; 
    std::cout << "全局坐标下：：x轴："<<matching_coordinates[i].first << "y轴："<<matching_coordinates[i].second << std::endl;        
    }     
  }
}

（三）运行结果

可以看到，匹配出来的反光柱坐标以及转换为全局坐标，且和保存的坐标相差不大

 （四）问题和下一目标

问题：由于每次重启小车的时候都会重启里程计，/odom的x，y会清零，所以当圆心坐标保存时和读取时全局的原点不一样时，会出现偏差。

目标：利用本次获取的反光柱全局坐标计算出小车的全局坐标。