當前位置：首頁 > 人工智能 > 循环神经网络 >内容正文

循环神经网络

激光雷达与组合惯导联合标定--方案二（matlab）

發布時間：2024/3/13 循环神经网络 48 豆豆

生活随笔收集整理的這篇文章主要介紹了激光雷达与组合惯导联合标定--方案二（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

對程序的改進

imu_lidar_fmincon.m

clc syms a b c d t11 t12 t13 tx t21 t22 t23 ty t31 t32 t33 tz; %********************************************************************************************** % 輸入參數 %********************************************************************************************* %設初始值 imux=0; %空間點數據在慣導坐標系的大致位置 imuy=0.5; imuz=0.75; txx=0.18;%手測位移tyy=0; tzz=0.24; l_i_z=0.2;%手測沿慣導坐標系z軸平移 l_i_x=0.1;%手測沿慣導坐標系x軸平移 l_i_z2=0.14;%手測沿慣導坐標系z軸平移(到雷達中心高度) aa=40;%沿慣導坐標系y軸旋轉角度 %-------------------------------------------------------------------------- [A,B,C,D]= funintial(imux,imuy,imuz); h=0; % [ | | | | m0=[A,B,C,h,h,1,txx,h,1,h,tyy,1,h,h,tzz,D]; A = [];b = []; Aeq = []; beq = []; vlb = []; vub = []; options = optimoptions('fmincon','Algorithm','sqp','Display','iter','ConstraintTolerance',1e-12); [x,fval]=fmincon(@funcon,m0,A, b, Aeq, beq, vlb, vub,@fcon,options); Rr=imutolidar(l_i_z,l_i_x,l_i_z2,aa); 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('/* 原始矩陣 '); disp(Rr); 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(T,'%6.8f %6.8f %6.8f %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4)); fclose(T);

fcon.m

%*********************************************************** % 約束條件 %***********************************************************function [c,ceq]=fcon(x) %構造約束函數 c=[]<0; ceq= 0 %----------------------------------------------------------- % 輸入參數 %----------------------------------------------------------- tx=0.18; %手測位移 ty=0; tz=0.24; l_i_z=0.2; %手測沿慣導坐標系z軸平移 l_i_x=0.1; %手測沿慣導坐標系x軸平移 l_i_z2=0.14; %手測沿慣導坐標系z軸平移(到雷達中心高度) a=40; %手測沿慣導坐標系y軸旋轉角度 %---------------------------------- L=0.3; %點偏離平面范圍（單位m） Lt=0.05; %偏離手測位移 Lr=0.05; %偏離旋轉 La=50; %偏離平面系數范圍 %----------------------------------- imux=0; %算空間平面時空間點數據在慣導坐標系的大致位置 imuy=0.5; imuz=0.75; %---------------------------------- R=imutolidar(l_i_z,l_i_x,l_i_z2,a); [A,B,C,D]= funintial(imux,imuy,imuz); %----------------------------------- %打開txt數據 F=importdata('.\data.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=gy22-gy11; gy2=gx22-gx11; gz2=gz_2-gz_1; gx3=gy33-gy11; gy3=gx33-gx11; gz3=gz_3-gz_1; gx4=gy44-gy11; gy4=gx44-gx11; gz4=gz_4-gz_1; %第一幀旋轉賦值 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);f1=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); f2=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); f3=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); f4=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); f5=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); f6=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); f7=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); f8=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);f9=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); f10=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); f11=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); f12=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); f13=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); f14=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); f15=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); f16=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);c=[x(1)-A-La;x(2)-B-La;x(3)-C-La;x(4)-R(1,1)-Lr;x(5)-R(1,2)-Lr;x(6)-R(1,3)-Lr;x(7)-tx-Lt;x(8)-R(2,1)-Lr;x(9)-R(2,2)-Lr;x(10)-R(2,3)-Lr;x(11)-ty-Lt;x(12)-R(3,1)-Lr;x(13)-R(3,2)-Lr;x(14)-R(3,3)-Lr;x(15)-tz-Lt;x(16)-D-La;-x(1)+A-La;-x(2)+B-La;-x(3)+C-La;-x(4)+R(1,1)-Lr;-x(5)+R(1,2)-Lr;-x(6)+R(1,3)-Lr;-x(7)+tx-Lt;-x(8)+R(2,1)-Lr;-x(9)+R(2,2)-Lr;-x(10)+R(2,3)-Lr;-x(11)+ty-Lt;-x(12)+R(3,1)-Lr;-x(13)+R(3,2)-Lr;-x(14)+R(3,3)-Lr;-x(15)+tz-Lt;-x(16)+D-La;f1-L;f2-L;f3-L;f4-L;f5-L;f6-L;f7-L;f8-L;f9-L;f10-L;f11-L;f12-L;f13-L;f14-L;f15-L;f16-L;L-f1;L-f2;L-f3;L-f4;L-f5;L-f6;L-f7;L-f8;L-f9;L-f10;L-f11;L-f12;L-f13;L-f14;L-f15;L-f16]; ceq=[]; end

funcon.m

%*********************************************************** % 目標函數 %********************************************************** function F=funcon(x) %構造約束函數 c=[]<0; ceq= 0%打開txt數據 F=importdata('.\data.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=gy22-gy11; gy2=gx22-gx11; gz2=gz_2-gz_1; gx3=gy33-gy11; gy3=gx33-gx11; gz3=gz_3-gz_1; gx4=gy44-gy11; gy4=gx44-gx11; gz4=gz_4-gz_1;%第一幀旋轉賦值 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);%點坐標 f1x=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; f1y=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; f1z=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;f2x=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; f2y=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; f2z=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;f3x=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; f3y=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; f3z=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;f4x=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; f4y=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; f4z=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;f5x=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; f5y=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; f5z=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;f6x=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; f6y=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; f6z=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;f7x=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; f7y=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; f7z=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;f8x=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; f8y=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; f8z=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;f9x=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; f9y=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; f9z=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;f10x=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; f10y=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; f10z=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;f11x=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; f11y=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; f11z=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;f12x=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; f12y=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; f12z=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;f13x=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; f13y=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; f13z=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;f14x=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; f14y=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; f14z=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;f15x=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; f15y=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; f15z=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;f16x=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; f16y=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; f16z=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; % d=|ax0+by0+cz0+d|/√(a?+b?+c?) d1=abs(f1x*x(1)+f1y*x(2)+f1z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d2=abs(f2x*x(1)+f2y*x(2)+f2z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d3=abs(f3x*x(1)+f3y*x(2)+f3z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d4=abs(f4x*x(1)+f4y*x(2)+f4z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d5=abs(f5x*x(1)+f5y*x(2)+f5z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d6=abs(f6x*x(1)+f6y*x(2)+f6z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d7=abs(f7x*x(1)+f7y*x(2)+f7z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d8=abs(f8x*x(1)+f8y*x(2)+f8z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d9=abs(f9x*x(1)+f9y*x(2)+f9z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d10=abs(f10x*x(1)+f10y*x(2)+f10z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d11=abs(f11x*x(1)+f11y*x(2)+f11z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d12=abs(f12x*x(1)+f12y*x(2)+f12z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d13=abs(f13x*x(1)+f13y*x(2)+f13z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d14=abs(f14x*x(1)+f14y*x(2)+f14z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d15=abs(f15x*x(1)+f15y*x(2)+f15z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2); d16=abs(f16x*x(1)+f16y*x(2)+f16z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);F=d1+d2+d3+d4+d5+d6+d7+d8+d9+d10+d11+d12+d13+d14+d15+d16;%-------------------------------------------------------------------------------------------------------------------------------------------------end

imutolidar.m

%*********************************************************** % 慣導坐標系到雷達坐標系 %********************************************************** function R=imutolidar(l_i_z,l_i_x,l_i_z2,a)%沿慣導坐標系z軸平移l_i_zm t_z=[1 0 0 0;0 1 0 0;0 0 1 l_i_z;0 0 0 1]; %繞慣導坐標系y軸逆時針旋轉a度 r_y=[cosd(a) 0 sind(a) 0;0 1 0 0;-sind(a) 0 cosd(a) 0;0 0 0 1]; %沿慣導坐標系x軸平移l_i_x t_x=[1 0 0 l_i_x;0 1 0 0;0 0 1 l_i_z2;0 0 0 1]; %繞慣導坐標系y軸逆時針旋轉180度,然后繞z軸逆時針旋轉90度 r_i_l=[0 0 1 0;0 -1 0 0;1 0 1 0;0 0 0 1]; R=t_z*r_y*t_x*r_i_l;end

[明天驗證后再補充]

小編之后演算了一下，發現初始值設置對結果影響比較大，最近在忙其他問題，可能最近沒有時間解決，僅提供思路參考。。。

【參考方案三】：https://blog.csdn.net/xx970829/article/details/115911534?utm_medium=distribute.pc_relevant.none-task-blog-2_defaultbaidujs_title~default-0.control&spm=1001.2101.3001.4242

總結

以上是生活随笔為你收集整理的激光雷达与组合惯导联合标定--方案二（matlab）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

上一篇： web前端面试总结(自认为还算全面哈哈哈
下一篇：七参数坐标转换 matlab,利用三点法