MID360安装位置

MID360安装位置

这里修改实车入口 src/robot_description/resource/xmacro/reality_robot.sdf.xmacro和simulation_robot.sdf.xmacro。

北极熊原方案的雷达放在底盘上；YSU当前已挂在gimbal_yaw。下面先保留原始底盘安装示例，后面说明当前挂载关系。

        <!--livox-->
        <xmacro_block name="livox" prefix="front_" parent="chassis" pose="0.0496 0.0936 0.435 -${pi/12} 0 0" update_rate="20" samples="1875"/>

这代表：

chassis
└── front_mid360

改为parent="gimbal_yaw"时，pose也必须按新的父坐标系重新表达安装外参，不能直接沿用相对chassis的数值。

我们想要的TF链是：

chassis
  │
  │ gimbal_yaw_odom_joint（fixed）
  ▼
gimbal_yaw_odom
  │
  │ fixed
  ▼
gimbal_pitch_odom
  │
  │ gimbal_yaw_joint = joint_yaw
  ▼
gimbal_yaw
  │
  │ gimbal_pitch_joint = joint_pitch
  ▼
gimbal_pitch

当前gimbal_yaw_odom_joint、gimbal_pitch_odom_joint固定，唯一实际yaw在gimbal_yaw_joint。因此gimbal_yaw_odom位于实际yaw关节之前，gimbal_yaw位于之后，两处挂载的运动关系不同；即使零位时两系重合，也不能认为挂在哪都一样。

MID360固定挂在gimbal_yaw，随实际yaw转动；实际pitch关节在雷达挂载层之后，所以雷达不随pitch转动。挂在gimbal_yaw_odom则不会跟随这个yaw关节转动。

chassis
  └── fixed → gimbal_yaw_odom
        └── fixed → gimbal_pitch_odom
              └── gimbal_yaw_joint（唯一实际yaw）→ gimbal_yaw
                    ├── front_mid360（固定安装，随yaw）
                    └── gimbal_pitch_joint（实际pitch）→ gimbal_pitch

当前reality_robot.sdf.xmacro与simulation_robot.sdf.xmacro的MID360挂载值一致：

        <!--livox-->
        <xmacro_block name="livox" prefix="front_" parent="gimbal_yaw" pose="0.0496 0.0936 0.435 -${pi/12} 0 0" update_rate="20" samples="1875"/>

这组pose是当前源码中的安装外参，实物安装尺寸和轴向仍需测量核对。

然后还有一个只影响仿真的参数需要修改：

src/robot_description/resource/models/mid360/model.sdf.xmacro：

我们前面已经规定好了，这个front_mid360就是Livox 官方点云坐标系 O。

所以要把

<sensor name="${prefix}mid360_lidar" type="gpu_lidar">
    <ignition_frame_id>${prefix}mid360</ignition_frame_id>
    <pose>0 0 0.03 0 0 0</pose>

改成

<sensor name="${prefix}mid360_lidar" type="gpu_lidar">
    <ignition_frame_id>${prefix}mid360</ignition_frame_id>
    <pose>0 0 0 0 0 0</pose>

