MID360安裝位置

1.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