CN115060260A - 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 - Google Patents
一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 Download PDFInfo
- Publication number
- CN115060260A CN115060260A CN202210160522.2A CN202210160522A CN115060260A CN 115060260 A CN115060260 A CN 115060260A CN 202210160522 A CN202210160522 A CN 202210160522A CN 115060260 A CN115060260 A CN 115060260A
- Authority
- CN
- China
- Prior art keywords
- anfis
- imu
- coordinate system
- wio
- time
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 44
- 230000004927 fusion Effects 0.000 title claims abstract description 32
- 238000005259 measurement Methods 0.000 claims abstract description 105
- 239000011159 matrix material Substances 0.000 claims abstract description 47
- 230000033001 locomotion Effects 0.000 claims abstract description 38
- 230000003044 adaptive effect Effects 0.000 claims abstract description 28
- 230000000007 visual effect Effects 0.000 claims abstract description 19
- 230000001537 neural effect Effects 0.000 claims abstract description 14
- 238000001914 filtration Methods 0.000 claims abstract description 7
- 230000008859 change Effects 0.000 claims description 46
- 238000012549 training Methods 0.000 claims description 42
- 238000006073 displacement reaction Methods 0.000 claims description 26
- 238000005070 sampling Methods 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000013461 design Methods 0.000 claims description 7
- 238000005457 optimization Methods 0.000 claims description 7
- 206010034719 Personality change Diseases 0.000 claims description 6
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 4
- 125000004432 carbon atom Chemical group C* 0.000 claims description 4
- 238000012360 testing method Methods 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- 230000009012 visual motion Effects 0.000 claims description 4
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000012937 correction Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000001052 transient effect Effects 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000005856 abnormality Effects 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
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
- 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
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
本发明公开了一种基于ANFIS‑ESKF的地面移动机器人多传感器融合定位方法。该方法包括:建立轮式‑IMU里程计(WIO)的测量模型,推导***的状态方程;建立视觉里程计(VO)的测量模型,推导***的观测方程;用ESKF对WIO和VO的运动估计信息进行融合;建立自适应神经模糊推理***ANFIS(Adaptive Neural Fuzzy Inference System),根据WIO和VO的直接信息,推断里程计的运动状态,实时调整误差状态卡尔曼滤波ESKF(Error State Kalman Filter)中预测模型和测量模型的协方差矩阵。本发明与现有技术相比,能够根据***的状态实时调整ESKF参数,定位精度更高,鲁棒性更强。
Description
技术领域
本发明属于导航技术领域,特别是一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法。
背景技术
里程计***使用本体传感器来估计车辆相对于起始点的瞬时位置和方向,这是自动导航***中的一个关键模块,可以在外部传感器不可用时提供位置信息。常用的机载传感器包括车轮编码器、惯性测量单元(IMU)、摄像机和激光雷达。使用单个传感器的里程计的性能受传感器特性、环境干扰和车辆状态的影响,通常不能提供稳定和满意的定位结果。
多传感器融合技术可以融合多个传感器的信息得到稳定的定位结果。应用最广泛的传感器融合方法是卡尔曼滤波器(KF)的变种,如扩展卡尔曼滤波器(EKF)和无迹卡尔曼滤波器(UKF)。如国内专利CN201310455813.5《GPS盲区下融合多源信息的车辆高精度定位方法及装置》,该方法使用陀螺仪输出的角速率信息和加速度信息、路面图像信息输入到卡尔曼滤波器得到最终融合信息、再由道路旁边设置的锚节点重新矫正融合算法产生的累计误差,可以提供稳定、可靠、高精度的车辆位置信息。如专利CN201310369354.9《一种面向室内移动机器人的光流场视觉/INS组合导航方法》中所述,利用光流场视觉和IMU等信息输入到扩展卡尔曼滤波器进行融合计算,能够提高室内移动机器人的导航精度。
尽管卡尔曼滤波器及其变种在某些应用中被证明是有效的,但它们有一个共同的假设,对于实际的工作场景可能是有问题的——不变协方差。在现实中,测量噪声不确定,模型误差存在,车辆经历暂时不可预测的运动变化,环境局部改变。这种变化的条件需要适当处理,否则定位性能将严重降低。以往的误差状态卡尔曼滤波(ESKF)方法在预测模型和测量模型中都不能解决具有不同不确定性的传感器融合问题。基于ANFIS-ESKF(AdaptiveNetwork-Based Fuzzy Inference System–Error State KalmanFilter,自适应网络模糊推理***-误差状态卡尔曼滤波)方法可以对***的瞬时变化做出响应,并且在融合算法中可以独立确定两个协方差矩阵,从而提高定位***的鲁棒性。
发明内容
为了解决现有技术问题,本发明的目的在于克服已有技术存在的卡尔曼滤波及其变种模型在多传感器融合技术中不足,提供一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法,能够在线检测异常工况并且调整融合增益的基于ANFIS-ESKF的地面移动机器人多传感器融合定位,能够解决现有卡尔曼滤波技术的弊端,从而实现对多传感器融合的瞬时变化做出响应,并确定独立确定两个协方差矩阵,提高定位***的鲁棒性。
为达到上述发明创造目的,本发明采用如下技术方案:
一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法,包括模块1、模块2、模块3和模块4共四个计算模块,其特征在于:
所述模块1,建立轮式-IMU里程计(WIO)的测量模型,推导***的状态方程;
所述模块2,建立视觉里程计(VO)的测量模型,推导***的观测方程;
所述模块3,用误差状态卡尔曼滤波(ESKF)对WIO和VO的运动估计信息进行融合;
所述模块4,建立自适应模糊神经推理***(ANFIS),根据WIO和VO的直接信息,推断里程计的运动状态,实时调整ESKF中预测模型和测量模型的协方差矩阵;该模块在使用前需要根据GPS数据对ANFIS进行训练;
所述模块1的输入是轮式里程计数据和IMU数据,输出是相邻时刻IMU坐标系的位姿估计;模块2的输入是双目相机拍摄的照片,输出是相邻时刻相机坐标系的位姿估计;模块3的输入是模块1和模块2的输出,输出是相邻时刻IMU坐标系的位姿估计;模块4的输入模块1中WIO和模块2中VO的中间数据,输出是模块3中需要使用的WIO和VO的协方差矩阵;
整个定位程序的输入是IMU数据、轮式里程计数据、相机数据,输出是IMU坐标系的位姿变化。
优选地,所述模块1中建立WIO测量模型,具体如下:
IMU包括三轴陀螺仪和三轴加速度计,考虑到测试***加速度计中的噪声极大地影响了位移估计,选择仅使用陀螺仪来估计方向变化;作为补偿,引入车轮编码器进行平移运动计算,并与陀螺仪集成,融合为轮式-IMU里程计WIO,提供完整的六自由度运动估计;WIO为ESKF***提供移动机器人运动状态的预测;
下面的式子(1)展示了i+1时刻WIO测得的IMU坐标系{I}的位置变化:
其中是i+1时刻测得的IMU坐标系的姿态变化,是轮式里程计坐标系{W}相对于IMU坐标系的位置标定关系,是轮式里程计坐标系相对于IMU坐标系的姿态标定关系,是i+1时刻测得的轮式里程计坐标系的位置变化,I是单位矩阵。
优选地,所述模块1中建立***状态方程,具体如下:
定义***的状态为:
那么根据WIO测量模型得到***的名义状态方程为:
其中是i时刻轮式里程计的测量值,是i时刻IMU坐标系相对于0时刻的姿态,是i时刻IMU测得的角速度,是i时刻IMU的角速度偏移,Δt是IMU的采样周期,[γi]×指的是γi对应的反对称矩阵;pi+1表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的位置,表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的姿态对时间的导数,表示i+1时刻IMU角速度偏移对时间的导数;
误差状态方程为:
优选地,所述模块2中建立视觉里程计(VO)的测量模型,推导***的观测方程,具体如下:
是相机观测到的从第j-1帧到第j帧的位移,它和相等;是的从时间j-1到时间j的旋转向量,和等价;ΔpC,n是随机位移误差,服从高斯分布ΔpC,n~N(0,V1);ΔθC,n是随机的姿态误差,服从高斯分布ΔθC,n~N(0,V2);表示第j-1帧到第j帧的相机位移变化的测量值;表示从时间j-1到时间j的相机姿态变化的测量值;
进一步地,所述模块2中建立***的观测方程,具体如下:
优选地,在所述模块4中,建立自适应模糊神经推理***ANFIS,根据WIO和VO的直接信息,推断里程计的运动状态,实时调整ESKF中预测模型和测量模型的协方差矩阵,具体步骤如下:
采用两个ANFIS,第一个ANFIS记作WIO-ANFIS,其输入的是移动机器人的角速度ω和左右车轮的转速差ρ,输出是WIO的协方差矩阵Q1;第二个ANFIS记作VO-ANFIS,其输入的是视觉里程计的特征点个数Nu和重投影误差Er,输出是VO的协方差矩阵V;
定义WIO-ANFIS的输入为Xw=(ω,ρ)T,定义它的模糊集为A;为A中的每一个成员Aa设计一个隶属度函数,并且记作Amf-a(xw)∈[0,1];Amf-a(xw)越接近1,Xw属于Aa的可能性越大;VO-ANFIS的输入是特征点数量Nu和重投影误差Er记作xv=(Nu,Er)T,定义它的模糊集为B;构建隶属度函数Bmf-b(xν)的方法与Amf-a(xw)类似;
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡;曲线形状通过改变函数中的自适应参数进行调整;选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量;
在A中为ρ定义了四个成员,它们分别趋向于+∞(A1),趋向于1(A2),趋向于-1(A3),趋向于-∞(A4);它们分别对应于移动机器人滑转、直走、旋转和滑转;A中的ω有两个成员:小(A5)和大(A6);它们分别对应着慢转弯和快转弯的运动情况;同样,B中Er的模糊集分别为小(B1)和大(B2),Nu的模糊集分别为小的(B3)和大的(B4);
隶属度函数定义了输入与其对应的模糊成员之间的关系,在每个隶属函数中,有一两个变量使其可调整,以更好地适应真实的***情况;表示A和B中的自适应变量分别为AW和Bv;AW1指定了ρ完全属于A1时初始值;AW2指定了ρ属于A2的终点;AW3指定了ρ开始属于A3的点,AW4指定了ρ完全属于A4时的最大值;函数的另一端固定在临界点±1,简化了复杂度,降低了ANFIS训练中需要优化的变量的维数;AW5,AW6,AW7,AW8,指定了ω的隶属度函数的临界点;Bv1,Bv2,Bv3,Bv4指定了Er的隶属度函数的临界点;Bv5,Bv6,Bv7,Bv8指定了Nu的隶属度函数的临界点;曲线形状的控制方式同AW1,AW2,AW3,AW4;
模糊规则库定义了模糊集与输出之间的逻辑关系;根据不同的输入条件,对WIO和VO的测量方差进行了推理;
对于WIO-ANFIS,当输入组合为(A3,A6)和(A5,A2)时,方差为低,用WL表示;物理的解释是,当车辆正在急转弯(A3),并且IMU测量表明同样的情况(A6),或移动机器人运动中没有大旋转(A2)并且IMU测量表明同样的情况(A5),移动机器人运动时可能没有打滑,从而WIO的方差应该比较小;否则,IMU的方差应该比较大,用WH表示;
对于每一条规则,例如πw7(A3,A6),它的输出定义如式子(9)所示;根据同样的规则,得到πwa的输出,用GaW表示;
G7W=min(Amf-6(xw),Amf-3(xw)) (9)
类似地,也为VO-ANFIS建立了规则库;为视觉的运动估计定义了高中低三个方差,分别用VH,VM,VL,表示,相应的模糊规则的输出用GbC表示;
根据式子(10)用GaW来计算WIO-ANFIS的归一化推理结果类似地,得到VO-ANFIS的a表示WIO-ANFIS中模糊规则的编号,b表示VO-ANFIS中模糊规则的编号,na表示WIO-ANFIS中的模糊规则总数;
根据各种工况的具体测量方差VH,VM,VL,WH WL,则WIO的方差Q1和VO的方差V的最终结果由式子(11)得到:
上式中Wπwa表示规则πwa对应的权重,Vπvb表示规则πvb对应的权重,a表示WIO-ANFIS中模糊规则的编号,b表示VO-ANFIS中模糊规则的编号。
进一步优选地,在所述模块4中,模糊集与相应***情况的关系如下表所示:
在本发明中,模糊推理中隶属函数的参数也可根据经验和直觉来选择;而为了实现ANFIS***的适应性,需要对里程计的测量有一个真值;它是为车辆提供无漂移定位的GPS单元;利用GPS测量值作为训练集的真值的难点在于GPS与IMU的位置不一致;这种偏差很大,特别是在车辆急转弯时,会引入训练数据的误差,导致参数优化错误;因此,将GPS的直接测量转化到IMU坐标系{I}中,以提供准确的训练数据;GPS接收机在IMU坐标系{I}中的坐标可从***设计参数中导出,但对于训练目的来说不够精确,因此在使用它进一步训练ANFIS参数之前有必要对其进行优化。
优选地,在所述模块4中,建立自适应模糊神经推理***(ANFIS)的训练方法,具体步骤如下:
将GPS的直接测量转化到IMU坐标系{I}中,以提供准确的训练数据;GPS接收机在坐标系{I0}中的位置可表示为:
其中,IXG是GPS接收机在IMU坐标系{I}中的坐标,是在时间i时,坐标系{I}相对于坐标系{I0}的旋转矩阵;是时间i时GPS路径点在GPS测量坐标系{GM}中的表示,是坐标系{GM}和坐标系{I0}之间的旋转矩阵;
相应地,定义相对于GPS测量的估计轨迹误差为:
IXG从***设计参数中导出,在使用它进一步训练ANFIS参数之前,有必要对其进行优化;对于优化后的GPS相对于坐标系{I}的位置估计,可根据求式子(14)的解得到;
其中i表示时间戳的索引,ni表示时间戳的总数,s表示训练轨迹的索引,ns表示训练轨迹的总数;
需要优化的参数包括Aw,Bv和方差WH,WL,VH,VM,VL,用式(15)表示:
xANFIS=[AW,Bv,WH,WL,VH,VM,VL] (15)
本发明与现有技术相比较,具有如下显而易见的突出实质性特点和显著优点:
1.本发明经过训练的ANFIS***能够根据地面移动机器人***的状态,比如机器人是否打滑或者环境中的纹理信息是否足够多等,来实时调整ESKF的参数,与传统的固定增益滤波器相比,大大减少了内部故障和外部干扰造成的误差,使得地面移动机器人在各种复杂工况下都能够保持较高的定位精度,鲁棒性更强;
2.采用本发明方法,在移动机器人上对算法进行验证,结果表明,与固定增益ESKF相比,基于ANFIS的ESKF的平均轨迹误差降低了80.6%,即使有意引入外部干扰,其平均位置漂移也在1%以下。
附图说明
图1为本发明提供的基于ANFIS-ESKF的移动机器人多传感器融合定位方法的原理图。
图2为本发明优选实施例提供的ANFIS框架,其中(a)为WIO的协方差推算的ANFIS框架,(b)为VO的协方差推算的ANFIS框架。
图3为本发明优选实施例提供的车轮里程计在一个采样周期内的航位推算方法。
图4为本发明优选实施例的ANFIS的隶属度函数。
具体实施方式
以下结合具体的实施例子对上述方案做进一步说明,本发明的优选实施例详述如下:
实施例一:
在本实施例中,参见图1,一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法,包括模块1、模块2、模块3和模块4共四个计算模块;
所述模块1,建立轮式-IMU里程计(WIO)的测量模型,推导***的状态方程;
所述模块2,建立视觉里程计(VO)的测量模型,推导***的观测方程;
所述模块3,用误差状态卡尔曼滤波(ESKF)对WIO和VO的运动估计信息进行融合;
所述模块4,建立自适应模糊神经推理***(ANFIS),根据WIO和VO的直接信息,推断里程计的运动状态,实时调整ESKF中预测模型和测量模型的协方差矩阵;该模块在使用前需要根据GPS数据对ANFIS进行训练;
所述模块1的输入是轮式里程计数据和IMU数据,输出是相邻时刻IMU坐标系的位姿估计;模块2的输入是双目相机拍摄的照片,输出是相邻时刻相机坐标系的位姿估计;模块3的输入是模块1和模块2的输出,输出是相邻时刻IMU坐标系的位姿估计;模块4的输入是模块1中WIO和模块2中VO的中间数据,输出是模块3中需要使用的WIO和VO的协方差矩阵;
整个定位程序的输入是IMU数据、轮式里程计数据、相机数据,输出是IMU坐标系的位姿变化。
本实施例能够在线检测异常工况并且调整融合增益实现基于ANFIS-ESKF的地面移动机器人多传感器融合定位,能够实现对多传感器融合的瞬时变化做出响应。
实施例二:
本实施例与实施例一基本相同,特别之处在于:
在本实施例中,一种基于ANFIS-ESKF的移动机器人多传感器融合定位方法,包括以下模块:
模块1,建立轮式-IMU里程计(WIO)的测量模型,推导***的状态方程;
模块2,建立视觉里程计(VO)的测量模型,推导***的观测方程;
模块3,用ESKF对WIO和VO的运动估计信息进行融合;
模块4,建立ANFIS***,根据WIO和VO的直接信息,推断里程计的运动状态,实时调整ESKF中预测模型和测量模型的协方差矩阵;
所述模块1中建立WIO测量模型,具体如下:
IMU包括三轴陀螺仪和三轴加速度计,考虑到测试***加速度计中的噪声极大地影响了位移估计,选择仅使用陀螺仪来估计方向变化;作为补偿,引入车轮编码器进行平移运动计算,并与陀螺仪集成,融合为轮式-IMU里程计WIO,提供完整的六自由度运动估计;WIO为ESKF***提供移动机器人运动状态的预测;
下面的式子(1)展示了i+1时刻WIO测得的IMU坐标系{I}的位置变化:
其中是i+1时刻测得的IMU坐标系的姿态变化,是轮式里程计坐标系{W}相对于IMU坐标系的位置标定关系,是轮式里程计坐标系相对于IMU坐标系的姿态标定关系,是i+1时刻测得的轮式里程计坐标系的位置变化,I是单位矩阵。
优选地,所述模块1中建立***状态方程,具体如下:
定义***的状态为:
那么根据WIO测量模型得到***的名义状态方程为:
其中是i时刻轮式里程计的测量值,是i时刻IMU坐标系相对于0时刻的姿态,是i时刻IMU测得的角速度,是i时刻IMU的角速度偏移,Δt是IMU的采样周期,[γi]×指的是γi对应的反对称矩阵;pi+1表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的位置,表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的姿态对时间的导数,表示i+1时刻IMU角速度偏移对时间的导数;
误差状态方程为:
在本实施例中,所述模块2中建立视觉里程计(VO)的测量模型,推导***的观测方程,具体如下:
是相机观测到的从第j-1帧到第j帧的位移,它和相等;是的从时间j-1到时间j的旋转向量,和等价;ΔPC,n是随机位移误差,服从高斯分布ΔpC,n~N(0,V1);ΔθC,n是随机的姿态误差,服从高斯分布ΔθC,n~N(0,V2);表示第j-1帧到第j帧的相机位移变化的测量值;表示从时间j-1到时间j的相机姿态变化的测量值;
在本实施例中,所述模块2中建立***的观测方程,具体如下:
优选地,在所述模块4中,建立自适应模糊神经推理***ANFIS,根据WIO和VO的直接信息,推断里程计的运动状态,实时调整ESKF中预测模型和测量模型的协方差矩阵,具体步骤如下:
采用两个ANFIS,第一个ANFIS记作WIO-ANFIS,其输入的是移动机器人的角速度ω和左右车轮的转速差ρ,输出是WIO的协方差矩阵Q1;第二个ANFIS记作VO-ANFIS,其输入的是视觉里程计的特征点个数Nu和重投影误差Er,输出是VO的协方差矩阵V;
定义WIO-ANFIS的输入为xw=(ω,ρ)T,定义它的模糊集为A;为A中的每一个成员Aa设计一个隶属度函数,并且记作Amf-a(xw)∈[0,1];Amf-a(xw)越接近1,Xw属于Aa的可能性越大;VO-ANFIS的输入是特征点数量Nu和重投影误差Er记作xv=(Nu,Er)T,定义它的模糊集为B;构建隶属度函数Bmf-b(xν)的方法与Amf-a(xw)类似;
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡;曲线形状通过改变函数中的自适应参数进行调整;选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量;
在A中为ρ定义了四个成员,它们分别趋向于+∞(A1),趋向于1(A2),趋向于-1(A3),趋向于-∞(A4);它们分别对应于移动机器人滑转、直走、旋转和滑转;A中的ω有两个成员:小(A5)和大(A6);它们分别对应着慢转弯和快转弯的运动情况;同样,B中Er的模糊集分别为小(B1)和大(B2),Nu的模糊集分别为小的(B3)和大的(B4);
隶属度函数定义了输入与其对应的模糊成员之间的关系,在每个隶属函数中,有一两个变量使其可调整,以更好地适应真实的***情况;表示A和B中的自适应变量分别为AW和Bv;AW1指定了ρ完全属于A1时初始值;AW2指定了ρ属于A2的终点;AW3指定了ρ开始属于A3的点,AW4指定了ρ完全属于A4时的最大值;函数的另一端固定在临界点±1,简化了复杂度,降低了ANFIS训练中需要优化的变量的维数;AW5,AW6,AW7,AW8,指定了ω的隶属度函数的临界点;Bv1,Bv2,Bv3,Bv4指定了Er的隶属度函数的临界点;Bv5,Bv6,Bv7,Bv8指定了Nu的隶属度函数的临界点;曲线形状的控制方式同AW1,AW2,AW3,AW4;
模糊规则库定义了模糊集与输出之间的逻辑关系;根据不同的输入条件,对WIO和VO的测量方差进行了推理;
对于WIO-ANFIS,当输入组合为(A3,A6)和(A5,A2)时,方差为低,用WL表示;物理的解释是,当车辆正在急转弯(A3),并且IMU测量表明同样的情况(A6),或移动机器人运动中没有大旋转(A2)并且IMU测量表明同样的情况(A5),移动机器人运动时可能没有打滑,从而WIO的方差应该比较小;否则,IMU的方差应该比较大,用WH表示;
对于每一条规则,例如πw7(A3,A6),它的输出定义如式子(9)所示;根据同样的规则,得到πwa的输出,用GaW表示;
G7W=min(Amf-6(xw),Amf-3(xw)) (9)
类似地,也为VO-ANFIS建立了规则库;为视觉的运动估计定义了高中低三个方差,分别用VH,VM,VL,表示,相应的模糊规则的输出用GbC表示;
根据式子(10)用GaW来计算WIO-ANFIS的归一化推理结果类似地,得到VO-ANFIS的a表示WIO-ANFIS中模糊规则的编号,b表示VO-ANFIS中模糊规则的编号,na表示WIO-ANFIS中的模糊规则总数;
根据各种工况的具体测量方差VH,VM,VL,WH WL,则WIO的方差Q1和VO的方差V的最终结果由式子(11)得到:
上式中,Wπwa表示规则πwa对应的权重,Vπvb表示规则πvb对应的权重,a表示WIO-ANFIS中模糊规则的编号,b表示VO-ANFIS中模糊规则的编号。
在本实施例中,在所述模块4中,模糊集与相应***情况的关系如下表所示:
在本实施例中,模糊推理中隶属函数的参数也可根据经验和直觉来选择;而为了实现ANFIS***的适应性,需要对里程计的测量有一个真值;它是为车辆提供无漂移定位的GPS单元;利用GPS测量值作为训练集的真值的难点在于GPS与IMU的位置不一致;这种偏差很大,特别是在车辆急转弯时,会引入训练数据的误差,导致参数优化错误;因此,将GPS的直接测量转化到IMU坐标系{I}中,以提供准确的训练数据;GPS接收机在IMU坐标系{I}中的坐标可从***设计参数中导出,但对于训练目的来说不够精确,因此在使用它进一步训练ANFIS参数之前有必要对其进行优化。
在本实施例中,在所述模块4中,建立自适应模糊神经推理***(ANFIS)的训练方法,具体步骤如下:
将GPS的直接测量转化到IMU坐标系{I}中,以提供准确的训练数据;GPS接收机在坐标系{I0}中的位置可表示为:
其中,IXG是GPS接收机在IMU坐标系{I}中的坐标,是在时间i时,坐标系{I}相对于坐标系{I0}的旋转矩阵;是时间i时GPS路径点在GPS测量坐标系{GM}中的表示,是坐标系{GM}和坐标系{I0}之间的旋转矩阵;
相应地,定义相对于GPS测量的估计轨迹误差为:
IXG从***设计参数中导出,在使用它进一步训练ANFIS参数之前,有必要对其进行优化;对于优化后的GPS相对于坐标系{I}的位置估计,可根据求式子(14)的解得到;
其中i表示时间戳的索引,ni表示时间戳的总数,s表示训练轨迹的索引,ns表示训练轨迹的总数;
需要优化的参数包括AW,Bv和方差WH,WL,VH,VM,VL,用式(15)表示:
XANFIS=[AW,Bv,WH,WL,VH,VM,VL] (15)
本实施例基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法建立轮式-IMU里程计(WIO)的测量模型,推导***的状态方程;建立视觉里程计(VO)的测量模型,推导***的观测方程;用ESKF对WIO和VO的运动估计信息进行融合;建立ANFIS(AdaptiveNeural Fuzzy Inference System),根据WIO和VO的直接信息,推断里程计的运动状态,实时调整ESKF(Error State Kalman Filter)中预测模型和测量模型的协方差矩阵。本实施例方法经过训练的ANFIS***能够根据地面移动机器人***的状态,比如机器人是否打滑或者环境中的纹理信息是否足够多等,来实时调整ESKF的参数,与传统的固定增益滤波器相比,能够根据***的状态实时调整ESKF参数,大大减少了内部故障和外部干扰造成的误差,使得地面移动机器人在各种复杂工况下都能够保持较高的定位精度,定位精度更高,鲁棒性更强。
实施例三:
本实施例与实施例一基本相同,特别之处在于:
参见图1-图4,在本实施例中,所述模块1中建立WIO测量模型,具体如下:
IMU包括三轴陀螺仪和三轴加速度计,考虑到测试***加速度计中的噪声极大地影响了位移估计,选择仅使用陀螺仪来估计方向变化;作为补偿,引入车轮编码器进行平移运动计算,并与陀螺仪集成,融合为轮式-IMU里程计WIO,提供完整的六自由度运动估计;WIO为ESKF***提供移动机器人运动状态的预测;
图3展示了四轮移动机器人运动学模型的推导,假设左侧车轮速度相同,右侧车轮的速度也相同,αL和αR表示在一个采样周期内,左右车轮的旋转角度;假设路面平滑,车轮不打滑,机器人在采样周期内以圆心O1为中心从点O移动到点M;左右车轮对应的行驶距离分别为ΔSL和ΔSR;式子(17)中ΔSL和ΔSR的原始测量值中的噪声ΔSn服从高斯分布轮式里程计坐标系{W}的原点位于车轮的几何中心,因此机器人在此采样时间内的行走距离ΔS可以由式子(17)得到;
其中Rw是车轮的半径;
ΔS=(ΔSR+ΔSL)/2 (18)
IMU的三轴陀螺仪测量移动机器人的角速度;为了实现ESKF,采用了姿态积分误差测量模型;
其中ωt是由三个轴合成的真实角速度,ωm是测量的角速度,ωn是服从高斯分布的随机游走,ωbt是角速度偏移的真实值,ωb是角速度偏移的名义值,δωb是角速度偏移的误差,ωbt的导数服从高斯分布ωw表示角速度偏移的噪声,表示角速度偏移的误差的导数;
在IMU坐标系{I}中测得的角速度矢量可以表示为:
IMU在第i+1个采样周期内的姿态可以由角速度的迭代积分得到;
WIO相对于轮式里程计坐标系{W}的瞬时位移为:
它可通过式子(1)转换到坐标系{I}中表示,下面的式子(1)展示了i+1时刻WIO测得的IMU坐标系{I}的位置变化:
在第i+1个采样时间处移动机器人相对于初始坐标系{I0}的位置可以通过式子(25)计算:
其中是对应于的旋转矩阵,式子(22)和式子(22)是六自由度WIO的位姿估计输出的迭代形式;其中是i+1时刻测得的IMU坐标系的姿态变化,是轮式里程计坐标系{W}相对于IMU坐标系的位置标定关系,是轮式里程计坐标系相对于IMU坐标系的姿态标定关系,是i+1时刻测得的轮式里程计坐标系的位置变化,I是单位矩阵。
在本实施例中,所述模块1中建立***状态方程,具体如下:
定义***的状态为:
那么根据WIO测量模型得到***的名义状态方程为:
其中是i时刻轮式里程计的测量值,是i时刻IMU坐标系相对于0时刻的姿态,是i时刻IMU测得的角速度,是i时刻IMU的角速度偏移,Δt是IMU的采样周期,[γi]×指的是γi对应的反对称矩阵;pi+1表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的位置,表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的姿态对时间的导数,表示i+1时刻IMU角速度偏移对时间的导数;
误差状态方程为:
在本实施例中,所述模块2中建立视觉里程计(VO)的测量模型,推导***的观测方程,具体如下:
是相机观测到的从第j-1帧到第j帧的位移,它和相等;是的从时间j-1到时间j的旋转向量,和等价;ΔpC,n是随机位移误差,服从高斯分布ΔpC,n~N(0,V1);ΔθC,n是随机的姿态误差,服从高斯分布ΔθC,n~N(0,V2);表示第j-1帧到第j帧的相机位移变化的测量值;表示从时间j-1到时间j的相机姿态变化的测量值;
在本实施例中,所述模块2中建立***的观测方程,具体如下:
进一步地,模块3所述用ESKF对WIO和VO的运动估计信息进行融合,具体如下:
ESKF分为预测和校正两个阶段;
ESKF的预测过程具体如下:
根据卡尔曼滤波模型,误差状态δxi+1和误差协方差矩阵P由下列式子更新:
上式中,表示的扰动;Fi表示误差状态方程相对于扰动的雅可比矩阵;Fx表示误差状态方程相对于误差状态的雅可比矩阵;Fi T表示Fi的转置;Qi表示扰动的协方差矩阵;Fx T表示Fx的转置;Pi表示i时刻误差状态的协方差矩阵,Pi+1表示i+1时刻误差状态的协方差矩阵;
式子(4)的结果给出了误差状态模型的线性表示,式子(26)中的矩阵Fx和矩阵Fi为:
上式中,I表示单位矩阵;Δt表示IMU采样周期;
另外,随机误差的协方差矩阵Q被定义为:
Q1,Q2,Q3是相应的三个误差状态的测量噪声的方差,它们被定义为:
ESKF的校正过程具体如下:
其中,是输出矩阵;是视觉测量的误差协方差矩阵;Δxj表示***误差状态的预测值;表示***状态的观测值与预测值之间的误差;HT表示H的转置;Sj是卡尔曼增益的分母;Pj表示***误差状态的协方差的预测值;表示卡尔曼增益;表示***误差状态的修正值;
定义名义模型在第j帧输出的机器人位姿为:
其中,qj表示j时刻IMU坐标系相对于0时刻IMU坐标系的姿态的名义值;Xj表示j时刻IMU坐标系相对于0时刻IMU坐标系的位置的名义值;
在本实施例中,在所述模块4中,建立自适应模糊神经推理***ANFIS,根据WIO和VO的直接信息,推断里程计的运动状态,实时调整ESKF中预测模型和测量模型的协方差矩阵,具体步骤如下:
采用两个ANFIS,第一个ANFIS记作WIO-ANFIS,结构如图2(a)所示,其输入的是移动机器人的角速度ω和左右车轮的转速差ρ,输出是WIO的协方差矩阵Q1;第二个ANFIS记作VO-ANFIS,结构如图2(b)所示,其输入的是视觉里程计的特征点个数Nu和重投影误差Er,输出是VO的协方差矩阵V;
定义WIO-ANFIS的输入为Xw=(ω,ρ)T,定义它的模糊集为A;为A中的每一个成员Aa设计一个隶属度函数,并且记作Amf-a(xw)∈[0,1];Amf-a(xw)越接近1,Xw属于Aa的可能性越大;VO-ANFIS的输入是特征点数量Nu和重投影误差Er记作Xv=(Nu,Er)T,定义它的模糊集为B;构建隶属度函数Bmf-b(xν)的方法与Amf-a(xw)类似;
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡,其数学表达式和曲线形状见图4;曲线形状通过改变函数中的自适应参数进行调整;选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量;
在A中为ρ定义了四个成员,它们分别趋向于+∞(A1),趋向于1(A2),趋向于-1(A3),趋向于-∞(A4);它们分别对应于移动机器人滑转、直走、旋转和滑转;A中的ω有两个成员:小(A5)和大(A6);它们分别对应着慢转弯和快转弯的运动情况;同样,B中Er的模糊集分别为小(B1)和大(B2),Nu的模糊集分别为小的(B3)和大的(B4);
在本实施例中,在所述模块4中,模糊集与相应***情况的关系如表1所示:
表1.模糊集与工作情况的关系表
隶属度函数定义了输入与其对应的模糊成员之间的关系,如图4所示;在每个隶属函数中,有一两个变量使其可调整,以更好地适应真实的***情况;表示A和B中的自适应变量分别为AW和Bv;AW1指定了ρ完全属于A1时初始值;AW2指定了ρ属于A2的终点;AW3指定了ρ开始属于A3的点,AW4指定了ρ完全属于A4时的最大值;函数的另一端固定在临界点±1,简化了复杂度,降低了ANFIS训练中需要优化的变量的维数;AW5,AW6,AW7,AW8,指定了ω的隶属度函数的临界点;Bv1,Bv2,Bv3,Bv4指定了Er的隶属度函数的临界点;Bv5,Bv6,Bv7,Bv8指定了Nu的隶属度函数的临界点;曲线形状的控制方式同AW1,AW2,AW3,AW4;具体情况见图4;
模糊规则库定义了模糊集与输出之间的逻辑关系;根据不同的输入条件,对WIO和VO的测量方差进行了推理;建立的规则库以及每条规则对应的测量方差列于表2和表3中;
表2.WIO-ANFIS的规则库
ω | ρ | 规则库 | 方差 |
A<sub>6</sub> | A<sub>1</sub> | π<sub>w5</sub> | W<sub>H</sub> |
A<sub>6</sub> | A<sub>2</sub> | π<sub>w6</sub> | W<sub>H</sub> |
A<sub>6</sub> | A<sub>4</sub> | π<sub>w8</sub> | W<sub>H</sub> |
A<sub>5</sub> | A<sub>1</sub> | π<sub>w1</sub> | W<sub>H</sub> |
A<sub>5</sub> | A<sub>4</sub> | π<sub>w4</sub> | W<sub>H</sub> |
A<sub>6</sub> | A<sub>3</sub> | π<sub>w7</sub> | W<sub>L</sub> |
A<sub>5</sub> | A<sub>2</sub> | π<sub>w2</sub> | W<sub>L</sub> |
A<sub>5</sub> | A<sub>3</sub> | π<sub>w8</sub> | W<sub>H</sub> |
表3.VO-ANFIS的规则库
N<sub>u</sub> | E<sub>r</sub> | 规则库 | 方差 |
B<sub>2</sub> | B<sub>3</sub> | π<sub>v2</sub> | V<sub>L</sub> |
B<sub>2</sub> | B<sub>4</sub> | π<sub>v4</sub> | V<sub>H</sub> |
B<sub>1</sub> | B<sub>3</sub> | π<sub>v1</sub> | V<sub>M</sub> |
B<sub>1</sub> | B<sub>4</sub> | π<sub>v3</sub> | V<sub>H</sub> |
对于WIO-ANFIS,当输入组合为(A3,A6)和(A5,A2)时,方差为低,用WL表示;物理的解释是,当车辆正在急转弯(A3),并且IMU测量表明同样的情况(A6),或移动机器人运动中没有大旋转(A2)并且IMU测量表明同样的情况(A5),移动机器人运动时可能没有打滑,从而WIO的方差应该比较小;否则,IMU的方差应该比较大,用WH表示;
对于每一条规则,例如πw7(A3,A6),它的输出定义如式子(9)所示;根据同样的规则,得到πwa的输出,用Gaw表示;
G7W=min(Amf-6(xw),Amf-3(xw)) (9)
类似地,也为VO-ANFIS建立了规则库;为视觉的运动估计定义了高中低三个方差,分别用VH,VM,VL,表示,相应的模糊规则的输出用GbC表示;
根据式子(10)用GaW来计算WIO-ANFIS的归一化推理结果类似地,得到VO-ANFIS的a表示WIO-ANFIS中模糊规则的编号,b表示VO-ANFIS中模糊规则的编号,na表示W1O-ANFIS中的模糊规则总数;
根据各种工况的具体测量方差VH,VM,VL,WH WL,则WIO的方差Q1和VO的方差V的最终结果由式子(11)得到:
上式中,Wπwa表示规则πwa对应的权重,Vπvb表示规则πvb对应的权重,a表示WIO-ANFIS中模糊规则的编号,b表示VO-ANFIS中模糊规则的编号。
在本实施例中,模糊推理中隶属函数的参数也可根据经验和直觉来选择;而为了实现ANFIS***的适应性,需要对里程计的测量有一个真值;它是为车辆提供无漂移定位的GPS单元;利用GPS测量值作为训练集的真值的难点在于GPS与IMU的位置不一致;这种偏差很大,特别是在车辆急转弯时,会引入训练数据的误差,导致参数优化错误;因此,将GPS的直接测量转化到IMU坐标系{I}中,以提供准确的训练数据;GPS接收机在IMU坐标系{I}中的坐标可从***设计参数中导出,但对于训练目的来说不够精确,因此在使用它进一步训练ANFIS参数之前有必要对其进行优化。
在本实施例中,在所述模块4中,建立自适应模糊神经推理***(ANFIS)的训练方法,具体步骤如下:
将GPS的直接测量转化到IMU坐标系{I}中,以提供准确的训练数据;GPS接收机在坐标系{I0}中的位置可表示为:
其中,IXG是GPS接收机在IMU坐标系{I}中的坐标,是在时间i时,坐标系{I}相对于坐标系{I0}的旋转矩阵;是时间i时GPS路径点在GPS测量坐标系{GM}中的表示,是坐标系{GM}和坐标系{I0}之间的旋转矩阵;
相应地,定义相对于GPS测量的估计轨迹误差为:
IXG从***设计参数中导出,在使用它进一步训练ANFIS参数之前,有必要对其进行优化;对于优化后的GPS相对于坐标系{I}的位置估计,可根据求式子(14)的解得到;
其中i表示时间戳的索引,ni表示时间戳的总数,s表示训练轨迹的索引,ns表示训练轨迹的总数;
需要优化的参数包括AW,Bv和方差WH,WL,VH,VM,VL,用式(15)表示:
XANFIS=[AW,Bv,WH,WL,VH,VM,VL] (15)
本实施例方法经过训练的ANFIS***能够根据地面移动机器人***的状态,比如机器人是否打滑或者环境中的纹理信息是否足够多等,来实时调整ESKF的参数,与传统的固定增益滤波器相比,大大减少了内部故障和外部干扰造成的误差,使得地面移动机器人在各种复杂工况下都能够保持较高的定位精度,鲁棒性更强。采用本实施例方法,在移动机器人上对算法进行验证,结果表明,与固定增益ESKF相比,基于ANFIS的ESKF的平均轨迹误差降低了80.6%,即使有意引入外部干扰,其平均位置漂移也在1%以下。
本发明上述实施例基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法。ANFIS分别推断WIO和VO的方差,并将结果用于在线自适应调整ESKF的融合增益。由于模糊网络能够根据***当前状态识别***异常,因此估计算法能够即时调整融合参数,与传统的固定增益滤波器相比,上述实施例方法大大减少了内部故障和外部干扰造成的误差。与固定增益ESKF相比,基于ANFIS的ESKF平均轨迹误差要小很多,即使有意引入外部干扰,其平均位置漂移也能维持在很小的范围内。
上面对本发明实施例结合附图进行了说明,但本发明不限于上述实施例,还可以根据本发明的发明创造的目的做出多种变化,凡依据本发明技术方案的精神实质和原理下做的改变、修饰、替代、组合或简化,均应为等效的置换方式,只要符合本发明的发明目的,只要不背离本发明的技术原理和发明构思,都属于本发明的保护范围。
Claims (6)
1.一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法,包括模块1、模块2、模块3和模块4共四个计算模块,其特征在于:
所述模块1,建立轮式-IMU里程计(WIO)的测量模型,推导***的状态方程;
所述模块2,建立视觉里程计(VO)的测量模型,推导***的观测方程;
所述模块3,用误差状态卡尔曼滤波(ESKF)对WIO和VO的运动估计信息进行融合;
所述模块4,建立自适应模糊神经推理***(ANFIS),根据WIO和VO的直接信息,推断里程计的运动状态,实时调整ESKF中预测模型和测量模型的协方差矩阵;该模块在使用前需要根据GPS数据对ANFIS进行训练;
所述模块1的输入是轮式里程计数据和IMU数据,输出是相邻时刻IMU坐标系的位姿估计;模块2的输入是双目相机拍摄的照片,输出是相邻时刻相机坐标系的位姿估计;模块3的输入是模块1和模块2的输出,输出是相邻时刻IMU坐标系的位姿估计;模块4的输入是模块1中WIO和模块2中VO的中间数据,输出是模块3中需要使用的WIO和VO的协方差矩阵;
整个定位程序的输入是IMU数据、轮式里程计数据、相机数据,输出是IMU坐标系的位姿变化。
2.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法,所述模块1中建立WIO测量模型,具体如下:
IMU包括三轴陀螺仪和三轴加速度计,考虑到测试***加速度计中的噪声极大地影响了位移估计,选择仅使用陀螺仪来估计方向变化;作为补偿,引入车轮编码器进行平移运动计算,并与陀螺仪集成,融合为轮式-IMU里程计WIO,提供完整的六自由度运动估计;WIO为ESKF***提供移动机器人运动状态的预测;
下面的式子(1)展示了i+1时刻WIO测得的IMU坐标系{I}的位置变化:
3.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法,其特征在于:所述模块1中建立***状态方程,具体如下:
定义***的状态为:
那么根据WIO测量模型得到***的名义状态方程为:
其中是i时刻轮式里程计的测量值,是i时刻IMU坐标系相对于0时刻的姿态,是i时刻IMU测得的角速度,是i时刻IMU的角速度偏移,Δt是IMU的采样周期,[γi]×指的是γi对应的反对称矩阵;pi+1表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的位置,表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的姿态对时间的导数,表示i+1时刻IMU角速度偏移对时间的导数;
误差状态方程为:
4.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法,其特征在于:所述模块2中建立视觉里程计(VO)的测量模型,推导***的观测方程,具体如下:
是相机观测到的从第j-1帧到第j帧的位移,它和相等;是的从时间j-1到时间j的旋转向量,和等价;ΔpC,n是随机位移误差,服从高斯分布ΔpC,n~N(0,V1);ΔθC,n是随机的姿态误差,服从高斯分布ΔθC,n~N(0,V2);表示第j-1帧到第j帧的相机位移变化的测量值;表示从时间j-1到时间j的相机姿态变化的测量值;
进一步地,所述模块2中建立***的观测方程,具体如下:
5.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法,其特征在于:在所述模块4中,建立自适应模糊神经推理***ANFIS,根据WIO和VO的直接信息,推断里程计的运动状态,实时调整ESKF中预测模型和测量模型的协方差矩阵,具体步骤如下:
采用两个ANFIS,第一个ANFIS记作WIO-ANFIS,其输入的是移动机器人的角速度ω和左右车轮的转速差ρ,输出是WIO的协方差矩阵Q1;第二个ANFIS记作VO-ANFIS,其输入的是视觉里程计的特征点个数Nu和重投影误差Er,输出是VO的协方差矩阵V;
定义WIO-ANFIS的输入为Xw=(ω,ρ)T,定义它的模糊集为A;为A中的每一个成员Aa设计一个隶属度函数,并且记作Amf-a(xw)∈[0,1];Amf-a(xw)越接近1,Xw属于Aa的可能性越大;VO-ANFIS的输入是特征点数量Nu和重投影误差Er记作xv=(Nu,Er)T,定义它的模糊集为B;构建隶属度函数Bmf-b(xν)的方法与Amf-a(xw)类似;
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡;曲线形状通过改变函数中的自适应参数进行调整;选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量;
在A中为ρ定义了四个成员,它们分别趋向于+∞(A1),趋向于1(A2),趋向于-1(A3),趋向于-∞(A4);它们分别对应于移动机器人滑转、直走、旋转和滑转;A中的ω有两个成员:小(A5)和大(A6);它们分别对应着慢转弯和快转弯的运动情况;同样,B中Er的模糊集分别为小(B1)和大(B2),Nu的模糊集分别为小的(B3)和大的(B4);
隶属度函数定义了输入与其对应的模糊成员之间的关系,在每个隶属函数中,有一两个变量使其可调整,以更好地适应真实的***情况;表示A和B中的自适应变量分别为AW和Bv;AW1指定了ρ完全属于A1时初始值;AW2指定了ρ属于A2的终点;AW3指定了ρ开始属于A3的点,AW4指定了ρ完全属于A4时的最大值;函数的另一端固定在临界点±1,简化了复杂度,降低了ANFIS训练中需要优化的变量的维数;AW5,AW6,AW7,AW8,指定了ω的隶属度函数的临界点;Bv1,Bv2,Bv3,Bv4指定了Er的隶属度函数的临界点;Bv5,Bv6,Bv7,Bv8指定了Nu的隶属度函数的临界点;曲线形状的控制方式同AW1,AW2,AW3,AW4;
模糊规则库定义了模糊集与输出之间的逻辑关系;根据不同的输入条件,对WIO和VO的测量方差进行了推理;
对于WIO-ANFIS,当输入组合为(A3,A6)和(A5,A2)时,方差为低,用WL表示;物理的解释是,当车辆正在急转弯(A3),并且IMU测量表明同样的情况(A6),或移动机器人运动中没有大旋转(A2)并且IMU测量表明同样的情况(A5),移动机器人运动时可能没有打滑,从而WIO的方差应该比较小;否则,IMU的方差应该比较大,用WH表示;
对于每一条规则,例如πw7(A3,A6),它的输出定义如式子(9)所示;根据同样的规则,得到πwa的输出,用GaW表示;
G7W=min(Amf-6(xw),Amf-3(xw)) (9)
类似地,也为VO-ANFIS建立了规则库;为视觉的运动估计定义了高中低三个方差,分别用VH,VM,VL,表示,相应的模糊规则的输出用GbC表示;
根据式子(10)用GaW来计算WIO-ANFIS的归一化推理结果类似地,得到VO-ANFIS的a表示WIO-ANFIS中模糊规则的编号,b表示VO-ANFIS中模糊规则的编号,na表示WIO-ANFIS中的模糊规则总数;
根据各种工况的具体测量方差VH,VM,VL,WHWL,则WIO的方差Q1和VO的方差V的最终结果由式子(11)得到:
上式中,Wπwa表示规则πwa对应的权重,Vπvb表示规则πvb对应的权重,a表示WIO-ANFIS中模糊规则的编号,b表示VO-ANFIS中模糊规则的编号。
6.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法,其特征在于:在所述模块4中,建立自适应模糊神经推理***(ANFIS)的训练方法,具体步骤如下:
将GPS的直接测量转化到IMU坐标系{I}中,以提供准确的训练数据;GPS接收机在坐标系{I0}中的位置可表示为:
其中,IXG是GPS接收机在IMU坐标系{I}中的坐标,是在时间i时,坐标系{I}相对于坐标系{I0}的旋转矩阵;是时间i时GPS路径点在GPS测量坐标系{GM}中的表示,是坐标系{GM}和坐标系{I0}之间的旋转矩阵;
相应地,定义相对于GPS测量的估计轨迹误差为:
IXG从***设计参数中导出,在使用它进一步训练ANFIS参数之前,有必要对其进行优化;对于优化后的GPS相对于坐标系{I}的位置估计,可根据求式子(14)的解得到;
其中i表示时间戳的索引,ni表示时间戳的总数,s表示训练轨迹的索引,ns表示训练轨迹的总数;
需要优化的参数包括Aw,Bv和方差WH,WL,VH,VM,VL,用式(15)表示:
XANFIS=[AW,Bv,WH,WL,VH,VM,VL] (15)
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210160522.2A CN115060260A (zh) | 2022-02-22 | 2022-02-22 | 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210160522.2A CN115060260A (zh) | 2022-02-22 | 2022-02-22 | 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115060260A true CN115060260A (zh) | 2022-09-16 |
Family
ID=83196894
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210160522.2A Pending CN115060260A (zh) | 2022-02-22 | 2022-02-22 | 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115060260A (zh) |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110986939A (zh) * | 2020-01-02 | 2020-04-10 | 东南大学 | 一种基于imu预积分的视觉惯性里程计方法 |
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN112556719A (zh) * | 2020-11-27 | 2021-03-26 | 广东电网有限责任公司肇庆供电局 | 一种基于cnn-ekf的视觉惯性里程计实现方法 |
CN113252033A (zh) * | 2021-06-29 | 2021-08-13 | 长沙海格北斗信息技术有限公司 | 基于多传感器融合的定位方法、定位***及机器人 |
-
2022
- 2022-02-22 CN CN202210160522.2A patent/CN115060260A/zh active Pending
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN110986939A (zh) * | 2020-01-02 | 2020-04-10 | 东南大学 | 一种基于imu预积分的视觉惯性里程计方法 |
CN112556719A (zh) * | 2020-11-27 | 2021-03-26 | 广东电网有限责任公司肇庆供电局 | 一种基于cnn-ekf的视觉惯性里程计实现方法 |
CN113252033A (zh) * | 2021-06-29 | 2021-08-13 | 长沙海格北斗信息技术有限公司 | 基于多传感器融合的定位方法、定位***及机器人 |
Non-Patent Citations (4)
Title |
---|
ZHU JIE等: "Multisensor Fusion Using Fuzzy Inference System for a Visual-IMU-Wheel Odometry", IEEE, 26 January 2021 (2021-01-26), pages 1 - 11 * |
徐田来;崔平远;崔祜涛;: "车载多传感器组合导航***设计与实现", ***工程与电子技术, no. 04, 15 April 2008 (2008-04-15) * |
荣健;乔文钊;: "基于模糊神经***的多传感器数据融合算法", 电子科技大学学报, no. 03, 30 May 2010 (2010-05-30) * |
路丹晖;周文晖;龚小谨;刘济林;: "视觉和IMU融合的移动机器人运动解耦估计", 浙江大学学报(工学版), no. 06, 15 June 2012 (2012-06-15) * |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110243358B (zh) | 多源融合的无人车室内外定位方法及*** | |
CN102538781B (zh) | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 | |
Liu et al. | Visual-inertial odometry tightly coupled with wheel encoder adopting robust initialization and online extrinsic calibration | |
CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
CN107014376A (zh) | 一种适用于农业机械精准作业的姿态倾角估计方法 | |
Wang et al. | Vehicle localization at an intersection using a traffic light map | |
Marin et al. | Event-based localization in ackermann steering limited resource mobile robots | |
Kang et al. | Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator | |
CN107607113A (zh) | 一种两轴姿态倾角测量方法 | |
CN112254741A (zh) | 里程传感器的异常检测方法、自移动机器人及存储介质 | |
CN106679672A (zh) | 一种基于dbn和卡尔曼滤波算法的agv定位算法 | |
CN110763224A (zh) | 一种自动导引运输车导航方法及导航*** | |
Tang et al. | OdoNet: Untethered speed aiding for vehicle navigation without hardware wheeled odometer | |
CN115060260A (zh) | 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 | |
CN112697153A (zh) | 自主移动设备的定位方法、电子设备及存储介质 | |
Su et al. | GR-SLAM: Vision-based sensor fusion SLAM for ground robots on complex terrain | |
Guo et al. | Model-based deep learning for low-cost IMU dead reckoning of wheeled mobile robot | |
Beauvisage et al. | Multimodal visual-inertial odometry for navigation in cold and low contrast environment | |
CN116300991A (zh) | 一种基于强化学习和事件触发的无人飞行器轨迹跟踪方法 | |
Song | An integrated GPS/vision UAV navigation system based on Kalman filter | |
Javed et al. | Attitude in motion: Constraints aided accurate vehicle orientation tracking in harsh environment | |
Zhou et al. | IMU dead-reckoning localization with RNN-IEKF algorithm | |
Leng et al. | An improved method for odometry estimation based on EKF and Temporal Convolutional Network | |
Zang et al. | Event-triggered Extended Kalman Filter for UAV Monitoring System | |
Hong et al. | Angle measurements for mobile robots with filtering of short-term noise in MEMS gyroscopes |
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 |