[发明专利]机器人的室内定位方法及装置

权利要求书

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

当机器人的相机捕获到路标点，且所述路标点在加权地图中，则根据上一时刻所述机器人的实际位姿、所述机器人的角度增量以及所述机器人的位移增量确定所述机器人当前时刻的预测位姿；其中，所述加权地图是基于卡尔曼滤波算法建立的，所述机器人的角度增量为当前时刻所述机器人中心点的角度相比于上一时刻所述机器人中心点的角度的变化量，所述位移增量为当前时刻的所述机器人中心点的位移相比于上一时刻所述机器人中心点的位移的变化量；

根据捕获到的所述路标点在所述加权地图中对应的协方差矩阵以及所述机器人的相机观测误差确定当前定位观测误差，具体包括：

将所述路标点在所述加权地图中对应的协方差矩阵上的主对角线上的每一项参数进行开平方运算，并将所述相机观测误差的标准差分别与开平方运算后的参数进行对应相加后，再进行平方运算；得到第一中间观测误差值；并根据所述协方差矩阵中的除所述主对角线之外的其他参数以及所述第一中间观测误差值得到所述当前定位观测误差；

利用所述当前定位观测误差、观测信息、所述路标点在所述加权地图中的位姿以及所述机器人的预测位姿进行卡尔曼滤波更新和卡尔曼滤波融合，得到所述机器人的实际位姿；其中，所述观测信息为所述机器人的相机本次捕获到所述路标点时确定出的所述路标点在相机坐标系下的坐标。

2.根据权利要求1所述的方法，其特征在于，所述将所述路标点在所述加权地图中对应的协方差矩阵上的主对角线上的每一项参数进行开平方运算，并将所述相机观测误差的标准差分别与开平方运算后的参数进行对应相加后，再进行平方运算；得到第一中间观测误差值；并根据所述协方差矩阵中的除所述主对角线之外的其他参数以及所述第一中间观测误差值得到所述当前定位观测误差，包括：

根据以下公式确定出所述当前定位观测误差：

其中，(k_x，k_y，k_θ)为所述机器人的相机观测误差的标准差；为所述路标点在所述加权地图中对应的协方差矩阵；为所述路标点在所述加权地图中对应的协方差矩阵上的主对角线上的每一项参数进行开平方运算后的参数。

3.根据权利要求1所述的方法，其特征在于，所述方法还包括：

当所述机器人的相机捕获到路标点，且所述路标点未在所述加权地图中，则利用卡尔曼滤波算法确定出所述路标点的位姿和所述路标点对应的协方差矩阵；

将确定出的所述路标点的位姿和所述路标点对应的协方差矩阵添加到所述加权地图中。

4.根据权利要求1所述的方法，其特征在于，所述利用所述当前定位观测误差、观测信息、所述路标点在所述加权地图中的位姿以及所述机器人的预测位姿进行卡尔曼滤波更新和卡尔曼滤波融合，得到所述机器人的实际位姿，包括：

通过所述路标点在所述加权地图中的位姿以及所述观测信息，确定所述相机在世界坐标系下的坐标；

利用预设的所述相机与所述机器人中心点的相对位置关系，根据所述相机在世界坐标系下的坐标，确定出所述机器人当前时刻的测量位姿；

利用所述当前定位观测误差、所述机器人当前时刻的测量位姿和所述机器人当前时刻的预测位姿进行卡尔曼滤波更新和卡尔曼滤波融合，得到所述机器人的实际位姿。