A path planning method, system, device, and medium

Abstract

This invention discloses a path planning method, system, device, and medium, relating to the field of path planning. The method includes: predicting the vehicle's position and motion state at the next moment using a Kalman filter based on the vehicle's position, speed, and acceleration; matching the vehicle's position and motion state data obtained from point cloud data at the next moment with the predicted vehicle position and motion state at the next moment using a Hungarian algorithm to update the Kalman filter; updating the vehicle's position in the constructed traffic environment map in real time using a Simultaneous Localization and Mapping (SLAM) algorithm based on the vehicle's position and motion state data at the current moment; predicting the vehicle's accurate position and motion state in the traffic environment map based on the updated Kalman filter; and using an improved genetic algorithm to plan the path of a traffic detection device to avoid vehicles in the traffic environment. This invention can perform reasonable path planning for moving vehicles or obstacles.

Description

Technical Field

[0001] This invention relates to the field of route planning, and in particular to a route planning method, system, device and medium. Background Technology

[0002] With the rapid increase in the number of motor vehicles, traffic management faces enormous challenges. To improve the efficiency of traffic parameter detection, various traffic detection devices have emerged. These devices mainly include license plate recognition cameras, radar speed detectors, radar-video integrated machines, and infrared sensors. They can monitor vehicles on the road in real time, acquiring information such as vehicle speed, travel time, license plate number, and vehicle type, thereby providing data support for traffic management departments.

[0003] In existing technologies, mobile traffic detection equipment is deployed on different road sections to monitor traffic, improving the coverage and efficiency of monitoring. However, when faced with increasingly complex traffic conditions, existing mobile traffic detection equipment cannot avoid moving vehicles or obstacles on the road in a timely manner during the monitoring process, thus failing to achieve reasonable planning of the monitoring path. Summary of the Invention

[0004] This invention provides a path planning method, system, device, and medium that can solve the problem that existing technologies cannot perform reasonable path planning for moving vehicles or obstacles.

[0005] This invention provides a path planning method, comprising the following steps: using traffic detection equipment to detect and generate point cloud data of the traffic environment and vehicles, and acquiring data related to vehicle position and motion state; at the current moment, using a Kalman filter to predict the vehicle position and motion state at the next moment based on the vehicle's position, speed, and acceleration; using a Hungarian algorithm to match the data related to the vehicle position and motion state at the next moment obtained from the point cloud data with the predicted vehicle position and motion state at the next moment, thereby updating the Kalman filter; using a Simultaneous Localization and Mapping (SLAM) algorithm to update the vehicle's position in the constructed traffic environment map in real time based on the vehicle position and motion state data at the current moment; using the updated Kalman filter to predict the vehicle's accurate position and motion state in the traffic environment map based on the real-time updated vehicle position in the constructed traffic environment map, and employing an improved genetic algorithm to plan the path of the traffic detection equipment to avoid vehicles in the traffic environment.

[0006] Furthermore, the step of using the Hungarian algorithm to match the vehicle position and motion state related data obtained from the point cloud data at the next moment with the predicted vehicle position and motion state at the next moment includes the following steps: taking the vehicle position and motion state related data obtained from the point cloud data at the next moment as the actual state, and matching the predicted vehicle position and motion state at the next moment as the predicted state; constructing an Euclidean distance to measure the difference between the actual state and the predicted state, and constructing a cost matrix using the Euclidean distance; labeling each row and each column in the cost matrix; using multiple straight lines as coverage lines to cover all zero elements in the cost matrix, and finding the coverage line with the minimum number of straight lines among all coverage lines; if the number of straight lines in the coverage line is equal to the size of the cost matrix, obtaining the matching result and completing the matching.

[0007] Furthermore, the updated Kalman filter includes the following steps: determining the actual state corresponding to each predicted state based on the matching results obtained by the Hungarian algorithm; obtaining the residual between the predicted state and the actual state; inputting the residual as an observation into the Kalman filter to update the state vector reflecting the vehicle state estimation and the covariance matrix reflecting the vehicle uncertainty.

[0008] Furthermore, the improved genetic algorithm includes the following specific steps: adding optimization and smoothing operators to the genetic operations of the genetic algorithm; using roulette wheel selection as the selection operator to obtain a new population; designing an adaptive crossover probability as the crossover operator to obtain the fitness values ​​of individuals in the new population, and designing different adaptive crossover probabilities based on different fitness values; and designing an adaptive probability as the mutation operator, wherein the design of the adaptive probability is determined based on the fitness values ​​of the new population.

[0009] Furthermore, the method of using an improved genetic algorithm to plan the path of traffic detection equipment to avoid vehicles in the traffic environment includes the following steps: obtaining the route from the starting point to the target location of the vehicle through genetic operations as the initial population; constructing a fitness function with short path length as the primary criterion and interval as the secondary criterion, and obtaining the fitness value of the initial population; selecting individuals from the initial population through selection operations, and obtaining the probability of each individual being selected using roulette wheel selection; using the selected individuals as a new population, and obtaining the fitness value of the new population, and designing different adaptive crossover probabilities based on different fitness values; when the adaptive crossover probability meets the set requirements, placing the individuals into a mating pool; mating individuals in pairs in the mating pool to determine whether the route corresponding to the mated individuals is feasible; mutating individuals according to the adaptive probability of the designed mutation operator, and selecting the individual with the highest fitness value from the mutated individuals for the next iteration; selecting the individual with the highest fitness value again from the fitness value of the new population after the iteration; when the new population does not meet the set conditions, the algorithm iteration terminates, and the obtained route is smoothed.

