CN111141290A - 机器人的定位方法、定位装置、设备和存储介质 - Google Patents

机器人的定位方法、定位装置、设备和存储介质 Download PDF

Info

Publication number
CN111141290A
CN111141290A CN202010018659.5A CN202010018659A CN111141290A CN 111141290 A CN111141290 A CN 111141290A CN 202010018659 A CN202010018659 A CN 202010018659A CN 111141290 A CN111141290 A CN 111141290A
Authority
CN
China
Prior art keywords
data
positioning data
measurement
laser
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
CN202010018659.5A
Other languages
English (en)
Other versions
CN111141290B (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.)
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Original Assignee
Guangzhou Shiyuan Electronics Thecnology 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 Guangzhou Shiyuan Electronics Thecnology Co Ltd filed Critical Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority to CN202010018659.5A priority Critical patent/CN111141290B/zh
Publication of CN111141290A publication Critical patent/CN111141290A/zh
Application granted granted Critical
Publication of CN111141290B publication Critical patent/CN111141290B/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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration

Abstract

本发明实施例公开了一种机器人的定位方法、机器人的定位装置、设备和存储介质，定位方法包括：接收机器人的多个传感器所采集的当前测量数据生成机器人的初始定位数据；将初始定位数据增加至预先建立的滑动窗口，从滑动窗口中确定所述当前测量数据关联的第一定位数据；根据滑动窗口中的历史定位数据、历史定位数据关联的历史测量数据、当前测量数据和第一定位数据对初始定位数据进行优化以得到目标定位数据。在求解当前初始定位数据的最优解时采用了多个历史定位数据、历史测量数据和当前测量数据，解决了根据上一预测的定位数据和当前测量数据来估计定位数据造成关联性差且非线性误差大的问题，使得优化得到的定位数据精度和鲁棒性更高。

Description

