CN109682373B - 一种无人平台的感知系统 - Google Patents

Abstract

本发明公开了一种无人平台的感知系统，包括：激光传感器，其获取激光数据并探测障碍物；激光算法模块，其包括第一前端位置估计模块和第一后端位置估计模块；视觉传感器，其获取图像数据；视觉算法模块，包括第二前端位置估计模块和第二后端位置估计模块；融合定位模块，其获取无人平台在室内环境中的位置和速度；卫星定位模块，其获取无人平台在室外环境中的位置和速度；室内外切换导航算法模块，其切换选择不同的定位方法，并将定位的无人平台的实时位置、速度发送给无人平台；环境地图构建算法模块，其建立地图，利用地图提取障碍物信息，并将障碍物信息发送给无人平台。本发明的有益效果：实现在室内外环境及室内外环境切换过程中的自主定位。

Description

一种无人平台的感知系统

技术领域

本发明涉及无人设备技术领域，具体而言，涉及一种无人平台的感知系统。

背景技术

目前，尚没有一种单一的定位技术能够满足人们对室内外全方位服务的要求。虽然在室外开阔区域，全球卫星导航系统已经足以提供成熟的定位服务，但是在室外遮挡环境下，单纯依靠全球卫星导航系统依然存在诸多的限制和不足。近年来，室内定位得到了飞速发展，甚至部分定位方案已经开始了商业化应用。但室内定位是独立发展起来的，与室外全球卫星导航系统存在先天的缝隙，在此情况下实现室外遮挡环境下定位、室内环境下定位、室内外交互区域定位和定位系统之间的切换问题就成为了室内外定位技术研究中的焦点。未来定位技术的发展趋势之一就是将室外导航定位和室内定位技术相结合，即可以实现室外环境下的定位，又可以实现室内环境下的位置服务，同时将两种技术综合使用以实现室内外过渡阶段的精确定位。而在无人车或无人机技术领域，多半采用的是全球卫星导航系统，只能实现在室外开阔区域的导航及路径规划，完成室外自主导航，进而完成一些简单的飞行任务。亦或是只能实现室内环境的单独定位，而在室内外环境的切换过程中，无法实现实时导航的切换衔接，实现任意环境中的导航定位。

发明内容

为解决上述问题，本发明的目的在于提供一种无人平台的感知系统，实现在室内环境和室外环境的自主定位，并能在室内外环境的切换中自主定位，实现复杂环境下的无缝定位。

本发明提供了一种无人平台的感知系统，包括：

激光传感器，其发射激光获取激光数据，并探测障碍物；

激光算法模块，其包括第一前端位置估计模块和第一后端位置估计模块，所述第一前端位置估计模块获取所述激光传感器的扫描帧数据，并对各个激光扫描帧之间的关联性建立实时的子图，利用扫描匹配的最佳估计位置处插入到子图中，在生成一个子图后，进行一次局部的回环，利用分支定位和预先计算的网格，所有子图完成后，进行全局的回环，实现对激光传感器的实时位置估计，再利用所述激光传感器和无人平台之间的坐标变换关系，实现对无人平台在室内环境中位置的实时估计；所述第一后端位置估计模块根据全局的位置约束关系，对所述第一前端位置估计模块的子图进行优化；

视觉传感器，其拍摄连续图像获取图像数据，进行环境感知；

视觉算法模块，其包括第二前端位置估计模块和第二后端位置估计模块，所述第二前端位置估计模块根据视觉传感器的图像数据获取图像序列，从图像序列中提取图像特征，利用相邻帧图像进行特征初始匹配后跟踪图像帧并剔除异常匹配点，得到位置和姿态信息，实现无人平台在室内环境中的运动估计，并对运动估计信息进行滤波优化；所述第二后端位置估计模块筛选满足条件的关键图像帧进行三维重建恢复其特征点的深度信息，并将重建后的图像以及运动估计信息进行优化和建图，获取最终优化后的位姿；

融合定位模块，其融合所述激光算法模块和所述视觉算法模块的定位数据，获取无人平台在室内环境中的位置和速度；

卫星定位模块，其获取无人平台在室外环境中的位置和速度；

