Path planning method and positioning system based on improved A* algorithm and grid

Abstract

The application relates to the technical field of path planning, in particular to a path planning method based on an improved A* algorithm and a grid and a positioning system, which comprises the following steps: constructing an abstract network model of a parking lot; finding a next node in the form of a dynamic search tree; selecting an optimal node after calculating the weighted value of the node by using a weighted heuristic function, and updating the weighted heuristic function in real time, so that the path planning of the addressing of the parking lot is realized. The application solves the problems that, when an intelligent vehicle frequently avoids obstacles and turns, the dependence on a map terrain is high, the type of obstacles is fixed, the emergency capability in the face of multiple conditions is poor, and the planning time and the occupied memory present exponential growth with the expansion of the grid map.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and in particular to a path planning method and positioning system based on an improved A* algorithm and a grid. Background Technology

[0002] In modern life, parking lots are an important part of people's daily travel.

[0003] With rapid economic development, the number of cars in society is increasing year by year. At the same time, the number of parking lots built in cities cannot meet the growing parking demand. This leads to problems such as difficulty for users to park and find their cars.

[0004] Existing parking solutions include using dense camera modules, but this is costly; others use fuzzy recognition algorithms, but these have large recognition errors; still others use QR codes for parking and retrieval, which require multiple user operations, are not intelligent enough, and lack convenience.

[0005] Existing path planning algorithms include the traditional Dijkatra algorithm, artificial potential field method, A* algorithm, D* algorithm, PRM algorithm, RRT algorithm, intelligent biomimetic neural network algorithm, ant colony algorithm, genetic algorithm, etc.

[0006] The A* algorithm is widely used by researchers due to its simple structure and fast solution. One robot path planning method is based on the IAPF-A* fusion algorithm. This algorithm integrates an improved artificial potential field method on top of the traditional A* algorithm, solving the problem of getting stuck in local traversal when encountering concave obstacles during the planning process. However, because the artificial potential field method has a limited sensing range, it is prone to getting stuck in local optima, thus having shortcomings in practical applications. Another example is a global path planning method for intelligent vehicles based on an improved A* algorithm. This algorithm classifies obstacle types by sensing the grid map in advance and adds obstacle collision field distance cost weights to the heuristic function, solving the problem of frequent obstacle avoidance and turning in intelligent vehicles. However, due to its high dependence on map terrain and fixed obstacle types, its emergency response capability in the face of multiple situations is poor. As the grid map expands, its planning time and memory consumption increase exponentially. Summary of the Invention

[0007] To address the shortcomings of existing methods, this invention utilizes an improved A* algorithm to solve the problem of limited node mobility and inability to guarantee path optimality in existing methods.

[0008] The technical solution adopted in this invention is: a path planning method based on an improved A* algorithm and a grid, comprising the following steps:

[0009] Step 1: Construct an abstract network model of the parking lot;

[0010] Furthermore, step one specifically includes:

[0011] An ultrasonic sensor is installed at the center of each parking space. In the initial state, all parking spaces are set to vacant and are marked with green dots on the map. When a vehicle is parked, the parking space is marked with red dots on the map.

[0012] The parking lot model is laid out using a grid network, with parking lines, walls, and load-bearing columns represented as obstacle points. The parking lot is defined as an N*M grid, with the grid set as follows:

[0013]

[0014] Step 2: Find the next node using a dynamic search tree; calculate the weighted value of the node using a weighted heuristic function, select the optimal node, and update the weighted heuristic function in real time to realize the path planning for parking lot addressing;

[0015] Furthermore, the weighted heuristic function is as follows:

[0016] f(n)=(1-ω)g(n)+ωh(n) (1)

[0017]

[0018] Where ω∈(-1,1), |AT||TB| are the lengths of the two vectors.<AT,TB> It is the inner product of two vectors; cosATB represents the cosine of the angle between the node to be selected and the initial node and the target node.

[0019] Furthermore, when calculating the weighted values ​​of nodes using the weighted heuristic function, a B-spline curve is used to modify the order. The overall equation is:

