CN113438615A - 定位方法和装置 - Google Patents

定位方法和装置 Download PDF

Info

Publication number
CN113438615A
CN113438615A CN202010143459.2A CN202010143459A CN113438615A CN 113438615 A CN113438615 A CN 113438615A CN 202010143459 A CN202010143459 A CN 202010143459A CN 113438615 A CN113438615 A CN 113438615A
Authority
CN
China
Prior art keywords
mobile robot
base station
value
positioning
distance
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
Application number
CN202010143459.2A
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.)
Beijing Jingdong Qianshi Technology Co Ltd
Original Assignee
Beijing Jingdong Qianshi Technology Co Ltd
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 Beijing Jingdong Qianshi Technology Co Ltd filed Critical Beijing Jingdong Qianshi Technology Co Ltd
Priority to CN202010143459.2A priority Critical patent/CN113438615A/zh
Publication of CN113438615A publication Critical patent/CN113438615A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
    • 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
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/02—Systems for determining distance or velocity not using reflection or reradiation using radio waves
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/024—Guidance services

Abstract

本发明公开了一种定位方法和装置，涉及计算机技术领域。其中，该方法包括：获取移动机器人的传感器测量数据；其中，所述传感器测量数据包括：移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号基站的距离测量值；根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值。通过以上步骤，使得只需在应用场景中布放1个信号基站，并结合移动机器人身上的其他传感器即可显著提高移动机器人的导航定位精度，降低移动机器人进行导航定位的成本。

Description