机器人的定位方法、定位装置、设备和存储介质
技术领域
本发明实施例涉及机器人定位技术领域，尤其涉及一种机器人的定位方法、机器人的定位装置、设备和存储介质。
背景技术
随着自动控制技术的发展，移动机器人也越来越多的被应用到了生产、军事和服务等领域以取代人力执行相关任务。
在机器人自主作业的过程中，机器人的自身定位是实现机器人自主作业的基础。目前，移动机器人定位是利用传感器获得的测量数据估计出机器人自身位置和姿态，由于单一传感器的测量局限性和传感器本身的不可靠性，单一传感器定位的方法并不能满足机器人高精度和鲁棒性的定位要求，因此在机器人实际定位中，多采用多传感器融合定位，然而，目前多传感器融合定位一般是基于贝叶斯滤波方法，如卡尔曼滤波和粒子滤波，以卡尔曼滤波作为例，主要是利用一次线性化和一阶马尔科夫假设，仅根据上一预测的定位数据和当前传感器获得的测量数据来估计当前的定位数据，容易造成测量数据关联性差且非线性误差大，当测量数据错误时还容易导致生成错误的定位数据。
发明内容
本发明实施例提供了一种机器人的定位方法、机器人的定位装置、设备和存储介质，以解决现有采用多传感器融合定位中存在测量数据关联性差和非线性误差大的问题。
第一方面，本发明实施例提供了一种机器人的定位方法，包括：
接收机器人的多个传感器所采集的当前测量数据，生成所述机器人的初始定位数据；
将所述初始定位数据增加至预先建立的滑动窗口，所述滑动窗口包括多个历史定位数据以及所述历史定位数据关联的历史测量数据；
从所述滑动窗口中确定所述当前测量数据关联的第一定位数据；
根据所述滑动窗口中的历史定位数据、所述历史测量数据、所述当前测量数据和所述第一定位数据对所述初始定位数据进行优化以得到目标定位数据。
第二方面，本发明实施例提供了一种机器人的定位装置，包括：
初始定位数据生成模块，用于接收机器人的多个传感器所采集的当前测量数据，生成所述机器人的初始定位数据；
滑动窗口数据增加模块，用于将所述初始定位数据增加至预先建立的滑动窗口，所述滑动窗口包括多个历史定位数据以及所述历史定位数据关联的历史测量数据；
测量数据关联模块，用于从所述滑动窗口中确定所述当前测量数据关联的第一定位数据；
初始定位数据优化模块，用于根据所述滑动窗口中的历史定位数据、所述历史测量数据、所述当前测量数据和所述第一定位数据对所述初始定位数据进行优化以得到目标定位数据。
第三方面，本发明实施例提供了一种设备，所述设备包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明任意实施例所述的机器人的定位方法。
第四方面，本发明实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现本发明任意实施例所述的机器人的定位方法。
在本发明实施例中，滑动窗口中包括多个历史定位数据以及历史定位数据关联的历史测量数据，根据当前测量数据生成初始定位数据并添加到滑动窗口后，将当前测量数据与滑动窗口中的第一定位数据相关联，能够在求解当前初始定位数据的最优解时采用滑动窗口中的多个历史定位数据、历史定位数据关联的历史测量数据、当前测量数据以及第一定位数据进行优化求解，解决了多传感器融合定位时，仅根据上一预测的定位数据和当前传感器获得的测量数据来估计当前的定位数据造成测量数据关联性差且非线性误差大的问题，同时也解决了当前测量数据错误导致定位数据错误的问题，使得优化得到的目标定位数据精度和鲁棒性更高，定位结果也更加平滑。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明实施例一提供的一种机器人的定位方法的流程图；
图2为本发明实施例中滑动窗口的示意图；
图3为本发明实施例二提供的一种机器人的定位方法的流程图；
图4为本发明实施例三提供的一种机器人的定位装置的结构示意图；
图5为本发明实施例四提供的一种设备的结构示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
实施例一
图1为本发明实施例一提供的一种机器人的定位方法的流程图，本发明实施例可适用于对机器人进行定位的情况中。本实施例提供的机器人的定位方法可以由本发明实施例提供的机器人的定位装置来执行，该装置可以通过软件和/或硬件的方式来实现，并集成在一设备中。具体的，参考图1，本发明实施例的机器人的定位方法可以包括如下步骤：
S101、接收机器人的多个传感器所采集的当前测量数据，生成所述机器人的初始定位数据。
本发明实施例中，机器人可以设置有多个传感器，例如设置有轮速里程计、激光传感器或者其他传感器等，每个传感器可按照预设的测量周期采集测量数据，不同传感器采集测量数据的周期可以相同也可以不相同，当前测量数据可以是传感器按照测量周期采集测量数据时，传感器在当前周期采集到的测量数据。
在多个传感器采集的测量数据中，可以指定其中一个传感器采集到的测量数据来计算机器人在当前时间的初始定位数据，例如，机器人设置有轮速里程计和激光传感器，则可以在轮速里程计采集到里程数据时采用里程数据计算机器人在当前时间的初始定位数据。
S102、将所述初始定位数据增加至预先建立的滑动窗口，所述滑动窗口包括多个历史定位数据以及所述历史定位数据关联的历史测量数据。
在实际应用中，机器人的定位数据是一个随运动变化的量，将一段时间内或者一定数量的连续多个定位数据设定为一个集合即得到一个窗口，并随着时间的推移不断更新该集合即形成滑动窗口。
具体到本发明实施中，当生成一个初始定位数据时，可以将该初始定位数据增加到预先建立的滑动窗口中，如图2所示，滑动窗口的首端和尾部可以分别是滑动窗口中第一个定位数据和最后一个定位数据所在的位置，假设滑动窗口的长度为3，滑动窗口中包括(x0，x1，x2)共3个定位数据，则滑动窗口的首端可以为定位数据x0所在的位置，滑动窗口的尾部可以为定位数据x3所在的位置。可选地，可以将初始定位数据增加到滑动窗口的尾部，从而使得该滑动窗口包括在窗口尾部的、当前时间的初始定位数据，以及位于该初始定位数据之前的多个历史定位数据，历史定位数据为已经优化后作为机器人在当前时间之前的定位数据，其中，历史定位数据关联有当前时间之前多个传感器测量得到的历史测量数据。
S103、从所述滑动窗口中确定所述当前测量数据关联的第一定位数据。
在本发明实施中，当前测量数据为多个传感器测量得到的数据，对于每个传感器的测量数据，可以建立该测量数据与滑动窗口中的定位数据的关联，例如，对于某个传感器当前输出的测量数据，可以建立该测量数据与历史定位数据和初始定位数据的关联，该测量数据关联的定位数据即为第一定位数据。
如图2所示，在本发明实施例中，传感器可以包括轮速里程和激光传感器，滑动窗口中包括多个定位数据xi，其中xi＝(x，y，θ)∈SE(2)是机器人定位数据在维空间中的表示，(x,y)表示位置，θ表示朝向。图2中每个圆圈表示定位数据xi，圆圈之间的连线表示两个定位数据xi所关联的测量数据，也称之为边；黑色实心圆为因子，表示测量数据与定位数据的误差函数。每生成一个初始定位数据xi，可以添加到图2所示的滑动窗口的尾部，并且建立当前传感器输出的测量数据与滑动窗口中历史定位数据的关联，即建立图2所示的连接两个定位数据的边。
S104、根据所述滑动窗口中的历史定位数据、所示历史测量数据、所述当前测量数据和所述第一定位数据对所述初始定位数据进行优化以得到目标定位数据。
具体地，对于滑动窗口中的每个测量数据，确定该测量数据所关联的定位数据后，采用该测量数据所关联的定位数据以及该测量数据计算误差因子，误差因子表达了测量与预测之间的误差，即在每次优化的过程中，预测是指每次优化时调整滑动窗口中各个定位数据的值，误差即表达了预测后的定位数据与其关联的测量数据之间的差距，示例性地，如果一个测量数据与滑动窗口中的一个定位数据相关联，在每次优化后，该定位数据具有一个预测值，则该预测值与测量数据的距离即为误差。对位于滑动窗口的尾部的初始定位数据，可以采用滑动窗口中所有测量数据计算出的误差因子累加以作为最小误差函数，并对该最小误差函数求解以获得初始定位数据的最优解即为目标定位数据。
在本发明实施例中，滑动窗口包括初始定位数据以及初始定位数据之前的多个历史定位数据，每个历史定位数据关联有历史测量数据，在确定当前测量数据关联的第一定位数据后，在求解当前初始定位数据的最优解时采用了滑动窗口中的多个历史定位数据、历史定位数据关联的历史测量数据和当前测量数据，解决了多传感器融合定位时，仅根据上一预测的定位数据和当前传感器获得的测量数据来估计当前的定位数据造成测量数据关联性差且非线性误差大的问题，同时也解决了当前测量数据错误导致定位数据错误的问题，使得优化得到的目标定位数据精度和鲁棒性更高，定位结果也更加平滑。
实施例二
图3为本发明实施例二提供的一种机器人的定位方法的流程图，本发明实施例在实施例一的基础上进行优化，具体的，参考图3，本发明实施例的机器人的定位方法可以包括如下步骤：
S201、确定机器人的起始定位数据。
在本发明实施例中，机器人的起始定位数据可以是机器人开始定位时所处的位置和姿态数据，在本发明的可选实施例中，机器人可以配置有激光传感器，在机器人开始定位之前或者重定位时，可以获取激光传感器所采集的激光测量数据，然后采用激光测量数据和AMCL(自适应蒙特卡罗定位，Adaptive Monte Carlo Localization)算法确定机器人的起始定位数据。
AMCL算法是机器人在二维环境下的概率定位算法，AMCL算法根据激光传感器获得的激光测量数据配合订阅到的电子地图数据，使用粒子滤波获取机器人的最佳定位点。
具体地，可以先给定机器人一个相对于电子地图的初始位置，在该初始位置随机生成大量代表机器人位姿(位置坐标、姿态方向)的粒子群，并且根据该初始位置得到一个初始定位数据，该初始定位数据表示对机器人在起始位置的位置和姿态的粗略估计，用于加快起始位置定位的收敛速度。
机器人上的激光传感器测量机器人实时移动距离和实时旋转角度后，更新粒子群中每一个粒子对应的位姿，即对于粒子群中的每一个粒子，将激光测量数据以每一个粒子对应的位姿映射到电子地图中得到激光点云，计算激光点云与电子地图中的各个物体的重合数量作为每一个粒子的得分，以每一个粒子的得分为权重计算粒子群的加权位姿均值作为AMCL算法估计位姿，并将AMCL算法估计位姿作为初始值，使用迭代法的扫描匹配算法得到扫描匹配位姿作为机器人运行的当前时刻的最优位姿，并将扫描匹配位姿作为高得分粒子插入到粒子群，使用AMCL算法重采样粒子，使粒子群更集中于最优位姿，在机器人运行的下一时刻继续通过激光传感器测量机器人的实时移动距离和实时旋转角度以更新重采样后每个粒子对应的位姿，直到粒子群收敛，最终得到机器人的最优位姿作为机器人的起始定位数据，可以执行S202，否则不断采用激光测量数据和AMCL算法获得起始定位数据。
当然，本领域技术人员还可以通过其他定位方法确定机器人开始定位或者重定位后的起始定位数据，本发明实施例对确定机器人的起始定位数据的方式不加以限制。
S202、将所述起始定位数据增加至预先建立的滑动窗口的首端。
如图2所示，起始定位数据为x0，则可以将起始定位数据x0增加至预先建立的滑动窗口的第一个位置。
S203、接收轮速里程计在当前时间采集到的第一里程数据。
在本发明实施例中，轮速里程计按照预设周期采集里程数据，当前时间可以为最近的一个周期的时间，轮速里程计在最近一个周期采集里程数据时，可以将该里程数据作为轮速里程计在当前时间采集到的第一里程数据。
S204、采用所述第一里程数据生成所述机器人在当前时间的初始定位数据，所述初始定位数据包括标识。
本发明实施例中，机器人设置有轮速里程计，则可以以轮速里程计测量得到的第一里程数据来计算生成机器人当前时间的初始定位数据，该初始定位数据具有标识，其中，标识用于区别机器人的多个定位数据以及用于查找机器人的定位数据，标识可以为数字或者字母，或者数字与字母的结合。
可选地，轮速里程计在当前时间输出第一里程数据时，可以获取轮速里程计在上一时间采集到的第二里程数据，采用第一里程数据和第二里程数据计算第一里程增量，在第一里程增量大于预设阈值时，获取当前时间之前的前一历史定位数据，并计算前一历史定位数据和第一里程增量的和值作为机器人在当前时间的初始定位数据。其中，前一历史定位数据可以为生成初始定位数据前，位于滑动窗口的尾部的定位数据，上一时间可以为用来生成前一历史定位数据的里程数据的采集时间。
具体地，轮速里程计按照预设周期输出测量得到的里程数据，当接收到轮速里程计在当前周期输出的第一里程数据时，可以计算第一里程数据与用来生成前一历史定位数据的第二里程数据的差值得到第一里程增量，如果第一里程增量大于预设阈值，则计算第一里程增量和已经输出的前一历史定位数据的和值作为机器人在当前时间的初始定位数据，其中，第一里程增量可以为位移增量δT和/或旋转角度增量△θ，即在位移增量δt＞tthreshold，或者δθ＞θthreshold时，生成当前时间的初始定位数据nodenew{id，xi}，id为初始定位数据的标识，其中，xi＝xi-1+△，xi-1是前一历史定位数据，△是第一里程增量。
S205、将所述标识和所述当前时间关联存储至预先建立的节点数据库中。
具体地，可以建立一个节点数据库，该节点数据库可以用来存储各个时间节点生成的定位数据、生成定位数据的时间和定位数据的标识，可选地，每生成一个定位数据nodenew{id，xi}后，可以建立一个数据结构来存储生成定位数据的时间和标识{time，id}，以便后续其他传感器生成测量数据时，可以根据生成测量数据的时间在节点数据库中查找相邻的时间，并确定该相邻的时间关联的标识，从而可以将生成的测量数据与该标识对应的定位数据关联。
S206、将所述初始定位数据增加至预先建立的滑动窗口。
具体地，可以将初始定位数据增加到滑动窗口的尾部，如图2所示，定位数据x0处于滑动窗口的首端，当新生成一个初始定位数据时，可以将初始定位数据依次添加在定位数据x0之后，如当前时间生成的初始定位数据为x2，则将初始定位数据x2添加至滑动窗口中并处于定位数据x1之后，使得初始定位数据x2处于滑动窗口的尾部。
S207、从所述滑动窗口中确定所述轮速里程计在所述当前时间输出的里程数据关联的定位数据。
可选地，可以在滑动窗口中确定初始定位数据的前一个历史定位数据，并确定生成前一个历史定位数据时轮速里程计采集到的第三里程数据，计算第一里程数据和第三里程数据的差值得到第二里程增量，确定初始定位数据和前一个历史定位数据为第二里程增量关联的定位数据。
具体地，当生成一个定位数据为xi时，获取前一个历史定位数据为xj，并获取到在生成历史定位数据为xj时轮速里程计采集的第三里程数据，采用生成定位数据为xi的第一里程数据和生成历史定位数据为xj的第三里程数据计算差值得到第二里程增量
Figure BDA0002359892110000101
并确认第二里程增量
Figure BDA0002359892110000102
关联的定位数据为历史定位数据为xj和定位数据为xi。
S208、基于所述节点数据库，从所述滑动窗口中确定所述激光ICP测量数据关联的定位数据。
在本发明实施例中，机器人还设置有激光传感器，激光ICP测量数据可以为激光传感器采集到的用于激光ICP配准的数据，在接到激光传感器当前采集的激光ICP测量数据后，可以通过激光ICP(Iterative Closest Point，迭代最近点)配准算法，将当前采集的激光ICP测量数据和上一回采集的激光ICP测量数据进行激光ICP配准以生成激光ICP测量增量，激光ICP测量增量表达了通过激光ICP定位时两个相邻的激光ICP测量数据的增量。
具体到本发明实施例中，可以获取当前接收到的激光ICP测量数据的第一时间，基于第一时间在节点数据库中确定当前接收到的激光ICP测量数据关联的第一标识，并确定第一标识对应的第二定位数据，在获取上一回接收到的激光ICP测量数据关联的第三定位数据后，对当前接收到的激光ICP测量数据和上一回接收到的激光ICP测量数据进行激光ICP配置，得到激光ICP测量增量，确定第二定位数据和第三定位数据为激光ICP测量增量关联的定位数据，其中，第一时间为激光传感器采集到当前激光ICP测量数据的时间。
本发明实施例中，轮速里程计输出里程数据的周期小于激光传感器输出激光测量数据的周期，当接收到一个激光ICP测量数据时，可以获取到该激光测量数据的第一时间，在节点数据库中查找与第一时间距离最近的时间，并将距离最近的时间关联的标识确定为第一标识，通过该第一标识可以确定其关联的第二定位数据nodenow{idm,xm}，同时获取上一回接收到的激光ICP测量数据关联的第三定位数据nodelast{idn,xn}，则可以通过相邻两帧激光ICP测量数据进行激光ICP配准得到激光ICP测量增量
Figure BDA0002359892110000111
该激光ICP测量增量
Figure BDA0002359892110000112
关联的定位数据即为第三定位数据xn和第二定位数据xm。
S209、基于所述节点数据库确定所述激光AMCL测量数据关联的定位数据。
在本发明实施例中，激光AMCL测量数据可以为采用激光传感器所采集的激光数据通过激光AMCL算法计算获得的数据。
具体地，可以获取接收到当前的激光AMCL测量数据Tk时的第二时间，在节点数据库中查找与第二时间距离最近的时间，将距离最近的时间关联的标识确定为第二标识，并确定第二标识对应的第四定位数据nodenow{idk，xk}，确定第四定位数据xk为当前激光AMCL测量数据Tk关联的定位数据，其中，第二时间为激光传感器采集到用于激光AMCL算法的激光数据的时间。
S210、采用所述里程数据计算里程增量，以及采用所述激光ICP测量数据计算激光ICP测量增量；其中，所述里程增量与所述滑动窗口中的两个定位数据相关联，所述激光ICP测量增量与所述滑动窗口中的两个定位数据相关联，所述激光AMCL测量数据与滑动窗口中的一个定位数据相关联。
在S207中可以通过里程数据计算里程增量并将该里程增量与滑动窗口中的两个定位数据相关联，在S208中将激光ICP测量数据进行激光ICP配准算法生成激光ICP测量增量并将该激光ICP测量增量与滑动窗口中的两个定位数据相关联，在S209通过激光AMCL算法得到激光AMCL测量数据并将激光AMCL测量数据与滑动窗口中的一个定位数据相关联。其中，滑动窗口中的定位数据包括所述历史定位数据和初始定位数据。
S211、采用所述里程增量以及所述里程增量关联的两个定位数据计算里程测量误差。
对于滑动窗口中的每个里程增量，可以采用里程增量以及里程增量关联的两个定位数据计算里程测量误差，具体地，针对滑动窗口中的每个里程增量
Figure BDA0002359892110000121
计算里程增量
Figure BDA0002359892110000122
关联的两个定位数据xj和xi的差值得到第一预测增量h(xi,xj)，计算第一预测增量h(xi,xj)和里程增量
Figure BDA0002359892110000123
的差值得到第一差值
Figure BDA0002359892110000124
然后采用里程增量
Figure BDA0002359892110000125
和预设的轮速误差系数计算里程测量不确定度
Figure BDA0002359892110000126
最后采用第一差值和里程测量不确定度计算里程测量误差如下：
Figure BDA0002359892110000127
其中，里程测量不确定度
Figure BDA0002359892110000128
表达了测量得到里程数据的误差，在本发明实施例中，轮子的运动模型中假设行走的误差正比与于行走的距离，如下公式所示：
Figure BDA0002359892110000131
kr和kl表示的是左右轮的误差系数，△sr和△sl为左右轮的里程增量，即里程计增量的不确定度与左右轮的行走距离以及各自的误差系数有关，在每次测量时，可以根据上述公式和里程正能量计算里程测量不确定度。
S212、采用所述激光ICP测量增量以及所述激光ICP测量增量关联的两个定位数据计算激光ICP测量误差。
对于通过激光传感器输出的测量数据得到的激光ICP测量增量，可以采用激光ICP测量增量以及激光ICP测量增量关联的两个定位数据计算激光ICP测量误差，具体地，针对滑动窗口中的每个激光ICP测量增量
Figure BDA0002359892110000132
计算激光ICP测量增量
Figure BDA0002359892110000133
关联的两个定位数据xm和xn的差值得到第二预测增量h(xm,xn)，计算第二预测增量h(xm,xn)和激光ICP测量增量
Figure BDA0002359892110000134
的差值得到第二差值
Figure BDA0002359892110000135
并获取激光ICP测量的不确定度
Figure BDA0002359892110000136
采用第二差值
Figure BDA0002359892110000137
和激光ICP测量的不确定度
Figure BDA0002359892110000138
计算激光ICP测量误差如下：
Figure BDA0002359892110000139
其中，激光ICP测量的不确定度
Figure BDA00023598921100001310
表达了激光ICP测量增量的误差，不确定度
Figure BDA00023598921100001311
可以通过激光ICP配准协防差计算，具体如下：
假设在定位数据x0和x2之间进行一次激光ICP配准，在定位数据x0时刻的激光点集合为P＝{p1，...pn}，在定位数据x2时刻的激光点集合为P＇＝{p＇1，...p＇n}，激光ICP配准结果T，激光ICP配准的误差均值为：
Figure BDA0002359892110000141
可以设定激光ICP配准的协防差为cov3x3＝error×Identity，Identity可以为一个3x3的单位矩阵。
S213、采用所述激光AMCL测量数据以及所述激光AMCL测量数据关联的定位数据计算激光AMCL测量误差。
对于通过激光传感器输出的测量数据得到的激光AMCL测量增量，可以采用激光AMCL测量数据以及激光AMCL测量数据关联的定位数据计算激光AMCL测量误差，具体地，针对滑动窗口中的每个激光AMCL测量数据Tk，可以计算激光AMCL测量数据Tk关联的定位数据xk和激光AMCL测量数据Tk的差值得到第三差值
Figure BDA0002359892110000142
在获取激光AMCL测量数据的不确定度
Figure BDA0002359892110000143
采用第三差值
Figure BDA0002359892110000144
和AMCL测量数据的不确定度
Figure BDA0002359892110000145
计算激光AMCL测量误差如下：
Figure BDA0002359892110000146
其中，激光AMCL测量数据的不确定度
Figure BDA0002359892110000147
表达了计算得到的激光AMCL测量数据的误差，不确定度
Figure BDA0002359892110000148
可以通过激光AMCL的协防差计算，具体如下：
假设激光AMCL的采样粒子有n个，每一个粒子都有一个位姿x＝(x，y，θ)，对所有粒子的每一个维度求出一个方差，例如对于x，方差如下：
Figure BDA0002359892110000149
同理得出var(y)，var(θ)，则对于激光AMCL测量数据的不确定度
Figure BDA00023598921100001410
可以表示为cov3x3协方差，cov3x3的协方差的对角元素分别是var(x)，var(y)，var(θ)，其他元素为0。
S214、将所述里程测量误差、所述激光ICP测量误差和所述激光AMCL测量误差累加作为最小误差函数。
可选地，最小误差函数如下：
Figure BDA0002359892110000151
上述公式中的每个子项为机器人的传感器的每种测量数据的误差，h()表示两个定位数据直接计算增量的函数，△T表示传感器测量的矩阵表现形式。当机器人增加传感器时可以增加相应的子项，使得定位方式更灵活。
S215、对所述最小误差函数求解得到所述初始定位数据的最优解以作为目标定位数据。
具体地，在对最小误差函数求解前，获取滑动窗口中位于首端的定位数据的值，基于滑动窗口中位于首端的定位数据的值对最小误差函数迭代求解得到初始定位数据的最优解以作为目标定位数据。
由S214中建立的最小误差函数E(χ)可知，每个传感器的测量△T已知，设置位于滑动窗口的首端的定位数据的值保持不变，其他定位数据初始时具有一个初始值，通过数值优化算法不断迭代调整滑动窗口中的各个定位数据xi的值，最终使得最小误差函数E(χ)最小时得到各个定位数据xi的最优解，则处于滑动窗口尾部的定位数据即为当前时间机器人所需的定位数据。
如图2所示，滑动窗口的长度为3，假设滑动窗口中有三个定位数据x0、x1和x2，目前需要对定位数据x2进行优化以获得目标定位数据，由于定位数据x0和x1已经经过优化，则可以通过定位数据x1和里程增量生成定位数据x2作为初始定位数据，在滑动窗口中的定位数据x0、x1和x2已经确定初始值，同时滑动窗口中还包括定位数据x0到定位数据x1、定位数据x1到定位数据x2的两个里程增量，定位数据x0到定位数据x2的激光ICP测量增量，以及在定位数据关联的激光AMCL测量数据，根据最小误差函数公式可知，最小误差函数为上述四个测量的误差之和，通过数值优化方法对最小误差函数迭代求解，得到最优的定位数据x0、x1和x2的目标值使得最小误差函数最小，输出定位数据x2的目标值即为机器人的当前定位数据。
S216、当所滑动窗口中的定位数据的数量大于预设值时，删除位于所述滑动窗口的首端的定位数据。
例如，假设设定滑动窗口的长度为3，当生成定位数据x3时，滑动窗口中已经有了x0到x2共3个定位数据，则删除最先的、位于滑动窗口的首端的定位数据x3。
S217、将所述滑动窗口中的定位数据向首端平移。
具体地，在删除最先的、位于滑动窗口的首端的定位数据后，将新生成的定位数据加入滑动窗口中以对新生成的定位数据进行优化以得到新生成的定位数据的最优解作为当前时刻机器人的定位数据。
本发明实施例采用轮速里程计在当前时间采集到的第一里程数据生成机器人在当前时间的初始定位数据，将标识和当前时间关联存储至预先建立的节点数据库中以及将初始定位数据增加至滑动窗口的尾端，并基于节点数据库确定里程数据、激光ICP测量数据以及激光AMCL测量数据关联的定位数据，分别采用里程增量、激光ICP测量增量和激光AMCL测量数据以及里程增量、激光ICP测量增量和激光AMCL测量数据关联的定位数据计算误差以建立最小误差函数对各个定位数据进行优化得到最优解。实现了每个测量数据可以关联多个定位数据，并且在求解当前初始定位数据的最优解时采用了滑动窗口中的多个历史定位数据、历史定位数据关联的历史测量数据、当前测量数据和当前测量数据关联的第一定位数据进行最优解求解，解决了多传感器融合定位时，仅根据上一预测的定位数据和当前传感器获得的测量数据来估计当前的定位数据造成测量数据关联性差且非线性误差大的问题，同时也解决了当前测量数据错误导致定位数据错误的问题，使得优化得到的目标定位数据精度和鲁棒性更高，定位结果也更加平滑。
进一步地，采用各个测量数据以及测量数据关联的定位数据计算误差以建立最小误差函数对各个定位数据进行优化得到最优解，当增加测量数据时仅需增加相应的误差项到最小误差函数中，使得定位方式更灵活。
更进一步地，计算每个测量数据的误差时考虑了测量的不确定度，使得测量数据更为准确，提高了机器人的定位精度。
实施例三
图4为本发明实施例三提供的一种机器人的定位装置的结构示意图，具体的，如图4所示，本发明实施的机器人的定位装置可以包括：
初始定位数据生成模块401，用于接收机器人的多个传感器所采集的当前测量数据，生成所述机器人的初始定位数据；
滑动窗口数据增加模块402，用于将所述初始定位数据增加至预先建立的滑动窗口，所述滑动窗口包括多个历史定位数据以及所示历史定位数据关联的历史测量数据；
测量数据关联模块403，用于从所述滑动窗口中确定所述当前测量数据关联的第一定位数据；
初始定位数据优化模块404，用于根据所述滑动窗口中的历史定位数据、所述历史测量数据、所述当前测量数据和所述第一定位数据对所述初始定位数据进行优化以得到目标定位数据。
可选地，还包括：
起始定位数据确定模块，用于确定机器人的起始定位数据；
起始定位数据增加模块，用于将所述起始定位数据增加至预先建立的滑动窗口的首端。
可选地，所述传感器包括激光传感器，所述起始定位数据确定模块包括：
激光测量数据获取子模块，用于获取所述激光传感器所采集的激光测量数据；
起始定位数据确定子模块，用于采用所述激光测量数据和AMCL算法确定所述机器人的起始定位数据。
可选地，所述传感器包括轮速里程计，所述初始定位数据生成模块401包括：
第一里程数据接收子模块，用于接收所述轮速里程计在当前时间采集到的第一里程数据；
初始定位数据生成子模块，用于采用所述第一里程数据生成所述机器人在当前时间的初始定位数据，所述初始定位数据包括标识；
标识和时间存储子模块，用于将所述标识和所述当前时间关联存储至预先建立的节点数据库中。
可选地，所述初始定位数据生成子模块包括：
第二里程数据获取单元，用于获取所述轮速里程计在上一时间采集到的第二里程数据；
第一里程增量计算单元，用于采用所述第一里程数据和所述第二里程数据计算第一里程增量；
前一历史定位数据获取单元，用于在所述第一里程增量大于预设阈值时，获取所述当前时间之前的前一历史定位数据；
初始定位数据生成单元，用于计算所述前一历史定位数据和所述第一里程增量的和值作为所述机器人在当前时间的初始定位数据。
可选地，所述传感器还包括激光传感器，所述测量数据还包括激光ICP测量数据和激光AMCL测量数据，所述测量数据关联模块403包括：
里程测量关联定位数据确定子模块，用于从所述滑动窗口中确定在所述当前时间所述轮速里程计输出的里程数据关联的定位数据；
激光ICP测量关联定位数据确定子模块，用于基于所述节点数据库，从所述滑动窗口中确定所述激光ICP测量数据关联的定位数据；以及，
激光AMCL测量关联定位数据确定子模块，用于从所述滑动窗口中确定所述激光AMCL测量数据关联的定位数据。
可选地，所述里程测量关联定位数据确定子模块包括：
前一历史定位数据查找单元，用于在所述滑动窗口中查找所述初始定位数据的前一个历史定位数据；
第三里程数据获取单元，用于获取生成所述前一个历史定位数据时所述轮速里程计采集到的第三里程数据；
第二里程增量计算单元，用于计算所述第一里程数据和所述第三里程数据的差值，得到第二里程增量；
里程计关联定位数据确定单元，用于确定所述初始定位数据和所述前一个历史定位数据为所述第二里程增量关联的定位数据。
可选地，所述激光ICP测量关联定位数据确定子模块包括：
第一时间获取单元，用于获取接收到当前激光ICP测量数据的第一时间；
第一标识确定单元，用于基于所述第一时间在所述节点数据库中确定所述当前激光ICP测量数据关联的第一标识；
第一定位数据确定单元，用于确定所述第一标识对应的第二定位数据；
第二定时数据获取单元，用于获取上一回输出的激光ICP测量数据关联的第三定位数据；
激光ICP测量增量计算单元，用于计算所述当前激光ICP测量数据和所述上一回接收到的激光ICP测量数据的差值，得到激光ICP测量增量；
激光ICP测量关联定位数据确定单元，用于确定所述第二定位数据和所述第三定位数据为所述激光ICP测量增量关联的定位数据。
可选地，所述第一标识确定单元包括：
第一时间查找子单元，用于在所述数据节点数据库中查找与所述第一时间距离最近的时间；
第一标识确定子单元，用于将所述距离最近的时间关联的标识确定为第一标识。
可选地，所述激光AMCL测量关联定位数据确定子模块包括：
第二时间获取单元，用于获取接收到当前激光AMCL测量数据时的第二时间；
第二标识确定单元，用于基于所述第二时间在所述节点数据库中确定当前激光AMCL测量数据关联的第二标识；
第三定位数据确定单元，用于确定所述第二标识对应的第四定位数据；
激光AMCL测量关联定位数据确定单元，用于确定所述第四定位数据为当前输出的激光AMCL测量数据关联的定位数据。
可选地，所述第二标识确定单元，包括：
第二时间查找子单元，用于在所述节点数据库中查找与所述第二时间距离最近的时间；
第二标识确定子单元，用于将所述距离最近的时间关联的标识确定为第二标识。
可选地，所述测量数据包括里程数据、激光ICP测量数据和激光AMCL测量数据，所述初始定位数据优化模块404包括：
增量计算子模块，用于采用所述里程数据计算里程增量，以及采用所述激光ICP测量数据计算激光ICP测量增量，其中，所述里程增量与所述滑动窗口中的两个定位数据相关联，所述激光ICP测量增量与所述滑动窗口中的两个定位数据相关联，所述激光AMCL测量数据与滑动窗口中的一个定位数据相关联；
里程测量误差计算子模块，用于采用所述里程增量以及所述里程增量关联的两个定位数据计算里程测量误差；
激光ICP测量误差计算子模块，用于采用所述激光ICP测量增量以及所述激光ICP测量增量关联的两个定位数据计算激光ICP测量误差；
激光AMCL测量误差计算子模块，用于采用所述激光AMCL测量数据以及所述激光AMCL测量数据关联的定位数据计算激光AMCL测量误差；
最小误差函数建立子模块，用于将所述里程测量误差、所述激光ICP测量误差和所述激光AMCL测量误差累加作为最小误差函数；
最小误差函数最优解求解子模块，用于对所述最小误差函数求解得到所述初始定位数据的最优解以作为目标定位数据。
可选地，所述里程测量误差计算子模块包括：
第一预测增量计算单元，用于针对所述滑动窗口中的每个里程增量，计算所述里程增量关联的两个定位数据的差值得到第一预测增量；
第一差值计算单元，用于计算所述第一预测增量和所述里程增量的差值，得到第一差值；
里程测量不确定度计算单元，用于采用所述里程增量和预设的轮速误差系数计算里程测量不确定度；
里程测量误差计算单元，用于采用所述第一差值和所述里程测量不确定度计算里程测量误差。
可选地，所述激光ICP测量误差计算子模块包括：
第二预测增量计算单元，用于针对所述滑动窗口中的每个激光ICP测量增量，计算所述激光ICP测量增量关联的两个定位数据的差值得到第二预测增量；
第二差值计算单元，用于计算所述第二预测增量和所述激光ICP测量增量的差值，得到第二差值；
激光ICP测量的不确定度获取单元，用于获取激光ICP测量的不确定度；
激光ICP测量误差计算单元，用于采用所述第二差值和所述激光ICP测量的不确定度计算激光ICP测量误差。
可选地，所述激光AMCL测量误差计算子模块包括：
第三差值计算单元，用于针对所述滑动窗口中的每个激光AMCL测量数据，计算所述激光AMCL测量数据与所述激光AMCL测量数据关联的定位数据的差值得到第三差值；
激光AMCL测量数据的不确定度获取单元，用于获取激光AMCL测量数据的不确定度；
激光AMCL测量误差计算单元，用于采用所述第三差值和所述激光AMCL测量数据的不确定度计算激光AMCL测量误差。
可选地，所述最小误差函数最优解求解子模块包括：
定位值获取单元，用于在对所述最小误差函数求解前，获取所述滑动窗口中位于首端的定位数据的值；
迭代求解单元，用于基于所述滑动窗口中位于首端的定位数据的值对所述最小误差函数迭代求解得到所述初始定位数据的最优解以作为目标定位数据。
可选地，还包括：
定位数据删除模块，用于当所滑动窗口中的定位数据的数量大于预设值时，删除位于所述滑动窗口的首端的定位数据；
滑动模块，用于将所述滑动窗口中的定位数据向首端平移。
本实施例提供的机器人的定位装置可执行本发明任意实施例提供的机器人的定位方法，具备相应的功能和有益效果。
实施例四
参照图5，示出了本发明一个示例中的一种设备的结构示意图。如图5所示，该设备具体可以包括：处理器50、存储器51、具有触摸功能的显示屏52、输入装置53、输出装置54以及通信装置55。该设备中处理器50的数量可以是一个或者多个，图5中以一个处理器50为例。该设备中存储器51的数量可以是一个或者多个，图5中以一个存储器51为例。该设备的处理器50、存储器51、显示屏52、输入装置53、输出装置54以及通信装置55可以通过总线或者其他方式连接，图5中以通过总线连接为例。
存储器51作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序以及模块，如本发明任意实施例所述的机器人的定位方法对应的程序指令/模块(例如，上述机器人的定位装置中的初始定位数据生成模块401、滑动窗口数据增加模块402、测量数据关联模块403和初始定位数据优化模块404)，存储器51可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作装置、至少一个功能所需的应用程序；存储数据区可存储根据设备的使用所创建的数据等。此外，存储器51可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实例中，存储器51可进一步包括相对于处理器50远程设置的存储器，这些远程存储器可以通过网络连接至设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
显示屏52为具有触摸功能的显示屏52，其可以是电容屏、电磁屏或者红外屏。一般而言，显示屏52用于根据处理器50的指示显示数据，还用于接收作用于显示屏52的触摸操作，并将相应的信号发送至处理器50或其他装置。可选的，当显示屏52为红外屏时，其还包括红外触摸框，该红外触摸框设置在显示屏52的四周，其还可以用于接收红外信号，并将该红外信号发送至处理器50或者其他设备。
通信装置55，用于与其他设备建立通信连接，其可以是有线通信装置和/或无线通信装置。
输入装置53可用于接收输入的数字或者字符信息，以及产生与设备的用户设置以及功能控制有关的键信号输入。输出装置54可以包括扬声器等音频设备。需要说明的是，输入装置53和输出装置54的具体组成可以根据实际情况设定。
处理器50通过运行存储在存储器51中的软件程序、指令以及模块，从而执行设备的各种功能应用以及数据处理，即实现上述机器人的定位方法。
具体地，实施例中，处理器50执行存储器51中存储的一个或多个程序时，具体实现本发明实施例提供的机器人的定位方法的步骤。
实施例五
本发明实施例五还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可实现本发明任意实施例中的机器人的定位方法。
该方法具体可以包括：
接收机器人的多个传感器所采集的当前测量数据，生成所述机器人的初始定位数据；
将所述初始定位数据增加至预先建立的滑动窗口，所述滑动窗口包括多个历史定位数据以及所述历史定位数据关联的历史测量数据；
从所述滑动窗口中确定所述当前测量数据关联的第一定位数据；
根据所述滑动窗口中的历史定位数据、所述历史测量数据、所述当前测量数据和所述第一定位数据对所述初始定位数据进行优化以得到目标定位数据。
当然，本发明实施例所提供的一种包含计算机可执行指令的存储介质，其计算机可执行指令不限于如上所述的方法操作，还可以执行本发明应用于设备上任意实施例所提供的机器人的定位方法中的相关操作。
需要说明的是，对于装置、设备、存储介质实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到，本发明可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(RandomAccess Memory，RAM)、闪存(FLASH)、硬盘或光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述机器人的定位方法。
值得注意的是，上述机器人的定位装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。
以上所述仅为本发明的优选实施例，并不用于限制本发明，对于本领域技术人员而言，本发明可以有各种改动和变化。凡在本发明的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (15)

