A robot path planning method based on angle constraint optimization

Abstract

The application belongs to the technical field of robot navigation technology, and is especially a robot path planning method based on angle constraint optimization. The method comprises the following steps: S1: converting a robot working scene into a grid model; S2: actual start point coordinates and actual end point coordinates correspond to grid coordinates respectively; S3: setting a node state located at a node grid coordinate as; S4: defining an actual cost matrix; S5: setting a closed list C; S6: performing cost calculation on neighbor nodes of a current node, S7: distance cost calculation; S8: turning cost calculation; S9: total cost calculation; and S10: path calculation. The application is more in line with robot kinematics constraints, and the navigation stability is significantly improved, and is suitable for scenarios such as warehousing, indoor service and the like which have strict requirements on path angles.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, specifically to a robot path planning method based on angle constraint optimization. Background Technology

[0002] With the development of industrial automation and service robot technologies, robots are being used more and more widely in scenarios such as warehousing, logistics, and indoor services. Path planning, as the core technology of robot navigation, directly affects the robot's working efficiency and motion stability.

[0003] Traditional path planning algorithms use heuristic search to find the shortest path from the starting point to the destination. However, in a grid environment, these algorithms typically allow movement in eight directions (including four right-angle directions and four diagonal directions), and the planned paths may include turns at arbitrary angles, such as 45° diagonal movements. While such paths are shorter, for robots with steering limitations in their mobile mechanisms (such as wheeled robots and warehouse AGVs), non-right-angle turns can lead to unstable motion, increased energy consumption, or even prevent the robot from performing steering maneuvers.

[0004] In existing technologies, to achieve right-angle turn constraints, the method of directly deleting diagonal directions is usually adopted, retaining only the four right-angle directions of movement. Although this method can force right-angle turns on the path, it has obvious drawbacks: first, it has poor flexibility and may not be able to find a feasible path in complex obstacle environments; second, it has high path redundancy and may generate unnecessary detours; and third, it does not consider the robot's kinematic characteristics and cannot adjust the turning preference according to actual needs.

[0005] Therefore, there is a need for a path planning method that can prioritize right-angle turning paths, retain the flexibility of non-right-angle turning when necessary, and dynamically adjust turning preferences according to scenario requirements. The purpose of this invention is to overcome the shortcomings of existing path planning methods that cannot simultaneously consider right-angle turning preferences and environmental adaptability, and to provide a robot path planning method based on angle constraint optimization. Summary of the Invention

[0006] The purpose of this section is to outline some aspects of the embodiments of the present invention and to briefly describe some preferred embodiments. Simplifications or omissions may be made in this section, as well as in the abstract and title of this application, to avoid obscuring the purpose of these documents; however, such simplifications or omissions should not be construed as limiting the scope of the invention.

[0007] To address the aforementioned technical problems, according to one aspect of the present invention, the present invention provides the following technical solution:

[0008] A robot path planning method based on angle constraint optimization includes the following steps:

[0009] S1: Convert the robot's working scene into a mesh model;

[0010] S2: The actual coordinates of the starting point and the actual coordinates of the ending point correspond to the grid coordinates respectively;

[0011] S3: Position the node grid coordinates The node state is set to k is the index of the direction of movement;

[0012] S4: Define the actual cost matrix Total cost matrix ;

[0013] S5: Set open list and close list C;

[0014] S5.1: Set the starting state List of its total cost middle;

[0015] S5.2: From list Select the state with the lowest total agent value as the current node;

[0016] S5.3: Check if the node is the endpoint. If it is, jump to S10; otherwise, execute S6.

[0017] S6:

[0018] S6.1: Use The value refers to the neighboring nodes in different directions;

[0019] S6.2: Order If the value is increased by 1, If the value is less than or equal to 8, then execute S6.3; otherwise, go to S5.2.

[0020] S6.3: Based on the current node's position and The value determines a neighbor node. If it is in the closed list, proceed to S6.2; otherwise, proceed to S6.4.

