CN111568304B - 扫地机器人定位方法、装置及扫地机器人 - Google Patents
扫地机器人定位方法、装置及扫地机器人 Download PDFInfo
- Publication number
- CN111568304B CN111568304B CN201910121095.5A CN201910121095A CN111568304B CN 111568304 B CN111568304 B CN 111568304B CN 201910121095 A CN201910121095 A CN 201910121095A CN 111568304 B CN111568304 B CN 111568304B
- Authority
- CN
- China
- Prior art keywords
- moment
- state quantity
- historical
- sweeping robot
- data
- 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.)
- Active
Links
Classifications
-
- A—HUMAN NECESSITIES
- A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
- A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
- A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
- A47L11/24—Floor-sweeping machines, motor-driven
-
- A—HUMAN NECESSITIES
- A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
- A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
- A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
- A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
-
- A—HUMAN NECESSITIES
- A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
- A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
- A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
- A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
- A47L11/4002—Installations of electric equipment
-
- A—HUMAN NECESSITIES
- A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
- A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
- A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
- A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
- A47L11/4011—Regulation of the cleaning machine by electric means; Control systems and remote control systems therefor
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- A—HUMAN NECESSITIES
- A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
- A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
- A47L2201/00—Robotic cleaning machines, i.e. with automatic control of the travelling movement or the cleaning operation
- A47L2201/04—Automatic control of the travelling movement; Automatic obstacle detection
-
- A—HUMAN NECESSITIES
- A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
- A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
- A47L2201/00—Robotic cleaning machines, i.e. with automatic control of the travelling movement or the cleaning operation
- A47L2201/06—Control of the cleaning action for autonomous devices; Automatic detection of the surface condition before, during or after cleaning
-
- 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
- Y02E—REDUCTION OF GREENHOUSE GAS [GHG] EMISSIONS, RELATED TO ENERGY GENERATION, TRANSMISSION OR DISTRIBUTION
- Y02E10/00—Energy generation through renewable energy sources
- Y02E10/50—Photovoltaic [PV] energy
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
本发明涉及互联网产品技术领域,具体而言,涉及一种扫地机器人定位方法、装置及扫地机器人。所述扫地机器人定位方法包括:选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量;根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,所述第二时刻位于所述第一时刻之后;获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量;对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量。所述扫地机器人定位方法、装置及扫地机器人具有较高的定位精度。
Description
技术领域
本发明涉及智能家居技术领域,具体而言,涉及一种扫地机器人定位方法、装置及扫地机器人。
背景技术
随着科技的发展,以及人们对生活质量要求的不断增高,智能家居逐渐出现在人们的日常生活中,其中,尤其具有代表性的扫地机器人越来越受人们的喜爱。扫地机器人在开机进行清扫工作之前,都需要进行定位,以确定自身的位置状态。但现有技术中的扫地机器人定位方法普遍存在定位精度不高的弊端,因此,如何提高扫地机器人的定位精度,成为目前智能家居技术领域亟待解决的技术问题。
发明内容
有鉴于此,本发明实施例的目的在于提供一种扫地机器人定位方法、装置及扫地机器人,以有效改善上述问题。
本发明实施例提供的扫地机器人定位方法,包括:
选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量;
根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,所述第二时刻位于所述第一时刻之后;
获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量;
对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量。
进一步地,所述根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,包括:
根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量。
进一步地,所述历史状态量包括历史位置、历史线速度和历史加速度,所述第一参考状态量包括第一参考位置,所述运动方程包括第一运动方程;
所述根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程,包括:
根据所述历史位置、所述历史线速度和所述历史加速度与所述第一参考位置的约束关系创建第一运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量,包括:
根据所述第一运动方程、所述历史位置、所述历史线速度和所述历史加速度,预测扫地机器人的所述第一参考位置。
进一步地,所述历史状态量包括历史姿态和历史角速度,所述第一参考状态量包括第一参考姿态,所述运动方程包括第二运动方程;
所述根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程,包括:
根据所述第一参考姿态与所述历史姿态和所述历史角速度之间的约束关系创建第二运动方程;
所述根据所述运动方程、所述历史状态量,预测扫地机器人的所述第一参考状态量,包括:
根据所述第二运动方程、所述历史姿态和所述历史角速度,预测扫地机器人的所述第一参考姿态。
进一步地,所述传感器包括左/右车轮码盘、激光雷达、惯性测量装置,所述实测数据包括所述左/右车轮码盘测量得到的状态增量数据、所述激光雷达测量得到的点云数据,以及所述惯性测量装置测量得到的实测角速度和实测加速度,所述第二参考状态量包括第二参考姿态和第二参考位置;
所述获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量,包括:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置;
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态。
进一步地,所述获得所述左/右车轮码盘在第二时刻测量得到的增量数据以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置,包括:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,并将获得所述增量数据的时刻标记为第一数据时刻;
获得所述激光雷达在第二时刻测量得到的点云数据,并将获得所述点云数据的时刻标记为第二数据时刻,所述第二数据时刻位于所述第一数据时刻之后;
根据所述增量数据和所述点云数据,获得所述第二参考位置,并将获得所述第二参考位置的时刻标记为第三数据时刻,所述第三数据时刻位于所述第二数据时刻之后。
进一步地,所述获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态,包括:
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并将获得所述实测角速度和所述实测加速度的时刻标记为第四数据时刻,所述第四数据时刻位于所述第二数据时刻和所述第三数据时刻之间;
对所述第四数据时刻和所述第三数据时刻之间获得的N个实测角速度和N个实测加速度进行存储,并为各实测角速度添加第一时刻标签,以及为各实测加速度添加第二时刻标签,其中,N为整数;
从多个所述第一时刻标签中选取出最靠后的第一时刻标签,作为第一待选取标签,并将与所述第一待选取标签对应的实测角速度作为备用实测角速度,以及从多个所述第二时刻标签中选取出最靠后的第二时刻标签,作为第二待选取标签,并将与所述第二待选取标签对应的实测加速度作为备用实测加速度;
根据所述备用实测角速度和所述备用实测加速度,获得第二参考姿态。
进一步地,所述结果状态量包括融合姿态和融合位置;
所述对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量,包括:
对所述第一参考姿态和所述第二参考姿态进行融合,获得扫地机器人的融合姿态;
对所述第一参考位置和所述第二参考位置进行融合,获得扫地机器人的融合位置。
本发明实施例提供的扫地机器人定位装置,包括:
历史状态量获取模块,用于选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量;
第一参考状态量获取模块,用于根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,所述第二时刻位于所述第一时刻之后;
第二参考状态量获取模块,用于获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量;
结果状态量获取模块,用于对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量。
进一步地,所述第一参考状态量获取模块,具体用于:
根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量。
进一步地,所述历史状态量包括历史位置、历史线速度和历史加速度,所述第一参考状态量包括第一参考位置,所述运动方程包括第一运动方程;
所述第一参考状态量获取模块,又具体用于:
根据所述历史位置、所述历史线速度和所述历史加速度与所述第一参考位置的约束关系创建第一运动方程;
根据所述第一运动方程、所述历史位置、所述历史线速度和所述历史加速度,预测扫地机器人的所述第一参考位置。
进一步地,所述历史状态量包括历史姿态和历史角速度,所述第一参考状态量包括第一参考姿态,所述运动方程包括第二运动方程;
所述第一参考状态量获取模块,还具体用于:
根据所述第一参考姿态与所述历史姿态和所述历史角速度之间的约束关系创建第二运动方程;
根据所述第二运动方程、所述历史姿态和所述历史角速度,预测扫地机器人的所述第一参考姿态。
进一步地,所述传感器包括左/右车轮码盘、激光雷达、惯性测量装置,实测数据包括所述左/右车轮码盘测量得到的状态增量数据、所述激光雷达测量得到的点云数据,以及所述惯性测量装置测量得到的实测角速度和实测加速度,所述第二参考状态量包括第二参考姿态和第二参考位置;
所述第二参考状态量获取模块,具体用于:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置;
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态。
进一步地,所述第二参考状态量获取模块,又具体用于:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,并将获得所述增量数据的时刻标记为第一数据时刻;
获得所述激光雷达在第二时刻测量得到的点云数据,并将获得所述点云数据的时刻标记为第二数据时刻,所述第二数据时刻位于所述第一数据时刻之后;
根据所述增量数据和所述点云数据,获得所述第二参考位置,并将获得所述第二参考位置的时刻标记为第三数据时刻,所述第三数据时刻位于所述第二数据时刻之后。
进一步地,所述第二参考状态量获取模块,又具体用于:
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并将获得所述实测角速度和所述实测加速度的时刻标记为第四数据时刻,所述第四数据时刻位于所述第二数据时刻和所述第三数据时刻之间;
对所述第四数据时刻和所述第三数据时刻之间获得的N个实测角速度和N个实测加速度进行存储,并为各实测角速度添加第一时刻标签,以及为各实测加速度添加第二时刻标签,其中,N为整数;
从多个所述第一时刻标签中选取出最靠后的第一时刻标签,作为第一待选取标签,并将与所述第一待选取标签对应的实测角速度作为备用实测角速度,以及从多个所述第二时刻标签中选取出最靠后的第二时刻标签,作为第二待选取标签,并将与所述第二待选取标签对应的实测加速度作为备用实测加速度;
根据所述备用实测角速度和所述备用实测加速度,获得第二参考姿态。
进一步地,所述结果状态量包括融合姿态和融合位置;
所述结果状态量获取模块,具体用于:
对所述第一参考姿态和所述第二参考姿态进行融合,获得扫地机器人的融合姿态;
对所述第一参考位置和所述第二参考位置进行融合,获得扫地机器人的融合位置。
本发明实施例提供的扫地机器人,包括处理器、存储器和上述扫地机器人定位装置,所述扫地机器人定位装置包括一个或多个存储于所述存储器并由所述处理器执行的软件功能模块。
本发明实施例提供的计算机可读存储介质,其上存储有计算机程序,所述计算机程序被执行时,可以实现上述扫地机器人定位方法。
本发明实施例提供的扫地机器人定位方法,通过选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量,根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,以及获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量,最后,对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量。显而易见,通过本发明实施例获得的结果状态量,不单根据扫地机器人的历史状态量获得,也不单根据设置于扫地机器人上的传感器在第二时刻的实测数据获得,而是根据扫地机器人的历史状态量,预测扫地机器人在第二时刻的第一参考状态量,并根据扫地机器人上的传感器在第二时刻的实测数据,获得扫地机器人在第二时刻的第二参考状态量,最后对第一参考状态量和第二参考状态量进行融合获得,因此,具有较高的定位精度。
上述说明仅是本发明技术方案的概述,为了能够更清楚了解本发明的技术手段,而可依照说明书的内容予以实施,并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂,以下特举本发明的具体实施方式。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本公开的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他相关的附图。
图1为本发明实施例提供的扫地机器人的示意性结构框图。
图2为本发明实施例提供的扫地机器人定位方法的流程示意性。
图3为本发明实施例提供的扫地机器人定位装置的示意性结构框图。
图标:100-扫地机器人;110-扫地机器人定位装置;111-历史状态量获取模块;112-第一参考状态量获取模块;113-第二参考状态量获取模块;114-结果状态量获取模块;120-处理器;130-存储器。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本公开一部分实施例,而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此,以下对在附图中提供的本公开的实施例的详细描述并非旨在限制要求保护的本公开的范围,而是仅仅表示本公开的选定实施例。基于本公开的实施例,本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例,都属于本公开保护的范围。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步定义和解释。
请参阅图1,为本发明实施例提供的一种应用扫地机器人定位方法及装置的扫地机器人100的示意性结构框图。进一步地,本发明实施例中,扫地机器人100包括扫地机器人定位装置110、处理器120和存储器130。
处理器120和存储器130之间直接或间接地电性连接,以实现数据的传输或交互,例如,这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。扫地机器人定位装置110包括至少一个可以软件或固件(Firmware)的形式存储在存储器130中或固化在扫地机器人100的操作系统(Operating SNstem,OS)中的软件模块。处理器120用于执行存储器130中存储的可执行模块,例如,扫地机器人定位装置110所包括的软件功能模块及计算机程序等。处理器120可以在接收到执行指令后,执行计算机程序。
其中,处理器120可以是一种集成电路芯片,具有信号处理能力。处理器120也可以是通用处理器,例如,可以是数字信号处理器(DSP)、专用集成电路(ASIC)、分立门或晶体管逻辑器件、分立硬件组件,可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。此外,通用处理器可以是微处理器或者任何常规处理器等。
此外,存储器130可以是,但不限于,随机存取存储器(Random Access MemorN,RAM),只读存储器(Read OnlN MemorN,ROM),可编程只读存储器(Programmable Read-OnlNMemorN,PROM),可擦可编程序只读存储器(Erasable Programmable Read-OnlN MemorN,EPROM),电可擦编程只读存储器Electric Erasable Programmable Read-OnlN MemorN,EEPROM)等。存储器130用于存储程序,处理器120在接收到执行指令后,执行该程序。
应当理解,图1所示的结构仅为示意,本发明实施例提供的扫地机器人100还可以具有比图1更少或更多的组件,或是具有与图1所示不同的配置。此外,图1所示的各组件可以通过软件、硬件或其组合实现。
请参阅图2,图2为本发明实施例提供的扫地机器人定位方法的流程示意图,该方法应用于图1所示的扫地机器人100。所应说明的是,本发明实施例提供的扫地机器人定位方法不以图2及以下所示的顺序为限制,以下结合图2对扫地机器人定位方法的具体流程及步骤进行详细阐述。
步骤S100,选取第一时刻,并获得扫地机器人在第一时刻的历史状态量。
本发明实施例中,可以在扫地机器人的行径过程中,建立包括多个历史时刻的行径时间轴。针对每个历史时刻,在获得与该历史时刻对应的历史状态量后,对该历史状态量进行存储,并建立该历史状态量与该历史时刻的对应关系。基于此,当第二时刻到来时,在行径时间轴上选取出位于第二时刻之前,且与第二时刻的间隔时长为预设时长的历史时刻,作为第一时刻,并从存储的各历史状态量中选取出扫地机器人在第一时刻的历史状态量。
本发明实施例中,历史状态量可以包括历史位置、历史线速度、历史加速度、历史姿态和历史角速度等。需要说明的是,现有技术中,扫地机器人定位方法除存在定位精度不高的弊端外,还存在只能够应用于扫地机器人在简单环境(环境地面为平面)的定位,而无法应用于扫地机器人在复杂环境(环境地面存在坡面等)的定位的问题。为解决该问题,本发明实施例中,历史位置、历史线速度、历史加速度、历史姿态和历史角速度均为三维状态数据,也即,本发明实施例中,历史位置、历史线速度、历史加速度、历史姿态和历史角速度均为基于预设三维空间坐标系(X,Y,Z)所表征的状态量,具体地,历史位置可以表征为(xt-1,yt-1,zt-1),历史线速度可以表征为(vxt-1,vyt-1,vzt-1),历史加速度可以表征为(axt-1,ayt-1,azt-1),历史姿态可以表征为(rollt-1,pitcht-1,yawt-1),历史角速度可以表征为(vrollt-1,vpitcht-1,vyawt-1)。
步骤S200,根据历史状态量,预测扫地机器人在第二时刻的第一参考状态量。
扫地机器人的行径过程符合物理运动模型,也即,根据扫地机器人前一时刻的状态量,可以预测后一时刻的状态量。以简单一维运动为例,扫地机器人在后一时刻的位置,可以根据前一时刻的位置、速度和加速度预测,也即:
xt=xt-1+vt-1*t+0.5*at-1*t2;
其中,xt为扫地机器人在后一时刻的位置,xt-1为扫地机器人在前一时刻的位置,vt-1为扫地机器人在前一时刻的速度,at-1为扫地机器人在前一时刻的加速度,t为后一时刻与前一时刻的间隔时长。基于此,本发明实施例中,根据历史状态量,预测扫地机器人在第二时刻的第一参考状态量,可以包括以下步骤。
根据扫地机器人在第二时刻的第一参考状态量与历史状态量之间的约束关系,创建运动方程,基于以上描述,可以理解的是,该约束关系即为扫地机器人的行径过程所符合的物理运动模型。运动方程建立之后,根据运动方程和历史状态量,预测扫地机器人的第一参考状态量。本发明实施例中,第一参考状态量可以包括第一参考位置和第一参考姿态。
针对第一参考位置,实际实施时,可以根据历史位置、历史线速度和历史加速度与第一参考位置的约束关系创建第一运动方程。第一运动方程建立之后,根据第一运动方程、历史位置、历史线速度和历史加速度,预测扫地机器人的第一参考位置。需要说明的是,本发明实施例中,由于历史位置、历史线速度和历史加速度均为基于预设三维空间坐标系(X,Y,Z)所表征的状态量,因此,实际实施时,可以分别对预设三维空间坐标系(X,Y,Z)中每个轴上的第一参考位置进行预测,最终获得扫地机器人在预设三维空间坐标系(X,Y,Z)中的第一参考位置。例如,针对扫地机器人在X轴上的第一参考位置,可以建立如下所示的第一运动子方程,第一运动子方程建立之后,根据第一运动子方程、X轴对应的历史位置、X轴对应的历史线速度,以及X轴对应的历史加速度,预测扫地机器人在X轴上的第一参考位置。
xt=xt-1+vxt-1*t+0.5*axt-1*t2
其中,xt为扫地机器人在X轴上的第一参考位置,xt-1为扫地机器人在X轴上的历史位置,vxt-1为扫地机器人在X轴上的历史线速度,axt-1为扫地机器人在X轴上的历史加速度,t为第二时刻与第一时刻的间隔时长。本发明实施例中,扫地机器人在Y轴上的第一参考位置的预测方法,以及扫地机器人在Z轴上的第一参考位置的预测方法,与上述扫地机器人在X轴上的第一参考位置的预测方法相似,此处不再赘述。
针对第一参考姿态,实际实施时,可以根据第一参考姿态与历史姿态和历史角速度之间的约束关系创建第二运动方程。此后,根据第二运动方程、历史姿态和历史角速度,预测扫地机器人的第一参考姿态。同样,需要说明的是,本发明实施例中,由于历史姿态和历史角速度均为基于预设三维空间坐标系(X,Y,Z)所表征的状态量,因此,实际实施时,可以分别对预设三维空间坐标系(X,Y,Z)中每个轴上的第一参考姿态进行预测,最终获得扫地机器人在预设三维空间坐标系(X,Y,Z)中的第一参考姿态。例如,针对扫地机器人在X轴上的第一参考姿态,可以建立第二运动子方程,第二运动子方程建立之后,根据第二运动子方程、X轴上的历史姿态,以及X轴上的历史角速度,预测扫地机器人在X轴上的第一参考姿态。本发明实施例中,扫地机器人在Y轴上的第一参考姿态的预测方法,以及扫地机器人在Z轴上的第一参考姿态的预测方法,与上述扫地机器人在X轴上的第一参考姿态的预测方法相似,此处不再赘述。
步骤S300,获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据实测数据,获得扫地机器人在第二时刻的第二参考状态量。本发明实施例中,第二参考状态量可以包括第二参考姿态和第二参考位置。
其中,设置于扫地机器人上的传感器可以包括左/右车轮码盘、激光雷达、惯性测量装置。左/右车轮码盘即为设置于扫地机器人左车轮上的第一码盘,以及设置于扫地机器人右车轮上的第二码盘,且第一码盘和第二码盘的结构、属性均相同,惯性测量装置包括陀螺仪和加速度计,本发明实施例中,惯性测量装置包括的陀螺仪和加速度计可以具有相同的测量频率。基于此,实测数据包括左/右车轮码盘测量得到的状态增量数据、激光雷达测量得到的点云数据,以及惯性测量装置测量得到的实测角速度和实测加速度。
针对第二参考位置,实际实施时,可以获得左/右车轮码盘在第二时刻测量得到的增量数据,以及激光雷达在第二时刻测量得到的点云数据,并根据增量数据和点云数据,获得第二参考位置。其中,增量数据包括左/右车轮码盘在第二时刻相对于左/右车轮码盘在第一时刻的位置增量数据,以及偏航角增量数据,此外,电云数据为三维点云数据。在获得增量数据和点云数据之后,可以基于位置增量数据,对点云经过ICP算法做帧间跟踪,再与预设环境地图进行匹配,获得扫地机器人在预设三维空间坐标系(X,Y,Z)中的第二参考位置。
针对第二参考姿态,实际实施时,获得惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据实测角速度和实测加速度,获得第二参考姿态。
本发明实施例中,左/右车轮码盘的测量频率为40Hz~100Hz,激光雷达的测量频率为5Hz,惯性测量装置的测量频率为100Hz~1000Hz。此外,针对左/右车轮码盘测量得到的状态增量数据、激光雷达测量得到的点云数据,以及惯性测量装置测量得到的实测角速度和实测加速度,其各自从采集时刻(第二时刻)到被发送至处理器,直至被处理器接收的时刻,所需的时长是不同的。具体地,在此过程中,状态增量数据所需的时长小于点云数据所需的时长,而点云数据所需的时长又小于实测角速度和实测加速度所经历的时长。
基于以上描述,本发明实施例中,在获得左/右车轮码盘在第二时刻测量得到的增量数据之后,可以将获得增量数据的时刻标记为第一数据时刻,可以理解的是,此处,第一时刻为处理器接收到增量数据的时刻。同样,获得激光雷达在第二时刻测量得到的点云数据之后,可以将获得点云数据的时刻标记为第二数据时刻,其中,第二数据时刻位于第一数据时刻之后,可以理解的是,此处,第二时刻为处理器接收到点云数据的时刻。最后,根据增量数据和点云数据,获得第二参考位置,并将获得第二参考位置的时刻标记为第三数据时刻,第三数据时刻位于第二数据时刻之后。然而,根据增量数据和点云数据,获得第二参考位置的过程所需的计算时长往往较长,根据实测角速度和实测加速度,获得第二参考姿态的过程所需的计算时长往往较短。于是,获得左/右车轮码盘在第二时刻测量得到的增量数据,以及激光雷达在第二时刻测量得到的点云数据,并根据增量数据和点云数据,获得第二参考位置所需的时长,通常长于获得惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据实测角速度和实测加速度,获得第二参考姿态50ms~200ms,而在这50ms~200ms之内,处理器可能已经获得多组新的参考姿态。鉴于此,本发明实施例中,为保证扫地机器人的第二参考姿态具有较高的准确性,针对获得惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据实测角速度和实测加速度,获得第二参考姿态的步骤,提出了以下所述的实施方式。
获得惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并将获得实测角速度和实测加速度的时刻标记为第四数据时刻,第四数据时刻位于第二数据时刻和第三数据时刻之间。此后,对第四数据时刻和第三数据时刻之间获得的N个实测角速度和N个实测加速度进行存储,并为各实测角速度添加第一时刻标签,以及为各实测加速度添加第二时刻标签,其中,N为整数。从多个第一时刻标签中选取出最靠后的第一时刻标签,作为第一待选取标签,并将与第一待选取标签对应的实测角速度作为备用实测角速度,以及从多个第二时刻标签中选取出最靠后的第二时刻标签,作为第二待选取标签,并将与第二待选取标签对应的实测加速度作为备用实测加速度。根据备用实测角速度和备用实测加速度,获得第二参考姿态。如此,第二参考姿态便替换成了上述50ms~200ms以内,处理器获得的最新参考姿态,从而保证了扫地机器人的第二参考姿态具有较高的准确性。
步骤S400,对第一参考状态量和第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量。本发明实施例中,结果状态量包括融合姿态和融合位置。
本发明实施例中,可以对第一参考姿态和第二参考姿态进行融合,获得扫地机器人的融合姿态,同时,对第一参考位置和第二参考位置进行融合,获得扫地机器人的融合位置。此外,实际实施时,对第一参考姿态和第二参考姿态进行融合,以及对第一参考位置和第二参考位置进行融合采用的融合算法可以是贝叶斯递归估计算法。
基于与上述扫地机器人定位方法同样的发明构思,本发明实施例还提供了一种扫地机器人定位装置110,请参阅图3,本发明实施例提供的扫地机器人定位装置110包括历史状态量获取模块111、第一参考状态量获取模块112、第二参考状态量获取模块113和结果状态量获取模块114。
历史状态量获取模块111,用于选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量。
关于历史状态量获取模块111的描述具体可参考上述步骤S100的详细描述,也即,步骤S100可以由历史状态量获取模块111执行,此处不再赘述。
第一参考状态量获取模块112,用于根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,所述第二时刻位于所述第一时刻之后。
所述第一参考状态量获取模块112,具体用于:
根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量。
所述历史状态量包括历史位置、历史线速度和历史加速度,所述第一参考状态量包括第一参考位置,所述运动方程包括第一运动方程。基于此,所述第一参考状态量获取模块112,又具体用于:
根据所述历史位置、所述历史线速度和所述历史加速度与所述第一参考位置的约束关系创建第一运动方程;
根据所述第一运动方程、所述历史位置、所述历史线速度和所述历史加速度,预测扫地机器人的所述第一参考位置。
所述历史状态量包括历史姿态和历史角速度,所述第一参考状态量包括第一参考姿态,所述运动方程包括第二运动方程。基于此,所述第一参考状态量获取模块112,还具体用于:
根据所述第一参考姿态与所述历史姿态和所述历史角速度之间的约束关系创建第二运动方程;
根据所述第二运动方程、所述历史姿态和所述历史角速度,预测扫地机器人的所述第一参考姿态。
关于第一参考状态量获取模块112的描述具体可参考上述步骤S200的详细描述,也即,步骤S200可以由第一参考状态量获取模块112执行,此处不再赘述。
第二参考状态量获取模块113,用于获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量。
所述传感器包括左/右车轮码盘、激光雷达、惯性测量装置,实测数据包括所述左/右车轮码盘测量得到的状态增量数据、所述激光雷达测量得到的点云数据,以及所述惯性测量装置测量得到的实测角速度和实测加速度,所述第二参考状态量包括第二参考姿态和第二参考位置。基于此,所述第二参考状态量获取模块113,具体用于:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置;
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态。
所述第二参考状态量获取模块113,又具体用于:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,并将获得所述增量数据的时刻标记为第一数据时刻;
获得所述激光雷达在第二时刻测量得到的点云数据,并将获得所述点云数据的时刻标记为第二数据时刻,所述第二数据时刻位于所述第一数据时刻之后;
根据所述增量数据和所述点云数据,获得所述第二参考位置,并将获得所述第二参考位置的时刻标记为第三数据时刻,所述第三数据时刻位于所述第二数据时刻之后。
所述第二参考状态量获取模块113,又具体用于:
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并将获得所述实测角速度和所述实测加速度的时刻标记为第四数据时刻,所述第四数据时刻位于所述第二数据时刻和所述第三数据时刻之间;
对所述第四数据时刻和所述第三数据时刻之间获得的N个实测角速度和N个实测加速度进行存储,并为各实测角速度添加第一时刻标签,以及为各实测加速度添加第二时刻标签,其中,N为整数;
从多个所述第一时刻标签中选取出最靠后的第一时刻标签,作为第一待选取标签,并将与所述第一待选取标签对应的实测角速度作为备用实测角速度,以及从多个所述第二时刻标签中选取出最靠后的第二时刻标签,作为第二待选取标签,并将与所述第二待选取标签对应的实测加速度作为备用实测加速度;
根据所述备用实测角速度和所述备用实测加速度,获得第二参考姿态。
关于第二参考状态量获取模块113的描述具体可参考上述步骤S300的详细描述,也即,步骤S300可以由第二参考状态量获取模块113执行,此处不再赘述。
结果状态量获取模块114,用于对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量。
所述结果状态量包括融合姿态和融合位置。所述结果状态量获取模块114,具体用于:
对所述第一参考姿态和所述第二参考姿态进行融合,获得扫地机器人的融合姿态;
对所述第一参考位置和所述第二参考位置进行融合,获得扫地机器人的融合位置。
关于结果状态量获取模块114的描述具体可参考上述步骤S400的详细描述,也即,步骤S400可以由结果状态量获取模块114模块执行,此处不再赘述。
综上所述,本发明实施例提供的扫地机器人定位方法,通过选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量,根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,以及获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量,最后,对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量。显而易见,通过本发明实施例获得的结果状态量,不单根据扫地机器人的历史状态量获得,也不单根据设置于扫地机器人上的传感器在第二时刻的实测数据获得,而是根据扫地机器人的历史状态量,预测扫地机器人在第二时刻的第一参考状态量,并根据扫地机器人上的传感器在第二时刻的实测数据,获得扫地机器人在第二时刻的第二参考状态量,最后对第一参考状态量和第二参考状态量进行融合获得,因此,具有较高的定位精度。
在本发明实施例所提供的上述实施例中,应该理解到,所揭露的装置和方法,也可以通过其它的方式实现。以上所描述的装置和方法实施例仅仅是示意性的,例如,附图中的流程图和框图显示了根据本公开的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分,模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意,在有些作为替换的实现方式中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个连续的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。也要注意的是,框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合,可以用执行规定的功能或动作的专用的基于硬件的系统来实现,或者可以用专用硬件与计算机指令的组合来实现。
另外,在本公开各个实施例中的各功能模块可以集成在一起形成一个独立的部分,也可以是各个模块单独存在,也可以两个或两个以上模块集成形成一个独立的部分。
功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本公开的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,电子设备,或者网络设备等)执行本公开各个实施例方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-OnlN MemorN)、随机存取存储器(RAM,Random Access MemorN)、磁碟或者光盘等各种可以存储程序代码的介质。需要说明的是,在本文中,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个......”限定的要素,并不排除在包括要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上仅为本公开的可选实施例而已,并不用于限制本公开,对于本领域的技术人员来说,本公开可以有各种更改和变化。凡在本公开的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本公开的保护范围之内。
A1.一种扫地机器人定位方法,其特征在于,包括:
选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量;
根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,所述第二时刻位于所述第一时刻之后;
获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量;
对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量。
A2.根据权利要求A1所述的扫地机器人定位方法,其特征在于,所述根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,包括:
根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量。
A3.根据权利要求A2所述的扫地机器人定位方法,其特征在于,所述历史状态量包括历史位置、历史线速度和历史加速度,所述第一参考状态量包括第一参考位置,所述运动方程包括第一运动方程;
所述根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程,包括:
根据所述历史位置、所述历史线速度和所述历史加速度与所述第一参考位置的约束关系创建第一运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量,包括:
根据所述第一运动方程、所述历史位置、所述历史线速度和所述历史加速度,预测扫地机器人的所述第一参考位置。
A4.根据权利要求A3所述的扫地机器人定位方法,其特征在于,所述历史状态量包括历史姿态和历史角速度,所述第一参考状态量包括第一参考姿态,所述运动方程包括第二运动方程;
所述根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程,包括:
根据所述第一参考姿态与所述历史姿态和所述历史角速度之间的约束关系创建第二运动方程;
所述根据所述运动方程、所述历史状态量,预测扫地机器人的所述第一参考状态量,包括:
根据所述第二运动方程、所述历史姿态和所述历史角速度,预测扫地机器人的所述第一参考姿态。
A5.根据权利要求A4所述的扫地机器人定位方法,其特征在于,所述传感器包括左/右车轮码盘、激光雷达、惯性测量装置,所述实测数据包括所述左/右车轮码盘测量得到的状态增量数据、所述激光雷达测量得到的点云数据,以及所述惯性测量装置测量得到的实测角速度和实测加速度,所述第二参考状态量包括第二参考姿态和第二参考位置;
所述获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量,包括:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置;
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态。
A6.根据权利要求A5所述的扫地机器人定位方法,其特征在于,所述获得所述左/右车轮码盘在第二时刻测量得到的增量数据以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置,包括:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,并将获得所述增量数据的时刻标记为第一数据时刻;
获得所述激光雷达在第二时刻测量得到的点云数据,并将获得所述点云数据的时刻标记为第二数据时刻,所述第二数据时刻位于所述第一数据时刻之后;
根据所述增量数据和所述点云数据,获得所述第二参考位置,并将获得所述第二参考位置的时刻标记为第三数据时刻,所述第三数据时刻位于所述第二数据时刻之后。
A7.根据权利要求A6所述的扫地机器人定位方法,其特征在于,所述获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态,包括:
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并将获得所述实测角速度和所述实测加速度的时刻标记为第四数据时刻,所述第四数据时刻位于所述第二数据时刻和所述第三数据时刻之间;
对所述第四数据时刻和所述第三数据时刻之间获得的N个实测角速度和N个实测加速度进行存储,并为各实测角速度添加第一时刻标签,以及为各实测加速度添加第二时刻标签,其中,N为整数;
从多个所述第一时刻标签中选取出最靠后的第一时刻标签,作为第一待选取标签,并将与所述第一待选取标签对应的实测角速度作为备用实测角速度,以及从多个所述第二时刻标签中选取出最靠后的第二时刻标签,作为第二待选取标签,并将与所述第二待选取标签对应的实测加速度作为备用实测加速度;
根据所述备用实测角速度和所述备用实测加速度,获得第二参考姿态。
A8.根据权利要求A5~A7任意一项所述的扫地机器人定位方法,其特征在于,所述结果状态量包括融合姿态和融合位置;
所述对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量,包括:
对所述第一参考姿态和所述第二参考姿态进行融合,获得扫地机器人的融合姿态;
对所述第一参考位置和所述第二参考位置进行融合,获得扫地机器人的融合位置。
B9.一种扫地机器人定位装置,其特征在于,包括:
历史状态量获取模块,用于选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量;
第一参考状态量获取模块,用于根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,所述第二时刻位于所述第一时刻之后;
第二参考状态量获取模块,用于获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量;
结果状态量获取模块,用于对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量。
B10.根据权利要求B9所述的扫地机器人定位装置,其特征在于,所述第一参考状态量获取模块,具体用于:
根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量。
B11.根据权利要求B10所述的扫地机器人定位装置,其特征在于,所述历史状态量包括历史位置、历史线速度和历史加速度,所述第一参考状态量包括第一参考位置,所述运动方程包括第一运动方程;
所述第一参考状态量获取模块,又具体用于:
根据所述历史位置、所述历史线速度和所述历史加速度与所述第一参考位置的约束关系创建第一运动方程;
根据所述第一运动方程、所述历史位置、所述历史线速度和所述历史加速度,预测扫地机器人的所述第一参考位置。
B12.根据权利要求B11所述的扫地机器人定位装置,其特征在于,所述历史状态量包括历史姿态和历史角速度,所述第一参考状态量包括第一参考姿态,所述运动方程包括第二运动方程;
所述第一参考状态量获取模块,还具体用于:
根据所述第一参考姿态与所述历史姿态和所述历史角速度之间的约束关系创建第二运动方程;
根据所述第二运动方程、所述历史姿态和所述历史角速度,预测扫地机器人的所述第一参考姿态。
B13.根据权利要求B12所述的扫地机器人定位装置,其特征在于,所述传感器包括左/右车轮码盘、激光雷达、惯性测量装置,所述实测数据包括所述左/右车轮码盘测量得到的状态增量数据、所述激光雷达测量得到的点云数据,以及所述惯性测量装置测量得到的实测角速度和实测加速度,所述第二参考状态量包括第二参考姿态和第二参考位置;
所述第二参考状态量获取模块,具体用于:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置;
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态。
B14.根据权利要求B13所述的扫地机器人定位装置,其特征在于,所述第二参考状态量获取模块,又具体用于:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,并将获得所述增量数据的时刻标记为第一数据时刻;
获得所述激光雷达在第二时刻测量得到的点云数据,并将获得所述点云数据的时刻标记为第二数据时刻,所述第二数据时刻位于所述第一数据时刻之后;
根据所述增量数据和所述点云数据,获得所述第二参考位置,并将获得所述第二参考位置的时刻标记为第三数据时刻,所述第三数据时刻位于所述第二数据时刻之后。
B15.根据权利要求B14所述的扫地机器人定位装置,其特征在于,所述第二参考状态量获取模块,又具体用于:
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并将获得所述实测角速度和所述实测加速度的时刻标记为第四数据时刻,所述第四数据时刻位于所述第二数据时刻和所述第三数据时刻之间;
对所述第四数据时刻和所述第三数据时刻之间获得的N个实测角速度和N个实测加速度进行存储,并为各实测角速度添加第一时刻标签,以及为各实测加速度添加第二时刻标签,其中,N为整数;
从多个所述第一时刻标签中选取出最靠后的第一时刻标签,作为第一待选取标签,并将与所述第一待选取标签对应的实测角速度作为备用实测角速度,以及从多个所述第二时刻标签中选取出最靠后的第二时刻标签,作为第二待选取标签,并将与所述第二待选取标签对应的实测加速度作为备用实测加速度;
根据所述备用实测角速度和所述备用实测加速度,获得第二参考姿态。
B16.根据权利要求B13~B15任意一项所述的扫地机器人定位装置,其特征在于,所述结果状态量包括融合姿态和融合位置;
所述结果状态量获取模块,具体用于:
对所述第一参考姿态和所述第二参考姿态进行融合,获得扫地机器人的融合姿态;
对所述第一参考位置和所述第二参考位置进行融合,获得扫地机器人的融合位置。
C17.一种扫地机器人,其特征在于,包括处理器、存储器和权利要求B9-B16所述的扫地机器人定位装置,所述扫地机器人定位装置包括一个或多个存储于所述存储器并由所述处理器执行的软件功能模块。
D18.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被执行时,可以实现权利要求A1-A8中任意一项所述的扫地机器人定位方法。
Claims (16)
1.一种扫地机器人定位方法,其特征在于,包括:
选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量;
根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,所述第二时刻位于所述第一时刻之后;
获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量;
对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量;
所述传感器包括左/右车轮码盘、激光雷达、惯性测量装置,所述实测数据包括所述左/右车轮码盘测量得到的状态增量数据、所述激光雷达测量得到的点云数据,以及所述惯性测量装置测量得到的实测角速度和实测加速度,所述第二参考状态量包括第二参考姿态和第二参考位置;
所述获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量,包括:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置;
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态。
2.根据权利要求1所述的扫地机器人定位方法,其特征在于,所述根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,包括:
根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量。
3.根据权利要求2所述的扫地机器人定位方法,其特征在于,所述历史状态量包括历史位置、历史线速度和历史加速度,所述第一参考状态量包括第一参考位置,所述运动方程包括第一运动方程;
所述根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程,包括:
根据所述历史位置、所述历史线速度和所述历史加速度与所述第一参考位置的约束关系创建第一运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量,包括:
根据所述第一运动方程、所述历史位置、所述历史线速度和所述历史加速度,预测扫地机器人的所述第一参考位置。
4.根据权利要求3所述的扫地机器人定位方法,其特征在于,所述历史状态量包括历史姿态和历史角速度,所述第一参考状态量包括第一参考姿态,所述运动方程包括第二运动方程;
所述根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程,包括:
根据所述第一参考姿态与所述历史姿态和所述历史角速度之间的约束关系创建第二运动方程;
所述根据所述运动方程、所述历史状态量,预测扫地机器人的所述第一参考状态量,包括:
根据所述第二运动方程、所述历史姿态和所述历史角速度,预测扫地机器人的所述第一参考姿态。
5.根据权利要求4所述的扫地机器人定位方法,其特征在于,所述获得所述左/右车轮码盘在第二时刻测量得到的增量数据以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置,包括:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,并将获得所述增量数据的时刻标记为第一数据时刻;
获得所述激光雷达在第二时刻测量得到的点云数据,并将获得所述点云数据的时刻标记为第二数据时刻,所述第二数据时刻位于所述第一数据时刻之后;
根据所述增量数据和所述点云数据,获得所述第二参考位置,并将获得所述第二参考位置的时刻标记为第三数据时刻,所述第三数据时刻位于所述第二数据时刻之后。
6.根据权利要求5所述的扫地机器人定位方法,其特征在于,所述获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态,包括:
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并将获得所述实测角速度和所述实测加速度的时刻标记为第四数据时刻,所述第四数据时刻位于所述第二数据时刻和所述第三数据时刻之间;
对所述第四数据时刻和所述第三数据时刻之间获得的N个实测角速度和N个实测加速度进行存储,并为各实测角速度添加第一时刻标签,以及为各实测加速度添加第二时刻标签,其中,N为整数;
从多个所述第一时刻标签中选取出最靠后的第一时刻标签,作为第一待选取标签,并将与所述第一待选取标签对应的实测角速度作为备用实测角速度,以及从多个所述第二时刻标签中选取出最靠后的第二时刻标签,作为第二待选取标签,并将与所述第二待选取标签对应的实测加速度作为备用实测加速度;
根据所述备用实测角速度和所述备用实测加速度,获得第二参考姿态。
7.根据权利要求5~6任意一项所述的扫地机器人定位方法,其特征在于,所述结果状态量包括融合姿态和融合位置;
所述对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量,包括:
对所述第一参考姿态和所述第二参考姿态进行融合,获得扫地机器人的融合姿态;
对所述第一参考位置和所述第二参考位置进行融合,获得扫地机器人的融合位置。
8.一种扫地机器人定位装置,其特征在于,包括:
历史状态量获取模块,用于选取第一时刻,并获得扫地机器人在所述第一时刻的历史状态量;
第一参考状态量获取模块,用于根据所述历史状态量,预测扫地机器人在第二时刻的第一参考状态量,所述第二时刻位于所述第一时刻之后;
第二参考状态量获取模块,用于获得设置于扫地机器人上的传感器在第二时刻的实测数据,并根据所述实测数据,获得扫地机器人在第二时刻的第二参考状态量;
结果状态量获取模块,用于对所述第一参考状态量和所述第二参考状态量进行融合,获得扫地机器人在第二时刻的结果状态量;
所述传感器包括左/右车轮码盘、激光雷达、惯性测量装置,所述实测数据包括所述左/右车轮码盘测量得到的状态增量数据、所述激光雷达测量得到的点云数据,以及所述惯性测量装置测量得到的实测角速度和实测加速度,所述第二参考状态量包括第二参考姿态和第二参考位置;
所述第二参考状态量获取模块,具体用于:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,以及所述激光雷达在第二时刻测量得到的点云数据,并根据所述增量数据和所述点云数据,获得第二参考位置;
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并根据所述实测角速度和所述实测加速度,获得第二参考姿态。
9.根据权利要求8所述的扫地机器人定位装置,其特征在于,所述第一参考状态量获取模块,具体用于:
根据扫地机器人在第二时刻的第一参考状态量与所述历史状态量之间的约束关系,创建运动方程;
根据所述运动方程和所述历史状态量,预测扫地机器人的所述第一参考状态量。
10.根据权利要求9所述的扫地机器人定位装置,其特征在于,所述历史状态量包括历史位置、历史线速度和历史加速度,所述第一参考状态量包括第一参考位置,所述运动方程包括第一运动方程;
所述第一参考状态量获取模块,又具体用于:
根据所述历史位置、所述历史线速度和所述历史加速度与所述第一参考位置的约束关系创建第一运动方程;
根据所述第一运动方程、所述历史位置、所述历史线速度和所述历史加速度,预测扫地机器人的所述第一参考位置。
11.根据权利要求10所述的扫地机器人定位装置,其特征在于,所述历史状态量包括历史姿态和历史角速度,所述第一参考状态量包括第一参考姿态,所述运动方程包括第二运动方程;
所述第一参考状态量获取模块,还具体用于:
根据所述第一参考姿态与所述历史姿态和所述历史角速度之间的约束关系创建第二运动方程;
根据所述第二运动方程、所述历史姿态和所述历史角速度,预测扫地机器人的所述第一参考姿态。
12.根据权利要求11所述的扫地机器人定位装置,其特征在于,所述第二参考状态量获取模块,又具体用于:
获得所述左/右车轮码盘在第二时刻测量得到的增量数据,并将获得所述增量数据的时刻标记为第一数据时刻;
获得所述激光雷达在第二时刻测量得到的点云数据,并将获得所述点云数据的时刻标记为第二数据时刻,所述第二数据时刻位于所述第一数据时刻之后;
根据所述增量数据和所述点云数据,获得所述第二参考位置,并将获得所述第二参考位置的时刻标记为第三数据时刻,所述第三数据时刻位于所述第二数据时刻之后。
13.根据权利要求12所述的扫地机器人定位装置,其特征在于,所述第二参考状态量获取模块,又具体用于:
获得所述惯性测量装置在第二时刻测量得到的实测角速度和实测加速度,并将获得所述实测角速度和所述实测加速度的时刻标记为第四数据时刻,所述第四数据时刻位于所述第二数据时刻和所述第三数据时刻之间;
对所述第四数据时刻和所述第三数据时刻之间获得的N个实测角速度和N个实测加速度进行存储,并为各实测角速度添加第一时刻标签,以及为各实测加速度添加第二时刻标签,其中,N为整数;
从多个所述第一时刻标签中选取出最靠后的第一时刻标签,作为第一待选取标签,并将与所述第一待选取标签对应的实测角速度作为备用实测角速度,以及从多个所述第二时刻标签中选取出最靠后的第二时刻标签,作为第二待选取标签,并将与所述第二待选取标签对应的实测加速度作为备用实测加速度;
根据所述备用实测角速度和所述备用实测加速度,获得第二参考姿态。
14.根据权利要求12~13任意一项所述的扫地机器人定位装置,其特征在于,所述结果状态量包括融合姿态和融合位置;
所述结果状态量获取模块,具体用于:
对所述第一参考姿态和所述第二参考姿态进行融合,获得扫地机器人的融合姿态;
对所述第一参考位置和所述第二参考位置进行融合,获得扫地机器人的融合位置。
15.一种扫地机器人,其特征在于,包括处理器、存储器和权利要求8-14任意一项所述的扫地机器人定位装置,所述扫地机器人定位装置包括一个或多个存储于所述存储器并由所述处理器执行的软件功能模块。
16.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被执行时,可以实现权利要求1-7中任意一项所述的扫地机器人定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910121095.5A CN111568304B (zh) | 2019-02-18 | 2019-02-18 | 扫地机器人定位方法、装置及扫地机器人 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910121095.5A CN111568304B (zh) | 2019-02-18 | 2019-02-18 | 扫地机器人定位方法、装置及扫地机器人 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111568304A CN111568304A (zh) | 2020-08-25 |
CN111568304B true CN111568304B (zh) | 2023-09-01 |
Family
ID=72124188
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910121095.5A Active CN111568304B (zh) | 2019-02-18 | 2019-02-18 | 扫地机器人定位方法、装置及扫地机器人 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111568304B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112535434B (zh) * | 2020-10-23 | 2022-01-11 | 湖南新视电子技术有限公司 | 一种无尘室智能扫地机器人 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106123890A (zh) * | 2016-06-14 | 2016-11-16 | 中国科学院合肥物质科学研究院 | 一种多传感器数据融合的机器人定位方法 |
CN106643694A (zh) * | 2016-11-04 | 2017-05-10 | 航天科工智能机器人有限责任公司 | 一种机器人室内定位方法 |
CN107782304A (zh) * | 2017-10-26 | 2018-03-09 | 广州视源电子科技股份有限公司 | 移动机器人的定位方法及装置、移动机器人及存储介质 |
WO2018148195A1 (en) * | 2017-02-08 | 2018-08-16 | Marquette University | Robotic tracking navigation with data fusion |
CN109084732A (zh) * | 2018-06-29 | 2018-12-25 | 北京旷视科技有限公司 | 定位与导航方法、装置及处理设备 |
-
2019
- 2019-02-18 CN CN201910121095.5A patent/CN111568304B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106123890A (zh) * | 2016-06-14 | 2016-11-16 | 中国科学院合肥物质科学研究院 | 一种多传感器数据融合的机器人定位方法 |
CN106643694A (zh) * | 2016-11-04 | 2017-05-10 | 航天科工智能机器人有限责任公司 | 一种机器人室内定位方法 |
WO2018148195A1 (en) * | 2017-02-08 | 2018-08-16 | Marquette University | Robotic tracking navigation with data fusion |
CN107782304A (zh) * | 2017-10-26 | 2018-03-09 | 广州视源电子科技股份有限公司 | 移动机器人的定位方法及装置、移动机器人及存储介质 |
CN109084732A (zh) * | 2018-06-29 | 2018-12-25 | 北京旷视科技有限公司 | 定位与导航方法、装置及处理设备 |
Also Published As
Publication number | Publication date |
---|---|
CN111568304A (zh) | 2020-08-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107703756B (zh) | 动力学模型参数辨识方法、装置、计算机设备及存储介质 | |
CN111278613B (zh) | 校准装置、校准方法和控制装置 | |
CN111649739B (zh) | 定位方法和装置、自动驾驶车辆、电子设备和存储介质 | |
CN108897836B (zh) | 一种机器人基于语义进行地图构建的方法和装置 | |
CN110262486B (zh) | 一种无人驾驶设备运动控制方法及装置 | |
KR20200015722A (ko) | 포인트 클라우드에서의 모션 기반 부정확성의 정정 | |
CN111288971B (zh) | 一种视觉定位方法及装置 | |
CN114179825B (zh) | 多传感器融合获取量测值置信度方法及自动驾驶车辆 | |
CN111319041B (zh) | 一种机器人位姿确定方法、装置、可读存储介质及机器人 | |
CN109141411B (zh) | 定位方法、定位装置、移动机器人及存储介质 | |
CN110825111A (zh) | 适用于高架仓库货物盘点的无人机控制方法、货物盘点方法、装置、服务器及存储介质 | |
US20230392938A1 (en) | Navigation information processing method, electronic device, and storage medium | |
CN114527737A (zh) | 用于自动驾驶的速度规划方法、装置、设备、介质和车辆 | |
CN111223145A (zh) | 数据处理方法、系统、服务装置及其存储介质 | |
CN111568304B (zh) | 扫地机器人定位方法、装置及扫地机器人 | |
CN110942474A (zh) | 机器人目标跟踪方法、设备及存储介质 | |
CN108236782B (zh) | 外接设备的定位方法及装置、虚拟现实设备及系统 | |
CN111761583B (zh) | 一种智能机器人运动定位方法及系统 | |
CN112902951A (zh) | 一种行驶设备的定位方法、装置、设备及存储介质 | |
CN109947116B (zh) | 无人驾驶汽车的定位方法及装置 | |
CN109459769B (zh) | 一种自主定位方法与系统 | |
CN114593735B (zh) | 一种位姿预测方法及装置 | |
CN114147717B (zh) | 机器人运动轨迹估计方法、装置、控制器及存储介质 | |
Valera et al. | Design and implementation of Kalman filters applied to Lego NXT based robots | |
CN114061573A (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |