CN107144292A - 一种运动设备的里程计方法以及里程计装置 - Google Patents

Abstract

本发明公开了一种运动设备的里程计方法和里程计装置，包括获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图；根据当前时刻的位姿，以及第二激光帧与局部点云地图，计算得到当前时刻的最优位姿等步骤，本发明提供的里程计方法，通过码盘转数获得初始相对运动量及位姿，通过迭代近邻算法等进行激光帧间匹配，以此获得较准确的相对运动量和位姿。以上一步获得的位姿为初值，对激光帧与局部点云地图进行匹配，获得更准确的位姿及运动量，并对外输出。本发明提供的里程计装置，解决了现有的现有里程计不精确以及累计误差的不足，可用于众多通用或专用的计算***环境或配置中。

Description

一种运动设备的里程计方法以及里程计装置

技术领域

本发明属于运动设备的运动技术领域，特别是涉及运动设备的里程计方法和里程计装置。

背景技术

移动机器人在室内环境中移动时，里程计信息是机器人运动状态的最基本信息。机器人的里程计是一种利用致动器的移动数据来估算机器人位姿随时间改变量的方法。这个位姿变化可由编码器和惯性导航传感器来得到。传统的编码器估计历程方法仅仅利用内部传感器信息的累计得到里程信息，例如一个机器人在其轮子或腿关节处配备有旋转编码器等设备，当它向前移动一段时间后，想要知道大致的移动距离，借助旋转编码器，可以测量出轮子旋转的圈数，如果知道了轮子的周长，便可以计算出机器人移动的距离。

但这类里程计总是会遇到精度问题。由于误差是由多种因素混合产生，并且误差随时间的累积导致了里程计的读数随着时间的增加而变得越来越不可靠。

发明内容

为了解决现有技术中里程计精度低误差高的缺陷，本发明提供了一种运动设备的里程计方法以及里程计装置。

本发明是通过以下技术方案来实现的：获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图；

根据当前时刻的位姿，以及第二激光帧与局部点云地图，计算得到当前时刻的最优位姿。

进一步的，以运动设备当前时刻的位姿为初值，对第二激光帧与局部点云地图进行点云匹配得到最优位姿，即所述最优位姿为根据以下公式获得的最优化解，所述公式为：

其中：为局部点云地图构成的曲面；p_i为第二激光帧内的激光点；符号代表旋转-平移变换；符号代表点到局部点云地图构成的曲面的欧几里得投影；P为计算过程中的位姿，以当前时刻的位姿为初值。

进一步的，所述第二激光帧为剔除动态激光点后的第二激光帧。

进一步的，剔除第二激光帧中的动态激光点的步骤包括：根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标；根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标；根据栅格坐标所在栅格的概率值确定该激光点是否为动态激光点，如果是则剔除动态激光点。

进一步的，所述当前时刻的位姿为较优位姿，具体的，所述较优位姿的获取包括步骤：获取运动设备第一时刻到当前时刻的初始相对运动量；获取第一时刻的第一激光帧以及当前时刻的第二激光帧；根据初始相对运动量，以及第一激光帧与第二激光帧，得到帧间相对运动量；根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿。

进一步的，以相对运动量为初值，对第一激光帧与第二激光帧进行帧间匹配得到帧间运动量。

进一步地，所述帧间运动量具体通过求解以下公式的最优化解获得，公式为：

其中：为第一激光帧构成的曲面；p_i为第二激光帧的激光点；q为计算过程中运动设备的相对运动量，以相对运动量为初值；符号代表旋转-平移变换；符号代表点到第一激光帧构成的曲面的欧几里得投影。

进一步的，根据最优位姿与第二激光帧更新地图，包括更新局部点云地图和/或更新局部概率栅格地图。

进一步的，当相对运动量的位移达到设定阈值时，重置栅格地图，所述重置栅格地图包括重置栅格地图的概率值，重置地图原点、更新局部概率栅格地图。

本发明还提供一种里程计装置，其技术方案是所述里程计装置包括

第一获取模块，获取运动设备当前时刻的位姿；

第二获取模块，获取运动设备当前时刻的第二激光帧；

第三获取模块，获取局部点云地图；

