CN115049910A - 一种基于双目视觉里程计的足式机器人建图与导航方法 - Google Patents
一种基于双目视觉里程计的足式机器人建图与导航方法 Download PDFInfo
- Publication number
- CN115049910A CN115049910A CN202210321993.7A CN202210321993A CN115049910A CN 115049910 A CN115049910 A CN 115049910A CN 202210321993 A CN202210321993 A CN 202210321993A CN 115049910 A CN115049910 A CN 115049910A
- Authority
- CN
- China
- Prior art keywords
- map
- camera
- image
- robot
- points
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 238000013507 mapping Methods 0.000 title claims abstract description 11
- 230000033001 locomotion Effects 0.000 claims abstract description 30
- 230000008569 process Effects 0.000 claims abstract description 18
- 239000013598 vector Substances 0.000 claims description 17
- 238000005259 measurement Methods 0.000 claims description 16
- 238000001914 filtration Methods 0.000 claims description 13
- 230000000007 visual effect Effects 0.000 claims description 13
- 238000013519 translation Methods 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 10
- 230000008447 perception Effects 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 8
- 238000005457 optimization Methods 0.000 claims description 8
- 238000012937 correction Methods 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 7
- 238000012545 processing Methods 0.000 abstract description 10
- 238000004364 calculation method Methods 0.000 abstract description 6
- 230000000694 effects Effects 0.000 abstract description 5
- 238000001514 detection method Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000018109 developmental process Effects 0.000 description 2
- 238000010606 normalization Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 241000124008 Mammalia Species 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 230000009975 flexible effect Effects 0.000 description 1
- 230000005021 gait Effects 0.000 description 1
- 230000008570 general process Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000004886 process control Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明实施例公开了一种基于双目视觉里程计的足式机器人建图与导航方法，涉及多足移动机器人技术领域，在脱离GPS的有效信号范围后，还能够实现足式机器人的自主导航。本发明包括：通过双目相机采集图像帧；提取各个时间戳下的图像帧的特征点，并建立相机坐标系，之后利用所提取的特征点和所述相机坐标系，得到里程计信息；在所述足式机器人移动的过程中，获取关键帧；进一步建立子地图之后，根据所述子地图建立地图，并在所述地图中规划行进路线，并按照所述规划行进路线进行导航。通过所设计的图像处理的方式，避免足式机器人运动过程中起伏不定的状态所拍摄出的图像效果对计算里程计产生影响。
Description
技术领域
本发明涉及多足移动机器人技术领域，尤其涉及一种基于双目视觉里程计 的足式机器人建图与导航方法。
背景技术
21世纪以来，国内外对机器人技术的发展越来越重视，机器人技术被认为 是对未来新兴产业发展具有重要意义的高技术之一。其中移动机器人应用广泛， 覆盖了地面、空中和水下乃至外太空。轮式机器人利用航迹推算、计算机视觉、 路标识别、无线定位、SLAM等技术进行定位能够基于地图完成机器人运动路径 的规划和运动控制，而足式移动机器人是模仿哺乳类动物的运动方式，通过研 究系统设计、步态规划、稳定性等方面而设计的机器人系统。相比于轮式机器 人，足式机器人的适应性能力更强，适用范围更加复杂。足式机器人可以轻易 地跨过各种障碍，有着良好的自由度、动作灵活、自如、稳定。
足式机器人的位姿估计排除GPS定位导航的方法外一直缺少有效的手段，主 要的问题在于，轮式机器人可以通过轮式里程计等手段对机器人的行走路线进 行估计，现有的大部分里程计方案目前适用于轮式、履带式等依靠驱动轮的轮 轴传感器来计算里程的。而这些方案是难以有效应用于足式机器人的。因此， 如何在脱离GPS的有效信号范围后，还能够实现足式机器人的自主导航，成为需 要研究的问题。
发明内容
本发明的实施例提供一种基于双目视觉里程计的足式机器人建图与导航方 法，在脱离GPS的有效信号范围后，还能够实现足式机器人的自主导航。
为达到上述目的，本发明的实施例采用如下技术方案：
S1、通过双目相机采集图像帧；
S2、提取各个时间戳下的图像帧的特征点，并建立相机坐标系，之后利用 所提取的特征点和所述相机坐标系，得到里程计信息；
S3、在所述足式机器人移动的过程中，获取关键帧；
S4、通过扩展卡尔曼滤波器对里程计信息进行滤波估计，得到里程计修正 数据；
S5、通过激光雷达获取环境感知数据，所述环境感知数据包括：所述足式 机器人与障碍物的距离数据；
S6、利用S4所获取的里程计修正数据、S5所获取的环境感知数据和惯性测 量单元采集的数据，建立用于底层运动控制的子地图，其中，由惯性测量单元 采集的数据包括：加速度测量数据和角速度测量数据；
S7、根据所述子地图进行优化更新并得到地图。
本发明实施例提供的基于双目视觉里程计的足式机器人建图与导航方法， 可以在没有GPS信号的范围内实现足式机器人的建图导航，以及自主移动和路径 规划。通过所设计的图像处理的方式，避免足式机器人运动过程中起伏不定的 状态所拍摄出的图像效果对计算里程计产生影响。优化了双目视觉里程计数据 的处理方法，从而可以提高里程计数据的准确度，优化建图效果。对雷达数据 进行滤波处理，防止噪声对建图的影响。在不外加传感器的条件下，大大提高 了足式机器人智能化的同时降低成本，提高生产效益。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要 使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一 些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还 可以根据这些附图获得其它的附图。
图1为本发明实施例提供的基于优化算法的双目视觉里程计设计流程图；
图2为本发明实施例提供的基于滤波算法的双目视觉里程计设计流程图；
图3为本发明实施例提供的加权计算的视觉里程计系统框图；
图4为本发明实施例提供的基于视觉里程计的足式机器人建图与导航系统 框图；
图5为本发明实施例提供的方法流程示意图；
图6为本发明实施例提供的ROS平台的交互方式示意图；
图7为本发明实施例提供的子地图(submap)的处理方式的示意图；
图8为本发明实施例提供的一种可能的子地图(submap)的示例的示意图。
具体实施方式
为使本领域技术人员更好地理解本发明的技术方案，下面结合附图和具体 实施方式对本发明作进一步详细描述。下文中将详细描述本发明的实施方式， 所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同 或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方 式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。本技术领 域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、 “所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书 中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件， 但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组 件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件 时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这 里使用的“连接”或“耦接”可以包括无线连接或耦接。这里使用的措辞“和/ 或”包括一个或更多个相关联的列出项的任一单元和全部组合。本技术领域技 术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学 术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应 该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的 上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于 正式的含义来解释。
足式机器人的优点在于其卓越的地形适应能力，相比于轮式、履带式机器 人，其能够去到更加复杂的地形环境中。而针对轮式、履带式机器人，能够计 算其里程计数据的硬件设备已经发展的相当成熟，其计算精度也相对较高。但 对于足式机器人，一直没有较好的硬件设备支撑其里程数据计算，因此本发明 着手于视觉里程计的方法研究并应用于建图导航算法中。
本发明实施例提供一种基于双目视觉里程计的足式机器人建图与导航方法， 如图5所示，包括：
S1、通过双目相机采集图像帧。
其中，如图4所示的，在所述足式机器人上安装有视觉里程计模块、底层运 动模块和智能控制模块。所述视觉里程计模块包括了安装在所述足式机器人上 的双目相机，所述双目相机包括左目相机和右目相机。所述底层运动模块包括 了惯性测量单元(IMU)，所述惯性测量单元用于获取所述足式机器人的加速度 数据和角速度数据。所述足式机器人上还安装有激光雷达，所述激光雷达用于 获取所述足式机器人与四周环境中的障碍物的距离数据。所采集的图像帧包括： 所述左目相机采集的左目灰度图像的图像帧和所述右目相机采集的右目灰度图 像的图像帧。足式机器人平台搭载了前视双目相机，该相机以每秒20帧图像的 速率采集数据。搭载了I MU以200Hz的速率进行加速度及角加速度的测量。搭载了激光雷达采集周围障碍物的数据。能够在没有GPS信号覆盖的情况下，通过机 器视觉得到的里程计数据采用s l am建图方法，建立全局的地图并进行导航。
S2、提取各个时间戳下的图像帧的特征点，并建立相机坐标系，之后利用 所提取的特征点和所述相机坐标系，得到里程计信息。
其中，相机坐标系可以理解为以相机本体为原点建立的空间坐标系。
S3、在所述足式机器人移动的过程中，获取关键帧。
S4、通过扩展卡尔曼滤波器对里程计信息进行滤波估计，得到里程计修正 数据。
其中，扩展卡尔曼滤波器作为一种算法部署在机器人嵌入式控制器中。滤 波估计的大致过程，指的就是通过重新计算，并消除累计误差，具体利用滤波 算法减小四足机器人运动过程中造成的累计误差。
本实施例中利用扩展卡尔曼滤波进行的数据处理，目的在于根据当前状态， 预估下一个状态，例如：如图1所示的，本实施例选用了两个状态向量，分别是 IMU的状态向量和相机的状态向量,构建扩展卡尔曼滤波模型，输入为不同时刻 的I MU状态向量和相机状态向量(向量包括位置p和姿态四元数q)，特征点在多 个相机状态之间形成几何约束，进而利用几何约束构建观测模型对卡尔曼滤波 模型进行更新，得到的最新的相机状态就是相机的预测位姿，即里程计信息。 其特点在于相机状态的个数始终维持不变，对历史状态的剔除能大大降低计算 量，避免了梯度爆炸的后果；
考虑到基于滤波的视觉里程计处理速度快但精度不高，而基于非线性优化 的优化方法处理速度慢但精度高，所以在过去的一段时间中里程计的真实数据 采用加权方法来处理并表达
其中Odom表示最终 里程计数据；
Odomnol分别为基于卡尔曼滤波的里程计数据和基于非线 性优化的里程计数据；n表示为单位时间内基于卡尔曼滤波的方法获取到的里程 计数据个数。
举例来说，本实施例中所应用的基于滤波算法的双目视觉里程计设计步骤 如图2所示的：1.首先初始化状态向量和协方差；2.然后进行IMU积分，状态向 量和协方差发生改变；3.将新的相机状态加入到状态向量中，扩充协方差矩阵； 4.进行观测更新，二次以后的所有状态和协方差发生改变；5.当相机状态个数 达到阈值，剔出相对原始的数据；6.重复2～5。
之后，如图1所示的，可以采用算术平均滤波方法平滑数据，其具体做法是 采取连续的N个数据进行算数平均运算，其中N不易太大，会影响正常的数据采 集，也不宜太小，会达不到滤波的效果，雷达数据经过处理后会减少噪声的干 扰，同时大大减少数据量，对算法有很好的优化效果。
S5、通过激光雷达获取环境感知数据，所述环境感知数据包括：所述足式 机器人与障碍物的距离数据。
S6、利用S4所获取的里程计修正数据、S5所获取的环境感知数据和惯性测 量单元采集的数据，建立用于底层运动控制的子地图(submap)。
其中，由惯性测量单元采集的数据包括：加速度测量数据和角速度测量数 据。本实施例中，如图7所示的，采用子地图(submap)的原理进行处理，每当 或得一次l aserscan(即距离数据)的数据后，便与当前最近建立的Submap去 进行匹配，使这一帧的l aserscan数据插入到Submap上最优的位置。(这里用的 是高斯牛顿解最小二乘问题)在不断插入新数据帧的同时该Submap也得到了更 新。一定量的数据组合成为一个Submap，当不再有新的scan插入到Submap时， 就认为这个submap已经创建完成，接着会去创建下一个submap，所有的submap 构建一张尺度信息不精确的全局地图，而这个“尺度信息不精确的全局地图” 即作为S6中建立的用于底层运动控制的子地图(submap)，而其尺度信息不精确的问题，则需要通过后续的更新迭代进行解决，即S7。
本实施例中所提及的子地图(submap)，具体是通过几个连续的激光雷达数 据(scans)创建而成的，如图8所示，由5cm*5cm大小的概率栅格
构 造而成，submap在创建完成时，栅格概率小于pmin表示该点无障碍，在pmin与 pmax之间表示未知，大于pmax表示该点有障碍。多个雷达数据会填满位置区域， 并更新已知区域。
本实施例中，可以通过ROS平台发送所述里程计修正数据信息，具体可以是 发送给最终的控制单元，用于后续步骤中的建图。具体来说，ROS平台部署在机 器人控制器上，实现各个部分消息的接收和发送。而所有的传感器数据都可以 通过ROS平台发送给最终的处理控制单元。例如图6所示的，为本实施例ROS平台 的部分消息节点流程图，首先由双目相机得到同一时间下左右两帧灰度图像， 经过位姿估计后得到大概的里程计数据，之后经过回环检测较少误差后得到较 为准确的里程计数据，接着联合激光雷达信息采用cartographer算法构建得到 地图。
S7、对所述子地图进行优化更新并得到地图。
其中，对所述子地图进行优化更新，可以理解为：子地图不断迭代，不断 确定出二次经过的空间点，最终迭代出可以使用的“地图”。而最终得到的地图， 则可以用于规划行进路线，并按照所述规划行进路线进行导航。所述对所述子 地图进行优化更新并得到地图，包括：所述多足机器人利用所述子地图开始进 行移动，并在移动过程中，实时检测所述多足机器人第二次路过所述地图中的 同一个空间点并记录，作为修订点；在每一给更新周期中，获取至少一个修订 点后并记录在所述子地图中；当所述子地图中的修订点停止增加后，停止更新 并将最后一次更新的子地图作为所述地图。
实际应用中，所述判定所述多足机器人第二次路过所述地图中的同一个空 间点，包括：所述双目相机当前拍摄的图像帧与之前拍摄的关键帧，存在至少 10个相同的特征点，则判定为同一个空间点。
其中，将上述子地图由ROS平台的固有节点 (cartographer_occupancy_grid_node)转化为ROS的GridMap网格地图格式， 用以后续导航。这里的目标点和上述的目标点并不一致，这里指的是地图构建 完成以后，使用者自主选取的导航点。具体的，可以通过slam建图的方法恢复 三维环境特征，构建全局地图，然后依据搭建的地图实时规划进行路线。
具体的，在S2中，所述提取各个时间戳下的图像帧的特征点，包括：对所 提取的特征点进行特征匹配。
其中，若两个特征点的描述子在向量空间的距离相近，则合并为同一个特 征点。一个图像帧的特征点的信息，包括了关键点和描述子，所述关键点表示 特征点在图像中的位置、方向和尺度信息，所述描述子表示特征点周围像素的 信息。具体来说，为了能够更好的进行图像匹配，需要在图像中选择具有代表 性的区域，也就是特征点。一幅图像中的特征点信息由关键点和描述子构成。 其中关键点表示特征点在图像中的位置、方向和尺度信息，描述子表示特征点 周围像素的信息。在进行特征匹配的时候，若两个特征点的描述子在向量空间 的距离相近，则认为是同一个特征点。进一步的，S2中，所述利用所提取的图像帧的特征点和所获取的三维坐标点，得到里程计信息，包括：获取特征点在 所述相机坐标系下不同时刻的三维坐标点，并建立旋转平移变换矩阵(RT，Rotate Trans l at i onMatr i x)。
通过所述旋转平移变换矩阵得到所述双目相机在空间中的运动轨迹，所述 里程计信息包括了所述运动轨迹的参数，所述运动轨迹的参数包括旋转方向和 平移距离。实际应用中，得到的旋转平移变换矩阵也就表明了相机在空间中的 运动轨迹，包括旋转方向和平移距离，也就获得了相机从开始以来的里程计信 息。由于相机和四足机器人运动本体相对位置保持不变，因此可以得出四足机 器人的里程计信息。考虑到嵌入式设备运算量的问题，需要筛选合适的关键帧 作为相邻帧之间变换矩阵的计算。
具体的在S3中，所述在所述足式机器人移动的过程中，获取关键帧，包括：
在所述足式机器人移动的过程中，获取各个时间段内拍摄的图像帧，并存 储为对应每个时间段的图像帧集合。在每个图像帧集合中，提取各个图像帧中 的特征点的描述子信息，并根据描述子信息在空间向量中的距离，筛选各个图 像帧集合的关键帧。
所述对所提取的特征点进行特征匹配，包括：提取同一时间戳下的左目灰 度图像和右目灰度图像的特征点，并进行特征匹配。如图1所示的，在进行了特 征匹配之后，依次通过三角化测量和PNP算法，获取所述双目相机的视野内拍摄 到的物体在所述相机坐标系下的三维坐标点。其中，对于同一特征点，计算得 到在相机坐标系下不同时刻的三维坐标点的RT旋转平移变换矩阵。三角测量即 通过左右相机中的同一特征点计算得到该特征点的深度信息。
PnP(Perspect i ve-n-Poi nt)则是求解3D到2D点对运动的方法。
具体的，在获取所述双目相机的视野内拍摄到的各种物体在所述相机坐标 系下的三维坐标点的过程中，包括：每次估计当前图像帧相对于上一图像帧的 位姿Tcr，则当前帧相对世界坐标系的坐标即为：Tcw＝TcrTrw，其中，上一帧相对 世界坐标系的坐标为Trw。
其中，所述PNP算法包括：
空间点P的齐次坐标为P＝(X,Y,Z,1)，X、Y、Z分别为xyz三轴的坐标， 投影到图像1中的特征点为x1＝(u1,v1,1)T，其中，u1表示空间坐标x轴归一 化之后的结果参数，v1表示空间坐标y轴归一化之后的结果参数，其中，在相机 1下的归一化坐标为x1＝(u1,v1,1)，归一化坐标可由像素坐标p1转换而来： x1＝K-1p1；。
其中，所述双目相机的位姿为[R|t]，R表示旋转矩阵，t表示平移向量，我 们可以以展开形式表示该投影关系,其中s是P点在相机坐标系的三维坐标的Z 轴的值，即P点的深度：
用最后一行消去s，得到约束：
定义行向量有：
t1＝(t1,t2,t3,t4)T,t2＝(t5,t6,t7,t8)T,t3＝(t9,t10,t11,t12)T
于是有：
假设有n个点，可以构建线性方程组，如下：
由于t具有12维度，所以一共需要6对匹配点对才能实现上式的线性求解， 也可以使用SVD(Singular Value Decomposition，奇异值分解)等方法求得最 小二乘解。
本实施例中采用基于两两帧的方法估计位姿，即每次估计当前帧相对于上 一帧的位姿Tcr，我们知道上一帧相对世界坐标系的坐标为Trw，则当前帧相对世 界坐标系的坐标即为：
Tcw＝TcrTrw
进一步的，在按照所述规划行进路线进行导航的过程中，还包括：
检测所述双目相机当前拍摄的图像帧与之前拍摄的关键帧是否有重合的部 分，若是则判定所述多足机器人第二次路过所述地图中的同一个空间点，并重 定位所述多足机器人。
通过TF算法(TransForm)计算当前所述多足机器人的位姿。例如：如多足 机器人路过相同的空间点，则双目相机拍摄的图像中的帧与前面时间的关键帧 有重合的部分，将重定位机器人，然后通过TF算法(TransForm)计算当前机器 人的位姿。
本实施例中，可以利用词袋模型进行图像的回环检测，来重定位机器人， 其具体步骤如下：
在机器人运动过程中，若行驶到曾经行驶过的路径，即相机拍摄到的图像 与存储的图像库中的某一帧吻合，则重新评估已经建立的里程计数据，构建回 环检测。具体来说，包括：
建立一个视觉字典树以及从初始时刻到现在的图像建立起来的顺序索引和 逆序索引，首先需要对历史图像进行特征提取，并将提取的特征描述聚类(将 相似的放在同一个路径)，字典树的作用在于丰富字典的内容以及查询当前图像 的特征信息是否已经存在；
本实施例中的视觉字典树的建立方式，包括：提取当前帧的特征点，计算 相应的特征描述子，得到描述该帧图像的词袋向量；在视觉字典树上利用逆序 索引找到与当前图像拥有相同单词的一系列图像作为回环检测的候选图像；接 着计算当前图像与候选图像(之前的图像)的相似度，将相似度最高的作为回 环对；最后对于得到的回环对进行验证，检验是否为正确的回环。这其中，可 以将里程计数据估计独立于回环检测的结果，即回环检测的全局估计会收到VIO 的影响，但VIO不受全局估计的影响。
具体举例来说，如图1所示的，将双目相机设置在机器人上，双目相机包括 左目相机和右目相机，将从左目相机和右目相机获取到的左目灰度图像和右目 灰度图像生成RGBD图像，直接获取目标点的深度信息。具体的，可以利用IMU(惯 性测量单元)获取加速度及角速度数据。利用激光雷达获取四周环境障碍物的距 离数据。
在实际应用中，如图1所示的，需要对双目相机输出的图像进行平滑处理， 以防止足式机器人在运动过程中的上下起伏影响之后的数据处理结果，方法在 于对输出图像的限幅处理，例如输入图像大小为1024*768，根据足式机器人的 步伐起伏大小，对图像的高采取上下限的削减，最终修改为1024*720；
获取双目相机的所拍摄图像的特征点的方式，可以理解为：
采用ORB算法实现对图像中特征点的提取，其可以快速地识别找到图像中的 所有特征点并对其添加描述子(使用二进制表示方法形容特征点的独特性)，其 特点在于使用区域分割和NMS(Non-Max imum Suppress ion)方法，对图像的各 个区域中特征点数量实现限制，防止特征点扎堆现象；同时对所有特征点根据 角点相应值(即特点明显度)进行排序，选取靠前的N个角点作为单张图像的特 征点集合；并且利用该方法选取出的特征点还具有尺度不变性以及旋转不变性 的特点，大大提高了特征点的独特性；
选择特征点数目超过设定值的帧作为关键帧，这其中，特征点的数目取决 于拍摄到的图片，如果图片是纯色那么一个特征点都没有，其中特征点数目最 多的关键帧为参考帧；若同一时间段内存在若干个参考帧，则从所述若干个参 考帧中选取在时间轴上与前一帧最接近的关键帧作为参考帧，
进行双目相机以及I MU的初始化，操作流程先对当前帧与首帧三角化求解空 间坐标系下的位姿，再利用3d-2d：Pnp(Perspect ive-n-Poi nt)方法求解每帧 特征点位姿，接着对IMU进行角速度零偏初始化，使用相邻两帧之间的相机旋转、 IMU积分旋转，之后可以采用常用的技术手段构建最小二乘问题，并求解。
在机器人运动过程中，双目相机由于自身结构的关系能够直接获取深度图 像，故能够直接获取特征点的三维坐标，之后利用求得至少三对特征点的三维 坐标点和图像上的二维坐标点，利用三角形对应关系最终求出各个特征点在相 机坐标系下的三维坐标，最后利用给定配对好的两组3D点，求解运动过程中的 旋转平移矩阵，根据相对运动的原理，也就可以得到相机(也可以直接理解为 足式机器人本体)的运动轨迹。
本实施例公开了一种基于双目视觉里程计的足式机器人自主建图与导航方 法，涉及智能行走设备技术领域，能够在降低硬件成本的场景下，完成机器人 的建图和导航功能。本方法在足式机器人平台上搭载有前视双目摄像头，激光 雷达雷达，高精度IMU以及两套嵌入式控制板。足式机器人的建图采用基于激 光雷达的SLAM(同步定位与地图构建)方法构建室内平面地图，由于足式机器 人不同于轮式机器人无法直接获取到里程计信息，所以采用视觉里程计信息作 为替代方案。使用视觉里程计数据、IMU数据以及激光雷达数据实现SLAM建图 功能，之后再利用双目相机、IMU以及激光雷达等传感器在之前建好的地图上进 行导航。本发明实现了足式机器人在缺少轮式里程计的背景下，采用双目相机 的图像数据处理得到的里程计数据作为输入之一，协同I MU数据以及激光雷达 数据，实现足式机器人在非GPS环境下的实时位姿估计以及环境感知，极大地 提高了足式机器人的智能水平。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相 似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。 尤其，对于设备实施例而言，由于其基本相似于方法实施例，所以描述得比较 简单，相关之处参见方法实施例的部分说明即可。以上所述，仅为本发明的具 体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术 人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发 明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。
Claims (10)
1.一种基于双目视觉里程计的足式机器人建图与导航方法，其特征在于，包括：
S1、通过双目相机采集图像帧；
S2、提取各个时间戳下的图像帧的特征点，并建立相机坐标系，之后利用所提取的特征点和所述相机坐标系，得到里程计信息；
S3、在所述足式机器人移动的过程中，获取关键帧；
S4、通过扩展卡尔曼滤波器对里程计信息进行滤波估计，得到里程计修正数据；
S5、通过激光雷达获取环境感知数据，所述环境感知数据包括：所述足式机器人与障碍物的距离数据；
S6、利用S4所获取的里程计修正数据、S5所获取的环境感知数据和惯性测量单元采集的数据，建立用于底层运动控制的子地图，其中，由惯性测量单元采集的数据包括：加速度测量数据和角速度测量数据；
S7、根据所述子地图进行优化更新并得到地图。
2.根据权利要求1所述的方法，其特征在于，在所述足式机器人上安装有视觉里程计模块、底层运动模块和智能控制模块；
所述视觉里程计模块包括了安装在所述足式机器人上的双目相机，所述双目相机包括左目相机和右目相机；
所述底层运动模块包括了惯性测量单元(IMU)，所述惯性测量单元用于获取所述足式机器人的加速度数据和角速度数据；所述足式机器人上还安装有激光雷达，所述激光雷达用于获取所述足式机器人与障碍物的距离数据；
所采集的图像帧包括：所述左目相机采集的左目灰度图像的图像帧和所述右目相机采集的右目灰度图像的图像帧。
3.根据权利要求1或2所述的方法，其特征在于，S2中，所述利用所提取的图像帧的特征点和所获取的三维坐标点，得到里程计信息，包括：
获取特征点在所述相机坐标系下不同时刻的三维坐标点，并建立旋转平移变换矩阵(RT，Rotate Translation Matrix)；
通过所述旋转平移变换矩阵得到所述双目相机在空间中的运动轨迹，所述里程计信息包括了所述运动轨迹的参数，所述运动轨迹的参数包括旋转方向和平移距离。
4.根据权利要求1所述的方法，其特征在于，在S2中，所述提取各个时间戳下的图像帧的特征点，包括：
对所提取的特征点进行特征匹配，其中，若两个特征点的描述子在向量空间的距离相近，则合并为同一个特征点；一个图像帧的特征点的信息，包括了关键点和描述子，所述关键点表示特征点在图像中的位置、方向和尺度信息，所述描述子表示特征点周围像素的信息。
5.根据权利要求4所述的方法，其特征在于，在S3中，所述在所述足式机器人移动的过程中，获取关键帧，包括：
在所述足式机器人移动的过程中，获取各个时间段内拍摄的图像帧，并存储为对应每个时间段的图像帧集合；
在每个图像帧集合中，提取各个图像帧中的特征点的描述子信息，并根据描述子信息在空间向量中的距离，筛选各个图像帧集合的关键帧。
6.根据权利要求4所述的方法，其特征在于，所述对所提取的特征点进行特征匹配，包括：提取同一时间戳下的左目灰度图像和右目灰度图像的特征点，并进行特征匹配；
在进行了特征匹配之后，依次通过三角测量和PNP算法，获取所述双目相机的视野内拍摄到的物体在所述相机坐标系下的三维坐标点。
7.根据权利要求1或6所述的方法，其特征在于，在获取所述双目相机的视野内拍摄到的各种物体在所述相机坐标系下的三维坐标点的过程中，包括：
每次估计当前图像帧相对于上一图像帧的位姿Tcr，则当前帧相对世界坐标系的坐标即为：Tcw＝TcrTrw，其中，上一帧相对世界坐标系的坐标为Trw。
8.根据权利要求1所述的方法，其特征在于，在按照所述规划行进路线进行导航的过程中，还包括：
检测所述双目相机当前拍摄的图像帧与之前拍摄的关键帧是否有重合的部分，若是则判定所述多足机器人第二次路过所述地图中的同一个空间点，并重定位所述多足机器人；
通过TF算法(TransForm)计算当前所述多足机器人的位姿。
9.根据权利要求8所述的方法，其特征在于，所述对所述子地图进行优化更新并得到地图，包括：
所述多足机器人利用所述子地图开始进行移动，并在移动过程中，实时检测所述多足机器人第二次路过所述地图中的同一个空间点并记录，作为修订点；
在每一给更新周期中，获取至少一个修订点后并记录在所述子地图中；
当所述子地图中的修订点停止增加后，停止更新并将最后一次更新的子地图作为所述地图。
10.根据权利要求8所述的方法，其特征在于，所述判定所述多足机器人第二次路过所述地图中的同一个空间点，包括：
所述双目相机当前拍摄的图像帧与之前拍摄的关键帧，存在至少10个相同的特征点，则判定为同一个空间点。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210321993.7A CN115049910A (zh) | 2022-03-29 | 2022-03-29 | 一种基于双目视觉里程计的足式机器人建图与导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210321993.7A CN115049910A (zh) | 2022-03-29 | 2022-03-29 | 一种基于双目视觉里程计的足式机器人建图与导航方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115049910A true CN115049910A (zh) | 2022-09-13 |
Family
ID=83157991
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210321993.7A Pending CN115049910A (zh) | 2022-03-29 | 2022-03-29 | 一种基于双目视觉里程计的足式机器人建图与导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115049910A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115946140A (zh) * | 2023-02-10 | 2023-04-11 | 苏州浪潮智能科技有限公司 | 一种基于机器人技术的存储设备上架装置及方法 |
| CN118365888A (zh) * | 2024-06-19 | 2024-07-19 | 广汽埃安新能源汽车股份有限公司 | 去除图像中动态物体的方法、装置、电子设备及存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建系统 |
| CN108827306A (zh) * | 2018-05-31 | 2018-11-16 | 北京林业大学 | 一种基于多传感器融合的无人机slam导航方法及系统 |
| CN111190424A (zh) * | 2020-02-19 | 2020-05-22 | 苏州大学 | 一种用于室内移动机器人自主定位建图的一体化传感器 |
| CN113390411A (zh) * | 2021-06-10 | 2021-09-14 | 中国北方车辆研究所 | 基于变构型感知装置的足式机器人导航及定位方法 |
| CN215642280U (zh) * | 2021-04-06 | 2022-01-25 | 南京工程学院 | 一种室内轮式搜救机器人的建图系统 |
-
2022
- 2022-03-29 CN CN202210321993.7A patent/CN115049910A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建系统 |
| CN108827306A (zh) * | 2018-05-31 | 2018-11-16 | 北京林业大学 | 一种基于多传感器融合的无人机slam导航方法及系统 |
| CN111190424A (zh) * | 2020-02-19 | 2020-05-22 | 苏州大学 | 一种用于室内移动机器人自主定位建图的一体化传感器 |
| CN215642280U (zh) * | 2021-04-06 | 2022-01-25 | 南京工程学院 | 一种室内轮式搜救机器人的建图系统 |
| CN113390411A (zh) * | 2021-06-10 | 2021-09-14 | 中国北方车辆研究所 | 基于变构型感知装置的足式机器人导航及定位方法 |
Non-Patent Citations (3)
| Title |
|---|
| DAI YATING等: "Asynchronous fusion in positioning of mobile robot based on vision-aided inertial navigation", 《2017 36TH CHINESE CONTROL CONFERENCE (CCC)》, 11 September 2017 (2017-09-11), pages 6685 - 6690 * |
| 于海龙: "基于双目视觉的室内移动机器人SLAM研究与实现", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 2, 15 February 2020 (2020-02-15) * |
| 彭格锋: "四足机器人位姿估计与自主导航", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 3, 15 March 2022 (2022-03-15), pages 140 - 1197 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115946140A (zh) * | 2023-02-10 | 2023-04-11 | 苏州浪潮智能科技有限公司 | 一种基于机器人技术的存储设备上架装置及方法 |
| CN118365888A (zh) * | 2024-06-19 | 2024-07-19 | 广汽埃安新能源汽车股份有限公司 | 去除图像中动态物体的方法、装置、电子设备及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11900536B2 (en) | Visual-inertial positional awareness for autonomous and non-autonomous tracking | |
| CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN108051002B (zh) | 基于惯性测量辅助视觉的运输车空间定位方法及系统 | |
| Kanade et al. | Real-time and 3D vision for autonomous small and micro air vehicles | |
| US20220292711A1 (en) | Pose estimation method and device, related equipment and storage medium | |
| CN112254729B (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
| CN109211241A (zh) | 基于视觉slam的无人机自主定位方法 | |
| CN106017463A (zh) | 一种基于定位传感装置的飞行器定位方法 | |
| CN112197770A (zh) | 一种机器人的定位方法及其定位装置 | |
| CN112740274A (zh) | 在机器人设备上使用光流传感器进行vslam比例估计的系统和方法 | |
| CN115049910A (zh) | 一种基于双目视觉里程计的足式机器人建图与导航方法 | |
| CN112652001B (zh) | 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统 | |
| CN113515128B (zh) | 一种无人车实时路径规划方法及存储介质 | |
| CN113189613B (zh) | 一种基于粒子滤波的机器人定位方法 | |
| CN115479598A (zh) | 基于多传感器融合的定位与建图方法及紧耦合系统 | |
| CN110260866A (zh) | 一种基于视觉传感器的机器人定位与避障方法 | |
| CN113639743B (zh) | 一种基于行人步长信息辅助的视觉惯性slam定位方法 | |
| EP4088884A1 (en) | Method of acquiring sensor data on a construction site, construction robot system, computer program product, and training method | |
| CN116352722A (zh) | 多传感器融合的矿山巡检救援机器人及其控制方法 | |
| CN113096190A (zh) | 基于视觉建图的全向移动机器人导航方法 | |
| CN110780325B (zh) | 运动对象的定位方法及装置、电子设备 | |
| CN116380079A (zh) | 一种融合前视声呐与orb-slam3的水下slam方法 | |
| CN115060268A (zh) | 一种机房融合定位方法、系统、设备及存储介质 | |
| CN114529585A (zh) | 基于深度视觉和惯性测量的移动设备自主定位方法 | |
| CN113971438A (zh) | 荒漠环境下多传感器融合的定位与建图方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |