sensor odometry 下_gz::sim::systems::odometrypublisher-优快云博客

本文链接：https://blog.youkuaiyun.com/BurseTang/article/details/145919620

1 ROS2 odometry

1.1 消息描述

ROS2中，里程计信息通常通过nav_msgs/msg/Odometry消息类型传递。这个消息类型包含了机器人的位置、姿态、线速度和角速度等信息。

$ ros2 interface show nav_msgs/msg/Odometry
# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id
# The twist in this message should be specified in the coordinate frame given by the child_frame_id

# Includes the frame id of the pose parent.
std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

# Frame id the pose points to. The twist is in this coordinate frame.
string child_frame_id

# Estimated pose that is typically relative to a fixed world frame.
geometry_msgs/PoseWithCovariance pose
        Pose pose
                Point position        # 位置 x y z, 单位：m
                        float64 x
                        float64 y
                        float64 z
                Quaternion orientation    # 四元数方式表示的姿态(转成欧拉角: k[0, 2π])
                        float64 x 
                        float64 y 
                        float64 z 
                        float64 w
        float64[36] covariance  # 协方差

# Estimated linear and angular velocity relative to child_frame_id.
geometry_msgs/TwistWithCovariance twist
        Twist twist
                Vector3  linear   
                        float64 x    # 线速度 单位：m/sec
                        float64 y
                        float64 z
                Vector3  angular
                        float64 x
                        float64 y
                        float64 z    #　角速度  单位: rad/sec
        float64[36] covariance  # 协方差

pose字段包含了机器人的位置和姿态(四元数表示)，其对应的协方差矩阵，用于表示估计的不确定性；twist字段包含了机器人的线速度和角速度，以及对应的协方差矩阵。
对于差分运动模型，我们需要关心的是机器人的位置和姿态 $(x,y,\theta_{[orientation]})$ ， $x$ 方向的线速度和绕 $z$ 轴旋转的角速度。

1.2 消息模拟

1.2.1 odometry消息解析

#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

// Converts from degrees to radians.
constexpr double DegToRad(const double& deg) { return M_PI * deg / 180.; }

// Converts form radians to degrees.
constexpr double RadToDeg(const double& rad) { return 180. * rad / M_PI; }

// to [-PI, +PI]  单位:弧度制
static inline double normalize_angle(double angle) {
  const double result = fmod(angle + M_PI, 2.0 * M_PI);
  if(result <= 0.0) return result + M_PI;
  return result - M_PI;
}

// [-PI, +PI]  四元数 --> 欧拉角
static double GetYaw(const geometry_msgs::msg::Pose& pose) {
  tf2::Quaternion quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
  double roll, pitch, yaw;
  tf2::Matrix3x3 matrix(quaternion); 
  matrix.getRPY(roll, pitch, yaw);

  return normalize_angle(yaw);
}

// callback in ros2 foxy 
void OdometrySubscriber::HandleOdometryMessage(const nav_msgs::msg::Odometry::ConstSharedPtr msg) {
  const auto yaw = GetYaw(msg->pose.pose);
  RCLCPP_INFO(node_ptr_->get_logger(), "Real odom: %f, %f, %f, %f", msg->pose.pose.position.x, 
                                                                    msg->pose.pose.position.y, 
                                                                    yaw, RadToDeg(yaw));
}

1.2.2 模拟机器人做直线运动

// 转换为[0, 2PI] 　单位:弧度制
static inline double normalize_angle_positive(const double& angle) {
  // fmod(x,y)为C 标准库 - <math.h>函数,返回Ｘ/Y的余数
  const double result = fmod(angle, 2.0 * M_PI);
  if (result < 0)
    return result + 2.0 * M_PI;
  return result;
}