[0010] This invention provides a path planning system, comprising:

[0011] The vehicle state acquisition module is used to detect and generate point cloud data of the traffic environment and vehicles using traffic detection equipment, and to acquire data related to vehicle position and motion state. The vehicle prediction module is used to predict the vehicle's position and motion state at the next moment based on the vehicle's position, speed, and acceleration using a Kalman filter. A Hungarian algorithm is used to match the predicted vehicle position and motion state data obtained from the point cloud data with the Kalman filter to update it. The path planning module is used to update the vehicle's position in the constructed traffic environment map in real time using the Simultaneous Localization and Mapping (SLAM) algorithm based on the current vehicle position and motion state data. Based on the real-time updated vehicle position in the constructed traffic environment map, the updated Kalman filter is used to predict the vehicle's accurate position and motion state in the traffic environment map, and an improved genetic algorithm is used to plan the path for the traffic detection equipment to avoid vehicles in the traffic environment.

[0012] This invention provides a computer device, including a memory and a processor; the memory stores a computer program, and the processor executes the computer program to implement the path planning method described above.

[0013] This invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the path planning method described above.

[0014] This invention provides a path planning method, system, device, and medium, which have the following advantages compared with the prior art:

[0015] The Kalman filter is updated using the Hungarian algorithm, enabling it to accurately predict the vehicle's position and motion. After constructing a traffic environment map, the updated Kalman filter is used to predict the vehicle's position and motion within the map. An improved genetic algorithm is then employed to plan the path for traffic detection equipment to avoid vehicles in the traffic environment. Ultimately, by predicting the accurate position and motion of vehicles on the road, timely avoidance is achieved. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of the structure provided for an embodiment of the present invention;

[0017] Figure 2 This is a top sectional view of the vehicle provided in an embodiment of the present invention;

[0018] Figure 3 This is a left sectional view of the vehicle provided in an embodiment of the present invention;

[0019] Figure 4 This is a front sectional view of the vehicle provided in an embodiment of the present invention;

[0020] Figure 5 This is a flowchart of multi-target tracking for traffic parameter detection provided in an embodiment of the present invention;

[0021] Figure 6 The algorithm provided in this embodiment of the invention is used to detect vehicle location maps;

[0022] Figure 7 This is a schematic diagram of the vehicle SLAM algorithm provided in an embodiment of the present invention;

[0023] Figure 8 This is a flowchart of vehicle path planning provided in an embodiment of the present invention;

[0024] Figure 9 This is a path planning result diagram using an improved genetic algorithm provided in an embodiment of the present invention;

[0025] Figure 10 This is a host computer page design diagram provided in an embodiment of the present invention;

[0026] Figure 11 The invention flowchart is provided for embodiments of the present invention.

[0027] Figure label:

[0028] 101-LiDAR detector, 102-Vehicle platform, 103-Drive motor, 104-Vehicle chassis structure, 105-Reserved space for traffic detection equipment, 106-Vehicle drive wheel, 201-Vehicle driven wheel, 202-Reserved standardized interface position, 203-Phillips head screw, 204-LED marker light, 301-LiDAR, 302-Battery pack, 401-Vehicle wheel hub, 402-Connecting column between upper and lower structure of vehicle. Detailed Implementation

[0029] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Many specific details are set forth in the following description to provide a thorough understanding of the present invention. However, the present invention can be practiced in many other ways different from those described herein, and those skilled in the art can make similar modifications without departing from the spirit of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0030] See Figures 1-4 This invention provides a path planning method, comprising the following steps:

[0031] Step 1: Use traffic detection equipment to detect and generate point cloud data of traffic environment and vehicles, and obtain data related to vehicle position and motion status.

[0032] Step 2: At the current moment, based on the vehicle's position, velocity, and acceleration, use a Kalman filter to predict the vehicle's position and motion state at the next moment; use the Hungarian algorithm to match the relevant data of the vehicle's position and motion state at the next moment obtained from the point cloud data with the predicted vehicle position and motion state at the next moment, so as to update the Kalman filter, so that the updated Kalman filter can predict the vehicle's accurate position and motion state.

[0033] Step 3: Based on the vehicle's current position and motion status data, use the Simultaneous Localization and Mapping (SLAM) algorithm to update the vehicle's position in the constructed traffic environment map in real time; based on the real-time updated vehicle position in the constructed traffic environment map, use an updated Kalman filter to predict the vehicle's accurate position and motion status in the traffic environment map, and use an improved genetic algorithm to plan the path of the traffic detection equipment to avoid vehicles in the traffic environment.

[0034] To implement the above path planning method, this invention mainly includes the following modules:

[0035] 1. Mobility Module: The mobility module is responsible for the movement of the vehicle, providing a stable and flexible traffic detection platform. It includes the chassis, drive motor, servo motor, vehicle wheel base structure, and performs motion stability control. In this invention, the traffic parameters defined in Definitions 1 and 2 refer to the parameters defined in this module. This module adopts a mechanical drive structure, with a chassis structure 104 and a drive motor 103. It uses a differential drive system, with the front wheels being driven wheels 201 and the rear wheels being drive wheels 106. When the vehicle turns, the speed difference between the rear wheels is adjusted to avoid operational complexity during cornering control.

[0036] In this module, Definition 1 is defined as follows: The basic principle of turning distance traffic parameters: By treating the left and right wheels as a single unit, during the extremely short time of a vehicle's turning motion, the vehicle's center point moves from one point to another. The corresponding formula for the difference in distance traveled by the left and right wheels is used to solve this problem, reflecting the vehicle's motion state during turning. Definition 2 is defined as follows: The lateral and longitudinal velocities of the vehicle's mass point: The velocity of the vehicle at a certain point is decomposed into lateral and longitudinal velocities in a cross-shaped direction, facilitating directional analysis of the motion state.

[0037] 2. Detection Module: The detection module is responsible for collecting traffic-related data, which in this invention refers to the traffic parameters defined in definitions 3 to 7. This module integrates a LiDAR 101 and has high scalability to adapt to different traffic monitoring needs. The module design reserves standardized interfaces, allowing compatibility with and integration of other types of traffic detection equipment 105, and provides two reserved slots for traffic detection equipment. That is, this vehicle has a built-in LiDAR detector and can also accommodate other types of traffic detectors, such as video detectors, radar-video integrated units, etc., up to three different types of traffic detection equipment, meeting the traffic parameter detection needs of most scenarios.

[0038] In this module, Definition 1 is defined as: the basic principle of lidar for measuring traffic parameters: by emitting laser pulses and measuring their return time, the distance and position of vehicles and other objects on the road are accurately measured. Through continuous scanning, a series of three-dimensional point cloud data can be generated, reflecting the position, shape, and motion state of objects on the road. Definition 2 is defined as: traffic parameter estimation: inferring and calculating traffic flow characteristic parameters through data collection and analysis. Traffic parameters mainly include traffic flow Q, vehicle speed v, and headway D. h Lane occupancy rate O and queue length L qSee Definitions 3-7. Definition 3: Traffic flow: The number of vehicles passing through a location per unit time. Definition 4: Vehicle speed: In this invention, it refers to the instantaneous speed of a vehicle passing through a cross-section of a road. Definition 5: Headway: The distance between the front and rear heads of two adjacent vehicles in the same lane. Definition 6: Lane occupancy: The percentage of a lane occupied by traffic flow within a certain time period. Definition 7: The length of queues formed due to vehicle congestion or stagnation at points such as traffic lights, toll booths, and road bottlenecks.

[0039] The specific calculation method is as follows:

[0040] (1) Vehicle speed detection and measurement is calculated using point cloud data from LiDAR, using the following formula:

[0041]

[0042] Among them, v i (t) is the velocity of vehicle i at time t; (x) i ,y i t1 and t2 are the vehicle's position in the lidar coordinate system; t1 and t2 are two consecutive time points.

[0043] (2) Traffic flow detection and measurement, the formula is as follows:

[0044]

[0045] Where N is the number of vehicles passing through the cross section within time T.

[0046] (3) The formula for detecting and measuring the distance between vehicle front ends is as follows:

[0047]