[0020]

[0021] Where, N i,k (u) is the basis function of the k-th degree B-spline curve, p i Given the coordinates of the control points;

[0022] N i,k The basis function expression is:

[0023]

[0024] Where C() is the permutation and combination formula, u is the fitting function variable, m is the number of accumulations, k is the order, and f is the index value of the control point.

[0025] Furthermore, path smoothing is performed using a cubic B-spline curve, resulting in the equation of the cubic B-spline curve:

[0026] p(u) = p0 * N 0,3 (u)+p1*N 1,3 (u)+p2*N 2,3 (u)+p3*N 3,3 (u) (10)

[0027] Where, N 0,3 (u), N 1,3 (u), N 2,3 (u), N 3,3 (u) represents the basis function values ​​of control points 1-4, and p0, p1, p2, and p3 are the coordinates of control points 1-4.

[0028] Furthermore, step two specifically includes:

[0029] Step 21: Set the initial node of the A* algorithm as the parent node, move the initial node to the surrounding 8 grid cells, and generate the initial set Open List();

[0030] Step 22: Use the weighted heuristic function to determine the heuristic function value of each node, select the node with the minimum weighted heuristic function value as the optimal node, add the optimal node to Close List(), and at the same time determine whether the node is an obstacle point;

[0031] Step 23: If the node is an obstacle, the parent node increases its movement speed and moves to the surrounding 15 grids. At the same time, the weighted heuristic function is used to determine the optimal node. If the optimal node is still an obstacle, the parent node moves to the surrounding 24 grids and selects the optimal node in the set excluding obstacle nodes. The maximum number of grids searched is 24 grids around the parent node.

[0032] Step 24: Determine if the next optimal node is the destination. If not, continue repeating steps 32-33 until the optimal node is the destination. Then, connect the nodes in Close List() to form a line to obtain the optimal path.

[0033] Step 25: Use cubic B-spline curves to smooth the corners of the path and obtain the final node set Close List().

[0034] A positioning system based on an improved A* algorithm and a grid-based path planning method includes: a data acquisition and processing module, a data query module, a data transmission module, and a data display module.

[0035] The data acquisition and processing module collects the usage status and coordinate information of parking spaces through a wireless sensor network, and collects the coordinate information of users and vehicles; it processes the data through an improved A* algorithm to complete the optimal route navigation indoors.

[0036] The data query module queries user data and parking space data within the parking lot using database commands;

[0037] The data transmission module is used to connect the front-end management platform and the wireless sensor network hardware platform. Through the connection of the ZigBee protocol and the network layer protocol, the parking space information is digitized and stored in the database.

[0038] Furthermore, the data transmission module uses RSSI-based fingerprint positioning to determine the user's location by receiving the wireless signal transmission model of WSN.

[0039] Furthermore, in order to reduce the impact of signal attenuation during propagation on the RSSI value, a logarithmic distance path model was selected.

[0040] The beneficial effects of this invention are:

[0041] 1. An improved A* algorithm is used to plan routes. By utilizing dynamic search range and weighted factor heuristic function, the route length and route planning time are shortened in terms of optimality and running efficiency, which is a significant improvement over traditional path planning algorithms.

[0042] 2. By integrating embedded systems, databases, data structures, web development, and geolocation technologies, the vehicle location system is applied to parking lot management and services, which is of great significance for speeding up users' parking and retrieval time and saving parking lot operating costs;

[0043] 3. It can be applied to the application and management of large parking lots such as stations, shopping malls, hospitals, and office buildings. Users can log in to the website to select various parking services (parking space status, route navigation, and charging rules), which not only solves the problem of users having difficulty parking and retrieving their cars, but also has great significance for promoting the intelligent and digital development of urban transportation. Attached Figure Description

[0044] Figure 1 This is a flowchart of the path planning method based on the improved A* algorithm and grid in this invention;

[0045] Appendix Figure 2 Here is a flowchart of the path planning algorithm.

