在医用环境下基于2D激光雷达与相机的SLAM方法及系统

在医用环境下基于2d激光雷达与相机的slam方法及系统
技术领域
1.本发明属于机器人同时定位与建图(slam)技术领域，具体涉及一种在医用环境下基于2d激光雷达与相机的slam方法及系统。


背景技术：

2.slam实现过程可以描述为:在已有轮式编码器预测数据的情况下，凭借自身传感器信息，如：激光点云信息，视觉图像、深度信息等来实时确定自身的位姿并构建后续用于定位的地图信息，作为机器人导航的基础，slam已成为近年来机器人领域的热点研究问题，并被认为是实现自主机器人的核心环节。
3.自起步发展至今，机器人室内定位精度随着硬件获取观测信息频率的加快以及各类定位算法的产生而不断提高，在满足仓储、居家机器人所需精度后，slam的稳定性显得更为重要。然而单一的观测传感器由于其观测信息内容的固定，将其用于slam都存在着专属问题，例如目前室内2d激光slam的局限主要有以下两点：1、获取观测信息内容少，仅有一个平面，需确保机器人运行过程处于同一平面；2、对金属，玻璃等障碍物容易导致观测误差。其次使用单一的双目相机进行slam的局限主要有：1、对光照条件即为敏感；2、定位精度不理想。
4.室内医用护理机器人和仓储、居家机器人不同的是：1、医院重复性环境较多，容易导致激光点云数据误匹配；2、平缓的上下坡较多，导致激光观测信息严重突变；3、24h全自动服务更为关键，导致视觉观测信息不统一4、动态障碍物多且行动较快，需要设计额外硬件装置保证运行的安全性。
5.综上，单一的观测传感器一旦在医护环境下用于构建定位所需的栅格地图就会导致观测误差甚至定位误匹配，最终导致建图的失败。


技术实现要素：

