改进的图优化SLAM方法技术

传统的激光三角定位算法的定位精度高，但是容易受到障碍物遮挡的影响导致AGV不能准确的定位；图优化SLAM算法以前端构图和后端优化进行构图的方法保证了构建地图的精度和准确性，以闭环检测的方法实现了构图的完整性。但是在车库这种特征较少、特征单一、特征高度对称的环境中，使得闭环检测的功能不能很好的实现，从而造成建立的地图上面有很多重叠的地方，这种地图是无法提供导航定位的。本文提出了将激光三角算法与SLAM算法相结合进行优化的方法，用事先布置在环境中的反光板，将大环境分成无数个小环境并给这些小环境进行独立编号。在小环境里面也可以用激光三角定位来进行定位，这样将环境分区域编号之后，就可以检测AGV到达的每个区域，然后不同的区域不同的编号最后就形成了图优化SLAM闭环检测的信号，这样建立的地图的精度就较高。

Improved SLAM method of graph optimization

High precision laser triangulation algorithm, but is easily affected by obstructions leads to localization of AGV is not an accurate figure; SLAM algorithm before end of composition and back-end optimization method to ensure the construction of the composition of the precision and accuracy of the map, to achieve a complete closed-loop detection method of composition. But in the environment with fewer features, single features and highly symmetrical features, the function of closed loop detection can not be well implemented, resulting in many overlapping maps on the map, which cannot provide navigation and location. In this paper, we propose a method of optimizing the combination of the laser triangulation algorithm and the SLAM algorithm. The environment is divided into numerous small environments by using the reflective panels arranged in advance, and the environment is numbered independently. In a small environment can also be used to locate the laser triangulation, then the environment is divided area number, each region can reach AGV detection, and different regions of different numbers and finally to form a signal optimization SLAM closed-loop detection, thus established the precision of map is higher.

【技术实现步骤摘要】
改进的图优化SLAM方法
本专利技术涉及改进图优化SLAM建图的方法，以及在智能车库中定位的应用，涉及到智能控制、传感器融合、机器人、导航和定位等

