程序流程

1. 获取各个传感器的当前位姿
   • 编码器(获取某段时间内由编码器测得的速度、位移)
     • 获取时间阈值内各个车轮编码器的脉冲数
     • 由脉冲数计算车身的相对位移和速度
     • 坐标变换为全局坐标
   • PGV传感器(获取PGV传感器在世界坐标系中的位姿)
     • 获取PGV状态
     • 获取PGV在全局坐标系中的位姿
   • 陀螺仪（待定）
2. 根据各个传感器的当前位姿，融合得到车体的位姿
   • 融合各个传感器的数据
     • PGV传感器的可信度最高
     • 编码器数据需滤波
   • 由融合数据更新传感器数据
     • 由融合数据更新编码器数据
3. 处理指令
   • 运动控制
     • 获取期望速度，期望位姿
   • 参数处理
   • 传感器位姿校正
4. 检查避障
   • 获取避障信息
   • 根据避障情况更新期望速度、期望位姿
5. 运动控制
   • 根据期望位姿，期望速度更新实际速度
     • 采用Lyapunov控制律
   • 根据逆运动学控制车轮转速