底盤運動學裏程計軟件及測試

背景：

---

1. 大量不同參數的移動機器人底盤。部分底盤本地並沒有樣機，更新代碼後無法保證是否正常工作。
2. 需要簡單有效的反轉定義每個輪的左右方向，高效互換各個輪的ID。
3. AGV停止狀態判斷。對於高分辨率的編碼器，不被誤判爲運動。
4. 在下發指令超速，下發加速度超標時，做基於不改變運動軌跡的速度限制。

底盤類型：

---

1. 雙輪差動
2. 三輪全向
3. 四輪麥克納姆
4. 單驅動輪舵輪
5. 定向驅動輪單舵輪
6. 四輪差動

程序模塊：

---

Chassis_base：保存底盤物理參數，如：編碼器線數，輪半徑，減速比，軸距。
Odometer：只放算法和全局座標系下機器人里程信息，包括:x, y, theta, steerangle。
ProtocolDriver：存放物理量到通信指令相互轉換的方法和通信協議。
ChassisKinematics: 只實現函數，不保存變量。

運行流程：

---

初始化配置 - 下發指令 - 速度分解 - 里程合成

速度指令下發由上層事件觸發。
SDO查詢指令由異步定時器回調觸發。
PDO狀態獲取由CAN郵箱回調觸發。

初始化配置：

protobuffer協議：
1. float wheelbase
2.

待補充。。。

下發速度指令

{“vx”: 0.0, “vy”: 0.0, “w”: 0.0, “steerangle”: 0.0}

下發速度指令由四個成員組成。

速度分解

定向輪底盤，速度分解分下面幾步：
1. 各輪着地點線速度解算 （m/s）
只與底盤類型有關的物理量浮點運算，僅涉及參數：軸距。
輸入爲車身速度指令。
返回一個線速度向量，向量長度等於輪子個數。

1. 各電機軸轉速折算 （rad/s）
   物理量浮點運算。涉及參數：減速比，輪半徑。
   輸入爲線速度。
   返回一個角速度。

2. 各電機協議速度指令計算（void）
   整形邏輯運算。涉及參數：編碼器線數，速度指令分辨率。
   輸入爲角速度。
   返回結果數據類型因協議而不同。可以是int32，也可能是方向加一個大小。

舵輪底盤，速度分解分以下幾步：

1. 各輪着地點線速度解算 （m/s）
   只與底盤類型有關的物理量浮點運算，僅涉及參數：軸距。
   輸入爲車身速度指令。
   輸出爲驅動輪轉速。

2. 驅動輪電機軸轉速計算 (rad/s)
   物理量浮點運算。涉及參數：減速比，輪半徑。
   輸入爲線速度。
   返回一個角速度。

3. 驅動輪及舵輪協議指令計算（void）
   整形邏輯運算。

   驅動輪指令計算涉及參數：驅動輪編碼器線數，驅動輪速度指令分辨率。
   輸入爲驅動輪角速度。
   返回結果數據類型因協議而不同。可以是int32，也可能是方向加一個大小。

   舵輪指令計算涉及參數：舵輪位置指令分辨率。
   輸入爲舵輪角度。
   返回結果數據類型因協議而不同。可以是int32，也可能是方向加一個大小。

注：部分底盤的舵角需要通過PID下發速度指令作位置控制。

可見第1，2步，可以歸入Odometer單例。
第3步歸入ProtocolDriver，作爲驅動器協議的內容。電機軸轉速到速度指令的轉換，由函數描述，作爲ProtocolDriver的一個方法。
軸距， 輪半徑，編碼器線數，減速比 均作爲底盤特性成員變量存入ChassisDriver。

里程合成

定向輪底盤，里程合成分下面幾步：

1. 輪軸轉動角度增量計算（rad）
   計算涉及參數：編碼器線數
   輸入爲編碼器增量或當前速度加採樣間隔
   返回輪軸轉動角度

2. 機體座標系下機器人座標增量計算
   輸入爲輪軸轉動角度
   返回機器人座標增量

3. 全局座標系下機器人座標更新
   做一次更新

舵輪底盤裏程合成：

1. 驅動輪角度增量計算（rad）
   輸入爲編碼器增量或當前速度加採樣間隔
   返回驅動輪輪軸轉動角度

2. 機體座標系下機器人座標增量計算
   輸入爲驅動輪輪軸轉動角度和當前舵角
   返回機器人座標增量

3. 全局座標系下機器人座標更新
   做一次更新
   因此第1步歸入ProtocolDriver，第2，3步歸入Odometer。

框圖

---