基于激光雷达实现三边定位算法开发记录（十一）——动态匹配定位

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

开发平台为ubuntu 18.04 + ros melodic

日期：2022.8.2

本次实现：在《记录（七）》到《记录（九）》中，已经完成了对当前反光柱坐标与预设反光柱坐标进行匹配，并输出匹配上的当前反光柱坐标，并通过坐标转换得到当前反光柱的全局坐标与距离，再而进行三边定位与计算。但是这种方法必须在每次启动机器人时保持在同一原点。因此本次通过改进，在匹配后输出对应的预设坐标点，省去再进行坐标转换，直接进行三边定位。该方法可在建图时保存全局反光柱坐标地图后，在导航中直接定位到机器人的全局坐标。

（一）代码编写

改进反光柱匹配函数，在分别计算距离时保存距离两端端点的对应编号，在利用距离信息进行匹配后，对应保存两端端点的编号。然后遍历匹配上后的数据，当存在两端以上以同一点为顶点的距离时，该点匹配有效，例如当前[1,2][1,3]和预设[a,b][a,c]，通过出现次数得出当前的1点对应预设的a点，找到当前点与预设点的对应关系并存在关联函数map中。

C++ map用法总结（整理）

//反光柱匹配——多边匹配  需要录入全局反光柱坐标进行遍历  效率较低
void reflectorMatching(std::vector< Point > circle,std::vector< std::pair< float, float > > preservation,std::vector< Distance > &matching_coordinates){
   std ::vector<Distance> distance_coordinate; //保存的各个坐标之间的距离
  std ::vector<Distance> distance_circle;     //当前各个反光柱之间的距离
  std ::vector<float> matching;                         //匹配的反光柱编号
  
  Distance matching_circle;   //匹配的反光柱坐标
  Distance dis_circle;
  Distance dis_coordinates;

  std::vector< std::pair< float, float > > num1;
  std::vector< std::pair< float, float > > num2;

  std::map<float,std::pair< float, float > >mapStudent;

  float x_ij;
  float y_ij;
  float distance_ij;

    for(long i = 1; i < preservation.size() ; i++){      //遍历记录的坐标距离
    for(long j = 0;j < i;j++){
        x_ij = fabs(preservation[j].first - preservation[i].first);
        y_ij = fabs(preservation[j].second - preservation[i].second);
        distance_ij = sqrt(pow(x_ij, 2) + pow(y_ij, 2));

        dis_coordinates.distance = distance_ij;
        dis_coordinates.one = i;
        dis_coordinates.two = j;
        distance_coordinate.push_back(dis_coordinates);//保存距离和该段距离两端端点编号
        }
    }

    for(long i = 1; i < circle.size() ; i++){                //遍历当前各个反光柱圆心的坐标距离
      for(long j = 0;j < i;j++){

        x_ij = fabs(circle[j].x - circle[i].x);
        y_ij = fabs(circle[j].y - circle[i].y);
        distance_ij = sqrt(pow(x_ij, 2) + pow(y_ij, 2));

        dis_circle.distance = distance_ij;
        dis_circle.one = i;
        dis_circle.two = j;
        distance_circle.push_back(dis_circle);//保存距离和该段距离两端端点编号
        }
    }
  for(long i = 0;i < distance_circle.size();i++){     //通过距离匹配记录的和当前的坐标距离
      // std::cout << "当前匹配距离：" << distance_circle[i].distance << std:: endl;
    for(long j = 0;j < distance_coordinate.size();j++){
      // std::cout << "预设匹配距离：" << distance_coordinate[j].distance<< std:: endl;
      if( fabs(distance_coordinate[j].distance - distance_circle[i].distance) < 0.02 ) {  //匹配误差 0.02 m
      // std::cout << "能匹配上" << j <<std:: endl;
    
    // 匹配上后将距离和两端点对应保存
    std::pair< float, float >  circle_num ( distance_circle[i].one, distance_circle[i].two);
    std::pair< float, float >  coordinate_num ( distance_coordinate[j].one, distance_coordinate[j].two);
    num1.push_back(circle_num);
    num2.push_back(coordinate_num);
      } else {     
        continue;
      }
    }
  }

  //遍历匹配上后的数据，当存在两端以上以同一点为顶点的距离时，该点匹配有效
  //找到当前点与预设点的对应关系并存在关联函数中
  //例如当前[1,2][1,3]和预设[a,b][a,c]，通过出现次数得出当前的1点对应预设的a点
  for(long i = 1;i < num1.size();i++){
    for(long j = 0;j < i;j++){
        if( num1[i].first == num1[j].first || num1[i].first == num1[j].second){
          if( num2[i].first == num2[j].first || num2[i].first == num2[j].second){          
            mapStudent.insert(std::map<float, std::pair< float, float > >::value_type (num1[i].first  , preservation[num2[i].first]));
          }else if(num2[i].second == num2[j].second || num2[i].second == num2[j].first){
            mapStudent.insert(std::map<float, std::pair< float, float > >::value_type (num1[i].first  , preservation[num2[i].second]));
          }
        }else if( num1[i].second == num1[j].first || num1[i].second == num1[j].second){
          if( num2[i].first == num2[j].first || num2[i].first == num2[j].second){          
            mapStudent.insert(std::map<float, std::pair< float, float > >::value_type (num1[i].second  , preservation[num2[i].first]));
          }else if(num2[i].second == num2[j].second || num2[i].second == num2[j].first){
            mapStudent.insert(std::map<float, std::pair< float, float > >::value_type (num1[i].second  , preservation[num2[i].second]));
          }
        }
        else{
            continue;
        }
    }
  }

    for(auto iter = mapStudent.begin(); iter != mapStudent.end(); ++iter) {   //遍历关联函数
      set_matching_marker_property(circle[iter->first].x ,circle[iter->first].y,iter->first);    //发布匹配标记

       std::cout << "匹配成功，编号：" << iter->first << std::endl;
       std::cout << "x轴："<<iter->second.first << "y轴："<< iter->second.second << "  距离：" << circle[iter->first].range << std::endl;

      matching_circle.distance = circle[iter->first].range;
      matching_circle.one = iter->second.first;
      matching_circle.two = iter->second.second;
      matching_coordinates.push_back(matching_circle);                   //记录匹配上的反光柱距离和对应预存坐标
    }
}