void OdometryPublisher::SimulateLinearMotion() {
  struct OdometryData odometry_data;

  odometry_data.linear_x = 0.1f;    // 以0.1m/s的速度前进
  odometry_data.linear_y = 0.f;
  odometry_data.angular = 0.f;

  rclcpp::Time last_time;
  odometry_data.now = node_ptr_->now();
  last_time = node_ptr_->now();

  rclcpp::WallRate wall_rate(50.0f); // 50Hz <-->  20ms 
  while (rclcpp::ok()) {
    odometry_data.now = node_ptr_->now();

    // compute odometry in a typical way given the velocities of the robot
    double dt = (odometry_data.now - last_time).seconds();
    const double cos_theta = std::cos(odometry_data.theta);
    const double sin_theta = std::sin(odometry_data.theta);
    double delta_x = (odometry_data.linear_x * cos_theta - odometry_data.linear_y * sin_theta) * dt;
    double delta_y = (odometry_data.linear_x * sin_theta + odometry_data.linear_y * cos_theta) * dt;
    double delta_theta = odometry_data.angular * dt;

    odometry_data.x += delta_x;
    odometry_data.y += delta_y;
    odometry_data.theta += delta_theta;

    // first, we'll publish the transform over tf2
    PublishTransform(odometry_data);

    // next, we'll publish the odometry message over ROS2
    PublishOdometry(odometry_data);

    RCLCPP_INFO(node_ptr_->get_logger(), "odom: %f, %f, %f",odometry_data.x, odometry_data.y, odometry_data.theta);

    last_time = odometry_data.now;
    wall_rate.sleep();
  }

  return;
}

void OdometryPublisher::PublishTransform(const struct OdometryData& odometry_data) {
  geometry_msgs::msg::TransformStamped odometry_transform;

  odometry_transform.header.stamp = odometry_data.now;
  odometry_transform.header.frame_id = "odom";
  odometry_transform.child_frame_id = "base_link";

  odometry_transform.transform.translation.x = odometry_data.x;
  odometry_transform.transform.translation.y = odometry_data.y;
  odometry_transform.transform.translation.z = 0.0f;

  tf2::Quaternion q;
  q.setRPY(0, 0, normalize_angle_positive(odometry_data.theta));  // 欧拉角 --> 四元数
  odometry_transform.transform.rotation.x = q.x();
  odometry_transform.transform.rotation.y = q.y();
  odometry_transform.transform.rotation.z = q.z();
  odometry_transform.transform.rotation.w = q.w();

  // send the transform
  tf_broadcaster_ptr_->sendTransform(odometry_transform);
}

void OdometryPublisher::PublishOdometry(const struct OdometryData& odometry_data) {
  nav_msgs::msg::Odometry odom;
  odom.header.stamp = odometry_data.now;
  odom.header.frame_id = "odom";

  // set the position
  odom.pose.pose.position.x = odometry_data.x;
  odom.pose.pose.position.y = odometry_data.y;
  odom.pose.pose.position.z = 0.0f;

  tf2::Quaternion orientation;
  orientation.setRPY(0, 0, normalize_angle_positive(odometry_data.theta));  // 欧拉角 --> 四元数
  odom.pose.pose.orientation.x = orientation.x();
  odom.pose.pose.orientation.y = orientation.y();
  odom.pose.pose.orientation.z = orientation.z();
  odom.pose.pose.orientation.w = orientation.w();

  // set the velocity
  odom.child_frame_id = "base_link";
  odom.twist.twist.linear.x = odometry_data.linear_x;
  odom.twist.twist.linear.y = odometry_data.linear_y;
  odom.twist.twist.angular.z = odometry_data.angular;

  publisher_ptr_->PublishOdometry(odom);

  return;
}

1.2.3 模拟机器人做圆周运动

基于上一节的代码，修改线速度和角速度的值，即可满足圆周运动。

odometry_data.linear_x = 0.f;    
odometry_data.linear_y = 0.f;
odometry_data.angular = 0.2f;    // 以0.2rad/s的速度旋转

1.3 ros2 odometry data

---
header:
  stamp:
    sec: 1709689014
    nanosec: 426830505
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: -0.7117398381233215
      y: 0.7416483163833618
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.24977706830722232
      w: 0.9683033698938825
  covariance:
 - 0.0
 ...........
 - 0.0
twist:
  twist:
    linear:
      x: 0.13097742984169408
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.23266780479680213
  covariance:
 - 0.0
  ...........
 - 0.0
---

2 机器人本体坐标系

base_link坐标系是建立在机器人本体上的一个固定坐标系，也被称为基座坐标系或底盘坐标系。它是机器人运动学和动力学分析的基础，用于描述机器人各个部件以及机器人与外部环境之间的相对位置和姿态关系。

