智能电站巡检智能体的定位与多粒度环境感知技术的制作方法

本发明涉及智能电站巡检智能体的定位与环境感知技术，尤其是涉及一种多粒度智能体的定位与环境感知方法。

背景技术：

要实现智能电站智能体自主巡检，需要实现智能体的自主定位和对特定场景的物理环境进行感知。其关键技术是同步定位与建图(simultaneouslocalizationandmapping,简称slam).这种技术的核心是在无需任何先验知识的情况下实时、同步地对完全陌生的环境实现目标的定位与环境模型构建.由于光学传感器相对廉价且能够提供丰富的信息,也更容易模拟人类认知世界的过程,所以通常被作为slam的感知设备.在定位方面,以视觉里程计(visualodometry,简称vo)定位最为常见.同时,使用惯性测量单元(inertialmeasurementunit,简称imu)实时测量并更新相机的位姿信息并输出运动增量用以配合解决视觉里程计的漂移误差的方法,称之为视觉惯性导航(visualinternalodometry,简称vio)也逐步成为多传感器集成时代的首选方案.通过基于滤波器如扩展卡尔曼滤波器(extendkalmanfilter,简称ekf)方法或者基于关键帧的光束平差法(bundleadjustment,简称ba)姿态融合与优化,移动工具能够实时根据摄像机输入的图像帧来确定自身当前的位置和朝向以及移动的距离.以此数据作为依据,可以对导航提供有效的数据支撑.

传统环境模型多以单粒度模型为主,而单粒度模型自身存在各异构图像传感器的固有不足,且受模型固有噪声、随机噪声的影响较大.多粒度模型的主要困难则在于如何统一多粒度模型的表示使其具有统一的语义背景.本案方法使用扩展卡尔曼滤波融合gps、惯性传感器和图像传感器三者的数据用于定位,与此同时使用卡尔曼滤波融合通过单目、多目、红外图像传感器生成的稀疏(sparse)、半稠密(semidense)、稠密(dense)环境模型,进而同步生成融合后的统一多粒度概率八叉树网格地图,实现不同辅助状态下所需场景粒度的导航.对于导航中的路径规划本发明提出了一种基于威胁度和d*算法的动态路径规划算法,这种算法可以充分利用上述定位与建图的信息并最终实现精确智能的自主导航.这种导航方法不但能够在变电站环境下成功避开障碍物并规划路径,帮助自主巡检智能体完成从初始结点到目标结点的路径规划,实现真正意义的无先验知识环境感知，与此同时还能够在导航过程中规避随机出现的危险.在保证实时性和可靠性的同时给自主巡检智能体的环境感知提供了理论基础.相对于传统成熟的gps导航,这种基于视觉信息的细粒度导航不但不受卫星信号强度和磁场的干扰,可以在变电站环境中进行,而且还具备应对随机产生移动物体突发事件的能力.

技术实现要素：

本发明所要解决的技术问题（发明目的）

传统方式的室外建图与导航一般都采用单一传感器实现自身位置和环境的感知.例如gps传感器获取地理坐标或者激光测距仪获取当前智能体和障碍物之间的距离.这种方式容易受到环境及天气的限制.例如在变电站等强电磁场环境下gps模块失效，在雷电暴雨等恶劣天气条件下激光测距仪测量数据不准确等问题.而视觉传感器获取的图像数据虽然可以规避以上问题，但是其处理数据量大且对计算存储资源的消耗可观.为了解决单一传感器在室外导航中的缺陷，本案提出了一种基于扩展卡尔曼滤波的同步定位与建图vio方法.对由不同传感器对真实环境感知所生成的稀疏、半稠密、稠密环境模型提出一种统一的概率八叉树表示,并通过卡尔曼滤波融合,避免了单一模型的固有噪声和随机噪声.最后根据上述八叉树网格地图和本文定义的基于d*算法考虑威胁度的动态路径规划模型,完成对巡检智能体多粒度导航并实现了一个自主移动工具原型.

本发明提供的完整技术方案（发明方案）