[0021] S6.4: For neighboring nodes , This represents the movement direction index of the neighbor node, for that neighbor node. The heuristic cost is calculated;

[0022] S7: Distance cost calculation;

[0023] S8: Calculate the angle between the two directions. and the cost of turning ;

[0024] S9:

[0025] S9.1: Calculate the temporary cost and heuristic cost of neighboring nodes;

[0026] S9.2: Determine if the neighboring node is in the open list. If not, proceed to S9.3; otherwise, proceed to S9.4.

[0027] S9.3: Add the neighbor node status to the open list, assign the temporary cost to the actual cost of the neighbor node, calculate the total cost of the neighbor node, record the predecessor node mapping relationship, and go to S6.2;

[0028] S9.4: Assign the temporary cost to the actual cost of the neighboring nodes, calculate the total cost of the neighboring nodes, record the predecessor node mapping relationship, and go to S6.2;

[0029] S10: Tracing back to the starting point through the list of predecessor nodes, obtain the node grid coordinates on the planned path and store them in set P.

[0030] As a preferred embodiment of the robot path planning method based on angle constraint optimization described in this invention, in step S1, the actual size of the scene is assumed to be... H is the height of the scene, W is the width of the scene, and the grid resolution is d, with a grid row count of... , the number of grid columns In practical path planning, let the actual coordinates of any location be... The actual position coordinates correspond to a grid coordinate. The relationship between the two is as follows: , ,in, , The state of this position is , where k is the direction of the node.

[0031] As a preferred embodiment of the robot path planning method based on angle constraint optimization described in this invention, in step S3, there are a total of 8 movement directions, namely up (… ),Down( ),Left( ),right( ), top left ( ), top right ( ), lower left ( ), bottom right ( ), and The range of values ​​is an interval. Integers within, and satisfy: Indicates moving upwards. Indicates moving downwards. Indicates moving to the left. Indicates moving to the right. Indicates moving to the upper left. Indicates moving to the upper right. This indicates moving to the lower left. This indicates moving to the lower right.

[0032] As a preferred embodiment of the robot path planning method based on angle constraint optimization described in this invention, the method for determining neighbor nodes in S6.3 specifically includes:

[0033]

[0034] .

[0035] As a preferred embodiment of the robot path planning method based on angle constraint optimization described in this invention, the heuristic cost calculation method in S6.4 employs a weighted combination of Manhattan distance and Euclidean distance to enhance guidance for right-angle paths. The heuristic cost... ,in, This represents the Manhattan distance calculation function. ; This represents the function for calculating Euclidean distance. The weights are set to... , .

[0036] As a preferred embodiment of the robot path planning method based on angle constraint optimization described in this invention, in step S7, the movement distance cost... Calculated by direction type:

[0037] .

[0038] As a preferred embodiment of the robot path planning method based on angle constraint optimization described in this invention, in step S8, ,in, and Satisfying the definition in S3, the angle is converted from radians to degrees to obtain the rotation angle in degrees. ;

[0039] in, , , The value has already been defined in S2.

[0040] As a preferred embodiment of the robot path planning method based on angle constraint optimization described in this invention, in step S9.1, neighbor nodes... Temporary cost , The actual cost of the current node, and the heuristic cost of the neighboring nodes. .

[0041] As a preferred embodiment of the robot path planning method based on angle constraint optimization described in this invention, in step S9.3, the state is... The method for calculating the total cost of the neighboring nodes is as follows: Record the predecessor node mapping relationship: ,in The state of neighboring nodes, This represents the current node state.

[0042] As a preferred embodiment of the robot path planning method based on angle constraint optimization described in this invention, in step S10, let the coordinates of any node in set P be... , Where s is the node index number, Let P be the total number of nodes in set P, and Q be the coordinates of nodes on the actual path. The coordinates of the nodes in set Q are... ,in , .