[0046] Appendix Figure 3 This is a schematic diagram of a raster network;

[0047] Appendix Figure 4 Schematic diagram of the path planning algorithm;

[0048] Appendix Figure 5 A schematic diagram of a vehicle location system in a large parking lot;

[0049] Appendix Figure 6A schematic diagram illustrating the weight function of the improved A* algorithm;

[0050] Appendix Figure 7 (a) and (b) are comparison charts showing the effects of B-spline curve optimization and optimization without B-spline curves, respectively. Detailed Implementation

[0051] The present invention will be further described below with reference to the accompanying drawings and embodiments. The drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0052] like Figure 1 As shown, a path planning method based on an improved A* algorithm and a grid includes the following steps:

[0053] Step 1: Construct an abstract network model of the parking lot;

[0054] The parking lot is divided into multiple areas, and the application scenario is abstracted using a grid map, while the terrain of the parking lot is displayed in the form of a raster map.

[0055] An ultrasonic sensor is installed at the center of the parking space. Initially, all parking spaces are set to vacant status and are displayed as green dots on the map. When a vehicle is parked, the ultrasonic sensor status changes, and the parking space is displayed as a red dot on the map.

[0056] The parking lot model is laid out using a grid network, with parking lines, walls, and load-bearing columns represented as obstacle points. The application scenario of the parking lot is defined as an N*M grid, denoted as S={s[i][j]}(i=1,2,3,L,n; j=1,2,3,L,n). The coordinates in the set are (x, y). The environmental information values ​​for each raster network are as follows:

[0057]

[0058] like Figure 3 As shown, using a grid network to represent the coordinate information of entities in the network as coordinate points is the data foundation for path planning and positioning.

[0059] Step 2: Find the next node using a dynamic search tree; calculate the weighted value of the node using a weighted heuristic function, select the optimal node, and update the weighted heuristic function in real time to realize the path planning for parking lot addressing;

[0060] For the traditional A* algorithm, the planned route starts with the parent node as the initial node, and the movement trajectory spreads to the surrounding grid nodes. Its search range is generally within four to eight grids. Although reducing the search range can increase the search speed and speed up the formation of the path, the optimality of the path cannot be guaranteed due to the limited movement ability of the nodes.

[0061] To ensure optimal parking lot route planning and operating speed, the following two improvements are proposed:

[0062] (1) Use a dynamic search tree to find the next node;

[0063] (2) Use weighted heuristic functions to accelerate convergence;

[0064] To ensure the optimality of the path, a dynamic search tree is used to search for nodes. Under normal conditions, the search range is the eight nearest nodes. When the next optimal node is an obstacle, the search range is expanded. The maximum number of searched nodes does not exceed twenty-four. However, this reduces the algorithm's running speed to some extent, causing delays in route planning.

[0065] As attached Figure 6 As shown, this invention employs a weighted heuristic function, dynamically adjusting the weights based on the cosine distance between the starting point (A), the candidate node (T), and the target node (B) as a weighting factor. The weighted heuristic function is as follows:

[0066] f(n)=(1-ω)g(n)+ωh(n) (1)

[0067] ω=cosATB=(AT 2 +TB 2 -AB 2 ) / 2*AT*TB (2)

[0068] The above formula is equivalent to:

[0069]

[0070] Where ω∈(-1,1), the denominator represents the length of the two vectors, and the numerator represents the inner product of the two vectors; cosATB represents the cosine of the angle between the candidate node and the initial node and the target node, which shows a monotonically decreasing trend in the interval [0,π]. When the value becomes smaller, the planned route is optimal.

[0071] like Figure 6 The planned route changes direction by turning corners, therefore path smoothing is required. B-spline curves are used to modify the route layout without affecting it. Furthermore, the order of the B-spline curve can be adjusted as needed to achieve a satisfactory smoothing effect. The overall equation is:

[0072]

[0073] Where, N i,k (u) is the basis function of the k-th degree B-spline curve, p i Given the coordinates of the control points;