室内外切换导航算法模块，其根据所述融合定位模块和卫星定位模块的状态，切换选择不同的定位方法，并将定位的无人平台的实时位置、速度发送给无人平台，当所述视觉算法模块定位检测到多次闭环时切换选择所述融合定位模块进行导航定位，当所述卫星定位模块有信号时进行切换选择所述卫星定位模块进行导航定位；

环境地图构建算法模块，其通过所述激光传感器或所述视觉传感器建立稠密或者半稠密地图，利用地图提取出障碍物信息，并将障碍物信息发送给无人平台。

作为本发明进一步的改进，所述视觉传感器为单目传感器，拍摄的图像序列为：I_0:n{I₀,...,I_n}，假设视觉传感器坐标系和无人平台坐标系重合，对于一个时间连续的相邻时刻，k-1和k时刻视觉传感器的位置用以下传递矩阵表示：

其中，T_k,k-1∈R^4×4，R_k,k-1∈SO(3)为旋转矩阵，t_k,k-1∈R^3×1为平移向量，T_1,n＝{T_1,0,...,T_n,n-1}中包含了所有的子运动；

假设视觉传感器的初始位置为C₀，则通过T_1,n可以获得从视觉传感器初始位置开始即k＝0到最后第n时刻即k＝n的整个运动轨迹C_0,n＝{C₀,...,C_n}，C_n和C_n-1的关系为：

C_n＝C_n-1T_n,n-1；

通过I_k和I_k-1时刻的图像计算出T_k,k-1，进而恢复视觉传感器的整个运动轨迹C_0,n。

作为本发明进一步的改进，获取T_k,k-1的方法有两种：

方法一，使用两个图像中的所有像素的信息；

方法二，使用在图像中提取的鲁棒性的特征点。

作为本发明进一步的改进，所述第二前端位置估计模块所提取的图像特征为图像中的显著点，即与其邻域的其他点在亮度、颜色、纹理上有显著不同的点，获取显著点与显著点间的关系有两种方法：

方法一，提取前一帧图像的显著点，通过特征跟踪的方法获取后一帧图像中对应的显著点，获得显著点对；

方法二，分别提取前后图像的显著点，利用特征点描述通过匹配的方式进行匹配，获取显著点对。

作为本发明进一步的改进，关键图像帧的选择方法采用基于视觉内容选择法和基于时间域选择法的结合方法，具体为：设置一个采样时间，在这个采样时间内利用场景的外观变化，将相似性超过一定阈值的连续图像被认为是对同一场景的成像，删除不是同一场景成像的图像数据。

作为本发明进一步的改进，所述融合定位模块采用视觉惯性紧耦合的方法，对于两帧图像i和i+1，首先，假设在第i帧图像上观察到了一个3D图像点，则利用预计分项和3D图像点在i帧图像投影的像素坐标，预测出在第i+1帧图像上的像素坐标，该3D图像点在i+1图像上投影点应该在预测点左右，接着，在完成特征匹配后，通过3D-2D或者2D-2D方法得到i+1帧运动状态的初始估计，最后，在完成i+1帧视觉传感器运动的初始估计之后，利用预测值和初始估计值差值，进行最大后验优化估计。

作为本发明进一步的改进，所述第二后端位置估计模块在优化时，将视觉传感器位置和特征点表示为节点，将位置C_i和特征点C_j的约束关系表示为边

利用约束关系对全局的位置进行优化，在高斯分布假设的基础上，构造误差的目标函数，使用梯度下降法进行迭代求解。

作为本发明进一步的改进，所述卫星定位模块包括GPS和IMU，IMU计算出无人机的位置、速度和姿态，将所得结果与GPS解算获得的位置、速度相组合，进行组合导航滤波，然后利用滤波结果闭环校正IMU输出的姿态导航参数，从而完成导航定位输出，同时对IMU的误差进行反馈校正，以进行下一次导航滤波。

作为本发明进一步的改进，所述视觉传感器采用IDS-ueyeUSB单目摄像头，其图像输出速率为80帧/秒，供电电压为5V，接口类型为USB，重量为30g，功耗为5V/30mA。

作为本发明进一步的改进，所述激光传感器采用UTM-30LX的2D激光雷达以实现对障碍物和门的识别与检查，该激光雷达的测量范围为30m，270°，输入电压为DC 12V，扫描时间为25msec/scan。

本发明的有益效果为：

