移动机器人——里程计矫正

一、问题分析

机器人可以基于码盘数据和底盘运动学模型进行航迹推演，得到机器人的轨迹，但实际轨迹与推演轨迹存在误差，分析原因如下：

• 底盘实际尺寸与理论模型存在偏差，如轮子半径，两轮间距等；
• 底盘在运动过程中打滑或路面不平整导致偏差；

注意：

• 方向上的偏差较平移的偏差影响更大，会随着移动扩大误差。
• 码盘里程计受电控，底盘机械结构影响，在路况复杂和室外场景下，误差会较大。

解决方案

• 直接线性（此处使用该方法）
• 基于模型（可参考2d Laser 和 Odomter 内外参数标定工具原理及使用方法）

二、航迹推算

• 欧拉积分

  该积分误差主要来源于平移积分过程中认为角度恒定，这在直线运动时不受影响，以及$△t$很小时误差影响也较小。
  $$\left[\begin{array}{c}{x}_{t+1}\\ y_{t+1}\\ {\theta }_{t+1}\end{array}\right]=\left[\begin{array}{c}{x}_t\\ y_t\\ {\theta }_t\end{array}\right]+\left[\begin{array}{c}\cos ({\theta }_t)\ast v△t\\ \sin ({\theta }_t)\ast v△t\\ w△t\end{array}\right]$$

• 二阶Runge-Kutta积分

  该积分过程中认为角度为运动前后的均值，误差比欧拉积分小一点。
  $$\left[\begin{array}{c}{x}_{t+1}\\ y_{t+1}\\ {\theta }_{t+1}\end{array}\right]=\left[\begin{array}{c}{x}_t\\ y_t\\ {\theta }_t\end{array}\right]+\left[\begin{array}{c}\cos ({\theta }_t+\frac{w△t}{2})\ast v△t\\ \sin ({\theta }_t+\frac{w△t}{2})\ast v△t\\ w△t\end{array}\right]$$

三、激光匹配

在移动机器人中，里程计一般由底盘编码器提供，但其存在误差，一般可将其相对坐标变换$[R,t]$（当前与前一状态）作为激光匹配算法的first guess，然后通过迭代匹配优化作为真值用于里程计的标定（实际上并不是真值）。

此处使用srf_laser_odometry替代ICP实现激光里程计。

四、算法实现

4.1 原理分析

$u(x,y,\theta )$为增加值，非累加值，每一小段构成一组方程。

航迹推算在odom坐标系下进行，每个步距的移动在机器人base_link坐标系下，需要转换到odom坐标系下，再进行累加。

1. 计算激光雷达两帧之间的scan-match数据作为真值$u_i^{\ast }$

2. 获取里程计两帧之间测量数据为测量值$u_i$

3. 构造超定方程组$X\ast u_i=u_i^{\ast }$，$X$为二维变换矩阵

   $$X=\left[\begin{array}{ccc}{x}_{11}& x_{12}& x_{13}\\ x_{21}& x_{22}& x_{23}\\ x_{31}& x_{32}& x_{33}\end{array}\right]$$

   对于每一组数据有：

   $$\left[\begin{array}{ccc}{x}_{11}& x_{12}& x_{13}\\ x_{21}& x_{22}& x_{23}\\ x_{31}& x_{32}& x_{33}\end{array}\right]\left[\begin{array}{c}{u}_{ix}\\ u_{it}\\ u_{i\theta }\end{array}\right]=\left[\begin{array}{c}{u}_{ix}^{\ast }\\ u_{iy}^{\ast }\\ u_{i\theta }^{\ast }\end{array}\right]$$

   该变换可改写为$Ax=b$的形式：

   $$\begin{gathered} n\times 9\ast 9\times 1=n\times 1 \\ \left[\begin{array}{ccccccccc}{u}_{ix}& u_{iy}& u_{i\theta }& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& u_{ix}& u_{iy}& u_{i\theta }& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& u_{ix}& u_{iy}& u_{i\theta }\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\end{array}\right]\left[\begin{array}{c}{x}_{11}\\ ⋮\\ x_{33}\end{array}\right]=\left[\begin{array}{c}{u}_{ix}^{\ast }\\ u_{iy}^{\ast }\\ u_{i\theta }^{\ast }\\ ⋮\end{array}\right] \end{gathered}$$

   其中

   $$\begin{gathered} A=\begin{array}{c}{A}_1\\ ⋮\\ A_n\end{array},b=\begin{array}{c}{b}_1\\ ⋮\\ b_n\end{array} \\ x=(A^{T}A{)}^{-1}{A}^{T}b \end{gathered}$$