[0043] Compared with existing technologies, the advantages of this invention are as follows: Based on traditional path planning algorithms, this invention achieves precise control of the robot's turning angle by introducing a direction memory mechanism, an extended cost function, and a heuristic function that adapts to angle constraints. By setting differentiated cost weights for different turning angles, this invention prioritizes guiding the robot to choose right-angle turning paths while retaining the flexibility of non-right-angle turns to cope with complex obstacle environments. The path planned by this method better conforms to robot kinematic constraints, significantly improving navigation stability, and is suitable for scenarios with strict requirements on path angles, such as warehousing and indoor services. Attached Figure Description

[0044] To more clearly illustrate the technical solutions of the embodiments of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and detailed embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. Wherein:

[0045] Figure 1 This is a schematic diagram of neighbor nodes for a robot path planning method based on angle constraint optimization according to the present invention.

[0046] Figure 2 This is a schematic diagram of a scene mesh model for a robot path planning method based on angle constraint optimization according to the present invention. Detailed Implementation

[0047] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0048] Secondly, the present invention is described in detail with reference to the schematic diagrams. When detailing the embodiments of the present invention, for ease of explanation, the cross-sectional views illustrating the device structure may be partially enlarged, not according to the usual scale. Furthermore, the schematic diagrams are merely examples and should not limit the scope of protection of the present invention. In addition, actual fabrication should include three-dimensional spatial dimensions of length, width, and depth.

[0049] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.

[0050] A robot path planning method based on angle constraint optimization includes the following steps:

[0051] S1: Mesh Environment Modeling

[0052] The robot's working scene is converted into a mesh model, achieving a mapping between actual coordinates and mesh coordinates. Let the actual size of the scene be... Where H is the scene height and W is the scene width. Let the mesh resolution be d (i.e., the actual side length of each mesh cell). Number of mesh rows. , the number of grid columns The symbol ⌈⌉ represents the floor function, which rounds a real number up to the nearest integer that is not less than it.

[0053] In practical path planning, let the actual coordinates of any location (i.e., any node) be... The actual position coordinates correspond to a grid coordinate. The relationship between the two is as follows: , ,in, , The state of this location (node) is , where k is the direction of the node.

[0054] S2: Parameter Initialization

[0055] actual coordinates of the starting point The corresponding grid coordinates , , Actual coordinates of the endpoint The corresponding grid coordinates , , Set the turning cost weight and the straight-going cost weight. The cost of a right-angle turn The cost of non-right-angle turns .

[0056] S3: Extended State Definition

