二.引言无人机在复杂城市环境中的自主导航离不开高效、平滑的三维路径规划。传统的RRT算法虽然应用广泛但依赖随机采样导致路径结果不确定、平滑性差A*算法虽能保证最优性但计算量随环境分辨率快速增长。2026年Cheriet等人在《IEEE Open Journal of Vehicular Technology》上发表了TIG*算法Tangent Intersection Guidance Star提出了一种基于几何推理的路径规划新方法。该算法通过计算无人机与障碍物之间的切线交点直接生成航点以几何计算替代随机采样在路径平滑度和计算效率上都有显著提升。本文复现该算法在MATLAB R2022b环境下完成仿真实验并分享完整的配置、运行和结果分析过程。三.算法原理回到顶部3.1核心思想TIG*算法的核心思想是用几何计算替代随机采样。当无人机前方遇到障碍物时算法不依赖随机试探而是精确计算绕行所需的切点将这些切点作为候选航点再通过A式搜索选择最优路径。TIG*分为两种规划模式S-TIG*静态规划和O-TIG*在线规划模式全程运用环境S-TIG*静态切线交点制导环境完全已知的情况O-TIG*在线切线交点制导环境并非完全已知需要在线计算S-TIG*利用全局地图信息一次性规划出完整路径O-TIG*依靠机载传感器局部感知遇到未知障碍时动态重规划。回到顶部3.2障碍物建模与原始TIG算法使用椭圆不同TIG*采用凸多边形棱柱表示障碍物。算法首先对不规则障碍物应用凸包算法Convex Hull将其转换为凸多边形再向上拉伸形成3D棱柱。每个障碍物定义为论文指出Section IV“Polygonal representations provide a more accurate and flexible way to capture the complex geometries of real-world obstacles.”说明多边形表示法能更准确、更灵活地描述真实世界障碍物的复杂几何形状。回到顶部3.3虚拟椭圆与安全距离为了确保无人机飞行安全TIG*算法在真实障碍物周围构建“虚拟椭圆”引入安全距离余量其中d表示安全距离即UAV与障碍物之间保持的最小间距。这种设计的核心思想是让无人机沿着虚拟椭圆的边界飞行而非真实的障碍物边界从而保证足够的安全余量。回到顶部3.4航点生成策略TIG*算法的核心在于航点的几何生成方法。当从当前节点N到目标点T的直线被障碍物阻挡时算法会通过切线交点来生成绕行航点其基本原理如下1.从当前节点N向首个碰撞的障碍物绘制两条切线2.从目标点T向同一障碍物绘制两条切线3.不同切线的交点构成候选航点。针对可能出现的几种不可行情况例如切线平行、无交点等算法采用虚拟椭圆技术即航点被定义为虚拟椭圆与切线的交点并选择距离当前节点较远的交点作为实际航点。设从当前节点N出发的切线交虚线椭圆与两点、其中比更接近N所以选择较远点作为航点即这种“故意绕远路”的策略可以避免无人机在飞行过程中出现急转弯让飞行轨迹在绕过障碍物后更加平缓流畅做到“平滑”。回到顶部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.最少障碍交互通过实现减少绕行次数3.高计算效率启发式引导搜索向目标方向迅速收敛。回到顶部3.6路径平滑处理TIG*算法生成的原始路径由直线段连接各航点构成在航点处存在转角。这些转角无法被无人机直接执行因此需要平滑处理。TIG*算法采用二次贝塞尔曲线对每三个连续节点进行平滑处理其中、、为三个连续的控制点航点。经过平滑处理后还需验证一下性能指标路径长度目标函数转弯角度目标函数回到顶部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; end end function 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; end end4.2.2计算转弯角度matlabfunction angle calculate_angle(A, B, C) % Calculate vectors AB and BC in 3D AB A - B; BC C - B; % Check if any two points are the same if all(round(A,6) round(B,6)) || all(round(B,6) round(C,6)) angle 180; else % Calculate dot product and magnitudes of the vectors dotProduct 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); end end4.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 end end function in_range is_obstacle_in_range(polygon, uav_pos, r) % UAV position and sensing radius x_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; end function d point2poly_distance(p, poly) % Compute shortest distance from point to 2D polygon if inpolygon(p(1), p(2), poly(:,1), poly(:,2)) d 0; else % Distance to polygon edges d inf; n size(poly,1); for i 1:n v1 poly(i,:); v2 poly(mod(i,n)1,:); d min(d, point2segment_dist(p, v1, v2)); end end end function d point2segment_dist(p, a, b) % Distance from point p to segment ab ap 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; end function [x_intersect, y_intersect z_intersect] lineCircleIntersection(A, B,range) % Calculate direction vector of the line segment h 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 equation A 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) []; end end function 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 faster anim_options.trail false; % leave red trail %animate_path(path, anim_options);在MATLAB R2022b中的运行结果如下图其路径呈现平滑、避障合理的特点4.3.2O-TIG*静态规划仿真TIG*算法作者在提供的资料中包含了一个完整O-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, short3.mat)); safety_distance 2; % meters UAV_range.x start(1); UAV_range.y start(2); UAV_range.z start(3); UAV_range.radius 100; % meters % ------------------------------------------------------------ % Run Planner % ------------------------------------------------------------ [path, elapsedTime] otig3d_unknown_planner(start, goal, mapSize, polygons,safety_distance, UAV_range); fprintf(Computation Time: %.4f s\n, elapsedTime); % ------------------------------------------------------------ % Metrics % ------------------------------------------------------------ ds 1.0; metrics compute_path_metrics(path, ds); disp(--- Unknown D-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 D-TIG*; options.show_index false; options.highlight_obstacle -1; display_environment_3d(env, results, options); % ------------------------------------------------------------ % Animate UAV Motion % ------------------------------------------------------------ anim_options.speed 1; % smaller faster anim_options.trail false; % leave red trail %animate_path(path, anim_options);在运行该代码的朋友一定要注意一下第三十三行代码中原作者有一个书写错误想要运行成功需要将这一行的“dtig3d_unknown_planner”改为“otig3d_unknown_planner”对应algorithms文件夹里的static文件夹的otig3d_unknown_planner.m文件。