4. 使用QR分解求解$Ax=b$

4.2 工程实现

激光匹配

通过第一帧初始化基本参数并作为参考帧，之后不断读入当前帧与参考帧做计算获取坐标变换。

注意

1. 雷达安装位置不在小车中心，则需要将变换结果转换到小车坐标系上。
2. 高频雷达可两到三帧匹配一次，降低计算量提高稳定性，但会降低匹配结果频率。
3. 旋转匹配因运动畸变的原因会造成一定偏差。

class SRFScanMatch
{
public:
  SRFScanMatch();

  /**
   * @brief 初始化srf
   *
   * @param last_scan 第一帧雷达信息
   */
  void init(sensor_msgs::LaserScan scan_msg);

  /**
   * @brief 激光匹配
   *
   * @param current_scan
   * @return Eigen::Vector3d
   */
  Eigen::Vector3d scanMatch(sensor_msgs::LaserScan current_scan);

private:
  // Core class of SRF
  SRF_RefS srf_obj;

  // 坐标变换
  mrpt::poses::CPose3D laser_pose_on_robot_inv_;
  mrpt::poses::CPose3D robot_pose_;
  nav_msgs::Odometry initial_robot_pose_;
  tf2_ros::Buffer tf_Buffer_;
  std::string base_frame_id_ = "base_link";

  bool first_laser_scan_;
  double laser_min_range_, laser_max_range_;
};

SRFScanMatch::SRFScanMatch()
{
  // tf2_ros::TransformListener tfListener(tf_Buffer_);

  initial_robot_pose_.pose.pose.position.x = 0;
  initial_robot_pose_.pose.pose.position.y = 0;
  initial_robot_pose_.pose.pose.position.z = 0;
  initial_robot_pose_.pose.pose.orientation.w = 1;
  initial_robot_pose_.pose.pose.orientation.x = 0;
  initial_robot_pose_.pose.pose.orientation.y = 0;
  initial_robot_pose_.pose.pose.orientation.z = 0;

  first_laser_scan_ = true;
}

void SRFScanMatch::init(sensor_msgs::LaserScan scan_msg)
{
  tf2_ros::TransformListener tfListener(tf_Buffer_);

  const unsigned int scan_size = scan_msg.ranges.size();            // Num of samples (size) of the scan laser
  const float fov = fabs(scan_msg.angle_max - scan_msg.angle_min);  // Horizontal Laser's FOV

  // 初始化，雷达数量，视场角，HYBRID模式
  srf_obj.initialize(scan_size, fov, 2);

  // 获取雷达坐标系
  geometry_msgs::TransformStamped laser_to_base;
  mrpt::poses::CPose3D LaserPoseOnTheRobot;
  try
  {
    laser_to_base = tf_Buffer_.lookupTransform(base_frame_id_, scan_msg.header.frame_id, ros::Time(0));

    // transform -> mrpt::CPose3D
    geometry_msgs::Pose laser_transform;
    laser_transform.orientation = laser_to_base.transform.rotation;
    laser_transform.position.x = laser_to_base.transform.translation.x;
    laser_transform.position.y = laser_to_base.transform.translation.y;
    laser_transform.position.z = laser_to_base.transform.translation.z;
    mrpt_bridge::convert(laser_transform, LaserPoseOnTheRobot);
    ROS_INFO_STREAM(laser_transform);
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
  }

  // 获取雷达坐标系
  geometry_msgs::TransformStamped base_to_laser;
  try
  {
    base_to_laser = tf_Buffer_.lookupTransform(scan_msg.header.frame_id, base_frame_id_, ros::Time(0));
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("%s", ex.what());
  }
  // transform -> mrpt::CPose3D
  geometry_msgs::Pose base_transform;
  base_transform.orientation = base_to_laser.transform.rotation;
  base_transform.position.x = base_to_laser.transform.translation.x;
  base_transform.position.y = base_to_laser.transform.translation.y;
  base_transform.position.z = base_to_laser.transform.translation.z;
  mrpt_bridge::convert(base_transform, laser_pose_on_robot_inv_);
  ROS_INFO_STREAM(base_transform);

  // Robot initial pose
  mrpt::poses::CPose3D robotInitialPose;
  geometry_msgs::Pose _src = initial_robot_pose_.pose.pose;
  mrpt_bridge::convert(_src, robotInitialPose);

  // Set the Laser initial pose = Robot_initial_pose + LaserPoseOnTheRobot
  srf_obj.laser_pose = mrpt::poses::CPose2D(robotInitialPose + LaserPoseOnTheRobot);
  srf_obj.laser_oldpose = mrpt::poses::CPose2D(robotInitialPose + LaserPoseOnTheRobot);

  ROS_INFO("[SRF] Configuration Done.");
}