实现在室内环境和室外环境的自主定位，并能在室内外环境的切换过程中自主定位，实现复杂环境下的无缝定位；

定位结果更为精确，为无人平台的运动控制过程提供了更为精确的数据。

附图说明

图1为本发明实施例所述的第二前端位置估计模块的实现框图；

图2为本发明实施例所述的第二后端位置估计模块的优化示意图。

具体实施方式

下面通过具体的实施例并结合附图对本发明做进一步的详细描述。

本发明实施例的一种无人平台的感知系统，应用于无人机或无人车等设备上，包括：激光传感器、激光算法模块、视觉传感器、视觉算法模块、融合定位模块、卫星定位模块、室内外切换导航算法模块和环境地图构建算法模块。

激光传感器发射激光获取激光数据，并探测障碍物。激光传感器一个很大的优势是可以不需要丰富的周围环境信息。利用各个激光扫描帧(scan)之间的关联性建立实时的子图(submap)，实现对激光传感器的实时位置估计；利用激光传感器和小车之间的坐标变换关系，实现对小车位置的实时估计。

优选的，激光传感器采用UTM-30LX的2D激光雷达以实现对障碍物和门的识别与检查，该激光雷达的测量范围为30m，270°，输入电压为DC 12V，扫描时间为25msec/scan。

激光算法模块用于对激光传感器获取的数据进行处理，包括第一前端位置估计模块和第一后端位置估计模块。第一前端位置估计模块获取激光传感器的扫描帧数据，并对各个激光扫描帧之间的关联性建立实时的子图，利用扫描匹配的最佳估计位置处插入到子图中，在生成一个子图后，进行一次局部的回环，利用分支定位和预先计算的网格，所有子图完成后，进行全局的回环，实现对激光传感器的实时位置估计，再利用激光传感器和无人平台之间的坐标变换关系，实现对无人平台在室内环境中位置的实时估计；第一后端位置估计模块根据全局的位置约束关系，对第一前端位置估计模块的子图进行优化。

用于闭环检测的基本单元是子图。一个子图是由一定数量的激光扫描帧构成。将一个激光扫描帧插入其对应的子图时，会基于子图已有的激光扫描帧及其它传感器数据估计其在该子图中的最佳位置。即当一个新的激光扫描帧加入到地图中时，如果该激光扫描帧的估计位置与地图中某个子图的某个激光扫描帧的位置比较接近的话，那么通过某种扫描匹配策略就会找到该闭环。通过在新加入地图的激光扫描帧的估计位置附近取一个窗口，进而在该窗口内寻找该激光扫描帧的一个可能的匹配，如果找到了一个足够好的匹配，则会将该匹配的闭环约束加入到位置优化问题中。子图创建在短时间内的误差累积被认为是足够小的。然而随着时间推移，当越来越多的子图被创建后，子图间的误差累积则会越来越大。因此需要通过闭环检测适当的优化这些子图的位置进而消除这些累积误差，这就将问题转化成一个位姿优化问题。当一个子图的构建完成时，也就是不会再有新的激光扫描帧插入到该子图时，该子图就会加入到闭环检测中。闭环检测会考虑所有的已完成创建的子图。

由激光传感器获取的数据得到的平移和旋转的信息，被称为扫描帧(scan)。在这里每个连续的scan和局部环境的匹配称为子图M，使用非线性优化方法，由子图来校准scan的过程进一步称为扫描匹配。在激光算法模块中，对于地图的表示采用的是栅格地图，地图上存储的是这个点是障碍物的概率，概率越大表示是障碍物可能性越大。那么在匹配的时候其实就是找到在网格中的概率最大的位姿得到的scan。子图的构造是一个重复迭代配准scan和子图的过程。利用配准估算出位姿对scan进行刚体变换，再插入到子图中。连续的scan用来构造子图，子图以概率格网的形式表现。对每一个scan，在插入子图时，每一个网格点有击中和未击中两种情况。离scan终点最近的网格点为击中，在scan原点和终点之间相交的网格点为未击中。

M_new(x)＝clamp(odds^-1(odds(M_old(x))·odds(p_hit)))

Odds表示占有率，clamp约束上下限，相邻网格点之间的距离即分辨率5cm。即每当将一个scan插入到概率网格中，会计算出一组击中和一个未击中的网格点集合。每一个以前未观察到的网格点如果在这些集合里面，会被指定一个概率。如果网格点已经被观察到，则更新其击中和未击中的概率。