6.为解决现有技术存在的单一的观测传感器一旦在医护环境下用于构建定位所需的栅格地图就会导致观测误差甚至定位误匹配，最终导致建图的失败的技术问题，本发明提供一种在医用环境下基于2d激光雷达与相机的slam方法及系统，解决了医护机器人在自主slam建图过程中，确保行人以及自身的安全。
7.本发明采用的技术方案是：
8.一种在医用环境下基于2d激光雷达与相机的slam方法，其特征在于，所述方法包括如下具体步骤：
9.s1:构建2d激光雷达与双目相机的联合标定算法；
10.s2:构建基于2d激光雷达的观测信息退化检测算法；
11.s3:构建基于2d激光雷达与双目相机的机器人里程计算法；
12.s4:构建包含z轴障碍物信息的优化栅格地图；
13.s5:构建基于2d激光雷达与双目相机的回环检测算法。
14.进一步的，在步骤s1中，为了避免激光雷达与双目相机标定误差的累计，采用基于平面约束的相机激光联合标定算法，先通过标定板平面的二维码计算得到标定板平面在相机坐标系下的坐标表示；同时，激光发出的光束落在标定板平面上，利用激光点在激光坐标系下的坐标和平面方程在相机坐标系下的坐标，构建点在平面上的约束从而求解外参数，即激光雷达与相机相对位姿。
15.进一步的，在步骤s2中，退化的情况分为两种，一种为机器人进行了除偏航角外的旋转动作而导致的观测信息退化，称为颠簸退化；另一种为机器人处于长廊环境下或因为扫描角度过小并且面向墙体从而导致观测信息为一条直线的情况，称为环境退化；
16.针对颠簸退化的检测方法具体如下：先计算由双目视觉里程计算法得到的前后帧相机位姿，再判断机器人的俯仰角与滚动角变化值是否大于设定阈值，若大于设定阈值，则观察由当前帧激光里程计修正后是否超过设定阈值，若超过，则判定该时刻发生了颠簸退化，记颠簸退化程度为τ1＝1，初始值τ1＝0；
17.针对环境退化的检测方法具体如下：采集当前帧点云数据，计算各个扫描点的法向量方向，再对法向量进行一致性分析，若一致性分析通过，则判定该时刻发生了环境退化，记环境退化程度为τ2，0≤τ2≤1。
18.进一步的，在步骤s3中，在识别观测信息退化的基础上，构建基于2d激光雷达与双目相机的机器人里程计算法包括如下步骤：
19.步骤s31：构建单独的2d激光雷达里程计匹配残差，具体包括：
20.步骤s311，实现传感器数据预处理部分；
21.步骤s312，生成距离栅格地图；
22.步骤s313，获取当前帧，点云匹配的残差量；
23.步骤s32：构建单独的双目里程计重投影误差，具体包括：
24.步骤s321，特征点匹配；
25.步骤s322，重投影误差e2计算；
26.步骤s33：双目相机与2d激光雷达紧耦合的融合，融合算法步骤具体如下：
27.步骤s331，在步骤s31的基础上获取联合误差函数；
28.步骤s332，进行残差优化。
29.进一步的，在步骤s4中，构建优化栅格地图信息过程具体包括：
30.在步骤s3获取的机器人联合里程计的基础上，将双目相机获取的稠密点云转化为八叉树地图，八叉树地图的形成过程需要满足的条件为：八叉树地图中体素有平面与栅格地图重叠或平行以及保持八叉树坐标系与栅格地图坐标系重叠；
31.形成八叉树地图后，对占用栅格地图中的栅格进行与高度有关的障碍物内容记载，每个占据栅格会记载有其z轴障碍物情况的向量。
32.进一步的，在步骤s5中，构建基于2d激光雷达与双目相机的回环检测算法，具体包括：
33.步骤s51：设定回环检测条件；
34.通过步骤s3中所得的联合里程计来判断是否进行回环检测，判断条件为：机器人运动距离到达水平阈值δd或机器人角度累计变化超过角度阈值δ
θ
；
35.步骤s52：进行回环检测，获取回环残差，通过最小化误差函数以进一步求得精确
位姿。
36.实施上述所述的一种基于2d激光雷达与相机同时定位与建图的方法的系统，其特征在于，所述系统包括：
37.机器人底盘，用于实现机器人的平移、旋转和转向功能；
38.传感器装配系统，用于感知周围环境和获取外部信息，为步骤s1至s5提供观测信息；
39.机械臂系统，用于实现物品的抓取、装配以及搬运动作；
40.人机交互系统，用以实现与机器人进行指令传递、信息交流和操作控制，实现人机共同工作和合作，
41.电源与控制系统，为整个系统提供能源和控制信号，保证机器人的长时间运行与协调各个部分工作。
42.与现有技术相比，本发明的有益效果体现在：本发明在判断2d雷达点云观测信息是否退化的基础上，加入视觉信息来确保复杂场景下的slam过程的稳定性，修改传统栅格框架保证回环优化的实时性，并配合全新硬件系统实现多动态环境下机器人与障碍物的安全性。
附图说明
43.图1是本发明方法的流程示意图。
44.图2是实施本发明方法的激光视觉联合标定示意图。
45.图3是实施本发明方法的颠簸退化的示意图。
46.图4a是实施本发明方法的环境退化，当2d激光雷达以360
°
的扫描角处于走廊环境下的示意图。
47.图4b是实施本发明方法的环境退化，当2d激光雷达以以180
°
的扫描角处于朝向墙壁的环境下的示意图。
48.图5是实施本发明方法的占据栅格地图。
49.图6是实施本发明方法的距离栅格地图。
50.图7是实施本发明方法的z轴障碍物信息示意图。
51.图8是实施本发明方法的医用护理机器人的示意图。
52.图9是实施本发明方法的医用护理机器人底盘的示意图。
53.图10是实施本发明方法的医用护理机器人的机械臂的示意图
54.图中，101-铝合金支撑底盘、102-主动轮、103-麦克纳姆从动轮、104-无刷电机、201-第一双目相机、202-2d激光雷达、203-力传感器、204-轮式编码器、301-机械臂搭载平台、302～308-旋转关节、309-第二双目相机、310-机械爪、401-人机应用界面、402-蓝牙音箱、403-力控交互装置、501-工控机、502-电源。
具体实施方式
55.以下结合附图对本发明实施例的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明实施例，并不用于限制本发明实施例。
56.需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相
互组合。
57.下面将参考附图并结合示例性实施例来详细说明本发明。
58.实施例1
59.参照图1，一种在医护环境下基于2d激光雷达与相机同时定位与建图的方法的工作流程图，如图所示，该流程为：
60.步骤s1：双目相机与激光雷达融合系统的联合标定如图2所示，所述第一、第二双目相机为小觅相机d1000-ir-120，双目的基线为120mm，最高帧率为60fps，分辨率为1280*720。2d激光雷达型号为倍加福r2000，扫描频率为40赫兹，一帧扫描点为1440个，测距范围为0.15-50m。激光点云会落在标定板上(图2中的黑色圆点)，可以得到激光点云在激光坐标系下的坐标，并且通过二维码(apriltag)可以获得标定板在相机坐标系下的平面方程，于是可以构建点在平面上的约束来求解激光雷达到相机的变换，即(r
cl
,t
cl
)，r
cl
为相机坐标系到雷达坐标系的旋转矩阵，t
cl
为平移向量。
61.最后，为了得到最优解需要采集n
cal
组数据进行联合优化，优化残差项为使用g2o或ceres等工具构建非线性最小二乘问题进行求解，该问题可以表示为:
[0062][0063]
其中，ni为第i帧激光落在标定板上的数目，n
ci
、d
ci
分别为相机坐标系下第i帧激光对应的标定板平面的三维法向量和相机坐标系原点到平面的距离，p
lim
为第i帧激光中第m个激光点在激光雷达坐标系下的坐标。
[0064]
步骤s2：在步骤s1标定2d激光雷达与双目相机外参的基础上，构建基于2d激光雷达的观测信息退化检测算法，在此说明：观测信息退化检测算法只针对2d激光雷达的平面观测信息。
[0065]
如图3、图4所示，观测信息退化情况分为两种：颠簸退化与环境退化。
[0066]
步骤s21，颠簸退化的原因如图3所示。此现象可表述为医护场景下场地的凹凸地段导致雷达传感器观测信息的突变，该突变对后续通过扫描匹配获取的雷达里程计影响极大。颠簸退化检测的传感器包括：双目相机与imu。
[0067]
值得注意的是上述两种传感器检测颠簸退化时各有其优劣势：其中通过双目相机观测信息获取的视觉里程计(获取方法在步骤s3中具体描述)来计算机器人位姿信息中的俯仰角pitch与横滚角roll。该方案优势在于无需多余传感器，其劣势在于判断有小量延迟且占用计算资源；使用imu的方案可直接通过imu驱动反馈来获取俯仰角pitch与横滚角roll，劣势在于需要添加额外传感器。
[0068]
结合上述所得俯仰角pitch与横滚角roll，计算颠簸退化程度τ1的依据为：
[0069][0070]
上述公式中，θ为设定颠簸的角度阈值，因为在机器人运行过程中不存在完全光滑的平面，因此机器人当前位姿的俯仰角pitch与横滚角roll必定有小幅度的抖动。
[0071]
步骤s22，环境退化的原因如图4a、4b所示。当2d激光雷达以360
°
的扫描角处于走
廊环境下,见图4(a)，或以180
°
的扫描角处于朝向墙壁的环境下，见图4(b)，由于其二维点云法向量方向特征单一，将其称为环境退化。
[0072]
值得注意的是：环境退化只是针对于2d激光雷达观测信息而言，并非对加入了双目相机观测信息后的环境信息进行判断。
[0073]
结合上述所述，判断环境退化的传感器信息来源只需来自2d激光雷达的观测信息即可，其判断步骤如下所述：
[0074]
步骤s221，计算当前帧中2d激光雷达各个扫描点的法向量信息。
[0075]
在此实施例中，首先假设计算扫描点p以及其领域点集n，计算点集n的质心p
centriod
：
[0076][0077]
其中，n为点集n中所包含的扫描点个数，pi表示点集n中第i个点，继续构建协方差矩阵c
p
:
[0078][0079]
对c
p
进行特征值分解：
[0080]cp
＝v
p
*d
p
*v
pt
ꢀꢀ
(5)
[0081]
其中，v
p
为扫描点p的特征向量矩阵，d
p
为特征值矩阵。
[0082]
结合上述所述：将最小特征值对应的特征向量作为扫描点p的法向量。
[0083]
步骤s222，使用ransec(随机抽样一致性)计算当前帧扫描点法向量一致性，步骤如下：
[0084]
a)先进行随机采样，在步骤s221计算所得的法向量合集中，随机选择一组法向量，作为当前迭代的样本集。
[0085]
b)设置局部拟合模型，其中局部拟合选取的度量为法向量的角度方向，进行局部拟合。
[0086]
c)在此基础上设定阈值，计算所有点到拟合模型的距离，将小于阈值的点标记为内点。
[0087]
d)如果内点数量大于预设的一致性阈值，则认为该模型是一致的。
[0088]
e)重复迭代，根据迭代次数，重复上述步骤a-d。
[0089]
通过上述步骤获取最优模型，若该模型通过一致性检验，则认为目前机器人处于环境退化的情况中，根据一致性程度，设置环境退化程度为τ2(0≤τ2≤1)。
[0090]
步骤s3：在步骤s2识别观测信息退化情况的基础上，构建基于2d激光雷达与双目相机的机器人联合里程计算法的步骤具体包括：
[0091]
步骤s31，构建单独的2d激光雷达里程计匹配残差计算方法步骤包括：
[0092]
步骤s311，实现传感器数据预处理部分。
[0093]
在t_1时刻获取轮式里程计与imu数据，再通过ekf(扩展卡尔曼)滤波融合获取当前时刻的非激光里程计变量，将该数据在上一时刻t_0通过残差优化修正后的机器人位姿
信息来预测当前时刻的机器人位姿信息。
[0094]
步骤s312，生成距离栅格地图。
[0095]
为了确保定位优化的实时性和建图的精确性，考虑到加入视觉里程计模块会增加较大的计算负载，我们决定将计算量过大的波束模型替换为空间似然域模型，以保证计算资源充足。
[0096]
进一步的，为了继续降低扫描匹配和图优化时获取残差的计算量，在占用栅格地图更新前的信息基础上(见图5)，将预先计算好每个栅格与其距离最近的占据栅格的欧式距离ε(p)，从而形成距离栅格地图。
[0097]
距离栅格地图形成的步骤可分为：
[0098]
1、距离上升步骤，在占用栅格地图中对占据栅格的周边栅格进行距离赋值，设定最大膨胀距离d；
[0099]
2、距离下降步骤，对占据栅格对应的距离栅格进行距离为0的赋值，如图6所示。
[0100]
值得注意的是，图6的距离栅格地图是以d＝2的形式形成的。
[0101]
步骤s313，获取当前帧，点云匹配的残差量。
[0102]
在上述步骤获取的距离栅格地图的基础上，设r为机器人旋转矩阵，t为平移向量，n
l
为当前帧点云的扫描点数量，一个扫描点连续坐标p
ε
＝[x,y]，匹配地图为m。通过查表法获得当前机器人所在位姿的点云匹配残差量e1，具体值为膨胀区域内所有激光落点欧氏距离ε(p)之和：
[0103][0104]
上述公式中，п(m,p)表示在当前地图中，找寻与点p欧几里得最近的占据栅格位置信息，可通过查表得知。
[0105]
进一步的，若运行计算资源允许，则可以通过双线性插值法获取更为精细每个激光落点的欧式距离残差，并且假设x,y∈[0,1),欧式距离残差的值ε(x,y)是从四个最近的整数坐标p
00
,p
01
,p
10
,p
11
计算获得，具体计算公式如下：
[0106][0107]
进一步的，采用数值微分法计算下降梯度。取最近的坐标p
00
,p
01
,p
10
,p
11
，相关欧几里得距离j
ε
的雅可比矩阵计算如下:
[0108][0109]
上述公式中，r是网格的分辨率。
[0110]
步骤s32,构建单独的双目里程计重投影误差获取步骤具体包括：
[0111]
步骤s321，特征点匹配。
[0112]
使用实时性较好的orb特征点提取算法，从左右相机的图像中提取特征点。orb算法结合了fast角点检测和brief描述子，能够在保持较高速度的同时提取具有描述能力的特征点。
[0113]
进一步的，在获取orb特征点后，使用汉明距离进行特征点匹配并过滤错误匹配，若两个描述子的汉明距离小于阈值，则认为匹配成功。
[0114]
步骤s322，重投影误差e2计算。
[0115]
e2可以通过将三维点p通过相机位姿投影到图像平面，并计算投影点与对应特征点的像素坐标之间的差距来定义。其计算步骤如下所示：
[0116]
a)假设有nc个特征点，第i个特征点的像素坐标pi为(ui,vi)，对应的三维点为pi。
[0117]
b)假设相机内参矩阵为c，第i个特征点的重投影误差为ei，si为尺度信息。
[0118]
c)第i个特征点的重投影误差可以简单地定义为：
[0119]ei
＝(ui,vi)-project(c,r,t,pi)
ꢀꢀ
(9)
[0120]
其中，project为投影函数，将三维点pi通过相机位姿投影到图像平面上得到的投影点。e2为所有特征点ei之和，具体见拓展公式(9)，将其定义为：
[0121][0122]
步骤s33，双目相机与2d激光雷达紧耦合的融合算法步骤可分为：
[0123]
步骤s331，在步骤s31的基础上获取联合误差函数。
[0124]
首先获取雷达此刻匹配误差为：e1，在步骤s32的基础上，获取相机两帧之间各个特征点匹配的重投影误差为：e2，在步骤s2中获取观测信息退化值τ1与τ2，得到两种观测信息的联合误差函数e，见公式(11):
[0125]
e＝(1-τ1)(1-τ2)e1+τ2e2ꢀꢀ
(11)
[0126]
上式简单解释为：当发生颠簸退化后，由于颠簸会导致点云与距离地图无法良好匹配甚至误匹配，因此雷达的匹配误差部分的置信度会归零。此外，环境的退化程度与雷达匹配误差部分的置信度成反比关系。
[0127]
步骤s332，进行残差优化。
[0128]
由上述所得，误差函数为非线性函数，并且优化量包含机器人位姿r，t。因此将李代数ξ代表机器人位姿，ξ为六维向量，用符号^表示将该六维向量转换为四维矩阵。因此将公式(11)结合公式(6)与公式(10)简化为李代数形式，即：
[0129][0130]
进一步的，可使用ceres或g2o均可获得机器人优化位姿以及联合里程计。
[0131]
步骤s4，构建优化栅格地图信息过程具体包括：
[0132]
步骤s41，形成基于雷达观测平面的八叉树地图。
[0133]
在步骤s3中获取的优化位姿的基础上，使用八叉树数据结构来表示双目相机建立的地图。在构建过程中，需要保持以下三个条件：
[0134]
1、八叉树地图的体素一个平面与激光雷达构建的栅格地图(下文称为初始栅格地图)保持重叠或平行；
[0135]
2、与初始栅格地图重叠或平行的平面的体素边界大小与初始栅格地图栅格大小相同；
[0136]
3、八叉树地图的坐标系与初始栅格地图坐标系重叠；
[0137]
步骤s42，构建优化栅格地图信息。
[0138]
结合八叉树地图中体素包含的占据标记(该体素是否为障碍物的信息)，如图7为图5标记的初始栅格地图中第七行栅格所对应的z轴方向的占据标记，其高度为7，记黑色占据标记为障碍物占据标记，记作1；白色占据标记为自由占据标记，记作0。
[0139]
结合上述所述，图5中的初始栅格地图点p
(5,7)
的z轴向量为[0,0,1,1,1,0,0]。对进行距离赋值，最终优化栅格地图中的其中di为第i层体素中心距离初始栅格地图平面的高度。
[0140]
值得注意的是：考虑到实际运用时计算资源的问题，优化后的栅格地图信息并不直接参与步骤s3的联合里程计部分，步骤s3中用于雷达已知地图信息的是初始栅格地图与距离栅格地图，本步骤说明的优化栅格地图信息仅仅用于步骤s5，即回环检测部分，原因是回环检测的频率远低于联合里程计计算的频率。
[0141]
步骤s5，构建基于2d激光雷达与双目相机的回环检测算法具体包括：
[0142]
步骤s51，设定回环检测条件。
[0143]
回环检测的主要作用为累计误差的修正，因此其频率不易过快。通过步骤s3中所得的联合里程计来判断是否进行回环检测，判断条件为：机器人运动距离到达水平阈值δd或机器人角度累计变化超过角度阈值δ
θ
。
[0144]
步骤s52，进行回环检测，获取回环残差。
[0145]
回环形式可采用map to map,scan to map,为计算资源考虑，优选地采用scan to map，即当前帧观测信息与已知地图的回环检测形式。为确保回环实时性，从以下两个角度出发：
[0146]
1、只对步骤s22中的内点进行z轴向量的残差计算，其余激光扫描点进行欧氏距离的残差计算，计算步骤与步骤s31一致。
[0147]
2、计算当前帧匹配残差ed，其中包含当前帧2d激光雷达所有内点z轴向量的余弦距离残差以及除内点外所有扫描点与其最近障碍物的欧氏距离残差。
[0148]
计算z轴方向向量通过余弦距离计算残差，公式如下：
[0149][0150]
上式中，sm为步骤s4中创建的优化栅格地图信息，sn为当前帧2d激光雷达扫描内点所对应的z轴向量信息，ns为内点数量，为内点pj所在优化栅格地图对应的z轴向量信息，由当前帧双目相机形成的八叉树所获取的内点pj的z轴向量信息。
[0151]
进一步的，与除内点外所有扫描点的欧氏距离之和相加，获得残差ed：
[0152][0153]
值得注意的是，上式中，并未加入平均化函数，因为回环检测该步骤的主要目的是为了消除累计误差。显而易见的，2d激光雷达进行slam的过程中，最容易导致累计误差甚至误匹配的情况就是步骤s2中所介绍的观测信息退化一块，因此，使用所有内点z轴余弦距离之和的目的是为了保证观测信息退化时，双目相机提供的z轴观测信息可以占有更高权重。
[0154]
进一步的，加入了扫描点远近归一化因子进一步的，加入了扫描点远近归一化因子α与当前帧扫描点到扫描中心(2d激光雷达旋转轴)欧氏距离的平均值成反比，简单说明，α的加入是为了将欧氏距离映射到与余弦距离一样的尺度范围。
[0155]
结合上述所述，当机器人在步骤s51设定的回环阈值内运行时，每隔一段设定阈值记录一个节点(节点内容包含机器人位姿，2d激光slam观测信息，内点z轴向量)，共计k个。通过上述步骤可获取各个节点的残差ed，再将对k个位姿优化问题通过构建非线性最小二乘问题来解决，最终回环残差为e：
[0156][0157]
上式中，d为两个回环检测中记录的节点个数。
[0158]
进一步的，使用ceres或g2o均可获得各个节点的最优解位姿。
[0159]
实施例2
[0160]
图8是为本发明实施例二提供的一种一种基于2d激光雷达与相机同时定位与建图的方法的系统，精准地建图定位是为后续机器人进行交互任务互动的。所述系统包括：
[0161]
机器人底盘，用于实现机器人的平移、旋转和转向功能。具体的，机器人底盘结构如图9所示，用于提供机器人的稳定支撑和移动能力。其中，由高强度铝合金结构的支撑底盘101为机器人整体提供稳定支撑；由主动轮102与无刷电机104实现驱动能力，主动轮102配合麦克纳姆从动轮103实现机器人的移动能力。
[0162]
传感器装配系统，用于感知周围环境和获取外部信息，为步骤s1至s5提供观测信息。具体的，传感器装配系统主要为实施例一中的slam算法提供观测信息，其中包括第一双目相机201提供的深度图信息，用于寻找特征点进行重投影误差的计算以及八叉树地图的形成，激光雷达202-2d用于采集激光扫描点数据信息，用于计算点云匹配残差，轮式编码器204用于里程计数据预处理部分或为联合里程计提供参考值，力传感器203用于采集接触力数据。
[0163]
机械臂系统，用于实现物品的抓取、装配以及搬运动作。具体的，机械臂系统结构如图10所示，末端配备夹具主要用于握持和操作医疗设备、物品或进行特定动作。包括了机械臂搭载平台301以及机械臂主体结构，机械主体结构由若干个旋转关节302～308构成，末端的旋转关节上安装有机械爪310以及安装在机械爪310上的第二双目相机309。机械臂系统的加入以及下面描述的人机交互系统是以实施例一中的slam算法为前提基础的，因此没
有机械臂系统与人机交互系统两个模块，实施例一的slam算法对病患并无实际帮助。
[0164]
人机交互系统，用以实现与机器人进行指令传递、信息交流和操作控制，实现人机共同工作和合作。具体的，人机交互系统主要用于方便医生或患者方便使用医用护理机器人并可对病患进行心理康复。包括了人机交互界面401，用于触屏交互实现特定功能，如递送物品，在线诊断等；蓝牙音箱402用于语音交互，定时关心患者用药，提醒患者注意事项等，力控交互装置403，可保证在医护环境下机器人运行安全以及周边物体安全。
[0165]
电源与控制系统，为整个系统提供能源和控制信号，保证机器人的长时间运行与协调各个部分工作。具体的，电源与控制系统可保证机器人的长时间运行与协调各个部分工作。包括了工控机501，用于处理传感器数据，进一步实现实施例一中的slam算法；电源502，保持机器人长时间工作。
[0166]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