[0057] Position located at The node state is set to ,in, Here are the node grid coordinates, and k is the movement direction index. There are 8 movement directions, namely up (…). ),Down( ),Left( ),right( ), top left ( ), top right ( ), lower left ( ), bottom right ( The direction vector of the current node is defined as follows: The direction vector of a neighboring node is defined as . and The range of values ​​is an interval. Integers within. and satisfy: Indicates moving upwards. Indicates moving downwards. Indicates moving to the left. Indicates moving to the right. Indicates moving to the upper left. Indicates moving to the upper right. This indicates moving to the lower left. This indicates moving to the lower right.

[0058] S4: Cost Matrix Initialization

[0059] Define the actual cost matrix Used to record the number of nodes reached from direction k. The cumulative cost is initialized to Define the total cost matrix. Used to record the total cost. Initialized to ,in, This is a heuristic cost. During initialization, for all directions ( )set up ,Right now .

[0060] S5: Open List and Node Selection

[0061] Set open list Store the state of the nodes to be explored and the total cost of that state .in, `k` represents the grid coordinates of the node, and `k` is the index of the movement direction to reach that node. Let `C` be the closed list, storing the explored nodes, initially set to empty.

[0062] S5.1 will change the starting state The total cost and its associated costs are stored in an open list. In the middle, assume the parent list of predecessor nodes is empty.

[0063] S5.2 Select the current node

[0064] Each time the current node is selected, from the list Select the state with the lowest overall value as the current node (this node is the most promising and lowest-cost path to the target), and record its state as... ,in, These are the grid coordinates of the current node. Add this node to the closed list C, indicating that the node has been explored, and remove this node from the open list.

[0065] S5.3 Check Node Is this the end? If it is the endpoint, then jump to S10. If checking a node... Not the end If so, then continue executing S6.

[0066] S6: Calculate the cost of the current node's neighboring nodes.

[0067] S6.1 Loop through the neighboring nodes of the current node.

[0068] The eight nodes immediately adjacent to each node are called neighboring nodes. The value refers to neighboring nodes in different directions, such as Figure 1 As shown. The state of a neighboring node is defined as follows. ,in Let be the grid coordinates of the neighboring nodes. =0.

[0069] S6.2 Determine if the neighboring nodes have been traversed.

[0070] make The value is incremented by 1. If If the value is less than or equal to 8, then execute S6.3. If If the value is greater than 8, then proceed to S5.2.

[0071] S6.3 Check if neighboring nodes have been explored.

[0072] Based on the current node's position and The value determines a neighbor node. Specifically:

[0073]

[0074]

[0075] If node If the node is in the closed list, it means that the node has already been searched; otherwise, proceed to S6.4.

[0076] S6.4 Heuristic Cost Calculation:

[0077] For neighboring nodes , This indicates the index of the neighbor node's movement direction. For this neighbor node... The heuristic cost is calculated using a weighted combination of Manhattan and Euclidean distances to enhance guidance for right-angled paths. Heuristic cost. .in, This represents the Manhattan distance calculation function. ; This represents the function for calculating Euclidean distance. The weights are set to... , .

[0078] S7: Distance Cost Calculation

[0079] Cost of travel distance Calculated by direction type:

[0080]

[0081] S8: Calculation of Turning Angle and Turning Cost

[0082] Current node direction index The index corresponding to the direction of the neighbor node Calculate the angle between the two directions. : ,in, and It satisfies the definition in S3. Convert the angle from radians to degrees to obtain the rotation angle in degrees. .

[0083] Calculate the cost of turning :

[0084]

[0085] in, , , The value has already been defined in S2.

[0086] S9

[0087] S9.1 Calculate the total cost

[0088] Calculate neighbor nodes Temporary cost , This represents the actual cost of the current node. The heuristic cost of neighboring nodes. .

[0089] S9.2 Determine if a node is in the open list

[0090] Determine the status of neighboring nodes Check if the node is in the open list. If the node is not in the open list, execute S9.3; otherwise, execute S9.4.

[0091] S9.3

[0092] View neighbor node status Add to the open list and set a temporary cost The actual cost assigned to neighboring nodes At this point, the actual cost of the neighboring node is equal to .

[0093] The calculation state is The total cost of neighboring nodes .

[0094] Record the predecessor node mapping relationship: ,in The state of neighboring nodes, This represents the current node state.

[0095] Proceed to S6.2.

[0096] S9.4

[0097] Temporary cost The actual cost assigned to neighboring nodes At this point, the actual cost of the neighboring node is equal to .

[0098] The calculation state is The total cost of neighboring nodes .

[0099] Record the predecessor node mapping relationship: ,in The state of neighboring nodes, This represents the current node state.

[0100] Proceed to S6.2.

[0101] S10: Calculation Path

[0102] By tracing back to the starting point from the list of predecessor nodes, the node grid coordinates on the planned path can be obtained, and the node grid coordinates on the planned path can be stored in set P.

[0103] Let the coordinates of any node in set P be... , Where s is the node index number, Let P be the total number of nodes in set P. Let Q be the coordinates of nodes on the actual path, and let the coordinates of the nodes in set Q be... ,in , .

[0104] Example

[0105] I. Scene and Parameter Settings

[0106] 1. Scenarios and Core Parameter Definitions

[0107] Actual scene dimensions: Height (H=200mm), Width (W=300mm) (simulating a small warehouse shelving area). Grid resolution: (d=50mm) (each grid cell has a side length of 50mm).

[0108] Grid size:

[0109] Number of lines ;

[0110] Number of columns ;

[0111] Grid coordinate range: , .

[0112] Obstacle setting: Simulate shelf location, mark grids (2, 2), (2, 3), and (3, 2) as obstacles (impassable).

[0113] 2. Core parameter initialization

[0114] Starting point: actual coordinates Corresponding grid coordinates ,in, , (i.e., grid) ).