[0048] Where (x) i-1 ,y i-1 ) and (x i ,y i These are the positions of the car in front and the car behind.

[0049] (4) Lane occupancy detection and measurement, the formula is as follows:

[0050]

[0051] Among them, L i L is the length of vehicle i; total It is the total length of the lane.

[0052] (5) Lane queue length, the formula is as follows:

[0053]

[0054] Where M is the number of vehicles in the queue; D hi It is the distance between the front ends of each vehicle.

[0055] In traffic parameter detection, the Hungarian algorithm combined with a Kalman filter is applied for multi-target tracking, aiming to improve the system's tracking accuracy and stability, and considering multi-vehicle detection in complex traffic scenarios. The following is the workflow, as shown in the flowchart. Figure 5 As shown.

[0056] (1) LiDAR point cloud data acquisition.

[0057] LiDAR continuously scans the environment and generates point cloud data, which reflects the spatial positions of surrounding objects. In traffic scenarios, certain points in the point cloud can be mapped to the positions of vehicles.

[0058] (2) Initialize the Kalman filter.

[0059] The initialization state transition model formula is shown below:

[0060] X t =FX t-1 +Bu t-1 +w t-1

[0061] where X t F is the state vector at the current time t; F is the state transition matrix, describing the state transition relationship from time t-1 to time t; and B is the control matrix, describing the control input u. t-1 The effect on the state, w k-1 It is process noise, usually assumed to be a zero-mean normal distribution with a covariance matrix of Q.

[0062] With 4 state variables (position and velocity) and 2 observation variables (position), the state transition matrix F is:

[0063]

[0064] Where the state vector X t Including position and velocity, the formula is as follows:

[0065]

[0066] The method for obtaining observations from the observation model is shown in the following formula:

[0067] z t =Hx t +v t

[0068] Where z tLet v be the observation vector at the current time t; H is the observation matrix, describing the mapping relationship from the state vector to the observation vector. t This is observation noise, usually assumed to be a zero-mean normal distribution with a covariance matrix of P. Since the direct observation is a two-dimensional position x... t y t Therefore, the observation matrix H is:

[0069]

[0070] (3) Vehicle state prediction is performed using a Kalman filter.

[0071] Suppose that at time t, we have already obtained the vehicle's initial state vector x. t This includes position, velocity, and acceleration information. Using Kalman filtering, based on the vehicle's motion model, the state x at the next time step t+1 can be predicted. t+1|t The prediction steps are formulated as follows:

[0072] x t+1|t =Fx t +Bu t +w t

[0073] P t+1|t =F t P t F t Τ +Q

[0074] (4) Matching lidar point cloud with predicted state.

[0075] The new point cloud data collected by the LiDAR at time t+1 can be used to update the vehicle's state. This new data is then matched with the existing vehicle state; the next step is to use the Hungarian algorithm to solve the optimal matching problem.

[0076] (5) Solving the matching problem using the Hungarian algorithm.

[0077] The Hungarian algorithm is used to solve the bipartite graph matching problem, specifically to match predicted locations with observed data. Its goal is to find a match that minimizes the total cost.

[0078] The detected predicted location dataset is and observation dataset {z k,1 ,z k,2 ,...,z k,m}, calculate the cost matrix (cost matrix) C ij As shown in the formula below:

[0079]

[0080] Where Cij It is the Euclidean distance between the predicted location and the observed data, representing the distance between the predicted location of the i-th vehicle and the j-th point cloud measurement point.

[0081] The Hungarian algorithm finds the optimal vehicle-point cloud data matching scheme by minimizing the total cost in this cost matrix. The algorithm flow is as follows:

[0082] Find the minimum value for each row (predicted vehicle location) and each column (new measurement point), and subtract these minimum values.

[0083] Cover all zero elements and try to find the minimum number of covering lines.

[0084] If the number of covering lines equals the size of the matrix, find the solution; otherwise, adjust the matrix and repeat.

[0085] The goal of the Hungarian algorithm is to find an optimal matching π, and its formula is as follows;

[0086]

[0087] In the PyCharm environment, 'from_scripy.optimize_import_linear_sum_assignment' is implemented through the 'scipy.optimize.linear_sum_assignment' function, which returns the best matching index pair.

[0088] (6) Status update.

[0089] Based on the matching results of the Hungarian algorithm, the vehicle's state vector and covariance matrix are updated. This step integrates the new measurement data into the system, making the tracking state more accurate. The update step formula is as follows:

[0090] K t+1 =P t+1|t H Τ HP t+1|t H Τ +R) -1

[0091] X t+1 =X t+1|t +K t+1 (z t+1 -HX t+1|t )

[0092] P t+1 =(EK) t+1 H)P t+1|t

[0093] Where K t+1It is the Kalman gain, H is the observation matrix, and z is the Kalman gain. t+1 R is the actual measured value, E is the covariance matrix of the measurement noise, and E is the identity matrix.

[0094] (7) Multi-target tracking and motion prediction.

[0095] Through the above steps, the accurate position and motion status of the vehicle can be obtained at each time step t. This information can be further used for traffic parameter detection tasks such as traffic flow analysis, vehicle speed determination, and accident prediction.

[0096] The innovation of this method lies in:

[0097] Combining the Hungarian algorithm with the Kalman filter: This solves the data correlation problem in multi-target tracking and can better match the point cloud data of the LiDAR with the actual position of the vehicle.

[0098] Multi-sensor fusion: It not only relies on lidar, but can also integrate other sensors (such as cameras, radar, etc.) to improve tracking accuracy.

[0099] Dynamically adjust model parameters: Adjust the process noise and measurement noise of the Kalman filter based on actual traffic conditions to adapt to different traffic scenarios.

[0100] This method enables high-precision tracking and real-time motion prediction of multiple vehicles in complex traffic environments. The algorithm results are as follows: Figure 6 As shown.

[0101] 3. Control Module: This module is mainly responsible for controlling the vehicle's movement and detection process. It uses the SLAM algorithm to calculate pose and localization information and constructs a map based on sensor environmental information, realizing pose estimation and map building functions. The pose estimation module obtains the vehicle's pose information through the detection module data to achieve real-time position updates. The map building module uses a map building algorithm to convert the measured values ​​into a map of the vehicle's surrounding environment. An improved genetic algorithm is used for path planning to ensure the accurate execution of the detection task, enabling the vehicle to perform well in complex road environments and solving the vehicle's motion problem in the global traffic environment.

[0102] In this module, Definition 1 is: SLAM algorithm, which stands for Real-Time Localization and Mapping or Concurrent Mapping and Localization; it constructs a 3D incremental map by perceiving its surrounding environment, thereby achieving autonomous localization and navigation. Definition 2 is: Pose estimation, which uses visual sensors to estimate robot pose for applications such as control, robot navigation, and augmented reality; it finds the corresponding points between the real world and the image projection, and adopts appropriate pose estimation methods based on the type of these point pairs, such as 2D-2D, 2D-3D, and 3D-3D. Definition 3 is: Path length refers to the total distance the vehicle needs to travel from the starting point to the destination. Definition 4 is: Interval refers to the distance between the path and obstacles.

