CN116518983B - 用于移动机器人定位的自适应融合方法及装置 - Google Patents
用于移动机器人定位的自适应融合方法及装置 Download PDFInfo
- Publication number
- CN116518983B CN116518983B CN202310814337.5A CN202310814337A CN116518983B CN 116518983 B CN116518983 B CN 116518983B CN 202310814337 A CN202310814337 A CN 202310814337A CN 116518983 B CN116518983 B CN 116518983B
- Authority
- CN
- China
- Prior art keywords
- noise
- mobile robot
- time
- system model
- sensor
- 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
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 18
- 238000000034 method Methods 0.000 claims abstract description 111
- 230000008569 process Effects 0.000 claims description 44
- 230000006870 function Effects 0.000 claims description 36
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000005259 measurement Methods 0.000 claims description 17
- 230000004927 fusion Effects 0.000 claims description 16
- 230000003044 adaptive effect Effects 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 4
- 238000005516 engineering process Methods 0.000 description 3
- 239000000203 mixture Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012946 outsourcing Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000007788 liquid Substances 0.000 description 1
- 230000005055 memory storage Effects 0.000 description 1
- 238000013508 migration Methods 0.000 description 1
- 230000005012 migration Effects 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 229920001296 polysiloxane Polymers 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请公开了一种用于移动机器人定位的自适应融合方法及装置，该方法包括：构建与传感器数量一致的第一***模型，为移动机器人设置初始时刻的状态值；将非线性模型转换为线性模型以生成有界噪声集；根据线性模型与有界噪声集生成第二***模型，辨识第二***模型中的未知噪声生成噪声子集；根据第二***模型获取第一时刻的第一噪声集合，并根据第一噪声集合预测第二时刻的第二噪声集合；根据第二时刻的状态值与第二***模型计算对应噪声集合，以此更新第二噪声集合生成第三噪声集合；整合多个传感器对应的噪声子集与第三噪声集合生成全局模型。能够避免多传感器中随机噪声与未知噪声对***的干扰，消除未知噪声对定位精确度的影响。
Description
技术领域
本申请涉及目标跟踪及定位导航技术领域，尤其涉及一种用于移动机器人定位的自适应融合方法及装置。
背景技术
移动机器人定位技术是实现移动机器人自主移动的关键技术之一。目前，移动机器人常用的定位技术为多传感器定位。多传感器定位分为集中式融合方法和分布式融合方法两类。分布式融合方法与集中式融合方法相比有更强的鲁棒性和容错性，所以现阶段对信息融合的研究主要针对分布式融合方法展开。
例如：基于高斯混合算法的北斗卫星导航***/捷联惯性导航***（BDS/SINS）组合导航算法、全球定位***/惯性导航***（GPS/INS）组合导航双模型方案等均为分布式融合方法。分布式融合方法通常假设受到的噪声干扰主要为服从高斯分布的随机噪声。但由于采集的数据不足或缺失，高斯噪声的假设在实际***中不能完全满足需求，特别是在工程应用中无法得到精确的协方差。一种对统计特性没有过多要求只需知道其边界的未知但有界噪声能够解决上述问题。但是在大规模网络化的多传感器***中，参数不确定引起的随机噪声与未知但有界噪声会同时对非线性***产生干扰，进而影响移动机器人定位的准确性。
发明内容
本申请实施例通过提供一种用于移动机器人定位的自适应融合方法及装置，解决了现有技术中在大规模网络化的多传感器***中，参数不确定引起的随机噪声与未知但有界噪声同时对非线性***产生干扰的问题，实现了将非线性分布的噪声转换为线性分布的噪声，并计算出未知噪声，进而形成一种能够解决上述问题的全局模型。
第一方面，本申请实施例提供了一种用于移动机器人定位的自适应融合方法，包括：为多个传感器的移动机器人构建与传感器数量一致的第一***模型，并为所述移动机器人设置初始时刻的状态值；获取所述第一***模型中的非线性模型，并将所述非线性模型转换为线性模型以生成有界噪声集；根据所述线性模型与所述有界噪声集生成第二***模型，并辨识所述第二***模型中的未知噪声生成噪声子集；根据所述第二***模型获取第一时刻的第一噪声集合，并根据所述第一噪声集合预测第二时刻的第二噪声集合；根据所述第二时刻的状态值与所述第二***模型计算对应噪声集合，并以此更新所述第二噪声集合生成第三噪声集合；整合多个所述传感器对应的所述噪声子集与所述第三噪声集合生成全局模型。
结合第一方面，在一种可能的实现方式中，所述第一***模型包括所述移动机器人的状态方程与单个所述传感器的量测方程；所述状态方程的公式如下：
；式中，为移动机器人在k时刻的状态方程，为k时刻所述移动
机器人的状态值，为稳定常数矩阵，为关于所述移动机器人的状态值的二阶可微非线性
函数，为所述移动机器人的过程噪声；所述量测方程的公式如下：
；式中，为第i个所述传感器在k时刻的
观测值，（,）为所述移动机器人在k时刻的坐标，,）为第i个所述传感器的坐标，
为所述移动机器人在k时刻的姿态角，为关于第i个所述传感器的观测值的二阶可微非
线性函数，为所述传感器在k时刻的观测噪声。
结合第一方面，在一种可能的实现方式中，所述过程噪声与所述观测噪声为相互独立的非高斯噪声。
结合第一方面，在一种可能的实现方式中，所述移动机器人的状态值的确定公式
如下：；式中，为在k时刻所述移动机器人的状态值，为所述移动机
器人在k时刻的横坐标，为所述移动机器人在k时刻的纵坐标，为所述移动机器人在k
时刻的横坐标方向上的速度，为所述移动机器人在k时刻的纵坐标方向上的速度。
结合第一方面，在一种可能的实现方式中，所述第二***模型的确定公式如下：
；
；式中，为k+1时
刻所述移动机器人的状态方程，为k时刻所述移动机器人的状态值，为稳定常数矩阵，
为关于所述移动机器人的状态值的二阶可微非线性函数，为所述移动机器人在k时刻的
状态估测值，为在k-1时刻对k时刻的状态估测值，与为所述移动机器人与第i个
所述传感器在k时刻的所述有界噪声集，为第i个所述传感器在k时刻的观测值，为关
于第i个所述传感器的观测值的二阶可微非线性函数。
结合第一方面，在一种可能的实现方式中，所述整合多个所述传感器对应的所述噪声子集与所述第三噪声集合生成全局模型，包括：获取多个所述传感器对应的所述噪声子集与第三噪声集合，并确定其全局并集；将多个所述传感器对应的所述噪声子集进行融合生成融合噪声；根据所述全局并集与所述融合噪声生成所述全局模型。
结合第一方面，在一种可能的实现方式中，所述噪声子集的确定公式如下：
；式中，为所述噪声子集，
为第i个所述传感器对应的所述第二***模型中所述移动机器人在k时刻的状态估测值，
第i个所述传感器对应的所述第二***模型中k时刻的形状矩阵，为k时刻所述移动机器
人的状态值，为阶数为n的实数，T表示矩阵的转置。
第二方面，本申请实施例提供了一种用于移动机器人定位的自适应融合装置，包括：第一***模型模块，用于为多个传感器的移动机器人构建与传感器数量一致的第一***模型，并为所述移动机器人设置初始时刻的状态值；有界噪声集模块，用于获取所述第一***模型中的非线性模型，并将所述非线性模型转换为线性模型以生成有界噪声集；噪声子集模块，用于根据所述线性模型与所述有界噪声集生成第二***模型，并辨识所述第二***模型中的未知噪声生成噪声子集；噪声集合模块，用于根据所述第二***模型获取第一时刻的第一噪声集合，并根据所述第一噪声集合预测第二时刻的第二噪声集合；更新模块，用于根据所述第二时刻的状态值与所述第二***模型计算对应噪声集合，并以此更新所述第二噪声集合生成第三噪声集合；全局模型模块，用于整合多个所述传感器对应的所述噪声子集与所述第三噪声集合生成全局模型。
结合第二方面，在一种可能的实现方式中，所述第一***模型包括所述移动机器人的状态方程与单个所述传感器的量测方程；所述状态方程的公式如下：
；式中，为移动机器人在k时刻的状态方程，为k时刻所述移动
机器人的状态值，为稳定常数矩阵，为关于所述移动机器人的状态值的二阶可微非线性
函数，为所述移动机器人的过程噪声；所述量测方程的公式如下：
；式中，为第i个所述传感器在k时刻的
观测值，（,）为所述移动机器人在k时刻的坐标，,）为第i个所述传感器的坐标，
为所述移动机器人在k时刻的姿态角，为关于第i个所述传感器的观测值的二阶可微非
线性函数，为所述传感器在k时刻的观测噪声。
结合第二方面，在一种可能的实现方式中，所述过程噪声与所述观测噪声为相互独立的非高斯噪声。
结合第二方面，在一种可能的实现方式中，所述移动机器人的状态值的确定公式
如下：；式中，为在k时刻所述移动机器人的状态值，为所述移动机
器人在k时刻的横坐标，为所述移动机器人在k时刻的纵坐标，为所述移动机器人在k
时刻的横坐标方向上的速度，为所述移动机器人在k时刻的纵坐标方向上的速度。
结合第二方面，在一种可能的实现方式中，所述第二***模型的确定公式如下：
；
；式中，为k+1时
刻所述移动机器人的状态方程，为k时刻所述移动机器人的状态值，为稳定常数矩阵，
为关于所述移动机器人的状态值的二阶可微非线性函数，为所述移动机器人在k时刻的
状态估测值，为在k-1时刻对k时刻的状态估测值，与为所述移动机器人与第i个
所述传感器在k时刻的所述有界噪声集，为第i个所述传感器在k时刻的观测值，为关
于第i个所述传感器的观测值的二阶可微非线性函数。
结合第二方面，在一种可能的实现方式中，所述整合多个所述传感器对应的所述噪声子集与所述第三噪声集合生成全局模型，包括：获取多个所述传感器对应的所述噪声子集与第三噪声集合，并确定其全局并集；将多个所述传感器对应的所述噪声子集进行融合生成融合噪声；根据所述全局并集与所述融合噪声生成所述全局模型。
结合第二方面，在一种可能的实现方式中，所述噪声子集的确定公式如下：
；式中，为所述噪声子集，
为第i个所述传感器对应的所述第二***模型中所述移动机器人在k时刻的状态估测值，
第i个所述传感器对应的所述第二***模型中k时刻的形状矩阵，为k时刻所述移动机器
人的状态值，为阶数为n的实数，T表示矩阵的转置。
第三方面，本申请实施例提供了一种设备，所述设备包括：处理器；用于存储处理器可执行指令的存储器；所述处理器执行所述可执行指令时，实现如第一方面或第一方面任一种可能实现的方式所述的方法。
第四方面，本申请实施例提供了一种非易失性计算机可读存储介质，所述非易失性计算机可读存储介质包括用于存储计算机程序或指令，当该计算机程序或指令被执行时，使如第一方面或第一方面任一种可能实现的方式所述的方法被实现。
本申请实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：
本申请实施例通过构建移动机器人与各传感器的局部***模型，将非线性模型转换为线性模型，辨识未知噪声以及构建全局模型，有效解决了现有技术中在大规模网络化的多传感器***中，参数不确定引起的随机噪声与未知但有界噪声同时对非线性***产生干扰，进而影响移动机器人定位的准确性的问题，进而实现了一种用于移动机器人定位的自适应融合方法，能够避免多传感器中随机噪声与未知噪声对***的干扰，还能够消除未知噪声对定位精确度的影响，进而通过多传感器提高移动机器人的定位精度并增大其使用范围。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的用于移动机器人定位的自适应融合方法的流程图；
图2为本申请实施例提供的整合多个传感器对应的噪声子集与第三噪声集合生成全局模型的流程图；
图3为本申请实施例提供的用于移动机器人定位的自适应融合装置的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
以下对本申请实施例涉及的部分技术做出说明，以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本申请的范围和精神。同样，为了清楚和简明，以下的描述中省略了部分对公知功能和结构的描述。
图1是本申请实施例提供的用于移动机器人定位的自适应融合方法的流程图，包括步骤101至步骤106。其中，图1仅为本申请实施例示出的一种执行顺序，并不代表用于移动机器人定位的自适应融合方法的唯一执行顺序，在可实现最终结果的情况下，图1所示出的步骤可以被并列或颠倒执行。
步骤101：为多个传感器的移动机器人构建与传感器数量一致的第一***模型，并为移动机器人设置初始时刻的状态值。其中，第一***模型包括移动机器人的状态方程与单个传感器的量测方程。在本申请实施例中，多传感器的移动机器人的状态方程与一个传感器的量测方程构成一个第一***模型。移动机器人与多个传感器分别构成多个第一***模型。第一***模型的数量与传感器的数量一致。具体地，
状态方程的公式如下：
。式中，为移动机器人在k时刻的状态方程，为k时刻移动机器
人的状态值，为稳定常数矩阵，为关于移动机器人的状态值的二阶可微非线性函数，
为移动机器人的过程噪声。
量测方程的公式如下：
。式中，为第i个传感器在k时刻的观测
值，（,）为移动机器人在k时刻的坐标，,）为第i个传感器的坐标，为移动机器人
在k时刻的姿态角，为关于第i个传感器的观测值的二阶可微非线性函数，为传感器在
k时刻的观测噪声。
其中，过程噪声与观测噪声为相互独立的非高斯噪声。并且，第一***模型的过程
噪声与观测噪声中存在未知噪声。过程噪声与观测噪声服从如下约束条件。，，式中，为移动机器人的过程噪声，为传感器在k时刻的观测噪声，为
椭球中心为0，形状矩阵为Q的椭球集，为椭球中心为0，形状矩阵为S的椭球集。
此外，移动机器人的状态值的确定公式如下：
。式中，为在k时刻移动机器人的状态值，为移动机器人在
k时刻的横坐标，为移动机器人在k时刻的纵坐标，为移动机器人在k时刻的横坐标方
向上的速度，为移动机器人在k时刻的纵坐标方向上的速度。在本申请实施例中，初始时
刻时，移动机器人的速度为零，故将移动机器人初始时刻的状态值设置为：
，其中，()为移动机器人初始时刻的横坐标与纵坐标。
步骤102：获取第一***模型中的非线性模型，并将非线性模型转换为线性模型以
生成有界噪声集。在本申请实施例中，对第i个传感器对应的第一***模型中的非线性函数与进行泰勒展开，如下：
，
。式中，为k+1时刻移动机器人的状态值，为稳定常数矩阵，为关于移动机器人的状态值
的二阶可微非线性函数，为移动机器人在k时刻的状态估测值，为在第i个传感器
对应的第一***模型中k-1时刻对k时刻的状态估测值，与为移动机器人与第i个传
感器在k时刻的有界噪声集，为第i个传感器在k时刻的观测值，为关于第i个传感器
的观测值的二阶可微非线性函数，为非线性函数泰勒展开的高阶余项，为非线
性函数泰勒展开的高阶余项。对上述公式的余项进行整合，生成有界噪声集。
步骤103：根据线性模型与有界噪声集生成第二***模型，并辨识第二***模型中的未知噪声生成噪声子集。具体地，第二***模型的确定公式如下：
，
。式中，为k+1时
刻移动机器人的状态方程，为k时刻移动机器人的状态值，为稳定常数矩阵，为关于移
动机器人的状态值的二阶可微非线性函数，为移动机器人在k时刻的状态估测值，
为在k-1时刻对k时刻的状态估测值，与为移动机器人与第i个传感器在k时刻的有界
噪声集，为第i个传感器在k时刻的观测值，为关于第i个传感器的观测值的二阶可微
非线性函数。
在本申请实施例中，采用局部最优滤波器计算第二***模型中的未知噪声，生成噪声子集。具体地，噪声子集的确定公式如下：
。式中，为噪声子集，为第i
个传感器对应的第二***模型中移动机器人在k时刻的状态估测值，为第i个传感器对应
的第二***模型中k时刻的形状矩阵，为k时刻移动机器人的状态值，为阶数为n的实
数，T表示矩阵的转置。对上式进行转换获取第二***模型中待辨识的噪声集，并对待辨识
的噪声集的可行解进行外包近似与最小迹方法，辨识出待辨识的噪声集中的未知噪声。
步骤104：根据第二***模型获取第一时刻的第一噪声集合，并根据第一噪声集合预测第二时刻的第二噪声集合。其中，第一时刻为第二时刻的先前时刻。具体地，根据步骤103生成的第二***模型，代入步骤103计算的噪声子集计算第二***模型中移动机器人在第一时刻的状态方程与第i个传感器在第一时刻的量测方程，获取第一时刻时第二***模型中的第一噪声集合。并根据第一噪声集合预测当前时刻的第二噪声集合。
步骤105：根据第二时刻的状态值与第二***模型计算对应噪声集合，并以此更新第二噪声集合生成第三噪声集合。具体地，代入步骤103计算的噪声子集计算第二***模型中移动机器人在第二时刻的状态方程与第i个传感器在第二时刻的量测方程，获取第二时刻时第二***模型中的对应噪声集合。并使用对应噪声集合更新步骤104中预测的第二噪声集合，获得第三噪声集合。
步骤106：整合多个传感器对应的噪声子集与第三噪声集合生成全局模型。具体地，根据上述步骤获取的噪声子集与第三噪声集合生成全局模型的具体步骤如图2所示，包括步骤201至步骤203，具体如下。
步骤201：获取多个传感器对应的噪声子集与第三噪声集合，并确定其全局并集。具体地，获取多个传感器对应的第二***模型算得的第三噪声集合，对所有第三噪声集合取并集，得到最小外部椭球集，即全局并集。
步骤202：将多个传感器对应的噪声子集进行融合生成融合噪声。在本申请实施例中，将每个传感器对应的第二***模型中辨识的未知噪声即噪声子集采用闵可夫斯基和方法进行融合，生成融合噪声。
步骤203：根据全局并集与融合噪声生成全局模型。全局模型如下：
，。式中，为全局模型，为全局模型的全局
状态估计，为全局模型椭球集的形状矩阵，为融合噪声，为全局模型的全局噪声估
计，为全局模型椭球集的形状矩阵。
其中，，，
，式中，i为整数，i=1，2，…，N，为全局模型
的全局状态估计，为全局模型椭球集的形状矩阵，为第i个传感器对应的第二***模
型中的噪声子集，为第i个传感器对应的第二***模型中的移动机器人的状态值，为
第i个传感器对应的第二***模型中的第三噪声集合的形状矩阵，为全局模型椭球集
的形状矩阵。
虽然本申请提供了如实施例或流程图所述的方法操作步骤，但基于常规或者无创造性的劳动可以包括更多或者更少的操作步骤。本实施例中列举的步骤顺序仅仅为众多步骤执行顺序中的一种方式，不代表唯一的执行顺序。在实际中的装置或客户端产品执行时，可以按照本实施例或者附图所示的方法顺序执行或者并行执行（例如并行处理器或者多线程处理的环境）。
如图3所示，本申请实施例还提供一种用于移动机器人定位的自适应融合装置300。该装置包括：第一***模型模块301、有界噪声集模块302、噪声子集模块303、噪声集合模块304、更新模块305与全局模型模块306。具体如下：
第一***模型模块301用于为多个传感器的移动机器人构建与传感器数量一致的第一***模型，并为移动机器人设置初始时刻的状态值。其中，第一***模型包括移动机器人的状态方程与单个传感器的量测方程。第一***模型模块301具体用于，移动机器人与多个传感器分别构成多个第一***模型。状态方程的公式如下：
。式中，为移动机器人在k时刻的状态方程，为k时刻移动机器
人的状态值，为稳定常数矩阵，为关于移动机器人的状态值的二阶可微非线性函数，
为移动机器人的过程噪声。
量测方程的公式如下：
。式中，为第i个传感器在k时刻的观测
值，（,）为移动机器人在k时刻的坐标，,）为第i个传感器的坐标，为移动机器人
在k时刻的姿态角，为关于第i个传感器的观测值的二阶可微非线性函数，为传感器在
k时刻的观测噪声。
其中，过程噪声与观测噪声为相互独立的非高斯噪声。并且，第一***模型的过程
噪声与观测噪声中存在未知噪声。过程噪声与观测噪声服从如下约束条件。，，式中，为移动机器人的过程噪声，为传感器在k时刻的观测噪声，为
椭球中心为0，形状矩阵为Q的椭球集，为椭球中心为0，形状矩阵为S的椭球集。
此外，移动机器人的状态值的确定公式如下：
。式中，为在k时刻移动机器人的状态值，为移动机器人在
k时刻的横坐标，为移动机器人在k时刻的纵坐标，为移动机器人在k时刻的横坐标方
向上的速度，为移动机器人在k时刻的纵坐标方向上的速度。在本申请实施例中，初始时
刻时，移动机器人的速度为零，故将移动机器人初始时刻的状态值设置为：，
其中，()为移动机器人初始时刻的横坐标与纵坐标。
有界噪声集模块302用于获取第一***模型中的非线性模型，并将非线性模型转
换为线性模型以生成有界噪声集。有界噪声集模块302具体用于，在本申请实施例中，对第i
个传感器对应的第一***模型中的非线性函数与进行泰勒展开，如下：
，
。式中，为k+1时刻移动机器人的状态值，为稳定常数矩阵，为关于移动机器人的状态值
的二阶可微非线性函数，为移动机器人在k时刻的状态估测值，为在第i个传感器
对应的第一***模型中k-1时刻对k时刻的状态估测值，与为移动机器人与第i个传
感器在k时刻的有界噪声集，为第i个传感器在k时刻的观测值，为关于第i个传感器
的观测值的二阶可微非线性函数，为非线性函数泰勒展开的高阶余项，为非线
性函数泰勒展开的高阶余项。对上述公式的余项进行整合，生成有界噪声集。
噪声子集模块303用于根据线性模型与有界噪声集生成第二***模型，并辨识第二***模型中的未知噪声生成噪声子集。噪声子集模块303具体用于，第二***模型的确定公式如下：
，
。式中，为k+1时
刻移动机器人的状态方程，为k时刻移动机器人的状态值，为稳定常数矩阵，为关于移
动机器人的状态值的二阶可微非线性函数，为移动机器人在k时刻的状态估测值，
为在k-1时刻对k时刻的状态估测值，与为移动机器人与第i个传感器在k时刻的有界
噪声集，为第i个传感器在k时刻的观测值，为关于第i个传感器的观测值的二阶可微
非线性函数。
在本申请实施例中，采用局部最优滤波器计算第二***模型中的未知噪声，生成噪声子集。具体地，噪声子集的确定公式如下：
。式中，为噪声子集，为第i
个传感器对应的第二***模型中移动机器人在k时刻的状态估测值，第i个传感器对应的
第二***模型中k时刻的形状矩阵，为k时刻移动机器人的状态值，为阶数为n的实数，T
表示矩阵的转置。对上式进行转换获取第二***模型中待辨识的噪声集，采用下式对待辨
识的噪声集的可行解进行外包近似与最小迹方法，辨识出待辨识的噪声集中的未知噪声。
噪声集合模块304用于根据第二***模型获取第一时刻的第一噪声集合，并根据第一噪声集合预测第二时刻的第二噪声集合。其中，第一时刻为第二时刻的先前时刻。噪声集合模块304具体用于，根据步骤103生成的第二***模型，代入步骤103计算的噪声子集计算第二***模型中移动机器人在第一时刻的状态方程与第i个传感器在第一时刻的量测方程，获取第一时刻时第二***模型中的第一噪声集合。并根据第一噪声集合预测当前时刻的第二噪声集合。
更新模块305用于根据第二时刻的状态值与第二***模型计算对应噪声集合，并以此更新第二噪声集合生成第三噪声集合。更新模块305具体用于，代入步骤103计算的噪声子集计算第二***模型中移动机器人在第二时刻的状态方程与第i个传感器在第二时刻的量测方程，获取第二时刻时第二***模型中的对应噪声集合。并使用对应噪声集合更新步骤104中预测的第二噪声集合，获得第三噪声集合。
全局模型模块306用于整合多个传感器对应的噪声子集与第三噪声集合生成全局模型。全局模型模块306具体用于，获取多个传感器对应的噪声子集与第三噪声集合，并确定其全局并集。将多个传感器对应的噪声子集进行融合生成融合噪声。根据全局并集与融合噪声生成全局模型。
本申请所述装置中的部分模块可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构、类等。也可以在分布式计算环境中实践本申请，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。
上述申请实施例阐明的装置或模块，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。为了描述方便，描述以上装置时以功能分为各种模块分别描述。在实施本申请实施例时可以把各模块的功能在同一个或多个软件和/或硬件中实现。当然，也可以将实现某功能的模块由多个子模块或子单元组合实现。
本申请中所述的方法、装置或模块可以以计算机可读程序代码方式实现控制器按任何适当的方式实现，例如，控制器可以采取例如微处理器或处理器以及存储可由该（微）处理器执行的计算机可读程序代码（例如软件或固件）的计算机可读介质、逻辑门、开关、专用集成电路（英文：Application Specific Integrated Circuit；简称：ASIC）、可编程逻辑控制器和嵌入微控制器的形式，控制器的例子包括但不限于以下微控制器：ARC 625D、Atmel AT91SAM、Microchip PIC18F26K20以及Silicone Labs C8051F320，存储器控制器还可以被实现为存储器的控制逻辑的一部分。本领域技术人员也知道，除了以纯计算机可读程序代码方式实现控制器以外，完全可以通过将方法步骤进行逻辑编程来使得控制器以逻辑门、开关、专用集成电路、可编程逻辑控制器和嵌入微控制器等的形式来实现相同功能。因此这种控制器可以被认为是一种硬件部件，而对其内部包括的用于实现各种功能的装置也可以视为硬件部件内的结构。或者甚至，可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
本申请实施例还提供了一种设备，所述设备包括：处理器；用于存储处理器可执行指令的存储器；所述处理器执行所述可执行指令时，实现如本申请实施例所述的方法。
本申请实施例还提供了一种非易失性计算机可读存储介质，其上存储有计算机程序或指令，当该计算机程序或指令被执行时，使如本申请实施例中所述的方法被实现。
此外，在本发明的各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独存在，也可以两个或两个以上模块集成在一个模块中。
上述存储介质包括但不限于随机存取存储器（英文：Random Access Memory；简称：RAM）、只读存储器（英文：Read-Only Memory；简称：ROM）、缓存（英文：Cache）、硬盘（英文：Hard Disk Drive；简称：HDD）或者存储卡（英文：Memory Card）。所述存储器可以用于存储计算机程序指令。
通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本申请可借助软件加必需的硬件的方式来实现。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，也可以通过数据迁移的实施过程中体现出来。该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备（可以是个人计算机，移动终端，服务器，或者网络设备等）执行本申请各个实施例或者实施例的某些部分所述的方法。
本说明书中的各个实施方式采用递进的方式描述，各个实施方式之间相同或相似的部分互相参见即可，每个实施方式重点说明的都是与其他实施方式的不同之处。本申请的全部或者部分可用于众多通用或专用的计算机***环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、移动通信终端、多处理器***、基于微处理器的***、可编程的电子设备、网络PC、小型计算机、大型计算机、包括以上任何***或设备的分布式计算环境等等。
以上实施例仅用以说明本申请的技术方案，而非对本申请限制；尽管参照前述实施例对本申请进行了详细的说明，本领域普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请技术方案的范围。
Claims (8)
1.一种用于移动机器人定位的自适应融合方法，其特征在于，包括：
为多个传感器的移动机器人构建与传感器数量一致的第一***模型，并为所述移动机器人设置初始时刻的状态值；
其中，所述第一***模型包括所述移动机器人的状态方程与单个所述传感器的量测方程；
所述状态方程的公式如下：
；式中，/>为移动机器人在k时刻的状态方程，/>为k时刻所述移动机器人的状态值，/>为稳定常数矩阵，/>为关于所述移动机器人的状态值的二阶可微非线性函数，/>为所述移动机器人的过程噪声；
所述量测方程的公式如下：
；式中，/>为第i个所述传感器在k时刻的观测值，（/>,/>）为所述移动机器人在k时刻的坐标，/>,/>）为第i个所述传感器的坐标，/>为所述移动机器人在k时刻的姿态角，/>为关于第i个所述传感器的观测值的二阶可微非线性函数，/>为所述传感器在k时刻的观测噪声；
获取所述第一***模型中的非线性模型，并将所述非线性模型转换为线性模型以生成有界噪声集；
根据所述线性模型与所述有界噪声集生成第二***模型，并辨识所述第二***模型中的未知噪声生成噪声子集；
其中，所述第二***模型的确定公式如下：
；
；式中，为k+1时刻所述移动机器人的状态方程，/>为k时刻所述移动机器人的状态值，/>为稳定常数矩阵，/>为关于所述移动机器人的状态值的二阶可微非线性函数，/>为所述移动机器人在k时刻的状态估测值，/>为在k-1时刻对k时刻的状态估测值，/>与/>为所述移动机器人与第i个所述传感器在k时刻的所述有界噪声集，/>为第i个所述传感器在k时刻的观测值，/>为关于第i个所述传感器的观测值的二阶可微非线性函数；
根据所述第二***模型获取第一时刻的第一噪声集合，并根据所述第一噪声集合预测第二时刻的第二噪声集合；
根据所述第二时刻的状态值与所述第二***模型计算对应噪声集合，并以此更新所述第二噪声集合生成第三噪声集合；
整合多个所述传感器对应的所述噪声子集与所述第三噪声集合生成全局模型。
2.根据权利要求1所述的方法，其特征在于，所述过程噪声与所述观测噪声为相互独立的非高斯噪声。
3.根据权利要求1所述的方法，其特征在于，所述移动机器人的状态值的确定公式如下：
；式中，/>为在k时刻所述移动机器人的状态值，/>为所述移动机器人在k时刻的横坐标，/>为所述移动机器人在k时刻的纵坐标，/>为所述移动机器人在k时刻的横坐标方向上的速度，/>为所述移动机器人在k时刻的纵坐标方向上的速度。
4.根据权利要求1所述的方法，其特征在于，所述整合多个所述传感器对应的所述噪声子集与所述第三噪声集合生成全局模型，包括：
获取多个所述传感器对应的所述噪声子集与第三噪声集合，并确定其全局并集；
将多个所述传感器对应的所述噪声子集进行融合生成融合噪声；
根据所述全局并集与所述融合噪声生成所述全局模型。
5.根据权利要求1所述的方法，其特征在于，所述噪声子集的确定公式如下：
；式中，/>为所述噪声子集，/>为第i个所述传感器对应的所述第二***模型中所述移动机器人在k时刻的状态估测值，/>为第i个所述传感器对应的所述第二***模型中k时刻的形状矩阵，/>为k时刻所述移动机器人的状态值，/>为阶数为n的实数，T表示矩阵的转置。
6.一种用于移动机器人定位的自适应融合装置，其特征在于，包括：
第一***模型模块，用于为多个传感器的移动机器人构建与传感器数量一致的第一***模型，并为所述移动机器人设置初始时刻的状态值；其中，所述第一***模型包括所述移动机器人的状态方程与单个所述传感器的量测方程；
所述状态方程的公式如下：
；式中，/>为移动机器人在k时刻的状态方程，/>为k时刻所述移动机器人的状态值，/>为稳定常数矩阵，/>为关于所述移动机器人的状态值的二阶可微非线性函数，/>为所述移动机器人的过程噪声；
所述量测方程的公式如下：
；式中，/>为第i个所述传感器在k时刻的观测值，（/>,/>）为所述移动机器人在k时刻的坐标，/>,/>）为第i个所述传感器的坐标，/>为所述移动机器人在k时刻的姿态角，/>为关于第i个所述传感器的观测值的二阶可微非线性函数，/>为所述传感器在k时刻的观测噪声；
有界噪声集模块，用于获取所述第一***模型中的非线性模型，并将所述非线性模型转换为线性模型以生成有界噪声集；
噪声子集模块，用于根据所述线性模型与所述有界噪声集生成第二***模型，并辨识所述第二***模型中的未知噪声生成噪声子集；其中，所述第二***模型的确定公式如下：
；
；式中，为k+1时刻所述移动机器人的状态方程，/>为k时刻所述移动机器人的状态值，/>为稳定常数矩阵，/>为关于所述移动机器人的状态值的二阶可微非线性函数，/>为所述移动机器人在k时刻的状态估测值，/>为在k-1时刻对k时刻的状态估测值，/>与/>为所述移动机器人与第i个所述传感器在k时刻的所述有界噪声集，/>为第i个所述传感器在k时刻的观测值，/>为关于第i个所述传感器的观测值的二阶可微非线性函数；
噪声集合模块，用于根据所述第二***模型获取第一时刻的第一噪声集合，并根据所述第一噪声集合预测第二时刻的第二噪声集合；
更新模块，用于根据所述第二时刻的状态值与所述第二***模型计算对应噪声集合，并以此更新所述第二噪声集合生成第三噪声集合；
全局模型模块，用于整合多个所述传感器对应的所述噪声子集与所述第三噪声集合生成全局模型。
7.一种用于执行用于移动机器人定位的自适应融合方法的设备，其特征在于，包括：
处理器；
用于存储处理器可执行指令的存储器；
所述处理器执行所述可执行指令时，实现如权利要求1至5中任意一项所述的方法。
8.一种非易失性计算机可读存储介质，其特征在于，包括用于存储计算机程序或指令，当该计算机程序或指令被执行时，使如权利要求1至5中任一项所述的方法被实现。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310814337.5A CN116518983B (zh) | 2023-07-05 | 2023-07-05 | 用于移动机器人定位的自适应融合方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310814337.5A CN116518983B (zh) | 2023-07-05 | 2023-07-05 | 用于移动机器人定位的自适应融合方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116518983A CN116518983A (zh) | 2023-08-01 |
| CN116518983B true CN116518983B (zh) | 2023-10-17 |
Family
ID=87406796
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310814337.5A Active CN116518983B (zh) | 2023-07-05 | 2023-07-05 | 用于移动机器人定位的自适应融合方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116518983B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108318038A (zh) * | 2018-01-26 | 2018-07-24 | 南京航空航天大学 | 一种四元数高斯粒子滤波移动机器人姿态解算方法 |
| CN108759838A (zh) * | 2018-05-23 | 2018-11-06 | 安徽科技学院 | 基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法 |
| CN109597864A (zh) * | 2018-11-13 | 2019-04-09 | 华中科技大学 | 椭球边界卡尔曼滤波的即时定位与地图构建方法及*** |
| CN110490273A (zh) * | 2019-09-12 | 2019-11-22 | 河南牧业经济学院 | 噪声方差不精确建模的多传感器***融合滤波算法 |
| CN110865334A (zh) * | 2019-11-26 | 2020-03-06 | 北京航空航天大学 | 一种基于噪声统计特性的多传感器目标跟踪方法及*** |
| CN111983926A (zh) * | 2020-08-31 | 2020-11-24 | 郑州轻工业大学 | 一种最大协熵扩展椭球集员滤波方法 |
| CN113138367A (zh) * | 2020-01-20 | 2021-07-20 | 中国科学院上海微***与信息技术研究所 | 一种目标定位方法、装置、电子设备及存储介质 |
| CN114179799A (zh) * | 2021-12-23 | 2022-03-15 | 南京理工大学 | 用于acc与aeb的融合多传感器信息的自车行驶路径识别方法 |
| CN114995403A (zh) * | 2022-05-20 | 2022-09-02 | 燕山大学 | 相关噪声及非高斯干扰下轮式移动机器人轨迹跟踪方法 |
| KR20220160881A (ko) * | 2021-05-28 | 2022-12-06 | 울산대학교 산학협력단 | 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140032167A1 (en) * | 2011-04-01 | 2014-01-30 | Physical Sciences, Inc. | Multisensor Management and Data Fusion via Parallelized Multivariate Filters |
| US12030660B2 (en) * | 2020-05-29 | 2024-07-09 | Embry-Riddle Aeronautical University, Inc. | Systems and methods for supressing noise from an aircraft engine |
-
2023
- 2023-07-05 CN CN202310814337.5A patent/CN116518983B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108318038A (zh) * | 2018-01-26 | 2018-07-24 | 南京航空航天大学 | 一种四元数高斯粒子滤波移动机器人姿态解算方法 |
| CN108759838A (zh) * | 2018-05-23 | 2018-11-06 | 安徽科技学院 | 基于秩卡尔曼滤波器的移动机器人多传感器信息融合方法 |
| CN109597864A (zh) * | 2018-11-13 | 2019-04-09 | 华中科技大学 | 椭球边界卡尔曼滤波的即时定位与地图构建方法及*** |
| CN110490273A (zh) * | 2019-09-12 | 2019-11-22 | 河南牧业经济学院 | 噪声方差不精确建模的多传感器***融合滤波算法 |
| CN110865334A (zh) * | 2019-11-26 | 2020-03-06 | 北京航空航天大学 | 一种基于噪声统计特性的多传感器目标跟踪方法及*** |
| CN113138367A (zh) * | 2020-01-20 | 2021-07-20 | 中国科学院上海微***与信息技术研究所 | 一种目标定位方法、装置、电子设备及存储介质 |
| CN111983926A (zh) * | 2020-08-31 | 2020-11-24 | 郑州轻工业大学 | 一种最大协熵扩展椭球集员滤波方法 |
| KR20220160881A (ko) * | 2021-05-28 | 2022-12-06 | 울산대학교 산학협력단 | 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법 |
| CN114179799A (zh) * | 2021-12-23 | 2022-03-15 | 南京理工大学 | 用于acc与aeb的融合多传感器信息的自车行驶路径识别方法 |
| CN114995403A (zh) * | 2022-05-20 | 2022-09-02 | 燕山大学 | 相关噪声及非高斯干扰下轮式移动机器人轨迹跟踪方法 |
Non-Patent Citations (4)
| Title |
|---|
| Alper Yilmaz ; Ashish Gupta.Indoor positioning using visual and inertial sensors.2016 IEEE SENSORS.2017,全文. * |
| 一种优化的扩展定界椭球集员滤波算法;何青;夏愉乐;柳一琳;费宇泉;;长沙理工大学学报(自然科学版);13(03);全文 * |
| 自校正分布式观测融合Kalman滤波器;邓自立;郝钢;;电子与信息学报;29(08);全文 * |
| 针对多目标的分布式多边融合定位;张利军, 杨波, 苏俊琦, 梁宇倩;计算机***应用;第30卷(第12期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116518983A (zh) | 2023-08-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Nazarahari et al. | 40 years of sensor fusion for orientation tracking via magnetic and inertial measurement units: Methods, lessons learned, and future challenges | |
| Shen et al. | Novel variable structure measurement system with intelligent components for flight vehicles | |
| CN109163720A (zh) | 基于渐消记忆指数加权的卡尔曼滤波跟踪方法 | |
| CN107228674B (zh) | 一种针对星敏感器和陀螺联合滤波的改进方法 | |
| CN108871341B (zh) | 一种全局优化的并发定位与建图方法 | |
| JP2014185996A (ja) | 計測装置 | |
| US20230333518A1 (en) | Adjustment system, adjustment method, and adjustment program | |
| CN115143954A (zh) | 一种基于多源信息融合的无人车导航方法 | |
| Zhang et al. | Correction adaptive square-root cubature Kalman filter with application to autonomous vehicle target tracking | |
| Ansari-Rad et al. | Pseudo DVL reconstruction by an evolutionary TS-fuzzy algorithm for ocean vehicles | |
| CN113218389B (zh) | 一种车辆定位方法、装置、存储介质及计算机程序产品 | |
| CN117268381B (zh) | 一种航天器状态的判断方法 | |
| CN116518983B (zh) | 用于移动机器人定位的自适应融合方法及装置 | |
| Jing et al. | Process noise identification based particle filter: an efficient method to track highly manoeuvring targets | |
| Ruoyu et al. | Modeling of MEMS gyro drift based on wavelet threshold denoising and improved Elman neural network | |
| CN110160530B (zh) | 一种基于四元数的航天器姿态滤波方法 | |
| CN116608859A (zh) | 一种基于阈值处理的自适应无迹卡尔曼滤波的导航方法、存储介质和设备 | |
| CN115950432A (zh) | 一种室内外移动机器人的无缝定位方法、设备及存储介质 | |
| JP2008008691A (ja) | 追尾処理装置及びその方法並びにプログラム | |
| CN112990256A (zh) | 多传感器噪声计算方法及信息融合方法、存储介质及设备 | |
| CN114636418A (zh) | 一种基于mems测量姿态相对变化的方法、装置及电子设备 | |
| Valera et al. | Design and implementation of Kalman filters applied to Lego NXT based robots | |
| Thalagala | Comparison of state marginalization techniques in visual inertial navigation filters | |
| Huang et al. | Bayesian Real-Time System Identification: From Centralized to Distributed Approach | |
| Jiang et al. | Multiple faults detection algorithm based on REKF for GNSS/INS integrated navigation |
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 |