[0115] End point: actual coordinates Corresponding grid coordinates ,in, , (i.e., grid) ).

[0116] Turning cost weight: Straight-going cost The cost of a right-angle turn The cost of non-right-angle turns .

[0117] Direction vector: Indicates moving upwards. Indicates moving downwards. Indicates moving to the left. Indicates moving to the right. Indicates moving to the upper left. Indicates moving to the upper right. This indicates moving to the lower left. This indicates moving to the lower right.

[0118] 3. Cost Matrix and List Initialization

[0119] Actual cost matrix: Record the distance from the starting point to the destination via direction k. The cumulative cost.

[0120] Heuristic Cost : .

[0121] Total cost matrix: .

[0122] Open list O: Stores the state of nodes to be explored And its total cost.

[0123] Close list C: Stores explored nodes, initially empty.

[0124] Predecessor list parent: Records the predecessor relationships of nodes, initially empty.

[0125] The open list O initially contains only all directions from the starting point. The Manhattan distance from the location to the destination is 3, and the Euclidean distance is... Heuristic Costs The total cost at the starting point is the heuristic cost. Therefore: Scene mesh model such as Figure 2 As shown.

[0126] II. Path Planning Process

[0127] Round 1 iteration:

[0128] Determine the current node as ,Will Add to the close list.

[0129] exist The coordinates of the neighbor in the (up) direction are The node is out of bounds and is invalid.

[0130] Will Removed from the open list.

[0131] Second iteration:

[0132] Determine the current node as ,Will Add to the close list.

[0133] In direction The coordinates of the neighbor in the (downward) direction are: The location is not an obstacle, new direction .

[0134] Actual cost ;

[0135] Heuristic Cost ;

[0136] Total cost .

[0137] Remove from the open list ,join in .

[0138] Front Driver List .

[0139] Third iteration:

[0140] Determine the current node as ,Will Add to the close list.

[0141] exist The coordinates of the neighbor in the (left) direction are The node is out of bounds and is invalid.

[0142] Remove from the open list .

[0143] 4th iteration:

[0144] Determine the current node as ,Will Add to the close list.

[0145] In direction The coordinates of the neighbor in the (right) direction are The location is not an obstacle, new direction .

[0146] Actual cost ;

[0147] Heuristic Cost ;

[0148] Total cost .

[0149] Remove from the open list ,join in .

[0150] Front Driver List .

[0151] 5th iteration:

[0152] Determine the current node as ,Will Add to the close list.

[0153] exist The coordinates of the neighbor in the (top left) direction are The node is out of bounds and is invalid.

[0154] Remove from the open list .

[0155] 6th iteration:

[0156] Determine the current node as ,Will Add to the close list.

[0157] exist The coordinates of the neighbor in the (top right) direction are The node is out of bounds and is invalid.

[0158] Remove from the open list .

[0159] 7th iteration:

[0160] Determine the current node as ,Will Add to the close list.

[0161] exist The coordinates of the neighbor in the (bottom left) direction are The node is out of bounds and is invalid.

[0162] Remove from the open list .

[0163] 8th iteration:

[0164] Determine the current node as ,Will Add to the close list.

[0165] exist The coordinates of the neighbor in the (bottom right) direction are This node is an obstacle and is invalid.

[0166] Remove from the open list .

[0167] 9th iteration:

[0168] Select the state with the minimum total cost from the open list. The cost is 52.82. Add to the close list.

[0169] exist The coordinates of the neighbor in the (right) direction are The location is not an obstacle, new direction .

