一种地铁车厢消毒机器人自动导航方法与流程

一种地铁车厢消毒机器人自动导航方法
1.本申请为“申请日：2020年10月14日，申请号：2020110988107，发明创造名称：一种地铁车厢消毒机器人及其自动导航方法”的发明专利申请的分案申请。
技术领域
2.本发明涉及一种地铁车厢消毒机器人自动导航方法，属于机器人技术领域。


背景技术：

3.新冠肺炎疫情期间，地铁车站安检设备、自助售票设备、闸机、楼梯扶手、电扶梯扶手、公共卫生间门把手和门帘、直梯轿厢四壁、内外部按键等乘客经常接触的部分以及地铁站公共区域、其他运营设备设施、卫生间等需定期消毒；此外列车在出库前或是到达终点站后进行折返作业时，还要对车厢内部进行喷洒式消毒。但目前多是采用人工消毒的方式，对人员的素质、技术和主观性依赖性大，且有些属于人工消毒难以覆盖的隐蔽空间场所，无法实现消毒全覆盖。因此采用自动化、智能化消毒机器人，代替工作人员对地铁车厢、运营设备设施进行定期消毒，是今后本领域技术变革、更新换代的发展方向。
4.地铁每节车厢的结构几乎完全一致，即使人在车厢内，也只能依靠车厢内标识进行分辨，而无法通过车厢内结构进行判断，其对称且重复结构是典型的长廊环境，而长廊移动导航是移动机器人导航中的难点问题。当仅采用激光雷达或其他单一导航设备时，极易导致机器人丢失坐标，无法实现自动导航和自主消毒任务。


技术实现要素：