技术特征：
1.一种在医用环境下基于2d激光雷达与相机的slam方法，其特征在于，所述方法包括如下具体步骤：s1:构建2d激光雷达与双目相机的联合标定算法；s2:构建基于2d激光雷达的观测信息退化检测算法；s3:构建基于2d激光雷达与双目相机的机器人里程计算法；s4:构建包含z轴障碍物信息的优化栅格地图；s5:构建基于2d激光雷达与双目相机的回环检测算法。2.如权利要求1所述的一种在医用环境下基于2d激光雷达与相机的slam方法，其特征在于，在步骤s1中，为了避免激光雷达与双目相机标定误差的累计，采用基于平面约束的相机激光联合标定算法，先通过标定板平面的二维码计算得到标定板平面在相机坐标系下的坐标表示；同时，激光发出的光束落在标定板平面上，利用激光点在激光坐标系下的坐标和平面方程在相机坐标系下的坐标，构建点在平面上的约束从而求解外参数，即激光雷达与相机相对位姿。3.如权利要求1所述的一种在医用环境下基于2d激光雷达与相机的slam方法，其特征在于，在步骤s2中，退化的情况分为两种，一种为机器人进行了除偏航角外的旋转动作而导致的观测信息退化，称为颠簸退化；另一种为机器人处于长廊环境下或因为扫描角度过小并且面向墙体从而导致观测信息为一条直线的情况，称为环境退化；针对颠簸退化的检测方法具体如下：先计算由双目视觉里程计算法得到的前后帧相机位姿，再判断机器人的俯仰角与滚动角变化值是否大于设定阈值，若大于设定阈值，则观察由当前帧激光里程计修正后是否超过设定阈值，若超过，则判定该时刻发生了颠簸退化，记颠簸退化程度为τ1＝1，初始值τ1＝0；针对环境退化的检测方法具体如下：采集当前帧点云数据，计算各个扫描点的法向量方向，再对法向量进行一致性分析，若一致性分析通过，则判定该时刻发生了环境退化，记环境退化程度为τ2，0≤τ2≤1。4.如权利要求1所述的一种在医用环境下基于2d激光雷达与相机的slam方法，其特征在于，在步骤s3中，在识别观测信息退化的基础上，构建基于2d激光雷达与双目相机的机器人里程计算法包括如下步骤：步骤s31：构建单独的2d激光雷达里程计匹配残差，具体包括：步骤s311，实现传感器数据预处理部分；步骤s312，生成距离栅格地图；步骤s313，获取当前帧，点云匹配的残差量；步骤s32：构建单独的双目里程计重投影误差，具体包括：步骤s321，特征点匹配；步骤s322，重投影误差e2计算；步骤s33：双目相机与2d激光雷达紧耦合的融合，融合算法步骤具体如下：步骤s331，在步骤s31的基础上获取联合误差函数；步骤s332，进行残差优化。5.如权利要求1所述的一种在医用环境下基于2d激光雷达与相机的slam方法，其特征在于，在步骤s4中，构建优化栅格地图信息过程具体包括：
在步骤s3获取的机器人联合里程计的基础上，将双目相机获取的稠密点云转化为八叉树地图，八叉树地图的形成过程需要满足的条件为：八叉树地图中体素有平面与栅格地图重叠或平行以及保持八叉树坐标系与栅格地图坐标系重叠；形成八叉树地图后，对占用栅格地图中的栅格进行与高度有关的障碍物内容记载，每个占据栅格会记载有其z轴障碍物情况的向量。6.如权利要求1所述的一种在医用环境下基于2d激光雷达与相机的slam方法，其特征在于，在步骤s5中，构建基于2d激光雷达与双目相机的回环检测算法，具体包括：步骤s51：设定回环检测条件；通过步骤s3中所得的联合里程计来判断是否进行回环检测，判断条件为：机器人运动距离到达水平阈值δ
d
或机器人角度累计变化超过角度阈值δ
θ
；步骤s52：进行回环检测，获取回环残差，通过最小化误差函数以进一步求得精确位姿。7.实施如权利要求1～6任意一项所述的一种在医用环境下基于2d激光雷达与相机的slam方法的系统，其特征在于，所述系统包括：机器人底盘，用于实现机器人的平移、旋转和转向功能；传感器装配系统，用于感知周围环境和获取外部信息，为步骤s1至步骤s5提供观测信息；机械臂系统，用于实现物品的抓取、装配以及搬运动作；人机交互系统，用以实现与机器人进行指令传递、信息交流和操作控制，实现人机共同工作和合作，电源与控制系统，为整个系统提供能源和控制信号，保证机器人的长时间运行与协调各个部分工作。

技术总结
本发明公开了一种在医用环境下基于2D激光雷达与相机的SLAM方法及系统，所述方法包括如下具体步骤：S1:构建2D激光雷达与双目相机的联合标定算法；S2:构建基于2D激光雷达的观测信息退化检测算法；S3:构建基于2D激光雷达与双目相机的机器人里程计算法；S4:构建包含Z轴障碍物信息的优化栅格地图；S5:构建基于2D激光雷达与双目相机的回环检测算法。本发明解决了室内SLAM在医护环境下机器人由于行驶颠簸或光照导致某类传感器观测信息产生误差最终建图失败的问题，在判断2D雷达点云观测信息是否退化的基础上，加入视觉信息来确保复杂场景下的SLAM过程的稳定性，修改传统栅格框架保证回环优化的实时性，并配合全新硬件系统实现多动态环境下机器人与障碍物的安全性。多动态环境下机器人与障碍物的安全性。多动态环境下机器人与障碍物的安全性。


技术研发人员：付明磊 郑家豪 刘锦元 格里高利普拉卡波维奇 乌拉迪斯劳锡丘
受保护的技术使用者：浙江工业大学
技术研发日：2023.05.30
技术公布日：2023/9/12