[0170] Actual cost ;

[0171] Heuristic Cost ;

[0172] Total cost .

[0173] Remove from the open list ,join in .

[0174] Front Driver List .

[0175] 10th iteration:

[0176] Select the state with the minimum total cost from the open list. The cost is 53. Add to the close list.

[0177] exist The coordinates of the neighbor in the (downward) direction are: The location is not an obstacle, new direction .

[0178] Actual cost ;

[0179] Heuristic Cost ;

[0180] Total cost .

[0181] Remove from the open list ,join in .

[0182] Front Driver List .

[0183] 11th iteration:

[0184] Select the state with the minimum total cost from the open list. The cost is 103. Add to the closed list. After filtering, there are 2 valid neighbors.

[0185] exist The coordinates of the neighbor in the (right) direction are Non-obstacles, new directions .

[0186] Actual cost ;

[0187] Heuristic Cost ;

[0188] Total cost .

[0189] exist The coordinates of the neighbor in the (downward) direction are: This location is the endpoint.

[0190] Actual cost ;

[0191] Heuristic Cost ;

[0192] Total cost .

[0193] Remove from the open list ,join in and .

[0194] 12th iteration:

[0195] Current node position is The iteration terminates when the endpoint is reached. Based on the predecessor list, the corresponding grid path is obtained as follows: (1,1)→(1,2)→(1,3)→(2,3); the actual node coordinates on the path are (50,50)→(50,100)→(50,150)→(100,150).

[0196] Although the present invention has been described above with reference to embodiments, various modifications can be made and components can be replaced with equivalents without departing from the scope of the invention. In particular, as long as there is no structural conflict, the features in the disclosed embodiments can be combined with each other in any manner. The lack of an exhaustive description of these combinations in this specification is merely for the sake of brevity and resource conservation. Therefore, the present invention is not limited to the specific embodiments disclosed herein, but includes all technical solutions falling within the scope of the claims.

Claims

1. A robot path planning method based on angle constraint optimization, characterized in that, Includes the following steps: S1: Convert the robot's working scene into a mesh model; S2: Actual coordinates of the starting point S = (S x ,S y ), corresponding grid coordinates S grid =(i s ,j s ), The actual coordinates of the endpoint G = (G x G y ), corresponding grid coordinates G grid =(i g ,j g ), Set the turning cost weights: the straight-going cost C0 = 1, and the right-angle turning cost C... 90 =1, cost of non-right-angle turn C 45 =5; S3: Set the state of the node located at node grid coordinates (i,j) to (i,j,k), where k is the movement direction index. There are 8 movement directions: up (k=1), down (k=2), left (k=3), right (k=4), upper left (k=5), upper right (k=6), lower left (k=7), and lower right (k=8). k and d k′ The value range of d is an integer within the interval [1, 8]. k and d k′ The following conditions must be met: d1 = (-1, 0) indicates moving upwards, d2 = (1, 0) indicates moving downwards, d3 = (0, -1) indicates moving to the left, d4 = (0, 1) indicates moving to the right, d5 = (-1, -1) indicates moving to the upper left, d6 = (-1, 1) indicates moving to the upper right, d7 = (1, -1) indicates moving to the lower left, and d8 = (1, 1) indicates moving to the lower right. S4: Define the actual cost matrix g(i,j,k) and the total cost matrix f(i,j,k); S5: Let there be an open list O and a closed list C; S5.1: Set the starting state (i s ,j s The total cost of k is stored in list O. S5.2: Select the state with the minimum total value from list O as the current node; S5.3: Check if the node is the endpoint. If it is, jump to S10; otherwise, execute S6. S6： S6.1: Use the value of k′ to refer to neighbor nodes in different directions; S6.2: Increment the value of k′ by 1. If the value of k′ is less than or equal to 8, then execute S6.3; otherwise, go to S5.