自主导航的基础是高度拟合真实环境的三维模型.智能电站自主巡检智能体通过各种异构相机获取外界图像，得到的绝大部分环境视频感知数据都来自于变电站设备、道路等复杂场景.传统环境模型的建立多以单一分辨率进行,生成的单粒度模型如稀疏、半稠密、稠密模型存在各异构图像传感器的固有不足,且受模型固有噪声、随机噪声的影响较大,难以全面的描述上述复杂场景.对于多粒度模型而言,建立的主要困难则在于如何统一各种粒度模型使其具有统一的语义背景.为了解决上述问题,本案提出一种多粒度环境的概率八叉树的统一表示,并根据室内外感知特性选择特定粒度的模型进行融合.如:在室内场景的定位和导航中,由于绝大多数障碍物距离与被导航对象的间距在红外相机的有效工作范围内,因此使用红外深度相机获取的深度稠密地图和单目rgb相机获取的图像建立稀疏地图;在室外场景的定位和导航中,为了克服随机噪声的影响和获取较远障碍物的图像信息,使用双目相机和单目相机分别建立半稠密和稀疏地图.另一方面,通过分析视觉里程计的数据融合方法,最终提出适用于室内建图导航的红外稠密加单目稀疏模型和室外导航的单目半稠密加双目稀疏模型实现定位、建图和导航.整个导航方法的结构如图1所示.图中硬件输入部分为单目、双目、红外深度相机.整个系统建立有世界坐标系g,避障坐标系m和工具坐标系.其中世界坐标系中建立的永久地图用于回环检测和重定位,避障坐标系采用深度相机等传感器建立的临时稠密地图配合视觉惯性里程计返回在该坐标系下的相机位姿信息,用来规避路径中存在的障碍物等.这两个坐标系通过在世界坐标系中的基线帧在世界坐标系和避障坐标系之间相互转换,并配合工具坐标系上的定位与建图并在gps导航的辅助下完成地图和姿态的坐标转换.两个坐标系的相互配合使得世界坐标系无需存储耗费巨大的稠密地图,且能够利用稠密地图为避障提供最丰富的决策信息,这样可以大大节省计算消耗并提高系统的时效性.

本案在深入研究现有算法的基础上,提出了基于光学图像的多粒度随动环境感知算法.该算法采集多种异构图像传感器及相应visualslam算法所生成的稀疏、半稠密、稠密多种粒度的异构三维点云模型,同时提出一种概率八叉树统一表示生成的若干三维模型.然后通过卡尔曼滤波,在相机运动期间不断融合多种异构点云的置信度并更新时态融合概率八叉树模型(temporalfusedprobabilisticoctreemodel,简称tfpom),这种融合方法既可以保证环境模型的时空一致性又可以满足后续增量更新的需求.同时利用一种有效的剪枝和归并策略,在压缩模型存储空间的同时使环境模型能够以任意粒度动态拟合真实环境,最终实现鲁棒的随动环境感知，实现多传感器融合的多粒度导航.

在定位过程中本案使用slam思想实现,姿态图作为存储定位与姿态约束信息的载体,便于ba的优化.视觉里程计通过经典的扩展卡尔曼滤波方法实时得出较为精确的变换矩阵,加入姿态图中并计算出6自由度的智能体姿态,从而完成定位的过程.本案所用姿态图的特点就是采用了在全局坐标系中跟子坐标系维持转换关系的基线帧,所有当前结点和地标(landmark)位置都在局部障碍坐标系的地图中表示,这就使得可以通过与基准帧之间的转换量来完成子坐标系和全局坐标系的对齐操作,而不需要每次都更新地图中的关键帧和地标.正由于此便可维持相对固定的参考地图,当新的建图信息到来时再由局部地图和参考地图间的转换量去更新全局地图.

当巡检智能体的移动路径不包含细粒度的移动姿态时,例如指定了从变电站入口按照一定规则移动到出口而不会给出需要如何转弯和倾斜.这个时候才需要稠密建图介入来完成位姿的估计和判断实现这种细粒度地导航.而为了实现该粒度的建图和导航,需要结合特征点立体匹配来预测相机姿态.由于存储稠密地图需要耗费巨大的空间和计算资源只有一小部分关键帧被保存下来.这样就只能根据对导航有关部分的信息来建立局部地图.在图像帧间和摄像机不同视角获取的帧中包含有特征点、特征点描述子和跟踪特征点时所用的三维地标,形成了一个由关键帧作为顶点和其他深度信息作为边的姿态图.

为了保持局部里程计框架的一致性,我们只对基准坐标系进行修改.对已建地图进行重定位之前需要将局部地图添加到参考地图上.为了在通过光束平差法进行图优化时能够增量地进行地图重建,我们通过一个框架来实现基于事务的(transaction-based)多线程访问地图更新.但是由于光束平差法本身的特点导致图中顶点的位置会经常出现大幅度变化,当创建新的局部坐标时仅采用最近一个参考坐标作为参考.详见公式(1)(2):

(1)

(2)

其中tva是执行光束平差法之前参考坐标系中最后一个顶点6自由度的偏移量,tvb是执行之后的偏移量.tgml是相对于参考坐标系的基准帧偏移量而tgmr是相对于局部坐标系的基准帧偏移量更新值.