第一运算模块，根据当前时刻的位姿，以及第二激光帧与局部点云地图，计算得到当前时刻的最优位姿；

进一步的，所述装置还包括剔除模块，用于剔除第二激光帧内的动态激光点；所述第二获取模块获取剔除动态激光点后的第二激光帧。

进一步的,所述剔除模块包括

第一坐标单元，根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标；

第二坐标单元，根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标；

判断单元，根据栅格坐标所在栅格的概率值判断该激光点是否为动态激光点；

剔除单元，判断单元的判断结果剔除动态激光点，即判断结果为动态激光点则剔除。

进一步的,所述装置还包括地图更新模块，用于根据最优位姿与第二激光帧更新局部点云地图和/或更新局部概率栅格地图。

进一步的,还包括地图重置模块，用于根据运动设备的移动距离重置局部概率栅格地图。

进一步的，所述第一获取模块具体用于获取当前时刻的较优位姿，包括

第一获取单元，获取运动设备第一时刻到当前时刻的初始相对运动量；

第二获取单元，获取第一时刻的第一激光帧以及当前时刻的第二激光帧；

第一计算单元，根据初始相对运动量，以及第一激光帧与第二激光帧，得到帧间相对运动量；

第二计算单元，根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿；

本发明还提供另一种里程计装置，包括处理器以及用于存储处理器可执行的指令的存储器；

所述处理器被配置为：

获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图，

根据当前时刻的位姿，以及第二激光帧与局部点云地图，得到当前时刻的最优位姿。本发明的有益效果如下：

本发明提供的里程计方法，通过码盘转数获得初始相对运动量及位姿，通过迭代近邻算法等进行激光帧间匹配，以此获得较准确的相对运动量和位姿。以上一步获得的位姿为初值，对激光帧与局部点云地图进行匹配，获得更准确的位姿及运动量，并对外输出。本方法无需先验全局地图，能有效抑制里程计的累积误差，对动态环境有较好的适应性；大范围闭环(200米)误差在1米之内。

本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)。

本发明提供的里程计装置，解决了现有的现有里程计不精确以及累计误差的不足，可用于众多通用或专用的计算***环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器***、基于微处理器的***、机顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何***或设备的分布式计算环境等等。

附图说明

图1是本发明里程计方法实施例1的流程示意图；

图2是本发明里程计方法实施例1获得较优当前位姿的流程示意图；

图3是本发明里程计方法实施例2的流程示意图；

图4是本发明里程计装置的模块构成示意图。

具体实施方式

结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例：

实施例一

图1是本实施例提供的里程计方法的流程示意图。

本方法应用的场景中的设备包括运动设备，运算设备，以及设置在运动设备上的激光扫描设备与物理里程设备。运动设备可以是移动机器人、扫地机器人、机器玩具或者需要在室内移动并定位的智能家居等。运算设备用于对各种数据进行计算，数据来源为运动设备、激光扫描设备、物理里程设备等。物理里程设备用于检测获得运动设备的运动数据来估算运动设备位姿随时间改变量，包括位置改变量、角度改变量等，一般设置于运动设备上，例如码盘，其他光电编码器，或者其他可以测量在一段时间内运动设备的位移量的检测元件。

一般对运动设备如移动机器人设定有一定的活动区域。根据不同的运动设备的可以建立不同的运动模型，常见的有差分驱动的运动模型、全向移动的运动模型。在特定的实施例中具有相邻的两个时刻，即前一时刻和当前时刻，一般根据不同的运动模型以及已知的运动设备前一时刻的位姿(x_t-1，y_t-1，θ_t-1)，可以获得运动设备当前时刻的位姿(x_t，y_t，θ_t)，并且可以根据两个时刻的位姿计算得到相对运动量(Δx_t，Δy_t，Δθ_t)。例如对于双轮差分的运动模型，左右两轮的驱动电机均安装***盘作为物理里程设备，可通过如下公式获得到进行当前时刻的位姿(x_t，y_t，θ_t)

ΔD_t＝(Δd_L+Δd_R)/2

Δθ_t＝(Δd_L-Δd_R)/ω

θ_t＝θ_t-1+Δθ_t

其中，左、右两轮之间的间距为ω，ΔD_t为根据码盘的信息计算获得的运动设备的距离变化量，Δθ_t为运动设备的角度变化量，Δd_L、Δd_R分别为通过左右码盘获得的左右两轮的运动量；

