基于ORB_SLAM2算法的智能轮椅床控制方法

基于orb_slam2算法的智能轮椅床控制方法
技术领域
1.本发明涉及智能轮椅床控制领域，具体为基于orb_slam2算法的智能轮椅床控制方法。


背景技术：

2.众所周知，护理工作是医院和家庭中较为繁重的工作内容，用现有的科学工程技术提高护理工作的效率和简化护理人员的工作强度是未来护理发展的方向。一款优秀的智能护理产品可以减轻家庭经济负担，节省护理人力资源，改善护理人员短缺的现状。
3.目前市面上已经出现许多种智能家居护理产品，如多功能轮椅、居家护理床、康复训练机器人、病患转移机器人等一系列产品。这些产品通常都需要护理人的陪伴才能完成起床、搭乘轮椅外出等操作。在床和轮椅的转换以及导航避障过程中都需要人员的协助才能完成，因此市场上急需一种集上述功能于一体的护理装置。
4.将orb-slam2加入智能轮椅床的运行中，可以提取医院等场所的图像特征，通过这些特征不断建立和完善医院场所模型。当接受搭载任务后，启动轮椅床，规划路线，并自主导航至指定区域。在行驶途中通过搭载的红外传感器、超声波传感器等实时发现障碍，并进行规避。确保智能轮椅床能安全、准确的完成任务。
5.目前现有技术如下：
6.申请号：202110387127.3，申请名称“一种结合脑机接口技术及orb_slam导航的移动机器人控制新方法”公开了一种结合脑机接口技术及orb_slam导航的移动机器人控制新方法，属于脑科学、无人系统控制交叉领域。本发明通过bci技术，在移动机器人自主导航过程中加入人的意识决策控制，在系统控制过程层面上达到人机智能混合的目的，克服移动机器人在非结构化的复杂环境中任务执行可靠性低、操作者操作负担重、控制系统智能化程度低的问题。自主导航系统通过基于ros系统的orb_slam技术来实现，移动机器人自主导航过程中，操作者通过视频传输反馈系统感知环境从而确定是否要引入人为决策控制，人为决策控制的实现通过基于ssvep范式的脑机接口系统来实现。本发明可以应用于特殊人群的生活辅助轮椅、无人作战平台以及月球车的控制系统设计等。
7.其将基于orb_slam技术的自主导航技术和基于ssvep范式的脑机接口技术相结合在移动机器人的控制过程层面实现人机智能混合。本文通过基于orb_slam改进的orb_slam2算法完成智能轮椅床的自主导航，当接受任务后，智能轮椅床不需要再进行交互，可以实现智能轮椅床的自动控制。


技术实现要素：