定位方法和装置
技术领域
本发明涉及计算机技术领域，尤其涉及一种定位方法和装置。
背景技术
移动机器人，尤其是室内移动机器人的应用场景非常广泛，比如室内服务机器人、扫地机器人等商用场景，以及搬运、牵引AGV(自动导引车)等工业、物流场景。定位技术作为其关键技术之一，对不同的应用场景有不同的技术需求。
在现有技术中，移动机器人可基于单一导航定位技术进行导航定位。目前常用的导航定位技术有惯性、视觉、激光、磁条、无线定位(如UWB，WIFI)等。
在实现本发明过程中，发明人发现现有技术中至少存在如下问题：第一、单一的导航方案通常很难满足移动机器人的定位精度需求。第二、现有的无线定位方式采用三角定位解算原理，需要至少三个信号基站才可对移动机器人进行定位。由于信号基站的建设成本较高，且由于信号基站的信号范围有限，需要每隔几百米就要安装信号基站，进一步增加了导航定位的成本。另外，对于一些对定位精度要求高的应用场景，现有无线定位方式仍难以满足精度要求。
发明内容
有鉴于此，本发明提供了一种定位方法和装置，通过以上方法和装置使得只需在应用场景中布放1个信号基站，并结合移动机器人身上的其他传感器即可显著提高移动机器人的导航定位精度，降低移动机器人进行导航定位的成本。
为实现上述目的，根据本发明的一个方面，提供了一种定位方法。
本发明的定位方法包括：获取移动机器人的传感器测量数据；其中，所述传感器测量数据包括：移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号基站的距离测量值；根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值；其中，所述移动机器人的位姿估计值包括：移动机器人的位置估计值、以及移动机器人的航向角估计值。
可选地，所述多源信息融合定位模型满足：所述多源信息融合定位模型中的系统状态方程是以移动机器人的位置、航向角为状态量构建的；所述多源信息融合定位模型中的观测方程是以移动机器人至预先设置的信号基站的距离为观测量构建的。
可选地，所述多源信息融合定位模型为扩展卡尔曼滤波模型；所述根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值包括：对系统状态方程和观测方程进行线性化处理，并计算状态转移矩阵和观测矩阵；根据所述状态转移矩阵、所述移动机器人的速度测量值、以及航向角测量值计算移动机器人位姿的一步预测值以及一步预测方差矩阵；在所述距离测量值有效的情况下，根据所述一步预测方差矩阵、所述观测矩阵、以及距离测量噪声方差矩阵计算滤波增益矩阵；根据所述观测矩阵计算所述移动机器人至预先设置的信号基站的距离的一步预测值；根据所述移动机器人位姿的一步预测值、所述距离的一步预测值、所述滤波增益矩阵、以及所述距离测量值，计算移动机器人的位姿估计值。
可选地，所述方法还包括：在所述距离测量值无效的情况下，将所述移动机器人位姿的一步预测值作为所述移动机器人的位姿估计值。
可选地，所述获取移动机器人的传感器测量数据包括：通过测速里程计获得移动机器人的速度测量值，通过惯导传感器获得移动机器人的航向角测量值，通过移动机器人携带的电子标签获得移动机器人至预先设置的信号基站的距离测量值。
可选地，所述信号基站为UWB超宽带基站；所述通过移动机器人携带的电子标签获得移动机器人至预先设置的信号基站的距离测量值包括：通过移动机器人携带的UWB标签接收UWB基站的信号，根据无线电测距原理确定移动机器人至UWB基站的距离测量值。
为实现上述目的，根据本发明的另一方面，提供了一种定位装置。
本发明的定位装置包括：获取模块，用于获取移动机器人的传感器测量数据；其中，所述传感器测量数据包括：移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号超宽带基站的距离测量值；定位模块，用于根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值；其中，所述移动机器人的位姿估计值包括：移动机器人的位置估计值、以及移动机器人的航向角估计值。
可选地，所述定位模块配置的多源信息融合定位模型满足：所述多源信息融合定位模型中的系统状态方程是以移动机器人的位置、航向角为状态量构建的；所述多源信息融合定位模型中的观测方程是以移动机器人至预先设置的信号基站的距离为观测量构建的。
为实现上述目的，根据本发明的再一个方面，提供了一种电子设备。
本发明的电子设备，包括：一个或多个处理器；以及，存储装置，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明的定位方法。
为实现上述目的，根据本发明的又一个方面，提供了一种计算机可读介质。
本发明的计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时实现本发明的定位方法。
上述发明中的一个实施例具有如下优点或有益效果：通过获取移动机器人的传感器测量数据；其中，所述传感器测量数据包括：移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号基站的距离测量值；根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值这些步骤，使得只需在应用场景中布放1个信号基站，并结合移动机器人身上的其他传感器即可显著提高移动机器人的导航定位精度，降低移动机器人进行导航定位的成本。
上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。
附图说明
附图用于更好地理解本发明，不构成对本发明的不当限定。其中：
图1是现有技术中的无线定位原理示意图；
图2是根据本发明一个实施例的定位方法的主要流程示意图；
图3是根据本发明一个实施例的定位原理示意图；
图4是根据本发明一个实施例的基于EKF滤波器进行位姿估计的主要流程示意图；
图5是根据本发明一个实施例的定位装置的主要模块示意图；
图6是本发明实施例可以应用于其中的示例性系统架构图；
图7是适于用来实现本发明实施例的电子设备的计算机系统的结构示意图。
具体实施方式
以下结合附图对本发明的示范性实施例做出说明，其中包括本发明实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本发明的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
需要指出的是，在不冲突的情况下，本发明中的实施例以及实施例中的特征可以相互组合。
在详细介绍本发明的实施例之前，首先对本发明实施例涉及的部分技术术语进行说明。
UWB：作为一种无线定位技术，近些年开始应用于室内移动机器人。UWB定位原理类似于GPS定位，主要用于没有GPS信号的室内环境，尤其是在一些类似工业搬运的大范围场景环境下，比如长走廊区域、工作人员聚集区域、机器人对接等对定位精度要求高的区域等。
图1是现有技术中的无线定位原理示意图。在图1中，方块A、B、C、D分别表示安装的信号基站设备，圆圈表示AGV(自动导引车，其属于一种移动机器人)。该AGV携带有电子标签，可以获取信号基站发出的信号。根据无线电测距原理，移动机器人可以获取自身到各个信号基站的距离测量值。在此基础上，可通过三角定位解算原理，获知移动机器人的定位信息。基于三角定位解算原理的处理过程具体包括：
假设AGV在t时刻的位置坐标为(x,y)，信号基站在t时刻的位置坐标为(xi,yi)，进一步，假设t时刻接收到3个可用的信号基站的信号，则t时刻AGV的位置坐标与信号基站的位置坐标满足如下方程：
Figure BDA0002399897500000061
Figure BDA0002399897500000062
Figure BDA0002399897500000063
进一步，用第1、第2行方程分别减去第3行方程，可得到如下线性关系：
AX＝b
其中：
Figure BDA0002399897500000064
Figure BDA0002399897500000065
进一步，根据最小二乘准则，可得到t时刻AGV的位置坐标为：
X＝(ATA)-1ATb
可见，现有基于信号基站的定位方式需要至少三个信号基站才可实现移动机器人的定位。由于信号基站的建设成本较高，且由于信号基站的信号范围有限，需要每隔几百米就要安装信号基站，进一步增加了导航定位的成本。另外，对于一些对定位精度要求高的应用场景，现有定位方式仍难以满足精度要求。
鉴于此，本发明的发明人提出了一种新的定位方法和装置，通过以上方法和装置使得只需在应用场景中布放1个信号基站，并结合移动机器人身上的其他传感器即可显著提高移动机器人的导航定位精度，降低移动机器人进行导航定位的成本。
图2是根据本发明一个实施例的定位方法的主要流程示意图。如图2所示，本发明实施例的定位方法包括：
步骤S201、获取移动机器人的传感器测量数据；其中，所述传感器测量数据包括：移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号基站的距离测量值。
其中，所述移动机器人可安装有多种传感器，比如激光器、摄像头、里程计、惯性器件等用于导航定位的传感器中的一种或多种。此外，所述移动机器人还安装有电子标签，以用于接收信号基站无线电信号，进而获取移动机器人自身到信号基站的距离信息。
在本发明实施例中，可先通过移动机器人(比如AGV)安装的传感器采集传感器测量数据，然后将所述传感器测量数据传递至定位装置，进而使得定位装置能够获取到这些传感器测量数据。示例性地，可通过测速里程计测得机器人的速度，通过惯导传感器测得移动机器人的航向角，通过移动机器人携带的电子标签可测得移动机器人到信号基站的距离，然后将这些传感器测量数据传递至定位装置。
在一个可选示例中，所述信号基站为UWB(超宽带)基站，所述移动机器人携带的电子标签为UWB标签。在该可选示例中，移动机器人携带的UWB标签可接收UWB基站发送的UWB信号，根据无线电测距原理可确定该UWB基站距移动机器人的距离测量值。通过选用UWB基站作为信号基站进行无线定位，相比WIFI、红外、蓝牙等其他无线定位方案，能够进一步提高定位精度和定位可靠性。需要指出的是，在不影响本发明实施的情况下，所述信号基站也可选用发送WIFI、红外、蓝牙信号的基站。
步骤S202、根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值。
其中，所述多源信息融合定位模型满足：所述多源信息融合定位模型中的系统状态方程是以移动机器人的位置、航向角为状态量构建的；所述多源信息融合定位模型中的观测方程是以移动机器人至预先设置的信号基站的距离为观测量构建的。
在一个可选示例中，假设k时刻移动机器人的位姿为
Figure BDA0002399897500000071
k-1时刻移动机器人的位姿为
Figure BDA0002399897500000072
则所述系统状态方程(又可称为“运动学方程”或者“运动学模型)可表示为：
Figure BDA0002399897500000073
式中，T为k-1时刻与k时刻的时间间隔；
Figure BDA0002399897500000074
为测速里程计测得的k-1时刻移动机器人的速度；
Figure BDA0002399897500000081
为惯导传感器测得的k-1时刻移动机器人的航向角；
Figure BDA0002399897500000082
为测速里程计的测量噪声；
Figure BDA0002399897500000083
为惯导传感器的测量噪声。
进一步，所述系统状态方程可简写为：
Xk＝f(Xk-1,uk-1,wk-1) (2)
式中，
Figure BDA0002399897500000084
Figure BDA0002399897500000085
为测速里程计和惯导传感器的测量噪声，两者为相互独立的高斯白噪声。
根据测速里程计和惯导传感器的测量噪声，可得运动学模型误差的方差矩阵为：
Figure BDA0002399897500000086
式中，Qk-1为运动学模型误差的方差矩阵；
Figure BDA0002399897500000087
为测速里程计、惯导传感器测量噪声的方差参数。
此外，在该可选示例中，假设信号基站的位置坐标为(x(u),y(u))，移动机器人由k-1时刻运动至k时刻，移动机器人在k-1时刻的位置坐标为
Figure BDA0002399897500000088
移动机器人在k时刻的位置坐标为
Figure BDA0002399897500000089
k-1时刻测得移动机器人至信号基站的距离为rk-1，k时刻测得移动机器人至信号基站的距离为rk，则观测方程可表示为：
Figure BDA00023998975000000810
进一步，所述观测方程可简化为：
Zk＝h(Xk,vk) (5)
式中，
Figure BDA00023998975000000811
为k时刻移动机器人至信号基站的距离测量值，vk为距离测量噪声。
在步骤S202中，可基于EKF(扩展卡尔曼滤波)模型、UKF(无损卡尔曼滤波)模型、或者粒子滤波模型等多源信息融合定位模型，对步骤S201获取到的传感器测量数据进行融合处理，以确定移动机器人的位姿估计值。其中，所述移动机器人的位姿估计值包括：移动机器人的位置估计值、以及移动机器人的航向角估计值。
在本发明实施例中，通过获取包括移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号基站的距离测量值在内的传感器测量数据；根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值这些步骤，使得只需在应用场景中布放1个信号基站，并结合移动机器人身上的其他传感器即可显著提高移动机器人的导航定位精度，降低移动机器人进行导航定位的成本。
图3是根据本发明一个实施例的定位原理示意图。在图3中，方块表示在应用场景中安装的一个UWB基站，圆圈表示AGV。该AGV上安装有多种用于导航定位的传感器，比如测速里程计、惯导传感器、摄像头、激光器等，该AGV还携带有UWB标签，用于接收k-1时刻、k时刻的UWB基站信号，进而可得到k-1时刻、k时刻移动机器人至UWB基站的距离测量值。在本发明实施例中，可通过融合多种传感器测量数据对移动机器人进行定位，使得只需在应用场景中布放1个UWB基站，并结合移动机器人身上的其他传感器即可显著提高移动机器人的导航定位精度，降低移动机器人进行导航定位的成本。
图4是根据本发明一个实施例的基于EKF滤波器进行位姿估计的主要流程示意图。如图4所示，本发明实施例的基于EKF滤波器进行位姿估计的流程包括：
步骤S401、对系统状态方程和观测方程进行线性化处理，并计算状态转移矩阵和观测矩阵。
在该步骤中，考虑到系统状态方程和观测方程的非线性，可对如上文中式(2)和式(5)所示的系统状态方程和观测方程进行线性化处理。示例性地，该线性化处理可具体包括：选取状态变量
Figure BDA0002399897500000091
对系统状态方程和观测方程分别围绕位姿估计值(或者称为状态量滤波值)
Figure BDA0002399897500000092
和位姿一步预测值
Figure BDA0002399897500000093
进行一阶泰勒展开，并忽略线性化误差，进而可得线性化处理后的系统状态方程和观测方程。
另外，在该步骤中，可通过系统状态方程对状态变量Xk-1在
Figure BDA0002399897500000101
点处求偏导可得状态转移矩阵，通过系统状态方程对系统输入噪声uk-1在
Figure BDA0002399897500000102
点处求偏导可得噪声驱动矩阵，通过观测方程对状态变量Xk在
Figure BDA0002399897500000103
点处求偏导可得观测矩阵。进一步，状态转移矩阵、噪声驱动矩阵、观测矩阵可分别表示为：
Figure BDA0002399897500000104
Figure BDA0002399897500000105
Figure BDA0002399897500000106
式中，Fk表示状态转移矩阵，
Figure BDA0002399897500000107
表示k-1时刻移动机器人的位姿估计值，Γk表示噪声驱动矩阵，
Figure BDA0002399897500000108
表示k-1时刻的系统输入噪声，Hk表示观测矩阵，
Figure BDA0002399897500000109
表示通过状态方程由k-1时刻移动机器人的位姿估计值得到的k时刻的位姿预测值，或者称为移动机器人位姿的一步预测值。
步骤S402、根据所述状态转移矩阵、所述移动机器人的速度测量值、以及航向角测量值计算移动机器人位姿的一步预测值以及一步预测方差值。
示例性地，在该步骤中，可根据如下公式计算移动机器人位姿的一步预测值以及一步预测方差矩阵：
Figure BDA00023998975000001010
Figure BDA00023998975000001011
式中，
Figure BDA00023998975000001012
表示移动机器人位姿的一步预测值，Pk|k-1表示机器人位姿的一步预测方差矩阵，
Figure BDA00023998975000001013
的表达式可通过状态转移矩阵等参数来表示。
步骤S403、判断距离测量值是否有效。
示例性地，该步骤可具体包括：判断距离测量值的误差是否小于预设阈值；在所述距离测量值的误差小于预设阈值的情况下，确定所述距离测量值有效；在所述距离测量值的误差大于或等于预设阈值的情况下，确定所述距离测量值无效。需要指出的是，本发明并不局限于以上一种判断距离测量值是否有效的实施方式。在不影响本发明实施的情况下，本发明还可采取其他判断距离测量值是否有效的实施方式。
进一步，在判断出所述距离测量值有效的情况下，可执行步骤S404至步骤S406；在判断出所述距离测量值无效的情况下，可执行步骤S407。
步骤S404、根据所述一步预测方差矩阵，所述观测矩阵、以及距离测量噪声方差矩阵计算滤波增益矩阵。
示例性地，在该步骤中，可根据如下公式计算滤波增益矩阵：
Figure BDA0002399897500000111
式中，Kk为滤波增益矩阵，Pkk-1为机器人位姿的一步预测方差值，Rk为距离测量噪声方差矩阵，且其满足：
Figure BDA0002399897500000112
步骤S405、根据所述观测矩阵计算所述移动机器人至预先设置的UWB基站的距离的一步预测值。
示例性地，在该步骤中，可根据如下公式计算所述观测矩阵计算所述移动机器人至预先设置的UWB基站的距离的一步预测值：
Figure BDA0002399897500000113
式中，Zk|k-1为移动机器人至预先设置的UWB基站的距离的一步预测值，
Figure BDA0002399897500000114
的表达式可通过观测矩阵等参数来表示。
步骤S406、根据所述移动机器人位姿的一步预测值、所述距离的一步预测值、所述滤波增益矩阵、以及所述距离测量值，计算移动机器人的位姿估计值。
示例性地，在该步骤中，可通过如下公式计算移动机器人的位姿估计值：
Figure BDA0002399897500000115
式中，
Figure BDA0002399897500000116
表示k时刻移动机器人的位姿估计值，Zk表示k时刻移动机器人至UWB基站的距离测量值。
进一步，步骤S406还可包括：根据滤波增益矩阵、观测矩阵、以及机器人位姿的一步预测方差值更新位姿估计值的误差矩阵，具体计算公式如下：
Pk＝(I-KkHk)Pk|k-1 (14)
式中，Pk表示由k时刻移动机器人的位姿估计值
Figure BDA0002399897500000121
代替真实状态量Xk的误差矩阵。
步骤S407、将移动机器人位姿的一步预测值作为所述移动机器人的位姿估计值。
进一步，步骤S407还可包括：将移动机器人位姿的一步预测方差矩阵作为所述移动机器人的位姿估计值的误差矩阵。
在本发明实施例中，通过以上步骤实现了一种可用于室内定位的定位方案，通过以上定位方案使得只需在对导航定位性能有特殊要求的场景布放1个UWB基站即可显著提高移动机器人导航定位功能的鲁棒性，降低移动机器人导航定位的成本。
需要指出的是，本发明并不局限于图4所示基于EKF滤波器的融合定位流程。在不影响本发明实施的情况下，本发明还可采用基于UKF滤波器、粒子滤波器等多源信息融合定位模型对包括移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的UWB超宽带基站的距离测量值在内的传感器测量数据进行融合处理，以得到移动机器人的位姿估计值。
图5是根据本发明一个实施例的定位装置的主要模块示意图。如图5所示，本发明实施例中的定位装置500包括：获取模块501、定位模块。
获取模块501，用于获取移动机器人的传感器测量数据。其中，所述传感器测量数据包括：移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号基站的距离测量值。
其中，所述移动机器人可安装有多种传感器，比如激光器、摄像头、里程计、惯性器件等用于导航定位的传感器中的一种或多种。此外，所述移动机器人还安装有电子标签，以用于接收信号基站无线电信号，进而获取移动机器人自身到信号基站的距离信息。
在本发明实施例中，可先通过移动机器人(比如AGV)安装的传感器采集传感器测量数据，然后将所述传感器测量数据传递至定位装置，进而使得定位装置能够获取到这些传感器测量数据。示例性地，可通过测速里程计测得机器人的速度，通过惯导传感器测得移动机器人的航向角，通过移动机器人携带的电子标签可测得移动机器人到信号基站的距离，然后将这些传感器测量数据传递至定位装置。
在一个可选示例中，所述信号基站为UWB(超宽带)基站，所述移动机器人携带的电子标签为UWB标签。在该可选示例中，移动机器人携带的UWB标签可接收UWB基站发送的UWB信号，根据无线电测距原理可确定该UWB基站距移动机器人的距离测量值。
定位模块，用于根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值。
其中，所述多源信息融合定位模型满足：所述多源信息融合定位模型中的系统状态方程是以移动机器人的位置、航向角为状态量构建的；所述多源信息融合定位模型中的观测方程是以移动机器人至预先设置的UWB基站的距离为观测量构建的。
示例性地，定位模块502可基于EKF(扩展卡尔曼滤波)模型、UKF(无损卡尔曼滤波)模型、或者粒子滤波模型等多源信息融合定位模型，对获取模块501获取到的传感器测量数据进行融合处理，以确定移动机器人的位姿估计值。其中，所述移动机器人的位姿估计值包括：移动机器人的位置估计值、以及移动机器人的航向角估计值。
进一步，定位模块502根据EKF模型对传感器测量数据进行融合处理可具体包括步骤A1至A7。
步骤A1、定位模块502对系统状态方程和观测方程进行线性化处理，并计算状态转移矩阵和观测矩阵。
在该步骤中，考虑到系统状态方程和观测方程的非线性，可对如上文中式(2)和式(5)所示的系统状态方程和观测方程进行线性化处理。示例性地，该线性化处理可具体包括：选取状态变量
Figure BDA0002399897500000131
对系统状态方程和观测方程分别围绕位姿估计值(或者称为状态量滤波值)
Figure BDA0002399897500000132
和位姿一步预测值
Figure BDA0002399897500000133
进行一阶泰勒展开，并忽略线性化误差，进而可得线性化处理后的系统状态方程和观测方程。
另外，在该步骤中，可通过系统状态方程对状态变量Xk-1在
Figure BDA0002399897500000141
点处求偏导可得状态转移矩阵，通过系统状态方程对系统输入噪声uk-1在
Figure BDA0002399897500000142
点处求偏导可得噪声驱动矩阵，通过观测方程对状态变量Xk在
Figure BDA0002399897500000143
点处求偏导可得观测矩阵。
步骤A2、定位模块502根据所述状态转移矩阵、所述移动机器人的速度测量值、以及航向角测量值计算移动机器人位姿的一步预测值以及一步预测方差矩阵。
步骤A3、定位模块502判断所述距离测量值是否有效。
示例性地，该步骤可具体包括：定位模块502判断距离测量值的误差是否小于预设阈值；在所述距离测量值的误差小于预设阈值的情况下，定位模块502确定所述距离测量值有效；在所述距离测量值的误差大于或等于预设阈值的情况下，定位模块502确定所述距离测量值无效。需要指出的是，本发明并不局限于以上一种判断距离测量值是否有效的实施方式。在不影响本发明实施的情况下，本发明还可采取其他判断距离测量值是否有效的实施方式。
在所述距离测量值有效的情况下，定位模块502可进一步执行步骤A4至A6；在所述距离测量值无效的情况下，定位模块502可进一步执行步骤A7。
步骤A4、定位模块502根据所述一步预测方差矩阵、所述观测矩阵、以及距离测量噪声方差矩阵计算滤波增益矩阵。
步骤A5、定位模块502根据所述观测矩阵计算所述移动机器人至预先设置的UWB基站的距离的一步预测值。
步骤A6、定位模块502根据所述移动机器人位姿的一步预测值、所述距离的一步预测值、所述滤波增益矩阵、以及所述距离测量值，计算移动机器人的位姿估计值。
步骤A7、定位模块502将所述移动机器人位姿的一步预测值作为所述移动机器人的位姿估计值。
进一步，在定位模块通过步骤A1至A7所示处理流程计算出移动机器人的位姿估计值之后，可通过数据链路将移动机器人的位姿信息输出至控制模块，进而使得控制模块能够根据所述位姿信息与控制任务控制移动机器人的移动。
在本发明实施例中，通过以上装置实现了一种可用于室内的定位方案，使得只需在对导航定位性能有特殊要求的场景布放1个UWB基站即可显著提高移动机器人导航定位功能的鲁棒性，降低移动机器人导航定位的成本。
图6示出了可以应用本发明实施例的定位方法或定位装置的示例性系统架构600。
如图6所示，系统架构600可以包括AGV 601、602、603，网络604和服务器605。网络X04用以在AGV 601、602、603和服务器605之间提供通信链路的介质。网络604可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
AGV 601、602、603可通过网络604与服务器605交互，以接收或发送消息等。AGV601、602、603上可以安装有多种传感器，比如激光器、摄像头、里程计、惯性器件等用于导航定位的传感器中的一种或多种。此外，AGV601、602、603上还安装有UWB标签，以用于接收UWB基站无线电信号，进而获取移动机器人自身到UWB基站的距离信息。
服务器605可以是提供各种服务的服务器，例如对AGV 601、602、603进行控制、管理的管理服务器。管理服务器可以对接收到的传感器测量数据等进行分析等处理，并将处理结果(例如移动机器人的位姿估计值)反馈给AGV。
需要说明的是，本发明实施例所提供的定位方法一般由服务器605执行，相应地，定位装置一般设置于服务器605中。
应该理解，图6中的AGV、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。
需要指出的是，本发明中的定位方法或定位装置也可应用于其他系统架构中。例如，本发明的定位装置也可设置在AGV或者其他移动机器人上。
下面参考图7，其示出了适于用来实现本发明实施例的电子设备的计算机系统700的结构示意图。图7示出的计算机系统仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
如图7所示，计算机系统700包括中央处理单元(CPU)701，其可以根据存储在只读存储器(ROM)702中的程序或者从存储部分708加载到随机访问存储器(RAM)703中的程序而执行各种适当的动作和处理。在RAM 703中，还存储有系统700操作所需的各种程序和数据。CPU 701、ROM 702以及RAM 703通过总线704彼此相连。输入/输出(I/O)接口705也连接至总线704。
以下部件连接至I/O接口705：包括键盘、鼠标等的输入部分706；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分707；包括硬盘等的存储部分708；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分709。通信部分709经由诸如因特网的网络执行通信处理。驱动器710也根据需要连接至I/O接口705。可拆卸介质711，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器710上，以便于从其上读出的计算机程序根据需要被安装入存储部分708。
特别地，根据本发明公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本发明公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分709从网络上被下载和安装，和/或从可拆卸介质711被安装。在该计算机程序被中央处理单元(CPU)701执行时，执行本发明的系统中限定的上述功能。
需要说明的是，本发明所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本发明实施例中所涉及到的模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块也可以设置在处理器中，例如，可以描述为：一种处理器包括获取模块、定位模块。其中，这些模块的名称在某种情况下并不构成对该模块本身的限定，例如，定位模块还可以被描述为“对移动机器人进行定位的模块”。
作为另一方面，本发明还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的设备中所包含的；也可以是单独存在，而未装配入该设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该设备执行时，使得该设备执行以下流程：获取移动机器人的传感器测量数据；其中，所述传感器测量数据包括：移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号基站的距离测量值；根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值；其中，所述移动机器人的位姿估计值包括：移动机器人的位置估计值、以及移动机器人的航向角估计值。
根据本发明实施例提供的技术方案，使得只需在对导航定位性能有特殊要求的场景布放1个信号基站即可显著提高移动机器人导航定位功能的鲁棒性，降低移动机器人导航定位的成本。
上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，取决于设计要求和其他因素，可以发生各种各样的修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。

