移动机器人超宽带与视觉惯性里程计组合的室内定位算法

对于移动机器人在室内环境的定位任务，新兴的基于视觉惯性里程计（VIO）的辅助定位技术受光线条件限制大，无法在黑暗环境中工作，且超宽带（UWB）定位易受非视距（NLOS）误差影响。针对以上问题，提出一种UWB与VIO组合的室内移动机器人定位算法。首先，采用立体视觉多状态约束下的Kalman滤波器（S-MSCKF）算法/双边双向测距（DS-TWR）算法和三边定位法，分别得到VIO输出的位置信息/UWB解算的定位信息；然后，建立位置测量系统的运动方程与观测方程；最后，通过误差状态扩展卡尔曼滤波（ES-EKF）算法来进行数据融合，得到机器人的最优位置估计。使用搭建的移动定位平台在不同的室内环境下对组合定位方算法进行验证。实验结果表明在有障碍物的室内环境下，与单一UWB定位方法相比，所提算法的总体定位的最大误差减小了约4.4%，均方误差减小了约6.3%；与VIO定位方法相比，所提算法的总体定位的最大误差减小了约31.5%，均方误差减小了约60.3%。可见所提算法可为室内环境下的移动机器人提供实时、精确且鲁棒的定位结果。

UWB-VIO integrated indoor positioning algorithm for mobile robots

For the positioning task of mobile robots in indoor environment， the emerging auxiliary positioning technology based on Visual Inertial Odometry （VIO） is heavily limited by the light conditions and cannot works in the dark environment. And Ultra-Wide Band （UWB）-based positioning methods are easily affected by Non-Line Of Sight （NLOS） error. To solve the above problems， an indoor mobile robot positioning algorithm based on the combination of UWB and VIO was proposed. Firstly， S-MSCKF （Stereo-Multi-State Constraint Kalman Filter） algorithm/DS-TWR （Double Side-Two Way Ranging） algorithm and trilateral positioning method were used to obtain the position information of VIO output/positioning information resolved by UWB respectively. Then， the motion equation and observation equation of the position measurement system were established. Finally， the optimal position estimation of the robot was obtained by data fusion carried out using Error State-Extended Kalman Filter （ES-EKF） algorithm. The built mobile positioning platform was used to verify the combined positioning method in different indoor environments. Experimental results show that in the indoor environment with obstacles， the proposed algorithm can reduce the maximum error of overall positioning by about 4.4% and the mean square error of overall positioning by about 6.3% compared with the positioning method only using UWB， and reduce the maximum error of overall positioning by about 31.5% and the mean square error of overall positioning by about 60.3% compared with the positioning method using VIO. It can be seen that the proposed algorithm can provide real-time， accurate and robust positioning results for mobile robots in indoor environment.