CN114413909A - 一种室内移动机器人定位方法及系统 - Google Patents

一种室内移动机器人定位方法及系统 Download PDF

Info

Publication number
CN114413909A
CN114413909A CN202210085378.0A CN202210085378A CN114413909A CN 114413909 A CN114413909 A CN 114413909A CN 202210085378 A CN202210085378 A CN 202210085378A CN 114413909 A CN114413909 A CN 114413909A
Authority
CN
China
Prior art keywords
positioning
position information
mobile robot
ultra
indoor
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.)
Withdrawn
Application number
CN202210085378.0A
Other languages
English (en)
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.)
Taizhou University
Original Assignee
Taizhou University
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 Taizhou University filed Critical Taizhou University
Priority to CN202210085378.0A priority Critical patent/CN114413909A/zh
Publication of CN114413909A publication Critical patent/CN114413909A/zh
Priority to US17/820,923 priority patent/US20230236280A1/en
Withdrawn legal-status Critical Current

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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • G01S5/0263—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems
    • G01S5/0264—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems at least one of the systems being a non-radio wave positioning system
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/418—Total factory control, i.e. centrally controlling a plurality of machines, e.g. direct or distributed numerical control [DNC], flexible manufacturing systems [FMS], integrated manufacturing systems [IMS] or computer integrated manufacturing [CIM]
    • G05B19/4189—Total factory control, i.e. centrally controlling a plurality of machines, e.g. direct or distributed numerical control [DNC], flexible manufacturing systems [FMS], integrated manufacturing systems [IMS] or computer integrated manufacturing [CIM] characterised by the transport system
    • G05B19/41895—Total factory control, i.e. centrally controlling a plurality of machines, e.g. direct or distributed numerical control [DNC], flexible manufacturing systems [FMS], integrated manufacturing systems [IMS] or computer integrated manufacturing [CIM] characterised by the transport system using automatic guided vehicles [AGV]
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G05D1/243—Means capturing signals occurring naturally from the environment, e.g. ambient optical, acoustic, gravitational or magnetic signals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G05D1/247—Arrangements for determining position or orientation using signals provided by artificial sources external to the vehicle, e.g. navigation beacons
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/579—Depth or shape recovery from multiple images from motion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01K—ANIMAL HUSBANDRY; AVICULTURE; APICULTURE; PISCICULTURE; FISHING; REARING OR BREEDING ANIMALS, NOT OTHERWISE PROVIDED FOR; NEW BREEDS OF ANIMALS
    • A01K1/00—Housing animals; Equipment therefor
    • A01K1/01—Removal of dung or urine, e.g. from stables
    • 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
    • G01S2205/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S2205/01—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations specially adapted for specific applications
    • G01S2205/02—Indoor
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/50—Machine tool, machine tool null till machine tool work handling
    • G05B2219/50393—Floor conveyor, AGV automatic guided vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2105/00—Specific applications of the controlled vehicles
    • G05D2105/50—Specific applications of the controlled vehicles for animal husbandry or control, e.g. catching, trapping or scaring of animals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2107/00—Specific environments of the controlled vehicles
    • G05D2107/20—Land use
    • G05D2107/21—Farming, e.g. fields, pastures or barns
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2109/00—Types of controlled vehicles
    • G05D2109/10—Land vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/10—Optical signals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/30—Radio signals
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30204—Marker
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Manufacturing & Machinery (AREA)
  • Quality & Reliability (AREA)
  • General Engineering & Computer Science (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种室内移动机器人定位方法及系统，它包括。获得室内的移动道路的布局以及移动道路在室内的相对位置信息，移动机器人获取移动道路布局以及相对位置信息；移动机器人上设置视觉定位模块，视觉定位模块采集室内图像数据进行视觉定位获得第一位置信息；移动机器人上设置定位标签，并通过超宽带定位模块获取并解算出定位标签的第二位置信息；建立自适应衰落扩展卡尔曼滤波系统，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据,融合后，超宽带可以校正视觉定位产生的累积误差，同时视觉定位还可以平滑超宽带测量数据，弥补不足。

Description

一种室内移动机器人定位方法及系统
技术领域
本发明属于室内移动机器人定位技术领域，具体涉及一种室内移动机器人定位方法。
背景技术
楼房养猪是一种新兴的养猪模式，楼房养猪有着节约土地、有利环保、便于管理、通风采光好、机械化程度高、高效率低成本等优点。因此，楼房养猪已经逐渐成为了未来养猪行业乃至整个畜牧业发展趋势。规模化养猪为了提高效率，一般采用机械化清粪，因此，市面上诞生了许多清粪机器人。定位是清粪机器人自主作业中的主要问题之一。
目前在移动机器人领域常用的定位方法有：
(1)基于信标的定位方法。移动机器人在运动过程中识别提前布置好的信标，通过三角测量等方法计算位姿信息。此方法易于实现，精度也较高，但需合理布局信标位置，此外安装和维护成本较高。
(2)基于激光的定位方法。利用激光雷达采集机器人自身相对于环境实时信息，并对获得的点云集合进行处理，从而获得机器人位姿信息。激光定位技术精度高，不仅可以用于机器人定位，也可用于避障和导航等方面。但大多数激光传感器成本较高，且易发生畸变。
(3)基于声音的定位方法。这种技术通过发声体的音色等对其进行辨识，从而进行定位跟踪等任务。基于声音定位技术成本低廉，但是受噪声影响大，且计算量较大。
(4)基于视觉的定位方法。视觉定位深度融合了采集摄像头及其他传感器数据，信息维度返回更多的六自由度位姿信息(包含朝向信息和三维位置信息)，对部分室内或室外场景的精准定位进行覆盖并支持精准的虚拟内容叠加显示，具有精准空间定位、高精地图三维重建、虚实融合叠加的功能。但是长期的工作会产生累积误差。
楼层养猪猪舍结构复杂庞大，每一层都设有数十个粪道，每个粪道结构大致相同但不完全相同，粪道内环境较差，清粪机器人在定位过程中将会受到光线、噪声、环境不确定等条件的影响。且一台清粪机器人将一次自动定位导航到多个粪道进行作业。在这种庞大且复杂的工作环境下，当前的定位方法将无法很好得满足清粪机器人的定位需求。
发明内容
本发明旨在至少在一定程度上解决相关技术中的技术问题之一：提供一种室内移动机器人定位方法，它包括，其特征在于：
获得室内的移动道路的布局以及移动道路在室内的相对位置信息，移动机器人获取移动道路布局以及相对位置信息；
移动机器人上设置视觉定位模块，视觉定位模块采集室内图像数据进行视觉定位获得第一位置信息；
移动机器人上设置定位标签，并通过超宽带定位模块获取并解算出定位标签的第二位置信息；
建立自适应衰落扩展卡尔曼滤波系统，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。
根据本发明的一个示例，所述的移动机器人获取移动道路布局以及相对位置信息，具体包括：对室内的移动道路进行编号，并对移动道路的布局以及相对位置信息进行整合，在分别用二维码进行表征，移动机器人对二维码识别获取相应的移动道路布局以及相对位置信息。
根据本发明的一个示例，所述的移动机器人获取移动道路布局以及相对位置信息，具体包括：
对室内的移动道路进行编号，并对移动道路的布局以及相对位置信息进行整合，在分别用二维码进行表征，移动机器人对二维码识别获取相应的移动道路布局以及相对位置信息。
根据本发明的一个示例，采集室内图像数据进行视觉定位获得第一位置信息，具体包括：
视觉传感器以一定的帧速率实时采集粪道内图像数据，取连续的两个位置的图像；
从连续的两个位置的图像中提取出共同的关键点，获取关键点的深度坐标；
根据深度坐标连续迭代得到移动机器人在空间运动中的轨迹。
根据本发明的一个示例，移动机器人上设置定位标签，并通过超宽带定位模块获取并解算出定位标签的第二位置信息，具体包括：
移动机器人上设置定位标签，每个移动道路四周布置多个超宽带定位锚；
通过测量从定位标签到超宽带定位锚的信号时间，获得从定位标签到基站的距离，计算得到定位标签位置。
根据本发明的一个示例，将第一位置信息和第二位置信息进行时间同步后，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。
根据本发明的一个示例，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据，具体包括：将第一位置信息转换距离测量值；即分别获得超宽带定位模块和视觉定位模块的距离测量值之后，超宽带定位模块和视觉定位模块的距离测量值的差值被用作自适应卡尔曼滤波器的测量输入，经过自适应卡尔曼滤波器滤波后得到最终全局位置。
本发明旨在至少在一定程度上解决相关技术中的技术问题之一：一种室内移动机器人定位系统，它包括，其特征在于，
识别模块，获取预设的移动道路布局以及移动道路的相对位置信息；
视觉定位模块，采集室内图像数据进行视觉定位获得第一位置信息；
超宽带定位模块，移动机器人上设置定位标签且在移动道路设置多个超宽带定位锚，并通过超宽带定位模块获取并解算出定位标签的第二位置信息；
建立自适应衰落扩展卡尔曼滤波系统，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。
根据本发明的一个示例，还包括存储模块对室内的移动道路进行编号并对移动道路的布局以及相对位置信息进行整合之后存储到存储模块内，再移动道路分别用二维码进行表征，识别模块对二维码识别获取相应的移动道路布局以及相对位置信息。
根据本发明的一个示例，采集室内图像数据进行视觉定位获得第一位置信息，具体包括：
视觉传感器以一定的帧速率实时采集粪道内图像数据，取连续的两个位置的图像；
从连续的两个位置的图像中提取出共同的关键点，获取关键点的深度坐标；
根据深度坐标连续迭代得到移动机器人在空间运动中的轨迹。
根据本发明的一个示例，通过超宽带定位模块获取并解算出定位标签的第二位置信息，具体包括：
通过测量从定位标签到超宽带定位锚的信号时间，获得从定位标签到基站的距离，计算得到定位标签位置。
根据本发明的一个示例，将第一位置信息和第二位置信息进行时间同步后，将第一位置信息转换距离测量值；即分别获得超宽带定位模块和视觉定位模块的距离测量值之后，超宽带定位模块和视觉定位模块的距离测量值的差值被用作自适应卡尔曼滤波器的测量输入，经过自适应卡尔曼滤波器滤波后得到最终全局位置。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
视觉定位模块可以获得视觉传感器的6自由度运动(位置、姿态)，获得运动车辆的相对定位信息。然而，定位误差会随着时间的推移而累积，并且不能为室内移动车辆提供长期可靠的定位。超宽带定位模块具有低功耗、高带宽的特点，能够以低能耗传输大量数据。同时，超宽带定位模块具有很强的穿透力，具有更高的定位精度。由于多径效应、非视距(NLOS)等因素，单纯的超宽带定位模块也无法为室内移动车辆提供稳定、可靠、准确的定位信息。为了克服上述两种定位方案的不足，适应复杂的室内场景，本发明通过建立自适应衰落扩展卡尔曼滤波系统，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。融合后，超宽带可以校正视觉定位产生的累积误差，同时视觉定位还可以平滑超宽带测量数据，弥补不足。
附图说明
图1是本发明一种室内移动机器人定位方法的总体框架图。
图2是本发明一种室内移动机器人定位方法的视觉定位流程图。
图3是本发明一种室内移动机器人定位方法的室内超宽带定位模块定位方案布局图。
图4是本发明一种室内移动机器人定位方法的视觉定位模块数据和超宽带定位模块数据融合流程图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
下面参考附图来详细描述根据本发明实施例的一种室内移动机器人定位方法。
如图1所示，一种室内移动机器人定位方法，它包括，
获得室内的移动道路的布局以及移动道路在室内的相对位置信息，移动机器人获取移动道路布局以及相对位置信息；
移动机器人上设置视觉定位模块，视觉定位模块采集室内图像数据进行视觉定位获得第一位置信息；
移动机器人上设置定位标签，并通过超宽带定位模块获取并解算出定位标签的第二位置信息；
建立自适应衰落扩展卡尔曼滤波系统，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。
视觉定位模块可以获得视觉传感器的6自由度运动(位置、姿态)，获得运动车辆的相对定位信息。然而，定位误差会随着时间的推移而累积，并且不能为室内移动车辆提供长期可靠的定位。超宽带定位模块具有低功耗、高带宽的特点，能够以低能耗传输大量数据。同时，超宽带定位模块具有很强的穿透力，具有更高的定位精度。由于多径效应、非视距(NLOS)等因素，单纯的超宽带定位模块也无法为室内移动车辆提供稳定、可靠、准确的定位信息。为了克服上述两种定位方案的不足，适应复杂的室内场景，本发明通过建立自适应衰落扩展卡尔曼滤波系统，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。融合后，超宽带可以校正视觉定位产生的累积误差，同时视觉定位还可以平滑超宽带测量数据，弥补不足。
现代化猪舍厂房具有多层结构，每层都设有数十个粪道，每个粪道大致相同但又不完全一样，一台清粪机器人一般要进行多个粪道的轮流作业。为了使清粪机器人能够获取到当前作业粪道在整个猪舍内的相对位置，提前对猪舍内所有粪道进行编号，并对猪舍内所有粪道的布局以及相对位置信息进行整合，分别用二维码进行表征。最后将获得的二维码喷涂到粪道入口不被遮挡的地方方便清粪机器人扫描读取。即可以获得室内的移动道路的布局以及移动道路在室内的相对位置信息，移动机器人获取移动道路布局以及相对位置信息，移动机器人即清粪机器人在进入作业粪道时，搭载的视觉传感器将对二维码进行识别和扫描，读取到当前作业粪道的编号以及在整个猪舍内的相对位置信息。
移动机器人上设置视觉定位模块，视觉定位模块采集室内图像数据进行视觉定位获得第一位置信息，具体包括：
视觉传感器以一定的帧速率实时采集粪道内图像数据，取连续的两个位置的图像；
从连续的两个位置的图像中提取出共同的关键点，获取关键点的深度坐标；
根据深度坐标连续迭代得到移动机器人在空间运动中的轨迹。
具体过程如下：
搭载的视觉传感器以一定的帧速率实时采集粪道内图像数据，取连续的两个位置的图像分别为Image1和Image2。首先，利用SIFT算法从图像(Image1和Image2)中提取出共同的关键点，分别得到RGB图像中的图像点坐标(SIFTData1和SIFTData2)。然后，从深度图像获取关键点的深度坐标(Depth1和Depth2)以及关键点到相机的距离d。
比例空间的定义如下：
L(x，y，σ)＝G(x，y，σ)×I(x，y) (1)
其中L由每个关键点的坐标(x，y)和σ组成，σ为刻度值。这里，“×”表示卷积运算，I(x，y)为原始图像，G(x，y，σ)表示高斯模糊，定义如下:
Figure BDA0003487584710000061
其中m和n代表高斯模糊模板的维数，(x，y)代表图像中像素的位置，σ代表比例值。比例值越大，图像的轮廓特征越多。比例值越小，图像的特征越详细。
利用方程(3)构造高斯差分尺度空间，其中k是常数。
D(x，y，σ)·I(x，y)＝[G(x，y，kσ)-G(x，y，σ)]·I(x，y)＝L(x，y，kσ)-L(x，y，σ)(3)
在尺度空间中，对中间层检测点和相同尺度的八个相邻点之间的26个点以及对应于上层和下层相邻尺度的9×2个点进行比较。如果该点是最大值或最小值，则该点是图像上作为候选特征点的极值。在提取所有候选特征点之后，执行进一步的筛选工作，包括噪声去除和边缘效应。将一组待选点拟合到三维二次函数以去除低对比度点，然后根据待选候选的主曲率的大小确定边缘效果。
每个关键点的方向值由邻域关键像素的梯度方向分布特征指定，使算子具有旋转不变性。
Figure BDA0003487584710000062
θ(x，y)＝tan-1((L(x，y+1)-L(x，y-1))/(L(x+1，y)-L(x-1，y))) (5)
其中L表示不带比例值σ的关键点坐标(x，y)。以上是(x，y)处梯度的模量方程和方向方程。
利用随机采样一致性(RANSAC)算法剔除匹配对中的不匹配点，得到位置信息(Data1，Data2)。采用冒泡排序法，选取间距较大的四个关键点，然后取这四个点周围附近点的三维坐标平均值作为正确结果以提高视觉传感器数据的准确性。
使用绝对定向算法来计算旋转矩阵，从该旋转矩阵获得方向(三个方向)，并且两个位置之间的偏移值是计算得出两点之间的距离。机器人最初位于坐标原点。当机器人通过第一和第二位置进一步移动到第三位置时，获得新的特征点作为新的Data2，并且在第二位置获得的特征点被替换为新的Data1。数据更新后，通过新的Data1和Data2计算机器人从第二点到第三点的相对运动参数，从而通过连续迭代得到移动机器人在空间运动中的轨迹。
移动机器人上设置定位标签，并通过超宽带定位模块获取并解算出定位标签的第二位置信息；具体包括：
移动机器人上设置定位标签，每个移动道路四周布置多个超宽带定位锚；
通过测量从定位标签到超宽带定位锚的信号时间，获得从定位标签到基站的距离，计算得到定位标签位置。
具体过程如下：
预先在每个粪道四周布置12个超宽带定位锚(根据场地大小确定数量和布置位置)，清粪机器人上安装一个UWB定位标签，通过定位标签到各个超宽带定位锚的距离，从而利用超宽带算法计算出定位标签在三维空间的坐标。
参考超宽带和其他传感器的融合方案，可以使用紧耦合或松耦合方法。在松耦合方法中，超宽带原始距离测量首先通过三角测量或最小二乘法获得位置估计，然后，超宽带位置估计被用作与其他传感器集成的数据。与松散耦合相反，紧密耦合方法直接使用每个锚点的原始TOA测量值。由于松耦合方法需要对超宽带原始测量数据进行预处理，因此在某些情况下，超宽带信息会丢失。通过紧密耦合的方法，我们可以为超宽带充分利用现有信息。因此，对于超宽带和视觉传感器，本文采用紧耦合的方法。
本发明采用的是TOA定位算法，通过测量从定位标签到超宽带定位锚的信号时间，获得从标签到基站的距离。分别以超宽带定位锚为中心，距离为半径画三个或三个以上的圆。圆圈的交点就是定位标签的位置。等式如下:
Figure BDA0003487584710000071
在等式(1)中，t0是从标签发送信号的时间。这是锚点接收信号的时间。τ是信号从标签到锚点的传播时间。di是从标签到锚点的距离。(xi、yi、zi)和(x、y、z)分别是锚点和标签的坐标。在三维坐标解中，方程(6)可以转换成方程(7)的形式:
Figure BDA0003487584710000072
X＝(x,y,z)T为标签坐标，由于超宽带定位锚的数量至少为三个或更多，因此在计算标签坐标时存在冗余观测，可以进行最小二乘平差计算。等式如下:
AX＝L (8)
A、L可以根据锚点坐标和标签到锚点的距离计算，如公式(7)和(8)。v是观测的残留误差，如公式(11)
A＝[x(i+1)-x1，y(i+1)-y1，z(i+1)-z](i＝1，2，3，...，n) (9)
Figure BDA0003487584710000073
V＝AX-L (11)
X＝(ATPA)-1ATPL (12)
标签坐标可以通过等式(12)计算。
将第一位置信息和第二位置信息进行时间同步后，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。
具体过程如下：
视觉传感模块的输出数据必须与超宽带定位模块的数据及时同步。基于软件机制和系统之间的计算要求，将视觉传感模块频率设置为1Hz，超宽带定位模块的采样频率可以设置为100Hz。视觉传感模块采集的RGB和深度图像通过计算机上的程序算法与计算机中的世界时间保存在一起，超宽带定位模块的数据时间标签也是一样的。两者都与计算机中的世界时间有关。将视觉传感模块和超宽带定位模块数据都打上时间戳后，进行插值和对齐，实现时间同步，为使用卡尔曼滤波器进行数据融合提供条件。
建立自适应衰落扩展卡尔曼滤波系统，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。进一步的，将第一位置信息转换距离测量值；即分别获得超宽带定位模块和视觉定位模块的距离测量值之后，超宽带定位模块和视觉定位模块的距离测量值的差值被用作自适应卡尔曼滤波器的测量输入，经过自适应卡尔曼滤波器滤波后得到最终全局位置。
具体过程如下：
如图4所述，超宽带定位模块的数据信息是原始距离测量di UWB(i＝0,1,2,3…n)，测量信息是从定位标签到所设n个锚点的距离。然而，基于视觉定位方法可以获得带有深度信息的相对位置，并不能像超宽带系统那样输出与锚点的距离，需要进一步处理。将视觉定位系统得到的相对位置转换成类似于超宽带系统的距离测量，这通常需要两个步骤。第一步:由于视觉定位系统获取的是载体的相对位置信息，因此需要转换成全局位置坐标。第二步:根据已知的锚的x、y、z方向坐标，计算视觉全局位置坐标到所设锚点的欧氏距离。最终得到的欧氏距离di VO(i＝0,1,2,3…n)就是视觉定位系统的距离测量值。
根据所示的融合系统结构，在分别获得超宽带距离和视觉定位系统的距离测量值之后，
Figure BDA0003487584710000081
和
Figure BDA0003487584710000082
的差值Δdi被用作自适应卡尔曼滤波器的测量输入，经过自适应卡尔曼滤波器滤波后得到最佳状态估计。然后将滤波后的最佳状态估计反馈到视觉传感器的距离测量。再用最小二乘法求解校正后的视觉定位系统距离
Figure BDA0003487584710000083
从而得到融合系统的最终全局位置。
卡尔曼滤波器通常使用线性系统状态方程，结合其他输入信息和观测数据，来估计系统的最佳状态。卡尔曼滤波器需要前一时刻系统状态的估计值和当前时刻的观测信息来估计当前状态的最优值。由于卡尔曼滤波器易于编程，能够实时处理和更新现场采集的数据，因此在工程领域得到了广泛的应用。
本发明的系统模型是线性的，所以我们使用线性卡尔曼滤波器。由于系统模型和噪声特性对卡尔曼滤波器的性能有影响，在实际应用中，噪声的统计特性很难获得。在此基础上，本发明采用自适应卡尔曼滤波器，动态估计系统噪声协方差矩阵Q和观测噪声协方差矩阵R。
在本文使用的卡尔曼滤波器中，系统状态方程如下:
xk＝Axk-1+ωk (13)
其中，xk表示融合系统在时刻k的系统状态向量，A表示从时刻k-1到时刻k的状态转移矩阵，ωk表示系统噪声，噪声满足高斯白噪声ωk～N(0，Q)。xk的具体定义如下，表示定位标签对各个锚点距离的误差，状态转移矩阵A为n阶单位矩阵。
xk＝[Δd0 Δd1 Δd2 Δd3 ... Δdn]T (14)
融合系统的测量公式为:
zk＝Hxk+vk (15)
其中，zk是融合系统在时间k的观测向量，H是观测矩阵，n表示超宽带定位锚点的个数。vk代表观测噪声，噪声满足高斯白噪声vk～N(0，R)。zk的具体定义如下，表示视觉定位系统得出的距离di VO和超宽带定位模块的距离
Figure BDA0003487584710000091
之差。观测矩阵H是一个n阶单位矩阵。
Figure BDA0003487584710000092
根据上面定义的可变参数，完整的自适应卡尔曼滤波预测过程如下。其中，
Figure BDA0003487584710000093
表示k-1时刻的最佳状态估计，
Figure BDA0003487584710000094
表示从系统状态方程获得的在时间k的状态的预测值。Pk-1表示状态更新值和k-1时刻真实值之间的误差协方差矩阵。Pk，k-1表示预测值和k时刻状态真实值之间误差的协方差矩阵。
Figure BDA0003487584710000095
Figure BDA0003487584710000096
Figure BDA0003487584710000097
Figure BDA0003487584710000098
Pk，k-1＝APk-1AT+Qk (21)
完整的自适应卡尔曼滤波器更新过程如下，其中Kk代表卡尔曼增益矩阵，Pk代表时间k时更新值与真值之间误差的协方差矩阵，在迭代过程中，系统噪声协方差矩阵Qk和观测噪声协方差矩阵Rk动态更新。
Figure BDA0003487584710000101
Kk＝Pk，k-1HT[HPk，k-1HT+Rk]-1 (23)
Figure BDA0003487584710000102
Pk＝(I-KkH)Pk，k-1 (25)
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
对于本领域的技术人员而言，阅读上述说明后，各种变化和修正无疑将显而易见。因此，所附的权利要求书应看作是涵盖本发明的真实意图和范围的全部变化和修正。在权利要求书范围内任何和所有等价的范围与内容，都应认为仍属本发明的意图和范围内。

Claims (11)

1.一种室内移动机器人定位方法，它包括，其特征在于，
获得室内的移动道路的布局以及移动道路在室内的相对位置信息，移动机器人获取移动道路布局以及相对位置信息；
移动机器人上设置视觉定位模块，视觉定位模块采集室内图像数据进行视觉定位获得第一位置信息；
移动机器人上设置定位标签，并通过超宽带定位模块获取并解算出定位标签的第二位置信息；
建立自适应衰落扩展卡尔曼滤波系统，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。
2.根据权利要求1所述的室内移动机器人定位方法，其特征在于，所述的移动机器人获取移动道路布局以及相对位置信息，具体包括：
对室内的移动道路进行编号，并对移动道路的布局以及相对位置信息进行整合，在分别用二维码进行表征，移动机器人对二维码识别获取相应的移动道路布局以及相对位置信息。
3.根据权利要求1所述的室内移动机器人定位方法，其特征在于，采集室内图像数据进行视觉定位获得第一位置信息，具体包括：
视觉传感器以一定的帧速率实时采集粪道内图像数据，取连续的两个位置的图像；
从连续的两个位置的图像中提取出共同的关键点，获取关键点的深度坐标；
根据深度坐标连续迭代得到移动机器人在空间运动中的轨迹。
4.根据权利要求1所述的室内移动机器人定位方法，其特征在于，移动机器人上设置定位标签，并通过超宽带定位模块获取并解算出定位标签的第二位置信息，具体包括：
移动机器人上设置定位标签，每个移动道路四周布置多个超宽带定位锚；
通过测量从定位标签到超宽带定位锚的信号时间，获得从定位标签到基站的距离，计算得到定位标签位置。
5.根据权利要求1所述的室内移动机器人定位方法，其特征在于，将第一位置信息和第二位置信息进行时间同步后，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。
6.根据权利要求5所述的室内移动机器人定位方法，其特征在于，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据，具体包括：将第一位置信息转换距离测量值；即分别获得超宽带定位模块和视觉定位模块的距离测量值之后，超宽带定位模块和视觉定位模块的距离测量值的差值被用作自适应卡尔曼滤波器的测量输入，经过自适应卡尔曼滤波器滤波后得到最终全局位置。
7.一种室内移动机器人定位系统，它包括，其特征在于，
识别系统，获取预设的移动道路布局以及移动道路的相对位置信息；
视觉定位模块，采集室内图像数据进行视觉定位获得第一位置信息；
超宽带定位模块，移动机器人上设置定位标签且在移动道路设置多个超宽带定位锚，并通过超宽带定位模块获取并解算出定位标签的第二位置信息；
建立自适应衰落扩展卡尔曼滤波系统，自适应衰落扩展卡尔曼滤波系统对第一位置信息和第二位置信息进行融合得出最终定位数据。
8.根据权利要求7所述的室内移动机器人定位系统，其特征在于，还包括存储系统对室内的移动道路进行编号并对移动道路的布局以及相对位置信息进行整合之后存储到存储系统内，再移动道路分别用二维码进行表征，识别系统对二维码识别获取相应的移动道路布局以及相对位置信息。
9.根据权利要求7所述的室内移动机器人定位系统，其特征在于，采集室内图像数据进行视觉定位获得第一位置信息，具体包括：
视觉传感器以一定的帧速率实时采集粪道内图像数据，取连续的两个位置的图像；
从连续的两个位置的图像中提取出共同的关键点，获取关键点的深度坐标；
根据深度坐标连续迭代得到移动机器人在空间运动中的轨迹。
10.根据权利要求7所述的室内移动机器人定位系统，其特征在于，通过超宽带定位模块获取并解算出定位标签的第二位置信息，具体包括：
通过测量从定位标签到超宽带定位锚的信号时间，获得从定位标签到基站的距离，计算得到定位标签位置。
11.根据权利要求1所述的室内移动机器人定位方法，其特征在于，将第一位置信息和第二位置信息进行时间同步后，将第一位置信息转换距离测量值；即分别获得超宽带定位模块和视觉定位模块的距离测量值之后，超宽带定位模块和视觉定位模块的距离测量值的差值被用作自适应卡尔曼滤波器的测量输入，经过自适应卡尔曼滤波器滤波后得到最终全局位置。
CN202210085378.0A 2022-01-25 2022-01-25 一种室内移动机器人定位方法及系统 Withdrawn CN114413909A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202210085378.0A CN114413909A (zh) 2022-01-25 2022-01-25 一种室内移动机器人定位方法及系统
US17/820,923 US20230236280A1 (en) 2022-01-25 2022-08-19 Method and system for positioning indoor autonomous mobile robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210085378.0A CN114413909A (zh) 2022-01-25 2022-01-25 一种室内移动机器人定位方法及系统

Publications (1)

Publication Number Publication Date
CN114413909A true CN114413909A (zh) 2022-04-29

Family

ID=81276487

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210085378.0A Withdrawn CN114413909A (zh) 2022-01-25 2022-01-25 一种室内移动机器人定位方法及系统

Country Status (2)

Country Link
US (1) US20230236280A1 (zh)
CN (1) CN114413909A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117177174A (zh) * 2023-11-03 2023-12-05 江苏达海智能系统股份有限公司 一种基于机器视觉与wsn的室内定位方法及系统

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11625510B2 (en) * 2017-02-22 2023-04-11 Middle Chart, LLC Method and apparatus for presentation of digital content
US12086507B2 (en) 2017-02-22 2024-09-10 Middle Chart, LLC Method and apparatus for construction and operation of connected infrastructure
US10740502B2 (en) 2017-02-22 2020-08-11 Middle Chart, LLC Method and apparatus for position based query with augmented reality headgear
US11507714B2 (en) 2020-01-28 2022-11-22 Middle Chart, LLC Methods and apparatus for secure persistent location based digital content
US11640486B2 (en) 2021-03-01 2023-05-02 Middle Chart, LLC Architectural drawing based exchange of geospatial related digital content
US12086943B2 (en) 2022-08-15 2024-09-10 Middle Chart, LLC Spatial navigation to digital content
CN117824667B (zh) * 2024-03-06 2024-05-10 成都睿芯行科技有限公司 一种基于二维码和激光的融合定位方法及介质
CN117848331A (zh) * 2024-03-06 2024-04-09 河北美泰电子科技有限公司 基于视觉标签地图的定位方法及装置

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117177174A (zh) * 2023-11-03 2023-12-05 江苏达海智能系统股份有限公司 一种基于机器视觉与wsn的室内定位方法及系统
CN117177174B (zh) * 2023-11-03 2024-01-26 江苏达海智能系统股份有限公司 一种基于机器视觉与wsn的室内定位方法及系统

Also Published As

Publication number Publication date
US20230236280A1 (en) 2023-07-27

Similar Documents

Publication Publication Date Title
CN114413909A (zh) 一种室内移动机器人定位方法及系统
AU2012376428B2 (en) Map data creation device, autonomous movement system and autonomous movement control device
KR20190082071A (ko) 전자 지도를 업데이트하기 위한 방법, 장치 및 컴퓨터 판독 가능한 저장 매체
KR101314588B1 (ko) 인공 표식물의 지도 작성 방법 및 장치와 이를 이용한 이동체 위치 측정 방법 및 장치
CN110703268B (zh) 一种自主定位导航的航线规划方法和装置
CN110361027A (zh) 基于单线激光雷达与双目相机数据融合的机器人路径规划方法
CN110345937A (zh) 一种基于二维码的导航定姿定位方法和系统
WO2019126950A1 (zh) 一种定位方法、云端服务器、终端、系统、电子设备及计算机程序产品
CN110542908A (zh) 应用于智能驾驶车辆上的激光雷达动态物体感知方法
CN113985429B (zh) 基于三维激光雷达的无人机环境扫描与重构方法
CN110260866A (zh) 一种基于视觉传感器的机器人定位与避障方法
CN109282813B (zh) 一种无人艇全局障碍物识别的方法
Kato et al. NLOS satellite detection using a fish-eye camera for improving GNSS positioning accuracy in urban area
JP2023525927A (ja) 車両位置推定システム及び方法
CN115728803A (zh) 一种城市驾驶车辆连续定位系统及方法
CN114114367A (zh) Agv室外定位切换方法、计算机装置及程序产品
Zhou et al. Comparative analysis of SLAM algorithms for mechanical LiDAR and solid-state LiDAR
CN118168545A (zh) 基于多源传感器融合的除草机器人定位导航系统及方法
Tao et al. Automated processing of mobile mapping image sequences
CN116358547B (zh) 一种基于光流估计获取agv位置的方法
Dehbi et al. Improving gps trajectories using 3d city models and kinematic point clouds
CN113850864B (zh) 一种面向室外移动机器人的gnss/lidar回环检测方法
Baldoni et al. On the Design of High Accuracy Rail Digital Maps based on Sensor Fusion
CN111239761B (zh) 一种用于室内实时建立二维地图的方法
Ai et al. Surround Mask Aiding GNSS/LiDAR SLAM for 3D Mapping in the Dense Urban Environment

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
WW01 Invention patent application withdrawn after publication

Application publication date: 20220429

WW01 Invention patent application withdrawn after publication