Claims (10)

1.一种定位方法，其特征在于，所述方法包括：
获取移动机器人的传感器测量数据；其中，所述传感器测量数据包括：移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号基站的距离测量值；
根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值；其中，所述移动机器人的位姿估计值包括：移动机器人的位置估计值、以及移动机器人的航向角估计值。
2.根据权利要求1所述的方法，其特征在于，所述多源信息融合定位模型满足：
所述多源信息融合定位模型中的系统状态方程是以移动机器人的位置、航向角为状态量构建的；所述多源信息融合定位模型中的观测方程是以移动机器人至预先设置的信号基站的距离为观测量构建的。
3.根据权利要求2所述的方法，其特征在于，所述多源信息融合定位模型为扩展卡尔曼滤波模型；所述根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值包括：
对系统状态方程和观测方程进行线性化处理，并计算状态转移矩阵和观测矩阵；根据所述状态转移矩阵、所述移动机器人的速度测量值、以及航向角测量值计算移动机器人位姿的一步预测值以及一步预测方差矩阵；在所述距离测量值有效的情况下，根据所述一步预测方差矩阵、所述观测矩阵、以及距离测量噪声方差矩阵计算滤波增益矩阵；根据所述观测矩阵计算所述移动机器人至预先设置的信号基站的距离的一步预测值；根据所述移动机器人位姿的一步预测值、所述距离的一步预测值、所述滤波增益矩阵、以及所述距离测量值，计算移动机器人的位姿估计值。
4.根据权利要求3所述的方法，其特征在于，所述方法还包括：
在所述距离测量值无效的情况下，将所述移动机器人位姿的一步预测值作为所述移动机器人的位姿估计值。
5.根据权利要求1所述的方法，其特征在于，所述获取移动机器人的传感器测量数据包括：
通过测速里程计获得移动机器人的速度测量值，通过惯导传感器获得移动机器人的航向角测量值，通过移动机器人携带的电子标签获得移动机器人至预先设置的信号基站的距离测量值。
6.根据权利要求5所述的方法，其特征在于，所述信号基站为UWB超宽带基站；所述通过移动机器人携带的电子标签获得移动机器人至预先设置的信号基站的距离测量值包括：通过移动机器人携带的UWB标签接收UWB基站的信号，根据无线电测距原理确定移动机器人至UWB基站的距离测量值。
7.一种定位装置，其特征在于，所述装置包括：
获取模块，用于获取移动机器人的传感器测量数据；其中，所述传感器测量数据包括：移动机器人的速度测量值、航向角测量值、以及移动机器人至预先设置的信号基站的距离测量值；
定位模块，用于根据多源信息融合定位模型对所述传感器测量数据进行融合处理，以得到移动机器人的位姿估计值；其中，所述移动机器人的位姿估计值包括：移动机器人的位置估计值、以及移动机器人的航向角估计值。
8.根据权利要求7所述的装置，其特征在于，所述定位模块配置的多源信息融合定位模型满足：
所述多源信息融合定位模型中的系统状态方程是以移动机器人的位置、航向角为状态量构建的；所述多源信息融合定位模型中的观测方程是以移动机器人至预先设置的信号基站的距离为观测量构建的。
9.一种电子设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1至6中任一所述的方法。
10.一种计算机可读介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现如权利要求1至6中任一所述的方法。
CN202010143459.2A 2020-03-04 2020-03-04 定位方法和装置 Pending CN113438615A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010143459.2A CN113438615A (zh) 2020-03-04 2020-03-04 定位方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010143459.2A CN113438615A (zh) 2020-03-04 2020-03-04 定位方法和装置

