[3D path planning] Based on MATLAB A_STAR algorithm 3D path planning [including MATLAB source 003]

紫极Q1564658423

First, Introduction [/ B] [Align = Left] [Color = RGB (0, 0, 0)] [Font = 0uO0quot] a algorithm
A algorithm is a typical heuristic search algorithm that is based on the Dijkstra algorithm, widely used in the game map, in the real world, used to find the shortest path between two points. A algorithm is the most important thing to maintain a heuristic valuation function, as shown in the formula (1).
f (n) = g (n) + h (n) (1)
Where f (n) is an algorithm that the corresponding inspiration function is in the case where each node is searched. It consists of two parts, and the first portion g (n) is the starting node to the actual passage of the current node, and the second portion h (n) is an estimate of the current node to the end of the terminal. Each time the algorithm is expanded, the node that is the smallest F (N) value is selected as the next node on the optimal path.
In practical applications, if the target is optimized at the shortest distance, H (n) is often used as the European Distance or Manhattan Distance of the current point to the end point. If h (n) = 0, the information that does not utilize any current node and the end point, the A [/ i] algorithm degrades to the non-laid Dijkstra algorithm, the algorithm search space becomes large, and the search time is growing.
A * algorithm steps are as follows, and the algorithm maintains two collections: P Table and Q Table. P The node stores those have been searched, but has not yet added the node on the optimal path tree; the Q table maintained the nodes on the Optimal Path tree.
(1) P table, Q table setting, add the starting point S to the P table, the G value is set 0, the parent node is empty, and the other node G value in the road network is infinite.
(2) If the P table is empty, the algorithm failed. Otherwise, select the node that is the smallest of the F value in the p table, records BT, add it to the Q table. Judging whether the BT is end point t, if it is, go to step (3); otherwise find each adjacency node NT of BT according to the road network topology properties and traffic rules, perform the following steps: [/ font] [/ color] [/ align] [align = left] [color = RGB (0, 0, 0)] [font = 0uO0quot] 1 calculate the inspiration value of NT
F (NT) = g (NT) + H (NT) (2)
G (NT) = g (bt) + COST (BT, NT) (3)
Among them, COST (BT, NT) is the pass price of BT to NT.
2 If the NT is in the P table, and the G value calculated by the equation (3) is smaller than the NT, the G value is updated to the formula (3) result, and the parent node of the NT is set to BT.
3 If the NT is in the Q table, and the G value calculated by the equation (3) is smaller than the NT, the G value is updated to the formula (3) result, and the parent node of NT is set to BT, and The NT is removed from the P table.
4 If NT is neither in the P table, it is not in the Q table, then the parent node of NT is set to BT and the NT is moved to the P table.
5 Go to Step (2) Continue.
(3) Back from the end point t, find the parent node sequentially, and add the optimized path until the starting point S can be obtained. [/ font] [/ align] [/ align] II, source code [/ b] clc; clear all; [datax, data, dataz] = loadingmap ('map.xyz'); startpoint = [datax 1, 30), DATAY (30, 1), DATAZ (30, 30) +0.1];% starting endpoint = [DataX (1,390), DATAY (390, 390) +0.1];% terminal Q = Endpoint-startPoint; q = q / norm (q); Direction = Q; position = startpoint; limited = [pi / 6, pi / 6]; Dataz = DATAZ. * 2.5;% r_posi = [300, 320];% R = 3;% DATAZ = Radar_pos (r_posi, r, datax, data, data);% r_posi1 = [100, 120]; r1 = 3; r_posi2 = [200, 150]; R_POSI3 = [300, 320]; R3 = 5; r_posi4 = [150, 320]; r_posi5 = [200, 80]; R5 = 4.2;% DATAZ = Radar_pos (r_posi1, r1, datax, datay, dataz); Dataz = radar_pos (r_posi2, r2, datax , DATAY, DATAZ); DATAZ = Radar_pos (r_posi3, r3, datax, datay, dataz); Dataz = radar_pos (r_posi4, r5, datax, data, dataz); dataz = radar_pos (r_posi5, r5, datax, data, dataz) LEST = SQRT (SUM ((position-endpoint). ^ 2)); h = 0.5; k = 0; while lest> hk = k + 1 route (k = position; [open_list, dir_list] = OpenList (position, direction, h, limited); %%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%% Fate = Zeros (25, 3); for i = 1: 25 XIPT = Open_List (i, 1); Yipt = Open_List (i, 2); [ZOPT, Xind, Yind] = MAppoint (XIPT, Yipt, DataX, Data, Dataz); if (Zopt> Open_LIST (i, 3)) Fate (i, = [1, 1, 1]; Open_List (i, = [0, 0, 0]; END end %%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%% G = K * h; h = h_func (open_list, endpoint); [MINI, IND] = min (h); lest = sqrt (SUM ((position-endpoint). ^ 2)); f = g + mini; position = open_list (ind, ; Direction = Dir_List (inde, ; end x = route (:, 1); y = route (:, 2); z = route (:, 3) +21; Figure (1) Mesh (Datax, Data, Dataz + 20); Hold ON;% View 0, 10) PLOT3 (X, Y, Z, 'R - *'); Axis Equal; [B] Third, Run Results [/ B] [Align = Left] [Color = RGB (0, 0, 0) ] [font =&quot] [img] https://img-blog.csdnimg.cn/2021022015161818.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L1RJUUNtYXRsYWI=,size_16,color_FFFFFF,t_70 [/ img ] [/ font] [/ color] [/ align] four, note [/ b] [align = left] [color = RGB (0, 0, 0)] [font = 0uO0quot] version: 2014A [/ fly ONT] [/ color] [/ align] [align = left] [color = RGB (0, 0, 0)] [font =&quot] full code or write plus QQ1564658423 [/ font] [/ color] [/ align ]