Eigen::Vector3d SRFScanMatch::scanMatch(sensor_msgs::LaserScan last_scan)
{
  // Initialize module on first scan
  if (first_laser_scan_)
  {
    init(last_scan);
    first_laser_scan_ = false;

    // Load first scan and build pyramid
    for (unsigned int i = 0; i < last_scan.ranges.size(); i++)
      srf_obj.range_wf(i) = last_scan.ranges[i];
    srf_obj.createScanPyramid();

    // Set laser min-max distances (if not set as parameters)
    laser_min_range_ = 0.0;
    laser_max_range_ = 0.99f * last_scan.range_max;
    return Eigen::Vector3d{ 0, 0, 0 };
  }
  else
  {
    // copy laser scan to internal srf variable
    for (unsigned int i = 0; i < last_scan.ranges.size(); i++)
    {
      // Check min-max distances, e.g. to avoid including points of the own robot
      if ((last_scan.ranges[i] > laser_max_range_) || (last_scan.ranges[i] < laser_min_range_))
      {
        srf_obj.range_wf(i) = 0.f;  // invalid measurement
        last_scan.ranges[i] = 0.0;
      }
      else
        srf_obj.range_wf(i) = last_scan.ranges[i];
    }

    // Process odometry estimation
    srf_obj.odometryCalculation();

    robot_pose_ = srf_obj.laser_pose + laser_pose_on_robot_inv_;
    return Eigen::Vector3d{ robot_pose_.x(), robot_pose_.y(), robot_pose_.yaw() };
    //return Eigen::Vector3d{ srf_obj.laser_pose.x(), srf_obj.laser_pose.y(), srf_obj.laser_pose.phi() };
  }
  return Eigen::Vector3d{ 0, 0, 0 };
}

里程计标定

读入真值和测量值，并设置最大数据防止过大，再通过Eigen实现QR分解计算最小二乘解。

class OdomCalib
{
private:
  Eigen::MatrixXd A_;  // 里程计测量值
  Eigen::VectorXd b_;  // 移动机器人真值
  int data_len_, now_len_;

public:
  OdomCalib();
  ~OdomCalib();

  /**
   * @brief 设置数据长度
   *
   * @param len
   */
  void setDataLen(int len);

  /**
   * @brief 构建最小二乘需要的超定方程组,Ax = b
   *
   * @param odom_value 里程计值
   * @param true_value 移动真值
   * @return true
   * @return false
   */
  bool addData(Eigen::Vector3d odom_value, Eigen::Vector3d true_value);

  /**
   * @brief 解最小二乘
   *
   * @return Eigen::Matrix3d
   */
  Eigen::Matrix3d solveFun();

  /**
   * @brief 用于判断数据是否满，满即进行最小二乘计算
   *
   * @return true
   * @return false
   */
  bool isFull();

  /**
   * @brief 数据清零
   *
   */
  void cleanData();
};

OdomCalib::OdomCalib()
{
  data_len_ = 0;
  now_len_ = 0;
}

OdomCalib::~OdomCalib(){}

void OdomCalib::setDataLen(int len)
{
  data_len_ = len;
  A_.conservativeResize(len * 3, 9);
  b_.conservativeResize(len * 3);
  A_.setZero();
  b_.setZero();
}

