一种室内定位方法及装置技术领域
本发明实施例涉及导航技术，尤其涉及一种室内定位方法及装置。
背景技术
随着电子技术和计算机技术的快速发展及机器人定位精度的不断提高，智
能移动机器人的应用越来越广泛。
目前，一种实现室内导航的方式是预先在室内布置无线传感器网络节点，
以形成无线传感器网络。根据无线传感器网络节点的数据构建全局拓扑移动地
图。该方法需要预先铺设相当数量的无线传感器，必然会增加投入的研发成本。
并且，根据这种方式获得的地图必然会因缺少未设置传感器位置的信息而不完
整，进而，无法为室内导航提供精确的导航地图。另一种实现室内导航的方式
是通过同步定位与地图构建(SLAM)技术描绘室内环境，根据传感器采集的
数据对机器人的位姿进行粗略计算，再结合SLAM算法进行精细定位，实现机
器人定位和导航。但是，由于采用SLAM算法将采集的环境数据与室内整体环
境数据进行匹配需要进行大量的迭代运算，算法的时间复杂度较高，进而不利
于机器人实时定位。同时，SLAM算法自身在数据匹配过程中存在累计误差，
使定位精度和导航的准确性均降低。
发明内容
本发明提供一种室内定位方法及装置，以实现精准定位的目的，降低SLAM
算法在匹配上的时间复杂度及累计误差。
第一方面，本发明实施例提供了一种室内定位方法，包括：
确定估计位姿，根据已获得的室内地图库与所述估计位姿确定局部地图库；
将采集到的当前帧激光雷达数据与上一帧激光雷达数据进行匹配确定第一
位姿；
在所述局部地图库中获得与所述第一位姿的偏移量最小的位姿，将所述当
前帧激光雷达数据与所述偏移量最小的位姿对应的激光雷达数据进行匹配确定
第二位姿；
将所述第一位姿与所述第二位姿中匹配误差较小的一个位姿确定为精确位
姿。
第二方面，本发明实施例还提供了一种室内定位装置，该装置包括：
定位地图确定单元，用于确定估计位姿，根据已获得的室内地图库与所述
估计位姿确定局部地图库；
第一位姿确定单元，用于将采集到的当前帧激光雷达数据与上一帧激光雷
达数据进行匹配确定第一位姿；
第二位姿确定单元，用于在所述局部地图库中获得与所述第一位姿的偏移
量最小的位姿，将所述当前帧激光雷达数据与所述偏移量最小的位姿对应的激
光雷达数据进行匹配确定第二位姿；
精确位姿确定单元，用于将所述第一位姿与所述第二位姿中匹配误差较小
的一个位姿确定为精确位姿。
本发明通过确定估计位姿，根据已获得的室内地图库与所述估计位姿确定
局部地图库；将采集到的当前帧激光雷达数据与上一帧激光雷达数据进行匹配
确定第一位姿；在所述局部地图库中获得与所述第一位姿的偏移量最小的位姿，
将所述当前帧激光雷达数据与所述偏移量最小的位姿对应的激光雷达数据进行
匹配确定第二位姿；将所述第一位姿与所述第二位姿中匹配误差较小的一个位
姿确定为精确位姿。本发明解决因SLAM算法的计算量大导致算法的时间复杂
度较高的问题，以及，通过将第一位姿与第二位姿进行比较，去掉匹配误差较
大的数据，限制了SLAM算法自身的累计误差，实现了快速精准定位的目的。
附图说明
图1是本发明实施例一中的一种室内定位方法的流程图；
图2是本发明实施例二中的一种室内定位方法中数据处理示意图；
图3是本发明实施例三中的一种室内定位装置的结构示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此
处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需
要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结
构。
实施例一
图1为本发明实施例一提供的室内定位方法的流程图，本实施例可适用于
快速精确定位机器人位置的情况，该方法可以由室内定位装置来执行，具体包
括如下步骤：
步骤110、确定估计位姿，根据已获得的室内地图库与所述估计位姿确定局
部地图库。
其中，为了确定机器人在平面中的位置，建立二维平面坐标系。以机器人
在室内环境中运动的运动起点作为坐标原点，以机器人开始运动时的正前方作
为X轴的正方向，即将与X轴重合的机器人位置的方向角记为0°，顺时针方向
为负，逆时针方向为正，夹角范围是-180°至180°。通过设置于机器人上的测速
编码器及加速度计确定机器人的当前位置(X0，Y0)，通过陀螺仪及电子罗盘确
定机器人的当前位置和室内地图库原点(坐标原点)的连线与X轴正方向的夹
角θ0，进而，可以估计机器人的当前位姿为(X0，Y0，θ0)。采用如陀螺仪及电
子罗盘等同类传感器采集数据，进行数据融合可以减少测量误差。
在确定估计位姿之前，需要获取室内地图库。具体可以为控制机器人于室
内的运动起点开始环绕室内运动一周，与此同时，通过机器人上设置的激光雷
达按照预设的周期值周期性的发射激光雷达数据，再通过机器人上设置的接收
器接收反射回的激光雷达数据。机器人可以根据发射的激光雷达数据和反射回
的激光雷达数据确定室内轮廓，根据所述室内轮廓绘制室内地图库。
机器人根据上述估计的初始位姿与室内地图库进行匹配，确定所述初始位
姿及其附近的坐标点组成的局部地图库。具体可以是获取室内地图库中与所述
估计位姿的距离偏移量不超过设定阈值的位姿，确定距离偏移量不超过设定阈
值的各个位姿对应的激光雷达数据。例如，估计初始位姿是(1m,2m,30°)，选
择与初始位姿包括的位置分量的距离偏移量为(±0.5m，±0.5m)的坐标点的激
光雷达的数据组成局部估计地图库。
为了进一步精准确定局部地图库，将当前采集的激光雷达数据与上述局部
估计地图库中各个位姿对应的激光雷达数据进行匹配，根据各个位姿对应的激
光雷达数据中与所述当前采集的激光雷达数据相匹配的激光雷达数据确定局部
地图库。例如，机器人将当前接收到的激光雷达数据依次与所述局部估计地图
库中各个坐标点对应的激光雷达数据进行匹配。在匹配误差满足设定规则时，
确定当前接收到的激光雷达数据与局部估计地图库中该坐标点对应的激光雷达
数据匹配成功。相应地，将匹配成功的该坐标点确定为匹配坐标点，根据所有
匹配坐标点确定局部地图库，从而减少了同步定位与地图构建(SLAM)算法进行
匹配运算的数据量，加快了匹配速度。
步骤120、将采集到的当前帧激光雷达数据与上一帧激光雷达数据进行匹
配确定第一位姿。
机器人将采集到的当前帧激光雷达数据与相邻的上一帧激光雷达数据进行
匹配确定第一距离偏移量及第一角度偏移量。根据所述第一距离偏移量和第一
角度偏移量调整所述上一帧雷达数据对应的位姿确定第一位姿(X1，Y1，θ1)。
在机器人图像采样精度足够高的前提下，机器人在正常行进过程中，在相邻两
个运动位姿处接收到的雷达数据的差别是比较小的，据此，将当前采集的激光
雷达数据与相邻的上一帧激光雷达数据进行匹配，确定第一距离偏移量和第一
角度偏移量。由于相邻的上一帧激光雷达数据对应的位姿是已知的，可以根据
上一帧激光雷达数据对应的位姿与所述第一距离偏移量和第一角度偏移量，确
定当前采集的激光雷达数据对应的当前位姿，即为第一位姿(X1，Y1，θ1)。
一般情况下，机器人在运动过程中采用平移的方式行进时，通过SLAM算
法进行匹配的匹配误差最小、精度最高。理想状态下，若机器人在行进过程中
只发生位置的平移而未发生旋转(及未改变方向角)，则只需确定机器人行进过
程中，采集到的当前帧激光雷达数据与相邻的上一帧的激光雷达数据之间的距
离平移量即可。根据上一帧激光雷达数据对应的位姿与所述距离平移量确定第
一位姿(X1，Y1，θ1)。
步骤130、在所述局部地图库中获得与所述第一位姿的偏移量最小的位姿，
将所述当前帧激光雷达数据与所述偏移量最小的位姿对应的激光雷达数据进行
匹配确定第二位姿。
由于SLAM算法本身存在累计误差，导致通过将当前帧激光雷达数据与上
一帧激光雷达数据进行匹配确定当前位姿的方法可能存在误差，为了验证其可
靠性，本实施例的技术方案中将所确定的第一位姿(X1，Y1，θ1)在所述局部
地图库中进行查找确定与其偏移量最小的位姿作为参考位姿。具体可以是获取
所述局部地图库包括的各个位姿，确定所述各个位姿与所述第一位姿(X1，Y1，
θ1)的距离偏移量；确定与所述第一位姿(X1，Y1，θ1)的距离偏移量最小的位
姿为参考位姿。若所述距离偏移量最小的位姿的数目超过1，则比较所述距离
偏移量最小的位姿与所述第一位姿(X1，Y1，θ1)的角度偏移量，确定角度偏
移量最小的位姿为参考位姿。然后，获取所述参考位姿对应的激光雷达数据，
将所述参考位姿对应的激光雷达数据与当前帧激光雷达数据进行匹配确定第二
距离偏移量和第二角度偏移量。根据所述参考位姿、第二距离偏移量和第二角
度偏移量确定第二位姿(X2，Y2，θ2)。由于采用匹配获得的较为精确的第一位
姿(X1，Y1，θ1)作为基础位姿，在局部地图库中查找与基础位姿最接近的位
姿的查找速度被大大加快，进而，将与第一位姿(X1，Y1，θ1)最接近的位姿
对应的激光雷达数据和当前帧激光雷达数据进行匹配确定第二位姿的步骤的执
行效率也大大提高。
步骤140、将所述第一位姿与所述第二位姿中匹配误差较小的一个位姿确
定为精确位姿。
由于SLAM算法本身存在累计误差，通过比较所述第一位姿(X1，Y1，θ1)
与所述第二位姿(X2，Y2，θ2)的匹配误差，确定其中匹配误差较小的一个为
精确位姿(X，Y，θ)，放弃匹配误差较大的一个位姿，实现累计误差的闭环控
制，剔除误差较大的数据以消除累计误差，实现在连续多次匹配的快速性和定
位结果的稳定性。
本实施例的技术方案，通过确定估计位姿，根据已获得的室内地图库与所
述估计位姿确定局部地图库；将采集到的当前帧激光雷达数据与上一帧激光雷
达数据进行匹配确定第一位姿；在所述局部地图库中获得与所述第一位姿的偏
移量最小的位姿，将所述当前帧激光雷达数据与所述偏移量最小的位姿对应的
激光雷达数据进行匹配确定第二位姿；将所述第一位姿与所述第二位姿中匹配
误差较小的一个位姿确定为精确位姿。本实施例的技术方案解决因SLAM算法
的计算量大导致算法的时间复杂度较高的问题，以及，通过将第一位姿与第二
位姿进行比较，去掉匹配误差较大的数据，限制了SLAM算法自身的累计误差，
达到了快速精准定位的效果。
在上述技术方案的基础上，在确定估计位姿之前，机器人生成室内地图库
的步骤优选可以替换为采集室内激光雷达数据发送至上位机，以使所述上位机
根据采集到的室内激光雷达数据创建室内地图库；接收所述上位机发送的所述
室内地图库。这样设置的好处在于上位机相比于机器人内的嵌入式芯片来说，
运算处理功能更加强大，可以缩短创建室内地图库的时间。同时，多台机器人
均可以访问上位机获取该室内地图库，为多台机器人协同工作提供了相同的室
内地图作为基准，使得同一室内位置对于不同的机器人具有唯一性和同一性。
实施例二
图2为本发明实施例二提供的一种室内定位方法中数据处理示意图，本实
施例可以以上述实施例为基础，提供了一种优选实例。
参见图2所示，输入数据包括加速度计采集的加速度数据、陀螺仪采集的
方向数据、电子罗盘采集的方向数据、测速编码器采集的速度数据、当前帧激
光雷达数据及室内地图库。机器人内的嵌入式芯片获取设置于机器人上的加速
度计采集的加速度数据、陀螺仪确定的方向数据、电子罗盘确定的方向数据和
测速编码器确定的速度数据(还可以采用霍尔传感器替代测速编码器)，根据上
述获取到的数据估计机器人的位置和姿态。例如，通过融合所述加速度数据、
速度数据、角度数据估计当前机器人的初始位姿(X0，Y0，θ0)。具体可以是通
过加速度计和测速编码器获取机器人当前的加速度和速度，通过所述加速度对
时间的二次积分以及所述速度对时间的积分确定机器人的当前位置坐标(X0，
Y0)。通过陀螺仪和电子罗盘确定机器人的当前方向角θ0。
以所述初始位姿(X0，Y0，θ0)为基础，结合所获取的室内地图库确定所
述初始位姿附近的位姿组成的局部地图库。例如，距离单位是米，初始位姿是
(1m，2m，30°)选择与初始位姿包括的位置分量的距离偏移量为(±0.5m，±0.5m)
的坐标点的激光雷达数据。即选择X坐标和Y坐标的范围是(0.5m，1.5m)至
(1.5m，2.5m)的激光雷达数据。其中，方向角可以与初始位姿的方向角30°
不同。将上述X坐标和Y坐标的范围是(0.5m，1.5m)至(1.5m，2.5m)的激
光雷达数据组成的区域作为局部估计地图库。此时，局部估计地图库中包括的
激光雷达数据仍然比较多，为了进一步减少数据，还通过嵌入式芯片将获取的
当前帧激光雷达数据与所述局部估计地图库中包括的激光雷达数据进行匹配，
去掉匹配误差较大的数据。例如，预先设置最大匹配误差，通过SLAM算法依
次获取将局部估计地图库中各个坐标对应的激光雷达数据，分别将每个坐标对
应的激光雷达数据与当前帧激光雷达数据进行匹配，若匹配误差超过设置的最
大匹配误差，则确定匹配失败，从所述局部估计地图库中去除该不匹配的坐标。
若匹配误差小于设置的最大匹配误差，则确定匹配成功，在所述局部估计地图
库中保留该匹配成功的坐标。在对所述局部估计地图库中包括的所有坐标完成
匹配操作后，根据所有匹配成功的坐标确定局部地图库。
通过嵌入式芯片获取当前时刻的上一帧位姿及激光雷达数据，将所述上一
帧激光雷达数据与当前帧激光雷达数据进行匹配，确定第一距离偏移量与第一
角度偏移量。根据上一帧位姿及第一距离偏移量与第一角度偏移量确定第一位
姿。例如，通过将上一帧位姿中X轴分量及Y轴分量分别叠加上第一距离偏移
量推算出当前位姿的X轴坐标和Y轴坐标(X1，Y1)。通过将上一帧位姿中方
向角叠加所述第一角度偏移量推算出当前位姿的方向角θ1，即推算得到第一位
姿(X1，Y1，θ1)。由于第一位姿是根据当前时刻的上一帧激光雷达数据进行推
算确定的，为了验证其可靠性，将所述第一位姿作为基础位姿，在所述局部地
图库中查找与所述基础位姿最接近的位姿作为参考位姿。例如，若考虑机器人
在行进过程中以平移的方式进行运动，将第一位姿中X轴和Y轴坐标(X1，Y1)
作为基础点，计算所述局部地图库中各个位姿对应的坐标点与基础点(X1，Y1)
的距离，确定距离最短的坐标点对应的位姿为参考位姿。若考虑机器人在行进
过程中存在旋转运动，并且按照上述平移方式确定两个或两个以上与所述基础
点的距离最近的坐标点，则计算距离所述基础点最近的坐标点的方向角与所述
第一位姿的方向角之间的角度偏移量，确定其中的角度偏移量最小的位姿为参
考位姿。然后，获取所述参考位姿对应的激光雷达数据，将所述参考位姿对应
的激光雷达数据与当前帧激光雷达数据进行匹配确定第二距离偏移量和第二角
度偏移量。根据所述参考位姿、第二距离偏移量和第二角度偏移量确定第二位
姿(X2，Y2，θ2)。例如，通过将参考位姿中X轴分量及Y轴分量分别叠加上
第二距离偏移量推算出第二位姿的X轴坐标和Y轴坐标(X2，Y2)。通过将参
考位姿中方向角叠加所述第二角度偏移量推算出当前位姿的方向角θ2，即得到
第二位姿(X2，Y2，θ2)。由于第一位姿已经是相对精确的定位结果，所以，根
据第一位姿结合所述局部地图库，通过最接近位姿查找及激光雷达数据匹配确
定第二位姿的操作的速度被极大提高。再通过比较第一位姿与第二位姿的匹配
误差，确定匹配误差较小的一个位姿为精确位姿。通过比较操作，实现对根据
上一帧激光雷达数据推算当前位姿的运算进行验证，消除SLAM算法的累计误
差，实现在连续多次匹配的过程中的快速性和定位结果的稳定性。
所述嵌入式芯片输出所述精确位姿(X，Y，θ)用于机器人导航。例如，
可以通过A*算法确定由(X，Y，θ)至目标点的最优路径。
实施例三
图3所示为本发明实施例三提供的一种室内定位装置的结构示意图，本实
施例可适用于无需预先在室内环境中部署辅助设备的情况下进行机器人快速室
内定位的情况，该室内定位装置的具体结构如下：
定位地图确定单元310，用于确定估计位姿，根据已获得的室内地图库与
所述估计位姿确定局部地图库；
第一位姿确定单元320，用于将采集到的当前帧激光雷达数据与上一帧激
光雷达数据进行匹配确定第一位姿；
第二位姿确定单元330，用于在所述局部地图库中获得与所述第一位姿的
偏移量最小的位姿，将所述当前帧激光雷达数据与所述偏移量最小的位姿对应
的激光雷达数据进行匹配确定第二位姿；
精确位姿确定单元340，用于将所述第一位姿与所述第二位姿中匹配误差
较小的一个位姿确定为精确位姿。
本实施例的技术方案，通过定位地图确定单元310确定估计位姿，根据已
获得的室内地图库与所述估计位姿确定局部地图库；采用第一位姿确定单元320
将采集到的当前帧激光雷达数据与上一帧激光雷达数据进行匹配确定第一位姿；
并采用第二位姿确定单元330在所述局部地图库中获得与所述第一位姿的偏移
量最小的位姿，将所述当前帧激光雷达数据与所述偏移量最小的位姿对应的激
光雷达数据进行匹配确定第二位姿；再通过精确位姿确定单元340将所述第一
位姿与所述第二位姿中匹配误差较小的一个位姿确定为精确位姿。本实施例的
技术方案解决因SLAM算法的计算量大导致算法的时间复杂度较高的问题，以
及，通过将第一位姿与第二位姿进行比较，去掉匹配误差较大的数据，限制了
SLAM算法自身的累计误差，达到了快速精准定位的效果。
进一步的，所述装置还包括：
雷达数据发送单元，用于在确定估计位姿之前，采集室内激光雷达数据发
送至上位机，以使所述上位机根据采集到的室内激光雷达数据创建室内地图库；
室内地图库接收单元，用于接收所述上位机发送的所述室内地图库。
这样设置的好处在于上位机相比于机器人内的嵌入式芯片来说，运算处理
功能更加强大，可以缩短创建室内地图库的时间。同时，多台机器人均可以访
问上位机获取该室内地图库，为多台机器人协同工作提供了相同的室内地图作
为基准，使得同一室内位置对于不同的机器人具有唯一性和同一性。
进一步的，所述定位地图确定单元310具体用于：
在室内地图库中获得与所述估计位姿的距离偏移量不超过设定阈值的位姿，
确定距离偏移量不超过设定阈值的各个位姿对应的激光雷达数据；
将采集到的当前帧激光雷达数据与所述各个位姿对应的激光雷达数据进行
匹配，根据各个位姿对应的激光雷达数据中与所采集到的当前帧激光雷达数据
相匹配的激光雷达数据确定局部地图库。
这样设置的好处在于缩小了数据匹配的范围，减少了匹配数据量，进而提
高SLAM算法的匹配速度，缩小SLAM算法的时间复杂度。
进一步的，所述第一位姿确定单元320具体用于：
将采集到的当前帧激光雷达数据与相邻的上一帧激光雷达数据进行匹配确
定第一距离偏移量及第一角度偏移量；
根据所述第一距离偏移量和第一角度偏移量调整所述上一帧激光雷达数据
对应的位姿确定第一位姿。
进一步的，所述第二位姿确定单元330具体用于：
获取所述局部地图库包括的各个位姿，确定所述各个位姿与所述第一位姿
的距离偏移量；
确定与所述第一位姿的距离偏移量最小的位姿为参考位姿；
若所述距离偏移量最小的位姿的数目超过1，则比较所述距离偏移量最小
的位姿与所述第一位姿的角度偏移量，确定角度偏移量最小的位姿为参考位姿；
将所述参考位姿对应的激光雷达数据与当前帧激光雷达数据进行匹配确定
第二距离偏移量和第二角度偏移量，根据所述参考位姿、第二距离偏移量和第
二角度偏移量确定第二位姿。
这样设置的好处在于由于第一位姿已经是相对精确的定位结果，所以，将
第一位姿作为基础位姿，在局部地图库中查找与基础位姿最接近的位姿的查找
速度被大大加快，进而，将与第一位姿最接近的位姿对应的激光雷达数据和当
前帧激光雷达数据进行匹配确定第二位姿的步骤的执行效率也大大提高。
上述室内定位装置可执行本发明任意实施例所提供的室内定位方法，具备
执行方法相应的功能模块和有益效果。
将意识到的是，本发明也扩展到适合于将本发明付诸实践的计算机程序，
特别是载体上或者载体中的计算机程序。程序可以以源代码、目标代码、代码
中间源和诸如部分编译的形式的目标代码的形式，或者以任何其它适合在按照
本发明的方法的实现中使用的形式。也将注意的是，这样的程序可能具有许多
不同的构架设计。例如，实现按照本发明的方法或者系统的功能性的程序代码
可能被再分为一个或者多个子例程。
用于在这些子例程中间分布功能性的许多不同方式将对技术人员而言是明
显的。子例程可以一起存储在一个可执行文件中，从而形成自含式的程序。这
样的可执行文件可以包括计算机可执行指令，例如处理器指令和/或解释器指令
(例如，Java解释器指令)。可替换地，子例程的一个或者多个或者所有子例程
都可以存储在至少一个外部库文件中，并且与主程序静态地或者动态地(例如
在运行时间)链接。主程序含有对子例程中的至少一个的至少一个调用。子例
程也可以包括对彼此的函数调用。涉及计算机程序产品的实施例包括对应于所
阐明方法中至少一种方法的处理步骤的每一步骤的计算机可执行指令。这些指
令可以被再分成子例程和/或被存储在一个或者多个可能静态或者动态链接的
文件中。
另一个涉及计算机程序产品的实施例包括对应于所阐明的系统和/或产品
中至少一个的装置中每个装置的计算机可执行指令。这些指令可以被再分成子
例程和/或被存储在一个或者多个可能静态或者动态链接的文件中。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员
会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进
行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽
然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以
上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，
而本发明的范围由所附的权利要求范围决定。