1.一种机器人的定位方法，其特征在于，包括：
接收机器人的多个传感器所采集的当前测量数据，生成所述机器人的初始定位数据；
将所述初始定位数据增加至预先建立的滑动窗口，所述滑动窗口包括多个历史定位数据以及所述历史定位数据关联的历史测量数据；
从所述滑动窗口中确定所述当前测量数据关联的第一定位数据；
根据所述滑动窗口中的历史定位数据、所述历史测量数据、所述当前测量数据和所述第一定位数据对所述初始定位数据进行优化以得到目标定位数据。
2.根据权利要求1所述的定位方法，其特征在于，所述传感器包括轮速里程计，所述接收机器人的多个传感器所采集的当前测量数据，生成所述机器人的初始定位数据，包括：
接收所述轮速里程计在当前时间采集到的第一里程数据；
采用所述第一里程数据生成所述机器人在当前时间的初始定位数据，所述初始定位数据包括标识；
将所述标识和所述当前时间关联存储至预先建立的节点数据库中。
3.根据权利要求2所述的定位方法，其特征在于，所述采用所述第一里程数据生成所述机器人在当前时间的初始定位数据，包括：
获取所述轮速里程计在上一时间采集到的第二里程数据；
采用所述第一里程数据和所述第二里程数据计算第一里程增量；
在所述第一里程增量大于预设阈值时，获取所述当前时间之前的前一历史定位数据；
计算所述前一历史定位数据和所述第一里程增量的和值作为所述机器人在当前时间的初始定位数据。
4.根据权利要求2所述的定位方法，其特征在于，所述传感器还包括激光传感器，所述测量数据还包括激光ICP测量数据和激光AMCL测量数据，所述从所述滑动窗口中确定所述当前测量数据关联的第一定位数据，包括：
从所述滑动窗口中确定所述轮速里程计在所述当前时间输出的里程数据关联的定位数据；
基于所述节点数据库，从所述滑动窗口中确定所述激光ICP测量数据关联的定位数据，以及，
从所述滑动窗口中确定所述激光AMCL测量数据关联的定位数据。
5.根据权利要求4所述的定位方法，其特征在于，所述从所述滑动窗口中确定在所述当前时间所述轮速里程计输出的里程数据关联的定位数据，包括：
在所述滑动窗口中确定所述初始定位数据的前一个历史定位数据；
确定生成所述前一个历史定位数据时所述轮速里程计采集到的第三里程数据；
计算所述第一里程数据和所述第三里程数据的差值，得到第二里程增量；
确定所述初始定位数据和所述前一个历史定位数据为所述第二里程增量关联的定位数据。
6.根据权利要求4所述的定位方法，其特征在于，所述从所述滑动窗口中确定所述激光ICP测量数据关联的定位数据，包括：
获取接收到当前激光ICP测量数据时的第一时间；
基于所述第一时间在所述数据节点数据库中确定所述当前激光ICP测量数据关联的第一标识；
确定所述第一标识对应的第二定位数据；
获取上一回接收到的激光ICP测量数据关联的第三定位数据；
对所述当前激光ICP测量数据和所述上一回接收到的激光ICP测量数据进行ICP配准，得到激光ICP测量增量；
确定所述第二定位数据和所述第三定位数据为所述激光ICP测量增量关联的定位数据。
7.根据权利要求4所述的定位方法，其特征在于，所述基于所述节点数据库，从所述滑动窗口中确定所述激光AMCL测量数据关联的定位数据，包括：
获取接收到当前激光AMCL测量数据时的第二时间；
基于所述第二时间在所述节点数据库中确定当前激光AMCL测量数据关联的第二标识；
确定所述第二标识对应的第四定位数据；
确定所述第四定位数据为当前激光AMCL测量数据关联的定位数据。
8.根据权利要求1所述的定位方法，其特征在于，所述测量数据包括里程数据、激光ICP测量数据和激光AMCL测量数据，所述根据所述滑动窗口中的历史定位数据、历史定位数据关联的历史测量数据和所述当前测量数据对所述初始定位数据进行优化以得到目标定位数据，包括：
采用所述里程数据计算里程增量，以及采用所述激光ICP测量数据计算激光ICP测量增量；其中，所述里程增量与所述滑动窗口中的两个定位数据相关联，所述激光ICP测量增量与所述滑动窗口中的两个定位数据相关联，所述激光AMCL测量数据与滑动窗口中的一个定位数据相关联；
采用所述里程增量以及所述里程增量关联的两个定位数据计算里程测量误差；
采用所述激光ICP测量增量以及所述激光ICP测量增量关联的两个定位数据计算激光ICP测量误差；
采用所述激光AMCL测量数据以及所述激光AMCL测量数据关联的定位数据计算激光AMCL测量误差；
将所述里程测量误差、所述激光ICP测量误差和所述激光AMCL测量误差累加作为最小误差函数；
对所述最小误差函数求解得到所述初始定位数据的最优解以作为目标定位数据。
9.根据权利要求8所述的定位方法，其特征在于，所述采用所述里程增量以及所述里程增量关联的两个定位数据计算里程测量误差，包括：
针对所述滑动窗口中的每个里程增量，计算所述里程增量关联的两个定位数据的差值得到第一预测增量；
计算所述第一预测增量和所述里程增量的差值，得到第一差值；
采用所述里程增量和预设的轮速误差系数计算里程测量不确定度；
采用所述第一差值和所述里程测量不确定度计算所述里程测量误差；
所述采用所述激光ICP测量增量以及所述激光ICP测量增量关联的两个定位数据计算激光ICP测量误差，包括：
针对所述滑动窗口中的每个激光ICP测量增量，计算所述激光ICP测量增量关联的两个定位数据的差值得到第二预测增量；
计算所述第二预测增量和所述激光ICP测量增量的差值，得到第二差值；
获取激光ICP测量的不确定度；
采用所述第二差值和所述激光ICP测量的不确定度计算激光ICP测量误差。
10.根据权利要求8所述的定位方法，其特征在于，所述采用所述激光AMCL测量数据以及所述激光AMCL测量数据关联的定位数据计算激光AMCL测量误差，包括：
针对所述滑动窗口中的每个激光AMCL测量数据，计算所述激光AMCL测量数据与所述激光AMCL测量数据关联的定位数据的差值得到第三差值；
获取激光AMCL测量的不确定度；
采用所述第三差值和所述激光AMCL测量数据的不确定度计算激光AMCL测量误差。
11.根据权利要求8所述的方法，其特征在于，所述对所述最小误差函数求解得到所述初始定位数据的最优解以作为目标定位数据，包括：
在对所述最小误差函数求解前，获取所述滑动窗口中位于首端的定位数据的值；
基于所述滑动窗口中位于首端的定位数据的值对所述最小误差函数迭代求解得到所述初始定位数据的最优解以作为目标定位数据。
12.根据权利要求1所述的定位方法，其特征在于，还包括：
当所滑动窗口中的定位数据的数量大于预设值时，删除位于所述滑动窗口的首端的定位数据；
将所述滑动窗口中的定位数据向首端平移。
13.一种机器人的定位装置，其特征在于，包括：
初始定位数据生成模块，用于接收机器人的多个传感器所采集的当前测量数据，生成所述机器人的初始定位数据；
滑动窗口数据增加模块，用于将所述初始定位数据增加至预先建立的滑动窗口，所述滑动窗口包括多个历史定位数据以及所述历史定位数据关联的历史测量数据；
测量数据关联模块，用于从所述滑动窗口中确定所述当前测量数据关联的第一定位数据；
初始定位数据优化模块，用于根据所述滑动窗口中的历史定位数据、所述历史测量数据、所述当前测量数据和所述第一定位数据对所述初始定位数据进行优化以得到目标定位数据。
14.一种设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-12中任一项所述的机器人的定位方法。
15.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-12中任一项所述的机器人的定位方法。
CN202010018659.5A 2020-01-08 2020-01-08 机器人的定位方法、定位装置、设备和存储介质 Active CN111141290B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010018659.5A CN111141290B (zh) 2020-01-08 2020-01-08 机器人的定位方法、定位装置、设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010018659.5A CN111141290B (zh) 2020-01-08 2020-01-08 机器人的定位方法、定位装置、设备和存储介质