把求位置的问题转换为一个求解非线性最小二乘问题，利用Ceres非线性优化库函数解决这个问题。局部匹配也是看成了一个非线性优化问题。优化的目标函数如下，M_smooth函数是一个双三次插值函数。

根据扫描的姿态，这里T_ε把h_k从扫描帧转换到子图帧。函数M_smooth是双三次插值进行局部子图概率值的平滑处理。这是二维空间中最常用的插值方法。在这种方法中，函数M_smooth在点(x,y)的值可以通过矩形网格中最近的十六个采样点的加权平均得到，在这里需要使用两个多项式插值三次函数，每个方向使用一个。

因为之前构建的栅格地图(submaps)存储的是概率值，所以通过双三次插值可以得到(x，y)这个scan点的概率值。使得scan在栅格地图中的概率值最大，那么就需要使得此目标函数最小，构建好这个优化目标函数之后，就可以采用ceres库来进行优化。

对于这种局部最优问题，一个好的初值(pose初值)对求解有很大影响。因此IMU能被用来提供pose初值的旋转变量。在缺乏IMU的时候，可以用提高扫描匹配频率或匹配精度来实现相似效果。

第一后端位置估计模块采用2分级的图结构，整个地图分为多个子图，而每个子图表示为占用格结构，新Scan只在子图内处理以保证快速和稳定的处理时间，另一方面，在检测到回环时通过对包括所有子图姿势的全图进行优化求解以消除Scan和子图匹配所引入的累计误差问题，这里由于不是对所有Scan进行处理，而是以子图为单位进行，使得整个的优化求解时间极大的缩小。回环相关的性能优化，比较直接的做法通过给予当前位置的估计值进行相邻范围内的回环检测，这种方法和当前位置的估计精度相关，或是通过缩小地图进行粗糙地匹配，此处采用分支界限+深度优先搜索(DFS)搜索的策略以进行回环的检测和相对位姿求解，相比传统的先检测回环，在求解相对位姿来说，显得结构上更为统一，将建立回环的过程转化为一个查找过程，而另一方面，当完成对离散候选解空间的树形构造后，在树中对解的查找过程速度很快，剩下的对于建树过程中的节点的界限的选择通过对子图的预计算来完成，这种中间结构的引入使得回环过程可以实时的完成，从而通过回环不断调整子图以消除累计误差。因此对于闭环检测部分的匹配：引入了分枝定界的方式，加快了闭环的查找。依据多分辨率多层的树型结构，单枝生长的方式，及时剪枝操作，深度优先搜索确定闭环。在子图中添加相应的闭环约束，构建优化问题，然后利用ceres进行优化处理。

视觉传感器拍摄连续图像获取图像数据，进行环境感知。视觉传感器一个很大的优势是可以提供丰富的周围环境信息。利用运动图像中各个图像帧之间的关联性和视觉传感器模型，实现对视觉传感器的姿态估计；利用视觉传感器和无人平台之间的坐标变换关系，实现对无人平台的实时估计。

优选的，视觉传感器采用IDS-ueyeUSB单目摄像头，其图像输出速率为80帧/秒，供电电压为5V，接口类型为USB，重量为30g，功耗为5V/30mA。

视觉算法模块包括第二前端位置估计模块和第二后端位置估计模块，如图1所示，第二前端位置估计模块根据视觉传感器的图像数据获取图像序列，从图像序列中提取图像特征，利用相邻帧图像进行特征初始匹配后跟踪图像帧并剔除异常匹配点，得到位置和姿态信息，实现无人平台在室内环境中的运动估计，并对运动估计信息进行滤波优化；第二后端位置估计模块筛选满足条件的关键图像帧进行三维重建恢复其特征点的深度信息，并将重建后的图像以及运动估计信息进行优化和建图，获取最终优化后的位置。

视觉传感器为单目传感器，拍摄的图像序列为：I_0:n{I₀,...,I_n}，假设视觉传感器坐标系和无人平台坐标系重合，对于一个时间连续的相邻时刻，k-1和k时刻视觉传感器的位置用以下传递矩阵表示：