8.为了解决以上问题，本发明提出基于orb_slam2算法的智能轮椅床控制方法，首先通过orb_slam2算法精确提取医院的特征点，通过这些特征点不断建立和完善医院模型。在接受任务之后，规划路线，搭载失去行动能力的老人或残疾人去往指定地点，途中通过自身携带的摄像头、激光雷达、红外传感器、超声波传感器发现障碍，并对障碍进行规避，实现实
时避障功能。
9.为实现上述目的，本发明采取的技术方案是：
10.基于orb_slam2算法的智能轮椅床控制方法，具体步骤如下：
11.第一步：建立医院地图：
12.智能轮椅床初始位置时位于医院病房，启动机器人，自检完成后，通过摄像头提取医院的环境特征，再通过基于orb_slam2算法提取特征点，建立环境模型；
13.orb_slam2系统分为跟踪模块、局部建图模块和闭环检测模块三大模块；
14.第二步：路径规划：
15.医院模型建立后，设置目标点，智能轮椅床搭载失能老人或者行动不便的残疾人到达指定位置，不论何种方式智能轮椅床都需要自主建立路径规划以到达准确的位置，采用a*算法，a*算法该算法通过启发函数来计算每个节点的优先级；
16.第三步：定位：
17.通过路线规划和特征匹配，路线的准确性得到了保证，最后在通过真实运动轨迹与算法生成轨迹的均方根差rmse对比与运动测量和传感器读数来确定是否正确，设智能轮椅床的实际运动轨迹为x＝(x1,x2,x3
…
xk)，算法生成的运动轨迹为通过真实运动轨迹与算法生成轨迹的均方根差rmse来判断定位是否精准；表示算法生成轨迹在第i点处偏移量，trans(xi)表示实际运动轨迹在在第i点处偏移量，n表示摄像头个数；
[0018][0019]
第四步：智能引导：
[0020]
采用轮椅底盘，搭载红外传感器、超声波传感器，通过实时的环境检测，对途中的障碍进行规避，顺利搭载失能老人或行动不便的残疾人到达目标点。
[0021]
作为本发明进一步改进，所述步骤一orb_slam2系统框架跟踪模块具体如下，分为特征提取与匹配、位姿预测或重定位、跟踪局部地图、关键帧判断，采用orb算法进行特征提取，并进行特征匹配，当存在足够多特征点后便根据这些点用几何原理来对相机进行位姿估计和重定位，基于双目和rgb-d相机的视觉里程计直接使用php算法进行位姿估算，推导过程如下：
[0022]
设空间存在点p＝[x,y,z]
t
，对应的齐次坐标为p＝[x，y，z，1]
t
，对应的匹配点在归一化齐次坐标系下的齐次坐标为x1＝(u1，v1，1)，定义一个3
×
4增广矩阵[r|t]，根据相机模型推导下式：
[0023][0024]
消除s，得到以下两个约束：
[0025][0026]
定义以下向量；
[0027]
t1＝(t1，t2，t3，t4)
t t1＝(t5，t6,t7，t8)
t t1＝(t9，t
10
，t
11
，t
12
)
t
；
[0028]
结合两式得：
[0029][0030]
由上式知若有n对特征点则约束关系如下线性方程组：
[0031][0032]
作为本发明进一步改进，所述步骤一orb_slam2系统框架局部建图模块具体如下，orb-slam2的后端通过前端算法获得到的相机的位姿和特征点的空间位置信息，并根据这些信息构建稀疏的特征点地图，局部建图模块为在关键帧队列中添加前端模块中处理出的关键帧以及对局部地图进行维护和优化更新，同时若检测到线程空闲时，将关键帧从关键帧队列中取出并进行处理。
[0033]
作为本发明进一步改进，所述步骤一orb_slam2系统框架闭环检测模块具体如下，生成全局地图，回环检测模块根据视觉里程计提供的相机运动轨迹和地图初值，构建全局一致的轨迹和地图，消除slam系统估计出来的运动轨迹和点云地图在长时间运动下的累积误差。
[0034]
作为本发明进一步改进，所述步骤二启发函数的形式为：f(n)＝g(n)+h(n)，其中f(n)表示节点n的综合优先级，g(n)是节点n距离起点的代价，h(n)是节点n距离终点的预计代价，a*算法在运算过程中，每次从优先队列中选取f(n)值最小，优先级最高的节点作为下一个待遍历的节点。
[0035]
有益效果：
[0036]
1.本发明为基于orb_slam2算法智能轮椅床控制方法，改进的orb_slam2算法具有累计误差低、运动估计精度高、输入项后端优化、能有效降低定位误差等优点，可以快速识别与建立地图，实现机器人长时间、高精度定位。
[0037]
2.相较于传统的gps而言，本发明更适合在室内环境使用下，减少因外界环境带来的不必要干扰。成本低、可使用范围更加广泛。
附图说明
[0038]
图1为本发明视觉slam框架；
[0039]
图2为本发明构建局部地图；
[0040]
图3为本发明orb-slam2算法的框架。
具体实施方式
[0041]
下面结合附图与具体实施方式对本发明作进一步详细描述：
[0042]
作为本发明一种具体实施例，本发明首先通过orb_slam2算法精确提取医院的特征点，通过这些特征点不断建立和完善医院模型。在接受任务之后，规划路线，搭载失去行
动能力的老人或残疾人去往指定地点，途中通过自身携带的摄像头、激光雷达、红外传感器、超声波传感器发现障碍，并对障碍进行规避，实现实时避障功能。
[0043]
基于orb_slam2算法的智能轮椅床控制方法，其特征在于可以使用该算法通过摄像头实现机器人在二维或三维条件下的定位及建图功能，具体步骤如下：
[0044]
第一步：建立医院地图：
[0045]
智能轮椅床初始位置时位于医院病房，启动机器人，自检完成后，通过摄像头提取医院的环境特征，再通过基于orb_slam2算法提取特征点，建立环境模型。
[0046]
orb_slam2系统分为跟踪、局部建图、闭环检测三大模块，orb_slam2系统框架如图1所示。第一部分跟踪模块主要分为特征提取与匹配、位姿预测或重定位、跟踪局部地图、关键帧判断。在本发明中拟采用orb算法进行特征提取，并进行特征匹配。当存在足够多特征点后便可根据这些点用几何原理来对相机进行位姿估计和重定位。基于双目和rgb-d相机的视觉里程计可直接使用php算法进行位姿估算。推导过程如下：设空间存在点p＝[x,y,z]
t
,对应的齐次坐标为p＝[x,y,z,1]
t
，对应的匹配点在归一化齐次坐标系下的齐次坐标为x1＝(u1,v1，1)。定义一个3
×
4增广矩阵[r|t]，根据相机模型可以推导下式
[0047][0048]
消除s，得到以下两个约束：
[0049][0050]
定义以下向量
[0051]
t1＝(t1，t2,t3,t4)
t t1＝(t5，t6,t7，t8)
t t1＝(t9,t
10
，t
11
，t
12
)
t
[0052]
结合两式可得
[0053][0054]
由上式可知若有n对特征点则约束关系如下线性方程组
[0055][0056]
第二部分是构建局部地图。orb-slam2的后端主要通过前端算法获得到的相机的位姿、特征点的空间位置等信息，并根据这些信息构建稀疏的特征点地图。局部建图模块的主要功能为在关键帧队列中添加前端模块中处理出的关键帧以及对局部地图进行维护和优化更新。同时若检测到线程空闲时，将关键帧从关键帧队列中取出并进行处理。具体流程图如图2所示
[0057]
第三部分是闭环检测，生成全局地图。回环检测模块根据视觉里程计提供的相机运动轨迹和地图初值，构建全局一致的轨迹和地图。它可以消除slam系统估计出来的运动
轨迹和点云地图在长时间运动下的累积误差
[0058]
orb-slam2是一个效果良好且系统完备的的slam系统，该slam算法能够确保智能轮椅床在跟踪失败时重新连接slam系统，继续定位，根据检测到的回环约束对所有位姿及其约束关系进行优化，使得智能轮椅床在大尺度场景下保证轨迹的精确。实现智能轮椅床的轨迹估计和地图构建，算法框架如图3所示。
[0059]
第二步：路径规划：
[0060]
医院模型建立后，设置目标点，智能轮椅床可以搭载失能老人或者行动不便的残疾人到达指定位置。不论何种方式智能轮椅床都需要自主建立路径规划以到达准确的位置，采用a*算法，a*算法该算法通过启发函数来计算每个节点的优先级。启发函数的形式为：f(n)＝g(n)+h(n)。其中f(n)表示节点n的综合优先级，g(n)是节点n距离起点的代价，h(n)是节点n距离终点的预计代价。a*算法在运算过程中，每次从优先队列中选取f(n)值最小(优先级最高)的节点作为下一个待遍历的节点。a*算法搜索速度较快，非常适用于医院这种地图较大的场景使用。
[0061]
第三步：定位：
[0062]
通过严格、精确的路线规划和特征匹配，路线的准确性得到了保证，最后在通过真实运动轨迹与算法生成轨迹的均方根差(rmse)对比与运动测量和传感器读数来确定是否正确，设智能轮椅床的实际运动轨迹为x＝(x1,x2,x3
…
xk)，算法生成的运动轨迹为通过真实运动轨迹与算法生成轨迹的均方根差(rmse)来判断定位是否精准；表示算法生成轨迹在第i点处偏移量，trans(xi)表示实际运动轨迹在在第i点处偏移量，n表示摄像头个数。
[0063]
第四步：智能引导：
[0064]
采用轮椅底盘，搭载红外传感器、超声波传感器，通过实时的环境检测，对途中的障碍进行规避，顺利搭载失能老人或行动不便的残疾人到达目标点。因为实现了导航的精确定位，故智能轮椅床的精确性能需要得到极大的保证。智能轮椅床在运行过程中的实时数据可以通过智能控制主机经过信号交换器传递给主控电脑，进行分析和处理。
[0065]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

