问题描述

我这里有个问题，就是我计算base_link到root的rpy，我通过gui调整r或者p，输出也只会变动r或者p，但是如果我r和p都改变了，那么计算的base_link到root的rpy中，rpy中y也变化了。

比如我在joint_state_publisher_gui设置的rp为0.704,0.620，但是通过tf计算的base_link到root的rpy为roll=0.807, pitch=0.458, yaw=0.433，为啥这里还有yaw的变换

<?xml version="1.0"?>
<robot xmlns:xacro="shturl.cc/sXZMd906f3wQULYYtEv" name="tilt_vehicle">

  <!-- 材料定义，用于 RViz -->
  <material name="gray">
    <color rgba="0.6 0.6 0.6 1.0"/>
  </material>
  <material name="dark">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>
  <material name="red">
    <color rgba="0.9 0.1 0.1 1.0"/>
  </material>
  <material name="green">
    <color rgba="0.1 0.9 0.1 1.0"/>
  </material>
  <material name="blue">
    <color rgba="0.1 0.1 0.9 1.0"/>
  </material>
  <!-- 新增坐标系标识颜色：黄色 -->
  <material name="yellow">
    <color rgba="1.0 1.0 0.0 1.0"/>
  </material>

  <!-- 车体尺寸 -->
  <xacro:property name="body_len" value="3.5"/>
  <xacro:property name="body_wid" value="2"/>
  <xacro:property name="body_hei" value="0.2"/>

  <!-- 倾角仪尺寸 -->
  <xacro:property name="sen_len" value="0.12"/>
  <xacro:property name="sen_wid" value="0.08"/>
  <xacro:property name="sen_hei" value="0.05"/>

  <!-- 标记点球半径 -->
  <xacro:property name="marker_radius" value="0.03"/>

  <!-- 新增：车体坐标系小方块尺寸 -->
  <xacro:property name="origin_box_size" value="0.1 0.1 0.1"/>
  <!-- 坐标系安装位置：车体前上方，按需修改xyz -->
  <xacro:property name="origin_x" value="${body_len/2.0 - 0.1}"/>
  <xacro:property name="origin_y" value="0.0"/>
  <xacro:property name="origin_z" value="${body_hei/2.0 + 0.05}"/>

  <link name="root">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="1e-6" iyy="1e-6" izz="1e-6" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <link name="roll_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="1e-6" iyy="1e-6" izz="1e-6" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <joint name="roll_joint" type="revolute">
    <parent link="root"/>
    <child link="roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-1.57" upper="1.57" effort="100" velocity="0.5"/>
    <origin xyz="0 0 1.0" rpy="0 0 0"/>
  </joint>

  <joint name="pitch_joint" type="revolute">
    <parent link="roll_link"/>
    <child link="base_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.57" upper="1.57" effort="100" velocity="0.5"/>
    <!-- 初始高度1m -->
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

  <!-- 车体 base_link -->
  <link name="base_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="10.0"/>
      <inertia ixx="${10.0*(body_wid*body_wid + body_hei*body_hei)/12.0}"
               iyy="${10.0*(body_len*body_len + body_hei*body_hei)/12.0}"
               izz="${10.0*(body_len*body_len + body_wid*body_wid)/12.0}"
               ixy="0.0" ixz="0.0" iyz="0.0"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${body_len} ${body_wid} ${body_hei}"/>
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${body_len} ${body_wid} ${body_hei}"/>
      </geometry>
    </collision>
  </link>

  <gazebo reference="base_link">
    <material>Gazebo/Grey</material>
    <turnGravityOff>true</turnGravityOff>
    <kinematic>false</kinematic>
    <selfCollide>false</selfCollide>
  </gazebo>

  <!-- ====================== 新增车体坐标系连杆 ====================== -->
  <!-- 固定关节：base_link -> vehicle_origin_link（你实际使用的车体坐标系） -->
  <joint name="base_to_vehicle_origin" type="fixed">
    <parent link="base_link"/>
    <child link="vehicle_origin_link"/>
    <!-- 安装在车体前端上方，可自行修改 x y z rpy -->
    <origin xyz="${origin_x} ${origin_y} ${origin_z}" rpy="0 0 0"/>
  </joint>

  <link name="vehicle_origin_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.05"/>
      <inertia ixx="${0.05*(0.1*0.1 + 0.1*0.1)/12.0}"
               iyy="${0.05*(0.1*0.1 + 0.1*0.1)/12.0}"
               izz="${0.05*(0.1*0.1 + 0.1*0.1)/12.0}"
               ixy="0.0" ixz="0.0" iyz="0.0"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${origin_box_size}"/>
      </geometry>
      <material name="yellow"/>
    </visual>
    <!-- 关闭碰撞，仅做坐标系可视化参考，避免干扰碰撞检测 -->
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${origin_box_size}"/>
      </geometry>
    </collision>
  </link>

  <gazebo reference="vehicle_origin_link">
    <material>Gazebo/Yellow</material>
    <turnGravityOff>true</turnGravityOff>
    <selfCollide>false</selfCollide>
  </gazebo>
  <!-- ================================================================= -->

  <!-- 倾角仪，安装在车体上，带一个小角度安装偏差 -->
  <joint name="base_to_sensor" type="fixed">
    <parent link="base_link"/>
    <child link="sensor_link"/>
    <origin xyz="0.5 0.6 ${body_hei/2.0 + sen_hei/2.0 + 0.1}" rpy="0.09 -0.04 0.03"/>
  </joint>

  <link name="sensor_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.1"/>
      <inertia ixx="${0.1*(sen_wid*sen_wid + sen_hei*sen_hei)/12.0}"
               iyy="${0.1*(sen_len*sen_len + sen_hei*sen_hei)/12.0}"
               izz="${0.1*(sen_len*sen_len + sen_wid*sen_wid)/12.0}"
               ixy="0.0" ixz="0.0" iyz="0.0"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${sen_len} ${sen_wid} ${sen_hei}"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${sen_len} ${sen_wid} ${sen_hei}"/>
      </geometry>
    </collision>
  </link>

  <gazebo reference="sensor_link">
    <material>Gazebo/Black</material>
    <turnGravityOff>true</turnGravityOff>
    <selfCollide>false</selfCollide>
  </gazebo>

  <!-- 三个固定标记点，位于车体上方 -->
  <xacro:macro name="marker" params="name parent x y z color gazebo_color">
    <joint name="base_to_${name}" type="fixed">
      <parent link="${parent}"/>
      <child link="${name}"/>
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0"/>
    </joint>

    <link name="${name}">
      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="0.01"/>
        <inertia ixx="${0.01*0.4*marker_radius*marker_radius}"
                 iyy="${0.01*0.4*marker_radius*marker_radius}"
                 izz="${0.01*0.4*marker_radius*marker_radius}"
                 ixy="0.0" ixz="0.0" iyz="0.0"/>
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${marker_radius}"/>
        </geometry>
        <material name="${color}"/>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${marker_radius}"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference="${name}">
      <material>${gazebo_color}</material>
      <turnGravityOff>true</turnGravityOff>
      <selfCollide>false</selfCollide>
    </gazebo>
  </xacro:macro>

  <!-- 前左、前右、后中，三点不共线 -->
  <xacro:marker name="marker_1" parent="base_link"
                x="0.2" y="0.8" z="1.14"
                color="red" gazebo_color="Gazebo/Red"/>
  <xacro:marker name="marker_2" parent="base_link"
                x="0.2" y="-0.8" z="1.2"
                color="green" gazebo_color="Gazebo/Green"/>
  <xacro:marker name="marker_3" parent="base_link"
                x="0.1" y="0.0" z="1.5"
                color="blue" gazebo_color="Gazebo/Blue"/>