bool OdomCalib::addData(Eigen::Vector3d odom_value, Eigen::Vector3d true_value)
{
  if (now_len_ < INT_MAX)
  {
    // 构建超定方程组
    A_(now_len_ % data_len_ * 3, 0) = odom_value(0);
    A_(now_len_ % data_len_ * 3, 1) = odom_value(1);
    A_(now_len_ % data_len_ * 3, 2) = odom_value(2);
    A_(now_len_ % data_len_ * 3 + 1, 3) = odom_value(0);
    A_(now_len_ % data_len_ * 3 + 1, 4) = odom_value(1);
    A_(now_len_ % data_len_ * 3 + 1, 5) = odom_value(2);
    A_(now_len_ % data_len_ * 3 + 2, 6) = odom_value(0);
    A_(now_len_ % data_len_ * 3 + 2, 7) = odom_value(1);
    A_(now_len_ % data_len_ * 3 + 2, 8) = odom_value(2);

    b_(now_len_ % data_len_ * 3) = true_value(0);
    b_(now_len_ % data_len_ * 3 + 1) = true_value(1);
    b_(now_len_ % data_len_ * 3 + 2) = true_value(2);

    now_len_++;
    return true;
  }
  else
  {
    return false;
  }
}

bool OdomCalib::isFull()
{
  if (now_len_ % data_len_ == 0 && now_len_ >= 1)
  {
    now_len_ = data_len_;
    return true;
  }
  else
    return false;
}

Eigen::Matrix3d OdomCalib::solveFun()
{
  Eigen::Matrix3d correct_matrix;

  // 求解线性最小二乘
  Eigen::VectorXd correct_vector = A_.colPivHouseholderQr().solve(b_);
  ROS_INFO_STREAM("correct_vector:" << correct_vector);
  correct_matrix << correct_vector(0), correct_vector(1), correct_vector(2), correct_vector(3), correct_vector(4),
      correct_vector(5), correct_vector(6), correct_vector(7), correct_vector(8);

  return correct_matrix;
}

void OdomCalib::cleanData()
{
  A_.setZero();
  b_.setZero();
}

校准节点

订阅里程计和雷达消息，然后通过tf2_ros::MessageFilter实现消息的时间对齐，再计算两帧之间的步距并添加到里程计标定中，最后进行计算矫正矩阵并对里程计进行矫正处理获得矫正路径。

class OdomCalibNode
{
private:
  // ros
  ros::NodeHandle* nh_;
  nav_msgs::Path path_odom_, path_scan_;
  ros::Time current_time_;

  std::string odom_frame_;
  std::string base_frame_;

  // odom & scan进行位姿积分
  Eigen::Vector3d scan_pos_cal_;
  Eigen::Vector3d odom_pos_cal_;

  //用来储存两帧之间的里程计的增量
  std::vector<Eigen::Vector3d> odom_increments_;

  Eigen::Vector3d odom_last_pos_, scan_last_pos_;

  //设置接收器
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;

  //初始化接发器
  ros::Subscriber calib_flag_sub_, odom_sub_;
  ros::Publisher odom_path_pub_, scan_path_pub_, calib_path_pub_;

  //时间同步
  message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
  tf2_ros::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;

  //初始化激光匹配和里程计标定
  ScanMatch scan_match;
  SRFScanMatch srf_scan_match;
  OdomCalib odom_calib;
  bool first_laser_scan_;

  //坐标变换
  tf2_ros::TransformBroadcaster broadcaster;
  geometry_msgs::TransformStamped transformStamped;

public:
  OdomCalibNode(ros::NodeHandle* nh);
  ~OdomCalibNode();

  /**
   * @brief 标定回调
   *
   * @param msg
   */
  void calibFlagCallBack(const std_msgs::Empty& msg);

  /**
   * @brief 雷达回调
   *
   * @param msg
   */
  void scanCallBack(const sensor_msgs::LaserScan::ConstPtr& msg);

  /**
   * @brief 读取里程计信息并发布坐标变换
   *
   * @param msg
   */
  void odomCallBack(const nav_msgs::Odometry::ConstPtr& msg);

  /**
   * @brief 发布odom & laser path
   *
   * @param pose
   * @param path
   * @param mcu_path_pub_
   */
  void pubMsg(Eigen::Vector3d& pose, nav_msgs::Path& path, ros::Publisher& mcu_path_pub);

