适用于医院场景下移动机器人空间位姿点云校正方法及系统

1.本发明设计一种适用于医院场景下移动机器人空间位姿点云校正方法，属于机器人应用技术领域。


背景技术：

2.应用于医院场景的服务机器人导航技术隶属室内移动机器人研究范畴，医疗服务机器人作为服务机器人代表之一，具备较高科技价值和社会价值的特点，逐渐成为行业研究的热点之一。医用物流机器人是移动机器人技术在医院服务领域的一种应用。因此提升物流机器人系统工作的稳定性以及高效性成为至关重要的任务，确保机器人不能因为系统问题，影响到医院的工作流程。目前agv(automated guided vehicle，自动引导运输车)所搭载的激光雷达slam(simultaneous localization and mapping，即使定位和建图)建图算法对特定场景地适配仍然存在一些问题，比如对于一些大范围场景下建造的地图准确程度不高，并且系统存在一定的响应延迟。此外，技术人员在对存在问题的地图进行编辑的时候没有快捷有效的可视化工具，这些问题都制约着机器人性能的稳定释放。
3.为了解决上述问题，现有技术专利“一种用于室内外场景的人为回环即时定位和建图方法及系统cn112697158a”利用提出的人为校正因子校准机器人产生的定位误差，此方法在初始确定机器人与环境地图初始化位姿关系时候并没有对环境特征进行分类描述，导致系统对问题点确认的加载时间过长。之前的校正方法停留在二维空间位姿数据，没有三维位姿数据准确。且技术人员在后期进行校正的时候没有便捷的环境进行操作，给校正过程带来了难度。


技术实现要素：

