跳到主要内容

7.15 状态估计接口

7.15.1 足式里程计​

  • 说明：获取足式里程计的状态信息
  • 获取方式：topic
  • 话题名称：/mc/leg/walking_odom
  • 数据定义位置：nav_msgs::msg::Odometry
  • 数据格式：
std_msgs/msg/Header header
string child_frame_id
geometry_msgs/msg/PoseWithCovariance pose
geometry_msgs/msg/TwistWithCovariance twist
  • 足式里程计msg核心部分的填充内容
pose.pose.position.x = footprint_pos.x(); // 两脚中心的位置(x, y, z)
pose.pose.position.y = footprint_pos.y();
pose.pose.position.z = footprint_pos.z();
pose.pose.orientation.x = base_quat.x(); // Base的四元数(x, y, z, w)
pose.pose.orientation.y = base_quat.y();
pose.pose.orientation.z = base_quat.z();
pose.pose.orientation.w = base_quat.w();
twist.twist.linear.x = base_linvel_local.x(); // Base的速度(vx, vy, wz)， 本体系
twist.twist.linear.y = base_linvel_local.y();
twist.twist.angular.x = left_phi; // 左右腿的摆动时间
twist.twist.angular.y = right_phi;
twist.twist.angular.z = base_angvel_local.z();

7.15.2 本体状态估计​

  • 说明：获取本体的状态信息
  • 获取方式：topic
  • 话题名称：/mc/leg/base_status
  • 数据定义位置：nav_msgs::msg::Odometry
  • 数据格式：
std_msgs/msg/Header header
string child_frame_id
geometry_msgs/msg/PoseWithCovariance pose
geometry_msgs/msg/TwistWithCovariance twist
  • 本体状态估计msg核心部分的填充内容
pose.pose.position.x = base_pos.x(); // Base的位置(x, y, z)
pose.pose.position.y = base_pos.y();
pose.pose.position.z = base_pos.z();
pose.pose.orientation.x = base_quat.x(); // Base的四元数(x, y, z, w)
pose.pose.orientation.y = base_quat.y();
pose.pose.orientation.z = base_quat.z();
pose.pose.orientation.w = base_quat.w();
twist.twist.linear.x = base_linvel_local.x(); // Base的线速度(vx, vy, vz), 本体系
twist.twist.linear.y = base_linvel_local.y();
twist.twist.linear.z = base_linvel_local.z();
twist.twist.angular.x = left_support; // 左右腿支撑状态，支撑为1, 摆动为0
twist.twist.angular.y = right_support;

7.15.3 接触状态估计​

  • 说明：获取本体的状态信息
  • 获取方式：topic
  • 话题名称：/mc/leg/contact_status
  • 数据定义位置：std_msgs::msg::Bool
  • 数据格式：
bool data
  • 接触估计msg核心部分的填充内容
data = contact_flag; // 任意腿在支撑则为True, 双腿悬空则为False

7.15.4 注意事项​

  1. 关于Base的四元数不同于IMU原始值，Base四元数去掉了启动时yaw-offset的偏置
  2. Base和Footprint的位置均对齐Base四元数坐标系（即去掉了yaw-offset偏置）
  3. Base和Footprint的位置估计仅仅是运动学 + IMU 估计，如果需要更精确的结果，请融合相机做VIO或其他可行的办法
  4. 没有特殊说明，速度均为本体系（局部系）