CN113175925A - 定位与导航系统以及方法 - Google Patents

定位与导航系统以及方法 Download PDF

Info

Publication number
CN113175925A
CN113175925A CN202110403035.XA CN202110403035A CN113175925A CN 113175925 A CN113175925 A CN 113175925A CN 202110403035 A CN202110403035 A CN 202110403035A CN 113175925 A CN113175925 A CN 113175925A
Authority
CN
China
Prior art keywords
information
point cloud
robot
cloud image
positioning
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.)
Granted
Application number
CN202110403035.XA
Other languages
English (en)
Other versions
CN113175925B (zh
Inventor
魏翼鹰
孟庆磊
罗鹏飞
李涛
陈怡锦
卢宇飞
吴会豪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN202110403035.XA priority Critical patent/CN113175925B/zh
Publication of CN113175925A publication Critical patent/CN113175925A/zh
Application granted granted Critical
Publication of CN113175925B publication Critical patent/CN113175925B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供一种定位与导航系统以及方法，所述系统包括：激光雷达模块，用于扫描所述激光雷达模块所在高度平面内的环境区域，识别预设半径内的障碍物的位置信息，并根据所述障碍物的位置信息生成环境点云图像；双目视觉相机模块，用于获取所述机器人行进前方的视觉点云图像；惯性测量模块，用于采集所述机器人的加速度信息和角速度信息；轮式里程测量模块，用于采集所述机器人的位移路径；工控机，用于根据所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径对所述机器人进行定位和导航。本发明提高了定位与导航系统的鲁棒性和定位精度。

Description

定位与导航系统以及方法
技术领域
本发明涉及机器人的定位导航技术领域，具体涉及一种定位与导航系统及方法。
背景技术
现有技术中实现机器人的定位与导航只基于激光雷达、只基于双目视觉相机、或者激光雷达与双目视觉相机结合的技术。
只基于激光雷达实现机器人的定位与导航技术由于激光雷达的精度越高测距范围越大其价格会很昂贵，低精度低测距范围的激光雷达不能满足市面上低成本特定环境下的导航使用。只基于双目视觉相机实现机器人的定位与导航技术由于双目视觉相机容易受到光照和自身运动的影响，工作中易积累定位误差或丢失视觉图像特征而导致定位失败。基于激光雷达与双目视觉相机结合的技术存在定位精度不高的技术问题。
因此，急需提出一种定位与导航系统及方法解决现有技术中存在的定位与导航系统适用性不高以及定位精度不高的技术问题。
发明内容
本发明提供一种定位与导航系统及方法，旨在解决现有技术中存在的定位与导航系统适用性不高以及定位精度不高的技术问题。
一方面，本发明提供一种定位与导航系统，所述定位与导航系统包括：
激光雷达模块，设置于所述机器人的顶部，用于扫描所述激光雷达模块所在高度平面内的环境区域，识别预设半径内的障碍物的位置信息，并根据所述障碍物的位置信息生成环境点云图像；
双目视觉相机模块，设置于所述机器人的第一侧壁，用于获取所述机器人行进前方的视觉点云图像；
惯性测量模块，设置于所述机器人的几何中心位置，用于采集所述机器人的加速度信息和角速度信息；
轮式里程测量模块，设置于所述机器人的车轮上，用于采集所述机器人的位移路径；
工控机，用于接收所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径，并根据所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径对所述机器人进行定位和导航。
在本发明一种可能的实现方式中，所述工控机包括：
定位模块，用于接收所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径，并将所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径进行融合，获得融合信息，并根据所述融合信息对所述机器人进行定位，获得定位信息；
导航模块，用于获取所述机器人的目的地以及场景地图，根据所述目的地、所述场景地图以及所述定位信息，对所述机器人进行导航。
在本发明一种可能的实现方式中，所述定位模块包括：
第一分析单元，用于接收所述加速度信息和所述角速度信息，并根据所述加速度信息和所述角速度信息计算所述机器人的第一位姿信息；
第二分析单元，用于接收所述环境点云图像，并根据所述环境点云图像计算所述机器人的第二位姿信息；
第三分析单元，用于接收所述视觉点云图像，并根据所述视觉点云图像和所述第二位姿信息计算所述机器人的第三位姿信息；
融合单元，用于采用扩展卡尔曼滤波算法将所述第一位姿信息、所述第二位姿信息、所述第三位姿信息以及所述位移路径进行融合，得到所述融合信息；
定位单元，用于接收所述融合信息，并根据所述融合信息对所述机器人进行定位，生成所述定位信息。
在本发明一种可能的实现方式中，所述视觉点云图像包括多帧子视觉点云图像，所述第三分析单元包括：
第一帧图像获取单元，用于获取所述多帧子视觉点云图像中的第一帧子视觉点云图像，提取所述第一帧子视觉点云图像的第一感兴趣区域，并提取所述第一感兴趣区域中的第一特征点；
图像获取时刻计算单元，用于根据所述第一位姿信息、所述位移路径和所述第一帧子视觉点云图像计算获取第二帧子视觉点云图像的获取时刻；
第二帧图像获取单元，用于在所述获取时刻获取所述多帧子视觉点云图像中的第二帧子视觉点云图像，并提取与所述第一特征点对应的第二特征点；
分析单元，用于根据所述第一特征点和所述第二特征点计算所述机器人的第三位姿信息。
在本发明一种可能的实现方式中，所述图像获取时刻计算单元包括：
预测单元，用于根据所述第一位姿信息和所述位移路径预测所述第一帧子视觉点云图像的消失时刻；
时刻确定单元，用于根据所述消失时刻确定获取所述第二帧子视觉点云图像的获取时刻；
其中，所述获取时刻早于所述消失时刻。
在本发明一种可能的实现方式中，所述第二帧图像获取单元包括：
子视觉点云图像获取单元，用于在所述获取时刻获取所述多帧子视觉点云图像中的第二帧子视觉点云图像；
第二感兴趣区域确定单元，用于根据所述第一位姿信息和所述位移路径估测所述第二帧子视觉点云图像中与所述第一感兴趣区域对应的第二感兴趣区域；
特征点提取单元，用于在所述第二感兴趣区域中提取与所述第一特征点对应的所述第二特征点。
在本发明一种可能的实现方式中，所述激光雷达模块还用于测量障碍物与所述机器人之间的距离，若所述障碍物与所述机器人之间的距离小于设定的报警距离，则发出报警信号；所述工控机还用于接收所述报警信号，并根据所述报警信号控制所述机器人停止行驶。
在本发明一种可能的实现方式中，所述场景地图包括全局地图，所述导航模块包括：
全局地图获取单元，用于获取预先构建好的全局地图；
第一路径规划单元，用于根据所述全局地图、所述机器人的目的地以及所述定位信息，对所述机器人的进行路径规划，获得最优规划路径；
第一导航单元，用于指示所述机器人按照所述最优规划路径进行行驶。
在本发明一种可能的实现方式中，所述场景地图包括局部地图，所述导航模块包括：
局部地图构建单元，用于根据所述环境点云图像和所述视觉点云图像构建所述局部地图；
第二路径规划单元，用于根据所述机器人的目的地、所述局部地图以及所述定位信息，对所述机器人进行初始路径规划，获得初始规划路径；
规划路径优化单元，用于在所述机器人按照所述初始规划路径行驶的过程中，对所述局部地图进行补充，获得补充局部地图，并根据所述补充局部地图多所述初始规划路径进行优化，获得优化规划路径；
第二导航单元，用于指示所述机器人按照所述优化规划路径进行行驶。
另一方面，本发明提供一种定位与导航方法，包括：
通过激光雷达模块扫描所述激光雷达模块所在高度平面内的环境区域，识别预设半径内的障碍物的位置信息，并根据所述障碍物的位置信息生成环境点云图像；
通过双目视觉相机模块获取所述机器人行进前方的视觉点云图像；
通过惯性测量模块采集所述机器人的加速度信息和角速度信息；
通过轮式里程测量模块采集所述机器人的位移路径；
通过工控机接收所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径，并根据所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径对所述机器人进行定位和导航。
本发明通过利用激光雷达模块、双目视觉相机模块、惯性测量模块和轮式里程测量模块共同对机器人进行定位，提高了定位与导航系统的鲁棒性，从而提高了定位与导航系统的定位精度。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的机器人的一个实施例结构示意图；
图2是本发明实施例提供的定位与导航系统的一个实施例结构示意图；
图3是本发明实施例提供的定位模块的一个实施例结构示意图；
图4是本发明实施例提供的第三分析单元的一个实施例结构示意图；
图5是本发明实施例提供的图像获取时刻计算单元的一个实施例结构示意图；
图6是本发明实施例提供的第二帧图像获取单元的一个实施例结构示意图；
图7是本发明实施例提供的导航模块的一个实施例结构示意图；
图8是本发明实施例提供的导航模块的另一个实施例结构示意图；
图9是本发明实施例提供的导航及定位方法的一个实施例流程示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本发明的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。
本发明提供了一种定位与导航系统及方法，以下分别进行详细说明。
图1为本发明实施例提供的机器人的一个实施例结构示意图，图2为本发明实施例提供的定位与导航系统的一个实施例结构示意图，如图1和图2所示，该定位与导航系统10用于对机器人2进行定位和导航，定位与导航系统10包括：
激光雷达模块101，设置于机器人2的顶部21，用于扫描激光雷达模块101所在高度平面内的环境区域，识别预设半径内的障碍物的位置信息，并根据障碍物的位置信息生成环境点云图像；
双目视觉相机模块102，设置于机器人2的第一侧壁22，用于获取机器人2行进前方的视觉点云图像；
惯性测量模块103，设置于机器人2的几何中心位置，用于采集机器人2的加速度信息和角速度信息；
轮式里程测量模块104，设置于机器人2的车轮23上，用于采集机器人2的位移路径；
工控机105，用于接收环境点云图像、视觉点云图像、加速度信息、角速度信息以及位移路径，并根据环境点云图像、视觉点云图像、加速度信息、角速度信息以及位移路径对机器人2进行定位和导航。
本发明实施例通过利用激光雷达模块101、双目视觉相机模块102、惯性测量模块103和轮式里程测量模块104共同对机器人2进行定位，提高了定位与导航系统10的鲁棒性，从而提高了定位与导航系统10的定位精度。
在本发明的一个实施例中，激光雷达模块101的预设半径为3m-8m。
进一步地，如图2所示，工控机105包括：
定位模块201，用于接收环境点云图像、视觉点云图像、加速度信息、角速度信息以及位移路径，并将环境点云图像、视觉点云图像、加速度信息、角速度信息以及位移路径进行融合，获得融合信息，并根据融合信息对机器人2进行定位，获得定位信息；
导航模块202，用于获取机器人2的目的地以及场景地图，根据目的地、场景地图以及定位信息，对机器人2进行导航。
本发明实施例通过将环境点云图像、视觉点云图像、加速度信息、角速度信息以及位移路径进行融合，获得融合信息，并根据融合信息对机器人2进行定位，可实现在不同场景切换下仍保证对机器人2定位的精确性，增强了定位与导航系统10的使用环境范围和抗干扰能力。
进一步地，如图3所示，定位模块201包括：
第一分析单元301，用于接收加速度信息和角速度信息，并根据加速度信息和角速度信息计算机器人2的第一位姿信息；
第二分析单元302，用于接收环境点云图像，并根据环境点云图像计算机器11人的第二位姿信息；
第三分析单元303，用于接收视觉点云图像，并根据视觉点云图像和第二位姿信息计算机器人2的第三位姿信息；
融合单元304，用于采用扩展卡尔曼滤波算法将第一位姿信息、第二位姿信息、第三位姿信息以及位移路径进行融合，得到融合信息；
定位单元305，用于接收融合信息，并根据融合信息对机器人2进行定位，生成定位信息。
具体地，融合单元304包括三次融合过程，分别是：首先，将第一位姿信息与位移路径进行第一次融合，获得第一子融合信息，然后，再将第一子融合信息与第二位姿信息进行第二次融合，获得第二子融合信息，最后，再将第二子融合信息与第三位姿信息进行第三次融合，获得融合信息。
其中，将第一位姿信息与位移路径进行第一次融合具体为：
惯性测量模块103(Inertial Measurement Unit，IMU)包含三轴陀螺仪与三轴加速度计，实时测量记录机器人的三轴转动的角速度wx、wy、wz和三个方向的加速度信息ax、ay、az。轮式里程测量模块104实时测量记录机器人车轮转动的里程数据S。惯性测量模块103利用测得的六个机器人运动信息和时间进行时刻计算估计出机器人的位姿和位移信息。
可以通过三个角速度信息求取相应的机器人姿态角度的变化，
Figure BDA0003019703730000071
将该信息进行Δt时间差的计算积累，计算估算出机器人t(t＝Δt1+Δt2+Δt3+…)时间差后的位姿信息。
可以通过三个加速度信息求取估算机器人的位移变化情况，
Figure BDA0003019703730000081
Figure BDA0003019703730000082
Figure BDA0003019703730000083
Figure BDA0003019703730000084
惯性测量模块103和轮式里程测量模块104二者的信息进行一定的融合，弥补二者在机器人运动过程中可能出现的问题，以及进行优化位姿和定位的精度。当惯性测量模块103的加速度信息基本没有变化时，轮式里程测量模块104位移信息S持续增加，表明机器人出现打滑现象，则将该段的轮式里程测量模块104位移信息S的增加值扣除。
轮式里程测量模块104实时测量记录机器人车轮转动的里程数据S，其中机器人的里程数据S可以配合惯性测量模块103的姿态角估计信息
Figure BDA0003019703730000085
将无方向的里程数据S融合θ后变为分段累计的矢量
Figure BDA0003019703730000086
将惯性测量模块103测量出的
Figure BDA0003019703730000087
和轮式里程测量模块104的位移信息
Figure BDA0003019703730000088
时时进行平均拟合计算。即机器人从开始运行之后，下一个“位置和位姿”的判断情况是，关于上一个状态及期间惯性测量模块103和轮式里程测量模块104收集的信息的基础上获得的累计平均结果。从而获得第一次融合信息。同时进行惯性测量模块103和轮式里程测量模块104数据的更新，从而使得下一次估计位置和位姿时能够更加的精确以及消除累计的误差。二者拟合计算可以增加机器人位置和位姿估计的准确概率。
其中，第一子融合信息与第二位姿信息进行第二次融合，获得第二子融合信息具体为：激光雷达模块101通过自身的旋转，对周围360度方向上的障碍物进行顺序时时测距，获得周围障碍物所在的角度和距离信息。通过两次测得的同一障碍物的距离信息变化，估计车辆的位置和位姿变化，即第二位姿信息。将第一次融合信息于第二位姿信息进行第二次的融合。第一次融合信息中可以获得估计的机器人位置和位姿信息，通过该信息在所建立好的地图中可以得知对于第一次融合信息的理论障碍物距离信息。同时与激光雷达模块101获得的实际距离信息进行比对，将二者多个同样的标志性障碍物距离信息进行比对,拟合出较为精确的距离值。
Figure BDA0003019703730000091
Figure BDA0003019703730000092
通过该拟合过后的多个距离信息，利用建立好的地图信息进行再次估算机器人的位置和姿态信息，实现第二次信息融合优化，获得第二次融合信息。更新惯性测量模块103和轮式里程测量模块104的信息，同时在地图中更新估计的位置和位姿信息，以及导航规划路线的最优路线。
其中，将第二子融合信息与第三位姿信息进行第三次融合具体为：双目视觉相机模块102进行图像处理和视觉里程计的构建。两目视觉相机模块102的两个摄像头在同时成像在一个坐标系下，A为左右目摄像头所在平面，xL、xR在其上分别为左右目摄像头光圈中心点，B为两目摄像头的成像平面，两目摄像头光心之间的长度为xLR。A、B两平面之间的距离为xAB。这些初始化信息可根据计算出图像中某一特征点的深度信息
Figure BDA0003019703730000093
双目视觉相机模块102的测距信息是采用的间歇性测量，所以数据之间会有突变，形成不连续的视觉里程计信息。第二次融合的信息估计出来的机器人运动位移路径和位置、位姿信息，和双目视觉相机模块102测距形成的视觉里程计在同一时间轴上进行拟合优化。将视觉里程计信息间歇性的与第二次融合信息进行融合，融合时将惯性测量模块103、轮式里程测量模块104和视觉里程计信息输入进扩展卡尔曼滤波中进行一次的迭代计算，从而将三者的数据优化估计出更加精确的位置和位姿信息。双目视觉相机模块102每测量一次距离信息就进行一次三者的信息融合，优化估计的信息，同时在地图中更新估计的位置和位姿信息，以及导航规划路线的最优路线。
通过三次融合，对位姿信息进行拟合优化，最终获得融合信息，提高了定位信息的精确性。
进一步地，视觉点云图像包括多帧子视觉点云图像，如图4所示，第三分析单元303包括：
第一帧图像获取单元401，用于获取多帧子视觉点云图像中的第一帧子视觉点云图像，提取第一帧子视觉点云图像的第一感兴趣区域，并提取第一感兴趣区域中的第一特征点；
图像获取时刻计算单元402，用于根据第一位姿信息、位移路径和第一帧子视觉点云图像计算获取第二帧子视觉点云图像的获取时刻；
第二帧图像获取单元403，用于在获取时刻获取多帧子视觉点云图像中的第二帧子视觉点云图像，并提取与第一特征点对应的第二特征点；
分析单元404，用于根据第一特征点和第二特征点计算机器人2的第三位姿信息。
通过设置根据第一位姿信息和位移路径确定第二帧自视觉点云图像的获取时刻，可避免连续获取每一帧子视觉点云图像，实现在不处理连续多帧子视觉点云图像的情况下，可以相对精确的捕捉到与第一特征点对应的第二特征点，从而计算第三位姿信息。可大大降低双目视觉相机模块200的计算量，环节双目视觉相机模块200的计算压力以及由于计算量过大导致的信息连续性丢失和延时的问题，进而提高定位与导航系统10的适配性和适用性。
进一步地，如图5所示，图像获取时刻计算单元402包括：
预测单元501，用于根据第一位姿信息和位移路径预测第一帧子视觉点云图像的消失时刻；
时刻确定单元502，用于根据消失时刻确定获取第二帧子视觉点云图像的获取时刻；
其中，获取时刻早于消失时刻。
通过上述设置，可实现视觉点云图像的间断式获取。在本发明的一些实施例中，获取时刻早于消失时刻10μS-100μS。
进一步地，如图6所示，第二帧图像获取单元403包括：
子视觉点云图像获取单元601，用于在获取时刻获取多帧子视觉点云图像中的第二帧子视觉点云图像；
第二感兴趣区域确定单元602，用于根据第一位姿信息估测第二帧子视觉点云图像中与第一感兴趣区域对应的第二感兴趣区域；
特征点提取单元603，用于在第二感兴趣区域中提取与第一特征点对应的所述第二特征点。
进一步地，为了提高定位与导航系统10的安全可靠性，在本发明的一些实施例中，激光雷达模块101还用于测量障碍物与机器人2之间的距离，若障碍物与机器人2之间的距离小于设定的报警距离，则发出报警信号；工控机105还用于接收报警信号，并根据报警信号控制机器人2停止行驶。
通过上述设置，可避免机器人2在双目视觉相机模块102的视野盲区内与机器人发生碰撞，导致机器人2损坏，提高定位与导航系统10的安全可靠性，从而提高机器2人在定位与导航过程中的安全可靠性。
进一步地，场景地图包括全局地图，如图7所示，导航模块202包括：
全局地图获取单元701，用于获取预先构建好的全局地图；
其中，预先构建好的全局地图可预先存储在全局地图获取单元701中，直接调用即可，也可以是通过全局地图获取单元701向其他存储有全局地图的存储器中调取。
第一路径规划单元702，用于根据全局地图、机器人2的目的地以及定位信息，对机器人2的进行路径规划，获得最优规划路径；
第一导航单元703，用于指示机器人2按照最优规划路径进行行驶。
进一步地，在本发明的一些其他实施例中，场景地图包括局部地图，如图8所示，导航模块702包括：
局部地图构建单元801，用于根据环境点云图像和视觉点云图像构建局部地图；
第二路径规划单元802，用于根据机器人2的目的地、局部地图以及定位信息，对机器人2进行初始路径规划，获得初始规划路径；
规划路径优化单元803，用于在机器人2按照初始规划路径行驶的过程中，对局部地图进行补充，获得补充局部地图，并根据补充局部地图多初始规划路径进行优化，获得优化规划路径；
第二导航单元804，用于指示机器人2按照优化规划路径进行行驶。
通过设置两种导航方式，可进一步提高定位与导航系统10的适用性。
另一方面，如图9所示，本发明实施例中还提供一种定位与导航方法，包括：
S901、通过激光雷达模块101扫描激光雷达模块101所在高度平面内的环境区域，识别预设半径内的障碍物的位置信息，并根据障碍物的位置信息生成环境点云图像；
S902、通过双目视觉相机模块102获取机器人2行进前方的视觉点云图像；
S903、通过惯性测量模块103采集机器人2的加速度信息和角速度信息；
S904、通过轮式里程测量模块104采集机器人2的位移路径；
S905、通过工控机105接收环境点云图像、视觉点云图像、加速度信息、角速度信息以及位移路径，并根据环境点云图像、视觉点云图像、加速度信息、角速度信息以及位移路径对机器人2进行定位和导航。
本发明实施例通过利用激光雷达模块101、双目视觉相机模块102、惯性测量模块103和轮式里程测量模块104共同对机器人2进行定位，提高了定位与导航方法的鲁棒性，从而提高了定位与导航方法的定位精度。
进一步地，S905具体为：接收环境点云图像、视觉点云图像、加速度信息、角速度信息以及位移路径，并将环境点云图像、视觉点云图像、加速度信息、角速度信息以及位移路径进行融合，获得融合信息，并根据融合信息对机器人2进行定位，获得定位信息；获取机器人2的目的地以及场景地图，根据目的地、场景地图以及定位信息，对机器人2进行导航。
进一步地，S905中的获得融合信息具体为：接收加速度信息和角速度信息，并根据加速度信息和角速度信息计算机器人2的第一位姿信息；接收环境点云图像，并根据环境点云图像计算机器11人的第二位姿信息；接收视觉点云图像，并根据视觉点云图像和第二位姿信息计算机器人2的第三位姿信息；采用扩展卡尔曼滤波算法将第一位姿信息、第二位姿信息、第三位姿信息以及位移路径进行融合，得到融合信息；接收融合信息，并根据融合信息对机器人2进行定位，生成定位信息。
进一步地，视觉点云图像包括多帧子视觉点云图像，接收视觉点云图像，并根据视觉点云图像和第二位姿信息计算机器人2的第三位姿信息具体为：
获取多帧子视觉点云图像中的第一帧子视觉点云图像，提取第一帧子视觉点云图像的第一感兴趣区域，并提取第一感兴趣区域中的第一特征点；
根据第一位姿信息、位移路径和第一帧子视觉点云图像计算获取第二帧子视觉点云图像的获取时刻；
在获取时刻获取多帧子视觉点云图像中的第二帧子视觉点云图像，并提取与第一特征点对应的第二特征点；
根据第一特征点和第二特征点计算机器人2的第三位姿信息。
通过设置根据第一位姿信息和位移路径确定第二帧自视觉点云图像的获取时刻，可避免连续获取每一帧子视觉点云图像，实现在不处理连续多帧子视觉点云图像的情况下，可以相对精确的捕捉到与第一特征点对应的第二特征点，从而计算第三位姿信息。可大大降低双目视觉相机模块200的计算量，环节双目视觉相机模块200的计算压力以及由于计算量过大导致的信息连续性丢失和延时的问题，进而提高定位与导航方法的适配性和适用性。
进一步地，在本发明的一些实施例中，对机器人2进行导航可包括获取预先构建好的全局地图后，根据全图地图、机器人2的目的地以及定位信息，对机器人2进行导航，或者，包括根据环境点云图像和视觉点云图像构建局部地图，根据局部地图、机器人2的目的地以及定位信息对机器人2进行导航。
通过设置两种导航方式，可进一步提高定位于导航方法的适用性。
以上对本发明所提供的定位与导航系统及方法进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种定位与导航系统，其特征在于，所述定位与导航系统包括：
激光雷达模块，设置于所述机器人的顶部，用于扫描所述激光雷达模块所在高度平面内的环境区域，识别预设半径内的障碍物的位置信息，并根据所述障碍物的位置信息生成环境点云图像；
双目视觉相机模块，设置于所述机器人的第一侧壁，用于获取所述机器人行进前方的视觉点云图像；
惯性测量模块，设置于所述机器人的几何中心位置，用于采集所述机器人的加速度信息和角速度信息；
轮式里程测量模块，设置于所述机器人的车轮上，用于采集所述机器人的位移路径；
工控机，用于接收所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径，并根据所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径对所述机器人进行定位和导航。
2.根据权利要求1所述的定位与导航系统，其特征在于，所述工控机包括：
定位模块，用于接收所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径，并将所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径进行融合，获得融合信息，并根据所述融合信息对所述机器人进行定位，获得定位信息；
导航模块，用于获取所述机器人的目的地以及场景地图，根据所述目的地、所述场景地图以及所述定位信息，对所述机器人进行导航。
3.根据权利要求2所述的定位与导航系统，其特征在于，所述定位模块包括：
第一分析单元，用于接收所述加速度信息和所述角速度信息，并根据所述加速度信息和所述角速度信息计算所述机器人的第一位姿信息；
第二分析单元，用于接收所述环境点云图像，并根据所述环境点云图像计算所述机器人的第二位姿信息；
第三分析单元，用于接收所述视觉点云图像，并根据所述视觉点云图像和所述第二位姿信息计算所述机器人的第三位姿信息；
融合单元，用于采用扩展卡尔曼滤波算法将所述第一位姿信息、所述第二位姿信息、所述第三位姿信息以及所述位移路径进行融合，得到所述融合信息；
定位单元，用于接收所述融合信息，并根据所述融合信息对所述机器人进行定位，生成所述定位信息。
4.根据权利要求3所述的定位与导航系统，其特征在于，所述视觉点云图像包括多帧子视觉点云图像，所述第三分析单元包括：
第一帧图像获取单元，用于获取所述多帧子视觉点云图像中的第一帧子视觉点云图像，提取所述第一帧子视觉点云图像的第一感兴趣区域，并提取所述第一感兴趣区域中的第一特征点；
图像获取时刻计算单元，用于根据所述第一位姿信息、所述位移路径和所述第一帧子视觉点云图像计算获取第二帧子视觉点云图像的获取时刻；
第二帧图像获取单元，用于在所述获取时刻获取所述多帧子视觉点云图像中的第二帧子视觉点云图像，并提取与所述第一特征点对应的第二特征点；
分析单元，用于根据所述第一特征点和所述第二特征点计算所述机器人的第三位姿信息。
5.根据权利要求4所述的定位与导航系统，其特征在于，所述图像获取时刻计算单元包括：
预测单元，用于根据所述第一位姿信息和所述位移路径预测所述第一帧子视觉点云图像的消失时刻；
时刻确定单元，用于根据所述消失时刻确定获取所述第二帧子视觉点云图像的获取时刻；
其中，所述获取时刻早于所述消失时刻。
6.根据权利要求4所述的定位与导航系统，其特征在于，所述第二帧图像获取单元包括：
子视觉点云图像获取单元，用于在所述获取时刻获取所述多帧子视觉点云图像中的第二帧子视觉点云图像；
第二感兴趣区域确定单元，用于根据所述第一位姿信息和所述位移路径估测所述第二帧子视觉点云图像中与所述第一感兴趣区域对应的第二感兴趣区域；
特征点提取单元，用于在所述第二感兴趣区域中提取与所述第一特征点对应的所述第二特征点。
7.根据权利要求1所述的定位与导航系统，其特征在于，所述激光雷达模块还用于测量障碍物与所述机器人之间的距离，若所述障碍物与所述机器人之间的距离小于设定的报警距离，则发出报警信号；所述工控机还用于接收所述报警信号，并根据所述报警信号控制所述机器人停止行驶。
8.根据权利要求2所述的定位与导航系统，其特征在于，所述场景地图包括全局地图，所述导航模块包括：
全局地图获取单元，用于获取预先构建好的全局地图；
第一路径规划单元，用于根据所述全局地图、所述机器人的目的地以及所述定位信息，对所述机器人的进行路径规划，获得最优规划路径；
第一导航单元，用于指示所述机器人按照所述最优规划路径进行行驶。
9.根据权利要求2所述的定位与导航系统，其特征在于，所述场景地图包括局部地图，所述导航模块包括：
局部地图构建单元，用于根据所述环境点云图像和所述视觉点云图像构建所述局部地图；
第二路径规划单元，用于根据所述机器人的目的地、所述局部地图以及所述定位信息，对所述机器人进行初始路径规划，获得初始规划路径；
规划路径优化单元，用于在所述机器人按照所述初始规划路径行驶的过程中，对所述局部地图进行补充，获得补充局部地图，并根据所述补充局部地图多所述初始规划路径进行优化，获得优化规划路径；
第二导航单元，用于指示所述机器人按照所述优化规划路径进行行驶。
10.一种定位与导航方法，其特征在于，包括：
通过激光雷达模块扫描所述激光雷达模块所在高度平面内的环境区域，识别预设半径内的障碍物的位置信息，并根据所述障碍物的位置信息生成环境点云图像；
通过双目视觉相机模块获取所述机器人行进前方的视觉点云图像；
通过惯性测量模块采集所述机器人的加速度信息和角速度信息；
通过轮式里程测量模块采集所述机器人的位移路径；
通过工控机接收所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径，并根据所述环境点云图像、所述视觉点云图像、所述加速度信息、所述角速度信息以及所述位移路径对所述机器人进行定位和导航。
CN202110403035.XA 2021-04-14 2021-04-14 定位与导航系统以及方法 Active CN113175925B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110403035.XA CN113175925B (zh) 2021-04-14 2021-04-14 定位与导航系统以及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110403035.XA CN113175925B (zh) 2021-04-14 2021-04-14 定位与导航系统以及方法

Publications (2)

Publication Number Publication Date
CN113175925A true CN113175925A (zh) 2021-07-27
CN113175925B CN113175925B (zh) 2023-03-14

Family

ID=76923227

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110403035.XA Active CN113175925B (zh) 2021-04-14 2021-04-14 定位与导航系统以及方法

Country Status (1)

Country Link
CN (1) CN113175925B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113776515A (zh) * 2021-08-31 2021-12-10 南昌工学院 一种机器人导航方法、装置、计算机设备和存储介质
CN113932814A (zh) * 2021-09-30 2022-01-14 杭州电子科技大学 一种基于多模态地图的协同定位方法
CN116026335A (zh) * 2022-12-26 2023-04-28 广东工业大学 一种适用于未知室内环境的移动机器人定位方法与系统

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104865578A (zh) * 2015-05-12 2015-08-26 上海交通大学 一种室内停车场高精细地图生成装置及方法
CN107688184A (zh) * 2017-07-24 2018-02-13 宗晖（上海）机器人有限公司 一种定位方法以及系统
CN107747941A (zh) * 2017-09-29 2018-03-02 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
CN109059906A (zh) * 2018-06-26 2018-12-21 上海西井信息科技有限公司 车辆定位方法、装置、电子设备、存储介质
CN110108269A (zh) * 2019-05-20 2019-08-09 电子科技大学 基于多传感器数据融合的agv定位方法
CN110726409A (zh) * 2019-09-09 2020-01-24 杭州电子科技大学 一种基于激光slam和视觉slam地图融合方法
CN110842940A (zh) * 2019-11-19 2020-02-28 广东博智林机器人有限公司 一种建筑测量机器人多传感器融合三维建模方法及系统
CN111240331A (zh) * 2020-01-17 2020-06-05 仲恺农业工程学院 基于激光雷达和里程计slam的智能小车定位导航方法及系统
CN210954736U (zh) * 2019-12-06 2020-07-07 深圳市千乘机器人有限公司 一种户外自动巡检机器人

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104865578A (zh) * 2015-05-12 2015-08-26 上海交通大学 一种室内停车场高精细地图生成装置及方法
CN107688184A (zh) * 2017-07-24 2018-02-13 宗晖（上海）机器人有限公司 一种定位方法以及系统
CN107747941A (zh) * 2017-09-29 2018-03-02 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
CN109059906A (zh) * 2018-06-26 2018-12-21 上海西井信息科技有限公司 车辆定位方法、装置、电子设备、存储介质
CN110108269A (zh) * 2019-05-20 2019-08-09 电子科技大学 基于多传感器数据融合的agv定位方法
CN110726409A (zh) * 2019-09-09 2020-01-24 杭州电子科技大学 一种基于激光slam和视觉slam地图融合方法
CN110842940A (zh) * 2019-11-19 2020-02-28 广东博智林机器人有限公司 一种建筑测量机器人多传感器融合三维建模方法及系统
CN210954736U (zh) * 2019-12-06 2020-07-07 深圳市千乘机器人有限公司 一种户外自动巡检机器人
CN111240331A (zh) * 2020-01-17 2020-06-05 仲恺农业工程学院 基于激光雷达和里程计slam的智能小车定位导航方法及系统

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113776515A (zh) * 2021-08-31 2021-12-10 南昌工学院 一种机器人导航方法、装置、计算机设备和存储介质
CN113932814A (zh) * 2021-09-30 2022-01-14 杭州电子科技大学 一种基于多模态地图的协同定位方法
CN113932814B (zh) * 2021-09-30 2024-04-02 杭州电子科技大学 一种基于多模态地图的协同定位方法
CN116026335A (zh) * 2022-12-26 2023-04-28 广东工业大学 一种适用于未知室内环境的移动机器人定位方法与系统
CN116026335B (zh) * 2022-12-26 2023-10-03 广东工业大学 一种适用于未知室内环境的移动机器人定位方法与系统

Also Published As

Publication number Publication date
CN113175925B (zh) 2023-03-14

Similar Documents

Publication Publication Date Title
CN113175925B (zh) 定位与导航系统以及方法
EP3715785B1 (en) Slam assisted ins
US11433880B2 (en) In-vehicle processing apparatus
CN111986506B (zh) 基于多视觉系统的机械式停车位泊车方法
CN106017463B (zh) 一种基于定位传感装置的飞行器定位方法
KR101725060B1 (ko) 그래디언트 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
Kelly et al. Combined visual and inertial navigation for an unmanned aerial vehicle
JP5966747B2 (ja) 車両走行制御装置及びその方法
KR101439921B1 (ko) 비젼 센서 정보와 모션 센서 정보를 융합한 모바일 로봇용 ｓｌａｍ 시스템
KR102219843B1 (ko) 자율주행을 위한 위치 추정 장치 및 방법
CN110865650B (zh) 基于主动视觉的无人机位姿自适应估计方法
CN112254729B (zh) 一种基于多传感器融合的移动机器人定位方法
EP2175237B1 (en) System and methods for image-based navigation using line features matching
JP2004198211A (ja) 移動体周辺監視装置
CN109933056A (zh) 一种基于slam的机器人导航方法以及机器人
CN113639743B (zh) 一种基于行人步长信息辅助的视觉惯性slam定位方法
CN112862818B (zh) 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法
CN117234203A (zh) 一种多源里程融合slam井下导航方法
JP7179687B2 (ja) 障害物検知装置
WO2020223868A1 (zh) 地面信息处理方法、装置和无人驾驶车辆
JP4250391B2 (ja) 指標検出装置、指標検出方法
CN115540889A (zh) 使用相机、gps和imu定位自动驾驶车辆
JP7302966B2 (ja) 移動体
CN113432595A (zh) 设备状态获取方法、装置、计算机设备及存储介质
Andert et al. A flight state estimator that combines stereo-vision, INS, and satellite pseudo-ranges

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
GR01 Patent grant
GR01 Patent grant