拥有一个执行过光束平差固定参考坐标的好处就是能够通过定位来修正视觉惯导的漂移误差.这是通过依靠在特征点描述子区域做近邻匹配来实现的,随后使用ransac机制的鲁棒估计方法来剔除野点.而当前流程中所有内点会被当做边约束加入到姿态图的顶点之间,同时在地图中三角测量特征点的三维坐标.相对于自主移动工具的运动而言姿态漂移的发生是很缓慢的,我们并不需要按照姿态预测的频率执行重定位操作.只需要每到一个新的关键帧便逆着全局地图执行回环检测,通过对滑动窗口大小的关键帧中关键帧和地标的约束执行非线性最小二乘优化更新定位预测.姿态图顶点的位置和地图中三角测量地标的三维坐标作为固定值保留下来,而只有局部坐标的基础框架转化是非固定值.唯一的残余量是参考坐标系中二维特征点和三维地标间的重投影误差.

在建图过程中本案引入了八叉树表示方法.八叉树是一种将环境空间迭代细分为八维子空间的存储表示方法.在环境感知中,八叉树的叶子节点存储所有的环境体素,非叶子节点称为内点(innernode),在插入、剪枝、归并或变更分辨率等树操作后可能转换为叶子节点或高层节点,如图2和图3所示.环境空间内的立体度量单位在转换成三维模型中的一个体素后可以分为三种状态.其中一种称为未知状态,表示该体素在当前时刻之前相机尚未捕捉到的环境区域中.剩余两种相互制约状态分别称为占有和空闲,表示该体素对应的立体度量单位在当前时刻被认为有实体占有和被认为无实体占有.

在实际对环境的感知过程中,受各种异构相机内部噪声和环境外部噪声产生共同影响,当前时刻体素的状态难以由三元状态完全解释.概率八叉树在第i个叶子节点使用logodds值l(i),如式(3)所示:

(3)

表示对占有状态的置信度.p(i)的初始值一般设置为0.5,表示处于置信度不明确状态.根据三元状态的定义,该叶子节点空闲状态的置信度为1-l(i),而未知状态则会被预先标识.而这种置信度关系随着相机移动可能会产生三种状态的置信度变化,例如状态为未知的环境区域被摄像机在后续时刻捕捉到而转换为其他两种状态或者非刚体环境中体素随时态在占有和未知状态相互转换.概率八叉树使用式(2)更新时间序列节点t上的logodds值l(i|z1:t),其中z1:t为传感器在当前时刻的累计度量,l(n|zt)根据异构模型的固有置信特性设置,如式(4)所示.因为这种logit转换可逆,更新过程可以被解释为一种概率度量更新.

(4)

其中logodds值通过反logit变换,如式(5)所示:

(5)

转换成p(n|z1:t),并由式(6)表示概率更新过程:

(6)

稀疏、半稠密和稠密模型经过上述过程后转化为统一的概率八叉树表示.在相机时间序列中,若干稀疏、半稠密和稠密的概率八叉树模型在同步定位与建图的每个时刻不断生成.在环境感知中,八叉树的叶子节点存储所有的环境体素,非叶子节点称为内点,在插入、剪枝、归并或变更分辨率等树操作后可能转换为叶子节点或高层节点,如图2所示,相机在沿轨迹运动的过程中算法不断以多粒度感知环境.由此,在时间序列中的每个时间节点,由不同异构模型所生成的若干概率八叉树模型如何有机融合成统一的时态融合概率八叉树模型成为了下一步需要解决的问题.

本案提出一种基于卡尔曼滤波的置信融合方法,设第i个概率八叉树模型的第j个叶子节点在k时刻的形式化表示为,其中i=0表示时态融合概率.

置信度融合算法首先引入一个离散系统的控制模型和测量方程,分别如式(7),式(8)所示,其中为系统在k时刻的状态变量,为系统在k时刻第i个观测样本的状态变量观测值,w(k),vⁱ(k)分别为过程激励噪声和第i个观测样本的观测噪声,协方差分别为q,rⁱ.

(7)

(8)

下一步使用系统的过程模型,如式(9)所示,预测下一时刻的系统状态.

(9)

更新状态的协方差矩阵,如式(10)所示:

(10)

计算每个观测值的卡尔曼增益,如式(11)所示.

(11)

结合观测值更新状态估计,如式(12)所示,其n为观测值个数,obv函数将模型置信度融入,如式(13)所示.

(12)

(13)

最后更新k+1时刻的误差估计,如式(14)所示,并返回式(9),使卡尔曼滤波器随时间序列不断迭代下去.

(14)