技术介绍
目前AGV的定位主要采用激光三角导航定位，磁钉导航定位，磁条导航定位，脉冲线导航定位，二维码定位，激光SLAM导航定位，视觉导航定位，混合定位等方法。采用激光三角定位算法需要在环境中预先布置反光板，且至少需要知道三个反光板的位置才能计算出AGV的位姿信息，虽然激光导航定位方法的定位精度很高，但是容易受到其它障碍物的遮挡，从而造成定位不准或者无法实现定位的功能。磁条定位的技术已经很成熟，磁钉的定位的定位精度高。但是磁条定位需要铺设磁条且后期还需要维护，这样使得成本增加；而磁钉定位需要在地面里埋设磁钉，对地面的破坏程度大，后期维护工作量过大。视觉SLAM目前还处于研发阶段，还没有应用到实际场景中，而且视觉SLAM定位需要大量的特征点。光照的强弱，特征点的多少都会对视觉SLAM产生严重的影响，造成定位不准或者无法定位的现象。但是视觉SLAM的优势是只需要摄像头，造价便宜。激光SLAM需要一个激光测距仪，对地面环境没有任何破坏。建图的时候周围环境的特征都通过相关的提取方法提取最后绘制到地图中，并且通过在线构图还可以实现实时构图和实时更新的功能。但是在车库这种高度对称、特征单一并且少的环境中，建立的SLAM地图可能会造成AGV错误定位或者无法正确定位AGV的位置。另外一方面，由于车库大环境的特点，使得在建图的过程中不能够实现闭环导致建立的地图上面出现很多重叠的地方以至于不能应用到AGV的导航中，但是采用本文的改进算法后，上面的问题都能够得到解决。混合定位一般是采用磁钉与激光三角定位或者磁条与激光三角定位等方法，在反光板被障碍物遮挡的情况下，AGV切换到用磁钉定位或者磁条定位。该种方法很好解决了反光板被遮挡之后无法定位的情况，但是在地面上铺设磁条或者注入磁钉不仅工作量大，而且维护起来较难，并且对整个的系统有较高的要求。
技术实现思路
本专利技术主要是针对上述
技术介绍
的不足，提出将激光图优化SLAM与激光三角算法相融合的方法，这样即实现了图优化SLAM的闭环检测功能使得建立的地图准确性和精度都教高。而且在导航定位的时遇到反光板被障碍物遮挡的情况，可以用建立的地图与当前激光观测到的数据进行匹配然后进行定位；在当SLAM由于车库对称环境造成定位不准的时候，可以先用里程计估计AGV所在区域，在小区域里面用三角定位算法计算出AGV的位姿；当SLAM和三角定位都同时不准的时候，此时可以用里程计的信息量来粗略估计AGV的当前的位姿，在AGV达到了能够用SLAM或者三角定位来计算位姿的时候，再将此时的位姿进行重新的估计。本专利技术为实现上述算法的功能将采用如下的技术方案。为实现上述算法我们方案需要用到下面的设备：四轮独立驱动全向驱动AGV，PC一台(需要安装Linux系统，4G内存，i7处理器，四核，硬盘大小为500G)，SICK151激光雷达(分辨率为0.5°，扫描频率50Hz)，遥控手柄或者键盘，若干个反光板，里程计。整个实验平台图如图1。四轮独立驱动全向AGV包括：车架、四个行走电机、四个转向电机、四个升降电机、四个行走驱动器、四个旋转驱动器、四个升降驱动器、动力锂电池、电池充电器、减速机、接近开关、紧急开关、陀螺仪、激光雷达、超声波传感器。三角定位算法在环境周围按照一定规则布置一定数量的反光板或者反光柱，并且这些反光板的坐标是预先存储在计算机里面的，其原理图如图2所示。当AGV在环境中运行时，通过不断接受三个或者3个以上已知位置反光板反射回来的信息，就可以确定AGV的位置。其中的原理图如图3所示。假设三个反光板的位置分别是(x1,y1),(x2,y2),(x3,y3)测得反观板与激光之间的夹角为θ1，θ2，θ3，令A＝θ1-θ2，B＝θ1-θ3，圆O12表示经过小车V，R1和R2的圆，O13表示经过小车V，R1和R3的圆，则圆心坐标O12和O13可以表示为：(x12,y12)＝(x1+P1,y1+Q1)(1)(x13,y13)＝(x1+P2,y2+Q2)(2)在式(1)和式(2)中有：反观板的坐标已知，则可以求得L12，L13的值，E1表示的是L12与x轴之间的夹角，E3表示的是L12与L13之间的夹角。由方程式(1)和(2)我们可以将O12和O13表示如下：通过对解方程式(5)和方程式(6)，我们可以将小车的位姿计算出来：(xv,yv)＝[x1+k(Q2-Q1),y1-k(P2-P1)](7)在式(7)中有：改进的图优化算法与三角定位算法的融合：图优化SLAM的理论是将机器人的位姿及其观测数据表示成图结构的形式，然后不断去优化图的配置，最终的最优图配置即作为SLAM问题的解。SLAM问题的本质是利用传感器对机器人位姿和环境等状态进行估计，基于图优化方法的通用框架如图4所示，将一般的图优化框架分为前端和后端。前端负责构建图结构，后端负责优化图配置.图5所示表示了图的模型结构。ci表示摸个位姿节点，两个节点之间靠边来链接。在构建图的过程中，可以根据传感器的数据建立c1与c2、c2与c3等数据数据之间的关联，也可以建立闭环的两个节点(c1与cn)之间的约束。闭环检测是图优化SLAM中的重要一环，因为它能很大程度的改善构建地图的不一致性，而激光闭环检测在单调的场景中难以匹配到正确的闭环，特别是在车库这种对称性较高、特征相似的环境中闭环很可能不能正确匹配。如果将对称性高的环境划分为不同的区域，那么就可以用不同的区域来进行闭环检测。改进后的SLAM方法需要预先在环境中布置一些反观板，然后用三角定位算法将环境划分为很多个区域并将这些区域进行编号。在构建地图的时候在环境中加入了反观板并且当AGV的X或者Y方向发生5m的移动的时候，我们就标定一个区域，然后将整个环境划分为各个不同的区域，而且每个不同的区域里面的反光板的布置是不一样的，这样就可以克服不能正确匹配闭环的特点，在AGV利用地图定位不准的时候可以根据所在区域里面的反光板来粗略定位AGV的位置，然后再根据激光数据的匹配地图从而准确定位AGV的位置。改进后的图优化的SLAM的框架如图6所示。图优化前端建立：前端的建立主要是建立位姿几点之间的约束关系，这主要靠激光扫描匹配来完成，激光扫描匹配的问题主要是通过通解两帧激光数据之间的相对位姿变换，在扫描匹配中，为了求得机器人的位姿，可以考虑用概率学模型建模，应用贝叶斯概率模型可知。p(ci|ci-1,u,m,z)∝p(z|ci,m)p(ci|ci-1,u)(10)其中，ci表示的机器人当前的待求位姿，ci-1表示的是前一时刻机器人位姿，m表示的是前面若干帧激光数据构成的已知地图，z表示的是待匹配激光观测数据，u表示的是运动模型中控制输入量。式(10)中的概率模型被分解成了观测模型和运动模型的乘积。运动模型由控制输入或者里程计得到AGV位姿ci的先验概率，观测模型是求给定地图和AGV位姿的条件下求观测数据z的似然值。观测模型较为复杂，将观测模型简化为：式(11)中zj表示的是激光扫描仪中第j个方向测量值，对上式进行求对数可以得到：为了提高匹配速度，采用了Olson的方法，构建两本文档来自技高网...

【技术保护点】
改进的图优化SLAM方法，其特征在于，融合了激光三角算法和传统的图优化SLAM方法，主要步骤如下：1)根据环境的特点，在环境中布置一定的反光板；2)让AGV沿着建图场地行走，利用激光三角算法建立一个包括反光板特征、直线和拐角的特征图，并根据X和Y方向上的变化，将区域进行独立编号；3)检测建立的特征图中每个区域是否有相同，如果有，就需要将反光板进行调整，然后返回步骤2)直到保证大环境里面的每个小区域的特征地图都不一样，如果每个区域都是唯一的就进入步骤4)；4)打开ROS的节点管理器，打开AGV的驱动程序和返回AGV编码器数据的程序；5)打开激光的配置文件以及激光的驱动程序，确保激光的数据正确返回；6)打开遥控手柄或者键盘，并运行AGV上与之相对应的程序，确保能够遥控AGV遍历整个环境；7)运行改进后的图优化SLAM程序，遥控AGV对整个环境进行遍历并回到AGV的起始点；8)结束。

【技术特征摘要】
1.改进的图优化SLAM方法，其特征在于，融合了激光三角算法和传统的图优化SLAM方法，主要步骤如下：1)根据环境的特点，在环境中布置一定的反光板；2)让AGV沿着建图场地行走，利用激光三角算法建立一个包括反光板特征、直线和拐角的特征图，并根据X和Y方向上的变化，将区域进行独立编号；3)检测建立的特征图中每个区域是否有相同，如果有，就需要将反光板进行调整，然后返回步骤2)直到保证大环境里面的每...

【专利技术属性】
技术研发人员：王昕，吴钟，管大功，
申请(专利权)人：深圳精智机器有限公司，
类型：发明
国别省市：广东,44