[0103] 3.1 This model employs the SLAM algorithm for real-time localization and map updating. t-1 Let z be the pose of the vehicle at time t-1. At this time, the intelligent vehicle further obtains the observation vector z by observing real road signs through lidar. t-1 The environmental map, m, is obtained based on the pose information and the observation vector. t Subject to motion control vector u t Controlling the movement process to form the pose x at time t t During the process, the pose of the intelligent vehicle and the road signs are continuously iterated and estimated to further complete the global update.

[0104] Depend on Figure 7 The motion control vector u can be obtained. t With the observation vector z t Together they determine the pose vector x t and map feature vector m t In other words, the SLAM algorithm can be mathematically described by a joint probability distribution, as shown in the formula:

[0105] Bel(x 1:t )=p(x 1:t ,m 1:t |u 1:t ,z 1:t )

[0106] To enable vehicles to perform well in complex road environments, this study addresses the motion problem of intelligent vehicles in global traffic environments by employing an improved genetic algorithm for path planning, ensuring the accurate execution of detection tasks.

[0107] Step 1: Initial Operations: This includes determining the encoding method, dividing path points, and designing the fitness function.

[0108] 3.2 This module uses polygonal environment modeling, treating vehicles as point masses. Through genetic operations, a suitable route from the starting point to the target location is found for the vehicle. Coordinate encoding is applied to encode the individual paths at the planned location from the starting point to the destination as follows:

[0109] S = l1 + l2 + ... + l n-2 +l n+1

[0110] In the formula, S represents the individual symbol, l i Let be the line segment connecting the i-th node and the (i+1)-th node.

[0111] 3.3 Classifying Path Points: The first type of infeasible path points are those that fall into obstacles, determined using the inpolygon function.

[0112] IN = inpolygon(x,y,xv,yv)

[0113] In the formula, (x,y) are the coordinates of the path points; (xv,yv) is the vertex coordinate matrix of the polygon.

[0114] The second type of infeasible path point: When a new individual is generated by connecting two points and passing through an obstacle, a detection mechanism is set up to judge. During the operation, the connecting line is divided into equal parts, and it is judged whether the dividing point in the line segment is within the obstacle for testing.

[0115] 3.4 Design of fitness function.

[0116] The fitness function f is based on the shortest path length as the primary criterion and indirectness as a secondary criterion. it1 It consists of two parts.

[0117] Path length.

[0118] This refers to the total distance a vehicle needs to travel from the starting point to the destination, and the specific formula is as follows:

[0119]

[0120] In the formula, f is a constant that can be adjusted appropriately during the simulation. As the path length becomes shorter, f... it1 It's getting bigger and bigger.

[0121] Interval degree.

[0122] Considering safety factors, the interval refers to the distance between the path and obstacles. The interval function is as follows:

[0123]

[0124] In the formula, g i Let be the shortest distance between the i-th line segment and the obstacle; α is a constant that can be adjusted appropriately during the simulation.

[0125] In summary, the fitness function f it It is obtained by summing the weights of their respective functions:

[0126] f it =w d *f it1 +w c *f it2

[0127] w d and w c These are the weighting coefficients.

[0128] Step 2: Generation of the first generation population and calculation of the fitness values ​​of individuals in the population.

[0129] Step 3: To further improve the genetic algorithm-based path planning, optimization and smoothing operators are added to the genetic operations. Selection operation operators, autocorrelation crossover and mutation probabilities are designed, and the following genetic operations are performed.

[0130] 3.5 Selection Operation:

[0131] A new selection operator is designed so that, after the selection operation in the initial population, individuals with higher fitness values ​​have a higher probability of being selected for inheritance, while individuals with lower fitness values ​​may also be passed on to the next generation, thus ensuring population diversity. A roulette wheel selection algorithm is used to calculate the probability of each individual being selected, as follows:

[0132]

[0133] 3.6 Autocorrelation Crossover Probability:

[0134] For the crossover operator, an adaptive crossover probability is designed. After selection, the fitness value of individuals in the new population is calculated, and different adaptive crossover probabilities are designed based on different fitness values. When the adaptive crossover probability of an individual meets the set conditions, the individual is placed into the mating pool. After the above judgment conditions are met, individuals in the mating pool are mated in pairs, and then it is judged whether the path after mating is a feasible path. If it is not feasible, it is abandoned; if it is feasible, it is retained.

[0135]

[0136] In the formula, p = 0.85, Fl max F represents the maximum fitness value of an individual in the population. m f is the average fitness value; it (i) is the fitness value of the i-th individual.

[0137] 3.7 Autocorrelation Probability Design:

[0138] The mutation operator also adopts adaptive probability. The probability design is determined based on the fitness value of the new population after the crossover process. Individuals mutate according to the set probability. After mutation, the feasibility of the path still needs to be checked. If it is an infeasible path, it will be mutated again.

[0139]

