无人机视角展示(无人机图像定位 )--某数学建模A题MATLAB代码
Posted slandarer
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了无人机视角展示(无人机图像定位 )--某数学建模A题MATLAB代码相关的知识，希望对你有一定的参考价值。
近期没啥空，水个简单的。。。。
目前只写了第一问，有空再写。。。。。
问题描述
无人驾驶飞机简称“无人机”，是利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机。搭载图像设备的无人机在高空航拍、区域巡视、军事侦查等方面有广泛应用。2018 年 9 月，世界海关组织协调制度委员会将无人机归类为“会飞的照相机”。
利用拍摄的图像对无人机所处的位置及其拍摄姿态进行确定是一项重要的技术。某平坦区域可近似看成水平面，现用某型号无人机对此区域进行巡视。以地面为𝑥𝑜𝑦面，竖直向上方向为𝑧轴按右手系建立直角坐标系。无人机在拍摄图像时，其对称中心平面𝑆始终保持竖直，记平面𝑆相对于𝑥𝑜𝑧面的的方位角为𝜃（0 ≤ 𝜃 ≤ 360°），即𝜃 = 0°时，无人机朝向𝑥轴正向；𝜃 = 90°时，无人机朝向𝑦轴正向。无人机的云台可上下调节照相机的拍摄角度，记镜头中心轴与水平面所夹的俯视角为𝜑（0 ≤ 𝜑 ≤ 90°），即𝜑 = 0°时，镜头水平；𝜑 = 90°时，镜头竖直朝下。这样，无人机的位置和拍摄角度就可以用五元组(𝑥, 𝑦, 𝑧, 𝜃,𝜑)刻画，以下简称为无人机的状态。
为确定无人机的状态，在其巡视区域上设置若干标记点（可看成点光源），其坐标数据见附件一。当无人机以某状态进行拍摄时，这些标记点的像就会出现在照片上。已知无人机的相机镜头为定焦，其水平视角𝛼 = 60°，所拍摄照片的像素为 840*630。
1： 当 无 人 机 的 状 态 分 别 为 (40,30,20,210°, 55°) 和(−20,40,35,43°, 27°)时，绘制出拍摄的（标记点的）照片。需提供单独的 jpg 文件
第一问函数
其实想要简单写不用写这么麻烦，后面部分主要为了画示意图：
function getViewUAV(coe)
% 无人机参数设置
UAV.Pos=coe(1:3);%[40,30,20];
UAV.Theta=coe(4);
UAV.Phi=coe(5);
UAV.Alpha=60;
UAV.Img_Row=630;
UAV.Img_Col=840;
% 读取数据
dataPos=readmatrix('标记点坐标.xls');
% 数据计算
r=tan((90-UAV.Phi)/180*pi)*UAV.Pos(3);
R=UAV.Pos(3)/cos((90-UAV.Phi)/180*pi);
Lcol_=R*tan(UAV.Alpha/2/180*pi);
Lrow_=Lcol_/UAV.Img_Col*UAV.Img_Row;
% 目标点坐标
targetPos=[UAV.Pos(1)+r*cos(UAV.Theta/180*pi),...
UAV.Pos(2)+r*sin(UAV.Theta/180*pi),0];
targetV=targetPos-UAV.Pos;
% 中轴线坐标
midLPos=[targetPos(1)+Lcol_*cos(UAV.Theta/180*pi+pi/2),...
targetPos(2)+Lcol_*sin(UAV.Theta/180*pi+pi/2),0];
midRPos=[targetPos(1)+Lcol_*cos(UAV.Theta/180*pi-pi/2),...
targetPos(2)+Lcol_*sin(UAV.Theta/180*pi-pi/2),0];
midL_V=midLPos-UAV.Pos;
midR_V=midRPos-UAV.Pos;
norm_V=cross(midL_V,midR_V);
norm_V=norm_V./norm(norm_V).*sign(norm_V(3));
% 垂直平面框
LUPos=midLPos+norm_V.*Lrow_;
RUPos=midRPos+norm_V.*Lrow_;
LDPos=midLPos-norm_V.*Lrow_;
RDPos=midRPos-norm_V.*Lrow_;
LU_V=LUPos-UAV.Pos;
RU_V=RUPos-UAV.Pos;
LD_V=LDPos-UAV.Pos;
RD_V=RDPos-UAV.Pos;
% 地面梯形框
LUPos0=UAV.Pos+LU_V./(UAV.Pos(3)-LUPos(3)).*UAV.Pos(3);
RUPos0=UAV.Pos+RU_V./(UAV.Pos(3)-RUPos(3)).*UAV.Pos(3);
LDPos0=UAV.Pos+LD_V./(UAV.Pos(3)-LDPos(3)).*UAV.Pos(3);
RDPos0=UAV.Pos+RD_V./(UAV.Pos(3)-RDPos(3)).*UAV.Pos(3);
% 查找梯形框内的点
polygon_X=[LUPos0(1),RUPos0(1),RDPos0(1),LDPos0(1)];
polygon_Y=[LUPos0(2),RUPos0(2),RDPos0(2),LDPos0(2)];
[in,on]=inpolygon(dataPos(:,1),dataPos(:,2),polygon_X,polygon_Y);
inpolygonPos=dataPos(in|on,:);
% 求地面梯形框内点与无人机的连线与目标平面交点
inpolygonVSet=inpolygonPos-UAV.Pos;
inpolygonLSet=sum(inpolygonVSet.*(targetV./norm(targetV)),2);
inpolygonRateSet=norm(targetV)./inpolygonLSet;
inboxPos=inpolygonVSet.*inpolygonRateSet+UAV.Pos;
% 映射到840*630方框
XYPos=inboxPos(:,1:2);
XYV=XYPos-UAV.Pos(1:2);
tTheta=((360-UAV.Theta)+90)/180*pi;
XYPos=[XYV(:,1).*cos(tTheta)-XYV(:,2).*sin(tTheta),...
XYV(:,2).*cos(tTheta)+XYV(:,1).*sin(tTheta)]+UAV.Pos(1:2);
LDV=LDPos(1:2)-UAV.Pos(1:2);
XYPos00=[LDV(1).*cos(tTheta)-LDV(2).*sin(tTheta),...
LDV(2).*cos(tTheta)+LDV(1).*sin(tTheta)]+UAV.Pos(1:2);
Lcol=2*Lcol_;
Lrow=2*Lrow_*cos((90-UAV.Phi)/180*pi);
XYPos=(XYPos-XYPos00)./[Lcol,Lrow].*[UAV.Img_Col,UAV.Img_Row];
% 绘制散点图
figure()
ax=gca;
hold(ax,'on')
ax.XLim=[0,UAV.Img_Col];
ax.YLim=[0,UAV.Img_Row];
ax.XTick=[];ax.XColor='none';
ax.YTick=[];ax.YColor='none';
outerpos=ax.OuterPosition;
ti=ax.TightInset;
left=outerpos(1) + ti(1);
bottom=outerpos(2) + ti(2);
ax_width=outerpos(3) - ti(1) - ti(3);
ax_height=outerpos(4) - ti(2) - ti(4);
ax.Position=[left bottom ax_width ax_height];
scatter(XYPos(:,1),XYPos(:,2),15,'filled','CData',[0 0 0])
saveas(ax,['c',num2str(coe),'.png'])
tImg=imread(['c',num2str(coe),'.png']);
tImg=imresize(tImg,[UAV.Img_Row,UAV.Img_Col]);
imwrite(tImg,['c',num2str(coe),'.png']);
% 映射示意图
figure()
ax=gca;
hold(ax,'on')
grid(ax,'on')
ax.DataAspectRatio =[1 1 1];
ax.DataAspectRatioMode='manual';
scatter3(UAV.Pos(1),UAV.Pos(2),UAV.Pos(3),30,'filled')
scatter3(targetPos(1),targetPos(2),targetPos(3),30,'filled')
scatter3(dataPos(:,1),dataPos(:,2),dataPos(:,3),3,'filled','CData',[0 0.251 0.451])
plot3([UAV.Pos(1),targetPos(1)],[UAV.Pos(2),targetPos(2)],[UAV.Pos(3),targetPos(3)],...
'LineWidth',2,'Color','k');
plot3([midLPos(1),midRPos(1)],[midLPos(2),midRPos(2)],[midLPos(3),midRPos(3)],...
'LineWidth',2,'Color',[0.8,0.2,0.2]);
fill3([LUPos(1),RUPos(1),RDPos(1),LDPos(1)],...
[LUPos(2),RUPos(2),RDPos(2),LDPos(2)],...
[LUPos(3),RUPos(3),RDPos(3),LDPos(3)],[0.3,0.3,0.3],'FaceAlpha',0.3)
fill3([LUPos0(1),RUPos0(1),RDPos0(1),LDPos0(1)],...
[LUPos0(2),RUPos0(2),RDPos0(2),LDPos0(2)],...
[LUPos0(3),RUPos0(3),RDPos0(3),LDPos0(3)],[0.3,0.3,0.6],'FaceAlpha',0.2)
plot3([LUPos0(1),UAV.Pos(1)],[LUPos0(2),UAV.Pos(2)],[LUPos0(3),UAV.Pos(3)],'Color',[0.3,0.3,0.6])
plot3([RUPos0(1),UAV.Pos(1)],[RUPos0(2),UAV.Pos(2)],[RUPos0(3),UAV.Pos(3)],'Color',[0.3,0.3,0.6])
plot3([LDPos(1),UAV.Pos(1)],[LDPos(2),UAV.Pos(2)],[LDPos(3),UAV.Pos(3)],'Color',[0.3,0.3,以上是关于无人机视角展示(无人机图像定位 )--某数学建模A题MATLAB代码的主要内容，如果未能解决你的问题，请参考以下文章
2022高教社杯全国大学生数学建模-无人机遂行编队飞行中的纯方位无源定位点评
无人机群编队分析的定位问题 分析与思考-2（数学建模竞赛2022年B题）
无人机群编队分析的定位问题 分析与思考-2（数学建模竞赛2022年B题）