一种基于中心极限定理与直方图置信区间估计和均值中值滤波器的 Lidar/GNSS/INS 三定位数据的融合策略
判断有效的思路: 1. GNSS失效的判据: sol_type的值 std_dev是否超过设定阈值 2. Lidar失效 残差是否超过设定阈值 Lidar与有效的INS输出四元数的差是否超过设定阈值 根据这个差值计算Lidar置信度,1最好0最差。 * 以上所说"设定阈值"均为对2000个样本点数学分析之后,得到的静态常数
相互校验的思路 1. GNSS状态好的时候,说明INS比较可信,可以用INS检验修正Lidar:通过计算的置信度conf调节计算 xyz时Lidar的浮动权重。 GNSS状态差的时候,说明INS不可信了,就要用Lidar检验修正INS:通过计算的置信度conf调节计算四元数时INS的浮动权重。
计算思路: 实时计算权重Ws = 预设权重W 与 浮动权重w 的差或积(要保证总权重不能大于预设权重) 浮动权重由自身的标准差std_dev/残差r和相互校验得到的置信度conf决定 实时输出融合值 = sum(Ws[i]* 值[i]) /sum(Ws);//加权平均
step1:计算Lidar数据的置信度conf
conf = (Lidar与ins测量差值 - 设定阈值)/设定阈值
*除以阈值的目的是归一化处理,1最好0最差。
*需要分别计算QXQYQZQW四个置信度取平均值后返回一个conf
step2:计算(qx,qy,qz,qw)姿态四元数
预设权重 Lidar INS
GNSS=NARROW_INT : 5 5 //精度较好
GNSS=NARROW_FLOAT: +1 -1 //精度较差
GNSS=PSRDIFF : +2 -1 //精度很差
Lidar残差超阈值 : -2 +1 //若Lidar不超过阈值,则为上面的三个
Lidar的浮动权重w = 残差 * para //para为待定参数,我姑且先设定3
GNSS可信的时候:INS没有浮动权重项,预设权重就是总权重
GNSS不可信的时候:INS总权重 = INS预设权重 * conf,
实时计算权重Ws = w + W
计算输出四元数融合值:
Q = (Lidar的实时总权重Ws*Lidar测量值 + INS权重*INS测量值 )/(Lidar的实时总权重Ws + INS权重);
step3:计算(X,Y,Z)定位坐标
预设权重 W Lidar GNSS INS
GNSS=NARROW_INT : 5 5 5 //GNSS精度较好
GNSS=NARROW_FLOAT: +2 -1 -1 //GNSS精度较差
GNSS=PSRDIFF : +3 -2 -1 //GNSS精度很差
Lidar残差超阈值 : -2 +1 +1 //若Lidar不超过阈值,则为上面的三个
GNSS浮动权重w = std_dev * para //para为待定参数,我姑且先设定4
GNSS可信的时候:INS没有浮动权重项,预设权重就是总权重
Lidar浮动权重w= conf * Lidar固定权重
GNSS不信的时候:INS总权重 = INS预设权重 * conf,
Lidar浮动权重w = para* 残差
计算输出坐标融合值:
X = (Lidar的x实时总权重Ws*Lidar测量x + INS权重*INS测量x + GNSS的x的实时总权重Ws*GNSS测量的X)
/(Lidar的x实时总权重Ws + INS权重 + GNSS的x的实时总权重Ws)
反思分析: 1. 需要调节的东西 · 固定权重的设置大小 · 计算某些项浮动权重的para的大小(就那些姑且先设定4啊3啊5啊的那些) · 判别阈值的设定:根据datasheet.histogram的直方图可以动态调一调 · 2. 自我校验 相互校验 INS: 无 Lidar GNSS: 标准差阈值 无 lidar: 残差阈值 INS 3.对于超出阈值的处理: 也未尝不可以取一点有用的东西出来呢?
一个完整过程表述的例子:"1574149171.1" 时刻各传感器的数据如下
GNSS_POSE:
GX GY GZ sol_type lat_std_dev lon_std_dev hi_std_dev
684705.352052 3112421.905949 41.150541 NARROW_INT 0.00585 0.00659 0.01560
INS_POSE
IX IY IZ Iqx Iqy Iqz Iqw
684703.7752890 3112420.482600 41.835366 0.003103 0.007091 0.984194 0.176925
LidarPose
LX LY LZ Lqx Lqy Lqz Lqw rx ry yz rw
684703.834915 3112420.472351 41.826940 0.004169 0.008907 0.984296
0.176257 0.067209 0.051231 0.038484 0.079860
(qx,qy,qz,qw)
step1:计算Lidar数据的置信度conf //规定 Δqx阈值:0.032;Δqy阈值:0.0125;Δqz阈值:1.98;Δqw阈值:1.8
confQX = fabs(fabs(0.004169-0.003103)-0.032)/0.032 = 0.966687;
confQY = fabs(fabs(0.008907-0.007091)-0.0125)/0.0125 = 0.854720;
confQZ = fabs(fabs(0.984296-0.984194)-1.98)/1.98 = 0.999948
confQW = fabs(fabs(0.176257-0.176925)-1.8)/1.8=0.999629;
conf = (0.966687+0.854720+0.999948+0.999629)/4=0.9552460
step2:计算权重
预设权重 Lidar INS
GNSS=NARROW_INT : 5 5 //精度较好
计算Lidar各浮动权重
Lidar_qx的浮动权重wx = 0.067209 * 4 = 0.268836 //para姑且先设定4,此时rx=0.067209
Lidar_qy的浮动权重wy = 0.051231 * 4 = 0.204924 //para姑且先设定4,此时rx=0.051231
Lidar_qz的浮动权重wz = 0.038484 * 4 = 0.153936 //para姑且先设定4,此时rx=0.038484
Lidar_qw的浮动权重ww = 0.079860 * 4 = 0.319440 //para姑且先设定4,此时rx=0.079860
GNSS和INS没有浮动权重项,故总权重就是预设权重
计算Lidar总权重:
Lidar_qx的总权重WSx = 5-0.268836 = 4.731164
Lidar_qy的总权重WSy = 5-0.204924 = 4.795076
Lidar_qz的总权重WSz = 5-0.153936 = 4.846064
Lidar_qw的总权重WSw = 5-0.319440 = 4.680560
计算输出四元数融合值:///两个传感器的加权平均
Qx = (4.731164*0.004169 + 5*0.003103 )/(4.731164 + 5) = 0.003621;
Qy = (4.795076*0.008907 + 5*0.007091 )/(4.795076 + 5) = 0.007980;
Qz = (4.846064*0.984296 + 5*0.984194 )/(4.846064 + 5) = 0.984244;
Qw = (4.680560*0.176257 + 5*0.176925 )/(4.680560 + 5) = 0.176602;
step3:计算(X,Y,Z)定位坐标
预设权重 W Lidar GNSS INS
GNSS=NARROW_INT : 5 5 5 //精度较好
计算GNSS浮动权重
GNSS_X 浮动权重wx = 0.00585 * 3 = 0.1755 //para为3,此时x的标准差lat_std_dev为0.00585
GNSS_Y 浮动权重wy = 0.00659 * 3 = 0.01977 //para为3,此时y的标准差lon_std_dev为0.00659
GNSS_Z 浮动权重wz = 0.01560 * 3 = 0.0468 //para为3,此时z的标准差hi_std_dev为0.01560
计算GNSS的实时总权重
GNSS_X的总权重WSx = 5 - 0.1755 = 4.8245;
GNSS_Y的总权重WSx = 5 - 0.01977 = 4.8023;
GNSS_Y的总权重WSx = 5 - 0.0468 = 4.9532;
INS没有浮动权重项
计算Lidar浮动权重(只有一个)
Lidar浮动权重w = 0.9552460 *5 =4.77623
计算输出坐标融合值:///三个传感器的加权平均
X = (4.77623*684703.834915 + 5*684703.7752890 + 4.8245*684705.352052)/(4.77623 + 5 + 4.8245) = 684704.315801740 ;
Y = (4.77623*3112420.472351+5*3112420.482600 + 4.8023*3112421.905949)/(4.77623 + 5 + 4.8023) = 3112420.948106280;
Z = (4.77623*41.826940 +5*41.835366 + 4.9532*41.150541)/(4.77623 + 5 + 4.9532) = 41.60234138845830;
融合输出数据和三个数据的比较
GNSS_POSE:
GX GY GZ
684705.352052 3112421.905949 41.150541
INS_POSE
IX IY IZ Iqx Iqy Iqz Iqw
684703.7752890 3112420.482600 41.835366 0.003103 0.007091 0.984194 0.176925
LidarPose
LX LY LZ Lqx Lqy Lqz Lqw
684703.834915 3112420.472351 41.826940 0.004169 0.008907 0.984296 0.176257
FusePose
FX FY FZ FQX FQY FQZ FQW
684704.31580174 3112420.9481063 41.6023413884583 0.003621 0.007980 0.984244 0.176602
结论:看起来还不错的亚子