[0140] In the formula, Fl max F represents the maximum fitness value of an individual in the population. m f is the average fitness value. it (i) is the fitness value of the i-th individual.

[0141] 3.8 Optimized operation.

[0142] To prevent the optimal path from being lost during iteration, an optimization process is added. After the mutation operation, the fitness value of the new population is calculated, and the individual with the highest fitness value is recorded. After the next iteration, it is checked whether a better individual has appeared. If so, the new individual with the highest fitness value is recorded.

[0143] Step 4: Smooth operation.

[0144] After the genetic operation is completed, it is determined whether the termination condition is met. If the new population does not meet the set condition, the third step of the genetic operation is continued; otherwise, if the condition is met, the algorithm iterative operation terminates, and the recorded optimal path is smoothed before the final result is output.

[0145] During smoothing, a user-defined angle β is set. If the angle at the turning point of the planned path is less than β, smoothing is required. If the angle at the point (x2, y2) is greater than β, two new nodes are added at that point, with the coordinate formula as follows:

[0146]

[0147] In the formula, x1, x2, and x3 are the x-coordinates of three points along the path; y1, y2, and y3 are the y-coordinates of three points along the path.

[0148] This paper optimizes the travel paths of vehicles on roads. Using a Python development environment, improvements are made to the genetic algorithm in aspects such as pathpoint partitioning, selection operation design, crossover and mutation autocorrelation probability design, smoothing, and optimization operations, further optimizing the vehicle paths. The flowchart of the improved genetic algorithm for path planning is shown below. Figure 8 As shown in the diagram. The result of the path planning is shown in the figure below. Figure 9 As shown, obstacles are set as (2,0), (2,2), (2,4), (6,6), (8,6), (8,4), which are points 8-13 in the diagram. The results show that the path planning can avoid obstacles by a relatively large distance, thus achieving successful vehicle path planning. The overall flowchart is as follows. Figure 11 As shown.

[0149] 4. Communication Module: This module is mainly responsible for enabling data exchange and remote control between the detection module, control module, etc., and external systems. It supports real-time data transmission and remote operation. It designs a ZigBee-based intelligent vehicle wireless communication system, including the vehicle's underlying serial communication design, ZigBee network construction, and user host computer design.

[0150] The system mainly consists of three parts: a host computer, a wireless communication module, and a carrier. The host computer is equipped with a vision tracking device to continuously track targets, performs real-time road traffic positioning based on pose estimation results, and communicates with the host computer. The vision module uses a wide-angle camera and a gimbal camera to achieve visual tracking of traffic conditions, expanding the camera's accuracy and range. The wide-angle camera performs area monitoring and target detection for traffic targets, transmitting the information to the gimbal camera. The dynamic gimbal camera searches for targets in space based on the feedback information, further achieving real-time positioning.

[0151] The communication module selected is the ZigBee CC2530 module. One module, connected to the host computer via RS232 serial port, acts as the coordinator, responsible for the construction and maintenance of the entire wireless network. The other module, connected to the vehicle via RS232 serial port, is used as a terminal node in the entire system. It can apply to join the network built by the coordinator and, with the coordinator's permission, achieve short-range communication between the two.

[0152] The communication function mainly includes two aspects: first, the host computer sends instructions to control the motion mode of the vehicle; second, the vehicle transmits information to the host computer through sensors and other detection modules, and the control module performs path planning and designs the actual path according to the detection requirements of each module.

[0153] Definition 1. Serial communication: A way for the CPU to exchange information with the external environment.

[0154] Definition 2. The ZigBee protocol is a collection of communication standards, specifically manifested as the ZigBee protocol stack. The Z-Stack consists of the physical layer, media access layer, network layer, and application layer.

[0155] 4.1 Serial Communication Design

[0156] The vehicle and terminal node communicate via a serial port, utilizing the CC2530 circuit. An LED status display circuit is provided on the CC2530 module baseboard. Power supply and CC2530 module output signals are observed through the display of four LEDs. LEDs 1, 2, 3, and 4 are connected to barcode P3, and LED 2 is connected to P4.

[0157] The ZigBee CC2530 module, connected to the host computer, acts as a coordinator. Network setup is an automatic process. The coordinator first scans the channels specified by the application, selecting the channel with the least interference and collisions. After determining the channel, it needs to set its network parameters, such as device logical type, startup mode, beacon time, and superframe length. Then, the coordinator starts and sends a network formation request to the network layer. The entire network setup process is completed after the network layer confirms the request.

[0158] Before an end node can apply to join a network, it must first search for existing ZigBee networks in the vicinity, directly call the beacon request command at the MAC layer, and after receiving a successful association response, configure its own network address according to the command transmitted by the coordinator. After the coordinator updates the network status, the end node successfully joins the ZigBee network.

[0159] 4.2 Host Computer Interface Design

[0160] To achieve bidirectional data transmission and reception with the communication module, a corresponding host computer system was designed. This system uses a graphical user interface (GUI) to control the intelligent vehicle and to capture environmental information and the vehicle's own motion status, such as... Figure 10 The image shown is a diagram of the host computer's web interface.

[0161] Graphical interface development utilizes the MSComm control and API functions, accessing underlying functions through the ActiveX control interface and calling those functions to complete predetermined tasks.