Publications (1)

Publication Number Publication Date
CN113438615A true CN113438615A (zh) 2021-09-24

Family

ID=77752359

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010143459.2A Pending CN113438615A (zh) 2020-03-04 2020-03-04 定位方法和装置

Country Status (1)

Country Link
CN (1) CN113438615A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113916231A (zh) * 2021-09-28 2022-01-11 上海浦东发展银行股份有限公司 移动设备定位导航方法、装置、移动设备及存储介质
CN114623823A (zh) * 2022-05-16 2022-06-14 青岛慧拓智能机器有限公司 一种融合里程计的uwb多模态定位系统、方法及装置

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105823480A (zh) * 2016-03-18 2016-08-03 中国海洋大学 基于单信标的水下移动目标定位算法
CN107289933A (zh) * 2017-06-28 2017-10-24 东南大学 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
CN107289941A (zh) * 2017-06-14 2017-10-24 湖南格纳微信息科技有限公司 一种基于惯导的室内定位方法与装置
CN109855621A (zh) * 2018-12-27 2019-06-07 国网江苏省电力有限公司检修分公司 一种基于uwb与sins的组合室内行人导航系统及方法
CN110207695A (zh) * 2019-05-28 2019-09-06 哈尔滨工程大学 一种适用于深海auv的无速度辅助单信标定位方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110702091A (zh) * 2019-07-24 2020-01-17 武汉大学 一种沿地铁轨道移动机器人的高精度定位方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105823480A (zh) * 2016-03-18 2016-08-03 中国海洋大学 基于单信标的水下移动目标定位算法
CN107289941A (zh) * 2017-06-14 2017-10-24 湖南格纳微信息科技有限公司 一种基于惯导的室内定位方法与装置
CN107289933A (zh) * 2017-06-28 2017-10-24 东南大学 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
CN109855621A (zh) * 2018-12-27 2019-06-07 国网江苏省电力有限公司检修分公司 一种基于uwb与sins的组合室内行人导航系统及方法
CN110207695A (zh) * 2019-05-28 2019-09-06 哈尔滨工程大学 一种适用于深海auv的无速度辅助单信标定位方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110702091A (zh) * 2019-07-24 2020-01-17 武汉大学 一种沿地铁轨道移动机器人的高精度定位方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113916231A (zh) * 2021-09-28 2022-01-11 上海浦东发展银行股份有限公司 移动设备定位导航方法、装置、移动设备及存储介质
CN114623823A (zh) * 2022-05-16 2022-06-14 青岛慧拓智能机器有限公司 一种融合里程计的uwb多模态定位系统、方法及装置
CN114623823B (zh) * 2022-05-16 2022-09-13 青岛慧拓智能机器有限公司 一种融合里程计的uwb多模态定位系统、方法及装置