再如全向移动的运动模型的四个驱动轮的驱动电机均安装***盘，可通过如下公式获得到进行当前时刻的位姿(x_t，y_t，θ_t)

x_t＝x_t-₁+Δx

y_t＝y_t-1+Δy

θ_t＝θ_t-1+Δθ

其中，Δd₁，Δd₂，Δd₃，Δd₄分别为通过码盘获得的四个驱动轮上的运动量，r为轮毂半径，r_r为辊子半径，前后轴距为2l_x，左右轮距为2l_y；

激光扫描设备的扫描方向为运动设备移动方向的前方，激光扫描设备扫描获得的每一激光帧数据通过现有技术以激光点云的形式呈现。设立全局坐标系，运动设备的位姿、激光扫描设备的位置以及获得的激光帧数据可以统一到全局坐标系中。

局部点云地图通过用激光扫描设备扫描局部环境形成，存储于运算设备中。局部概率栅格地图，则是把局部点云地图分成一系列栅格，建立局部概率栅格地图坐标系，其中每一栅格给定一个可能值，表示该栅格被占据的概率。点云地图坐标系、栅格地图坐标系与全局坐标系可以相互转换，以确定运动设备在地图坐标系中的位姿。

坐标系的设立、坐标变换以及坐标系内的点映射到其他坐标系均可以通过现有技术实现。

如图1所示的里程计方法，包括以下步骤：

步骤S110：获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图。

所述当前时刻为t，所述当前时刻的位姿是通过物理里程设备或者现有技术可获得当前时刻的初始位姿

同时在当前时刻t，通过激光扫描设备扫描得到的第二激光帧，激光扫描设备将获得的激光帧数据发送给运算设备，具体的可以通过有线或无线的方式，也可以通过总控设备将激光帧数据转发给运算设备。

步骤S120：根据当前时刻的位姿，以及第二激光帧与局部点云地图，得到当前时刻的最优位姿。

具体地，所述局部点云地图存储在运算设备中，运算设备以当前时刻的位姿为初值，对第二激光帧与局部点云地图进行点云匹配得到最优位姿，即根据以下公式获得的最优化解即为最优位姿所述公式为：

其中：为局部点云地图构成的曲面；p_i为第二激光帧内的激光点；符号代表旋转-平移变换；符号代表点到局部点云地图构成的曲面的欧几里得投影；P为计算过程中的位姿，以当前时刻的位姿为初值，其最终的值即为最优位姿

所述第二激光帧与局部点云地图进行点云匹配可以通过现有技术实现，例如迭代近邻算法、暴力匹配等。所述最优位姿是由运算设备通过上述技术方案计算获得的当前时刻的位姿，比步骤S110直接通过现有技术或者物理里程设备获得的位姿更准确的。

在本实施例中，作为进一步改进，所述当前时刻的位姿还可以是通过以下技术方案获得的较优位姿用于代替通过物理里程设备或者现有技术可获得当前时刻的初始位姿图2是本发明里程计方法实施例1获得较优当前位姿的流程示意图；具体的，所述较优位姿的获取包括以下步骤：

步骤S111：获取运动设备第一时刻到当前时刻的初始相对运动量。

所述第一时刻为当前时刻之前的某一时间点，一般为当前时刻的前一时刻t-1。所述初始相对运动量用于描述运动设备从第一时刻t-1所处的位姿P_t-1＝(x_t-1，y_t-1，θ_t-1)移动到当前时刻t所处的位姿的变化，包括运动设备的位置变化量和角度变化量，用表示。

其中，第一时刻t-1所处的位姿P_t-1＝(x_t-1，y_t-1，θ_t-1)已知，优选地，所述第一时刻t-1所处的位姿P_t-1＝(x_t-1，y_t-1，θ_t-1)为上一次里程计方法获得的最优位姿。在一具体实施中，可以根据第一时刻t-1所处的位姿P_t-1＝(x_t-1，y_t-1，θ_t-1)与当前时刻t所处的初始位姿计算转化为初始相对运动量计算方法为现有技术，且在前面已经描述。