2. S6.3: Based on the current node's position and the value of k′, determine a neighbor node (i n ,j n If it's in the close list, go to S6.2; Otherwise, proceed to S6.4; S6.4: For neighboring nodes (i n ,j n ), k′ represents the index of the neighbor node's movement direction, for that neighbor node (i n ,j n The heuristic cost is calculated using ) S7: Distance cost calculation; S8: Calculate the angle θ between the two directions and the turning cost T(θ), θ = arccos((d k ·d k′ ) / (|d k |·|d k′ |)), where d k and d k′ Satisfying the definition in S3, the angle is converted from radians to degrees, resulting in a rotation angle θ′ = (θ × 180) / π in degrees. Among them, C0, C 90 C 45 The value has already been defined in S2; S9： S9.1: Calculate the temporary cost and heuristic cost of neighboring nodes; S9.2: Determine if the neighboring node is in the open list. If not, proceed to S9.3; otherwise, proceed to S9.

4. S9.3: Add the neighbor node status to the open list, assign the temporary cost to the actual cost of the neighbor node, calculate the total cost of the neighbor node, record the predecessor node mapping relationship, and go to S6.2; S9.4: Assign the temporary cost to the actual cost of the neighboring nodes, calculate the total cost of the neighboring nodes, record the predecessor node mapping relationship, and go to S6.2; S10: Tracing back to the starting point through the list of predecessor nodes, obtain the node grid coordinates on the planned path and store them in set P.

2. The robot path planning method based on angle constraint optimization according to claim 1, characterized in that, In step S1, let the actual size of the scene be H×W, where H is the height of the scene and W is the width of the scene. Let the grid resolution be d and the number of grid rows be... Number of grid columns In practical path planning, let the actual coordinates of any location be (x1, y1), which correspond to a grid coordinate (i1, j1). The relationship between the two is as follows: Where i∈[1,M], j∈[1,N], the state of this position is (i1,j1,k), where k is the direction of the node.

3. The robot path planning method based on angle constraint optimization according to claim 1, characterized in that, The method for determining neighboring nodes in S6.3 is as follows:

4. The robot path planning method based on angle constraint optimization according to claim 1, characterized in that, The heuristic cost calculation method in S6.4 uses a weighted combination of Manhattan distance and Euclidean distance to enhance guidance for right-angled paths. The heuristic cost h(i) is calculated as follows: n ,j n ,k′)=αf Manhattan (i n ,j n )+βf Euclidean (i n ,j n ), where f Manhattan This represents the Manhattan distance calculation function. f Euclidean This represents the function for calculating Euclidean distance. The weights are set to α = 0.7 and β = 0.

3.

5. The robot path planning method based on angle constraint optimization according to claim 1, characterized in that, In S7, the movement distance cost D(k′) is calculated according to the direction type:

6. The robot path planning method based on angle constraint optimization according to claim 1, characterized in that, In S9.1, the neighbor node (i n ,j n Temporary cost g temp (i n ,j n ,k′)=g(i c ,j c ,k)+D(k′)+T(θ),g(i c ,j c (i, k) represents the actual cost of the current node, and h(i) represents the heuristic cost of the neighboring nodes. n ,j n )=αf Manhattan (i n ,j n )+βf Euclidean (i n ,j n ).

7. The robot path planning method based on angle constraint optimization according to claim 1, characterized in that, In S9.3, the state is (i n ,j n The method for calculating the total cost of the neighbor nodes of (k′) is as follows: f n (i n ,j n ,k′)=g(i n ,j n ,k′)+h(i n ,j n ,k′), record the predecessor node mapping relationship: parent(i n ,j n ,k′)=(i c ,j c ,k), where (i n ,j n (i, k′) represents the state of the neighboring node, (i c ,j c ,k) represents the current node state.

8. The robot path planning method based on angle constraint optimization according to claim 1, characterized in that, In S10, let the coordinates of any node in set P be... Where s is the node index number, N P Let P be the total number of nodes in set P, and Q be the coordinates of nodes on the actual path. The coordinates of the nodes in set Q are... in

Images