  /**
   * @brief 发布correct path
   *
   * @param path_eigen
   * @param path_pub_
   */
  void publishPathEigen(std::vector<Eigen::Vector3d>& path_eigen, ros::Publisher& path_pub);

  /**
   * @brief 获取当前机器人在odom位姿
   *
   * @param pose 返回位姿
   * @param t 时间点
   * @return true
   * @return false
   */
  bool getOdomPose(Eigen::Vector3d& pose, const ros::Time& t);

  /**
   * @brief 计算当前位姿与前一位姿的距离
   *
   * @param last_pose 前一位姿
   * @param now_pose 当前位姿
   * @return Eigen::Vector3d
   */
  Eigen::Vector3d calDeltaDistence(Eigen::Vector3d last_pose, Eigen::Vector3d now_pose);

  /**
   * @brief 欧垃角转旋转变换矩阵
   *
   * @param yaw
   * @param pitch
   * @param roll
   * @return Eigen::Matrix3d
   */
  Eigen::Matrix3d eul2rotm(double yaw, double pitch, double roll);
};

OdomCalibNode::OdomCalibNode(ros::NodeHandle* nh) : nh_(nh), tf_listener_(tf_buffer_)
{
  // 设置坐标系
  if (!nh_->getParam("odom_frame", odom_frame_))
    odom_frame_ = "odom";
  if (!nh_->getParam("base_frame", base_frame_))
    base_frame_ = "base_link";

  // 初始化变量
  scan_pos_cal_.setZero();
  odom_pos_cal_.setZero();
  odom_increments_.clear();
  odom_last_pos_.setZero();
  scan_last_pos_.setZero();
  first_laser_scan_ = true;

  // 初始化里程计标定
  odom_calib.setDataLen(150);
  odom_calib.cleanData();

  //最小二乘的解算
  calib_flag_sub_ = nh_->subscribe("calib_flag", 5, &OdomCalibNode::calibFlagCallBack, this);

  //发布路径
  odom_path_pub_ = nh_->advertise<nav_msgs::Path>("odom_path_pub_", 1, true);
  scan_path_pub_ = nh_->advertise<nav_msgs::Path>("scan_path_pub_", 1, true);
  calib_path_pub_ = nh_->advertise<nav_msgs::Path>("calib_path_pub_", 1, true);
  current_time_ = ros::Time::now();

  path_odom_.header.stamp = current_time_;
  path_scan_.header.stamp = current_time_;
  path_odom_.header.frame_id = "odom";
  path_scan_.header.frame_id = "odom";

  // odom_sub_ = nh_->subscribe<nav_msgs::Odometry>("/odom", 10, &OdomCalibNode::odomCallBack, this);

  //进行里程计和激光雷达数据的同步
  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(*nh_, "/scan", 10);
  scan_filter_ =
      new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_buffer_, odom_frame_, 10, *nh_);
  scan_filter_->registerCallback(boost::bind(&OdomCalibNode::scanCallBack, this, _1));

  ROS_INFO_STREAM("Calibration Online,Wait for Data");
}

OdomCalibNode::~OdomCalibNode()
{
}