[0162] (1) Implement the interface operation design to complete the opening and closing of the serial port and the configuration of the serial port;

[0163] (2) The host computer interface of the intelligent vehicle sends data to perform walking tests and mode selection operations, and sets parameters such as network address, speed, and distance.

[0164] (3) Use the WriteFile function to copy a specified number of characters to the output buffer and send them out via the serial port. Pre-set the corresponding path selection and perform path planning.

[0165] (4) Receiving area setting: Using the data information corresponding to the detection module, a character detection mechanism is added to identify the information. According to different characters, the data is transmitted to the corresponding editing box. The host computer obtains the data and reads a specified number of characters from the serial port input buffer through the ReadFile function to further realize the reception and termination of the data.

[0166] Its application scenarios are introduced as follows:

[0167] This invention can be deployed in road networks to collect real-time data on traffic flow, vehicle speed, and congestion, providing dynamic monitoring and data support for traffic management. In emergency response scenarios, vehicles equipped with various traffic detection devices can be rapidly deployed to provide traffic information at disaster sites, detect road conditions, and offer optimal route suggestions to rescue vehicles. For research and development in intelligent driving and vehicle-to-everything (V2X) communication, the vehicle can simulate real traffic scenarios, providing dynamic road data to support the testing and optimization of intelligent driving algorithms. It can also be used as laboratory teaching equipment, providing students majoring in intelligent transportation with opportunities to learn about traffic data collection.

[0168] After application, the advantages of this invention will be demonstrated in the following aspects:

[0169] 1. Improve detection flexibility: Through the mobile vehicle platform, it can move freely in different traffic scenarios, adapt to various complex road environments, and carry out traffic detection from multiple angles and locations.

[0170] 2. Improve detection accuracy: Integrating high-precision sensors such as radar, lidar and cameras, it can accurately collect traffic data, such as vehicle speed, traffic flow and lane occupancy, to ensure the accuracy of detection results.

[0171] 3. Enhanced real-time performance and intelligence: Through advanced control algorithms and communication modules, it can process and transmit traffic data in real time, support remote control and real-time monitoring, and improve the efficiency and response speed of traffic management.

[0172] 4. Detection Function Combination: The vehicle is designed with reserved interfaces and a stable platform that can carry different detection devices. It can carry up to three different traffic detection devices, enabling multiple devices to support multiple detection tasks, such as travel time detection, traffic flow analysis, vehicle speed recognition, etc., realizing multiple devices and multiple uses on one vehicle, and data fusion.

[0173] This invention provides a path planning system, comprising:

[0174] The vehicle state acquisition module uses traffic detection equipment to detect and generate point cloud data of the traffic environment and vehicles, and acquires data related to vehicle position and motion state. The vehicle prediction module uses a Kalman filter to predict the vehicle's position and motion state at the next moment based on the vehicle's position, speed, and acceleration at the current moment; it then uses a Hungarian algorithm to match the predicted vehicle position and motion state data obtained from the point cloud data with the Kalman filter to update the Kalman filter. The path planning module uses Simultaneous Localization and Mapping (SLAM) to update the vehicle's position in the constructed traffic environment map in real time based on the current vehicle position and motion state data; it then uses the updated Kalman filter to predict the vehicle's accurate position and motion state in the traffic environment map based on the updated vehicle position in the constructed traffic environment map, and employs an improved genetic algorithm to plan the path for the traffic detection equipment to avoid vehicles in the traffic environment.

[0175] This invention provides a computer device, including: a memory and a processor; the memory stores a computer program, and the processor executes the computer program to implement the steps of a path planning method.

[0176] This invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of a path planning method.

[0177] A specific example is as follows:

[0178] 1. The mobile module is responsible for the movement of the vehicle, providing a stable and flexible traffic detection platform.

[0179] 2. The detection module is responsible for collecting traffic-related data, such as vehicle speed, traffic flow, and lane occupancy.

[0180] 3. The control module is responsible for controlling the movement of the vehicle and the detection process to ensure the accurate execution of the detection task.

[0181] 4. The communication module is responsible for enabling data exchange and remote control between the detection module, control module, etc., and external systems. It supports real-time data transmission and remote operation, and designs a ZigBee-based intelligent vehicle wireless communication system.

[0182] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of this invention patent should be determined by the appended claims.

Claims