为可视化算法的完整过程,对生成的点云地图生成统一的概率八叉树表示.接着对上步每个时间节点所生成的若干概率八叉树融合.最后通过时间序列地不断增量融合,完整的多粒度表示环境模型,如图4所示.该方法的导航部分依赖于经过以上定位和概率八叉树建图过程得到的包含了惯导单元测量数据的栅格地图.传统的dijkstra算法以出发点为中心向外层层扩展直到终点,能得到最短路径最优解,但是遍历节点多,计算代价太大不适用于嵌入式设备.a*算法是静态路网路径规划中最有效的方法,但是由于智能辅助系统的应用环境是动态的,例如会有人从摄像机前走过或者车辆掠过,且定位和建图过程也是动态进行的,因此这个栅格地图是动态变化的.因此只能选用d*算法作为路径规划的方法.而惯导单元测量后的数据经过和视觉信息的融合还提供了障碍信息,因此在这个地图中还有需要避开的障碍物.这些障碍物有可能是行人、车辆或者建筑物等不同形状和体积的物体,本文将这些不规则的障碍物简化为具有一定作用半径的圆,而由于这些障碍有的是在缓慢移动或者高低变化,因此对自主移动工具的威胁也是不确定的,需要对每个障碍圆留有一定的余度.定义o是智能辅助系统受到的障碍代价,r是障碍物的有效作用半径,d是摄像机到障碍中心点的距离,s根据自主移动工具特性设计的安全距离,威胁度建模可用式(15)表示:

(15)

由于d*算法是在a*算法和dijkstra算法的基础上建立起来适合含有不确定因素地图的路径规划算法,在考虑威胁度时需要选取合适的代价函数.我们令g(x,r)是点x到摄像机当前位置的估计代价,o(x,r)是x到目标点r的障碍代价.则代价函数c(x,r)可以用式(16)表示：

(16)

具体实施方法

下面结合附图和实施例子对本发明做进一步说明。

如图5所示为一个智能电站自主巡检智能体的应用实例，包括一个搭载nvidiadevelopmentkittx1嵌入式设备的四轮小车，其中tx1负责运行本发明方法并通过舵机控制轮轴实现自主移动及导航.相关环境、运动感知传感器包含红外深度相机kinect(用于室内)、双目摄像头(用于室外)、板载单目摄像头.在智能体向目标节点移动过程中该算法检查最短路径上下一节点的变化情况。这个变化是由slam建图和惯导单元测量数据综合而得的.但是d*算法对于距离远的最短路径发生的变化并不敏感,由于惯导单元测量距离的局限不能感应远距离的变化,因此这并不影响算法的正确执行和路径规划.具体的路径规划算法如下所示：

1.创建两个表open,close.open表保存所有已生成但未考察的节点,close表保存已访问过的节点.

2.用dijsktra算法从目标节点d开始向起始节点搜索.储存路网中目标节点到各个节点的最短路和该位置到目标点的实际值h,并设置中间变量k存储变化中h的最小值,当前为k=h.

3.沿最短路径开始移动,在下一节点没有变化时利用上一步计算出的最短路径信息从出发点向后追踪.定义x为下一节点,y是当前节点.当探测到x状态发生改变时,调整当前位置y到目标节点d的实际值h(y),由x节点到y节点的新代价权值c(x,y)与原权值h(x)相加得到.

4.使用a*算法遍历y的子节点并放入close表中.令y的子节点为a,则h(a)=h(y)+c(y,a),其中h(a)为子节点a的h值,c(y,a)为y到子节点a的权重代价.

5.从open表中取k值最小的节点y,遍历y的子节点a并计算h(a)=h(y)+c(y,a).

6.如果a节点在open表中,向下继续执行.如果在close表中跳转到步骤8,如果都不在则跳转至步骤9.

7.如果计算所得a的权值h(a)小于open表中存储的权重值,则更新open表中相应的值为新计算的最小值.

8.如果计算所得a的权值h(a)小于close表中存储的权重值,则更新close表中相应的值为新计算的最小值并将a节点放入open表中.

9.将a节点插入open表中.

10.将y节点放入close表中.

11.输出open表中所有节点并按照k值大小进行排序.

12.使用第一步dijsktra计算出的最短路径信息生成从当前位置到目标节点d的路径.

在变电站真实场景,智能巡检体通过一段包含较多障碍物的站内崎岖道路.行走的起始节点与终止节点间路线相对固定,但需要实现绕行直角转弯并在极为狭窄的道路间行进.这对多传感器数据融合是巨大的挑战.尽管本发明只对其中定位、建图和导航的具体实施方式进行了描述，但是本领域普通技术人员应当了解到本发明可以在不偏离其主旨与范围内以许多其他的形式实施。因此，所展示的例子与实施方式被视为示意性的而非限制性的，在不脱离如所附各权利要求所定义的本发明精神及范围的情况下，本发明可能涵盖各种修改与替换。