一种基于SLAM建图的开环优化方法及系统与流程
本发明涉及导航技术领域，特别是涉及一种基于slam建图的开环优化方法及系统。
背景技术：
即时定位与地图构建(simultaneouslocalizationandmapping，slam)技术，即在一个静态的未知环境中，通过机器人的运动和测量，来学习环境地图，并且同时确定机器人在地图中的位置。
通俗的来讲，slam回答两个问题：“我在哪儿？”“我周围是什么？”，就如同人到了一个陌生环境中一样，slam试图要解决的就是恢复出观察者自身和周围环境的相对空间关系，“我在哪儿”对应的就是定位问题，而“我周围是什么”对应的就是建图问题，给出周围环境的一个描述。
slam问题最早由1985年cheeseman和smith撰写的文章提出，文中创立了描述几何不确定性和特征与特征之间相互关系的统计学原理，这些原理构成了求解slam问题的数学基础。smith，self和cheeseman在1987年提出基于卡尔曼滤波的slam算法，该理论研究框架使基于卡尔曼滤波的slam算法成为最广泛应用的方法。自从上世纪80年代slam概念的提出到现在，slam技术已经走过了30多年的历史。slam系统使用的传感器在不断拓展，从早期的声呐，到后来的2d/3d激光雷达，再到单目、双目、rgbd、tof等各种相机，以及与惯性测量单元imu等传感器的融合；slam的算法也从开始的基于滤波器的方法(ekf、pf等)向基于优化的方法转变，技术框架也从开始的单一线程向多线程演进。
无人车是一种轮式机器人。近年来，随着人工智能技术的发展，无人车技术已经成为了当前十分热门的领域之一。其中基于卫星的导航定位技术发展已经相对成熟，但由于卫星信号易受到外界干扰，而且在城市道路中由于高楼、大桥、隧道等的遮挡致使gps信号丢失，室内环境下gps导航无法使用，因此发展不依赖卫星的定位技术已经成为了必然趋势，slam技术成为解决此技术需求的重要方法。无人车作为一种特殊机器人，其对周围环境地图的构建以及对于自身在环境中的精确定位是实现无人车安全行驶的前提。无人车slam技术，是通过车载传感器(摄像头，激光雷达，超声波等)对周围环境进行感知认知的过程，是未来人工智能方向的一个主要研究技术。
slam建图技术包括视觉slam和激光slam，其中，激光slam技术由于发展较早，技术比较成熟，一直处于技术主导地位。
视觉slam相比激光slam更便宜，轻便，图像信息更加丰富，但对数据运算处理的实时性有一定要求，且易受环境变化的影响。激光雷达具有精度高、速度快、能够快速响应动静状态下的环境变化等特点，因此激光slam技术的可靠性和安全性均优于图像slam。
无论何种slam技术在建图的过程中都将依赖上一时刻自身所在的位置来确定下一时刻自身的位置。由于任何传感器都有一定的误差，在基于激光slam的机器人建图过程中，误差会逐渐累计，当在场景较小的环境中可以通过局部闭环检测对偏差进行修正。但在大回环和长廊环境下，累积误差过大则会导致机器人无法闭环，建图失败。
技术实现要素：
本说明书实施方式的目的是提供一种基于slam建图的开环优化方法，能够根据环境中的绝对位置信息进行slam建图的实时地图构建，适用于大场景下的机器人稳定建图和多机器人协同地图构建。
本说明书实施方式提供一种基于slam建图的开环优化方法，通过以下技术方案实现：
获取需要建图设备的所处的局部环境地图信息，并基于该局部环境地图信息进行建图；
开环状态下，需要建图设备在接收到外部绝对定位信息时，建立绝对定位信息与当前运动位置信息之间的约束，根据约束信息进行全局地图的构建。
本说明书实施方式提供一种基于slam建图的开环优化系统，通过以下技术方案实现：
局部环境地图的构建单元，被配置为：获取所处的局部环境地图信息，并基于该局部环境地图信息进行建图；
全局地图的构建单元，被配置为：在接收到外部绝对定位信息时，建立绝对定位信息与当前运动位置信息之间的约束，根据约束信息进行全局地图的构建。
本说明书实施方式提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现一种基于slam建图的开环优化方法步骤。
本说明书实施方式提供一种计算机可读存储介质，其上存储有计算机程序(指令)，其特征在于，该程序(指令)被处理器执行时实现一种基于slam建图的开环优化方法步骤。
本说明书实施方式提供一种机器人，所述机器人包括机器人本体，所述机器人本体上搭载有激光雷达传感器及获取外部绝对定位信息的传感器；
激光雷达传感器检测的数据及外部绝对定位信息均传输至运算处理单元，所述运算处理单元被配置为：
获取需要建图设备的所处的局部环境地图信息，并基于该局部环境地图信息进行地图；
需要建图设备在接收到外部绝对定位信息时，建立绝对定位信息与当前运动位置信息之间的约束，根据约束信息进行全局地图的构建。
与现有技术相比，本公开的有益效果是：
本公开技术方案在传统的单机移动的建图模式基础上改进了建图算法，采用开环优化的方法，通过接收环境中的绝对定位信息增加了建图的稳健性。
本公开技术方案通过接收环境中的绝对定位信息能够消除移动建模模式中的累积误差，在大场景环境下得到更加准确的导航地图。
本公开技术方案通过在原有建图基础上增添不连续的绝对位置信息，保证建图质量提高的同时不影响建图的更新频率。
本公开技术方案通过开环优化接收绝对位置信息的加入，保证了坐标系的统一，有助于解决多机器人协同地图构建时的多地图坐标融合问题。
附图说明
构成本公开的一部分的说明书附图用来提供对本公开的进一步理解，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。
图1为本公开实施例子slam建图系统硬件架构图；
图2为本公开实施例子slam建图系统软件架构图；
图3为本公开实施例子基于开环优化的地图构建工作流程图；
图4为本公开实施例子基于开环优化的地图构建算法流程图。
具体实施方式
应该指出，以下详细说明都是例示性的，旨在对本公开提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本公开所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本公开的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
专利号“cn201810804313.0”公开了“一种实时slam场景地图构建系统、导航系统及方法”，该专利通过多机器人协同建图分别上传到云端，进行整体地图优化。本公开技术方案是利用预先设定的绝对位置信息，对机器人建图进行开环优化。此方法可用于优化单个机器人大场景下的建图误差，也可将多个机器人优化后的地图直接进行拼接，实现多机器人协同建图。
在大场景下，如果使用多个机器人进行协同建图，可以在场景下随机的添加分散的绝对定位信息(如带有自身全局坐标的二维码等)，每个机器人看到二维码后，根据机器人相对二维码的位姿和二维码中包含的全局定位信息，即可知道每个机器人在全局坐标中的位置，且机器人建图过程中有二维码包含的绝对定位信息进行修正。根据绝对定位信息将每个机器人建的已优化的地图拼接起来，即是多机器人协同构图。该方法只需获得场景中稀疏的绝对定位信息即可。
在普通的闭环优化中，只有当机器人检测到之前经过的场景的时候才进行闭环全局优化，且存在一定误差。当在建图过程中加入如包含绝对定位信息的二维码时，机器人检测到二维码时即可进行全局优化，不需要等到回到之前经过的场景再优化。即通过外部绝对信息进行开环优化。
实施例子一
本说明书实施方式提供一种基于slam建图的开环优化方法，通过以下技术方案实现：
获取需要建图设备的所处的局部环境地图信息，并基于该局部环境地图信息进行建图；
需要建图设备在接收到外部绝对定位信息时，建立绝对定位信息与当前运动位置信息之间的约束，根据约束信息进行全局地图的构建。
参见附图3所示，针对环境信息，利用传感器获取机器人的绝对位姿信息，利用激光雷达获取激光数据，针对绝对位姿信息，首先进行判断是否为第一帧绝对位姿信息，若是，则基于该信息确定全局坐标系，之后将绝对位姿作为观测位姿，否则，直接将绝对位姿作为观测位姿，针对激光数据，利用scanmatcher扫描匹配算法求出机器人运动位姿，利用bbs闭环检测求出观测位姿。
然后判断是否出现绝对位姿数据，若是，则添加scan与绝对位姿之间的加权约束，若否，则添加scan与submap之间的闭环约束，构建残差方程进行spa优化。
整体步骤可参见附图4所示，机器人各模块上电启动，运行ros操作系统，采集各节点传感器数据，进行全局地图构建，当出现绝对位姿时进行开环优化。
在具体实施时，利用360°激光雷达获取到当前位置的地图信息，具体进行地图的构建参见以下方案：
搭载激光雷达的移动平台获取环境中的激光数据，前端建图采用端到端的建图方式，将每一帧激光数据通过scan-to-map(扫描点直接映射到地图)的方式映射到子地图中，即很多相邻的scan扫描点通过扫描匹配估计位姿，插入到submap子地图中形成子地图。通过扫描匹配，scan扫描点可以匹配到一个在子地图中最好的估计位姿。由于整个过程时间很短，因此累积的误差也很小，并且通过扫描匹配确定了机器人的运动轨迹。
在子地图数量增多后，通过扫描匹配确定机器人的运动轨迹与实际运动轨迹之间的累计误差慢慢变大，因此需要进行后端优化。每一帧scan扫描帧不仅只与上一帧submap子地图进行匹配，还与之前几帧的submap子地图进行比较，建立局部回环，对局部地图进行优化。
常规的闭环检测过程是当运动平台又回到之前走过的位置时，将当前激光的扫描帧与之前在该位置所存储的地图进行对比，通过分支定界算法找到在存储地图中对应的位置。如果在理想的，没有偏差的情况下，当前通过scan-matcher扫描匹配的轨迹位姿与存储在地图中对应的位姿是一致的。但是由于运动误差的存在，使这两者产生偏差。
因此，在本公开的具体实施例子中，将当前的激光数据帧在存储地图中对应的位姿作为观测位姿，与运动位姿产生的偏差作为约束，构建残差方程，对全局地图进行spa优化，修正误差，生成优化后的地图。
在开环优化状态下，由于绝对定位信息是不连续的，当没有检测到外部的绝对位置信息时，按照上述建图过程，进行前端地图和运动轨迹的创建，后端进行全局地图闭环优化。
当接收到外部绝对位置信息时，首先进行判断是不是第一次接收到该类信息，如果是，则将机器人自身坐标系转换到外部绝对位置的坐标系中；如果不是则省略该步骤。之后将机器人通过接收到的绝对位姿作为观测位姿，与前端生成地图时计算的运动位姿进行加权约束，构建残差方程，进行全局地图的spa优化，修正误差。即只要接收到绝对位姿，则构建当前运动位姿与绝对观测位姿之间的约束；当没有收到绝对观测位姿的时候，进行正常的闭环约束的创建。开环优化过程是发生在前端建图过程中的，且进行开环优化时，不再进行后端的闭环优化。
在具体实施例中，设scan扫描点的位姿信息由ξ＝(ξx,ξy,ξθ)表示，包括ξx,ξy平移分量和ξθ旋转分量。在局部扫描中，每一帧scan扫描点只和地图的一部分进行匹配。匹配方式通过非线性最小二乘优化方法计算scan扫描点与submap子地图的对齐关系。
首先，scan扫描点数据通过坐标转换公式tξ表示。每一个scan扫描点数据在scan坐标系中表示为
p表示每一个scan扫描点在scan坐标系中的坐标，tξp即表示将scan坐标系转化为submap坐标系中的坐标。每一帧scan中的所有扫描点通过同一套ξ参数进行转换，多个相邻的scan不断迭代，形成submap。
在将scan插入到submap之前，通过ceres-scanmatch扫描匹配方式寻找使scan扫描点在submap子地图中概率最大的一个位置，即寻找最优解的过程。
ceres求解归为求解一个非线性最小二乘问题：
其中，hk表示在scan坐标系下的scanpoint，tξhk表示转换到submap坐标系中的点的位置。msmooth表示经过双三次插值后的网格概率值m的平滑版本。k为一帧激光数据中扫描点的个数。
公式本质为：对scan扫描点中的每一个点通过同一个转换参数转换到submap中，计算每个scanpoint经过双三次插值后对应的msmooth上的概率，即寻找一个位姿pose使得∑msmooth最大。
submap的形式实质上是概率网格的形式存储的。一系列相近的scan形成了submap概率网格
即每一个网格点的概率都是由周围最邻近的点的概率组成。当一针scan扫描点被插入到概率网格中的时候，hit表示scan中的点转换到submap中的pose位姿中最接近的一个grid被占用一次。miss表示scan中的点转换到submap中的pose位姿到scan扫描数据原点之间连线上的grid没被占用。
若grid未被分配过(hit或miss)，则p＝0.5。若grid已被观测过，则更新
公式如下：
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(p)))(4)。
在一实施例子中，如果没有检测到绝对位置信息，则进行常规闭环约束：
在又一实施例子中，当机器人运动到之前走过的位置时，将当前激光扫描数据帧与存储的地图通过分支定界算法快速找到相匹配的位置，即寻找一个最佳位置ξ使在该处的一帧scan扫描数据中的所有scanpoint扫描点所在的概率网格的概率值之和最大。
通过该算法后即可找到一个最佳匹配位置，即观测位置。如果在理想的、没有误差的条件下，该匹配位置与当前的运动位姿应该一致。由于偏差的存在，使得观测位姿与运动位姿不一致，根据约束构建残差方程。
残差e的计算为
最终归结为一个非线性最小二乘问题：
其中，
通过求解该非线性最小二乘问题，即可使全局误差在该约束下达到最小，优化全局地图。
当传感器第一次接收到外部绝对位置信息ξabsolute＝(ξx,ξy,ξθ)时，首先根据此绝对位置信息进行坐标变换，即将存储的地图坐标系变换到绝对位置坐标系中。通过坐标变换矩阵tξabsolute，
即可实现坐标变换。p表示每一个scan扫描点在scan坐标系中的坐标，tξp即表示将scan坐标系转化为世界坐标系中的坐标
当接收到第二帧绝对位姿信息后，便不再进行坐标变换，而是将此观测的绝对位姿作为观测位姿，与运动位姿进行对比。理想情况下，两者应该相同，但是由于误差的存在，使得运动位姿产生累积误差。因此根据运动位姿和观测位姿的约束，对两者进行加权，构建残差方程。其中由于绝对位置信息的可信度非常高，因此在构建残差方程时需加大观测位姿的权重。
同样归结为一个最小二乘问题：
其中，由于绝对位姿数据不是连续的，存在观测不到绝对位姿信息的时候，因此i，j只表示能观测到绝对位姿信息的某一段局部地图参数，即i，j是从某一段路径中观测到绝对位姿时开始，到绝对位姿中断时结束。
通过求解该非线性最小二乘问题，可将外部绝对位姿信息融合到构建的局部地图中，即通过开环优化建立了激光扫描数据scan与外部绝对位姿信息之间的约束。此方法可修复slam前端创建局部地图的过程中产生的误差。
需要说明的是，关于外部绝对位姿信息的说明：
获取外部绝对定位信息的传感器有很多，如室外有gps，室内有uwb或带有绝对位置信息的二维码等，本公开实施例子以二维码为例，通过摄像头获取二维码对应的绝对位置。
在室内条件下，可通过uwb或二维码等方式提供绝对位姿信息。以二维码为例，在运行开环优化时需事先在室内确定全局绝对坐标系，根据此坐标系进行二维码的布置，每个二维码包含其所在位置的位姿信息。且二维码需按一定密度分布，在特征较少的环境下密度高，在特征丰富的环境下密度低。当采用摄像头观测到二维码时，根据二维码包含的位姿信息和机器人与二维码之间的相对位姿，即可求出机器人在二维码坐标系下对应的绝对位姿。
在室外条件下，可以使用如gps等具有绝对位姿信息的传感器。由于gps信号在建筑物较多的场所会有接收不到的时候，采用开环优化可以充分利用接收到的绝对位姿信息。
该方法不仅适用于单机器人大场景下的建图，也适用于多机器人协同建图。基于以上叙述的机器人开环优化建图方法，在多机器人协同建图的场景下，每个机器人从全局场景下的不同稀疏位置开始进行随机建图，通过绝对定位信息进行开环优化，优化建图误差，且将局部子地图转化为全局地图坐标。将经过开环优化的局部地图上传至同一主机，直接将各个机器人建的局部地图进行拼接即可得到全局地图信息。
实施例子二
本说明书实施方式提供一种基于slam建图的开环优化系统，通过以下技术方案实现：
局部环境地图的构建单元，被配置为：获取所处的局部环境地图信息，并基于该局部环境地图信息进行建图；
全局地图的构建单元，被配置为：在接收到外部绝对定位信息时，建立绝对定位信息与当前运动位置信息之间的约束，根据约束信息进行全局地图的构建。
上述系统可基于服务器实现，在服务器中配置为包括局部环境地图的构建单元及全局地图的构建单元，上述单元在具体实施例子中的技术实现可参见实施例子一中的具体算法执行过程。
应当注意，尽管在上文的详细描述中提及了设备的若干模块或子模块，但是这种划分仅仅是示例性而非强制性的。实际上，根据本公开的实施例，上文描述的两个或更多模块的特征和功能可以在一个模块中具体化。反之，上文描述的一个模块的特征和功能可以进一步划分为由多个模块来具体化。
实施例子三
本说明书实施方式提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现一种基于slam建图的开环优化方法步骤。
本说明书实施方式还提供一种计算机可读存储介质，其上存储有计算机程序(指令)，其特征在于，该程序(指令)被处理器执行时实现一种基于slam建图的开环优化方法步骤。
在本实施例中，计算机程序产品可以包括计算机可读存储介质，其上载有用于执行本公开的各个方面的计算机可读程序指令。计算机可读存储介质可以是可以保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是――但不限于――电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意合适的组合。
实施例四
本说明书实施方式提供一种机器人，所述机器人包括机器人本体，所述机器人本体上搭载有激光雷达传感器及获取外部绝对定位信息的传感器；机器人本体可为普通的机器人的机械设备。
激光雷达传感器检测的数据及外部绝对定位信息均传输
激光雷达检测到的局部环境地图信息和绝对位置信息，观测传感器接收的绝对位置信息加入时间戳信息后发送到至运算处理单元。
由于绝对定位信息不一定是连续的，当接收不到绝对定位信息时，运算处理单元只接收激光的局部环境地图信息进行前端地图构建，后端闭环约束。
当接收到绝对位置信息时，立即建立绝对位置信息与当前运动位置信息之间的约束，根据约束信息进行全局地图的构建。
具体的硬件框图可参见附图1所示，软件架构参见附图2所示，在附图1中，运算处理单元为arm处理器，该处理器通过网络与用户终端相连，用户终端可通过用户界面实现对机器人的操作控制，该处理器还与传感器相连，例如为imu、激光雷达、gps、uwb及相机等，传感器作为基本的数据采集单元进行数据的采集并传输至运算处理单元，运算处理单元还通过接口层与嵌入式处理单元通信，利用嵌入式处理单元通过电机驱动实现对电机的驱动，该嵌入式处理单元还与安全传感器及开关量传感器相连，接收相应的数据。
在软件的实现上，参见附图2所示，移动平台底层的主程序控制着上层决策程序，手柄节点、激光数据、绝对位姿数据为上层决策程序提供数据支撑，编码器数据为移动平台底层的主程序提供数据支撑，上层决策程序根据输入的数据输出相应的地图信息。
在该实施例子中，上述机器人只是作为移动平台的表现形式，在具体实现时，可以为别的移动平台的形式，例如为无人车等。
可以理解的是，在本说明书的描述中，参考术语“一实施例”、“另一实施例”、“其他实施例”、或“第一实施例～第n实施例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本公开的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
以上所述仅为本公开的优选实施例而已，并不用于限制本公开，对于本领域的技术人员来说，本公开可以有各种更改和变化。凡在本公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！