通过物理里程设备或者其他现有技术获得的数据发送给运算设备，通过有线或无线的方式，也可以通过总控设备转发给运算设备进行计算。

步骤S112：获取第一时刻的第一激光帧以及当前时刻的第二激光帧。

在第一时刻t-1，激光扫描设备扫描得到的激光帧为第一激光帧，在当前时刻t，激光扫描设备扫描得到的激光帧为第二激光帧。

步骤S113：根据初始相对运动量，以及第一激光帧与第二激光帧，得到帧间相对运动量。

具体地，以步骤S111中获得的初始相对运动量为初值，对第一激光帧与第二激光帧进行帧间匹配得到帧间相对运动量，即根据以下公式获得的最优化解即为帧间运动量，所述公式为：

其中：为第一激光帧构成的曲面；p_i为第二激光帧的激光点；q为计算过程中运动设备的相对运动量，以相对运动量为初值，其最终的值即为帧间相对运动量；符号代表旋转-平移变换；符号代表点到第一激光帧构成的曲面的欧几里得投影。

所述第一激光帧与第二激光帧的帧间匹配可以通过现有技术实现，例如迭代近邻算法、暴力匹配等。

所述帧间相对运动量是由运算设备在初始相对运动量的基础上计算获得的从第一时刻t-1所处的位姿移动到当前时刻t所处的位姿的变化量，相对于步骤S111中获得的初始相对运动量更准确。

步骤S114：根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿。

第一时刻运动设备的位姿已知，根据第一时刻t-1运动设备的位姿P_t-1＝(x_t-1，y_t-1，θ_t-1)，以及帧间相对运动量可以计算转化为当前时刻的较优位姿所述较优位姿相对于直接通过物理里程设备或其他现有技术获得的当前时刻的位姿更加准确。

进一步地，本发明还包括步骤S131：根据第一时刻运动设备的位姿以及最优位姿计算转化为最优相对运动量。

第一时刻运动设备的位姿已知，根据第一时刻t-1运动设备的位姿P_t-1＝(x_t-1，y_t-1，θ_t-1)，以及最优位姿可以计算获得最优相对运动量

本发明中，步骤S111中，可以根据第一时刻t-1的位姿P_t-1＝(x_t-1，y_t-1，θ_t-1)与当前时刻t的位姿的计算转化为初始相对运动量，计算公式如下：

x_t＝x_t-1+cos(θ_t-1)Δx_t-sin(θ_t-1)Δy_t

y_t＝y_t-1+sin(θ_t-1)Δx_t+cos(θ_t-1)Δy_t

θ_t＝θ_t-1+Δθ_t

其中，用当前时刻的较优位姿代替当前时刻t的初始位姿可实现步骤S114中较优位姿与帧间相对运动量的计算转化；用最优位姿代替当前时刻t的位姿可实现步骤S131中最优位姿与最优相对运动量计算转化。同时，通过本发明还可以上述公式根据运动设备第一时刻的位姿以及最优位姿计算获得最优相对运动量。

本发明提供的里程计方法，通过码盘转数获得初始相对运动量及位姿，通过迭代近邻算法等进行激光帧间匹配，以此获得较准确的相对运动量和位姿。以上一步获得的位姿为初值，对激光帧与局部点云地图进行匹配，获得更准确的位姿及运动量，并对外输出。本方法无需先验全局地图，能有效抑制里程计的累积误差，对动态环境有较好的适应性；大范围闭环(200米)误差在1米之内。

实施例二

图3是本发明里程计方法实施例2的流程示意图。

本实施例的里程计方法包括

步骤S210：获取运动设备当前时刻的位姿、当前时刻的剔除激光动态点后的第二激光帧以及局部点云地图。

引入局部概率栅格地图，将概率栅格地图的概率值作为删除动态点的依据。将第二激光帧映射到局部概率栅格地图，若激光点所在栅格的概率值小于静态栅格的最低概率值则为动态栅格，即该栅格内的激光点均为动态激光点，具体地，包括以下步骤：

步骤S211：根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标；

步骤S212：根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标；

步骤S213：根据栅格坐标所在栅格的概率值确定该激光点是否为动态激光点，如果是则剔除动态激光点。

原理如下：

第二激光帧内激光点为激光点i，通过旋转-平移变换将第二激光帧映射到局部概率栅格地图得到激光点i的位置坐标L_i，即