void OdomCalibNode::calibFlagCallBack(const std_msgs::Empty& msg)
{
  Eigen::Matrix3d correct_matrix = odom_calib.solveFun();
  Eigen::Matrix3d tmp_transform_matrix;
  ROS_INFO_STREAM("correct_matrix:" << correct_matrix);

  //计算矫正之后的路径
  Eigen::Vector3d calib_pos(0, 0, 0);             //矫正之后的位姿
  std::vector<Eigen::Vector3d> calib_path_eigen;  //矫正之后的路径
  for (int i = 0; i < odom_increments_.size(); i++)
  {
    Eigen::Vector3d odom_inc = odom_increments_[i];
    Eigen::Vector3d correct_inc = correct_matrix * odom_inc;

    double c = cos(calib_pos(2) + correct_inc(2) / 2);
    double s = sin(calib_pos(2) + correct_inc(2) / 2);
    tmp_transform_matrix << c, -s, 0, s, c, 0, 0, 0, 1;
    calib_pos += tmp_transform_matrix * correct_inc;

    calib_path_eigen.push_back(calib_pos);
  }

  //发布矫正之后的路径
  publishPathEigen(calib_path_eigen, calib_path_pub_);

  // 写入参数
  std::vector<double> correct_vector{ correct_matrix(0, 0), correct_matrix(0, 1), correct_matrix(0, 2),
                                      correct_matrix(1, 0), correct_matrix(1, 1), correct_matrix(1, 2),
                                      correct_matrix(2, 0), correct_matrix(2, 1), correct_matrix(2, 2) };
  ros::param::set("/odom_calib", correct_vector);

  // 矫正完毕，退出订阅
  scan_filter_sub_->unsubscribe();
  ROS_INFO_STREAM("calibration over");

  std::string pkg_path = ros::package::getPath("robot_driver");
  std::string param_file = pkg_path + "/param/odom_calib.yaml";
  YAML::Node yamlConfig = YAML::LoadFile(param_file);

  //修改参数
  yamlConfig["odom_calib"] = correct_vector;

  //写
  std::ofstream file;
  file.open(param_file);
  file.flush();
  file << yamlConfig;
  file.close();
}

void OdomCalibNode::scanCallBack(const sensor_msgs::LaserScan::ConstPtr& msg)
{
  static long int dataCnt = 0;
  sensor_msgs::LaserScan scan;
  Eigen::Vector3d odom_pose;               //底盘编码器里程计位姿变换
  Eigen::Vector3d scan_pose;               //激光里程计位姿变换
  Eigen::Vector3d d_point_odom;            //里程计计算的dpose
  Eigen::Vector3d d_point_scan;            //激光的scanmatch计算的dpose
  Eigen::MatrixXd transform_matrix(3, 3);  //临时的变量

  d_point_odom << 0, 0, 0;

  double c, s;
  scan = *msg;

  //得到对应的里程计数据
  if (!getOdomPose(odom_pose, msg->header.stamp))
    return;

  //初始化srf雷达信息和里程计当前信息
  if (first_laser_scan_)
  {
    scan_pose = srf_scan_match.scanMatch(scan);
    first_laser_scan_ = false;

    odom_last_pos_ = odom_pose;
    return;
  }

  //若静止则不计算
  if (abs(odom_pose(0) - odom_last_pos_(0)) < 0.0001 && abs(odom_pose(1) - odom_last_pos_(1)) < 0.0001 &&
      abs(odom_pose(2) - odom_last_pos_(2)) < tf2Radians(0.01))
  {
    return;
  }

  //前后两帧里程计的位姿差
  d_point_odom = calDeltaDistence(odom_last_pos_, odom_pose);

  //如果运动的距离太短，则不进行处理．
  if (std::isnan(d_point_odom(0)) || std::isnan(d_point_odom(1)) || std::isnan(d_point_odom(2)))
  {
    return;
  }
  if (abs(d_point_odom(0)) < 0.05 && abs(d_point_odom(1)) < 0.05 && abs(d_point_odom(2)) < tf2Radians(3))
  {
      return;
  }

  // srf scan match计算
  scan_pose = srf_scan_match.scanMatch(scan);
  d_point_scan = calDeltaDistence(scan_last_pos_, scan_pose);

  // 去除跳变
  // if (abs(d_point_scan(0)) > 0.1 || abs(d_point_scan(1)) > 0.1 || abs(d_point_scan(2)) > tf2Radians(10))
  //{
  //  return;
  //}

  // 记录当前位姿
  scan_last_pos_ = scan_pose;
  odom_last_pos_ = odom_pose;

  // 里程计累计的位姿,for odom_path visualization
  c = cos(odom_pos_cal_(2) + d_point_odom(2) / 2);
  s = sin(odom_pos_cal_(2) + d_point_odom(2) / 2);
  transform_matrix << c, -s, 0, s, c, 0, 0, 0, 1;
  odom_pos_cal_ += transform_matrix * d_point_odom;

  //记录下里程计的增量数据
  odom_increments_.push_back(d_point_odom);

  // 两针scan计算本身累计的位姿 for laser_path visualization
  c = cos(scan_pos_cal_(2) + d_point_scan(2) / 2);
  s = sin(scan_pos_cal_(2) + d_point_scan(2) / 2);
  transform_matrix << c, -s, 0, s, c, 0, 0, 0, 1;
  scan_pos_cal_ += transform_matrix * d_point_scan;

  // 显示路径
  current_time_ = ros::Time::now();
  pubMsg(odom_pos_cal_, path_odom_, odom_path_pub_);
  pubMsg(scan_pos_cal_, path_scan_, scan_path_pub_);

  //构造超定方程组
  odom_calib.addData(d_point_odom, d_point_scan);
  dataCnt++;

  if (odom_calib.isFull())
    ROS_INFO_STREAM("Get enough data:" << dataCnt);
  else
    ROS_INFO_STREAM("Data Cnt:" << dataCnt);
}