Publications (2)

Publication Number Publication Date
CN111141290A true CN111141290A (zh) 2020-05-12
CN111141290B CN111141290B (zh) 2023-09-26

Family

ID=70524025

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010018659.5A Active CN111141290B (zh) 2020-01-08 2020-01-08 机器人的定位方法、定位装置、设备和存储介质

Country Status (1)

Country Link
CN (1) CN111141290B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3078935A1 (en) * 2015-04-10 2016-10-12 The European Atomic Energy Community (EURATOM), represented by the European Commission Method and device for real-time mapping and localization
CN107610175A (zh) * 2017-08-04 2018-01-19 华南理工大学 基于半直接法和滑动窗口优化的单目视觉slam算法
CN108844553A (zh) * 2018-06-27 2018-11-20 广州视源电子科技股份有限公司 校正机器人移动过程中的里程的方法、装置及机器人
KR20190001086A (ko) * 2017-06-26 2019-01-04 서울대학교산학협력단 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
CN110285806A (zh) * 2019-07-05 2019-09-27 电子科技大学 基于多次位姿校正的移动机器人快速精确定位算法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3078935A1 (en) * 2015-04-10 2016-10-12 The European Atomic Energy Community (EURATOM), represented by the European Commission Method and device for real-time mapping and localization
US20180075643A1 (en) * 2015-04-10 2018-03-15 The European Atomic Energy Community (Euratom), Represented By The European Commission Method and device for real-time mapping and localization
KR20190001086A (ko) * 2017-06-26 2019-01-04 서울대학교산학협력단 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
CN107610175A (zh) * 2017-08-04 2018-01-19 华南理工大学 基于半直接法和滑动窗口优化的单目视觉slam算法
CN108844553A (zh) * 2018-06-27 2018-11-20 广州视源电子科技股份有限公司 校正机器人移动过程中的里程的方法、装置及机器人
CN110285806A (zh) * 2019-07-05 2019-09-27 电子科技大学 基于多次位姿校正的移动机器人快速精确定位算法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
SEUNGWOOK LIM ET AL: ""Adaptive Sliding Window for Hierarchical Pose-Graph-Based SLAM"", 《2012 12TH INTERNATIONAL CONFERENCE ON CONTROL，AUTOMATION AND SYSTEMS》 *

