一种基于因子图优化的腿式机器人本体感知里程计方法

本发明请求保护一种基于因子图优化的腿式机器人本体感知里程计方法，应用于腿式机器人运动感知和定位领域。腿式机器人可以利用本体传感器实现对自身运动状态的估计，本发明利用本体多传感器进行联合估计自身状态，通过躯干惯性测量单元IMU进行预积分得到机体相对位姿因子，利用足部IMU预积分得到混合足部惯性导航系统INS因子，利用关节角度传感器得到正运动学模型因子和预积分混合铰接接触模型因子，在因子图优化框架下将传感器因子进行融合并通过距离约束得到最优运动状态估计。本发明利用多本体传感器进行感知，能够解决腿式机器人在视觉、激光雷达感知受限时，单一本体感知造成里程计快速产生漂移的问题。