void OdomCalibNode::odomCallBack(const nav_msgs::Odometry::ConstPtr& msg)
{
  transformStamped.header.stamp = msg->header.stamp;
  transformStamped.header.frame_id = "odom";
  transformStamped.child_frame_id = "base_link";
  transformStamped.transform.translation.x = msg->pose.pose.position.x;
  transformStamped.transform.translation.y = msg->pose.pose.position.y;
  transformStamped.transform.translation.z = msg->pose.pose.position.z;
  transformStamped.transform.rotation = msg->pose.pose.orientation;

  broadcaster.sendTransform(transformStamped);
}

void OdomCalibNode::pubMsg(Eigen::Vector3d& pose, nav_msgs::Path& path, ros::Publisher& mcu_path_pub)
{
  // current_time_ = ros::Time::now();
  geometry_msgs::PoseStamped cur_pose_stamped;
  cur_pose_stamped.header.stamp = current_time_;
  cur_pose_stamped.header.frame_id = "odom";

  cur_pose_stamped.pose.position.x = pose(0);
  cur_pose_stamped.pose.position.y = pose(1);
  cur_pose_stamped.pose.position.z = 0.0;
  tf2::Quaternion goal_quat;
  goal_quat.setRPY(0, 0, pose(2));
  cur_pose_stamped.pose.orientation.x = goal_quat.x();
  cur_pose_stamped.pose.orientation.y = goal_quat.y();
  cur_pose_stamped.pose.orientation.z = goal_quat.z();
  cur_pose_stamped.pose.orientation.w = goal_quat.w();

  path.poses.push_back(cur_pose_stamped);
  mcu_path_pub.publish(path);
}

void OdomCalibNode::publishPathEigen(std::vector<Eigen::Vector3d>& path_eigen, ros::Publisher& path_pub)
{
  nav_msgs::Path visual_path;

  current_time_ = ros::Time::now();

  visual_path.header.stamp = current_time_;
  visual_path.header.frame_id = "odom";

  geometry_msgs::PoseStamped tmpPose;
  tmpPose.header.stamp = current_time_;
  tmpPose.header.frame_id = "odom";

  for (int i = 0; i < path_eigen.size(); i++)
  {
    Eigen::Vector3d poseEigen = path_eigen[i];

    tmpPose.pose.position.x = poseEigen(0);
    tmpPose.pose.position.y = poseEigen(1);
    tf2::Quaternion goal_quat;
    goal_quat.setRPY(0, 0, poseEigen(2));
    tmpPose.pose.orientation.x = goal_quat.x();
    tmpPose.pose.orientation.y = goal_quat.y();
    tmpPose.pose.orientation.z = goal_quat.z();
    tmpPose.pose.orientation.w = goal_quat.w();

    visual_path.poses.push_back(tmpPose);
  }
  path_pub.publish(visual_path);
}

bool OdomCalibNode::getOdomPose(Eigen::Vector3d& pose, const ros::Time& t)
{
  geometry_msgs::PoseStamped base_pose;
  base_pose.header.frame_id = base_frame_;
  base_pose.header.stamp = t;
  base_pose.pose.position.x = 0.0;
  base_pose.pose.position.y = 0.0;
  base_pose.pose.position.z = 0.0;
  base_pose.pose.orientation.w = 1;
  base_pose.pose.orientation.x = 0;
  base_pose.pose.orientation.y = 0;
  base_pose.pose.orientation.z = 0;

  // 获得t时间点下，base_link坐标系相对odom的坐标变换
  try
  {
    tf_buffer_.transform(base_pose, base_pose, "odom");

    // 变换数据类型
    tf2::Stamped<tf2::Transform> odom_pose;
    tf2::fromMsg(base_pose, odom_pose);
    pose << odom_pose.getOrigin().x(), odom_pose.getOrigin().y(), tf2::getYaw(odom_pose.getRotation());
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN("can't transform: %s", ex.what());
    return false;
  }

  return true;
}