[0074] N i,k The basis function expression is:

[0075]

[0076] Where C() is the permutation and combination formula, u is the fitting function variable, m is the number of accumulations, k is the order, and f is the index value of the control point.

[0077] Path smoothing is performed using cubic B-spline curves. When there are 4 control points, i.e., k=3, the expression for the basis function is:

[0078]

[0079]

[0080]

[0081]

[0082] Substituting into the overall equation (4), we can obtain the equation of the cubic B-spline curve:

[0083] p(u) = p0 * N 0,3 (u)+p1*N 1,3 (u)+p2*N 2,3 (u)+p3*N 3,3 (u) (10)

[0084] like Figure 7 (a) is a graph showing the B-spline curve fitting effect using the present invention, compared to... Figure 7 (b) shows that the path is smoother when avoiding obstacles without using B-splines.

[0085] As attached Figure 2 The steps to improve the A* algorithm are as follows:

[0086] (1) Set the initial node of the A* algorithm as the parent node. The initial node can move to the surrounding 8 grid cells, generating an initial set Open List(), as shown in the appendix. Figure 4 As shown;

[0087] (2) Using the weighted heuristic function f(n), determine the heuristic function value of each node, select the node with the minimum value of f(n) as the optimal node, add the optimal node to Close List(), and at the same time determine whether the node is an obstacle point;

[0088] (3) If the node is an obstacle, the parent node speeds up its movement and moves to the surrounding 15 grids. At the same time, the weighted heuristic function is used to determine the optimal node. If the optimal node is still an obstacle, the parent node moves to the surrounding 24 grids and selects the optimal node in the set excluding the obstacle node. The maximum number of grids searched is 24 grids around the parent node.

[0089] (4) Determine if the next optimal node is the endpoint. If not, continue to repeat steps (2)-(3) until the optimal node is the endpoint. Then connect the nodes in Close List() into a line to obtain the optimal path.

[0090] (5) Use cubic B-spline curves to smooth the corners of the path and obtain the final node set CloseList();

[0091] Improved A* algorithm pseudocode flow:

[0092]

[0093]

[0094] The node search method of the improved A* algorithm is based on a grid model. It uses the connectivity between nodes to pass heuristic function values, ultimately achieving the goal of searching for the optimal path.

[0095] A positioning system based on an improved A* algorithm and a grid-based path planning method includes:

[0096] Parking space status information collected by wireless sensor networks and ultrasonic sensors is uploaded to the database via the sensor network. During the user parking phase, the optimal parking path is planned based on the selected parking space. During the reverse vehicle search phase, the optimal vehicle search path is planned based on the node information at the time of parking.

[0097] It consists of a data acquisition and processing module, a data query module, a data transmission module, and a data display module;

[0098] The data acquisition and processing module collects the usage status and coordinate information of parking spaces through a wireless sensor network, as well as the coordinate information of users and vehicles. It then processes the data using an improved A* algorithm to complete the optimal route navigation indoors.

[0099] The data query module queries user data and parking space data in the parking lot using database commands.

[0100] The data transmission module is used to connect the front-end management platform and the wireless sensor network hardware platform. Through the connection of the ZigBee protocol and the network layer protocol, the parking space information is digitized and stored in the database for use by the front-end human-machine interface.

[0101] like Figure 5 ZigBee positioning technology, which offers high positioning accuracy, high security, low power consumption and cost, and is easy to network, was selected. At the same time, a fingerprint positioning method based on RSSI (Received Signal Strength Indication) was adopted to determine the user's location by receiving wireless signal transmission models from WSN (Wireless Sensor Networks).

[0102] The RSSI fingerprint localization method consists of two stages: offline and online. In the offline stage, a coordinate system is established according to different scenarios, and a number of feature points are selected as fingerprint points. The selected points are dense and comprehensive. The RSSI of the fingerprint points is then measured using WSN, and the corresponding real coordinates are recorded and added to the fingerprint database.