5.为解决上述问题，本发明的目的在于提供一种地铁车厢消毒机器人自动导航方法。其中，所述自动导航方法采用激光雷达与视觉里程计或视觉导航复合，闭环slam建图导航策略，以实现机器人在地铁车厢内的自动精准导航。
6.本发明的目的是通过以下技术方案实现的：
7.一种地铁车厢消毒机器人，包括机器人本体和和雾化系统，所述雾化系统置于所述机器人本体内部空腔，且从上至下依次设有出雾口、出雾软管和雾化箱，所述雾化箱的底部设有若干个雾化板；
8.雾化板压电基底下设有压电圆片，液体在雾化板的流道上雾化；所述流道还经进气口连通微型空压机，所述流道的竖管侧壁上设有互为反向的出气口，在所述雾化板下还设有气孔，所述气孔直径为0.10
‑
0.85mm，加压空气进入流道内喷出的气体形成旋流至所述气孔处，将雾化后的颗粒进行二次破碎。
9.进一步的，所述雾化箱内设有存水箱，所述存水箱内设有浮球阀；所述雾化板表面设有液位传感器。
10.进一步的，所述消毒机器人还包括驱动机构、主控模块、控制模块和电源模块；其中，
11.所述驱动机构包括两个主动轮和两个从动轮，所述主动轮由伺服轮毂电机带动，
所述从动轮为万向轮；
12.所述主控模块包括工控电脑、激光雷达传感器和无线模块，其中所述激光雷达传感器设于所述机器人本体顶部；
13.所述控制模块包括伺服控制器和控制板；
14.所述电源模块包括电控面板按钮和锂电池。
15.一种地铁车厢消毒机器人自动导航方法，包括以下步骤：
16.步骤1，一段时间内连续扫描的激光雷达扫描帧生成一个子图，子图采用概率栅格的地图表达模型，当新的激光雷达扫描帧插入到概率栅格时，栅格状态更新；
17.步骤2，在把激光雷达扫描帧插入到子图之前，对激光雷达扫描帧位姿和当前的子图通过ceres solver求解器进行优化；
18.步骤3，通过稀疏位姿调整方法优化所有激光雷达扫描帧和子图的位姿，激光雷达扫描帧在插入子图时的位姿会被缓存到内存中进行回环检测，当子图不再变化时，所有的激光雷达扫描帧和子图都被进行回环检测，从而消除子图的累积误差，得到环境地图；
19.步骤4，在消毒机器人平台顶端安装双目摄像头，并每隔k秒拍摄一次环境信息，左摄像头拍摄到的n时刻的图像为左图像i
nl
，右摄像头拍摄到的n时刻的图像为右图像i
nr
；
20.步骤5，分别将左图像i
nl
和右图像i
nr
进行特征点检测及描述，并将左图像i
nl
和右图像i
nr
进行特征点匹配得到图像f
nlr
；
21.步骤6，再将前后帧图像f
(n
‑
1)lr
与f
nlr
进行特征点匹配；
22.步骤7，利用三角测量法重建前后帧图像匹配特征点的三维坐标，进行运动估计，得到机器人全局位姿。
23.进一步的，步骤1中，将激光雷达扫描帧通过自适应阈值的分割方法进行区域分割，将3d点云图处理成2d平面图。
24.进一步的，步骤3中，使用分支定界扫描匹配算法加速回环检测和相对位姿的求解过程，确定搜索窗口，采用查找的方法构建回环。
25.一种地铁车厢消毒机器人自动导航方法，包括以下步骤：
26.步骤1，一段时间内连续扫描的激光雷达扫描帧生成一个子图，子图采用概率栅格的地图表达模型，当新的激光雷达扫描帧插入到概率栅格时，栅格状态更新；
27.步骤2，在把激光雷达扫描帧插入到子图之前，对激光雷达扫描帧位姿和当前的子图通过ceres solver求解器进行优化；
28.步骤3，通过稀疏位姿调整方法优化所有激光雷达扫描帧和子图的位姿，激光雷达扫描帧在插入子图时的位姿会被缓存到内存中进行回环检测，当子图不再变化时，所有的激光雷达扫描帧和子图都被进行回环检测，从而消除子图的累积误差，得到环境地图；
29.步骤4，在基于摄像头的观测模型中，用极坐标来表示t时刻摄像头获取的环境特征i的观测量z(t)：
[0030][0031]
其中，和分别表示环境特征i与机器人的距离关系和角度关系；
[0032]
步骤5，基于摄像头的观测模型表示为：
[0033]
z(k)＝h(x(k))+v(k)
[0034]
x(k)为机器人在k时刻时的位姿；z(k)、h( )和v(k)分别为摄像头在k时刻的观测信息、观测函数和观测噪声；
[0035]
步骤6，机器人在k时刻的位姿为(x
t
(k),y
t
(k),θ
t
(k))
t
，此时摄像头观测到n个特征，每个特征的观测值为(x
i
,y
i
)，得如下表达式：
[0036][0037]
其中，x
i
，y
i
表示在k时刻传感器探测到的第i个路标特征的坐标；和分别表示第i个路标在k时刻与机器人的位置关系和角度关系；x
t
(k)，y
t
(k)，θ
t
(k)分别表示移动机器人在k时刻的位姿；
[0038]
步骤7，记录摄像头与环境特征的相对位置。
[0039]
进一步的，步骤1中，将激光雷达扫描帧通过自适应阈值的分割方法进行区域分割，将3d点云图处理成2d平面图。
[0040]
进一步的，步骤3中，使用分支定界扫描匹配算法加速回环检测和相对位姿的求解过程，确定搜索窗口，采用查找的方法构建回环。
[0041]
本发明的有益效果为：
[0042]
所述消毒机器人采用气动超声波雾化器，使得雾化后雾量变大，雾滴粒径更细。
[0043]
采用激光雷达与视觉里程计或视觉导航复合，闭环slam建图导航策略，以实现机器人在地铁车厢内的自动精准导航，克服了仅依靠激光雷达定位的长廊移动导航的问题，也避免了视觉里程计或视觉导航受光线影响较大的问题。
[0044]
激光雷达构建环境地图时，使用分支定界方法可以高效的计算值，采用深度优先搜索的分支定界扫描匹配算法，使得机器人在行驶中搜索目标点反馈速度高精度值高。激光雷达结合视觉里程计的位姿估计结果更加精确，对环境有更好的适应性，采用基于特征点匹配的方案获得机器人行驶里程。
[0045]
另外还可通过机器人视觉系统观测移动机器人在移动时刻的位姿。
附图说明
[0046]
图1为地铁车厢消毒机器人外观结构示意图；
[0047]
图2为地铁车厢消毒机器人内部结构示意图；
[0048]
图3为雾化箱结构示意图；
[0049]
图4为雾化系统结构示意图；
[0050]
图5为雾化器结构示意图；
[0051]
图6为雾化板流道气孔出气结构示意图；
[0052]
图7为雾化板结构示意图；
[0053]
图8为整体雾化器组合结构示意图；
[0054]
图9为实施例2及实施例3中子图栅格状态更新示意图；
[0055]1‑
手提把手，2
‑
出雾口，3
‑
出雾软管，4
‑
雾化箱，5
‑
控制模块，6
‑
风扇，7
‑
主动轮，8
‑
从动轮，9
‑
锂电池，10
‑
排水口，11
‑
激光雷达传感器，12
‑
主控模块，13
‑
存水箱，14
‑
出雾管
口，15
‑
风扇进风口，16
‑
浮球阀入口，17
‑
进水口，18
‑
通气口，19
‑
出风口，20
‑
雾化板，21
‑
入水口，22
‑
微型空压机，23
‑
气孔，24
‑
压电圆片，25
‑
接气源，26
‑
出气口，27
‑
出气方向，28
‑
进气口，29
‑
雾化片，30
‑
液位传感器。
具体实施方式
[0056]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
[0057]
实施例1
[0058]
一种地铁车厢消毒机器人，包括机器人本体和和雾化系统，如图2和图4所示，所述雾化系统置于所述机器人本体内部空腔，且从上至下依次设有出雾口2、出雾软管3和雾化箱4。如图8所示，所述雾化箱4的底部设有若干个雾化板20。
[0059]
雾化板压电基底下设有压电圆片24，压电圆片通电后相互振动下产生多个波形，在运动惯性的作用下，液体在雾化板的流道上雾化。同时，如图5所示，在所述雾化板下还设有直径为0.1mm的气孔23。所述气孔23的出气结构如图6所示，流道经进气口28连通微型空压机22，加压空气经进气口28进入流道内，流道的竖管侧壁上设有2个互为反向的出气口，加压空气进入流道内喷出的气体形成旋流至所述气孔23处，将雾化后的颗粒进行二次破碎，即进行二次雾化，从而形成气动超声波雾化器，使得雾化器进一步改善了液体雾化效果，同时消耗的电能也大大降低。
[0060]
所述消毒机器人还包括驱动机构、主控模块12、控制模块13和电源模块；其中，
[0061]
所述驱动机构包括两个主动轮7和两个从动轮8，所述主动轮7由伺服轮毂电机带动，所述从动轮8为万向轮。
[0062]
所述主控模块12包括工控电脑、激光雷达传感器11和无线模块，其中所述激光雷达传感器设于所述机器人本体顶部。
[0063]
所述控制模块13包括伺服控制器和控制板。
[0064]
所述电源模块包括电控面板按钮和锂电池9。
[0065]
为机器人的定位导航，所述消毒机器人上还安装有摄像头或双目摄像头。
[0066]
雾化板结构如图7所示，供电采用48v供电，所述雾化板20表面设有液位传感器，当水位较低时，液位传感器传出开关量信号并向主控模块发送，主控模块控制雾化器的启停命令。
[0067]
如图3所示，为雾化箱外观结构示意图，在雾化箱的顶部分别设有出雾管口14，用于连接出雾软管3；设有风扇进风口15，用于给雾化器送风；设有浮球阀入口16，用于存水箱13内浮球阀的安装；设有进水口17，用于给存水箱13蓄水；设有通气口18，用于保证雾化箱内部的气体流通；另外在雾化箱底部还设有排水口10，用于存水箱13及雾化器的排水。
[0068]
所述雾化箱3内设有存水箱13，所述存水箱进水口17蓄水，再经入水口21连通雾化器。所述存水箱13内设有浮球阀，可控制存水箱液面，水位不受水压干扰，开闭紧密不漏水。
[0069]
如图1所示，为消毒机器人的外观结构示意图，在消毒机器人的顶部和侧方分别设有手提把手1，主要是用于地铁车厢消毒，方便机器人的搬运和移动，如将其放入地铁车厢，开始运作进行消毒作业。
[0070]
其中，出雾口通过出雾装置可设有多个，分别设置在机器人顶部、左、右、后端出雾，对车厢进行消毒，由于出雾颗粒在5微米左右，出雾是可以漫到整个车厢，可对车厢把手，座位、及卫生死角进行消毒。
[0071]
实施例2
[0072]
一种地铁车厢消毒机器人自动导航方法，包括以下步骤：
[0073]
步骤1，一段时间内连续扫描的激光雷达扫描帧生成一个子图，子图采用概率栅格的地图表达模型，当新的激光雷达扫描帧插入到概率栅格时，栅格状态更新，具体如下：
[0074]
机器人的位姿可以用ξ＝(ξ
x
,ξ
y
,ξ
θ
)来表示，ξ
x
和ξ
y
表示在x和y方向的平移量，ξ
θ
表示在二维平面的旋转量。将激光雷达传感器测量的数据记做h＝{h
k
}
k＝1,...,k
,初始激光点为激光雷达扫描数据帧映射到子图的位姿变换记作t
ξ
，可以通过公式(1)映射到子图坐标系下：
[0075][0076]
式中，r
i
表示激光雷达观测位姿的旋转角，t
i
表示激光雷达观测位姿的平移坐标，p表示扫描点到子图中的概率。
[0077]
一段时间内连续扫描的激光雷达扫描帧生成一个子图，子图采用概率栅格的地图表达模型，当新的激光雷达扫描帧插入到概率栅格时，栅格的状态将会被计算，每一个栅格都有命中(hit)和丢失(miss)两种状态。命中的栅格，将相邻的栅格插入到命中集合中，将扫描中心和扫描点连接射线上的所有相关点添加到丢失集合中。对每个之前没有观测到的栅格会设置一个概率值，对于已经观测到的栅格则会按照公式(2)对其进行概率更新，示意图如图9所示，图中带有叉号和阴影的栅格表示命中，只有阴影的栅格表示丢失。
[0078][0079]
m
new
(x)＝clamp(odds
‑1(odds(m
old
(x))
·
odds(p
hit
)))
[0080]
式中，odds表示发生比，m表示更新的网格的相应值，clamp函数表示将随机变化的数值限制在一个给定的区域内，返回的值介于两个数min和max之间，若值小于min返回min，若值大于max则返回max。
[0081]
由于激光雷达本身的误差以及测量过程中路面不平导致的小车颠簸的问题使得激光雷达扫描点存在较大范围的波动，严重影响了占据栅格地图构建中各栅格的概率计算和更新，于是本文提出了一种激光雷达扫描帧特征提取结合占据栅格地图构建的方法，该方法在激光雷达扫描帧中提取直线段特征并进行拟合，之后根据拟合出来的直线段将激光雷达扫描点平移到直线上。经过实地测试证明，该方法能够有效得减小激光雷达扫描点的误差，提高建图边缘的清晰度和锐度。
[0082]
步骤2，在把激光雷达扫描帧插入到子图之前，对激光雷达扫描帧位姿和当前的子图通过ceres solver求解器进行优化，便可以将上述问题转化为求解非线性最小二乘问题：
[0083][0084]
ξ属于扫描位姿，其中t
ξ
根据特征位姿h
ξ
将特征点坐标转换成子图坐标，m
smooth
函数完成从局部子图中概率值的平滑形式，该函数采用双三次插值方法。
[0085]
步骤3，由于激光雷达扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差。通过稀疏位姿调整方法(sparse pose adjustment，spa)优化所有激光雷达扫描帧和子图的位姿：激光雷达扫描帧在插入子图时的位姿会被缓存到内存中进行回环检测；当子图不再变化时，所有的激光雷达扫描帧和子图都被进行回环检测，从而消除子图的累积误差，得到环境地图。
[0086]
具体的，使用稀疏位姿调整方法构建的优化问题的数学表达式为：
[0087][0088]
上式中，分别表示在一定约束条件下，子1图的位姿和特征点的位姿集合；ρ为损失函数；表示子图的位姿，m为子图的数量；表示特征点的位姿，s为特征点的数量；相对位姿ξ
ij
表示特征点j在子图i中的匹配位置，与其相关的协方差矩阵∑ij,共同构成优化约束。这个约束的代价函数使用残差e表示，可由公式(5)计算：
[0089][0090][0091]
和表示同一雷达角度下的子图位姿和特征点位姿，表示表示子图位姿矩阵的转置，表示特征点位姿矩阵的转置，表示子图位姿逆矩阵的秩。
[0092]
此外，本实施例还使用分支定界扫描匹配算法加速回环检测和相对位姿的求解过程，确定搜索窗口，采用查找的方法构建回环。
[0093][0094]
ξ属于扫描位姿，w是搜索空间，m
nearest
是像素对应的网格点的m值，t
ξ
表示子图坐标，h
k
表示特征点位姿，其中w由下列公式得到，将特征点带入下列公式：
[0095]
d
max
＝max
k＝1,...k
||h
k
||
[0096][0097]
d
max
表示特征点中最远的点到原点的距离，r表示分辨率，将得到的角度步长δ
θ
带入公式(8)：
[0098][0099]
w
x
、w
y
表示雷达水平范围的值，w
θ
表示扫描角度的值，假设w
x
＝w
y
＝7m，w
θ
＝30
°
，根据上式计算出线性和角度窗口大小，将得到的值带入公式(9)：
[0100][0101][0102]
ξ0表示初始搜索框中心的特征点位姿，j
x
、j
y
和j
θ
表示搜索框中的任意水平和角度值；
[0103]
由式(9)求得搜索空间大小，将得到的w值代入公式(6)求得约束关系，进而进行回环优化。
[0104]
全局优化主要使用图优化将局部优化后建立完成的子图位姿加入到全局位姿优化中来，全局优化采用分支定界法进行加速，以进行回环的检测和相对位姿求解，相比传统的先检测回环，在求解相对位姿来说，显得结构上更为统一，将建立回环的过程转化为一个查找过程，而另一方面，当完成对离散候选解空间的树形构造后，在树中对解的查找过程速度很快，剩下的对于建树过程中的节点的界限的选择通过对子图的预先计算来完成，这种中间结构的引入使得回环过程可以实时的完成，从而通过回环不断调整子图以消除累计误差。
[0105]
即使用分支定界方法可以高效的计算值，采用深度优先搜索的分支定界扫描匹配算法，使得机器人在行驶中搜索目标点反馈速度高精度值高。
[0106]
步骤4，在消毒机器人平台顶端安装双目摄像头，并每隔k秒拍摄一次环境信息，左摄像头拍摄到的n时刻的图像为左图像i
nl
，右摄像头拍摄到的n时刻的图像为右图像i
nr
；
[0107]
步骤5，分别将左图像i
nl
和右图像i
nr
进行特征点检测及描述，并将左图像i
nl
和右图像i
nr
进行特征点匹配得到图像f
nlr
；
[0108]
步骤6，再将前后帧图像f
(n
‑
1)lr
与f
nlr
进行特征点匹配；
[0109]
步骤7，利用三角测量法重建前后帧图像匹配特征点的三维坐标，进行运动估计，得到机器人全局位姿。
[0110]
其中，双目摄像头的坐标系一般以左侧摄像头的坐标系为基准建立，通过刚体变换可以表示双目摄像头在n
‑
1到n相邻两帧间的位姿变化量，也就是移动机器人在帧间的位姿变化量：
[0111][0112]
其中，r
n,n
‑1代表姿态变化量，是3
×
3的矩阵，t
n,n
‑1代表位置变化量，是3
×
1的向量。左右摄像头拍摄的图像i
n
对应的位姿记为c
n
，且c
n
＝t
n,n
‑1c
n
‑1。坐标或角度变化量的下标从两个相邻时刻改为后一时刻(将r
n,n
‑1改成r
n
)，这意味着视觉里程计最主要的作用就是联立n
‑
1时刻的图像i
n
‑1和n时刻的图像i
n
计算出图像的位姿变化t
n
。
[0113]
移动机器人在三维空间中完整的定位信息有六个自由度，包括x,y,z三个坐标和
θ,φ三个欧拉角。在不依靠其他定位导航方式辅助时，单目视觉只能获得包括θ,φ的三个欧拉角和两个依靠尺度因子才能求出的位置信息在内的五个自由度的信息。为了获得机器人六个自由度的定位信息，通常选择基于双目摄像头的视觉里程计模型。对于匹配相邻两帧图像间的一致性信息的方法，由于致密匹配方法如光流法等在光照变化时会使匹配精度降低，且计算量较大，为了使视觉里程计的位姿估计结果更加精确，对环境有更好的适应性，因此本实施例采用基于特征点匹配的方案。
[0114]
优选的，步骤1中，将激光雷达扫描帧通过自适应阈值的分割方法进行区域分割，将3d点云图处理成2d平面图。
[0115]
当激光雷达扫描帧中有大量点波动的时候，就会影响到地图构建过程中栅格点的概率计算和更新，导致地图构建不清晰甚至错误。激光雷达每隔1
°
测量一次距离，旋转一周共获得360个扫描点数据，所以，对于距离激光雷达近的墙体，那么激光雷达扫描点落在这个墙体上面的点就会变得密集，而距离激光雷达远的墙体，落在它上面的激光雷达扫描点就会变得稀疏。
[0116]
如果我们用一个固定的阀值去分割区域，那么就会导致距离激光雷达远的整块墙体被切分成多块，这样显然不符合区域分割的要求。为此区域分割应该采用自适应阀值的分割方法，当扫描点与激光雷达之间的距离假设为d的时候，区域分割的阀值设置为g，当扫描点与激光雷达之间的距离为2d的时候，区域分割的阀值就设置为2g，这样能够减小距离对区域分割的影响。
[0117]
对激光雷达扫描帧进行区域分割后，就将一帧激光雷达扫描帧分割成了几个区域，每个区域内的所有点就可以被认为是连续的，之后对这个区域内的所有点做特征提取。
[0118]
首先假设这些点构成一条折线，折线的角点可以是一个或者多个，然后将这些点使用最小二乘法拟合成一条直线，接着检查所有点与拟合后的直线的距离，找出距离最远的那个点，如果这个距离大于某个阈值，那么就可以认为这个点就是一个角点，，从这个角点将这条直线折成两条直线，否者就说明所有点构成一条直线。如果这个区域有多个角点，那么就需要进行多次拟合，每次拟合找出一个角点，将所有数据点分成两个部分，接着再在每个部分继续进行数据拟合找出角点，如此循环进行直到所有区域内都不存在角点为止。
[0119]
实施例3
[0120]
一种地铁车厢消毒机器人自动导航方法，包括以下步骤：
[0121]
步骤1，一段时间内连续扫描的激光雷达扫描帧生成一个子图，子图采用概率栅格的地图表达模型，当新的激光雷达扫描帧插入到概率栅格时，栅格状态更新，具体如下：
[0122]
机器人的位姿可以用ξ＝(ξ
x
,ξ
y
,ξ
θ
)来表示，ξ
x
和ξ
y
表示在x和y方向的平移量，ξ
θ
表示在二维平面的旋转量。将激光雷达传感器测量的数据记做h＝{h
k
}
k＝1,...,k
,初始激光点为激光雷达扫描数据帧映射到子图的位姿变换记作t
ξ
，可以通过公式(1)映射到子图坐标系下：
[0123][0124]
式中，r
i
表示激光雷达观测位姿的旋转角，t
i
表示激光雷达观测位姿的平移坐标，p
表示扫描点到子图中的概率。
[0125]
一段时间内连续扫描的激光雷达扫描帧生成一个子图，子图采用概率栅格的地图表达模型，当新的激光雷达扫描帧插入到概率栅格时，栅格的状态将会被计算，每一个栅格都有命中(hit)和丢失(miss)两种状态。命中的栅格，将相邻的栅格插入到命中集合中，将扫描中心和扫描点连接射线上的所有相关点添加到丢失集合中。对每个之前没有观测到的栅格会设置一个概率值，对于已经观测到的栅格则会按照公式(2)对其进行概率更新，示意图如图9所示，图中带有叉号和阴影的栅格表示命中，只有阴影的栅格表示丢失。
[0126][0127]
m
new
(x)＝clamp(odds
‑1(odds(m
old
(x))
·
odds(p
hit
)))
[0128]
式中，odds表示发生比，m表示更新的网格的相应值，clamp函数表示将随机变化的数值限制在一个给定的区域内，返回的值介于两个数min和max之间，若值小于min返回min，若值大于max则返回max。
[0129]
由于激光雷达本身的误差以及测量过程中路面不平导致的小车颠簸的问题使得激光雷达扫描点存在较大范围的波动，严重影响了占据栅格地图构建中各栅格的概率计算和更新，于是本文提出了一种激光雷达扫描帧特征提取结合占据栅格地图构建的方法，该方法在激光雷达扫描帧中提取直线段特征并进行拟合，之后根据拟合出来的直线段将激光雷达扫描点平移到直线上。经过实地测试证明，该方法能够有效得减小激光雷达扫描点的误差，提高建图边缘的清晰度和锐度。
[0130]
步骤2，在把激光雷达扫描帧插入到子图之前，对激光雷达扫描帧位姿和当前的子图通过ceres solver求解器进行优化，便可以将上述问题转化为求解非线性最小二乘问题：
[0131][0132]
ξ属于扫描位姿，其中t
ξ
根据特征位姿h
ξ
将特征点坐标转换成子图坐标，m
smooth
函数完成从局部子图中概率值的平滑形式，该函数采用双三次插值方法。
[0133]
步骤3，由于激光雷达扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差。通过稀疏位姿调整方法(sparse pose adjustment，spa)优化所有激光雷达扫描帧和子图的位姿：激光雷达扫描帧在插入子图时的位姿会被缓存到内存中进行回环检测；当子图不再变化时，所有的激光雷达扫描帧和子图都被进行回环检测，从而消除子图的累积误差，得到环境地图。
[0134]
具体的，使用稀疏位姿调整方法构建的优化问题的数学表达式为：
[0135][0136]
上式中，分别表示在一定约束条件下，子1图的位姿和特征点的位姿集合；ρ为损失函数；表示子图的位姿，m为子图的数量；表示特征点的位姿，s为特征点的数量；相对位姿ξ
ij
表示特征点j在子图i中的匹配位置，与其相关的协
方差矩阵∑ij,共同构成优化约束。这个约束的代价函数使用残差e表示，可由公式(5)计算：
[0137][0138][0139]
和表示同一雷达角度下的子图位姿和特征点位姿，表示表示子图位姿矩阵的转置，表示特征点位姿矩阵的转置，表示子图位姿逆矩阵的秩。
[0140]
此外，本实施例还使用分支定界扫描匹配算法加速回环检测和相对位姿的求解过程，确定搜索窗口，采用查找的方法构建回环。
[0141][0142]
ξ属于扫描位姿，w是搜索空间，m
nearest
是像素对应的网格点的m值，t
ξ
表示子图坐标，h
k
表示特征点位姿，其中w由下列公式得到，将特征点带入下列公式：
[0143]
d
max
＝max
k＝1,...k
||h
k
||
[0144][0145]
d
max
表示特征点中最远的点到原点的距离，r表示分辨率，将得到的角度步长δ
θ
带入公式(8)：
[0146][0147]
w
x
、w
y
表示雷达水平范围的值，w
θ
表示扫描角度的值，假设w
x
＝w
y
＝7m，w
θ
＝30
°
，根据上式计算出线性和角度窗口大小，将得到的值带入公式(9)：
[0148][0149][0150]
ξ0表示初始搜索框中心的特征点位姿，j
x
、j
y
和j
θ
表示搜索框中的任意水平和角度值；
[0151]
由式(9)求得搜索空间大小，将得到的w值代入公式(6)求得约束关系，进而进行回环优化。
[0152]
全局优化主要使用图优化将局部优化后建立完成的子图位姿加入到全局位姿优化中来，全局优化采用分支定界法进行加速，以进行回环的检测和相对位姿求解，相比传统的先检测回环，在求解相对位姿来说，显得结构上更为统一，将建立回环的过程转化为一个查找过程，而另一方面，当完成对离散候选解空间的树形构造后，在树中对解的查找过程速度很快，剩下的对于建树过程中的节点的界限的选择通过对子图的预先计算来完成，这种
中间结构的引入使得回环过程可以实时的完成，从而通过回环不断调整子图以消除累计误差。
[0153]
即使用分支定界方法可以高效的计算值，采用深度优先搜索的分支定界扫描匹配算法，使得机器人在行驶中搜索目标点反馈速度高精度值高。
[0154]
步骤4，在基于摄像头的观测模型中，用极坐标来表示t时刻摄像头获取的环境特征i的观测量z(t)：
[0155][0156]
其中，和分别表示环境特征i与机器人的距离关系和角度关系；
[0157]
步骤5，基于摄像头的观测模型表示为：
[0158]
z(k)＝h(x(k))+v(k)
[0159]
x(k)为机器人在k时刻时的位姿；z(k)、h(
·
)和v(k)分别为摄像头在k时刻的观测信息、观测函数和观测噪声；
[0160]
步骤6，机器人在k时刻的位姿为(x
t
(k),y
t
(k),θ
t
(k))
t
，此时摄像头观测到n个特征，每个特征的观测值为(x
i
,y
i
)，得如下表达式：
[0161][0162]
其中，x
i
，y
i
表示在k时刻传感器探测到的第i个路标特征的坐标；和分别表示第i个路标在k时刻与机器人的位置关系和角度关系；x
t
(k)，y
t
(k)，θ
t
(k)分别表示移动机器人在k时刻的位姿；
[0163]
步骤7，记录摄像头与环境特征的相对位置。
[0164]
摄像头作为非常常见并且被广泛应用的感知元器件，是移动机器人获取外界信息的一项重要工具，是基于视觉导航的移动机器人采集路标信息或特征观测信息并应用于机器人定位导航的关键硬件支持。摄像头通过采集和识别外界的环境信息，来理解和记录摄像头与环境特征的相对位置。
[0165]
同实施例2，步骤1中，将激光雷达扫描帧通过自适应阈值的分割方法进行区域分割，将3d点云图处理成2d平面图。
[0166]
本发明中，单板计算机通过rs
‑
232串行接口与激光测距仪和微控制器通信。消毒机器人使用了客户端
‑
服务器移动机器人控制体系结构。单板计算机是客户端，微控制器是服务器。微控制器装有机器人的服务器操作软件，该操作系统存储在微控制器的flash rom中。微控制器管理机器人控制和操作的所有低级细节，包括运动，航向和测距；它还包括发射声纳传感器，从声纳传感器，车轮编码器和陀螺仪收集数据。服务器(微控制器)使用特殊的客户端
‑
服务器通信数据包协议与控制客户端(单板计算机)进行通信，使用了两个基本的命令包；一个从客户端到服务器的数据包，另一个从服务器到客户端的数据包(服务器信息包，sip)。
[0167]
微控制器上的操作系统具有结构化的命令格式，用于接收和响应来自计算机的指令以控制和操作机器人。在特定时间段内可以发送到微控制器的命令数量取决于串行接口
的波特率和同步链接。微控制器上的aros接受几种不同的计算机运动命令，但有两种互斥的类型：独立轮速模式或平台平移/旋转模式。在独立的车轮速度模式下，机器人的微控制器试图保持精确的车轮速度，而在平移/旋转模式下，微控制器保持平台速度和航向。计算机发送给微控制器的所有运动命令参数都使用毫米或度单位，aros使用两个独立的参数将这些单位转换为编码器相关的运动值，一个用于平移，另一个用于旋转。
[0168]
用户还可以灵活地在单板计算机上使用特殊的客户端命令来启用或禁用任何特定的声纳，或更改周期时间。车载陀螺仪用于补偿车轮编码器未检测到的机器人航向变化，例如车轮打滑，变速箱间隙，车轮不平衡或表面状况。微控制器每25毫秒收集10位陀螺仪速率和8位温度数据，并根据请求将数据发送到sip中的单板计算机。机器人的方向和位置的调整均在机载计算机上完成。
[0169]
本发明在具体应用时，首开打开客户端软件，连接wifi，通过socket与通讯设置区的激光雷达、风机以及移动平台控制器的ip和端口号port进行连接。
[0170]
设定作业参数后，启动移动平台，通过采集到的图像，选取相应的防疫消毒模式：自动消毒和遥控消毒。自动消毒情况下，系统调用自动消毒模式函数，按原计划设定好的参数值执行；遥控模式下，客户端通过调用激光雷达、风机、雾化器，底盘运动等控制移动平台的运行轨迹、调用雾化器功率处理程序控制雾化距离和药液流量。
[0171]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。