Similar Documents

Publication Publication Date Title
JP6952165B2 (ja) 障害物検出方法および装置
CN111442722B (zh) 定位方法、装置、存储介质及电子设备
US10289122B2 (en) Communication link accessibility aware navigation
KR20150058679A (ko) 단지내 도로에서 자율주행차량의 위치 및 해딩 정보 제공 장치 및 방법
CN113108791A (zh) 一种导航定位方法及导航定位设备
US20200159238A1 (en) Detecting a location of an autonomous device
CN113438615A (zh) 定位方法和装置
CN110988949A (zh) 定位方法、定位装置、计算机可读存储介质与可移动设备
JP2015219089A (ja) 無線測位装置、無線測位用地図データベース
CN110554688A (zh) 用于生成拓扑地图的方法和装置
CN111836185A (zh) 一种基站位置坐标的确定方法、装置、设备及存储介质
CN112925302A (zh) 机器人位姿控制方法和装置
CN111198567B (zh) 一种多agv协同动态追踪方法及装置
CN114554392B (zh) 一种基于uwb与imu融合的多机器人协同定位方法
CN113759349B (zh) 激光雷达与定位设备的标定方法、设备及自动驾驶车辆
CN109029418A (zh) 一种在封闭区域内对车辆进行定位的方法
CN111380556B (zh) 用于自动驾驶车辆的信息处理方法和装置
CN115900697B (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN111580530A (zh) 一种定位方法、装置、自主移动设备及介质
CN112699135B (zh) 信息更新方法和装置
CN113761091A (zh) 闭环检测方法、装置、电子设备、系统和存储介质
KR20140025079A (ko) 로봇, 로봇의 위치 추정 방법, 및 그 방법을 실행시키기 위한 프로그램이 기록된 기록 매체
US11262432B2 (en) Positioning apparatus and positioning method
CN116908860A (zh) 机器人定位更新方法、装置、电子设备及可读存储介质
CN115993137B (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