TIG*算法的核心思想是用几何计算替代随机采样。当无人机前方遇到障碍物时算法不依赖随机试探而是精确计算绕行所需的切点将这些切点作为候选航点再通过A式搜索选择最优路径。TIG分为两种规划模式S-TIG静态规划和O-TIG*在线规划模式 全程 运用环境S-TIG* 静态切线交点制导 环境完全已知的情况O-TIG* 在线切线交点制导 环境并非完全已知需要在线计算S-TIG利用全局地图信息一次性规划出完整路径O-TIG依靠机载传感器局部感知遇到未知障碍时动态重规划。回到顶部3.2障碍物建模与原始TIG算法使用椭圆不同TIG*采用凸多边形棱柱表示障碍物。算法首先对不规则障碍物应用凸包算法Convex Hull将其转换为凸多边形再向上拉伸形成3D棱柱。每个障碍物定义为V_{{k}}\left { \left ( x{k1},y_{k1}\right),…,\left ( x_{kn} ,y_{kn}\right ) \right}z\epsilon \left [ z_{kmin},z_{kmax} \right ]论文指出Section IV“Polygonal representations provide a more accurate and flexible way to capture the complex geometries of real-world obstacles.”说明多边形表示法能更准确、更灵活地描述真实世界障碍物的复杂几何形状。回到顶部3.3虚拟椭圆与安全距离为了确保无人机飞行安全TIG*算法在真实障碍物周围构建“虚拟椭圆”引入安全距离余量\left{\begin{matrix} a_{vir} a d \ b_{vir} b d \end{matrix}\right.其中d表示安全距离即UAV与障碍物之间保持的最小间距。这种设计的核心思想是让无人机沿着虚拟椭圆的边界飞行而非真实的障碍物边界从而保证足够的安全余量。回到顶部3.4航点生成策略TIG*算法的核心在于航点的几何生成方法。当从当前节点N到目标点T的直线被障碍物阻挡时算法会通过切线交点来生成绕行航点其基本原理如下1.从当前节点N向首个碰撞的障碍物绘制两条切线2.从目标点T向同一障碍物绘制两条切线3.不同切线的交点构成候选航点。针对可能出现的几种不可行情况例如切线平行、无交点等算法采用虚拟椭圆技术即航点被定义为虚拟椭圆与切线的交点并选择距离当前节点较远的交点作为实际航点。设从当前节点N出发的切线交虚线椭圆与两点P_{1}、P_{2}其中P_{1}比P_{2}更接近N所以选择较远点P_{2}作为航点即WP_{2}这种“故意绕远路”的策略可以避免无人机在飞行过程中出现急转弯让飞行轨迹在绕过障碍物后更加平缓流畅做到“平滑”。回到顶部3.5启发式函数为了在多个候选航点中选择最优者TIG*算法定义了如下启发式函数其中1.D(N,W)从当前节点N到候选航点W的路径长度2.P从N到W的切线段上穿过的障碍物数量3.α障碍物数量的权重系数用于衡量路径长度与安全性4.D(W,T)从W到目标点T的欧式距离估计。启发式函数H(W)的设计体现了三个优化目标1.最短路径通过D(N,W)D(W,T)实现2.最少障碍交互通过\alpha\cdot P实现减少绕行次数3.高计算效率启发式引导搜索向目标方向迅速收敛。回到顶部3.6路径平滑处理TIG*算法生成的原始路径由直线段连接各航点构成在航点处存在转角。这些转角无法被无人机直接执行因此需要平滑处理。TIG*算法采用二次贝塞尔曲线对每三个连续节点进行平滑处理其中P_{1}、P_{2}、P_{3}为三个连续的控制点航点。经过平滑处理后还需验证一下性能指标路径长度目标函数转弯角度目标函数回到顶部3.7算法流程四.仿真实现回到顶部4.1开发环境与工具本研究的仿真实验采用以下开发环境项目 配置操作系统 Windows11编程环境 MATLAB R2022b核心依赖 TIG-3D-UAV-Planning开源框架可视化工具 MATLAB plot3三位绘图在MATLAB R2022b环境中TIG-3D-UAV-Planning框架的代码结构如下main/主入口文件run_static.m用于静态规划run_unknown.m用于在线规划algorithms/S-TIG和O-TIG核心算法geometry/碰撞检测、切线生成等几何计算waypoint_generation/虚拟椭圆与航点生成visualization/3D可视化绘图evaluation/性能指标计算maps/预定义测试地图本报告使用run_static.m和map_city.mat进行仿真实验。回到顶部4.2核心代码实现4.2.1虚拟椭圆计算虚拟椭圆与安全距离matlabfunction inflated_obstacles inflate_obstacles(obstacles, safety_distance)% ------------------------------------------------------------% Inflates polygonal prism obstacles by safety_distance% in XY plane and Z direction.% ------------------------------------------------------------inflated_obstacles obstacles;for i 1:length(obstacles)prism obstacles{i}; vertices prism.vertices; % --- Inflate polygon in XY --- inflated_vertices inflate_polygon(vertices, safety_distance); % --- Inflate in Z --- z_min prism.z_range(1) - safety_distance; z_max prism.z_range(2) safety_distance; prism.vertices inflated_vertices; prism.z_range [z_min z_max]; prism.height z_max; inflated_obstacles{i} prism;endendfunction inflated_vertices inflate_polygon(vertices, d)% ------------------------------------------------------------% Inflates convex polygon outward by distance d% ------------------------------------------------------------centroid mean(vertices,1);inflated_vertices zeros(size(vertices));for i 1:size(vertices,1)direction vertices(i,:) - centroid;direction direction / norm(direction eps);inflated_vertices(i,:) vertices(i,:) d * direction;endend4.2.2计算转弯角度matlabfunction angle calculate_angle(A, B, C)% Calculate vectors AB and BC in 3DAB A - B;BC C - B;% Check if any two points are the sameif all(round(A,6) round(B,6)) || all(round(B,6) round(C,6))angle 180;else% Calculate dot product and magnitudes of the vectorsdotProduct dot(AB, BC);magAB norm(AB);magBC norm(BC);% Calculate the cosine of the angle between AB and BC cosTheta dotProduct / (magAB * magBC); % Ensure the cosine value is within the valid range for acos cosTheta max(min(cosTheta, 1), -1); % Calculate the angle in radians and convert to degrees angle acosd(cosTheta);endend4.2.3获取感知内障碍物matlabfunction in_range_indices obstacles_in_range(uav_pos, polygons, r)% polygons: array of structs with fields ‘vertices’ (Nx2) and ‘height’% uav_pos: 1x3 [x, y, z]% r: sensing radius (scalar)in_range_indices {}; for i 1:length(polygons) if is_obstacle_in_range(polygons{i}, uav_pos, r) in_range_indices [in_range_indices polygons{i}] ; %#okAGROW end endendfunction in_range is_obstacle_in_range(polygon, uav_pos, r)% UAV position and sensing radiusx_uav uav_pos(1);y_uav uav_pos(2);z_uav uav_pos(3);% Polygon base and height base_xy polygon.vertices; % Nx2 z_min 0; z_max polygon.height; % 1. Project UAV to polygon base plane (2D) dist_xy point2poly_distance([x_uav, y_uav], base_xy); % 2. Compute vertical (z) distance to polygon if z_uav z_min dz z_min - z_uav; elseif z_uav z_max dz z_uav - z_max; else dz 0; end % 3. Euclidean distance to polygon prism d sqrt(dist_xy^2 dz^2); % 4. Check if within sensing range in_range d r;endfunction d point2poly_distance(p, poly)% Compute shortest distance from point to 2D polygonif inpolygon(p(1), p(2), poly(:,1), poly(:,2))d 0;else% Distance to polygon edgesd inf;n size(poly,1);for i 1:nv1 poly(i,:);v2 poly(mod(i,n)1,:);d min(d, point2segment_dist(p, v1, v2));endendendfunction d point2segment_dist(p, a, b)% Distance from point p to segment abap p - a;ab b - a;t dot(ap, ab) / dot(ab, ab);t max(0, min(1, t));proj a t * ab;d norm(p - proj);end4.2.4计算感知内航点matlabfunction waypoint calculate_inrange_waypoint(waypoint,current_point,range)[x,y,z] lineCircleIntersection(current_point.coor, waypoint.coor,range); %disp([x and y,num2str([x,y])]) %disp([current_point.coor,waypoint.coor,[range.x range.y]]) if(length(x)2) d1 norm([x(1) y(1) z(1)]-waypoint.coor); d2 norm([x(2) y(2) z(2)]-waypoint.coor); if(d1d2) waypoint.coor [x(1),y(1),z(1)] ; else waypoint.coor [x(2),y(2),z(2)] ; end else waypoint.coor [x,y,z] ; end waypoint.tang [-1 -1 -1]; waypoint.istop current_point.istop; waypoint.currobs current_point.currobs;endfunction [x_intersect, y_intersect z_intersect] lineCircleIntersection(A, B,range)% Calculate direction vector of the line segmenth range.x;k range.y;v range.z;r range.radius;x1 A(1);x2 B(1);y1 A(2);y2 B(2);z1 A(3);z2 B(3);dx x2 - x1;dy y2 - y1;dz z2 - z1;% Calculate parameters for quadratic equationA dx^2 dy^2 dz^2;B 2 * (dx * (x1 - h) dy * (y1 - k) dz * (z1 - v));C (x1 - h)^2 (y1 - k)^2 (z1 - v)^2 - r^2;% Calculate discriminant discriminant B^2 - 4 * A * C; if discriminant 0 % No intersection x_intersect []; y_intersect []; z_intersect []; elseif discriminant 0 % One intersection t -B / (2 * A); x_intersect x1 t * dx; y_intersect y1 t * dy; z_intersect z1 t * dz; if ~isBetween(x_intersect, x1, x2) || ~isBetween(y_intersect, y1, y2) || ~isBetween(z_intersect, z1, z2) % Intersection point lies outside the line segment x_intersect []; y_intersect []; z_intersect []; end else % Two intersections t1 (-B sqrt(discriminant)) / (2 * A); t2 (-B - sqrt(discriminant)) / (2 * A); x_intersect [x1 t1 * dx, x1 t2 * dx]; y_intersect [y1 t1 * dy, y1 t2 * dy]; z_intersect [z1 t1 * dz, z1 t2 * dz]; % Filter out points outside the line segment outside_idx ~isBetween(x_intersect, x1, x2) | ~isBetween(y_intersect, y1, y2)| ~isBetween(z_intersect, z1, z2); x_intersect(outside_idx) []; y_intersect(outside_idx) []; z_intersect(outside_idx) []; endendfunction inside isBetween(value, lowerBound, upperBound)inside value min(lowerBound, upperBound) value max(lowerBound, upperBound);end回到顶部4.3多环境仿真4.3.1S-TIG静态规划仿真TIG算法作者在提供的资料中包含了一个完整S-TIG*静态规划仿真仿真代码。matlab% ------------------------------------------------------------% TIG*3D Planner%% Author: H. Cheriet% Affiliation: USTO-MB% Year: 2026% ------------------------------------------------------------clc; clear; close all;% ------------------------------------------------------------% Setup Paths% ------------------------------------------------------------currentFolder fileparts(mfilename(‘fullpath’));projectRoot fileparts(currentFolder);addpath(genpath(projectRoot));% ------------------------------------------------------------% Load Environment% ------------------------------------------------------------load(fullfile(projectRoot, “maps”, “short1.mat”));safety_distance 2; % meters% ------------------------------------------------------------% Run Planner% ------------------------------------------------------------[path, elapsedTime] stig3d_planner(start, goal, mapSize, polygons, safety_distance);fprintf(‘Computation Time: %.4f s\n’, elapsedTime);% ------------------------------------------------------------% Metrics% ------------------------------------------------------------ds 1.0;metrics compute_path_metrics(path, ds);disp(‘— Static TIG* —’)disp(metrics)% ------------------------------------------------------------% Visualization% ------------------------------------------------------------env.start start;env.goal goal;env.polygons polygons;env.mapSize mapSize;results.path path;results.path_smoothed [];results.algorithm_name “S-TIG*”;options.show_index false;options.highlight_obstacle -1;display_environment_3d(env, results, options);% ------------------------------------------------------------% Animate UAV Motion% ------------------------------------------------------------anim_options.speed 1; % smaller fasteranim_options.trail false; % leave red trail%animate_path(path, anim_options);