[0103] During the online phase, when a user enters the established fingerprint database application scenario, the RSSI signal values ​​at each location are first collected using WSN to construct an online RSSI vector. The processed values ​​are then sent to the fingerprint database for matching with the offline collected data. The physical address corresponding to the matching value is used to estimate the location information of the measured entity.

[0104] Because RSSI signal characteristics are affected by the surrounding environment, they are mainly divided into fast fading and slow fading during propagation in space.

[0105] Fast fading is mainly affected by the multipath effect. After the signal is transmitted at the transmitting end, it travels through multiple paths. When it reaches the receiving end, the terminal will receive these signals of the same frequency. However, due to the different paths, the propagation time of these signals is different, and the received signal strength and carrier phase are also different, which will ultimately affect the signal strength of the receiving device.

[0106] Slow fading is mainly affected by the shadowing effect. The shadowing effect is due to the fact that during signal transmission in space, obstacles can affect the signal between the receiver and receiver, especially around the receiver. An electromagnetic blind zone is formed behind the obstacle, and the strength of the received signal in the blind zone will be greatly reduced.

[0107] To reduce the impact of signal attenuation during propagation on the RSSI value, a logarithmic distance path model is selected, as shown in the following formula:

[0108]

[0109] Here, it is assumed that the loss coefficient of the space environment is n, d0 is the reference distance, and PL LD (d) represents the path loss when the distance between the transmitting and receiving devices is d. PL(d0) represents the path loss of the signal at d0 (generally taken as 1m), which is obtained from actual testing. It depends on the surrounding environment and the type of buildings and is used as a reference for the signal attenuation value. The unit is dB. It is derived from the logarithmic distance loss model. Δd represents a certain distance in a fixed direction.

[0110]

[0111] Developed using the Visual Studio software platform, the database management tool SSMS is used to establish the connection, and the connection to SQL Server is made using the TCP / IP protocol.

[0112] The data display module is used for the visualization of digital information, displaying parking space, vehicle, and parking lot information through a web front-end. At the same time, it uses the software's logic program to perform secondary processing on the data, enriching the system's functionality. It mainly includes a login interface, user interface, administrator interface, parking space status information interface, parking space search interface, and reverse parking space search interface.

[0113] Login interface: includes website icon, account text box, password text box, login button, and change password button.

[0114] User interface: includes website icon, parking space status information button, parking space search navigation button, reverse parking space search button, my information button, and parking duration text box.

[0115] Administrator interface: includes buttons for parking space status information, parking duration, and user information.

[0116] Parking space status information interface: Includes a parking lot map module with several parking space boxes. If a parking space is occupied, it is displayed in red; if a parking space is vacant, it is displayed in green.

[0117] The parking space navigation interface includes a prompt text box, an input text box, and a search button. Entering the parking space number and clicking the search button will display a map of the parking lot and plan a route to the parking space.

[0118] Reverse parking space search interface: A one-click car search button. After clicking the button, the system will navigate according to the parking space where the car was parked, display a map of the parking lot and plan a route to the parking space.

[0119] This method uses SQL Server as the database management software, which has the advantages of high efficiency in development, flexibility in use, and high cost-effectiveness.

[0120] The database stores data including parking space information, parking time, departure time, and user information, requiring operations such as adding, deleting, querying, and modifying this information. Therefore, to implement the required functions of the vehicle location system, the following three data tables are created:

[0121] 1) User Table

[0122] Used to store basic information of visiting users (username, password, email, account balance, license plate number).

[0123] Table 1: User

[0124]

[0125]

[0126] 2) Vehicle Information Table (Table Car)

[0127] This table stores basic information about vehicles entering the parking lot, typically linked to the license plate number. Staff can access a user's basic information, such as the vehicle's parking location and duration, through the license plate. The information includes the license plate number, parking space number, entry time, exit time, and parking area.

[0128] Table 2: Car

