底盘运动学里程计软件及测试

背景：

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。

框图

来源：不纯洁的锌