Eigen::Vector3d OdomCalibNode::calDeltaDistence(Eigen::Vector3d last_pose, Eigen::Vector3d now_pose)
{
  double theta, x, y;
  Eigen::Vector3d d_pos;

  //前一次的位姿
  Eigen::Isometry3d T_prev = Eigen::Isometry3d::Identity();
  T_prev.rotate(eul2rotm(last_pose(2), 0, 0));
  T_prev.pretranslate(Eigen::Vector3d(last_pose(0), last_pose(1), 0));

  // 当前位姿
  Eigen::Isometry3d T_now = Eigen::Isometry3d::Identity();
  T_now.rotate(eul2rotm(now_pose(2), 0, 0));
  T_now.pretranslate(Eigen::Vector3d(now_pose(0), now_pose(1), 0));

  // 相对位姿
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  T = T_prev.inverse() * T_now;

  d_pos(0) = T.translation()(0);
  d_pos(1) = T.translation()(1);
  d_pos(2) = atan2(T.matrix()(1, 0), T.matrix()(0, 0));

  return d_pos;
}

Eigen::Matrix3d OdomCalibNode::eul2rotm(double yaw, double pitch, double roll)
{
  Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
  Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
  Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
  Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
  Eigen::Matrix3d R = q.matrix();
  return R;
}

矫正

里程计轨迹为运动累积，每一步距在机器人坐标系中运动$(d_x,0,d_{\theta })$，因为差速机器人无法侧移，所以默认其在y方向无法移动，在对运动步距进行矫正后再转换到odom坐标系下进行累积。
$$\begin{gathered} \left[\begin{array}{c}Correc{t}_x\\ Correc{t}_y\\ Correc{t}_{\theta }\end{array}\right]=\left[\begin{array}{ccc}{x}_{11}& x_{12}& x_{13}\\ x_{21}& x_{22}& x_{23}\\ x_{31}& x_{32}& x_{33}\end{array}\right]\left[\begin{array}{c}{d}_x\\ 0\\ d_{\theta }\end{array}\right] \\ \left[\begin{array}{c}{x}_{t+1}\\ y_{t+1}\\ {\theta }_{t+1}\end{array}\right]=\left[\begin{array}{c}{x}_t\\ y_t\\ {\theta }_t\end{array}\right]+\left[\begin{array}{ccc}\cos ({\theta }_t+\frac{Correc{t}_{\theta }}{2})& -\sin ({\theta }_t+\frac{Correc{t}_{\theta }}{2})& 0\\ \sin ({\theta }_t+\frac{Correc{t}_{\theta }}{2})& -\cos ({\theta }_t+\frac{Correc{t}_{\theta }}{2})& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}Correc{t}_x\\ Correc{t}_y\\ Correc{t}_{\theta }\end{array}\right] \end{gathered}$$

  double ds = linear_speed_ * dt;
  double d_theta = angular_speed_ * dt;

  // 割线模型
  Eigen::Vector3d odom_inc{ ds, 0, d_theta };
  Eigen::Vector3d correct_inc = correct_matrix_ * odom_inc;
  transform_matrix_ << cos(theta_ + correct_inc[2] / 2), -sin(theta_ + correct_inc[2] / 2), 0, sin(theta_ + correct_inc[2] / 2),
      cos(theta_ + correct_inc[2] / 2), 0, 0, 0, 1;
  Eigen::Vector3d calib_pos = transform_matrix_ * correct_inc;

  //累积
  position_x_ += calib_pos[0];
  position_y_ += calib_pos[1];
  theta_ += calib_pos[2];

参考

读取编码器的计数脉冲转化成ROS的odom数据

传感器数据处理1:里程计运动模型及标定

srf_laser_odometry

Eigen学习（九）稠密问题之线性代数和分解

图解差速机器人的三种运动学模型

标定系列二 | 实践之Camera-Odometry标定