局部概率栅格地图的原点坐标O，栅格大小reso，激光点i所在栅格的栅格坐标(m，n)，则

则当0≤Bel(m，n)≤Bel_thres时，判定该栅格内激光点i为动态激光点。其中，Bel(m，n)为栅格的概率值，Bel_thres为静态栅格的最低概率值，Bel(m，n)、Bel_thres的值、动态激光点的剔除均通过现有技术实现，本发明不再赘述。

步骤S220：根据当前时刻的位姿，以及剔除动态激光点后的第二激光帧与局部点云地图，得到当前时刻的最优位姿。

具体地，以当前时刻的位姿为初值，对剔除动态点后的第二激光帧与局部点云地图进行点云匹配得到最优位姿，即根据以下公式获得的最优化解即为最优位姿所述公式为：

其中：为局部点云地图构成的曲面；为剔除动态激光点后的第二激光帧内的激光点；P为计算过程中的位姿，以当前时刻的位姿为初值，其最终的值即为最优位姿；符号代表旋转-平移变换；符号代表点到局部点云地图构成的曲面的欧几里得投影。

本实施例所述的里程计方法与实施例一中不同的是步骤S210与步骤S220中的第二激光帧为剔除动态激光点后的激光帧，步骤S210、步骤S220与步骤S211中的当前时刻的位姿也可以用通过步骤S111-步骤S114的方法获得的当前时刻的较优位姿。

在另一实施例中，本发明的里程计方法还包括步骤S240：根据步骤S220获得的最优位姿与第二激光帧更新地图，包括更新局部概率栅格地图和更新局部点云地图。此步骤获得的局部概率栅格地图和局部点云地图应用于下一次里程计方法实现的过程，以获得更精确的里程数据，具体包括以下子步骤：

步骤S241：将第二激光帧内的激光点映射到全局坐标系中得到激光点在全局坐标系的位置。其中，激光点i可以为剔除动态激光点后的第二激光帧内的激光点，激光点i在全局坐标系的位置通过旋转-平移变换将第二激光帧映射到全局坐标系中，即

步骤S242：将所述位置***到局部点云地图，完成局部点云地图的更新；此步骤可以通过现有技术实现，这里不再赘述；

步骤S243：以光线追踪方式，更新局部概率栅格地图；包括更新位置点附近的栅格的概率值以及激光扫描设备和位置点连线上的概率值。具体地，一般以方差较小的高斯分布，更新位置点附近栅格的概率值；将激光扫描设备和位置点连线上的栅格概率置零，零表示该栅格被占据的概率为0。

在另一实施例中，里程计不断累积里程，作为本发明的又一种改进，还包括步骤S250根据运动设备的移动距离重置概率栅格地图。具体的，包括子步骤：

步骤S251：计算运动设备的移动距离，并判断是否到达设定距离；若到达设定距离则进行步骤S252、步骤S253、步骤S254进行重置地图，所述重置栅格地图包括重置栅格地图的概率值，重置地图原点、更新局部概率栅格地图。所述运动设备的移动距离通过本发明的里程计方法实现的里程计获得。

步骤S252：将所有栅格概率值均置为表示未知的概率值，一般-1表示该栅格的概率值未知，即该栅格不知道是否被占据；

步骤S253：根据最优位姿在地图中的位置确定地图原点；具体的，以最优位姿在地图中的位置或者平移一定距离的位置为地图原点；

步骤S254：用当前时刻最近的N帧激光帧以光线追踪方式更新局部概率栅格地图。更新方法与步骤S243一致，以此在下一次里程计方法中可以获得更准确的运动设备的位姿。

实施例三

图4是本发明里程计装置的模块构成示意图。

一种里程计装置，包括

第一获取模块，获取运动设备当前时刻的位姿；

第二获取模块，获取运动设备当前时刻的第二激光帧；

第三获取模块，获取局部点云地图；

第一运算模块，根据当前时刻的位姿，以及第二激光帧与局部点云地图，计算得到当前时刻的最优位姿。

在本实施例中，作为进一步改进，所述第一获取模块具体用于获取当前时刻的较优位姿位姿，具体地，所述第一获取模块包括