1. mid360
   1. mid360的原点：
   
   大疆官方资料
   规定的方向：
   
   
   如上图可知，位于激光雷达的中心轴上，并距离底面47mm处。
   也就是

   底部安装面中心
       │
       │ +47 mm
       ↓
      O 点
   

   
   先量 chassis / 云台坐标原点 到 Mid360 底部安装面中心的位置，然后 Z 再补这 47 mm，就能得到 chassis / 云台 → Mid360 官方 O 点的位置。
   官方说明：
   官方手册明确规定：Mid-360 的 O-XYZ 是点云坐标系，其中 O 是点云原点。
   
   而内置 IMU 芯片相对于这个点云坐标系的位置是：

   x = +11.00 mm
   y = +23.29 mm
   z = -44.12 mm
   

   
   

   front_mid360 = LiDAR 点云 O 点
           ●
           │
           │  (+11, +23.29, -44.12) mm
           │
           ● IMU 芯片原点
   

   
   这里先提前说明下``front_mid360`就是雷达的O_XYZ原点，具体怎么推出来的，请往下看。
   2. mid360的pose

   <!--livox-->
   <xmacro_block
       name="livox"
       prefix="front_"
       parent="chassis"
       pose="0.16 0.0 0.18 ${pi/6} 0.0 ${pi/2}"
       ...
   />
   

   • 这个 pose 是 ： chassis 坐标系 → front_mid360 坐标系
   • 按北极熊这个来，是严格相对于 chassis 原点。
   • 他的雷达是装在底盘上的，所以parent="chassis"，如果雷达要装在云台，就改成parent="gimbal_yaw"，那么偏移也会变成相对于gimbal_yaw 坐标系的偏移。.

   x = +0.16 m
   y =  0
   z = +0.18 m
   

   
   意思是 Mid360 O原点相对于 chassis 原点：

   向前 160 mm
   左右不偏
   向上 180 mm
   

   
   他这里pose的后三个参数是RPY,也就是相对于chassis 原点的偏移：

   roll  = π/6 = 30°
   pitch = 0°
   yaw   = π/2 = 90°
   

   
   经过测量，只有roll偏了40度，也就是${2*pi/9}
   另外你在 Mid360 model 里还会看到：

   <visual>
       <pose>0 0 0 0 0 ${pi}</pose>
   

   
   这个只是外观 mesh 转了 180°，不等于雷达传感器 frame 转了 180°。不要拿这个来算 TF。
   1. mid360到底量它外壳哪个点？
   
   这里还要再精确一层。 北极熊当前 Mid360 模型内部是：

   <joint ...>
       <child>${prefix}mid360</child>
   </joint>
   
   <link name="${prefix}mid360">
   

   
   这里的${prefix}mid360会被解析为front_mid360：
   这个front_mid360一直很懵逼，他到底是什么？ 他在readme里写的仿真里是简化的，所以不要在仿真里反推这个坐标系的原点到底是什么？ 在reality的Point LIO里你可以看到extrinsic_T: [-0.011, -0.02329, 0.04412]，这正是官方IMU的偏移。 而frame_id = front_mid360，所以说，反推出来front_mid360是我们点云的坐标系，也就是O_XYZ.
   而北极熊的 LiDAR 光学/扫描传感器实际上还额外：

   <sensor name="${prefix}mid360_lidar" ...>
   <pose>0 0 0.03 0 0 0</pose>
   

   front_mid360 link 原点
           │
           │ +30 mm
           ▼
   LiDAR 扫描原点
   

   
   而 IMU：

   <sensor name="${prefix}mid360_imu" type="imu">
   

   
   没有额外 pose，所以当前仿真里基本相当于：IMU 原点 = front_mid360 原点
   2. mid360最后各个数据究竟填什么？
   
   front_mid360为mid360的O_XYZ的原点。
   那么Mid360官方点云原点就和O_XYZ原点重合。 那么Mid360官方IMU原点就和O_XYZ有(+11, +23.29, -44.12)mm的偏移量。（但这里没必要给pose,在point_lio里给pose即可）
   所以

   <!-- chassis -> Mid360 官方点云 O -->
   <xmacro_block
       name="livox"
       prefix="front_"
       parent="chassis"
       pose="实际 x 实际 y 实际 z 实际 roll 实际 pitch 实际 yaw"
   />
   

   
   然后 Mid360 仿真内部：

   <sensor name="${prefix}mid360_lidar" type="gpu_lidar">
   <pose>0 0 0 0 0 0</pose>
   

   
   而IMU这边，北极熊的方案我看懂了，虽然他让IMU坐标系与点云坐标系直接重合，并且仿真里也是重合的。 而在reality里，他是extrinsic_T: [-0.011, -0.02329, 0.04412]，这里明显是已经考虑到了这个偏移，所以我们没必要修改IMU的坐标系偏移。

先平行于底面，做一个参考基准面。

再基于这个面进行画草图，然后点击底面，点转换实体。

在这个面上把中点标出来。（这个点就是点云OXYZ点）

再测量点云OXYZ点到底盘中点（轮轴中点）的距离。

如下图，朝后偏X=-2.99mm，朝左偏Y=104.69mm，朝上偏Z=255.58mm + 50.46mm = 306.04mm

然后横滚偏roll = 45°，deg2rad = 0.25pi 然后航向角偏yaw = 180°，deg2rad = pi

这还没完，刚才只是测到底盘的距离，而我们真正挂在云台Yaw上，所以z要减去底盘到云台yaw的距离，所以最终朝上偏移 z = 255.58mm + 50.46mm - 210.46mm = 95.58mm

        <!--livox-->
        <xmacro_block name="livox" prefix="front_" parent="gimbal_yaw" pose="-0.00299 0.10469 0.30604 ${pi/4} 0 ${pi}" update_rate="20" samples="1875"/>

综上，你应该需要的数据是：

1. 轮子实际直径

2. 前后轮中心轴距
   前轮中心 → 后轮中心

3. 左右轮中心轮距
   左轮中心面 → 右轮中心面

4. chassis 刚性主体：
   X 方向长度
   Y 方向宽度
   Z 方向高度

5. chassis/轮轴中心 → 云台 yaw 旋转轴中心：
   Δx
   Δy
   Δz

   如果云台不在底盘正中央，x/y 也得知道。

6. yaw 旋转轴中心 → 云台 pitch 旋转轴中心：
   Δx
   Δy
   Δz

7. Mid360 如果挂 chassis：
   chassis 原点 → Mid360 reference frame（mid360的原点：距离底面中心高47mm处）
   Δx Δy Δz
   roll pitch yaw

   如果挂 gimbal_yaw：
   yaw 轴原点 → Mid360 reference frame（mid360的原点：距离底面中心高47mm处）
   Δx Δy Δz
   roll pitch yaw

修改完后：

chmod +x ./src/rmoss_gz_resources/update_model.sh
./src/rmoss_gz_resources/update_model.sh