技术特征：
1.基于orb_slam2算法的智能轮椅床控制方法，其特征在于，具体步骤如下：第一步：建立医院地图：智能轮椅床初始位置时位于医院病房，启动机器人，自检完成后，通过摄像头提取医院的环境特征，再通过基于orb_slam2算法提取特征点，建立环境模型；orb_slam2系统分为跟踪模块、局部建图模块和闭环检测模块三大模块；第二步：路径规划：医院模型建立后，设置目标点，智能轮椅床搭载失能老人或者行动不便的残疾人到达指定位置，不论何种方式智能轮椅床都需要自主建立路径规划以到达准确的位置，采用a*算法，a*算法该算法通过启发函数来计算每个节点的优先级；第三步：定位：通过路线规划和特征匹配，路线的准确性得到了保证，最后在通过真实运动轨迹与算法生成轨迹的均方根差rmse对比与运动测量和传感器读数来确定是否正确，设智能轮椅床的实际运动轨迹为x＝(x1,x2,x3
…
xk)，算法生成的运动轨迹为通过真实运动轨迹与算法生成轨迹的均方根差rmse来判断定位是否精准；表示算法生成轨迹在第i点处偏移量，trans(x
i
)表示实际运动轨迹在在第i点处偏移量，n表示摄像头个数；第四步：智能引导：采用轮椅底盘，搭载红外传感器、超声波传感器，通过实时的环境检测，对途中的障碍进行规避，顺利搭载失能老人或行动不便的残疾人到达目标点。2.根据权利要求1所述的基于orb_slam2算法的智能轮椅床控制方法，其特征在于，所述步骤一orb_slam2系统框架跟踪模块具体如下，分为特征提取与匹配、位姿预测或重定位、跟踪局部地图、关键帧判断，采用orb算法进行特征提取，并进行特征匹配，当存在足够多特征点后便根据这些点用几何原理来对相机进行位姿估计和重定位，基于双目和rgb-d相机的视觉里程计直接使用php算法进行位姿估算，推导过程如下：设空间存在点p＝[x,y,z]
t
,对应的齐次坐标为p＝[x,y,z,1]
t
，对应的匹配点在归一化齐次坐标系下的齐次坐标为x1＝(u1,v1,1)，定义一个3
×
4增广矩阵[r|t]，根据相机模型推导下式：消除s，得到以下两个约束：定义以下向量；t1＝(t1，t2，t3，t4)
t t1＝(t5，t6，t7，t8)
t t1＝(t9，t
10
，t
11
，t
12
)
t
；结合两式得:
由上式知若有n对特征点则约束关系如下线性方程组:3.根据权利要求1所述的基于orb_slam2算法的智能轮椅床控制方法，其特征在于，所述步骤一orb_slam2系统框架局部建图模块具体如下，orb-slam2的后端通过前端算法获得到的相机的位姿和特征点的空间位置信息，并根据这些信息构建稀疏的特征点地图，局部建图模块为在关键帧队列中添加前端模块中处理出的关键帧以及对局部地图进行维护和优化更新，同时若检测到线程空闲时，将关键帧从关键帧队列中取出并进行处理。4.根据权利要求1所述的基于orb_slam2算法的智能轮椅床控制方法，其特征在于，所述步骤一orb_slam2系统框架闭环检测模块具体如下，生成全局地图，回环检测模块根据视觉里程计提供的相机运动轨迹和地图初值，构建全局一致的轨迹和地图，消除slam系统估计出来的运动轨迹和点云地图在长时间运动下的累积误差。5.根据权利要求1所述的基于orb_slam2算法的智能轮椅床控制方法，其特征在于，所述步骤二启发函数的形式为：f(n)＝g(n)+h(n)，其中f(n)表示节点n的综合优先级，g(n)是节点n距离起点的代价，h(n)是节点n距离终点的预计代价，a*算法在运算过程中，每次从优先队列中选取f(n)值最小，优先级最高的节点作为下一个待遍历的节点。

技术总结
本发明提出了基于ORB_SLAM2算法的智能轮椅床控制方法，具体步骤如下，第一步：建立医院地图：第一部分跟踪模块主要分为特征提取与匹配、位姿预测或重定位、跟踪局部地图、关键帧判断。第二部分是构建局部地图。第三部分是闭环检测，生成全局地图。第二步：路径规划：第三步：定位：第四步：智能引导：本发明首先通过ORB_SLAM2算法精确提取医院的特征点，通过这些特征点不断建立和完善医院模型。在接受任务之后，规划路线，搭载失去行动能力的老人或残疾人去往指定地点，途中通过自身携带的摄像头、激光雷达、红外传感器、超声波传感器发现障碍，并对障碍进行规避，实现实时避障功能。实现实时避障功能。实现实时避障功能。


技术研发人员：吕章锟 鞠全勇 姚一
受保护的技术使用者：盐城工学院
技术研发日：2023.03.14
技术公布日：2023/7/20