Also Published As

Publication number Publication date
CN111141290B (zh) 2023-09-26

Similar Documents

Publication Publication Date Title
CN111536964B (zh) 机器人定位方法及装置、存储介质
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
WO2021232470A1 (zh) 基于多传感器融合的slam制图方法、系统
US20190195631A1 (en) Positioning method, positioning device, and robot
Borrmann et al. Globally consistent 3D mapping with scan matching
CN104501814B (zh) 一种基于视觉和惯性信息的姿态与位置估计方法
CN106123890A (zh) 一种多传感器数据融合的机器人定位方法
CN105652871A (zh) 移动机器人的重定位方法
CN112444246B (zh) 高精度的数字孪生场景中的激光融合定位方法
EP3852065A1 (en) Data processing method and apparatus
CN105467838A (zh) 一种随机有限集框架下的同步定位与地图构建方法
CN109655059B (zh) 一种基于θ-增量学习的视觉-惯性融合导航系统及方法
CN111415417A (zh) 一种集成稀疏点云的移动机器人拓扑经验地图构建方法
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
Guan et al. A novel feature points tracking algorithm in terms of IMU-aided information fusion
CA2894863A1 (en) Indoor localization using crowdsourced data
CN103903253A (zh) 一种可移动终端定位方法及系统
CN111141290B (zh) 机器人的定位方法、定位装置、设备和存储介质
CN116429116A (zh) 一种机器人定位方法及设备
WO2023087681A1 (zh) 定位初始化方法、装置、计算机可读存储介质和计算机程序产品
CN116429112A (zh) 多机器人协同定位方法和装置、设备及存储介质
CN115900697A (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN115493579A (zh) 定位修正方法、装置、割草机器人以及存储介质
Rybski et al. Appearance-based minimalistic metric SLAM

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