北太真元无人机工具箱：从数据处理到算法验证，增强无人机自主飞行能力

北太真元无人机工具箱，面向无人机算法开发与飞行数据处理，提供 MAVLink 消息与日志分析、姿态和坐标变换、飞行器降阶建模、任务与轨迹生成、路径规划与避障、制导跟踪及编队评估等能力——为无人机算法设计、桌面验证和工程分析提供一套基础工具。作为北太真元（基于北太天元科学计算软件）的重要组成部分，它帮助用户在统一的平台上构建可复用、可验证的模型、算法与数据处理流程，兼顾自主可控、数据安全与工程复用。

一、适用场景

无人机产业应用正由单机遥控，向自主任务、集群协同和数据闭环演进。算法研发需要在统一平台上完成任务表达、飞行器建模、轨迹与路径计算、MAVLink 消息处理和飞行数据分析。北太真元无人机工具箱主要面向四类场景：1. 教学科研：在一个平台上完成从任务定义、飞行器建模到算法验证的完整链路，适合课堂教学与科研实验；2. 算法原型验证：固定翼与多旋翼降阶模型、轨迹与任务对象提供桌面建模环境，支持算法快速迭代；3. 飞行数据分析：支持读取 MAVLink TLog、PX4 ULog、ArduPilot BIN 等真实飞行日志，完成信号映射、数据复盘与飞行后评估；4. 地面站相关开发：基于 MAVLink 方言解析、消息与命令创建能力，为地面站与协议数据处理提供开发基础。

二、核心能力

围绕无人机算法开发与飞行数据处理，工具箱形成七项核心能力：1. MAVLink 消息与日志分析：方言解析、消息与命令创建、TLog 读取与签名密钥管理；支持 PX4 ULog、ArduPilot BIN 读取与飞行日志信号映射；2. 姿态和坐标变换：轴角、欧拉角、四元数、旋转矩阵、齐次变换互转，经纬高与 NED、ENU 局部坐标转换，为模型、轨迹与日志数据提供统一坐标约定；3. 飞行器建模：8 状态固定翼与 13 状态多旋翼降阶制导模型，统一 state、control、environment、derivative 接口，同一工作流即可切换机型；4. 任务与轨迹生成：最小 Jerk、最小 Snap 平滑轨迹与航点轨迹生成，轨迹对象可按时间查询位置、速度、加速度与姿态；5. 路径规划与避障：三维 Dubins 路径连接、凹多边形覆盖分解与覆盖路径求解、VFH3D 三维局部避障；6. 制导跟踪：航点跟踪器与圆轨道跟踪器输出前视点、期望航向与横向误差，路径管理器组织航点、保持、重复与返航指令；7. 编队评估：计算多机任务的安全性、速度一致性、聚合度、连通性与机间距离。

三、应用场景

北太天元无人机工具箱适用于 MAVLink 消息处理、飞行数据分析、飞行器建模、任务与轨迹生成、路径规划、局部避障、制导跟踪和编队评估等典型应用。场景一：编队评估——四机编队性能与连通性。把 4 架无人机的位置和速度组织成时间表，设定安全半径与连通距离，一次性计算安全性、速度一致性、聚合度、连通性和机间距离，并绘制性能指标曲线与末时刻编队拓扑。轨迹数据与指标计算：

