[发明专利]一种机器人的定位方法及其定位装置

权利要求书

1.一种机器人的定位方法，其特征在于，包括：

根据所述机器人当前采集到的IMU数据、轮速里程计数据、图像数据和激光数据、以及通过UWB技术确定出的所述机器人的中间位姿，按照预设的多传感器融合算法，确定当前关键帧及所述当前关键帧的位姿；

根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理，得到处理后的所述当前关键帧的位姿；

根据处理后的所述当前关键帧的位姿，确定所述机器人的位姿；

其中，所述多传感器融合算法包括：根据确定出的所述机器人采集到的每一帧图像中角点的匹配结果、采集到的所有连续两帧图像得到的状态数据、以及所述中间位姿，对每一个图像帧进行滑窗优化处理后，确定出所述当前关键帧的位姿；所述状态数据包括：激光位姿信息、IMU角度和位移的积分量信息、轮速里程计的角度和位移的积分量信息。

2.如权利要求1所述的定位方法，其特征在于，根据当前处于滑窗中图像帧的位姿、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧和激光闭环关键帧，对所述当前关键帧的位姿进行滑窗闭环优化处理，具体包括：

采用以下公式，对所述当前关键帧的位姿进行滑窗闭环优化处理：

X^*=argmin（Res1+Res2+Res3+Res4+Res5+Res6+Res7+Res8）

其中，表示被边缘化的所述图像帧的先验残差，γ_p表示先验残差的线性化残差，H_p表示所述先验残差的一阶导数矩阵，表示第k个数据帧和第k+1个数据帧之间的IMU预积分约束，表示所述第k个数据帧和所述第k+1个数据帧之间的IMU预积分量，表示第j个特征点在第l个数据帧中的观测值，表示该IMU预积分的协方差，表示视觉观测的协方差，X^*表示滑窗中优化的状态，B表示具备有效IMU预积分的图像帧的列表，C表示滑窗中图像帧的列表，表示滑窗中单个特征点的重投影误差，表示所述第k个数据帧和所述第k+1个数据帧之间的轮速里程计预积分约束，表示轮速里程计预积分的观测协方差，表示所述第k个数据帧和所述第k+1个数据帧之间的轮速里程计预积分观测量，E表示具备有效轮速里程计预积分的图像帧的列表，表示所述第k个数据帧和所述第k+1个数据帧之间的激光位姿相对约束，表示激光相对位姿的观测协方差，表示所述第k个数据帧和所述第k+1个数据帧之间的激光相对位姿的观测值，L表示具备有效激光位姿的图像帧的列表，表示所述第k个数据帧的UWB位姿的全局约束，表示UWB位姿的观测协方差，表示所述第k个数据帧的UWB位姿的观测值，U表示具备有效UWB位姿的图像帧的列表，表示当前滑窗中特征点和图像闭环关键帧之间的重投影残差，表示与当前关键帧处于闭环匹配状态的图像闭环关键帧中的单个特征点的重投影误差，M表示当前滑窗中和图像闭环关键帧中匹配成功的特征点组合，(w, v)表示在第v个数据帧上的第w个特征点，表示第w个特征点在第v个数据帧的观测值，和分别表示图像闭环关键帧的姿态和位置，表示当前滑窗中的关键帧和激光闭环关键帧之间的位姿残差，表示单个激光闭环关键帧位姿的残差，N表示当前滑窗中和激光闭环关键帧中匹配成功的关键帧组合，(z, v)表示滑窗中第z个数据帧和第v个数据帧构成了激光闭环，表示滑窗中第z个数据帧和第v个数据帧之间的位姿约束，ρ(.)表示鲁棒核函数；所述UWB位姿为根据所述中间位姿确定。