基于激光雷达实现三边定位算法开发记录（十二）——位姿发布

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

开发平台为ubuntu 18.04 + ros melodic

日期：2022.8.6

本次实现：在计算出机器人的全局坐标之后，通过反光柱的全局坐标与当前坐标之间的关系，可以解算出小车当前在全局坐标系下的偏角，加上坐标就是当前小车的全局位姿。再通过tf变换将该位姿广播。

（一）代码编写

首先是计算当前小车角度，将三边定位相关代码放入匹配函数中，将匹配结果直接传入三边定位函数，方便在下面计算小车当前角度。

计算角度原理为当前反光柱在小车坐标系下的角度与原来建图时反光柱在全局坐标系下的角度的差值，即为小车当前变化的角度，由于原来通过里程计得到小车位姿，在计算后会出现较大误差，这里取平均值。

float R_yaw;
//反光柱匹配——多边匹配  需要录入全局反光柱坐标进行遍历  效率较低
void reflectorMatching(std::vector< Point > circle,std::vector< std::pair< float, float > > preservation,std::vector< Distance > &matching_coordinates){
......
      matching_coordinates.push_back(matching_circle);                   //记录匹配上的反光柱距离和对应预存坐标
    }
      point points[3];

    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,yaw = %3.5f\r\n", targetPose.x, targetPose.y,odom_point.Angle_yaw);//打印未知点坐标
    } else {
      ROS_ERROR("Insufficient effective points!!!");
    }
    R_yaw = 0;
    for(auto iter = mapStudent.begin(); iter != mapStudent.end(); ++iter) {   //遍历关联函数

      float dx = iter->second.first - targetPose.x;
      float dy = iter->second.second - targetPose.y;
      float c_yaw = atan2(dy,dx);

      //转换为全局坐标
      float r_x = odom_point.Angle_x+(WHEELBASE*cos(odom_point.Angle_yaw))+(circle[iter->first].x*cos(odom_point.Angle_yaw))-(circle[iter->first].y*sin(odom_point.Angle_yaw)); 
      float r_y = odom_point.Angle_y+(WHEELBASE*sin(odom_point.Angle_yaw))+(circle[iter->first].x*sin(odom_point.Angle_yaw))+(circle[iter->first].y*cos(odom_point.Angle_yaw));  

      float d_yaw = atan2(r_y,r_x);
      float e_yaw = c_yaw - d_yaw;

      R_yaw += e_yaw;

    }
    R_yaw /= mapStudent.size();
}

于是便得到了小车的世界坐标和位姿，再经过一系列变换，最后发布一个tf变换，关于odom-----map两个坐标系的TF变换，传达给navigation导航包的move_base路径规划模块。

参考：2D激光SLAM：：AMCL发布的odom----map坐标TF变换解读

将其中以里程计作为真实位姿改为计算出来的位姿。

            /******************发布坐标变换**********************************/
            //取 目标坐标、odom角度+角度差值 作为真实位姿
            pose_vector_t truePose_v;
            truePose_v.v[0]=targetPose.x;
            truePose_v.v[1]=targetPose.y;
            truePose_v.v[2]=tf::getYaw(odomMsg->pose.pose.orientation) + R_yaw;

（二）运行结果

 （三）总结

至此，整个三边定位算法的大体框架便已经完成，从结果来看，代码框架还很粗糙，各个环节还需要很大的改进。

从实际落地情况来看，使用当前算法时，由于过度依赖里程计和IMU，导致这两者的误差会累计到小车上，从而使小车会出现角度偏移，地图跑丢的情况。解决方法是将小车第一帧记录的反光柱为基础进行三边定位，获取小车当前位置，再以此为基础进行运动，并在运动过程中记录新出现的反光柱坐标。

由于后面组内的技术大佬将整套代码进行了重写和优化，基本上和现在的代码不一样了，且大体框架已经搭建完毕，剩下的只是对于各个环节之间的优化，例如寻找更加高效精准的方法完善识别，匹配，定位功能。所以这里便不再更新了。