其中，T_k,k-1∈R^4×4，R_k,k-1∈SO(3)为旋转矩阵，t_k,k-1∈R^3×1为平移向量，T_1,n＝{T_1,0,...,T_n,n-1}中包含了所有的子运动；

假设视觉传感器的初始位置为C₀，则通过T_1,n可以获得从视觉传感器初始位置开始即k＝0到最后第n时刻即k＝n的整个运动轨迹C_0,n＝{C₀,...,C_n}，C_n和C_n-1的关系为：

C_n＝C_n-1T_n,n-1；

通过I_k和I_k-1时刻的图像计算出T_k,k-1，进而恢复视觉传感器的整个运动轨迹C_0,n。

因此，第二前端位置估计模块是一个姿态一个姿态地逐步积累恢复运动路径的。获取T_k,k-1的方法有两种：

方法一，基于全局的方法：使用两个图像中的所有像素的信息；

方法二，基于特征的方法：使用在图像中提取的鲁棒性的特征点。

这里的特征为图像中的显著点，即与其邻域的其他点在亮度、颜色、纹理上有显著不同的点，获取显著点与显著点间的关系有两种方法：

方法一，提取前一帧图像的显著点，通过特征跟踪的方法获取后一帧图像中对应的显著点，获得显著点对；

方法二，分别提取前后图像的显著点，利用特征点描述通过匹配的方式进行匹配，获取显著点对。

当连续两帧图像的视点和运动变化小时，第一种方法更适合，当有较大的运动或图像视点改变时，后一种方法更适合。对第二前端位置估计模块而言，角点或者图像块都是很好的显著点。角点是直线的交点，图像块不同于角点和边缘直线。两种显著点各有优缺点。基于角点的运算快速性好但速特性差，在图像中的位置和尺度更好估计，图像块则相反，所以本发明采用基于特征点的方法，在提取ORB特征点的基础上进行特征匹配，计算图像帧之前的相对位置变换。

如果把前端获取的图像以及相应的特征点信息在后端优化和建图中都考虑进去，会造成算法计算量很大、实时性很难保证，同时会造成建立的地图模型随着算法运行时间增加而规模越来越大，使算法计算计算量随着迭代次数增加而增加。而事实上前端获取的图像，不同帧之间存在很大的信息冗余，因此采取一定策略对前端的图像帧进行筛选，将满足筛选条件的图像帧进行三维重建恢复其特征点的深度信息，并将重建后的图像以及其运动估计信息传送到后端进行优化和建图。

关键帧的选择方法大致可以分为三类：基于时间域选择法、基于空间域选择法和基于视觉内容选择法。基于时间域选择法通过设置一个采样时间来选取关键帧；基于空间域的方法通过设置一定的距离阈值，超过阈值便设置一个关键帧；基于视觉内容的采样方法直接利用了场景的外观变化，相似性超过一定阈值的连续图像被认为是对同一场景的成像，信息量小，从原始图像数据中删除不会造成太大的系统计算误差。本发明采取视觉内容变化为主，时间域选择法为辅的关键帧选择策略，主要思想就是选择的关键帧对于场景变化具有代表性，两个连续的关键帧之间的场景要有所区别，同时又要有所联系。具体为：设置一个采样时间，在这个采样时间内利用场景的外观变化，将相似性超过一定阈值的连续图像被认为是对同一场景的成像，删除不是同一场景成像的图像数据。

第二后端位置估计模块在优化时，如图2所示，将视觉传感器位姿和特征点表示为节点，将位置C_i和特征点C_j的约束关系表示为边P_i ^j，利用约束关系对全局的位姿进行优化，在高斯分布假设的基础上，构造误差的目标函数，使用梯度下降法进行迭代求解。常用的迭代方法有高斯-牛顿法、Leverburg-Marquatdt法等。

融合定位模块用于融合激光算法模块和视觉算法模块的定位数据，获取无人平台在室内环境中的位置和速度。将不同传感器采集的数据进行融合，以给出关于状态的最优估计量。由此可见，多传感器数据融合的最终目的和结果，是以最优估计方法进行系统的状态向量估计。一般而言，多传感器融合系统具有以下优点：提高系统的可靠性和鲁棒性；扩展时间上和空间上的观测范围；增强数据的可信任度；增强系统的分辨能力。