4.本发明的目的在于：针对现有技术的不足，提供一种适用于医院场景下移动机器人空间位姿点云校正方法，进行三维位姿获取，弥补了传统二维检测方法的不足，由此来获取环境物体三维空间位姿数据信息。
5.本发明具体技术方案为：
6.一种适用于医院场景下移动机器人空间位姿点云校正方法，其特征在于：
7.包括以下步骤：
8.步骤一：建立障碍物检测模型，定位误差点区域，确定校正对象；
9.步骤二：基于误差点区域，利用三维空间位姿数据算法获得障碍物三维空间位姿数据；
10.步骤三：基于改进后的em算法估算出人为校正因子的参数；
11.步骤四：基于人为校正因子参数，利用可视化地图编辑工具进行地图修正。
12.步骤一具体包括以下步骤：
13.s101，基于栅格地图建模方法，定义地图上机器人的初始位置坐标为(x
rot
,y
rot
)，定义探测的障碍物的方向向量基于方向向量障碍物相对于机器人的角度值为θ，障碍
物与机器人点位之间的距离d，通过增加d值探测障碍物，当检测到方向向量上存在障碍物，则计算障碍物的观测值平面位姿数据，其中障碍物的二维坐标信息计算公式为式(1)：
[0014][0015]
(x
obstacle
,y
obstacle
)表示障碍物的二维坐标信息；调整θ角度，其中0
°
≤θ≤90
°
；更新方向向量调整距离d，探测其他障碍物点位的二维坐标信息；
[0016]
s102，读取激光雷达和里程计传感器数据，得到占据栅格地图，占据栅格地图的每一个单元格都存在一个占据可能性p，占据可能性p的取值范围是0≤p≤100，根据经验设定了空闲单元和占用单元边界值为pb＝65，其中单元格的单元状态判定标准公式为式(2)：
[0017][0018]
占用单元表示当前单元格为障碍物占据状态，空闲单元表示当前单元格为自由状态，没有识别为障碍物；通过此次判定，对栅格地图进行分类整理，有效识别障碍物点位信息。
[0019]
s103，当障碍物被检测到后，进行计数统计，统计机器人观测到的障碍物的二维坐标信息和障碍物数量；当检测到的障碍物数量达到设定的阈值，则表示该地图区域特征点分布明显，出现点云漂移现象可能想较低；相反，如果检测到的障碍物数量低于设定的阈值，则表示该地图区域特征点分布稀疏，有很大概率出现点云漂移现象，造成机器人与环境地图之间位姿关系模糊的问题，最终造成建造的环境地图不准确，误差较大，所述地图区域确定为误差点区域，校正对象就是误差点区域所包含的点云数据(点云表示的是具有坐标数据信息的数据点，本技术具体指的是激光雷达扫描环境中，反射回来的脉冲被接收器检测，利用传播时长计算出障碍物点云的坐标数据)。正是因为如此，该地图区域问题部分才会被系统筛选，成为重点关注和校正的对象。通过此障碍物检测模型，我们就可以实现对误差点位的快速定位，优化系统对故障点的响应效率。
[0020]
步骤二具体包括以下步骤：
[0021]
s201，基于卷积神经网络对相机获取的图像数据进行二维目标检测,
[0022]
s202，为了降低激光雷达噪声干扰，提升初始数据的有效性，对激光雷达获取的点云数据进行数据过滤，数据过滤函数为式(3)：
[0023]dmax
＝d
avg
+λ
×
s(3)
[0024]davg
表示每一个点云与邻近的点云之间距离的平均值；d
max
表示点云之间的最大距离值；λ是标准偏差系数，s为标准偏差，用来表示点云分布的分散程度。邻近点云的数量选取是根据点云数据的密集层度和点云个数来决定的。这里的λ，是通过置信度区间来选定的。本发明通过设计点云数据过滤器，达到对原始点云数据进行降噪地目的。
[0025]
s203，考虑到相机采集的图像和激光雷达的点云数据格式的不同，在利用图像数据获取对应的雷达数据时候，本专利利用投影矩阵，提取二维范围框(二维范围框指的是对图片进行的平面识别，并用矩形框来界定物体与环境，进行范围框定。)所对应的降噪之后的点云数据q。
[0026]
s204，考虑到利用转换矩阵得出的二维范围框内点云数据对环境可能存在对环境
信息还原不完整的现象。所以本专利对提取的二维范围框内点云数据进行了数据补充。点云数据补充是通过点云数据的旋转来实现的，增加数据样本的丰富度。具体是将点云数据q，进行旋转矩阵变换，得到旋转后的点云数据q
′
，变换后的数据样本的数量n是根据每次旋转角度α来设定的。旋转后的点云数据q
′
计算公式为式(4)：
[0027][0028]
n＝π/α
[0029]
由此可以对点云数据进行扩充，增加数据样本数量，提升了检测系统稳定性。
[0030]
因为二维范围框的界定直接影响着后续点云数据的提取。因此，本专利在现有技术基础上，通过调整范围框长宽比例，增加范围框覆盖区域，提升点云提取的容错率，尽可能地避免激光点云数据的丢失，确保了三维位姿获取的准确性能。
[0031]
调整范围框长宽比例，增加范围框覆盖区域，使得其囊括更多的点云。
[0032]
s205，通过点云网络结构获取障碍物的三维空间位姿数据。所述点云网络架构，本专利设定输入数据为提取后的三维点云数据信息，用[x,y,z]来表示三维空间位姿数据，经过转换网络(转换网络是用来提升数据维度的)，将点云进行多维度提升处理，获得点云的多维数据，达到1024维度。经过池化，将点云的多维数据维度降低到6维度，用g＝[x,y,z,l,w,h] 来表示障碍物空间位姿数据，其中x,y,z,l,w,h分别表示的三维位姿获取点云网络结构识别出的障碍物物体质心坐标以及长宽高信息，至此障碍物的障碍物空间位姿数据就被清晰的描述出来。
[0033]
基于机器人观测到的三维空间位姿数据信息，利用em(exception maxinization)算法近似估算人为校正因子参数，人为校正因子指的是人为输入位姿值，以校正位姿偏差。机器人得到障碍物三维空间位姿数据g＝[x,y,z,l,w,h]后，利用em算法进行近似的人为校正。
[0034]
步骤三具体包括以下步骤，
[0035]
s301,利用完全数据，定义出新的似然函数。
[0036]
在医院环境中，障碍物空间位姿数据g＝[x,y,z,l,w,h]服从某种概率分布，但是存在部分缺失，用s来表示缺失的位姿数据，完全数据表示为(g,s)。参数β表示未知参数，参数β表示人为输入的位姿值，取值范围是在实数范围内取值。p(g,β|(g,s))表示的是完全数据下，空间位姿数据概率密度函数。通过概率公式推导出基于完全数据的似然函数为式(5)：
[0037][0038]
这里的似然函数是通过观测值后验概率密度来表述得。em算法通过将概率数理统计与似然函数相结合，在实验方案的合理性以及过程的易操作性方面取得了一定的增益效果。避免了原本复杂庞大的数据量计算过程，节省了计算时间，可以使得结果快速收敛。
[0039]
s302,在障碍物空间位姿数据g和估计值参数β已知的前提下，对上述对数似然函数进行期望求解；
[0040]
s303,基于所述期望，进行参数β的求解，获得能够使得似然函数期望值最大的β，
能够使得期望值最大的β值是校正因子。
[0041]
经过这三步，就可以完成一次完整的em算法迭代过程。接着，可继续返回e-step步骤，进行循环求解，直到满足条件为止。至此，最终使得似然函数收敛的参数值β
final
就可以得出，人为校正因子即可被近似估算出来。
[0042]
步骤四具体包括以下步骤：
[0043]
本专利基于python语言，开发了可视化地图编辑工具。首先是调用了python库中的 tkinter模块，基于“tk()函数”生成可视化窗口界面，基于title()函数进行窗口标题文字的编写，本专利设定的窗口标题文字为“地图编辑器”。
[0044]
基于geomotry()函数调整窗口的显示面积，基于insert()函数再指定位置插入所需文本，例如本专利所用到的文本名为添加、清除、导出、功能区、点位等。
[0045]
基于delete()函数用来清空文本框内容，对于输错的内容进行删除修改。get()用来读取文本框内容，方便后续函数数值地传递。
[0046]
设置单选按钮，用选择在地图中具体是加入哪些功能点位或者是分区，比如点位有路径点、工位点、充电点，分区有禁行区、货架区、密集行人区等。单选按钮是通过button() 函数来添加的。菜单下拉框里面版含量一系列命令分组，目的是方便用户进行使用,用到的函数为menu()、add_command()、add_cascade()。针对可视化界面的工具栏的布局工作，本专利使用的函数为grid()。在行数和列数上进行设置，从而改变按钮摆放位置。点云位姿参数的调整是通过在输入框中键入二维坐标信息，从而达到点位校准的目的。
[0047]
一种适用于医院场景下移动机器人空间位姿点云校正系统，包括相机、激光雷达、里程计传感器、校正对象确认单元、三维空间建模单元、校正因子估算单元和修正单元；
[0048]
校正对象确认单元建立障碍物检测模型，定位误差点区域，确定校正对象；
[0049]
三维空间建模单元基于误差点区域，利用三维空间位姿数据算法获得障碍物三维空间位姿数据；
[0050]
校正因子估算单元基于改进后的em算法估算出人为校正因子的参数；
[0051]
修正单元基于人为校正因子参数，利用可视化地图编辑工具进行地图修正。
[0052]
校正对象确认单元具体工作过程包括以下步骤：
[0053]
s101，基于栅格地图建模方法，定义地图上机器人的初始位置坐标为(x
rot
,y
rot
)，定义探测的障碍物的方向向量基于方向向量障碍物相对于机器人的角度值为θ，障碍物与机器人点位之间的距离d，通过增加d值探测障碍物，当检测到方向向量上存在障碍物，则计算障碍物的观测值平面位姿数据，其中障碍物的二维坐标信息计算公式为式(1)：
[0054][0055]
(x
obstacle
,y
obstacle
)表示障碍物的二维坐标信息；调整θ角度，其中0
°
≤θ≤90
°
；更新方向向量调整距离d，探测其他障碍物点位的二维坐标信息；
[0056]
s102，读取激光雷达和里程计传感器数据，得到占据栅格地图，占据栅格地图的每一个单元格都存在一个占据可能性p，占据可能性p的取值范围是0≤p≤100，设定了空闲单元和占用单元边界值为pb＝65。其中单元格的单元状态判定标准公式为式(2)：
[0057][0058]
占用单元表示当前单元格为障碍物占据状态，空闲单元表示当前单元格为自由状态；
[0059]
s103，在一个单元格的地图区域，当障碍物被检测到后，进行计数统计，统计机器人观测到的障碍物的二维坐标信息和障碍物数量；当检测到的障碍物数量达到设定的阈值，表示所述地图区域特征点分布明显；相反，如果检测到的障碍物数量低于设定的阈值，则表示所述地图区域特征点分布稀疏，所述地图区域确定为误差点区域，校正对象是误差点区域所包含的点云数据。
[0060]
三维空间建模单元包括激光点云数据过滤器，具体工作过程包括以下步骤：
[0061]
s201，基于卷积神经网络对相机获取的图像数据进行二维目标检测；
[0062]
s202，对激光雷达获取的点云数据进行数据过滤，激光点云数据过滤器用于删除点云噪声，激光点云数据过滤器的数据过滤函数为式(3)：
[0063]dmax
＝d
avg
+λ
×sꢀꢀꢀ
(3)
[0064]davg
表示每一个点云与邻近的点云之间距离的平均值；d
max
表示点云之间的最大距离值；λ是标准偏差系数，s为标准偏差，用来表示点云分布的分散程度；
[0065]
s203，在利用图像数据获取对应的雷达数据时，利用投影矩阵，提取二维范围框所对应的降噪之后的点云数据q；
[0066]
s204，将点云数据q进行旋转矩阵变换，得到旋转后的点云数据q
′
，变换后的数据样本的数量n是根据每次旋转角度α来设定的，旋转后的点云数据q
′
计算公式为式(4)：
[0067][0068]
n＝π/α
[0069]
通过调整范围框长宽比例，增加范围框覆盖区域；
[0070]
s205，通过点云网络结构获取障碍物的三维空间位姿数据，用[x,y,z]来表示三维空间位姿数据，经过转换网络将点云进行多维度提升处理，获得点云的多维数据，经过池化，将点云的多维数据维度降低到6维度，用g＝[x,y,z,l,w,h]来表示障碍物空间位姿数据，其中x,y,z,l,w,h分别表示的三维位姿获取点云网络结构识别出的障碍物物体质心坐标以及长宽高信息。
[0071]
校正因子估算单元工作过程具体包括以下步骤，
[0072]
s301，定义s表示障碍物空间位姿数据g＝[x,y,z,l,w,h]中缺失的位姿数据，完全数据表示为(g,s)，参数β表示未知参数，参数β表示人为输入的位姿值，取值范围是在实数范围内取值。p(g,β|(g,s))表示的是完全数据下，空间位姿数据概率密度函数。通过概率公式推导出基于完全数据的似然函数为式(5)：
[0073][0074]
s302,在障碍物空间位姿数据g和估计值参数β已知的前提下，对对数似然函数进
行期望求解；
[0075]
s303,基于所述期望，进行参数β的求解，获得能够使得似然函数期望值最大的β，能够使得期望值最大的β值是校正因子。
[0076]
修正单元实现调整修正地图，具体包括以下步骤：
[0077]
s401，调用python库中的tkinter模块，基于“tk()函数”生成可视化窗口界面，基于 title()函数进行窗口标题文字的编写；
[0078]
s402，基于geomotry()函数调整窗口的显示面积，基于insert()函数指定位置插入所需文本；
[0079]
s403，基于delete()函数用来清空文本框内容，对于输错的内容进行删除修改，get()用来读取文本框内容；
[0080]
s404，设置单选按钮，单选按钮是通过button()函数来添加；基于menu()、add_command() 和add_cascade()函数生产菜单下拉框；基于grid()函数实现可视化界面的工具栏的布局。
[0081]
本发明的有益效果在于：
[0082]
本发明公开一种适用于医院场景下移动机器人空间位姿点云校正方法，本技术在进行误差定位点确定之前，对整个医院复杂的环境地图进行一个建模工作，主要目的是将现实环境中的物理地图通过计算机数据化存储的形式转化成抽象的矢量地图，实现了从真实物理环境到数字化矢量地图的映射过程。本发明采用栅格地图建模方法，因为栅格地图有着简易高效，能够清晰显示各个观测点的坐标信息，方便坐标系建模。
[0083]
本技术步骤二中，相比于传统二维数据对环境描述存在空间信息数据缺失的情况，本发明利用传感器获取的激光和图像数据进行三维位姿获取，弥补了传统二维检测方法的不足，由此来获取环境物体三维空间位姿数据信息，本发明通过点云数据过滤，达到对原始点云数据进行降噪地目的，对点云数据进行扩充，增加数据样本数量，提升了检测系统稳定性，通过调整范围框长宽比例，增加范围框覆盖区域，提升点云提取的容错率，尽可能地避免激光点云数据的丢失，确保了三维位姿获取的准确性能。
[0084]
本技术利用完全数据，定义出新的似然函数，本技术的似然函数是通过观测值后验概率密度来表述。
[0085]
本技术技术人员可以实时查看编辑地图的效果，整个操作方便快捷，给地图编辑工作带来了很大的便利。
[0086]
针对现有技术的不足，本发明的目的在于提供一种适用于医院场景下移动机器人空间位姿点云校正方法，该方法避免了在初始化定义机器人位姿关系的时候出现误差较大，耗时较长的问题，提供了快速定位误差点的障碍物检测模型，以及可视化的人为校正地图编辑界面。本发明提出的应用于医院复杂场景下的slam建图改进方法保证了agv再复杂场景中建图的准确性和高效性以及技术人员对于地图校正操作的便捷性，简化了操作流程。
附图说明
[0087]
图1为本发明实施例提供的一种适用于医院场景下移动机器人空间位姿点云校正方法流程图；
[0088]
图2为本发明实施例提供的一种面向医院复杂场景的障碍物检测模型流程图；
[0089]
图3为本发明实施例提供的一种面向医院复杂场景的空间三维位姿数据获取流程图；
[0090]
图4为本发明实施例提供的一种应用em算法确定校正因子参数流程图。
具体实施方式
[0091]
为使本发明的目的、技术方案、优点更加清楚，下面结合附图对本发明的具体实施方式进行详细说明。
[0092]
以下将结合本发明的附图，对本发明实施例中的技术方案进行清楚、完整的描述和讨论，显然，这里所描述的仅仅是本发明的一部分实例，并不是全部的实例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
[0093]
如图1所示，为了使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0094]
本发明提出了一种适用于医院场景下移动机器人空间位姿点云校正方法，主要是分为三个步骤进行。具体流程图如图1所示。首先，利用障碍物检测模型筛选出误差点区域。依据障碍物多的环境特征点较多，这样机器人与环境地图的位姿数量关系就较为清晰。但是，对于障碍物较少的地方，位姿关系就较为模糊，此区域优先进行误差标记。其次，利用em算法近似估算人为校正因子参数，这里结合了em算法的优势，通过em算法快速收敛得到人为校正因子参数。最后，开发可视化界面进行地图编辑。为的是方便技术人员快捷操作，缩短整个人为回环slam建图算法时长，提升整个系统的高效性。
[0095]
实施例1
[0096]
本实施例公开了如图2所示的一种面向医院复杂场景的障碍物检测模型流程图，从而方便系统快速定位误差点区域。首先，在进行误差定位点确定之前，需要对整个医院复杂的环境地图进行一个建模工作，实现了从真实物理环境到数字化矢量地图的映射过程。本发明采用正方形的栅格地图进行数字地图建模工作。在正方形栅格地图中，每一个单元格有两种状态显示，当该单元格占据率为1时，则表示该单元格为障碍物，并标记为黑色栅格；当该单元格占据率为0时，则表示该单元格为没有障碍物，并标记为空白栅格。
[0097]
在障碍物的检测判断方面，一些细则具体阐述如下：通过读取激光雷达及里程计传感器数据，得到一个占据栅格地图。每一个单元格都存在一个占据可能性p，整个p的取值范围是0≤p≤100，人为根据经验值设定了空闲单元和占用单元边界值为pb＝65。占用单元表示当前单元格为障碍物占据状态，空闲单元表示当前单元格为自由状态，没有识别为障碍物。通过此次判定，就可以对栅格地图进行分类整理，有效识别障碍物点位信息。
[0098]
基于上述栅格地图建模方法，定义地图上机器人的初始位置节点坐标为(x
rot
,yrot
)，接着定义需要探测的障碍物的方向向量基于此方向向量，设定待检测的障碍物相对于机器人的角度值为θ，并且检测的距离为d(是变化的)，一旦检测到该方向向量上存在障碍物则会计算障碍物点位信息，具体计算公式如下所示：这里的 (x
obstacle
,y
obstacle
)表示的是障碍物的坐标信息。接下来，继续调整θ角度，进行方向向量更新。在此基础上继续调整变量d，探测其余障碍物点位坐标信息。
[0099]
当障碍物被检测到后，系统会自动执行计数指令，统计机器人观测到的障碍物坐标信息以及障碍物数量。当检测到的障碍物数量达到一定的阈值，则表示该地图区域特征点分布明显，出现点云漂移现象可能想较低；相反，如果传感器检测到的障碍物数量低于设定的阈值，则表示该地图区域特征点分布稀疏，有很大概率出现点云漂移现象，造成机器人与环境地图之间位姿关系模糊的问题，最终造成建造的环境地图不准确，误差较大。正是因为如此，该地图问题部分才会被系统筛选，成为重点关注和校正的对象。
[0100]
实施例2
[0101]
本实施例公开了如图3所示的一种面向医院复杂环境的空间三维位姿数据获取流程图，相比于传统二维数据对环境描述存在空间信息数据缺失的情况，本发明利用传感器获取的激光和图像数据进行三维位姿获取，本发明利用传感器获取的激光和图像数据进行三维位姿获取，弥补了传统二维检测方法的不足，由此来获取环境物体三维空间位姿数据信息。
[0102]
一种适用于医院场景下移动机器人空间位姿点云校正方法，其特征在于：
[0103]
包括以下步骤：
[0104]
步骤一：建立障碍物检测模型，定位误差点区域，确定校正对象；
[0105]
步骤二：基于误差点区域，利用三维空间位姿数据算法获得障碍物三维空间位姿数据；
[0106]
步骤三：基于改进后的em算法近似估算出人为校正因子的参数；
[0107]
步骤四：基于基于人为校正因子参数，利用可视化地图编辑工具进行地图修正。
[0108]
步骤一包括以下步骤：
[0109]
s101，基于栅格地图建模方法，定义地图上机器人的初始位置坐标为(x
rot
,y
rot
)，定义探测的障碍物的方向向量基于方向向量待检测的障碍物相对于机器人的角度值为θ，检测障碍物与机器人点位之间的距离d，通过增加d值探测障碍物，当检测到方向向量上存在障碍物，则计算障碍物的观测值平面位姿数据，其中障碍物的二维坐标信息计算公式为式(1)：
[0110][0111]
(x
obstacle
,y
obstacle
)表示障碍物的二维坐标信息；调整θ角度，其中0
°
≤θ≤90
°
，更新方向向量调整距离d，探测其他障碍物点位的二维坐标信息；
[0112]
s102，通过约束条件检测判断障碍物：(激光雷达获取的是障碍物的点云数据，里程计获取的是机器人的位姿和速度信息，是用来机器人建图的。)
[0113]
读取激光雷达和里程计传感器数据，得到占据栅格地图，占据栅格地图的每一个单元格都存在一个占据可能性p，占据可能性p的取值范围是0≤p≤100，根据经验设定了空闲单元和占用单元边界值为pb＝65；
[0114][0115]
占用单元表示当前单元格为障碍物占据状态，空闲单元表示当前单元格为自由状态，没有识别为障碍物；通过此次判定，对栅格地图进行分类整理，有效识别障碍物点位信息。
[0116]
s103，当障碍物被检测到后，进行计数统计，统计机器人观测到的障碍物的二维坐标信息和障碍物数量；当检测到的障碍物数量达到设定的阈值，则表示该地图区域特征点分布明显，出现点云漂移现象可能想较低；相反，如果检测到的障碍物数量低于设定的阈值，则表示该地图区域特征点分布稀疏，有很大概率出现点云漂移现象，造成机器人与环境地图之间位姿关系模糊的问题，最终造成建造的环境地图不准确，误差较大，所述地图区域确定为误差点区域，校正对象就是误差点区域所包含的点云数据(点云表示的是具有坐标数据信息的数据点，本技术具体指的是激光雷达扫描环境中，反射回来的脉冲被接收器检测，利用传播时长计算出障碍物点云的坐标数据)。正是因为如此，该地图区域问题部分才会被系统筛选，成为重点关注和校正的对象。通过此障碍物检测模型，我们就可以实现对误差点位的快速定位，优化系统对故障点的响应效率。
[0117]
步骤s102判定栅格状态是占用还是空闲，通过此来确定需要校正的对象。校正对象指的是障碍物数量低于设定的阈值的地图区域，因为障碍物数量越少，特征点分布就可能会会因为稀疏，从而造成建图准确度降低的情况。)
[0118]
简单理解，校正对象就是误差点区域所包含的点云，因为这些区域所包含的激光点云存在大概率的漂移现象。误差点区域就是障碍物数量低于设定阈值的地图区域。
[0119]
本技术在进行误差定位点确定之前，对整个医院复杂的环境地图进行一个建模工作，此工作流程是重要的环节，主要目的是将现实环境中的物理地图通过计算机数据化存储的形式转化成抽象的矢量地图，实现了从真实物理环境到数字化矢量地图的映射过程。本发明采用栅格地图建模方法，因为栅格地图有着简易高效，能够清晰显示各个观测点的坐标信息，方便坐标系建模。
[0120]
本实施例采用正方形的占据栅格地图进行数字地图建模工作。在正方形的占据栅格地图中，每一个单元格有两种状态显示，当该单元格占据率为1时，则表示该单元格为障碍物，并标记为黑色栅格；当该单元格占据率为0时，则表示该单元格为没有障碍物，并标记为空白栅格。为了提升医疗服务运输机器人工作的稳定性，本发明利用栅格地图建模方法将医院物理场景地图进行改造，可以有效提升机器人利用slam算法建图的效率。
[0121]
基于上述栅格地图建模方法，定义地图上机器人的初始位置节点坐标为(x
rot
,y
rot
)，接着定义需要探测的障碍物的方向向量基于此方向向量，设定待检测的障碍物相对于机器人的角度值为θ，并且检测的距离为d(是变化的)，一旦检测到该方向向量上存在障碍物则会计算障碍物点位信息，具体计算公式为式(1)：
[0122][0123]
这里的(x
obstacle
,y
obstacle
)表示的是障碍物的坐标信息
。
接下来，继续调整θ角度，其中 0
°
≤θ≤90
°
，进行方向向量更新。在此基础上继续调整变量d，探测其余障碍物点位坐标信息。
[0124]
更进一步的，再障碍物的检测判断方面，本专利进行了一些细则约束，具体阐述如下：
[0125]
通过读取激光雷达及里程计传感器数据，得到一个占据栅格地图。每一个单元格都存在一个占据可能性p，整个p的取值范围是0≤p≤100，人为根据经验值设定了空闲单元和占用单元边界值为pb＝65。
[0126][0127]
占用单元表示当前单元格为障碍物占据状态，空闲单元表示当前单元格为自由状态，没有识别为障碍物。通过此次判定，就可以对栅格地图进行分类整理，有效识别障碍物点位信息。当障碍物被检测到后，系统会自动执行计数指令，统计机器人观测到的障碍物坐标信息以及障碍物数量。当检测到的障碍物数量达到一定的阈值，则表示该地图区域特征点分布明显，出现点云漂移现象可能想较低；相反，如果传感器检测到的障碍物数量低于设定的阈值，则表示该地图区域特征点分布稀疏，有很大概率出现点云漂移现象，造成机器人与环境地图之间位姿关系模糊的问题，最终造成建造的环境地图不准确，误差较大。正是因为如此，该地图问题部分才会被系统筛选，成为重点关注和校正的对象。通过此障碍物检测模型，我们就可以实现对误差点位的快速定位，优化系统对故障点的响应效率。
[0128]
机器人在利用激光雷达，里程计传感器设备进行误差点确认的时候，接入占据可能性对扫描出的点云地图进行特征点数量判别。因为地图中特征点较多的地方，建造出的点云地图精确度一般来说是比较高的，不会出现点云漂移现象，但是特征点较少的部分就会容易出现点云漂移现象，导致建造的地图偏差较大。利用此方法可以降低系统数据加载量过大的问题，便于快速确定误差点位。
[0129]
上述步骤一对误差点区域进行了初步定位，接下来就涉及到系统对该问题的相应处理。步骤二中，相比于传统二维数据对环境描述存在空间信息数据缺失的情况，本发明利用传感器获取的激光和图像数据进行三维位姿获取，弥补了传统二维检测方法的不足，由此来获取环境物体三维空间位姿数据信息。
[0130]
步骤二具体包括，包括以下步骤：
[0131]
s201，基于卷积神经网络对相机获取的图像数据进行二维目标检测，目的是更快地提取障碍物的二维范围框，这也是整个三维位姿获取的初始工作。
[0132]
s202，为了降低激光雷达噪声干扰，提升初始数据的有效性，对激光雷达获取的点云数据进行数据过滤，本技术设计了激光点云数据过滤器，用于删除点云噪声，简化系统的处理计算量。激光点云数据过滤器的数据过滤函数为式(3)：
[0133]dmax
＝d
avg
+λ
×sꢀꢀꢀ
(3)
[0134]davg
表示每一个点云与邻近的点云之间距离的平均值；d
max
表示点云之间的最大距
离值；λ是标准偏差系数，s为标准偏差，用来表示点云分布的分散程度。邻近点云的数量选取是根据点云数据的密集层度和点云个数来决定的。这里的λ，是通过置信度区间来选定的。本发明通过设计点云数据过滤器，达到对原始点云数据进行降噪地目的。
[0135]
s203，考虑到相机采集的图像和激光雷达的点云数据格式的不同，在利用图像数据获取对应的雷达数据时候，本专利利用投影矩阵，提取二维范围框(二维范围框指的是对图片进行的平面识别，并用矩形框来界定物体与环境，进行范围框定。)所对应的降噪之后的点云数据q。投影矩阵是kitti数据集中提供的转换矩阵，此处不做具体介绍。
[0136]
s204，考虑到利用转换矩阵得出的二维范围框内点云数据对环境可能存在对环境信息还原不完整的现象。所以本专利对提取的二维范围框内点云数据进行了数据补充。点云数据补充是通过点云数据的旋转来实现的，增加数据样本的丰富度。具体是将点云数据q，进行旋转矩阵变换，得到旋转后的点云数据q
′
，变换后的数据样本的数量n是根据每次旋转角度α来设定的。旋转后的点云数据q
′
计算公式为式(4)：
[0137][0138]
n＝π/α
[0139]
由此可以对点云数据进行扩充，增加数据样本数量，提升了检测系统稳定性。
[0140]
因为二维范围框的界定直接影响着后续点云数据的提取。因此，本专利在现有技术基础上，通过调整范围框长宽比例，增加范围框覆盖区域，提升点云提取的容错率，尽可能地避免激光点云数据的丢失，确保了三维位姿获取的准确性能。
[0141]
调整范围框长宽比例，增加范围框覆盖区域，使得其囊括更多的点云。
[0142]
s205，通过点云网络结构获取障碍物的三维空间位姿数据。所述点云网络架构，本专利设定输入数据为提取后的三维点云数据信息，用[x,y,z]来表示三维空间位姿数据，经过转换网络(转换网络是用来提升数据维度的)，将点云进行多维度提升处理，获得点云的多维数据，达到1024维度。经过池化，将点云的多维数据维度降低到6维度，用g＝[x,y,z,l,w,h] 来表示，其中x,y,z,l,w,h分别表示的三维位姿获取点云网络结构识别出的障碍物物体质心坐标以及长宽高信息，至此障碍物的三维空间位姿信息就被清晰的描述出来。
[0143]
基于机器人观测到的三维空间位姿数据信息，利用em(exception maxinization)算法近似估算人为校正因子参数，人为校正因子指的是人为输入位姿值，以校正位姿偏差。机器人得到障碍物空间位姿数据g＝[x,y,z,l,w,h]后，利用em算法进行近似的人为校正。
[0144]
步骤三具体包括以下步骤，
[0145]
s301，,利用完全数据，定义出新的似然函数。
[0146]
在医院环境中，障碍物空间位姿数据g＝[x,y,z,l,w,h]服从某种概率分布，但是存在部分缺失，用s来表示缺失的位姿数据，完全数据表示为(g,s)。参数β表示未知参数，参数β表示人为输入的位姿值，取值范围是在实数范围内取值。p(g,β|(g,s))表示的是完全数据下，空间位姿数据概率密度函数。通过概率公式推导出基于完全数据的似然函数为式(5)：
[0147]
[0148]
这里的似然函数是通过观测值后验概率密度来表述得。em算法通过将概率数理统计与似然函数相结合，在实验方案的合理性以及过程的易操作性方面取得了一定的增益效果。避免了原本复杂庞大的数据量计算过程，节省了计算时间，可以使得结果快速收敛。
[0149]
s302,在障碍物空间位姿数据g＝[x,y,z,l,w,h]和估计值参数β已知的前提下，对上述对数似然函数进行期望求解；
[0150]
s303,基于所述期望，进行参数β的求解，获得能够使得似然函数期望值最大的β，能够使得期望值最大的β值是校正因子。
[0151]
经过这三步，就可以完成一次完整的em算法迭代过程。接着，可继续返回e-step步骤，进行循环求解，直到满足条件为止。至此，最终使得似然函数收敛的参数值β
final
就可以得出，人为校正因子即可被近似估算出来。
[0152]
实施例3
[0153]
本实施例公开了如图4所示的一种应用em算法确定校正因子参数流程图，利用em 算法近似估算人为校正因子参数，人为校正因子指的是人为输入位姿值，以校正偏差。算法每一次迭代过程涉及到三个步骤：(1)利用完全数据，定义出新的似然函数。(2)在观测值位姿数据g和估计值参数β已知的前提下，对上述对数似然函数进行期望求解；(3)基于所述期望，进行关于变量β的求解，获得能够使得期望值最大的β，能够使得期望值最大的β值就是所求的校正因子。至此，最终使得似然函数收敛的参数值β
final
就可以得出，人为校正因子即可被近似估算出来。
[0154]
实施例4
[0155]
本实施例还公开了一种面向医院复杂场景地图编辑器，其中地图编辑器可视化界面主要包括：管理员账户验证界面、地图当前状态显示界面以及工具栏。管理员账户验证界面，是给技术人员获取编辑地图的权限所开发的。账户登陆界面主要包含内容为账户名输入、密码以及密码找回这三块。初始阶段，系统开发人员前期在服务器数据库中会将技术人员信息导入。考虑到突发情况，技术人员在忘记密码时候也可以通过密码找回功能自己自行修改密码，并上传数据库更新自己的账户信息，从而获取最新的登陆权限。整个界面上部分工具栏分为文件栏、绘图工具栏两大部分，工具栏下方区域为地图显示区域。文件栏主要是用来拉取待编辑的地图或者导出已经经过人为校正后的地图。绘图工具栏包括一些绘图笔、橡皮擦、位姿数值调整以及路径点属性设置按钮。利用工具栏下方的显示区域，技术人员可以实时查看编辑地图的效果，整个操作方便快捷。由于本地图编辑器是基于web 网页开发的，可以跨平台使用，在ubuntu以及windows系统的电脑都可以使用，给地图编辑工作带来了很大的便利。
[0156]
步骤四具体包括以下步骤：
[0157]
本专利基于python语言，开发了可视化地图编辑工具。首先是调用了python库中的tkinter模块，基于“tk()函数”生成可视化窗口界面，基于title()函数进行窗口标题文字的编写，本专利设定的窗口标题文字为“地图编辑器”。
[0158]
基于geomotry()函数调整窗口的显示面积，基于insert()函数再指定位置插入所需文本，例如本专利所用到的文本名为添加、清除、导出、功能区、点位等。
[0159]
基于delete()函数用来清空文本框内容，对于输错的内容进行删除修改。get()
用来读取文本框内容，方便后续函数数值地传递。
[0160]
设置单选按钮，用选择在地图中具体是加入哪些功能点位或者是分区，比如点位有路径点、工位点、充电点，分区有禁行区、货架区、密集行人区等。单选按钮是通过button() 函数来添加的。菜单下拉框里面版含量一系列命令分组，目的是方便用户进行使用,用到的函数为menu()、add_command()、add_cascade()。针对可视化界面的工具栏的布局工作，本专利使用的函数为grid()。在行数和列数上进行设置，从而改变按钮摆放位置。点云位姿参数的调整是通过在输入框中键入二维坐标信息，从而达到点位校准的目的。
[0161]
基于步骤三中的参数，利用可视化地图编辑工具进行参数调整以修正地图。本发明在可视化界面开发中结合了模块化分层开发的思想。主要包括：管理员账户验证界面、地图当前状态显示界面以及工具栏。
[0162]
进一步的，所述管理员账户验证界面，是给技术人员获取编辑地图的权限所开发的。账户登陆界面主要包含内容为账户名输入、密码以及密码找回这三块。初始阶段，系统开发人员前期在服务器数据库中会将技术人员信息导入。考虑到突发情况，技术人员在忘记密码时候也可以通过密码找回功能自己自行修改密码，并上传数据库更新自己的账户信息，从而获取最新的登陆权限。需要注意的是，修正地图是至关重要的环节，必须有专业人员进行调试，所以此界面是面向技术人员开发的跨平台登陆界面，并没有面向普通客户。
[0163]
一种适用于医院场景下移动机器人空间位姿点云校正系统，包括相机、激光雷达、里程计传感器、校正对象确认单元、三维空间建模单元、校正因子估算单元和修正单元；
[0164]
校正对象确认单元建立障碍物检测模型，定位误差点区域，确定校正对象；
[0165]
三维空间建模单元基于误差点区域，利用三维空间位姿数据算法获得障碍物三维空间位姿数据；
[0166]
校正因子估算单元基于改进后的em算法估算出人为校正因子的参数；
[0167]
修正单元基于人为校正因子参数，利用可视化地图编辑工具进行地图修正。
[0168]
校正对象确认单元具体工作过程包括以下步骤：
[0169]
s101，基于栅格地图建模方法，定义地图上机器人的初始位置坐标为(x
rot
,y
rot
)，定义探测的障碍物的方向向量基于方向向量障碍物相对于机器人的角度值为θ，障碍物与机器人点位之间的距离d，通过增加d值探测障碍物，当检测到方向向量上存在障碍物，则计算障碍物的观测值平面位姿数据，其中障碍物的二维坐标信息计算公式为式(1)：
[0170][0171]
(x
obstacle
,y
obstacle
)表示障碍物的二维坐标信息；继续调整θ角度，其中0
°
≤θ≤90
°
，更新方向向量调整距离d，探测其他障碍物点位的二维坐标信息；
[0172]
s102，读取激光雷达和里程计传感器数据，得到占据栅格地图，占据栅格地图的每一个单元格都存在一个占据可能性p，占据可能性p的取值范围是0≤p≤100，设定了空闲单元和占用单元边界值为pb＝65。其中单元格的单元状态判定标准公式为式(2)：
[0173]
[0174]
占用单元表示当前单元格为障碍物占据状态，空闲单元表示当前单元格为自由状态；
[0175]
s103，在一个单元格的地图区域，当障碍物被检测到后，进行计数统计，统计机器人观测到的障碍物的二维坐标信息和障碍物数量；当检测到的障碍物数量达到设定的阈值，表示所述地图区域特征点分布明显；相反，如果检测到的障碍物数量低于设定的阈值，则表示所述地图区域特征点分布稀疏，所述地图区域确定为误差点区域，校正对象是误差点区域所包含的点云数据。
[0176]
三维空间建模单元包括激光点云数据过滤器，具体工作过程包括以下步骤：
[0177]
s201，基于卷积神经网络对相机获取的图像数据进行二维目标检测；
[0178]
s202，对激光雷达获取的点云数据进行数据过滤，激光点云数据过滤器用于删除点云噪声，激光点云数据过滤器的数据过滤函数为式(3)：
[0179]dmax
＝d
avg
+λ
×sꢀꢀꢀ
(3)
[0180]davg
表示每一个点云与邻近的点云之间距离的平均值；d
max
表示点云之间的最大距离值；λ是标准偏差系数，s为标准偏差，用来表示点云分布的分散程度；
[0181]
s203，在利用图像数据获取对应的雷达数据时，利用投影矩阵，提取二维范围框所对应的降噪之后的点云数据q；
[0182]
s204，将点云数据q进行旋转矩阵变换，得到旋转后的点云数据q
′
，变换后的数据样本的数量n是根据每次旋转角度α来设定的，旋转后的点云数据q
′
计算公式为式(4)：
[0183][0184]
n＝π/α
[0185]
通过调整范围框长宽比例，增加范围框覆盖区域；
[0186]
s205，通过点云网络结构获取障碍物的三维空间位姿数据，用[x,y,z]来表示三维空间位姿数据，经过转换网络将点云进行多维度提升处理，获得点云的多维数据，经过池化，将点云的多维数据维度降低到6维度，用g＝[x,y,z,l,w,h]来表示障碍物空间位姿数据，其中x,y,z,l,w,h分别表示的三维位姿获取点云网络结构识别出的障碍物物体质心坐标以及长宽高信息。
[0187]
校正因子估算单元工作过程具体包括以下步骤，
[0188]
s301，定义s表示障碍物空间位姿数据g＝[x,y,z,l,w,h]中缺失的位姿数据，完全数据表示为(g,s)，参数β表示未知参数，参数β表示人为输入的位姿值，取值范围是在实数范围内取值。p(g,β|(g,s))表示的是完全数据下，空间位姿数据概率密度函数。通过概率公式推导出基于完全数据的似然函数为式(5)：
[0189][0190]
s302,在障碍物空间位姿数据g和估计值参数β已知的前提下，对对数似然函数进行期望求解；
[0191]
s303,基于所述期望，进行参数β的求解，获得能够使得似然函数期望值最大的β，能够使得期望值最大的β值是校正因子。
[0192]
修正单元实现调整修正地图，具体包括以下步骤：
[0193]
s401，调用python库中的tkinter模块，基于“tk()函数”生成可视化窗口界面，基于 title()函数进行窗口标题文字的编写；
[0194]
s402，基于geomotry()函数调整窗口的显示面积，基于insert()函数指定位置插入所需文本；
[0195]
s403，基于delete()函数用来清空文本框内容，对于输错的内容进行删除修改，get()用来读取文本框内容；
[0196]
s404，设置单选按钮，单选按钮是通过button()函数来添加；基于menu()、add_command() 和add_cascade()函数生产菜单下拉框；基于grid()函数实现可视化界面的工具栏的布局。
[0197]
进一步的，地图当前状态显示界面以及工具栏，是技术人员在进行身份认证之后所进入的界面。在此界面，可以看到整个界面上部分工具栏分为文件栏、绘图工具栏两大部分，工具栏下方区域为地图显示区域。文件栏主要是用来拉取待编辑的地图或者导出已经经过人为校正后的地图。绘图工具栏包括一些绘图笔、橡皮擦、位姿数值调整以及路径点属性设置按钮。利用工具栏下方的显示区域，技术人员可以实时查看编辑地图的效果，整个操作方便快捷，给地图编辑工作带来了很大的便利。
[0198]
在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下被实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
[0199]
类似地，应当理解，为了精简本公开并帮助理解各个发明方面中的一个或多个，在上面对本发明的示例性实施例的描述中，本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多特征。更确切地说，如权利要求书所反映的那样，发明方面在于少于前面公开的单个实施例的所有特征。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本发明的单独实施例。
[0200]
本领域那些技术人员应当理解在本文所公开的示例中的设备的模块或单元或组间可以布置在如该实施例中所描述的设备中，或者可替换地可以定位在与该示例中的设备不同的一个或多个设备中。前述示例中的模块可以组合为一个模块或者此外可以分成多个子模块。
[0201]
本领域那些技术人员可以理解，可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组间组合成一个模块或单元或组间，以及此外可以把它们分成多个子模块或子单元或子组间。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外，可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述，本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。
[0202]
此外，本领域的技术人员能够理解，尽管在此所述的一些实施例包括其它实施例
中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在下面的权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
[0203]
此外，所述实施例中的一些在此被描述成可以由计算机系统的处理器或者由执行所述功能的其它装置实施的方法或方法元素的组合。因此，具有用于实施所述方法或方法元素的必要指令的处理器形成用于实施该方法或方法元素的装置。此外，装置实施例的在此所述的元素是如下装置的例子：该装置用于实施由为了实施该发明的目的的元素所执行的功能。
[0204]
这里描述的各种技术可结合硬件或软件，或者它们的组合一起实现。从而，本发明的方法和设备，或者本发明的方法和设备的某些方面或部分可采取嵌入有形媒介，例如软盘、cd-rom、硬盘驱动器或者其它任意机器可读的存储介质中的程序代码(即指令)的形式，其中当程序被载入诸如计算机之类的机器，并被所述机器执行时，所述机器变成实践本发明的设备。
[0205]
在程序代码在可编程计算机上执行的情况下，计算设备一般包括处理器、处理器可读的存储介质(包括易失性和非易失性存储器和/或存储元件)，至少一个输入装置，和至少一个输出装置。其中，存储器被配置用于存储程序代码；处理器被配置用于根据该存储器中存储的所述程序代码中的指令，执行本发明的方法。
[0206]
以示例而非限制的方式，计算机可读介质包括计算机存储介质和通信介质。计算机可读介质包括计算机存储介质和通信介质。计算机存储介质存储诸如计算机可读指令、数据结构、程序模块或其它数据等信息。通信介质一般以诸如载波或其它传输机制等已调制数据信号来体现计算机可读指令、数据结构、程序模块或其它数据，并且包括任何信息传递介质。以上的任一种的组合也包括在计算机可读介质的范围之内。
[0207]
如在此所使用的那样，除非另行规定，使用序数词“第一”、“第二”、“第三”等等来描述普通对象仅仅表示涉及类似对象的不同实例，并且并不意图暗示这样被描述的对象必须具有时间上、空间上、排序方面或者以任意其它方式的给定顺序。
[0208]
尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。此外，应当注意，本说明书中使用的语言主要是为了可读性和教导的目的而选择的，而不是为了解释或者限定本发明的主题而选择的。因此，在不偏离所附权利要求书的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的，本发明的范围由所附权利要求书限定。
[0209]
以上仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。