ROS2中静态TF坐标系转换、欧拉角、求逆简析，用于传感器（激光雷达/相机）与飞控等

ROS2坐标系和TF参数

ROS2静态坐标转换（static_transform_publisher）的旋转参数，是基于内旋Yaw-Pitch-Roll（旋转体依次绕自身变化的z、y’、x”轴旋转），或外旋Roll-Pitch-Yaw（依次绕固定的x、y、z轴）的顺序，两者等价，遵循右手旋转。写参数建议带“–yaw”这些标签避免歧义：

ros2 run tf2_ros static_transform_publisher --x 1.0 --y 0.5 --z 0.0 --yaw 0.2 --pitch 0.1 --roll 0.3 --frame-id parent_frame --child-frame-id child_frame

以上面的两个传感器为例（mid-360s和一部深度相机），相机和雷达在ros2中的都遵循右手坐标系（x朝前，y朝左，z朝上），雷达的原点在半球体中心底部。相机的原点每个厂家可能不同，我这边的虽然是第三方厂家，但遵循orbbec的底层，原点在左红外相机处（指相机正视你的前方时的左边，或者你看着相机正面时右边的那个IR摄像头）。

至于它们的的坐标转换参数，其实应该做下外参标定的但是还没搞（欸嘿）。所以先用了个简单的方法，毕竟是固定着的，让卖家帮我在模型上测了一下两个中心点间的Δx、y、z（图上给深度相机中心点圈反了，不过反正是对称的）。这个时候得到的参数还不能直接用，因为是在软件坐标系下的。此时记相机为父坐标系，雷达为子坐标系，得到平移变换参数为Δx，Δy，Δz = -10.9056，-19.2047，65.2601（mm），旋转变换参数为yaw = -90°， pitch = 0°， roll = -50°，注意需要转换成弧度，yaw = -1.57079633， roll = -0.87266463。

静态变换参数求逆、万向锁

然而，事情没有这么简单。因为使用fast-lio2时，tf树里odom后面接的就是激光雷达的坐标系，没法让深度相机的坐标系再成为雷达的第二个爸爸（），所以只能让雷达成为深度相机的父坐标系，即对上面的参数进行求逆。基本公式如下，R是旋转矩阵，t是xyz平移向量。因为旋转矩阵正交，其转置就是它的逆。

实际上ros2提供了一些方便的工具可以算这个，如先发布一个已知的静态变换，再反向查询变换：

ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll A B

# 再开一个terminal
ros2 run tf2_ros tf2_echo B A

也可以写个简单的python代码，使用invert_transform(x, y, z, roll, pitch, yaw)求解一组参数的逆，先把三个旋转弧度转成矩阵形式，再计算：

import numpy as np
import math

def euler_to_rot(roll, pitch, yaw):
    """
    将 Z-Y-X 内旋欧拉角（弧度）转换为旋转矩阵。
    旋转顺序：先绕 Z 轴转 yaw，再绕 Y 轴转 pitch，最后绕 X 轴转 roll。
    即 R = Rz(yaw) * Ry(pitch) * Rx(roll)
    """
    cr = math.cos(roll); sr = math.sin(roll)
    cp = math.cos(pitch); sp = math.sin(pitch)
    cy = math.cos(yaw); sy = math.sin(yaw)

    R = np.array([
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
        [-sp,   cp*sr,            cp*cr]
    ])
    return R

def rot_to_euler(R):
    """
    从旋转矩阵提取 Z-Y-X 内旋欧拉角 (roll, pitch, yaw)。
    处理万向锁情况（pitch ≈ ±π/2）。
    """
    # 计算 pitch，使用 atan2(-R[2,0], sqrt(R[0,0]^2 + R[1,0]^2))
    pitch = math.atan2(-R[2,0], math.sqrt(R[0,0]**2 + R[1,0]**2))

    if abs(pitch - math.pi/2) < 1e-6:
        # pitch = +90°，存在万向锁，roll 固定为 0
        roll = 0.0
        yaw = math.atan2(R[0,1], R[0,2])
    elif abs(pitch + math.pi/2) < 1e-6:
        # pitch = -90°，万向锁
        roll = 0.0
        yaw = -math.atan2(R[0,1], R[0,2])
    else:
        roll = math.atan2(R[2,1], R[2,2])
        yaw = math.atan2(R[1,0], R[0,0])

    return roll, pitch, yaw

def invert_transform(x, y, z, roll, pitch, yaw):
    """
    输入 A→B 的变换参数：
        x, y, z        ：平移量
        roll, pitch, yaw：Z-Y-X 内旋欧拉角（弧度）
    返回 B→A 的变换参数（同样格式）。
    """
    # 构造 A→B 的旋转矩阵和平移向量
    R_AB = euler_to_rot(roll, pitch, yaw)
    t_AB = np.array([x, y, z])

    # 反向旋转矩阵
    R_BA = R_AB.T
    # 反向平移：t_BA = -R_BA * t_AB
    t_BA = -R_BA @ t_AB

    # 从 R_BA 提取欧拉角
    roll_BA, pitch_BA, yaw_BA = rot_to_euler(R_BA)

    return t_BA[0], t_BA[1], t_BA[2], roll_BA, pitch_BA, yaw_BA

# ==================== 测试 ====================
if __name__ == "__main__":

    print(invert_transform(-10.9056, -19.2047, 65.2601, -0.87266463, 0, -1.57079633))

    # 得到输出：(np.float64(-19.204700034953575), np.float64(57.00212161349289), np.float64(-33.59420922700847), -3.819693587816054e-09, -0.87266463, 1.5707963317811524)

代码中对于万向锁的解决方式是固定roll为0。简单解释万向锁，即当中间那个旋转角（比如 Z-Y-X 顺序中的 pitch）达到±90°时，第一个旋转轴和第三个旋转轴在空间上重合，导致系统丧失一个旋转自由度的现象。概念比较抽象，数学上就是：

以内旋z-y-x顺序为例，当pitch = 90°时，旋转矩阵：$R=R_z(ψ)R_y(2π)R_x(ϕ)$，得：

此时矩阵中只有yaw – roll的组合，没有它们各自单独的项，也就是说最终旋转出来的状态只跟它俩的差值有关了，如这种情况下yaw = 0°，roll = 30°和yaw = 30°，roll = 60°将会等价，导致对于一个旋转矩阵，无法唯一反解出一组欧拉旋转参数。但我的这个例子中最后得到的逆变换中pitch是-50°所以没触发这个东西。

想避免万向锁的问题就要用到四元数—————————我还没用到所以不讲了（不会就直说），嘻嘻>ω<