估计问题一般分为三类：从当前和过去的观测值来估计当前状态，称为滤波；从过去的观测值来估计当前或将来状态，称为预测或外推；从过去的观测值来估计过去的状态，称为平滑或内插。用于导航的数据融合技术可采用“滤波”方法求解。目前常用的多传感器信息融合方法主要有：经典推理法、卡尔曼滤波法、贝叶斯估计法、Dempster-Shafer证据推理法、聚类分析法和专家系统法等。卡尔曼滤波(Kalman Filter，KF)是常用于实现数据融合技术的算法之一，它对动态系统的状态序列进行线性最小方差误差估计。卡尔曼滤波把被估计的随机变量作为系统的状态，利用系统状态方程来描述状态的转移过程。由于采用了状态转移矩阵来描述实际的动态系统，其适用面大为扩展；卡尔曼滤波的估计值利用了以前以及当前时刻的观测值，这种递推利用了所有的观测数据，但每次运算只要求得到前一时刻的估计值以及当前的观测值，而不必存储历史数据，降低了对计算机的存储要求，并且这种算法在满足系统为线性、噪声高斯分布、后验概率高斯型的条件下是最优算法。当考虑到非线性系统时，可以利用扩展卡尔曼滤波器(ExtendedKalmanFilter，EKF)求解，通过保留系统状态方程泰勒展开的一阶项来近似表示非线性系统。但EKF忽略高阶项常导致在状态的后验分布的估计上产生较大误差，影响滤波算法的性能。相比之下，多状态约束卡尔曼滤波(Multi-State Constraint Kalman Filter)的计算复杂度和空间特征点的数量呈线性关系，从时间复杂度上来说比EKF-SLAM更加高效。同时，由于没有对空间特征点的坐标值的概率分布进行任何假设，因而可以获得比EKF-SLAM更高的精度。

本发明融合定位模块采用视觉惯性紧耦合的方法，对于两帧图像i和i+1，首先，假设在第i帧图像上观察到了一个3D图像点，则利用预计分项和3D图像点在i帧图像投影的像素坐标，预测出在第i+1帧图像上的像素坐标，该3D图像点在i+1图像上投影点应该在预测点左右，接着，在完成特征匹配后，通过3D-2D或者2D-2D方法得到i+1帧运动状态的初始估计，最后，在完成i+1帧视觉传感器运动的初始估计之后，利用预测值和初始估计值差值，进行最大后验优化估计。也就是说采用了融合的方法进行了跟踪和估计，克服纯视觉方法在快速运动和特征较少环境下效果退化的情况。

卫星定位模块获取无人平台在室外环境中的位置和速度。在室外定位策略的选择上，GPS己经成为主要的选择对象。GPS会在城市遮挡环境中出现接收卫星数不足的问题。惯性导航系统的优势在于能够实现自主导航，劣势在于其定位误差会随时间的推移而逐渐累积，从而导致长时间定位结果可靠性不高。组合导航系统可充分发挥各导航装置的优点，使得整个组合定位系统兼具高精度与高容错性，又可在复杂环境状况下长时间的保证定位结果的精度与可靠性。本发明的卫星定位模块包括GPS和IMU，IMU和GPS松组合方式构成组合导航系统，根据位置速度组合导航方案，采用间接Kalman滤波及闭环校正方式，IMU输出运动载体三个方向的加速度及角速度，GPS可在差分条件下输出载体的三个方向的位置和速度。IMU计算出无人机的位置、速度和姿态，将所得结果与GPS解算获得的位置、速度相组合，进行组合导航滤波，然后利用滤波结果闭环校正IMU输出的姿态导航参数，从而完成导航定位输出，同时对IMU的误差进行反馈校正，以进行下一次导航滤波。

优选的，卫星定位模块采用Xsens公司生产的MTi-G-700作为IMU和GPS的数据源。MTi-G-700型号IMU能够提供高品质的方向和位置信息，具有振动抑制陀螺仪和高运行偏置不稳定性。通过增加了一个机载GPS接收器，能够利用GPS和IMU输出一个广泛范围的数据如无漂移3D定位数据和速度数据、3D校准加速度、转弯速率、磁场数据和压力数据。IMU增强的3D位置和3D速度弥补了GPS中断时的数据欠缺，相比GPS位置和速度具有更高的完整性。获得的位置更新速率可达400Hz。

