目录
-
- 原理
- matlab代码
【说明】：
网上关于雷达与惯导联合标定的方法有很多，大多数方法在实际操作起来并不太方便，为了获得矩阵变换关系有的还用到了slam建图、神经网络等方法。
之前学习了一下lidar_align标定雷达与惯导，但实际操作效果并不太理想，可能是自己采集的数据不太满足要求，所以自己尝试着通过坐标系转换关系建立方程并用matlab求解的，精度不一定满足要求，可能有很多不足之处，仅供参考。
【lidar_align标定参考链接：https://blog.csdn.net/tfb760/article/details/108581092】
方案一（靠特征点计算）：https://blog.csdn.net/xx970829/article/details/115072158
原理
按照方案一，在实际采集数据时比较困难，纸箱或车等标志物在点云中都不够明显，若要良好效果需要以一栋小平房的一角来做角点，于是提出了方案二：通过室外楼梯或台阶一条明显的点云线在地图坐标系中建立一条空间直线方程，在不同时刻，不同帧之间对应雷达坐标系中的点云线在地图坐标系中在同一条空间直线Ax+By+Cz+D=0上。
然后通过无人机搭载激光雷达和组合惯导来采集该标志线，通过点云可以看到该标志线，而且可以得到该标志线对应角点的在雷达坐标系中的坐标（x1,y1,z1）,时间戳对齐后，可以提取出该帧对应的四元数（qw,qx,qy,qz）及gps经度，纬度，海拔。
通过四元数可以算出相对地图坐标系的旋转矩阵，gps可以得到相对地图坐标系的平移，可以得到变换矩阵M1。
根据对比方案一，多了一个未知数（共16个未知数），标志线上每个点都可以带入空间直线方程，所以至少需要16个点。
上图公式可知：（a,b,c）表示点云中线特征上某一点在地图 坐标系下空间直线上的坐标，M0（4x4）是不变的，M1和点云中线特征上的点云坐标在雷达坐标系中的位置是可以知道的，那么如果知道多组点云（至少16组）数据，其实就可以求出空间直线方程和M0（4x4)了。
matlab代码
（下面的代码中只写了取四帧不同位姿下的点云，每帧4个雷达点）
[使用时，将提取到的数据保存到一个param.txt文件里，按四元数（w x y z）、经度、纬度、海拔、点云（x,y,z）的顺序保存（每帧写4个点云坐标），param.txt文件与这几个.m文件放在同一目录下。运行imu_lidar.m文件，将生成一个result.txt文件来保存结果]
imu_lidar.m
clc
syms a b c d t11 t12 t13 tx t21 t22 t23 ty t31 t32 t33 tz;
m0=[0,0,0,0,0,1,0.1,0,1,0,0.1,1,0,0,0.2,0];
x=fsolve(@funline,m0,optimset('fsolve'));
a=x(1); b=x(2); c=x(3);
t11=x(4); t12=x(5); t13=x(6); tx=x(7);
t21=x(8); t22=x(9); t23=x(10);ty=x(11);
t31=x(12);t32=x(13);t33=x(14);tz=x(15);
R=[t11,t12,t13;t21,t22,t23;t31,t32,t33];
q=dcm2quat(R);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 旋转矩阵 ');
fprintf('%6.8f %6.8f %6.8f \n',t11,t12,t13);
fprintf('%6.8f %6.8f %6.8f \n',t21,t22,t23);
fprintf('%6.8f %6.8f %6.8f \n',t31,t32,t33);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 平移 ');
fprintf('%6.8f %6.8f %6.8f \n',tx,ty,tz);
disp('/*------------------------------------------------------------------------------------------------');
disp('/* 四元数 ');
fprintf('%6.8f %6.8f %6.8f %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
%----------------------------------------------------------------------------------------------------------
T=fopen('.\result.txt','w');%创建文件
fprintf(T,'旋转矩阵： \n');
fprintf(T,'%6.8f %6.8f %6.8f \n',t11,t12,t13);
fprintf(T,'%6.8f %6.8f %6.8f \n',t21,t22,t23);
fprintf(T,'%6.8f %6.8f %6.8f \n',t31,t32,t33);
fprintf(T,'平移： \n');
fprintf(T,'%6.8f %6.8f %6.8f \n',tx,ty,tz);
fprintf(T,'四元数： \n');
fprintf('%6.8f %6.8f %6.8f %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
fclose(T);
funline.m
function P=funline(x)
%打开txt数据
F=importdata('.\param.txt');
%四元数转旋转矩阵
R1=quat2dcm([F(1,1) F(1,2) F(1,3) F(1,4)]);
R2=quat2dcm([F(2,1) F(2,2) F(2,3) F(2,4)]);
R3=quat2dcm([F(3,1) F(3,2) F(3,3) F(3,4)]);
R4=quat2dcm([F(4,1) F(4,2) F(4,3) F(4,4)]);
%经度 纬度 海拔
gx_1=F(1,5); gy_1=F(1,6); gz_1=F(1,7);
gx_2=F(2,5); gy_2=F(2,6); gz_2=F(2,7);
gx_3=F(3,5); gy_3=F(3,6); gz_3=F(3,7);
gx_4=F(4,5); gy_4=F(4,6); gz_4=F(4,7);
% 点云
x11=F(1,8); y11=F(1,9); z11=F(1,10); x12=F(1,11); y12=F(1,12); z12=F(1,13); x13=F(1,14); y13=F(1,15); z13=F(1,16); x14=F(1,17); y14=F(1,18); z14=F(1,19);
x21=F(2,8); y21=F(2,9); z21=F(2,10); x22=F(2,11); y22=F(2,12); z22=F(2,13); x23=F(2,14); y23=F(2,15); z23=F(2,16); x24=F(2,17); y24=F(2,18); z24=F(2,19);
x31=F(3,8); y31=F(3,9); z31=F(3,10); x32=F(3,11); y32=F(3,12); z32=F(3,13); x33=F(3,14); y33=F(3,15); z33=F(3,16); x34=F(3,17); y34=F(3,18); z34=F(3,19);
x41=F(4,8); y41=F(4,9); z41=F(4,10); x42=F(4,11); y42=F(4,12); z42=F(4,13); x43=F(4,14); y43=F(4,15); z43=F(4,16); x44=F(4,17); y44=F(4,18); z44=F(4,19);
%经纬度转墨卡托
[gx11,gy11]=gpstoMercator(gx_1,gy_1);
[gx22,gy22]=gpstoMercator(gx_2,gy_2);
[gx33,gy33]=gpstoMercator(gx_3,gy_3);
[gx44,gy44]=gpstoMercator(gx_4,gy_4);
%纬度差为飞机的x方向位移，经度差为飞机的y方向位移
gx1=0; gy1=0; gz1=0;
gx2=gy11-gy22; gy2=gx11-gx22; gz2=gz_1-gz_2;
gx3=gy11-gy33; gy3=gx11-gx33; gz3=gz_1-gz_3;
gx4=gy11-gy44; gy4=gx11-gx44; gz4=gz_1-gz_4;
%第一帧旋转赋值
m1_11=R1(1,1); m1_12=R1(1,2); m1_13=R1(1,3);
m1_21=R1(2,1); m1_22=R1(2,2); m1_23=R1(2,3);
m1_31=R1(3,1); m1_32=R1(3,2); m1_33=R1(3,3);
%第2帧旋转赋值
m2_11=R2(1,1); m2_12=R2(1,2); m2_13=R2(1,3);
m2_21=R2(2,1); m2_22=R2(2,2); m2_23=R2(2,3);
m2_31=R2(3,1); m2_32=R2(3,2); m2_33=R2(3,3);
%第3帧旋转赋值
m3_11=R3(1,1); m3_12=R3(1,2); m3_13=R3(1,3);
m3_21=R3(2,1); m3_22=R3(2,2); m3_23=R3(2,3);
m3_31=R3(3,1); m3_32=R3(3,2); m3_33=R3(3,3);
%第4帧旋转赋值
m4_11=R4(1,1); m4_12=R4(1,2); m4_13=R4(1,3);
m4_21=R4(2,1); m4_22=R4(2,2); m4_23=R4(2,3);
m4_31=R4(3,1); m4_32=R4(3,2); m4_33=R4(3,3);
% t11=x(4)t12=x(5)t13=x(6);tx=x(7); t21=x(8)t22=x(9)t23=x(10) ty=x(11); t31=x(12)t32=x(13)t33=x(14);tz=x(15);
% c--------c--------c---------------------c---------c--------c-----------------------c----------c---------c---------------------------------c--------c--------c---------------------c---------c--------c-----------------------c----------c---------c-------------------------------c--------c--------c---------------------c-------c---------c------------------------c--------c---------c--------------------
P=[ x(1)*(m1_11*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_12*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_13*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gx1)+x(2)*(m1_21*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_22*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_23*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gy1)+x(3)*(m1_31*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_32*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_33*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gz1)+x(16);
x(1)*(m1_11*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_12*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_13*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gx1)+x(2)*(m1_21*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_22*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_23*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gy1)+x(3)*(m1_31*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_32*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_33*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gz1)+x(16);
x(1)*(m1_11*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_12*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_13*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gx1)+x(2)*(m1_21*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_22*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_23*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gy1)+x(3)*(m1_31*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_32*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_33*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gz1)+x(16);
x(1)*(m1_11*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_12*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_13*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gx1)+x(2)*(m1_21*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_22*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_23*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gy1)+x(3)*(m1_31*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_32*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_33*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gz1)+x(16);
x(1)*(m2_11*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_12*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_13*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gx2)+x(2)*(m2_21*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_22*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_23*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gy2)+x(3)*(m2_31*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_32*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_33*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gz2)+x(16);
x(1)*(m2_11*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_12*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_13*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gx2)+x(2)*(m2_21*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_22*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_23*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gy2)+x(3)*(m2_31*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_32*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_33*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gz2)+x(16);
x(1)*(m2_11*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_12*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_13*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gx2)+x(2)*(m2_21*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_22*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_23*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gy2)+x(3)*(m2_31*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_32*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_33*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gz2)+x(16);
x(1)*(m2_11*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_12*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_13*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gx2)+x(2)*(m2_21*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_22*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_23*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gy2)+x(3)*(m2_31*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_32*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_33*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gz2)+x(16);
x(1)*(m3_11*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_12*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_13*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gx3)+x(2)*(m3_21*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_22*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_23*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gy3)+x(3)*(m3_31*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_32*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_33*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gz3)+x(16);
x(1)*(m3_11*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_12*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_13*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gx3)+x(2)*(m3_21*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_22*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_23*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gy3)+x(3)*(m3_31*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_32*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_33*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gz3)+x(16);
x(1)*(m3_11*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_12*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_13*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gx3)+x(2)*(m3_21*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_22*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_23*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gy3)+x(3)*(m3_31*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_32*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_33*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gz3)+x(16);
x(1)*(m3_11*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_12*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_13*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gx3)+x(2)*(m3_21*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_22*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_23*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gy3)+x(3)*(m3_31*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_32*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_33*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gz3)+x(16);
x(1)*(m4_11*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_12*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_13*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gx4)+x(2)*(m4_21*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_22*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_23*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gy4)+x(3)*(m4_31*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_32*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_33*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gz4)+x(16);
x(1)*(m4_11*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_12*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_13*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gx4)+x(2)*(m4_21*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_22*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_23*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gy4)+x(3)*(m4_31*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_32*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_33*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gz4)+x(16);
x(1)*(m4_11*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_12*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_13*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gx4)+x(2)*(m4_21*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_22*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_23*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gy4)+x(3)*(m4_31*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_32*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_33*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gz4)+x(16);
x(1)*(m4_11*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_12*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_13*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gx4)+x(2)*(m4_21*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_22*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_23*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gy4)+x(3)*(m4_31*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_32*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_33*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gz4)+x(16)];
end
gpstoMercator.m
function [jing,wei] = gpstoMercator(j,w )
jing = j * 20037508.34 / 180;
ly = log(tand((90+ w)*pi/360))/(pi/180);
wei = ly *20037508.34/180;
end
[明天验证后再补充]