2.1 本体坐标系的作用

描述机器人状态：为机器人的位置、姿态等状态的描述提供了一个基准。通过在 base_link 坐标系下定义机器人的位姿(位置和姿态)，可以方便地对机器人的运动进行建模和控制。例如，在移动机器人中，机器人的位置和朝向通常是相对于base_link坐标系来表示的，这使得机器人控制系统能够准确地知道机器人在空间中的状态，从而规划和执行相应的运动指令。
多传感器融合：在具有多个传感器的机器人系统中，base_link坐标系是进行传感器数据融合的重要参考。不同传感器(如激光雷达、相机、IMU等)采集到的数据需要转换到同一个坐标系下才能进行有效的融合和处理。通常会将各个传感器的坐标系与base_link坐标系进行标定，建立起它们之间的转换关系，这样就可以将不同传感器的数据统一到base_link坐标系中，以便进行更准确的环境感知和目标识别等任务。
运动规划与控制：机器人的运动规划和控制算法通常也是基于base_link坐标系来设计的。在规划机器人的路径和运动轨迹时，需要以base_link坐标系为基础，考虑机器人的运动学和动力学约束，计算出合适的控制量，使机器人能够按照预期的方式运动。例如，在机器人避障任务中，需要根据base_link坐标系下的环境信息和机器人的当前位置，规划出一条安全的路径，然后通过控制机器人的底盘运动来实现路径跟踪。

2.2 与其他坐标系的关系

与世界坐标系的关系：世界坐标系是一个全局的、固定的坐标系，用于描述整个场景和其中所有物体的位置。base_link坐标系相对于世界坐标系的位置和姿态是不断变化的，它随着机器人的移动和姿态调整而改变。通过建立base_link坐标系与世界坐标系之间的转换关系，可以将机器人在自身坐标系下的信息转换到世界坐标系中，以便与其他物体或系统进行交互和协作。
与传感器坐标系的关系：如前文所述，机器人上的各种传感器都有各自的坐标系。为了使传感器数据能够被机器人系统有效地利用，需要通过外参标定等方法确定传感器坐标系与base_link坐标系之间的转换关系。这样，传感器采集到的数据就可以转换到base_link坐标系下，与机器人的其他信息进行融合和处理。

2.3 举例

以一个轮式移动机器人为例，通常会将base_link坐标系的原点设置在机器人底盘的中心位置， $z$ 轴垂直向上， $x$ 轴指向机器人的前进方向， $y$ 轴根据右手定则确定。当机器人在环境中移动时，它在base_link坐标系下的位姿会不断变化，而机器人上搭载的激光雷达、相机等传感器的数据也会根据与base_link坐标系的转换关系，被转换到统一的坐标系下进行处理，从而为机器人的导航、避障等任务提供支持。
在这里插入图片描述

3 误差

3.1 误差分析

车轮打滑：在加速、减速、转弯或者地面摩擦力不足(如遇到光滑地面、水渍等)的情况下，车轮容易出现打滑现象，使得编码器记录的转动信息与实际行驶距离不符，从而引入误差。
地面不平：不平整的地面(如坑洼、凸起)会使车轮产生跳动或颠簸，导致编码器测量的转动角度不准确，影响里程计的精度。
车轮磨损：随着使用时间的增加，车轮会逐渐磨损，导致车轮半径变小，进而影响距离计算的准确性。
编码器精度：编码器的分辨率有限，其测量的最小角度变化存在一定限制，这会引入量化误差。

3.2 改进方法

多传感器融合：将轮式里程计与其他传感(如激光雷达、视觉传感器、惯性测量单元(IMU))的数据进行融合。例如，结合IMU的角速度信息可以辅助修正轮式里程计在角度测量上的误差；利用激光雷达或视觉传感器获取的环境信息进行定位校准，减少累积误差。
打滑检测与修正：通过监测车轮的加速度、电机电流等信息，判断车轮是否发生打滑。一旦检测到打滑，可采用一些算法(如基于模型预测的方法)对里程计数据进行修正。
定期校准：定期对车轮半径、编码器参数进行校准，以减小因车轮磨损、编码器精度变化等因素导致的误差。