室内外切换导航算法模块根据融合定位模块和卫星定位模块的状态，切换选择不同的定位方法，并将定位的无人平台的实时位置、速度发送给无人平台。室内外导航源的切换重点关注与室内与室外交接区域，通过一定的切换策略选择合适的系统进行定位，以达到“无缝”的目的。室外GPS卫星距离地面很远，用户接收到的信号强度比较稳定，所以可用接收到的卫星数目来判断是否能够定位。当无人机从室内走向室外进入到GPS信号存在的区域时，触发切换机制。如果搜索到的可见星的数量等于或大于三颗时，认为GPS满足定位条件，开始引发切换，同时开启计时器。GPS可见星数目作为接收信号强度门限的另一种表现形式。为减少乒乓效应的影响，在切换触发判决的条件中加入驻留时间及运动趋势等参数。本发明的室内外切换导航算法模块判断切换的条件为：当视觉算法模块定位检测到多次闭环时切换选择融合定位模块进行导航定位，当卫星定位模块有信号时进行切换选择卫星定位模块进行导航定位。

当从室外切换到室内时：事先规划一条无人机在GPS系统坐标系下的运动轨迹，引导无人机从室外向室内，当无人机进入室内且GPS信号不可使用后，无人机停在原处启动激光定位，待激光建图稳定后，在室内导航坐标系下运行一段轨迹后结束；当从室内切换到室外时：无人机在室内环境下启动激光定位，无人机在激光定位下朝着出口运动，出门后若激光定位不可使用，丢弃激光定位并继续朝外运动，等待GPS信号有效之后切换为GPS导航信号并在GPS导航下沿着规划的运动轨迹运动到终点；当从室外切换到室内再切换到室外：事先规划一条无人机在GPS系统坐标系下的运动轨迹，引导无人机从室外向室内，当无人机进入室内且GPS信号不可使用后，无人机停在原处启动视觉定位，并与激光定位对比完成自检，待视觉建图稳定后，在室内导航坐标系下运行一段轨迹后，无人机在视觉定位下朝着出口运动，出门后继续朝外运动，等待GPS信号有效之后切换为GPS导航信号并在GPS导航下沿着规划的运动轨迹运动。

环境地图构建算法模块通过激光传感器或视觉传感器建立稠密或者半稠密地图，利用地图提取出障碍物信息，并将障碍物信息发送给无人平台。提取出的障碍物信息，用于无人平台的避障。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种无人平台的感知系统，其特征在于，包括：

激光传感器，其发射激光获取激光数据，并探测障碍物；

激光算法模块，其包括第一前端位置估计模块和第一后端位置估计模块，所述第一前端位置估计模块获取所述激光传感器的扫描帧数据，并对各个激光扫描帧之间的关联性建立实时的子图，利用扫描匹配的最佳估计位置处插入到子图中，在生成一个子图后，进行一次局部的回环，利用分支定位和预先计算的网格，所有子图完成后，进行全局的回环，实现对激光传感器的实时位置估计，再利用所述激光传感器和无人平台之间的坐标变换关系，实现对无人平台在室内环境中位姿的实时估计；所述第一后端位置估计模块根据全局的位置约束关系，对所述第一前端位置估计模块的子图进行优化；

视觉传感器，其拍摄连续图像获取图像数据，进行环境感知；

视觉算法模块，其包括第二前端位置估计模块和第二后端位置估计模块，所述第二前端位置估计模块根据视觉传感器的图像数据获取图像序列，从图像序列中提取图像特征，利用相邻帧图像进行特征初始匹配后跟踪图像帧并剔除异常匹配点，得到位置和姿态也即位姿信息，实现无人平台在室内环境中的运动估计，并对运动估计信息进行滤波优化；所述第二后端位置估计模块筛选满足条件的关键图像帧进行三维重建恢复其特征点的深度信息，并将重建后的图像以及运动估计信息进行优化和建图，获取最终优化后的位姿，其中，关键图像帧的筛选方法为：设置一个采样时间，在这个采样时间内利用场景的外观变化，将相似性超过一定阈值的连续图像被认为是对同一场景的成像，删除不是同一场景成像的图像数据；

