✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
一、传统 GPS 定位面临的挑战
- 信号遮挡与干扰：在许多复杂环境中，如城市峡谷、茂密森林或室内场景，GPS 信号容易受到建筑物、树木等障碍物的遮挡，导致信号减弱甚至丢失，从而无法实现精确的定位。此外，一些电子设备或恶意干扰源也可能对 GPS 信号产生干扰，影响定位的准确性和可靠性。
- 精度限制：即使在开阔环境下，传统 GPS 定位的精度也存在一定局限性，通常在数米到十几米的范围内。对于一些对定位精度要求极高的无人机应用场景，如精准农业中的农药喷洒、电力巡检中的杆塔定位等，这样的精度无法满足实际需求。
二、协作式 FREE GPS 定位系统的概念
- 协作理念：协作式 FREE GPS 定位系统旨在通过多架无人机之间的相互协作，克服传统 GPS 定位的不足。多架无人机共享各自获取的定位信息、环境数据等，通过协同处理和优化算法，实现更精确、可靠的定位。这种协作模式充分利用了无人机的机动性和灵活性，将多架无人机视为一个整体的定位网络，而不是独立的个体。
- FREE 含义：“FREE” 可能代表着灵活（Flexible）、可靠（Reliable）、高效（Efficient）和精确（Exact）等特性。该系统强调在复杂多变的环境中，能够灵活应对各种定位挑战，提供可靠的定位结果，高效地处理定位数据，并达到精确的定位精度。
三、系统工作原理
- 数据采集：每架无人机配备多种传感器，除了传统的 GPS 模块外，还可能包括惯性测量单元（IMU）、激光雷达（LiDAR）、视觉传感器（摄像头）等。GPS 模块获取大致的位置信息，IMU 实时测量无人机的加速度和角速度，用于推算无人机的姿态和短期位置变化。LiDAR 通过发射激光束并测量反射光的时间来获取周围环境的三维点云数据，识别障碍物和地形特征。视觉传感器则通过拍摄图像，利用计算机视觉技术识别地标、特征点等信息。这些传感器数据为后续的协作定位提供了丰富的信息源。
- 信息共享与交互：多架无人机通过无线通信技术（如 Wi-Fi、蓝牙、专用无线链路等）相互连接，组成一个通信网络。在这个网络中，每架无人机将自身采集到的定位相关信息（如 GPS 位置、IMU 数据、LiDAR 点云、视觉特征等）实时共享给其他无人机。这样，每架无人机都能获取到整个团队的综合信息，为协作定位提供更全面的数据支持。
- 协作定位算法：基于共享的信息，系统采用专门的协作定位算法进行处理。常见的算法包括扩展卡尔曼滤波（EKF）及其变体、粒子滤波等。以 EKF 为例，它将每架无人机的状态（位置、速度、姿态等）作为待估计的参数，利用所有无人机的传感器测量值作为观测数据。通过不断地预测和更新过程，融合来自不同传感器和不同无人机的信息，逐步优化对每架无人机状态的估计，从而提高定位精度。同时，算法还考虑了传感器误差、噪声以及无人机之间的相对位置关系等因素，以确保定位的可靠性。
- 环境感知与地图构建：结合 LiDAR 和视觉传感器数据，无人机可以构建周围环境的地图。LiDAR 点云数据提供了高精度的三维环境信息，而视觉图像则可以补充纹理和语义信息。通过同时定位与地图构建（SLAM）技术，无人机在定位自身位置的同时，不断更新和完善环境地图。这个地图不仅有助于无人机更好地规划飞行路径、避开障碍物，还可以为协作定位提供额外的约束条件。例如，当某架无人机的 GPS 信号丢失时，它可以利用地图信息和其他无人机的位置信息，通过相对定位的方式推算出自己的位置。
- 故障容错与冗余机制：为了确保系统的可靠性，协作式 FREE GPS 定位系统通常具备故障容错和冗余机制。如果某架无人机的某个传感器出现故障或通信链路中断，其他无人机可以通过共享的信息继续提供定位支持。系统会自动检测故障，并调整协作定位算法，利用剩余可用的信息进行定位计算。此外，多架无人机之间的信息冗余也提高了定位的可靠性，即使部分信息存在误差或丢失，整体系统仍能保持一定的定位精度。
⛳️ 运行结果
📣 部分代码
function [Pose,Rotation, LocalRelocFail, GlobalRelocFail] = getOutputSizeImpl(obj)
% Return size for each output port
Pose = [1 3];
Rotation = [3 3];
LocalRelocFail = [1 1];
GlobalRelocFail = [1 1];
end
function [Pose,Rotation, LocalRelocFail, GlobalRelocFail] = getOutputDataTypeImpl(obj)
% Return data type for each output port
Pose = 'double';
Rotation = 'double';
LocalRelocFail = 'double';
GlobalRelocFail = 'double';
end
function [Pose,Rotation,LocalRelocFail, GlobalRelocFail] = isOutputComplexImpl(obj)
% Return true for each output port with complex data
Pose = false;
Rotation = false;
LocalRelocFail = false;
GlobalRelocFail = false;
end
function [Pose,Rotation,LocalRelocFail, GlobalRelocFail] = isOutputFixedSizeImpl(obj)
% Return true for each output port with fixed size
Pose = true;
Rotation = true;
LocalRelocFail = true;
GlobalRelocFail = true;
end
function [Pose,Rotation,LocalRelocFail, GlobalRelocFail] = getOutputNamesImpl(obj)
% Return output port names for System block
Pose = 'Agent Pose';
Rotation = 'Agent Rotation';
LocalRelocFail = 'Local relocalization fail';
GlobalRelocFail = 'Global relocalization fail';
end
function flag = isInputSizeMutableImpl(~,~)
flag = false;
end
function icon = getIconImpl(~)
% Define icon for System block
icon = ["Helper", "Visual", "Localization"];
end
function [name1] = getInputNamesImpl(~) % ,name2,name3]
% Return input port names for System block
name1 = 'Image';
% name2 = 'Location';
% name3 = 'Orientation';
end
function sts = getSampleTimeImpl(obj)
if obj.SampleTime == -1
sts = createSampleTime(obj,'Type','Inherited');
else
sts = createSampleTime(obj,'Type','Discrete',...
'SampleTime',obj.SampleTime);
end
end
%% Utility functions
function [features, validPoints] = detectAndExtractFeatures(obj, Irgb)
%detectAndExtractFeatures detect and extract features
% Detect ORB features
Igray = im2gray(Irgb);
points = detectORBFeatures(Igray, 'ScaleFactor', obj.ScaleFactor, 'NumLevels', obj.NumLevels);
% Select a subset of features, uniformly distributed throughout the image
points = selectUniform(points, obj.NumPoints, size(Igray, 1:2));
% Extract features
[features, validPoints] = extractFeatures(Igray, points);
end
function updateVisulization(obj, currPose, mapPointIdx) %relTranslation
% Plot the camera pose of the current frame
if isempty(obj.CameraPlot)
obj.CameraPlot = plotCamera('AbsolutePose', currPose, 'Parent', obj.MapPlotAxes, 'Size', 1);
else
obj.CameraPlot.AbsolutePose = currPose;
end
% Plot local map points observed in the current frame
if isempty(obj.LocalMapPlot)
obj.LocalMapPlot = scatter3(obj.MapPlotAxes, obj.WorldPoints(mapPointIdx, 1), ...
obj.WorldPoints(mapPointIdx, 2), obj.WorldPoints(mapPointIdx, 3), 8, 'w', 'o', 'filled');
else
set(obj.LocalMapPlot, 'XData', obj.WorldPoints(mapPointIdx, 1), 'YData', ...
obj.WorldPoints(mapPointIdx, 2), 'ZData', obj.WorldPoints(mapPointIdx, 3));
end
% Plot camera trajectory
estimatedLocation = currPose.Translation;
if isempty(obj.CameraTrajectoryPlot)
if any(estimatedLocation)
obj.CameraTrajectoryPlot= plot3(obj.MapPlotAxes, ...
estimatedLocation(1), estimatedLocation(2), estimatedLocation(3), ...
'r.', 'LineWidth', 2, 'DisplayName', 'Estimated Trajectory');
end
else
set(obj.CameraTrajectoryPlot, ...
'XData', [obj.CameraTrajectoryPlot.XData, estimatedLocation(1)], ...
'YData', [obj.CameraTrajectoryPlot.YData, estimatedLocation(2)], ...
'ZData', [obj.CameraTrajectoryPlot.ZData, estimatedLocation(3)]);
end
% % Plot the ground truth in the camera coordinate system
% R = [0 0 -1; 1 0 0; 0 -1 0];
% gTruthLocation = obj.InitialEstimatedPose.Translation + relTranslation * R;
% if isempty(obj.GroundTruthPlot)
% obj.GroundTruthPlot = plot3(obj.MapPlotAxes, ...
% gTruthLocation(1), gTruthLocation(2), gTruthLocation(3), ...
% 'g.', 'LineWidth', 2, 'DisplayName', 'Ground truth');
% else
% set(obj.GroundTruthPlot, ...
% 'XData', [obj.GroundTruthPlot.XData, gTruthLocation(1)], ...
% 'YData', [obj.GroundTruthPlot.YData, gTruthLocation(2)],...
% 'ZData', [obj.GroundTruthPlot.ZData, gTruthLocation(3)]);
% end
% legend([obj.CameraTrajectoryPlot, obj.GroundTruthPlot], 'location', 'northwest','color','w', 'FontWeight', 'bold');
% drawnow;
end
end
methods(Static, Access = protected)
%% Simulink customization functions
function header = getHeaderImpl
% Define header panel for System block dialog
header = matlab.system.display.Header(mfilename("class"));
end
function group = getPropertyGroupsImpl
% Define property section(s) for System block dialog
% Section for pre-built map data
mapSection = matlab.system.display.Section(...
'Title','Pre-built Map ',...
'PropertyList',{'MapDataFileName'});
% Section for visualization
vizSection = matlab.system.display.Section(...
'Title','Visualization',...
'PropertyList',{'XLim','YLim','ZLim'});
% Section for the camera parameters
cameraSection = matlab.system.display.Section(...
'Title','Camera Intrinsic Parameters',...
'PropertyList',{'FocalLength','PrincipalPoint','ImageSize'});
% Section for sample time
sampleTimeSection = matlab.system.display.Section(...
'Title','',...
'PropertyList',{'SampleTime'});
group = [mapSection, cameraSection, vizSection, sampleTimeSection];
end
function simMode = getSimulateUsingImpl
simMode = 'Interpreted execution';
end
function flag = showSimulateUsingImpl
% Return false if simulation mode hidden in System block dialog
flag = false;
end
end
end
🔗 参考文献
[1]周树春.基于GPS的无人机自动着陆控制系统设计与实现[D].西北工业大学,2007.DOI:10.7666/d.y1033865.
🍅往期回顾扫扫下方二维码