</robot>

原因

核心原因：欧拉角顺序问题 + 万向锁效应

URDF里的旋转层级是：
root → 先绕X轴roll旋转 → roll_link → 再绕当前局部Y轴pitch旋转 → base_link
采用的是 RPY（XYZ 内在旋转，绕当前体轴），也就是航空常用的横滚-俯仰-偏航内在旋转顺序。

1. 为什么单独调一个角没问题，两个一起调就出现yaw？

1. 只调 roll（X）：只有绕X旋转，pitch、yaw一定=0，计算出来完全和你设置一致；
2. 只调 pitch（Y）：只有绕Y旋转，roll、yaw一定=0；
3. 先roll、再pitch两个都旋转后，两次局部轴旋转复合之后，无法再只用纯粹的X、Y全局欧拉角描述，必然会产生等效的yaw分量。

通俗解释

• 第一次绕全局X转了 0.704rad，此时原来的Y轴已经被旋转偏移；
• 第二次是绕旋转之后的局部Y轴转 0.620rad，不是最开始的全局Y轴；
• 两次局部轴旋转叠加后的总姿态，解算成全局XYZ（固定轴）欧拉角时，就会拆出三个分量 roll/pitch/yaw，yaw不再是0。

你GUI设置的是两次分步局部轴旋转量，而你代码用 euler_from_quaternion 解出来的是全局固定坐标系下的XYZ欧拉角，二者定义维度不一样，所以数值不一致，还多出yaw。