第一获取单元，获取运动设备第一时刻到当前时刻的初始相对运动量。

第二获取单元，获取第一时刻的第一激光帧以及当前时刻的第二激光帧。

第一计算单元，根据初始相对运动量，以及第一激光帧与第二激光帧，得到帧间相对运动量。

第二计算单元，根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿。

进一步地，本发明的里程计装置还包括剔除模块，用于剔除第二激光帧内的动态激光点，所述第二获取模块获取剔除动态激光点后的第二激光帧。具体的，所述剔除模块包括：

第一坐标单元，根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标；

第二坐标单元，根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标；

判断单元，根据栅格坐标所在栅格的概率值判断该激光点是否为动态激光点，

剔除单元，判断单元的判断结果剔除动态激光点，即判断结果为动态激光点则剔除。

进一步地，本发明的里程计装置还包括

用于根据最优位姿与第二激光帧更新局部点云地图以及局部概率栅格地图。具体地，所述地图更新模块包括：

映射单元，将第二激光帧内的激光点映射到全局坐标系中，得到激光点在全局坐标系的位置。通过旋转-平移变换，即通过公式将第二激光帧内的激光点映射到全局坐标系中。

第一更新单元，将位置***到局部点云地图，以此完成局部点云地图的更新。

第二更新单元，以光线追踪方式，更新局部概率栅格地图。具体地，一般以方差较小的高斯分布，更新位置点附近栅格的概率值；将激光扫描设备和位置点连线上的栅格概率置零，零表示该栅格点未被占据的概率值。

进一步的，本发明的里程计装置还包括地图重置模块，用于根据运动设备的移动距离重置局部概率栅格地图。所述地图重置模块包括

距离计算单元，计算运动设备的移动距离；所述计算方法为现有技术，或者通过本方法的里程计根据最优位姿计算获得。

第二判断单元，判断所述移动距离是否到达设定距离；

若到达设定距离，所述地图重置模块通过概率值重置单元、原点重置单元、地图重置单元重置局部概率栅格地图。即概率值重置单元，根据判断结果将所有栅格概率值均置为表示未知的概率值；原点重置单元，根据判断结果将最优位姿在地图中的位置重置为地图原点；地图重置单元，根据判断结果，以光线追踪方式，利用当前时刻最近的N帧激光帧更新局部概率栅格地图。

本实施例中的装置与前述实施例中的方法是基于同一发明构思下的两个方面，在前面已经对方法实施过程作了详细的描述，所以本领域技术人员可根据前述描述清楚地了解本实施中的***的结构及实施过程，为了说明书的简洁，在此就不再赘述。

为了描述的方便，描述以上装置时以功能分为各种模块分别描述。当然，在实施本发明时可以把各模块的功能在同一个或多个软件和/或硬件中实现。

通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。

描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的模块或单元可以是或者也可以不是物理上分开的，作为模块或单元示意的部件可以是或者也可以不是物理模块，既可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明可用于众多通用或专用的计算***环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器***、基于微处理器的***、机顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何***或设备的分布式计算环境等等，如实施例四。

实施例四

如图所示的里程计装置，包括：处理器以及用于存储处理器可执行的指令的存储器；

所述处理器被配置为：

获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图，

根据当前时刻的位姿，以及第二激光帧与局部点云地图，得到当前时刻的最优位姿。

本发明实施例提供的里程计，解决了现有的现有里程计不精确以及累计误差的不足。对于本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及变形，而所有的这些改变以及变形都应该属于本发明权利要求的保护范围之内。

Claims (17)

1.一种里程计方法，其特征在于，包括

获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图；

根据当前时刻的位姿，以及第二激光帧与局部点云地图，计算得到当前时刻的最优位姿。

2.根据权利要求1所述的里程计方法，其特征在于，以运动设备当前时刻的位姿为初值，对第二激光帧与局部点云地图进行点云匹配得到最优位姿。

3.根据权利要求2所述的里程计方法，其特征在于，所述最优位姿具体通过求解以下公式的最优化解获得，所述公式为：

其中：为局部点云地图构成的曲面；p_i为第二激光帧内的激光点；符号代表旋转-平移变换；符号代表点到局部点云地图构成的曲面的欧几里得投影；P为计算过程中的位姿，以当前时刻的位姿为初值。