修改处理激光信息函数，由于匹配函数输出的是预存的反光柱全局坐标和对应的距离，这里就不用再去对数据做处理，直接将之导入到三边定位算法中，即可得到机器人的全局坐标。

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

  point points[3];
  point targetPose;

  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);     //读文件状态下 保存的数据与当前的数据进行匹配                       
    if(matching_coordinates.size() > 2){   //三个点以上时有效

      //基点1
      points[0].x= matching_coordinates[0].one;
      points[0].y = matching_coordinates[0].two;
      float r1 = matching_coordinates[0].distance; //基点距离未知点的距离


      //基点2
      points[1].x = matching_coordinates[1].one;
      points[1].y = matching_coordinates[1].two;
      float r2=matching_coordinates[1].distance;//基点距离未知点的距离

      //基点3
      points[2].x = matching_coordinates[2].one;
      points[2].y = matching_coordinates[2].two;
      float r3 =matching_coordinates[2].distance;//基点距离未知点的距离


      trilateration_1(points[0], points[1], points[2], r1, r2, r3,targetPose);

      //结果为激光雷达坐标，进行转换得到小车坐标
      targetPose.x = targetPose.x - WHEELBASE * cos(odom_point.Angle_yaw);
      targetPose.y = targetPose.y - WHEELBASE * sin(odom_point.Angle_yaw);

      printf("小车全局坐标:x = %3.5f,y = %3.5f\r\n", targetPose.x, targetPose.y);//打印未知点坐标
    }
  }
}

（二）运行结果

首先先进行建图，在建图过程中录入全局反光柱坐标。

然后存手动模式将机器人移动到原点前1.1m，然后进行定位，从结果显示，机器人直接利用保存的反光柱全局坐标加上对应距离进行机器人定位，定位结果较为理想。

 （三）问题与下一目标

问题：目前的主要误差来源于录制反光柱全局坐标的时候，由于机器人移动导致反光柱识别位置出现误差，将一个反光柱识别为多个距离极近的多个反光柱。

目标：落地测试定位精度。