融合定位模块，其融合所述激光算法模块和所述视觉算法模块的定位数据，获取无人平台在室内环境中的位置和速度；采用视觉惯性紧耦合的方法，对于两帧图像i和i+1，首先，假设在第i帧图像上观察到一个3D图像点，则利用预计分项和3D图像点在i帧图像投影的像素坐标，预测出在第i+1帧图像上的像素坐标，该3D图像点在i+1图像上投影点在预测点左右，接着，在完成特征匹配后，通过3D-2D或者2D-2D方法得到i+1帧运动状态的初始估计，最后，在完成i+1帧视觉传感器运动的初始估计之后，利用预测值和初始估计值差值，进行最大后验优化估计；

卫星定位模块，其获取无人平台在室外环境中的位置和速度；

室内外切换导航算法模块，其根据所述融合定位模块和卫星定位模块的状态，切换选择不同的定位方法，并将定位的无人平台的实时位置、速度发送给无人平台，当所述视觉算法模块定位检测到多次闭环时切换选择所述融合定位模块进行导航定位，当所述卫星定位模块有信号时进行切换选择所述卫星定位模块进行导航定位；

环境地图构建算法模块，其通过所述激光传感器或所述视觉传感器建立稠密或者半稠密地图，利用地图提取出障碍物信息，并将障碍物信息发送给无人平台。

2.根据权利要求1所述的感知系统，其特征在于，所述视觉传感器为单目传感器，拍摄的图像序列为：I_0:n{I₀,...,I_n}，假设视觉传感器坐标系和无人平台坐标系重合，对于一个时间连续的相邻时刻，k-1和k时刻视觉传感器的位置用以下传递矩阵表示：

其中，T_k,k-1∈R^4×4，R_k,k-1∈SO(3)为旋转矩阵，t_k,k-1∈R^3×1为平移向量，T_1,n＝{T_1,0,...,T_n,n-1}中包含了所有的子运动；

假设视觉传感器的初始位置为C₀，则通过T_1,n获得从视觉传感器初始位置开始即k＝0到最后第n时刻即k＝n的整个运动轨迹C_0,n＝{C₀,...,C_n}，C_n和C_n-1的关系为：

C_n＝C_n-1T_n,n-1；

通过I_k和I_k-1时刻的图像计算出T_k,k-1，进而恢复视觉传感器的整个运动轨迹C_0,n。

3.根据权利要求2所述的感知系统，其特征在于，获取T_k,k-1的方法有两种：

方法一，使用两个图像中的所有像素的信息；

方法二，使用在图像中提取的鲁棒性的特征点。

4.根据权利要求1所述的感知系统，其特征在于，所述第二前端位置估计模块所提取的图像特征为图像中的显著点，即与其邻域的其他点在亮度、颜色、纹理上有显著不同的点，获取显著点与显著点间的关系有两种方法：

方法一，提取前一帧图像的显著点，通过特征跟踪的方法获取后一帧图像中对应的显著点，获得显著点对；

方法二，分别提取前后图像的显著点，利用特征点描述通过匹配的方式进行匹配，获取显著点对。

5.根据权利要求1所述的感知系统，其特征在于，所述第二后端位置估计模块在优化时，将视觉传感器位姿和特征点表示为节点，将位置C_i和特征点C_j的约束关系表示为边

利用约束关系对全局的位姿进行优化，在高斯分布假设的基础上，构造误差的目标函数，使用梯度下降法进行迭代求解。

6.根据权利要求1所述的感知系统，其特征在于，所述卫星定位模块包括GPS和IMU，IMU计算出无人机的位置、速度和姿态，将所得结果与GPS解算获得的位置、速度相组合，进行组合导航滤波，然后利用滤波结果闭环校正IMU输出的姿态导航参数，从而完成导航定位输出，同时对IMU的误差进行反馈校正，以进行下一次导航滤波。

7.根据权利要求1所述的感知系统，其特征在于，所述视觉传感器采用IDS-ueyeUSB单目摄像头，其图像输出速率为80帧/秒，供电电压为5V，接口类型为USB，重量为30g，功耗为5V/30mA。

8.根据权利要求1所述的感知系统，其特征在于，所述激光传感器采用UTM-30LX的2D激光雷达以实现对障碍物和门的识别与检查，该激光雷达的测量范围为30m，270°，输入电压为DC 12V，扫描时间为25msec/scan。

Images