1. A path planning method, characterized in that, Includes the following steps: Traffic detection equipment is used to detect and generate point cloud data of traffic environment and vehicles, and to obtain data related to vehicle position and motion status. At the current moment, based on the vehicle's position, velocity, and acceleration, a Kalman filter is used to predict the vehicle's position and motion state at the next moment. The Hungarian algorithm is used to match the vehicle position and motion state data obtained from the point cloud data at the next moment with the predicted vehicle position and motion state at the next moment in order to update the Kalman filter. Based on the vehicle's current position and motion status data, the Simultaneous Localization and Mapping (SLAM) algorithm is used to update the vehicle's position in the constructed traffic environment map in real time. Based on the real-time updated position of the vehicle in the constructed traffic environment map, an updated Kalman filter is used to predict the vehicle's accurate position and motion status in the traffic environment map, and an improved genetic algorithm is used to plan the path of the traffic detection equipment to avoid vehicles in the traffic environment. The method of using an improved genetic algorithm to plan the path of traffic detection equipment to avoid vehicles in the traffic environment includes the following steps: The route from the starting point to the target location of the vehicle is obtained through genetic operations as the initial population; Using short path length as the primary criterion and interval as the secondary criterion, a fitness function is constructed, and the fitness values ​​of the initial population are obtained. Individuals are selected from the initial population through a selection operation, and the probability of each individual being selected is obtained using a roulette wheel method. Multiple selected individuals are used as a new population, and the fitness values ​​of the new population are obtained. Different adaptive crossover probabilities are designed based on different fitness values. When the adaptive crossover probability meets the set requirements, the individuals are put into the mating pool. In the mating pool, individuals are paired up to determine whether the route corresponding to the mated individuals is feasible. Individuals are mutated according to the adaptive probability of the designed mutation operator, and the individual with the highest fitness value is selected from the mutated individuals for the next iteration; the individual with the highest fitness value is selected again from the fitness values ​​of the new population after the iteration. When the new population meets the set conditions, the algorithm iteration terminates and the obtained route is smoothed.

2. The path planning method as described in claim 1, characterized in that, The process of using the Hungarian algorithm to match the vehicle position and motion state data obtained from point cloud data at the next moment with the predicted vehicle position and motion state at the next moment includes the following steps: The vehicle position and motion state data obtained from point cloud data at the next moment are used as the actual state, and the predicted vehicle position and motion state at the next moment are matched as the predicted state. Construct a Euclidean distance to measure the difference between the actual state and the predicted state, and construct a cost matrix using the Euclidean distance; Label each row and column of the cost matrix; The cost matrix is ​​covered by multiple straight lines connected together, and the minimum number of straight lines connected to cover the zero elements is found among all the covering lines. If the number of straight lines covering the coverage line is equal to the size of the cost matrix, the matching result is obtained and the matching is completed.

3. The path planning method as described in claim 2, characterized in that, The specific steps for updating the Kalman filter include: Based on the matching results obtained from the Hungarian algorithm, determine the actual state corresponding to each predicted state; Obtain the residual between the predicted state and the actual state; The residuals are input as observations into the Kalman filter to update the state vector reflecting the vehicle state estimation and the covariance matrix reflecting the vehicle uncertainty.

4. The path planning method as described in claim 1, characterized in that, The improved genetic algorithm includes the following steps: Optimization and smoothing operators are added to the genetic operations of the genetic algorithm; A new population is obtained by using roulette wheel selection as the selection operator. We design an adaptive crossover probability as a crossover operator to obtain the fitness values ​​of individuals in the new population, and design different adaptive crossover probabilities based on different fitness values. An adaptive probability is designed as a mutation operator, and the design of the adaptive probability is determined based on the fitness value of the new population.

5. A path planning system, characterized in that, include: The vehicle status acquisition module is used to detect and generate point cloud data of traffic environment and vehicles using traffic detection equipment, and to acquire data related to vehicle position and motion status. The vehicle prediction module is used to predict the vehicle's position and motion state at the next moment based on the vehicle's position, speed, and acceleration at the current moment using a Kalman filter. The Hungarian algorithm is used to match the vehicle position and motion state data obtained from the point cloud data at the next moment with the predicted vehicle position and motion state at the next moment in order to update the Kalman filter. The path planning module is used to update the vehicle's position in the constructed traffic environment map in real time based on the vehicle's current position and motion state data using the Simultaneous Localization and Mapping (SLAM) algorithm. Based on the real-time updated vehicle position in the constructed traffic environment map, the module uses an updated Kalman filter to predict the vehicle's accurate position and motion state in the traffic environment map, and employs an improved genetic algorithm to plan the path of the traffic detection equipment to avoid vehicles in the traffic environment. The method of using an improved genetic algorithm to plan the path of traffic detection equipment to avoid vehicles in the traffic environment includes the following steps: The route from the starting point to the target location of the vehicle is obtained through genetic operations as the initial population; Using short path length as the primary criterion and interval as the secondary criterion, a fitness function is constructed, and the fitness values ​​of the initial population are obtained. Individuals are selected from the initial population through a selection operation, and the probability of each individual being selected is obtained using a roulette wheel method. Multiple selected individuals are used as a new population, and the fitness values ​​of the new population are obtained. Different adaptive crossover probabilities are designed based on different fitness values. When the adaptive crossover probability meets the set requirements, the individuals are put into the mating pool. In the mating pool, individuals are paired up to determine whether the route corresponding to the mated individuals is feasible. Individuals are mutated according to the adaptive probability of the designed mutation operator, and the individual with the highest fitness value is selected from the mutated individuals for the next iteration; the individual with the highest fitness value is selected again from the fitness values ​​of the new population after the iteration. When the new population meets the set conditions, the algorithm iteration terminates and the obtained route is smoothed.

6. A computer device, comprising: Memory and processor; The memory stores a computer program, characterized in that the processor executes the computer program to implement a path planning method according to any one of claims 1 to 4.

7. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements a path planning method as described in any one of claims 1 to 4.

Images