基于四足机器人状态估计器的改进ORB-SLAM算法

Improved ORB-SLAM Algorithm Based on Quadruped Robot State Estimator

摘要:

四足仿生机器人凭借优异的仿生运动特性，能够在非结构化地形中实现高效移动，但运动过程因机体振动引发的多源噪声干扰，导致传统同步定位与建图（SLAM）算法面临视觉特征退化与位姿估计精度下降。为解决上述问题，本文提出一种融合卡尔曼滤波状态估计器的改进ORB-SLAM3算法。基于四足机器人运动学模型构建状态空间方程，设计离散卡尔曼滤波器实现机体位姿实时最优估计;通过改进ORB-SLAM3的恒速运动模型，将状态估计器位姿提供给改进系统，构造相机运动模型;最后将状态估计器输出的先验位姿信息与运动里程计观测结果进行融合。仿真试验结果表明，改进算法相较传统ORB-SLAM3算法平均轨迹误差降低7.71%、均方根误差（RMSE）下降8.29%、标准差（STD）下降9.97%。在Unitree-GO1四足机器人平台开展的实物测试验证，仅在估计轨迹的Z轴上，相比于ORB-SLAM3算法存在0.25~0.38 m波动，状态估计器能够有效估计高度为0.32 m;模拟农业场景试验结果表明，本文方法能有效降低振动误差与转向误差，提升了四足机器人在动态复杂环境下视觉定位系统的抗干扰能力与鲁棒性。研究结果为非结构化场景中四足机器人自主导航提供了技术支撑。

Abstract:

Quadruped bionic robots possess superior bionic locomotion capabilities and can traverse unstructured terrain efficiently. Nevertheless, body vibration during movement introduces multi-source noise, which leads to visual feature degradation and degraded pose estimation accuracy for conventional simultaneous localization and mapping (SLAM) algorithms. To address this issue, an improved ORB-SLAM3 algorithm integrated with a Kalman filter state estimator was proposed. Firstly, a state-space equation was established based on the kinematic model of the quadruped robot, and a discrete Kalman filter was adopted to achieve real-time optimal estimation of robot pose. Secondly, the constant velocity motion model of ORB-SLAM3 was optimized, and pose data from the state estimator was imported to construct the camera motion model. Finally, the prior pose information output by the state estimator was fused with observations from the kinematic odometer. Simulation results demonstrated that compared with the original ORB-SLAM3, the proposed algorithm reduced the average trajectory error by 7.71%, the root mean square error (RMSE) by 8.29%, and the standard deviation (STD) by 9.97%. Physical experiments were conducted on the Unitree-GO1 quadruped robot platform. On the Z-axis of the estimated trajectory, the pose height was stably estimated at 0.32 m by the proposed method, while the result of original ORB-SLAM3 fluctuated between 0.25 m and 0.38 m. Experiments in simulated agricultural scenarios verified that the proposed method can effectively suppress vibration and steering errors. It enhanced the anti-interference performance and robustness of the visual positioning system for quadruped robots in dynamic and complex environments. The research result can provide technical support for the autonomous navigation of quadruped robots operating in unstructured scenarios.