2. 两种欧拉角的本质区别

① 你GUI设置的：内在旋转（Tait–Bryan，先X再Y，体轴）

R_total = R_Y(pitch) · R_X(roll)

先绕当前物体X转roll，再绕旋转后物体Y转pitch，只有两个输入角度。

② tf_transformations解出的：外在旋转（固定全局XYZ轴）

euler_from_quaternion 默认顺序 sxyz：绕世界固定X、Y、Z轴依次旋转得到当前姿态。
两个体轴旋转复合后的总姿态，映射到固定三轴上，就会出现三个角度，yaw自然不为0。

3. 为什么数值对不上（0.704,0.620 → 0.807,0.458,0.433）

1. 你输入的是两次连续局部轴旋转的角度；
2. 输出的是等价全局三轴固定旋转角度；
   这两组欧拉角是同一姿态的两种不同表达方式，不是计算错误，只是描述规则不一样。

举个生活化例子：
你先侧身躺（roll），再抬头低头（pitch），这个姿态不能等价于单纯全局左右转+前后仰，一定会附带身体轻微偏转（等效yaw）。

4. 两种解决方式，按需选择

方式1：只读取你设置的原始关节角（最精准，不会出现yaw）

不要从TF四元数反解全局RPY，直接订阅 /joint_states 话题读取：
roll_joint、pitch_joint 的原始角度，就是你GUI拖拽设置的0.704、0.620，永远不会出现yaw。

方式2：改用四元数/旋转矩阵描述姿态，放弃全局RPY

欧拉角本身有歧义、顺序依赖、万向锁缺陷，做标定推荐直接用四元数或旋转矩阵存储姿态，避免不同顺序带来的角度混淆。

方式3：如果一定要用欧拉角，必须保证旋转顺序一致

你是 X(roll) → Y(pitch) 内在旋转，不能用固定sxyz全局顺序解析，否则必然出现多余yaw。

补充：万向锁潜在风险

当pitch接近 ±90°时，X和Z轴会重合，roll和yaw会耦合，角度会出现跳变，这也是为什么多轴旋转场景工业上优先用四元数而不是欧拉角。

总结

1. 不是代码/URDF错误，是欧拉角旋转定义（体轴vs固定轴、旋转顺序）不一致导致；
2. GUI设置的是两步局部体轴旋转量，TF解出的是全局固定三轴等效欧拉角；
3. 两个角度同时不为0时，等效yaw必然产生，属于正常数学现象；
4. 想拿到你拖拽的原始rp值，直接读/joint_states最准确。

ros2 topic echo --once /joint_states

这里的rp就是你gui里的rp。