4.根据权利要求1所述的里程计方法，其特征在于，所述第二激光帧为剔除动态激光点后的第二激光帧。

5.根据权利要求4所述的里程计方法，其特征在于，剔除第二激光帧中的动态激光点的步骤包括：

根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标；

根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标；

根据栅格坐标所在栅格的概率值确定该激光点是否为动态激光点，如果是则剔除动态激光点。

6.根据权利要求1所述的里程计方法，其特征在于，所述当前时刻的位姿为较优位姿，具体的，所述较优位姿的获取包括步骤：

获取运动设备第一时刻到当前时刻的初始相对运动量；

获取第一时刻的第一激光帧以及当前时刻的第二激光帧；

根据初始相对运动量，以及第一激光帧与第二激光帧，得到帧间相对运动量；

根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿；

7.根据权利要求6所述的里程计方法，其特征在于，以相对运动量为初值，对第一激光帧与第二激光帧进行帧间匹配得到帧间运动量。

8.根据权利要求7所述的里程计方法，其特征在于，所述帧间运动量具体通过求解以下公式的最优化解获得，所述公式为：

其中：为第一激光帧构成的曲面；p_i为第二激光帧的激光点；q为计算过程中运动设备的相对运动量，以相对运动量为初值；符号代表旋转-平移变换；符号代表点到第一激光帧构成的曲面的欧几里得投影。

9.根据权利要求1-8任一项所述的里程计方法，其特征在于，根据最优位姿与第二激光帧更新地图，包括更新局部点云地图和/或更新局部概率栅格地图。

10.根据权利要求1-8任一项所述的里程计方法，其特征在于，当相对运动量的位移达到设定阈值时，重置栅格地图，所述重置栅格地图包括重置栅格地图的概率值，重置地图原点、更新局部概率栅格地图。

11.一种里程计装置，其特征在于，包括

第一获取模块，获取运动设备当前时刻的位姿；

第二获取模块，获取运动设备当前时刻的第二激光帧；

第三获取模块，获取局部点云地图；

第一运算模块，根据当前时刻的位姿，以及第二激光帧与局部点云地图，计算得到当前时刻的最优位姿。

12.根据权利要求11所述的里程计装置，其特征在于，所述装置还包括剔除模块，用于剔除第二激光帧内的动态激光点；所述第二获取模块获取剔除动态激光点后的第二激光帧。

13.根据权利要求12所述的里程计装置，其特征在于，所述剔除模块包括

第一坐标单元，根据当前时刻的位姿获得第二激光帧内激光点在局部概率栅格地图的位置坐标；

第二坐标单元，根据所述位置坐标确定第二激光帧内激光点在局部概率栅格地图上的栅格坐标；

判断单元，根据栅格坐标所在栅格的概率值判断该激光点是否为动态激光点；

剔除单元，判断单元的判断结果剔除动态激光点，即判断结果为动态激光点则剔除。

14.根据权利要求11-13任一项所述的里程计装置，其特征在于，所述装置还包括地图更新模块，用于根据最优位姿与第二激光帧更新局部点云地图和/或更新局部概率栅格地图。

15.根据权利要求11-13任一项所述的里程计装置，其特征在于，还包括地图重置模块，用于根据运动设备的移动距离重置局部概率栅格地图。

16.根据权利要求11-13任一项所述的里程计装置，其特征在于，所述第一获取模块具体用于获取当前时刻的较优位姿，包括

第一获取单元，获取运动设备第一时刻到当前时刻的初始相对运动量；

第二获取单元，获取第一时刻的第一激光帧以及当前时刻的第二激光帧；

第一计算单元，根据初始相对运动量，以及第一激光帧与第二激光帧，得到帧间相对运动量；

第二计算单元，根据第一时刻运动设备的位姿以及帧间相对运动量计算获得当前时刻的较优位姿。

17.一种里程计装置，其特征在于，所述的里程计装置包括：处理器以及用于存储处理器可执行的指令的存储器；

所述处理器被配置为：

获取运动设备当前时刻的位姿、当前时刻的第二激光帧以及局部点云地图，

根据当前时刻的位姿，以及第二激光帧与局部点云地图，得到当前时刻的最优位姿。