tFormation = [0;1];uav1 = timetable(seconds(tFormation),[1 0 10;1 0 10.5], ...    [0 0 0.5;1 0 0], ...    'VariableNames',{'Position','Velocity'});uav2 = timetable(seconds(tFormation),[0.5 0.8 10;0.5 1.8 11], ...    [0 1 1;1 0 0], ...    'VariableNames',{'Position','Velocity'});uav3 = timetable(seconds(tFormation),[1.5 1 10;1.5 1.8 11], ...    [0 1 1;1 0 0], ...    'VariableNames',{'Position','Velocity'});uav4 = timetable(seconds(tFormation),[2 0 10;1.5 0 10.5], ...    [0 0 0.5;1 0 0], ...    'VariableNames',{'Position','Velocity'});formation = uavFormationMetrics({uav1,uav2,uav3,uav4}, ...    'SafetyRadius',0.25,'ConnectivityRange',1.5,'MaxNumNeighbors',5);[perfMetrics,distMetrics] = metrics(formation);info = formationInfo(formation);formationMetricValues = [perfMetrics.Safety,perfMetrics.Order, ...    perfMetrics.Union,perfMetrics.Connectivity];formationPositions = [info.PositionX(end,:).',info.PositionY(end,:).', ...    info.PositionZ(end,:).'];fig = figure('Color','w','Position',[80 80 1500 620], ...    'Visible','on','Name','图 3-2 编队指标与拓扑','NumberTitle','off');ax1 = subplot(1,2,1);plot(ax1,tFormation,formationMetricValues,'o-','LineWidth',1.8, ...    'MarkerSize',6);grid(ax1,'on'); box(ax1,'on');xlabel(ax1,'时间 (s)'); ylabel(ax1,'归一化指标');title(ax1,'编队性能指标');legend(ax1,{'安全性','速度一致性','连通分量','代数连通度'}, ...    'Location','southwest');ylim(ax1,[-0.05 1.10]);ax2 = subplot(1,2,2);hold(ax2,'on'); grid(ax2,'on'); box(ax2,'on');scatter3(ax2,formationPositions(:,1),formationPositions(:,2), ...    formationPositions(:,3),90,[0.18 0.45 0.75],'filled');for i = 1:size(formationPositions,1)    text(ax2,formationPositions(i,1),formationPositions(i,2), ...        formationPositions(i,3)+0.035,sprintf('UAV %d',i), ...        'HorizontalAlignment','center','FontSize',10);    for j = i+1:size(formationPositions,1)        if norm(formationPositions(i,:)-formationPositions(j,:)) <= 1.5            plot3(ax2,formationPositions([i j],1), ...                formationPositions([i j],2),formationPositions([i j],3), ...                '-','Color',[0.40 0.48 0.56],'LineWidth',1.2);        end    endendxlabel(ax2,'东向 (m)'); ylabel(ax2,'北向 (m)'); zlabel(ax2,'高度 (m)');title(ax2,'t = 1 s 的编队拓扑');view(ax2,35,24); axis(ax2,'equal');xlim(ax2,[0.30 1.75]); ylim(ax2,[-0.20 2.05]); zlim(ax2,[10.40 11.25]);

指标曲线反映两个时刻的安全性、速度一致性和连通性变化，三维拓扑图同步给出无人机空间位置及邻接关系。场景二：轨迹生成——最小 Jerk 与最小 Snap。为同一组 4 个三维航点生成 3×121 采样点的两条轨迹，都通过首末航点、满足时间约束；最小 Jerk 约束加速度变化率，最小 Snap 进一步约束更高阶导数，可按轨迹平滑性要求选用。航点与时间配置：

waypoints = [0 5 10 15;             0 3  0 -2;             0 -1 -3 -2];timePoints = [0 2 4 6];numSamples = 121;[qJerk,qdJerk] = minjerkpolytraj( ...    waypoints,timePoints,numSamples);[qSnap,qdSnap] = minsnappolytraj( ...    waypoints,timePoints,numSamples);

轨迹与速度可视化：

tTrajectory = linspace(timePoints(1),timePoints(end),numSamples);speedJerk = sqrt(sum(qdJerk.^2,1));speedSnap = sqrt(sum(qdSnap.^2,1));fig = figure('Color','w','Position',[80 80 1500 620], ...    'Visible','on','Name','图 3-3 轨迹生成对比','NumberTitle','off');ax1 = subplot(1,2,1);plot3(ax1,qJerk(1,:),qJerk(2,:),qJerk(3,:), ...    'LineWidth',2.0,'Color',[0.20 0.45 0.75]);hold(ax1,'on');plot3(ax1,qSnap(1,:),qSnap(2,:),qSnap(3,:),'--', ...    'LineWidth',2.0,'Color',[0.78 0.30 0.27]);scatter3(ax1,waypoints(1,:),waypoints(2,:),waypoints(3,:),70, ...    [0.10 0.10 0.10],'filled');grid(ax1,'on'); box(ax1,'on'); axis(ax1,'equal');xlabel(ax1,'X (m)'); ylabel(ax1,'Y (m)'); zlabel(ax1,'Z (m)');title(ax1,'三维空间轨迹');legend(ax1,{'最小 Jerk','最小 Snap','航点'},'Location','northeast');view(ax1,34,24);ax2 = subplot(1,2,2);plot(ax2,tTrajectory,speedJerk,'LineWidth',2.0, ...    'Color',[0.20 0.45 0.75]);hold(ax2,'on');plot(ax2,tTrajectory,speedSnap,'--','LineWidth',2.0, ...    'Color',[0.78 0.30 0.27]);grid(ax2,'on'); box(ax2,'on');xlabel(ax2,'时间 (s)'); ylabel(ax2,'速度 (m/s)');title(ax2,'速度变化对比');legend(ax2,{'最小 Jerk','最小 Snap'},'Location','northeast');

两种方法采用不同高阶导数约束：最小 Jerk 重点约束加速度变化率，最小 Snap 进一步约束更高阶导数，可根据轨迹平滑性要求选择。场景三：任务规划与避障——覆盖路径与三维 VFH。对 30m×20m 的矩形覆盖区域、以 2m×3m 作业单元求解，输出 22×3 的三维覆盖航点并确认路径可行；凹多边形可分解为凸子区域，VFH3D 依据障碍点与目标位置计算无碰方向与期望偏航角。覆盖区域分解：

polygon = [0 0;0 20;10 20;10 10;20 10;20 0];parts = coverageDecomposition(polygon, ...    'IntersectionTol',1e-6);

三维避障方向计算：

vfh = controllerVFH3D('HistogramResolution',5);vfh.WindowSize = 3; vfh.DistanceLimits = [0.2 20];position = [0;0;0]; orientation = [1;0;0;0];obstaclePoints = [5 5 5]; target = [10;0;0];[direction,yaw,status] = ...    step(vfh,position,orientation,obstaclePoints,target);fig = figure('Color','w','Position',[80 80 1500 620], ...    'Visible','on','Name','图 3-4 规划与避障','NumberTitle','off');ax1 = subplot(1,2,1);hold(ax1,'on'); box(ax1,'on'); grid(ax1,'on');partColors = [0.76 0.84 0.93;0.94 0.79 0.79;0.82 0.90 0.78];for k = 1:numel(parts)    vertices = parts{k};    patch(ax1,vertices(:,1),vertices(:,2), ...        partColors(1+mod(k-1,size(partColors,1)),:), ...        'FaceAlpha',0.75,'EdgeColor',[0.16 0.16 0.16], ...        'LineWidth',1.5);    center = mean(vertices,1);    text(ax1,center(1),center(2),sprintf('P%d',k), ...        'FontSize',14,'HorizontalAlignment','center');endplot(ax1,[polygon(:,1);polygon(1,1)], ...    [polygon(:,2);polygon(1,2)],'k-','LineWidth',1.8);axis(ax1,'equal'); xlabel(ax1,'X (m)'); ylabel(ax1,'Y (m)');title(ax1,sprintf('凹多边形分解：%d 个凸子区',numel(parts)));ax2 = subplot(1,2,2);hold(ax2,'on'); grid(ax2,'on'); box(ax2,'on');scatter3(ax2,obstaclePoints(:,1),obstaclePoints(:,2), ...    obstaclePoints(:,3),100,[0.45 0.45 0.45],'filled');targetDirection = target/norm(target);quiver3(ax2,0,0,0,targetDirection(1),targetDirection(2), ...    targetDirection(3),5,'LineWidth',2.2,'Color',[0.20 0.45 0.75]);quiver3(ax2,0,0,0,direction(1),direction(2), ...    direction(3),5,'LineWidth',2.2,'Color',[0.78 0.30 0.27]);plot3(ax2,0,0,0,'ko','MarkerFaceColor','k','MarkerSize',6);xlabel(ax2,'X'); ylabel(ax2,'Y'); zlabel(ax2,'Z');title(ax2,sprintf('VFH3D 输出：yaw = %.3f rad，status = %d', ...    yaw,status));legend(ax2,{'障碍点','目标方向','无碰方向','当前位置'}, ...    'Location','northwest');view(ax2,30,22); axis(ax2,'equal');

场景四：制导跟踪——航点前视制导与横向误差。多旋翼航点跟踪器根据当前位姿与前视距离，输出前视点、期望航向和横向误差；前视点随飞行位置沿航段推进，横向误差反映相对期望路径的偏离，可作为制导与路径跟踪算法的验证量。航点与前视参数设置：

waypoints = [0 0 5;10 0 5;10 10 5;20 10 5];follower = uavWaypointFollower( ...    'UAVType','multirotor','StartFrom','first', ...    'Waypoints',waypoints, ...    'TransitionRadius',[1;1;1;1]);x1 = linspace(0,9.6,16).';y1 = 0.35*sin(linspace(0,pi,16)).';x2 = 10 + 0.30*sin(linspace(0,pi,16)).';y2 = linspace(0.4,9.6,16).';x3 = linspace(10.4,20,16).';y3 = 10 + 0.30*sin(linspace(0,pi,16)).';positions = [x1 y1 5*ones(16,1); ...             x2 y2 5*ones(16,1); ...             x3 y3 5*ones(16,1)];yaw = [zeros(16,1);(pi/2)*ones(16,1);zeros(16,1)];lookAheadPoints = zeros(size(positions));crossTrackError = zeros(size(positions,1),1);status = zeros(size(positions,1),1);for k = 1:size(positions,1)    pose = [positions(k,:).';yaw(k)];    [point,~,~,~,crossTrackError(k),status(k)] = ...        step(follower,pose,2);    lookAheadPoints(k,:) = point(:).';endrelease(follower);fig = figure('Color','w','Position',[80 80 1500 620], ...    'Visible','on','Name','图 3-5 航点前视制导','NumberTitle','off');ax1 = subplot(1,2,1);plot(ax1,waypoints(:,1),waypoints(:,2),'k--o', ...    'LineWidth',1.6,'MarkerFaceColor','k');hold(ax1,'on');plot(ax1,positions(:,1),positions(:,2), ...    'LineWidth',2.0,'Color',[0.20 0.45 0.75]);plot(ax1,lookAheadPoints(:,1),lookAheadPoints(:,2),'.', ...    'MarkerSize',13,'Color',[0.78 0.30 0.27]);grid(ax1,'on'); box(ax1,'on'); axis(ax1,'equal');xlabel(ax1,'X (m)'); ylabel(ax1,'Y (m)');title(ax1,'航点路径、飞行位置与前视点');legend(ax1,{'航点路径','飞行位置','前视点'}, ...    'Location','northwest');ax2 = subplot(1,2,2);sampleIndex = 1:numel(crossTrackError);plot(ax2,sampleIndex,crossTrackError,'LineWidth',2.0, ...    'Color',[0.20 0.45 0.75]);hold(ax2,'on');plot(ax2,[sampleIndex(1) sampleIndex(end)],[0 0],'k:');grid(ax2,'on'); box(ax2,'on');xlabel(ax2,'采样序号'); ylabel(ax2,'横向误差 (m)');title(ax2,sprintf('横向误差与跟踪状态（末状态 %d）',status(end)));

前视点随飞行位置沿航段推进，横向误差反映当前位置相对期望路径的偏离程度，可作为制导和路径跟踪算法的验证量。

四、从“能用”到“自主”，一步一个脚印

从数据处理到算法验证，无人机的“自主”能力正在一块块底板上被补起来。北太真元无人机工具箱的意义，不只是多了一款国产工具箱，而是让教学科研、算法验证、飞行数据分析背后的每一行计算，有机会跑在完全自主、数据可控的平台上。

五、下载链接

北太天元 2026 版本已正式发布，无人机工具箱 随版本同步上线。下载链接：https://www.baltamatica.com/download

*无人机工具箱需在已激活的许可下使用，社区版不包含该工具箱