[0129] field name Remark type primary key length CarID license plate number Varchar T 20 SpaceID Parking space number Varchar F 20 ParkTime entry time Datetime F 20 AwayTime Departure time Datetime F 20 Area Parking area Varchar F 20

[0130] 3) Parking space table (Space)

[0131] This table is used to store parking space information. Users can view the current status of parking spaces (vacant or occupied), and administrators can query usage information (license plate number of the vehicle, vehicle user information, and usage duration), facilitating the management and recording of parking spaces. The parking space table and the vehicle table share a common key, with the parking space number being the primary key of the parking space table.

[0132] Table 3 Space

[0133] field name Remark type primary key length SpaceID Parking space number Varchar T 20 CarID license plate number Varchar F 20 Duration Duration Varchar F 20 State state Boolean F Area area Varchar F 20

[0134] The above are the three data tables created in the SQL Server database. The data collected by the front-end hardware is written into the database, updated in real time, and then operated on in the browser through database command statements.

[0135] Based on the above-described preferred embodiments of the present invention, and through the foregoing description, those skilled in the art can make various changes and modifications without departing from the inventive concept. The technical scope of this invention is not limited to the contents of the specification, but must be determined according to the scope of the claims.

Claims

1. Based on improved A The algorithm and the path planning method of the grid are characterized by, Includes the following steps: Step 1: Construct an abstract network model of the parking lot; Step 2: Find the next node using a dynamic search tree; calculate the weighted value of the node using a weighted heuristic function, select the optimal node, and update the weighted heuristic function in real time to realize the path planning for parking lot addressing; Step two specifically includes: Step 21, Set A The algorithm starts with the parent node as the initial node, moves the initial node to the surrounding 8 grid cells, and generates the initial set Open List(). Step 22: Use the weighted heuristic function to determine the heuristic function value of each node, select the node with the minimum weighted heuristic function value as the optimal node, add the optimal node to Close List(), and at the same time determine whether the node is an obstacle point; The weighted heuristic function is as follows: （1） （3） in, , Let the lengths of the two vectors be denoted as ','. It is the inner product of two vectors; This represents the cosine of the angle between the node to be selected and the initial node and the target node; Step 23: If the node is an obstacle, the parent node increases its movement speed and moves to the surrounding 15 grids. At the same time, the weighted heuristic function is used to determine the optimal node. If the optimal node is still an obstacle, the parent node moves to the surrounding 24 grids and selects the optimal node in the set excluding obstacle nodes. The maximum number of grids searched is 24 grids around the parent node. Step 24: Determine if the next optimal node is the destination. If not, continue repeating steps 22-23 until the optimal node is the destination. Then, connect the nodes in Close List() to form a line to obtain the optimal path. Step 25: Use cubic B-spline curves to smooth the corners of the path and obtain the final node set CloseList().

2. The improved A-based method according to claim 1 The algorithm and the path planning method of the grid are characterized by, Step one specifically includes: An ultrasonic sensor is installed at the center of each parking space. In the initial state, all parking spaces are set to vacant and are marked with green dots on the map. When a vehicle is parked, the parking space is marked with red dots on the map. The parking lot model uses a grid network layout, with parking lines, walls, and load-bearing columns represented as obstacle points. The parking lot is defined as N. There are M network grids, and the grid set is: 。 3. The improved A-based method according to claim 1 The algorithm and the path planning method of the grid are characterized by, When calculating the weighted values ​​of nodes using a weighted heuristic function, a B-spline curve is used to modify the order. The overall equation is: （4） in, for k Basis functions of B-spline curves Given the coordinates of the control points; The basis function expression is: （5） in, C () is a permutation and combination formula. These are the variables of the fitting function. It's the cumulative number of times. For order, f This is the index value of the control point.

4. The improved A-based method according to claim 3 The algorithm and the path planning method of the grid are characterized by, Path smoothing is performed using a cubic B-spline curve, resulting in the equation of the cubic B-spline curve: （10） in, , , , These are the basis function values ​​for control points 1-4, respectively. , , , The coordinates of control points 1-4.

Images