Obstacle recognition method for road roller
The location of obstacles is predicted through Kalman filtering and particle filtering algorithms, and combined with artificial potential field method and kinematic constraints, the problem of low efficiency and insufficient accuracy of obstacles avoided by the roller in the prior art is solved, and the problem of further improvement Efficient and safe avoidance decisions.
Patent Information
- Application Number
- CN202410593166.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-05-14
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-05-14
AI Technical Summary
The existing rollers have low efficiency in avoiding obstacles, the sensors are susceptible to environmental interference, and cannot accurately predict the movement trend of obstacles, resulting in insufficient initiative and continuity in avoidance.
Kalman filtering and particle filtering algorithms are used to predict obstacle positions, and local avoidance paths are generated by combining artificial potential field method and kinematic constraints. The global travel route is optimized through the A* path planning algorithm to improve avoidance efficiency.
It improves the efficiency and safety of the roller to avoid obstacles, enhances the accuracy of the motion state of the obstacle and the reliability of path planning, and adapts to dynamic environmental needs.
Smart Images

Figure CN118518127B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the technical field of obstacle avoidance, and in particular to an obstacle recognition method for a road roller. Background Art
[0002] At present, there are two main methods for rollers to avoid obstacles: one is manual operation, that is, the driver manually controls the roller to bypass obstacles based on his own experience and observation; the other is sensor detection, that is, ultrasonic, laser and other sensors are installed on the roller to detect the position of the obstacle and automatically control the roller to avoid it.
[0003] However, both methods have certain limitations. The manual operation method is overly dependent on the driver's experience and reaction ability. Once the driver makes a misjudgment or improper operation, it is easy to cause a safety accident. In addition, the efficiency of manual obstacle avoidance is low and cannot adapt to the ever-increasing construction progress requirements. Although the sensor detection method realizes the automation of obstacle avoidance, the sensor is easily disturbed by environmental factors, resulting in reduced accuracy and reliability of obstacle identification. In addition, the sensor detection method can only obtain the instantaneous position information of the obstacle, and cannot predict the movement trend of the obstacle, and the initiative and continuity of the avoidance are insufficient.
[0004] In the related art, for example, Chinese patent document CN116872940A provides an obstacle identification method for a roller group, wherein the roller group includes a plurality of rollers that are connected to each other in communication; wherein the obstacle identification method for the roller group includes: obtaining the perceived obstacle information of each roller in the roller group; wherein each roller respectively senses an obstacle to obtain corresponding perceived obstacle information; integrating the perceived obstacle information of all rollers in the roller group to obtain the marked obstacle information of the working area of the roller group; and sending the marked obstacle information to all rollers in the roller group. However, in this scheme, the obstacle identification and information distribution of the roller group is an offline process, and the real-time avoidance decision of the roller is not considered, so the avoidance efficiency of this scheme needs to be further improved. Summary of the invention
[0005] In response to the problem of low efficiency of obstacle avoidance for rollers in the prior art, the present application provides an obstacle recognition method for a roller, which uses Kalman filtering and particle filtering algorithms to predict the position of obstacles, and combines artificial potential field method and roller kinematic constraints to generate local avoidance paths, thereby improving the avoidance efficiency.
[0006] The purpose of this application is achieved through the following technical solutions.
[0007] This specification provides an obstacle identification method for a road roller, including: Step 1: Setting a sensor on the road roller to collect environmental data. The sensor can be a laser radar, a millimeter wave radar, an ultrasonic sensor, etc., and the environmental information around the road roller is collected by the sensor to obtain the type of obstacles (such as pedestrians, vehicles, stones, etc.) and position coordinates. Step 2: Obtain the road roller's travel trajectory record and obstacle prior information. The road roller's travel trajectory record can be obtained through positioning devices such as GPS and INS. The obstacle prior information includes the type of obstacles, location distribution, movement trend, etc., which can be obtained by analyzing historical data. Using the A* path planning algorithm, with the current position of the road roller as the starting point and the target construction area as the end point, the travel trajectory record and obstacle prior information are comprehensively considered to set the global travel route of the road roller and generate a path planning instruction. Step 3: According to the path planning instruction, control the road roller to operate along the global travel route. The control system of the road roller receives the path planning instruction and controls the speed, direction, etc. of the road roller so that it travels according to the set global travel route. During the operation, the sensor continuously collects environmental data and monitors obstacle information in real time. Step 4: Predict the position of the obstacle based on the collected environmental data. First, the Kalman filter algorithm is used to predict the position and speed of the obstacle at the next moment based on the kinematic model of the obstacle (such as the uniform motion model). Then, the particle filter algorithm is used to update the predicted obstacle position based on the observation model of the obstacle (such as the ranging error model) to obtain the predicted coordinates of the obstacle. The particle filter algorithm initializes a group of particles and continuously approaches the true position of the obstacle through steps such as resampling, prediction, and updating. Step 5: Calculate the distance between the predicted coordinates of the obstacle and the roller, and compare the distance with the preset distance threshold. By calculating the Euclidean distance between the predicted coordinates of the obstacle and the current position of the roller, it is determined whether the distance between the two is less than or equal to the preset distance threshold. The setting of the distance threshold needs to comprehensively consider factors such as the braking distance of the roller and the time required for avoidance. Step 6: When the distance between the obstacle and the roller is less than or equal to the distance threshold, the avoidance mechanism is triggered. With the roller as a reference, an artificial potential field model is established. In this model, obstacles exert repulsive force on the roller, and the target point (the next target position on the global route) exerts attractive force on the roller. By searching for the path with the minimum potential energy in the artificial potential field, the local path to avoid the obstacle is obtained. In order to make the local path meet the kinematic constraints of the roller, such as the minimum turning radius, maximum speed, etc., the local path is smoothed to generate a path adjustment instruction that meets the actual movement capacity of the roller. Step 7: According to the path adjustment instruction, the roller is controlled to move along the local path to avoid the obstacle until the obstacle is bypassed. The control system of the roller receives the path adjustment instruction, controls the roller to temporarily deviate from the global route, and executes the local avoidance path. When the roller successfully bypasses the obstacle, the control system replans the path back to the global route, controls the roller to return to the global route, and continues to perform the task.
[0008] Among them, the prior information of obstacles includes one or more of the following: Type of obstacle: According to the shape, size and material of the obstacle, the obstacles are divided into different types, such as stones, trees, vehicles, pedestrians, etc.; different types of obstacles have different movement characteristics and danger levels, and different avoidance strategies are required; Location distribution of obstacles: Statistically analyze historical data to obtain the spatial distribution law of obstacles in the roller operation area, and generate a probability distribution map of the obstacle location; Based on the probability distribution map, predict the location of the obstacle on the global route to improve the pertinence and efficiency of obstacle detection; Movement trend of obstacles: Analyze the historical trajectory data of obstacles to obtain the movement direction, speed and acceleration of the obstacles. Speed and other motion parameters are used to establish the motion model of the obstacle; based on the motion model, the movement trend of the obstacle in the future is predicted to provide a basis for formulating the avoidance strategy; the danger level of the obstacle: according to the type of obstacle, movement trend and distance from the roller, the danger degree of the obstacle to the safety of the roller is evaluated and divided into different danger levels; for obstacles with high danger levels, emergency avoidance measures are taken, and for obstacles with low danger levels, conventional avoidance measures are taken; the recognition features of the obstacle: the color, texture, edge and other features of the obstacle are extracted to establish the recognition model of the obstacle; in the environmental data collected by the sensor, the recognition model is used to detect and identify the obstacles to improve the accuracy and real-time performance of obstacle recognition.
[0009] Furthermore, the path planning instructions are generated, including: Step 2.1: Divide the working area of the roller into grids of equal size, each grid represents a node, and the nodes are connected by edges to construct a grid map. The size of the grid needs to be reasonably set according to factors such as the size of the roller, the size of the working area, and the distribution density of obstacles. It is necessary to ensure that the grid is small enough to improve the accuracy of path planning, and to avoid the grid being too small, resulting in too large a search space and too high computational complexity. Step 2.2: According to the collected environmental data and obstacle prior information, the nodes corresponding to the obstacles are marked in the grid map as non-passable nodes, and the other nodes except the obstacles are passable nodes. The environmental data is collected in real time by sensors, reflecting the current position of the obstacles; the obstacle prior information is obtained by analyzing historical data, reflecting the type, location distribution, movement trend, etc. of the obstacles. Considering the two types of information comprehensively, the nodes in the grid map are marked. The non-passable nodes represent the areas that the roller cannot pass through, and the passable nodes represent the areas that the roller can pass through. Step 2.3: According to the roller's travel trajectory record, the starting node and target node of the roller in the grid map are determined. The starting node indicates the current location of the roller, which can be obtained through positioning devices such as GPS; the target node indicates the target location that the roller needs to reach, which can be set according to the requirements of the construction task. The starting node and target node are mapped to the corresponding nodes in the grid map as the starting point and end point of the path planning.
[0010] Step 2.4: Create an open list and a closed list. The open list is used to store the nodes to be searched, and the closed list is used to store the nodes that have been searched. The open list and the closed list are the core data structures of the A* algorithm, which are used to record the status of the nodes during the search process. Initially, both the open list and the closed list are empty. Step 2.5: Add the starting node to the open list, calculate the actual distance and estimated distance of the starting node, the actual distance represents the actual path length from the starting node to the current node, the estimated distance uses the Manhattan distance to calculate the estimated path length from the current node to the target node, and the sum of the actual distance and the estimated distance of the starting node is used as the evaluation function value of the starting node. Among them, the actual distance is initialized to 0, and the estimated distance can be obtained by calculating the sum of the absolute values of the row and column differences between the starting node and the target node in the grid map. The evaluation function value is used to evaluate the priority of the node. The smaller the value, the more likely the node is to be on the optimal path and should be searched first. In the subsequent steps, the A algorithm is based on the starting node, and continuously searches for the optimal path by expanding nodes, updating evaluation function values, and other operations until the target node is found or all possible paths are searched. By selecting the node with the smallest evaluation function value for expansion, the A algorithm can quickly converge to the optimal solution and ensure that the path found is the global optimal one. This solution constructs a grid map to discretize the continuous working area, simplifying the solution of the path planning problem; by comprehensively utilizing real-time sensor data and prior information, the obstacle position is accurately marked, and the environmental adaptability of path planning is improved; by using the A* algorithm for search, the optimal path can be found efficiently and accurately, and path planning instructions can be generated, providing a decision-making basis for the autonomous operation of the roller.
[0011] Further, generating a path planning instruction also includes: Step 2.6: Selecting the node with the smallest evaluation function value from the open list as the current node, removing the current node from the open list, and adding it to the closed list. The node with the smallest evaluation function value indicates that it is the node most likely to be on the optimal path in the current search state, and should be searched first. Adding the current node to the closed list indicates that the node has been searched to avoid repeated searches. Step 2.7: Determine whether the current node is the target node. If so, execute step 2.12; otherwise, execute step 2.8. This step is used to determine whether the optimal path from the start node to the target node has been found. Step 2.8: Traverse all adjacent passable nodes of the current node, and for each adjacent node, calculate the actual distance from the current node to the adjacent node. The adjacent node refers to the node directly connected to the current node, and the passable node represents the area that the roller can pass through. The actual distance can be obtained by calculating the geometric distance between the current node and the adjacent node in the grid map. Step 2.9: Determine whether the adjacent node is in the closed list. If so, skip the corresponding node and continue to traverse the next adjacent node; otherwise, execute step 2.10. The nodes in the closed list have been searched and do not need to be searched again. Step 2.10: Determine whether the adjacent node is in the open list. If not, add the corresponding adjacent node to the open list, calculate the estimated distance and evaluation function value of the corresponding adjacent node, and set the current node as the parent node of the adjacent node; if so, compare the actual distance currently calculated with the actual distance stored in the open list. If the actual distance currently calculated is less than the actual distance stored, update the actual distance, estimated distance, evaluation function value and parent node of the adjacent node. The estimated distance uses Manhattan distance to calculate the estimated path length from the adjacent node to the target node. The evaluation function value is the sum of the actual distance and the estimated distance. By comparing the actual distances, it can be determined whether a better path has been found. If so, update the relevant information.
[0012] Step 2.11: Repeat steps 2.6 to 2.10 until the current node is the target node or the open list is empty; then execute step 2.12 or step 2.13. This step is used to control the search process. When the target node is found or all possible paths are searched, the search ends. Step 2.12: If the open list is empty, it means that the complete path from the start node to the target node cannot be found. The currently searched path is used as the global travel route of the roller, and according to the end node of the global travel route, the corresponding adjacent passable nodes are added to the open list, and then return to step 2.6 to continue searching for the remaining paths. This step is used to deal with the situation where the complete path cannot be found. By using the searched partial path as the global travel route and continuing to search the remaining paths, a feasible path is planned as much as possible. Step 2.13: Starting from the target node, backtrack along the parent node of the node to generate the optimal path from the start node to the target node as the global travel route of the roller. By backtracking in reverse, the complete optimal path can be obtained as the basis for the roller's travel. This solution uses the A* algorithm for path planning. By setting an open list and a closed list and using the evaluation function value to guide the search direction, it can efficiently and accurately find the optimal path from the start node to the target node. At the same time, by setting alternative strategies, it can handle situations where a complete path cannot be found, thereby improving the robustness of path planning. The generated global route provides a decision-making basis for the autonomous operation of the roller, improving operating efficiency and safety.
[0013] Furthermore, generating a path planning instruction also includes: Step 2.14: Smoothing the generated global route of the roller to obtain a smooth global route. Since the path generated by the A* algorithm is based on a grid map, there may be sharp angles at the turning points on the path, which does not conform to the motion characteristics of the roller. In order to enable the roller to move smoothly and continuously, the global route needs to be smoothed. Commonly used smoothing algorithms include Bezier curves, B-spline curves, etc., which generate a smooth and continuous path by interpolation. Smoothing needs to consider kinematic constraints such as the minimum turning radius and maximum speed of the roller to ensure that the generated path is within the range of the roller's motion capability. Step 2.15: Based on the smoothed global route of the roller, combined with the speed, acceleration and steering angle of the roller, a cubic spline interpolation algorithm is used to generate a path planning instruction. The cubic spline interpolation algorithm is a commonly used trajectory generation method. By selecting control points on the path and interpolating using a cubic polynomial function, a continuous and smooth trajectory is generated. The specific steps are as follows: Select an appropriate number of control points on the smoothed global route. The number and distribution of control points need to be reasonably set according to the length, curvature and other characteristics of the path. For every two adjacent control points, use a cubic polynomial function to interpolate and generate a trajectory segment connecting the two control points. The coefficients of the cubic polynomial function can be obtained by solving the boundary condition equation, which includes the position, velocity, acceleration and other information of the control points. All trajectory segments are spliced together to form a complete trajectory. In order to ensure the continuity and smoothness of the trajectory at the control points, it is necessary to constrain the boundary conditions of adjacent trajectory segments to ensure that the position, velocity, acceleration and other parameters of the trajectory at the control points are continuous. According to the generated trajectory, combined with the speed, acceleration and steering angle restrictions of the roller, the trajectory is adjusted to ensure that the trajectory meets the kinematic constraints of the roller. The adjusted trajectory is used as the final path planning instruction to control the movement of the roller. Through the cubic spline interpolation algorithm, a continuous and smooth trajectory can be generated, the sudden change and oscillation of the trajectory can be reduced, and the stability and comfort of the roller movement can be improved. At the same time, by combining the kinematic constraints of the roller, it can be ensured that the generated trajectory is within the range of the roller's motion capabilities, improving the feasibility and safety of the path planning instructions.
[0014] Preferably, the global travel route is smoothed using the Dijkstra algorithm, and a smooth global travel route is obtained through the following sub-steps: the global travel route is divided into several segments, each segment contains at least two nodes, and there is at least one overlapping node between two adjacent segments; for each segment, the starting node of the segment is used as the source node, and the end node of the segment is used as the target node, and the Dijkstra algorithm is used to search for the shortest path from the source node to the target node to obtain a smooth path for the segment; the smooth paths of each segment are connected in sequence to obtain a smooth global travel route for the roller. Specifically, the speed, acceleration, steering angle and other restriction conditions of the roller transmitted by the roller control system are obtained, and the path planning instructions are generated by using the cubic spline interpolation algorithm: based on the smooth global travel route, the key points on the route are extracted as the control points of the spline interpolation; with the speed, acceleration and steering angle and other restriction conditions of the roller as boundary conditions, the control points are interpolated by using the cubic spline interpolation function to generate a continuous change curve of the speed, acceleration and steering angle of the roller; based on the generated continuous change curve of speed, acceleration and steering angle, combined with the dynamic model of the roller, a path planning instruction containing time, position, speed, acceleration and steering angle information is generated; the path planning instruction is transmitted to the roller control system.
[0015] Furthermore, the predicted coordinates of the obstacle are obtained, including: obtaining the characteristic information of the obstacle based on the collected environmental data. The position and velocity information of the obstacle can be obtained through the environmental data collected by sensors, such as lidar, millimeter wave radar, etc. The position information includes the coordinates of the obstacle in the environment, and the velocity information includes the direction and size of the obstacle's movement. Establish a kinematic model and an observation model for the obstacle. The kinematic model describes the motion characteristics of the obstacle. This scheme adopts a uniform motion model, assuming that the obstacle maintains uniform linear motion in a short period of time. The state variable contains the position and velocity of the obstacle, represented by a vector. The observation model describes the observation characteristics of the sensor on the obstacle. In this scheme, the observation variable is the position of the obstacle. According to the uniform motion model of the obstacle, the state equation of the Kalman filter is established. The state equation describes the change law of the state variable, which is used to predict the motion state of the obstacle. Under the uniform motion model, the state transfer matrix is a unit matrix, indicating that the state variable remains unchanged during the prediction process. The form of the state equation is:
[0016] X(k|k-1)=A×X(k-1|k-1)+B×U(k)+W(k), where X(k|k-1) is the predicted state variable, A is the state transfer matrix, X(k-1|k-1) is the state variable at the previous moment, B is the control matrix, U(k) is the control amount, and W(k) is the process noise. Under the uniform motion model, A is the unit matrix, B and U(k) are zero matrices, and W(k) is Gaussian white noise. According to the state equation of the Kalman filter, the position and speed of the obstacle at the next moment are predicted to obtain the predicted state variable X(k|k-1). According to the observation model of the obstacle, the observation equation of the Kalman filter is established. The observation equation describes the relationship between the observed variable and the state variable, which is used to correct the predicted value. The form of the observation equation is: Z(k) = H × X(k|k-1) + V(k), where Z(k) is the observed variable, H is the observation matrix, X(k|k-1) is the predicted state variable, and V(k) is the observation noise. In this scheme, the observed variable is the position of the obstacle, and the observation matrix H is used to extract the position information from the state variable. Using the observation equation, the residual between the predicted state variable and the actual observed variable is calculated. The residual reflects the difference between the predicted value and the actual value and is used to correct the predicted value. The calculation formula of the residual is: r(k) = Z(k)-H × X(k|k-1). Based on the residual and Kalman gain, the predicted state variable is corrected to obtain the corrected obstacle position and speed. The Kalman gain is used to weigh the credibility of the predicted value and the observed value, and adjust the predicted value according to the residual to make it closer to the actual value. The calculation formula of the Kalman gain is: K(k) = P(k|k-1) × H' × (H × P(k|k-1) × H' + R) -1 , where K(k) is the Kalman gain, P(k|k-1) is the covariance matrix of the predicted state variable, H' is the transpose of the observation matrix, and R is the covariance matrix of the observation noise. The calculation formula of the corrected state variable X(k|k) is: X(k|k)=X(k|k-1)+K(k)×r(k). The corrected state variable is used as the predicted value of the obstacle at the next moment to update the motion state of the obstacle.
[0017] Furthermore, based on the prediction value of the obstacle at the next moment obtained by Kalman filtering, a particle filter algorithm is further introduced to represent the state distribution of the obstacle through a particle set to improve the accuracy and robustness of the prediction, including: according to the prediction value of the obstacle at the next moment obtained by Kalman filtering, a group of particles is initialized through the observation model of the obstacle. Each particle contains the position, speed and weight information of the obstacle, which is used to represent the possible distribution of the obstacle state. The position and speed of the particle are initialized according to the prediction value of the Kalman filter, and the weight is initialized to a uniform distribution. At the same time, the prior map information is set to constrain the position of the particle to ensure that the particle is located in a feasible area. According to the uniform motion model of the obstacle, the position and speed of each particle are predicted to obtain a predicted particle set. The prediction process of the particle is similar to that of the Kalman filter. The state variables of the particle are updated through the state transfer matrix to obtain the prediction value at the next moment. Process noise is added in the prediction process to simulate the uncertainty of the obstacle motion. According to the observation model of the obstacle, the likelihood probability between each predicted particle and the actual observed variable is calculated. The likelihood probability reflects the degree of consistency between the predicted particle and the actual observed value, and is used to adjust the weight of the particle. The calculation of likelihood probability is based on the probability distribution of observation noise. Commonly used observation noise models include Gaussian noise, Cauchy noise, etc. According to the likelihood probability, adjust the weight of each particle. The purpose of weight adjustment is to increase the weight of particles that are highly consistent with the actual observation value and reduce the weight of particles that are less consistent with the actual observation value. The adjusted particle weight reflects the importance of the particle. Particles with large weights are more likely to represent obstacle states. The formula for weight adjustment is:
[0018] w(i,k)=w(i,k-1)×p(Z(k)|X(i,k|k-1)), where w(i,k) is the weight of the i-th particle at the k-th moment, w(i,k-1) is the weight of the i-th particle at the k-1th moment, and p(Z(k)|X(i,k|k-1)) is the likelihood probability between the i-th predicted particle and the actual observation value.
[0019] According to the adjusted particle weights, the system resampling algorithm is used to resample the particles to obtain the resampled particle set. The purpose of resampling is to eliminate particles with low weights, increase the number of particles with high weights, and improve the quality of the particle set. The system resampling algorithm randomly selects particles from the original particle set according to the weight distribution of the particles to generate a new particle set. Particles with high weights have a greater probability of being selected, and particles with low weights have a lower probability of being selected. The resampled particle set can better reflect the true distribution of the obstacle state. The weighted mean of the resampled particle set is calculated, and the weighted mean is used as the predicted coordinates of the obstacle. The weighted mean comprehensively considers the position and weight information of all particles and can represent the center of gravity of the obstacle state distribution.
[0020] The calculation formula of the weighted mean is: X(k|k)=∑w(i,k)×X(i,k|k-1), where X(k|k) is the predicted coordinates of the obstacle at the kth moment, w(i,k) is the weight of the ith particle at the kth moment, and X(i,k|k-1) is the predicted position of the ith particle at the kth moment. The particle filter algorithm can more accurately predict the position and speed of the obstacle, overcoming the limitations of the Kalman filter in nonlinear and non-Gaussian situations. The particle filter approximates the posterior probability distribution of the obstacle state through a collection of particles, can handle complex motion models and observation models, and has stronger adaptability to environmental changes. At the same time, the particle filter dynamically adjusts the weight of the particles through the resampling mechanism, eliminates low-quality particles, and improves the accuracy and stability of the prediction. Combining the particle filter with the Kalman filter can achieve complementary advantages in obstacle prediction, improve the reliability of the predicted coordinates, and provide a more accurate reference for the obstacle avoidance decision of the roller.
[0021] Preferably, setting the prior map information includes: obtaining map data of the roller operation area, the map data including information such as road boundaries, building outlines, and terrain elevations; converting the map data into a raster map, the raster map consisting of a plurality of grid cells, each of which contains map attributes within the area; in the raster map, marking the grid cells of the road area as free space, and marking the grid cells of the non-road area as obstacle areas; the free space represents the area where the roller can pass, and the obstacle area represents the area where the roller cannot pass; setting the obstacle prior probability distribution in the raster map according to the position distribution of the obstacle area; the obstacle prior probability distribution represents the prior probability of the obstacle appearing at each position in the map, and the higher the prior probability, the greater the possibility of the obstacle appearing at the position; using the roller's own positioning device to obtain the real-time position coordinates (x am ,y am ), the position coordinates of the roller (x am ,y am) is used as the initial value of the particle position in the particle filter algorithm, and the particles are restricted to a local area centered on the roller position; in the prediction stage of the particle filter algorithm, the free space information in the grid map is used to constrain the movement of the particles; if the predicted position of the particle is in the obstacle area, the position of the particle is adjusted to be in the free space; in the update stage of the particle filter algorithm, the weight of the particle is corrected using the obstacle prior probability distribution; if the predicted position of the particle is in an area with a high prior probability of an obstacle, the weight of the particle is increased, indicating that the particle is more consistent with the actual obstacle position. By setting the prior map information, the known information of the roller's environment is introduced into the particle filter algorithm, the position and weight of the particles are constrained and corrected, the search range of the particle filter algorithm is reduced, and the accuracy and convergence speed of obstacle position prediction are improved.
[0022] Further, a kinematic model of an obstacle is established, including: according to the motion state of the obstacle, kinematic models of a stationary obstacle and a uniformly moving obstacle are established respectively as two sub-models of the kinematic model of the obstacle. Specifically, the following steps are included: according to the characteristic information of the obstacle, the speed of the obstacle is extracted. The position and speed information of the obstacle can be obtained through environmental data collected by sensors, such as laser radar, millimeter wave radar, etc. The speed information includes the movement direction and size of the obstacle, which is used to judge the motion state of the obstacle. The motion state of the obstacle is divided into a stationary state and a uniform speed state according to the speed. A speed threshold is set. When the speed of the obstacle is less than the speed threshold, the obstacle is judged to be in a stationary state; when the speed of the obstacle is greater than or equal to the speed threshold, the obstacle is judged to be in a uniform motion state. The selection of the speed threshold needs to be set according to the actual application scenario and the type of obstacle to accurately distinguish between the stationary and moving states. When the obstacle is in a stationary state, a kinematic model of the stationary obstacle is established. The position coordinates (x, y) in two-dimensional space are used to describe the position state of the obstacle, and the position coordinates of the obstacle are used as state variables. A state transfer equation is established to describe the change law of the state variables of the stationary obstacle. The state transfer equation of a stationary obstacle is: X(k)=X(k-1)+W(k), where X(k) is the position coordinate of the obstacle at the kth moment, X(k-1) is the position coordinate of the obstacle at the k-1th moment, and W(k) is the process noise, which represents a small disturbance in the obstacle position.
[0023] When the obstacle is in a uniform motion state, a kinematic model of the uniform motion obstacle is established. The position coordinates (x, y) and velocity (v x ,v y) describes the motion state of the obstacle, and takes the position coordinates and velocity of the obstacle as state variables. Establish a state transfer equation to describe the change law of the state variables of the uniform motion obstacle. The state transfer equation of the uniform motion obstacle is: X(k) = A×X(k-1)+W(k), where X(k) is the state variable of the obstacle at the kth moment, including the position coordinates and velocity, A is the state transfer matrix, X(k-1) is the state variable of the obstacle at the k-1th moment, and W(k) is the process noise. The state transfer matrix A is in the form of: Among them, Δt represents the sampling time interval of the sensor. Δt in the state transfer matrix reflects the relationship between the position coordinate and the speed, that is, the position change of the obstacle within the Δt time is proportional to the speed. The kinematic model of the stationary obstacle and the kinematic model of the uniformly moving obstacle are used as two submodels of the obstacle kinematic model. According to the motion state of the obstacle, the corresponding submodel is selected to generate the obstacle kinematic model. The obstacle kinematic model contains state variables, state transfer equations and state transfer matrices, which are used to describe the motion characteristics of the obstacle and provide a basis for subsequent state prediction and filtering.
[0024] By establishing kinematic models of stationary obstacles and uniformly moving obstacles, the motion characteristics of obstacles under different motion states can be accurately described. The kinematic model of stationary obstacles assumes that the position of the obstacle remains unchanged and only considers small position disturbances; the kinematic model of uniformly moving obstacles considers the changes in position and velocity, and establishes the relationship between position and velocity through the state transfer matrix. According to the actual motion state of the obstacle, the corresponding sub-model is selected to improve the accuracy and applicability of kinematic modeling. The establishment of the obstacle kinematic model lays the foundation for obstacle state prediction and filtering. Through the kinematic model, the position and velocity of the obstacle at the future moment can be predicted, providing a reference for obstacle avoidance decision-making. At the same time, the kinematic model can be combined with sensor observation information, and the state of the obstacle can be estimated and updated through filtering algorithms (such as Kalman filtering, particle filtering, etc.) to improve the accuracy and robustness of the prediction. The selection of the obstacle kinematic model needs to be adjusted according to the actual application scenario and the characteristics of the obstacle. For more complex obstacle motion situations, such as accelerated motion, curved motion, etc., it can be expanded on the basis of the uniform motion model, introducing state variables such as acceleration and steering angle, and establishing a higher-dimensional kinematic model. At the same time, the parameters of the obstacle kinematic model, such as the covariance matrix of the process noise, need to be adjusted through experimental data and empirical knowledge to adapt to the uncertainty of the actual environment.
[0025] Preferably, the kinematic model of the obstacle is generated, including: Definition of state variables: For stationary obstacles and uniformly moving obstacles, the definition of state variables is slightly different. The state variable of a stationary obstacle is its position coordinate, expressed as: X s=[x,y] T , where x and y represent the horizontal and vertical coordinates of the obstacle in two-dimensional space respectively. The state variables of the uniformly moving obstacle include position coordinates and velocity components, expressed as: X d =[x,y,v x ,v y ] T , where x and y represent the location coordinates of the obstacle, and v x and v y Represents the velocity components of the obstacle in the x and y directions. Kinematic model of a stationary obstacle: For a stationary obstacle, its position remains unchanged, and the state transfer equation can be expressed as: s (k) = X s (k-1), where X s (k) represents the state variable of the stationary obstacle at the kth moment. The state transfer matrix of the stationary obstacle is the unit matrix: A s =[1,0; 0,1], Kinematic model of uniform motion obstacle: For uniform motion obstacle, its position and velocity satisfy the kinematic equation of uniform motion, and the state transfer equation can be expressed as: d (k) = A d ×X d (k-1)+W d (k), where X d (k) represents the state variable of the uniformly moving obstacle at the kth moment, A d is the state transfer matrix, W d (k) is the process noise. The state transfer matrix of the uniform moving obstacle is:
[0026]
[0027] Where Δt is the time interval. According to the motion state of the obstacle, select the corresponding kinematic model, that is, the stationary obstacle model or the uniform motion obstacle model. Combine the state variables, state transfer equations and state transfer matrices to form a complete obstacle kinematic model. The obstacle kinematic model can be expressed in the form of a state space model, that is: X(k) = A × X(k-1) + W(k), where X(k) is the state variable of the obstacle, A is the state transfer matrix, and W(k) is the process noise.
[0028] Furthermore, an observation model of the obstacle is established, including: according to the characteristic information and kinematic model of the obstacle, an observation equation and a probability distribution of the observation noise are established to generate an observation model of the obstacle. Specifically, the following steps are included: according to the characteristic information of the obstacle, the position coordinates (x, y) of the obstacle in the two-dimensional space are obtained as the observation variables of the obstacle observation model. The position coordinates of the obstacle in the sensor coordinate system can be directly obtained through environmental data collected by sensors, such as lidar, millimeter-wave radar, etc. The observation variable represents the direct observation result of the sensor on the position of the obstacle. According to the state variables in the kinematic model of the obstacle, the observation equation of the obstacle observation model is established. The observation equation describes the relationship between the observation variable and the state variable, and maps the real state of the obstacle to the observation space. The form of the observation equation is: [x obs ;y obs ]=[1,0,0,0;0,1,0,0]×[x;y;v x ;v y ]+[n x ;n y ], where [x obs ;y obs ] is the observed position coordinate of the obstacle, [x; y; v x ;v y ] is the state variable in the obstacle kinematic model, namely the position coordinates and velocity of the obstacle, [n x ;n y ] is the observation noise. The observation equation extracts the position coordinates from the state variable through the observation matrix [1, 0, 0, 0; 0, 1, 0, 0], indicating that the observation variable is only related to the position coordinates of the obstacle and has nothing to do with the speed. x ;n y ] represents the uncertainty and error in the sensor measurement process. The probability distribution of the observation noise is determined based on the sensor type and installation location. The observation noise is usually assumed to obey a Gaussian distribution with a mean of zero and a covariance matrix of R, which represents the statistical characteristics of the observation error. The covariance matrix R is in the form of:
[0029] Among them, σ x and σ y are the standard deviations of the sensor's measurement errors in the x and y directions, respectively, reflecting the sensor's measurement accuracy. The diagonal elements of the covariance matrix R represent the independence of the observation noise in different directions, and the non-diagonal elements are zero, indicating that the observation noise is uncorrelated in different directions.
[0030] The probability distribution of the observation noise needs to be determined based on the performance indicators of the sensor and the actual working environment. By statistically analyzing the measurement data of the sensor, the mean and covariance matrix of the observation noise can be estimated. At the same time, the parameters of the observation noise can also be obtained through experimental calibration or empirical setting. The observation equation and the probability distribution of the observation noise are used as parameters of the obstacle observation model to generate the observation model of the obstacle. Preferably, the state space model is used to represent the obstacle observation model, including: state equation: The state equation describes the dynamic evolution process of the obstacle state, and represents the relationship between the state variables at the current moment and the state variables at the previous moment. For the obstacle motion model, the state equation can be expressed as: X(k) = A × X(k-1) + B × U(k) + W(k), where X(k) is the state variable at the kth moment, including the position and velocity information of the obstacle; A is the state transfer matrix, which describes the natural evolution law of the state variable; B is the control matrix, which describes the influence of the external control input on the state variable; U(k) is the control input at the kth moment, which is usually assumed to be zero for the obstacle motion model; W(k) is the process noise, which represents the uncertainty and interference in the state evolution process. Observation equation: The observation equation describes the relationship between the observed variable and the state variable, mapping the state space to the observation space.
[0031] For the observation model of the obstacle, the observation equation can be expressed as: Z(k) = H × X(k) + V(k), where Z(k) is the observed variable at the kth moment, that is, the sensor's measurement of the obstacle's position; H is the observation matrix, which maps the state variable to the observation space; V(k) is the observation noise, which represents the uncertainty and error in the sensor's measurement process. Noise assumption: In the state space model, it is usually assumed that the process noise W(k) and the observation noise V(k) are independent of each other and obey a Gaussian distribution. The probability distribution of the process noise W(k) can be expressed as: W(k) ~ N(0, Q), where Q is the covariance matrix of the process noise, which represents the uncertainty in the state evolution process. The probability distribution of the observation noise V(k) can be expressed as: V(k) ~ N(0, R), where R is the covariance matrix of the observation noise, which represents the uncertainty in the sensor's measurement process. Initial state distribution: In the state space model, it is necessary to specify the state variable distribution at the initial moment, that is, the probability distribution of X(0). It is usually assumed that the initial state follows a Gaussian distribution, that is: X(0)~N(μ 0 ,∑0), where μ 0is the mean of the initial state, and ∑0 is the covariance matrix of the initial state. State estimation and filtering: Based on the state space model, various state estimation and filtering algorithms can be used to estimate and track the state of obstacles. Common algorithms include Kalman filtering, extended Kalman filtering, unscented Kalman filtering, particle filtering, etc. These algorithms estimate and update the state of obstacles by recursion, combining observation information and state prediction. Parameter learning and tuning: The parameters in the state space model, such as the state transfer matrix A, the observation matrix H, the noise covariance matrix Q and R, need to be learned and tuned according to actual data. The maximum likelihood estimation, expectation maximization algorithm and other methods can be used to estimate the model parameters by optimizing a certain objective function (such as likelihood function or posterior probability). Through parameter learning and tuning, the state space model can be better adapted to the actual environment and the performance of state estimation and filtering can be improved. Model evaluation and verification: The learned state space model is evaluated and verified to ensure its effectiveness in practical applications. The data set can be divided into training set and test set by cross validation, leave-one-out method and other methods to evaluate the performance of the model on unknown data. Evaluation indicators can include state estimation error, filter convergence speed, computational efficiency, etc.
[0032] The observation model of the obstacle contains the observation variables, observation equations and probability distribution of observation noise, which are used to describe the observation characteristics of the sensor to the obstacle position and provide a basis for subsequent state estimation and filtering. By establishing the observation model of the obstacle, the observation information of the sensor can be linked to the motion state of the obstacle. The observation model describes the observation method of the sensor to the obstacle position and the characteristics of the observation error, providing an observation basis for state estimation. In the process of state estimation and filtering, the observation model is used to calculate the residual between the observation value and the state prediction value, and adjust the state estimation value according to the residual to make it closer to the true value. The establishment of the observation model needs to consider the characteristics of the sensor and the actual working environment. Different types of sensors, such as lidar, millimeter wave radar, and visual sensors, may have different characteristics of observation equations and observation noise. The observation equation needs to be constructed according to the measurement principle and coordinate system definition of the sensor, and the probability distribution of the observation noise needs to be determined according to the performance indicators and environmental factors of the sensor. At the same time, the parameters of the observation model, such as the covariance matrix of the observation noise, can be optimized through experimental calibration or online estimation to adapt to changes in the actual environment. The establishment of the obstacle observation model provides a basis for multi-sensor information fusion.
[0033] Further, a path adjustment instruction is generated, including: calculating the distance between the obstacle and the roller; obtaining the predicted coordinates of the obstacle (x obs ,y obs ) and the current position of the roller (x am ,y am)。Calculate the distance d between the obstacle and the roller using the Euclidean distance formula: Compare the calculated distance d with the preset distance threshold d_thresh. Trigger the local path generation step: If the distance d is less than the distance threshold d_thresh, i.e., d < d_thresh, then trigger the local path generation step. The setting of the distance threshold d_thresh needs to consider the safety distance and reaction time of the roller and can be determined through experiments or empirical values. Establish an artificial potential field model: Taking the current position (x am , y am ) of the roller as the origin, establish a two-dimensional artificial potential field model.
[0034] Map the predicted coordinates (x obs , y obs ) of the obstacle to the corresponding position in the artificial potential field. The obstacle generates a repulsive force in the artificial potential field, and the magnitude of the repulsive force is inversely proportional to the distance between the obstacle and the roller. The repulsive potential energy function can be defined as: If d ≤ d thresh , U rep (d) = 0, if d > d thresh , where k rep is the gain coefficient of the repulsive potential energy, and d is the distance between the obstacle and the roller. Map the target point (x goal , y goal ) on the target path to the corresponding position in the artificial potential field. The target point generates an attractive force in the artificial potential field, and the magnitude of the attractive force is proportional to the distance between the target point and the roller. The attractive potential energy function can be defined as: U att (d) = k att × d 2 , where k att is the gain coefficient of the attractive potential energy, and d is the distance between the target point and the roller. Path planning and selection: Use the A* algorithm to search for multiple candidate paths from the current position (x am , y am ) of the roller to the target point (x goal , y goal ) in the artificial potential field. The A* algorithm takes the current position of the roller as the starting point and the target point as the ending point, considering the repulsive and attractive forces in the artificial potential field, and searches for multiple feasible paths. For each candidate path, calculate the potential energy distribution of each point on the path. The potential energy distribution is obtained by superimposing the repulsive potential energy and the attractive potential energy of each point on the path: U total (x, y) = U rep (d obs ) + U att (d goal ), where d obs is the distance between the obstacle and the path point (x, y), and d goalis the distance between the target point and the path point (x, y). Calculate the sum of the potential energy of each selected path, that is, the potential energy of each point on the path: E path =∑U total (x, y), select the candidate path with the smallest total potential energy as the local path to avoid obstacles. Smooth the selected local path to meet the kinematic constraints of the roller.
[0035] Specifically, the kinematic constraints of the roller include the following parameters: the minimum turning radius Rmin of the roller: when the roller is turning, the turning radius cannot be less than the minimum turning radius Rmin, Rmin depends on the wheelbase and maximum steering angle of the roller, and the value of Rmin is 5m; the maximum steering angle θmax of the roller: the steering mechanism of the roller can adjust the steering angle of the wheel, and the absolute value of the steering angle cannot exceed the maximum steering angle θmax, and the value of θmax is 30°; the maximum speed vmax of the roller: the driving speed of the roller cannot exceed the maximum speed vmax, vmax depends on the engine power and transmission system parameters of the roller, and the value of vmax is 5m / s; the maximum acceleration amax of the roller: during the acceleration process of the roller, the absolute value of the acceleration cannot exceed the maximum acceleration amax, amax depends on the engine torque and curb weight of the roller, and the value of amax is 1m / s 2 ; Maximum deceleration dmax of the roller: During the deceleration process of the roller, the absolute value of the deceleration cannot exceed the maximum deceleration dmax. dmax depends on the brake performance of the roller and the ground adhesion conditions. The value of dmax is 2m / s 2 .
[0036] Preferably, the local path is smoothed according to the kinematic constraints of the roller, including: judging the line between two adjacent points on the local path, if the absolute value of the slope of the line is greater than tan(θmax), inserting a transition point between the two points so that the absolute value of the slope of the line after the transition point is inserted is not greater than tan(θmax); smoothing the inflection point on the local path, transitioning the sharp angle at the inflection point to an arc, and the radius of the arc is not less than the minimum turning radius Rmin of the roller; judging the distance between two adjacent points on the local path, if the distance is greater than the braking distance of the roller under the maximum acceleration amax, inserting a deceleration point between the two points, and the position of the deceleration point is determined according to the maximum deceleration dmax of the roller; speed planning is performed on the smoothed local path, and the target speed of each point is calculated according to the maximum speed vmax of the roller and the curvature of each point on the path, to ensure that the speed of the roller does not exceed vmax during driving and the turning radius is not less than Rmin.
[0037] Furthermore, taking the current position of the roller as the origin, a two-dimensional artificial potential field model is established, including: obstacle prediction position coordinates (x obs ,y obs ) and the current position coordinates of the roller output by the positioning device (x am ,y am ), calculate the Euclidean distance d between the predicted obstacle position coordinates and the current position coordinates of the roller, and the calculation formula is:
[0038] The Euclidean distance d is compared with the preset distance threshold dth. When d≤dth, the local path generation operation is triggered. The current position coordinates of the roller (x am ,y am ) as the origin, a two-dimensional artificial potential field model is established. The potential energy function U(x, y) of the artificial potential field model includes the repulsive potential energy function Urep(x, y) and the gravitational potential energy function U att (x, y), the potential energy function U(x, y) is calculated as: U(x, y) = U rep (x,y)+U att (x,y), where the repulsive potential energy function U rep The calculation formula for (x,y) is: Among them, k 1 is the gain coefficient of the repulsive potential energy, σ is the attenuation coefficient of the repulsive potential energy, d is the Euclidean distance between the predicted position of the obstacle and the current position of the roller; gravitational potential energy function U att The calculation formula for (x,y) is: Among them, k 2 is the gain coefficient of gravitational potential energy, (x g ,y g ) is the coordinate of the target point on the target path; search in the artificial potential field model from the current position coordinate of the roller (x am ,y am ) to the target point coordinate (x g ,y g ), the search method for feasible paths is A* algorithm. During the search process, the potential energy function value U(x,y) of each position point on each feasible path is calculated to obtain the potential energy distribution curve of each feasible path; the total potential energy U of each feasible path is calculated sum , the total potential energy U sum The calculation formula is: sum =∫U(x,y)ds, where ds is the path element between two adjacent points on the path; select the total potential energy U sumThe minimum feasible path is used as the local path for avoiding obstacles. According to the kinematic constraints of the roller, the local path is smoothed to generate a smoothed local path. According to the smoothed local path, a path adjustment instruction including a target steering angle and a target speed is generated and output to the actuator to control the roller to travel along the smoothed local path to avoid obstacles.
[0039] Beneficial effects: Compared with the prior art, the advantages of this application are:
[0040] By installing sensors on the roller to collect environmental data and using the path planning algorithm A* to set the global travel route, the global optimization of the roller's travel path is achieved, thereby improving the efficiency of the roller's operation and the rationality of path planning.
[0041] The Kalman filter algorithm and the particle filter algorithm are combined to predict and update the position of obstacles, which improves the accuracy and robustness of obstacle position estimation and provides reliable obstacle information for subsequent obstacle avoidance decisions.
[0042] By establishing the kinematic model and observation model of the obstacle and combining it with sensor information, the motion state of the obstacle is judged and predicted, which improves the accuracy of obstacle motion state estimation and provides important prior information for real-time obstacle avoidance.
[0043] By using the artificial potential field model, obstacles and target points are mapped into the potential field. By constructing the potential energy function, the requirements of obstacle avoidance and target tracking are balanced, and a safe, smooth and efficient local obstacle avoidance path is generated, which improves the reliability and real-time performance of obstacle avoidance.
[0044] During the path planning and obstacle avoidance process, the kinematic constraints of the roller are fully considered, the generated path is smoothed, and combined with the speed, acceleration and steering angle restrictions of the roller, path adjustment instructions that conform to the actual motion characteristics of the roller are generated, thereby improving the feasibility and safety of obstacle avoidance decisions.
[0045] By dividing the map into grids and using the A* algorithm for path search, the computational complexity of path planning is reduced, the efficiency and real-time performance of path planning are improved, and the dynamic environment requirements of roller operations are met.
[0046] The cubic spline interpolation algorithm is used to smooth the path, generating a continuous and smooth global travel route, reducing the vibration and impact of the roller during travel and improving the stability and comfort of driving.
[0047] By judging the distance between the predicted coordinates of the obstacle and the roller, the local obstacle avoidance decision is triggered, realizing the organic combination of global path planning and local obstacle avoidance, improving the adaptability and flexibility of the system, and being able to cope with dynamically changing working environments.
[0048] In the artificial potential field model, through the design of repulsive potential energy function and gravitational potential energy function, the virtual repulsive force between the obstacle and the roller and the virtual gravitational force between the target point and the roller are realized, the optimal path of balanced obstacle avoidance and target tracking is generated, and the intelligence level of obstacle avoidance is improved. BRIEF DESCRIPTION OF THE DRAWINGS
[0049] Figure 1 is an application scenario diagram of an obstacle recognition method for a road roller according to some embodiments of this specification;
[0050] Figure 2 is an exemplary flow chart of an obstacle identification method for a road roller according to some embodiments of this specification;
[0051] Figure 3 is an exemplary flow chart for obtaining a predicted value of an obstacle at the next moment according to some embodiments of this specification;
[0052] Figure 4 is an exemplary flow chart of obtaining predicted coordinates of obstacles according to some embodiments of this specification;
[0053] Figure 5 is an exemplary flowchart of obtaining a local path according to some embodiments of the present specification. DETAILED DESCRIPTION
[0054] The method and system provided in the embodiments of this specification are described in detail below with reference to the accompanying drawings.
[0055] Figure 1 is an application scenario diagram of an obstacle recognition method for a road roller according to some embodiments of this specification, Figure 2 is an exemplary flow chart of an obstacle identification method for a road roller according to some embodiments of this specification; Figure 1 and Figure 2As shown, 5G cameras are installed at the front and rear ends of the roller to transmit the operation process to the handheld terminal, and unmanned operation can be performed. Due to the complex working environment in the limited space, in order to avoid collision between the construction machinery and the structure and embedded parts, early warning radars are installed at the front and rear ends of the roller. When the distance between the machinery and the component is less than the safe distance, the radar automatically prompts the handheld terminal, and the operator performs avoidance operations. The method includes step 1: setting environmental perception sensors such as laser radar, millimeter wave radar, and ultrasonic sensor on the roller to collect environmental data around the roller in real time. The environmental data includes the type and location information of the obstacle, and the sensor sends the collected raw data to the central processor of the roller. Step 2: The central processor of the roller receives the real-time position information of the roller provided by the global positioning system (GPS) and the attitude information of the roller provided by the inertial navigation system (INS), combines the pre-stored map data and obstacle prior information, and uses the A* path planning algorithm to generate the global travel route of the roller. The generated path planning instruction is sent to the control system of the roller.
[0056] Generate path planning instructions, including: Step 2.1: Divide the working area into a grid map, input: the boundary coordinates of the working area of the roller, expressed by the longitude and latitude coordinates obtained by GPS, [(lng1, lat1), (lng2, lat2), ..., (lngn, latn)]. According to the boundary coordinates of the working area, calculate the length and width of the working area, assuming the length is L and the width is W. Set the resolution r of the grid map to represent the actual length of each grid in meters. Calculate the number of rows m and columns n of the grid map, m = ceil (W / r), n = ceil (L / r), where ceil means rounding up. Create a two-dimensional array map with m rows and n columns to represent the grid map, and initialize each element to 0, indicating that the node can be passed. According to the number of rows and columns and the resolution of the grid map, calculate the coordinates of the center point of each grid, and store them in a two-dimensional array nodes, each element of which is an (x, y) coordinate. According to the positional relationship of adjacent nodes, the edges between nodes are calculated and stored in a list edges, where each element is (node1, node2), indicating that there is an edge between two nodes. Output: grid map map, represented as a two-dimensional array with m rows and n columns. Node coordinates nodes, represented as a two-dimensional array with a length of m*n, where each element is an (x, y) coordinate. Edges between nodes edges, represented as a list, where each element is (node1, node2).
[0057] Step 2.2: Mark obstacle information in the grid map. Input: Environmental data collected by environmental perception sensors, including obstacle location information obtained by sensors such as lidar, millimeter wave radar, and ultrasonic sensors, represented as a list, each element is an (x, y) coordinate. Obstacle prior information, including the location information of known fixed obstacles (such as buildings, large equipment, etc.), represented as a list, each element is an (x, y) coordinate. Fusion of obstacle location information collected by environmental sensors and prior information to obtain a complete obstacle location list obsstacle_list. Traverse each obstacle location (x, y) in obsstacle_list: Calculate the grid index (i, j) where the obstacle location is located, i = floor (y / r), j = floor (x / r), where floor means rounding down. Mark the corresponding element map[i][j] in the grid map map as 1, indicating that the node is an obstacle node and cannot be passed. Perform an expansion operation on the grid map, and mark the nodes around the obstacle node as obstacle nodes to ensure a certain safety distance between the roller and the obstacle. Output: A grid map with obstacle information marked on it, where 0 indicates a passable node and 1 indicates an obstacle node.
[0058] Step 2.3: Determine the starting node and target node in the grid map. Input: GPS location information of the roller, expressed as longitude and latitude coordinates (lng, lat). INS attitude information of the roller, expressed as yaw angle, in radians. The roller's trajectory record, represented as a list, each element is an (x, y) coordinate. Convert the roller's GPS position (lng, lat) to the position (x, y) in the plane coordinate system. Calculate the grid index (i, j) where the roller is located, i = floor (y / r), j = floor (x / r). Mark the corresponding node map[i][j] in the grid map as the starting node, and store its coordinates in the variable start. According to the roller's trajectory record, predict the next target position (target_x, target_y). Calculate the grid index (ti, tj) where the target position is located, i = floor (target_y / r), tj = floor (target_x / r). Mark the corresponding node map[ti][tj] in the grid map as the target node, and store its coordinates in the variable goal. Output: starting node coordinates start, represented as (i, j) index. Target node coordinates goal, represented as (ti, tj) index. Step 2.4: Create open lists and closed lists, input: None. Create an empty open list open_list to store the nodes to be explored. The open list can be implemented using a priority queue (such as a minimum heap) to quickly obtain the node with the smallest evaluation function value. Create an empty closed list closed_list to store the explored nodes. The closed list can be implemented using a hash table to quickly determine whether the node has been visited. Output: An empty open list open_list, represented as an empty priority queue. An empty closed list closed_list, represented as an empty hash table.
[0059] Step 2.5: Add the starting node to the open list, input: starting node coordinates start, expressed as (i, j) index. Create a node object start_node, containing the following attributes: Coordinates coord: starting node coordinates (i, j). Actual distance g_score: the actual distance from the starting node to itself, initialized to 0. Estimated distance h_score: Manhattan distance from the starting node to the target node, calculated as abs(i-ti)+abs(j-tj), where (ti, tj) is the target node coordinate. Evaluation function value f_score: the sum of the actual distance and the estimated distance, that is, f_score=g_score+h_score. Parent node parent: The starting node has no parent node, initialized to None. Add start_node to the open list open_list. Output: The open list open_list containing the starting node, represented as a priority queue containing start_node. Steps 2.6-2.11: Execute the main loop of the A* algorithm, input: open list open_list, containing the starting node. Close the list closed_list, which is initially empty. The grid map map is marked with obstacle information. The target node coordinates goal, represented by the (ti, tj) index. While open_list is not empty: Take the node current_node with the smallest evaluation function value from the open_list. Remove current_node from the open_list and add it to the closed_list. If the coordinates of current_node are equal to goal: Find the target node, jump out of the loop, and enter step 2.13. Traverse the neighboring nodes of current_node (up, down, left, and right): Calculate the coordinates of the neighboring node neighbor_coord. If neighbor_coord exceeds the map boundary or is an obstacle node: Skip the neighboring node and continue to traverse the next one. Create a neighboring node object neighbor_node, which contains the following attributes: Coordinate coord: neighboring node coordinate neighbor_coord. Actual distance g_score: current_node's g_score plus the distance from current_node to neighbor_node (default is 1). Estimated distance h_score: Manhattan distance from the neighboring node to the target node. Evaluation function value f_score: the sum of actual distance and estimated distance. Parent node parent: set to current_node. If neighbor_node is in closed_list: skip the neighbor node and continue traversing the next one.If neighbor_node is not in open_list: add neighbor_node to open_list. Elseif neighbor_node is in open_list and the newly calculated g_score is less than the original g_score: update neighbor_node's g_score, f_score and parent. If open_list is empty: the target node is not found, go to step 2.12. Output: updated open list open_list and closed list closed_list. If the target node is found, go to step 2.13 and output the optimal path; if open_list is empty, go to step 2.12 and output the currently searched path.
[0060] Step 2.12: Generate a global route (when the open list is empty), input: open list open_list, empty. Closed list closed_list, containing searched nodes. Grid map map, with obstacle information marked. The currently searched path current_path, represented as a coordinate sequence of nodes in closed_list. Use current_path as the global route globsal_path of the roller. Get the end node end_node of current_path. Traverse the adjacent nodes of end_node (in four directions: up, down, left, and right): calculate the coordinates of the adjacent node neighbor_coord. If neighbor_coord exceeds the map boundary or is an obstacle node or in closed_list: skip the adjacent node and continue traversing the next one. else: create an adjacent node object neighbor_node and set its parent node to end_node. Add neighbor_node to open_list. Output: global route globsal_path, represented as a coordinate sequence of nodes in current_path. The updated open list open_list includes the adjacent passable nodes of end_node.
[0061] Step 2.13: Generate the optimal path (the case where the target node is found), input: target node goal_node, including coordinates, parent node and other information. The parent node information of the node is stored in the parent attribute of each node. Create an empty list optimal_path to store the optimal path. Initialize the current node current_node to goal_node. whilecurrent_node is not None: insert the coordinates of current_node into the head of optimal_path. Update current_node to its parent node current_node.parent. Reverse optimal_path to obtain the optimal path from the start node to the target node. Output: The optimal path optimal_path, represented as a sequence of node coordinates from the start node to the target node, that is, the global travel route of the roller.
[0062] Step 2.14: Smooth the global route. Input: The global route of the roller globsal_path, represented as a sequence of node coordinates [(x1, y1), (x2, y2), ..., (xn, yn)]. Create an empty list smoothed_path to store the smoothed route. Add the start and end points of globsal_path to smoothed_path respectively. Set the smoothing factor smooth_factor to indicate the degree of smoothing. The larger the value, the more obvious the smoothing effect. Traverse the nodes in globsal_path, starting from the second node and ending at the second to last node: get the coordinates of the current node, the previous node, and the next node. Calculate the smoothed coordinates of the current node: smoothed_x = (pre v_x + cur r_x + next_x) / 3, smoothed_y = (pre v_y + cur r_y + next_y) / 3, and add the smoothed coordinates (smoothed_x, smoothed_y) to smoothed_path. Repeat the above smoothing process smooth_factor times to get the final smoothed route. Output: The smoothed global route smoothed_path, expressed as a node coordinate sequence [(sx1, sy1), (sx2, sy2), ..., (sxn, syn)]. Step 2.15: Generate path planning instructions, input: The smoothed global route smoothed_path, expressed as a node coordinate sequence. The speed limit of the roller is max_speed, in m / s. The acceleration limit of the roller is max_acceleration, in m / s^2. The steering angle limit of the roller is max_steering_angle, in radians. According to the coordinates of the adjacent nodes in smoothed_path, calculate the speed, acceleration and steering angle of each node: calculate the distance distance and angle angle between adjacent nodes. According to distance and max_speed, calculate the speed of the node to ensure that it does not exceed the maximum speed limit. According to the speed change of the adjacent nodes and max_acceleration, calculate the acceleration of the node to ensure that it does not exceed the maximum acceleration limit. According to angle and max_steering_angle, calculate the steering angle steering_angle of the node to ensure that it does not exceed the maximum steering angle limit. Use the cubic spline interpolation algorithm to generate continuous path planning instructions based on the speed, acceleration and steering angle of the node: use the node attributes (speed, acceleration, steering angle) as control points, and perform cubic spline interpolation on each attribute separately.Based on the interpolation results, dense path planning instructions are generated. Each instruction contains timestamp, position, speed, acceleration and steering angle information. Output: path planning instructions, represented as a list, each element is an instruction, containing the following information: timestamp timestamp, in seconds. Position position, represented as (x, y) coordinates. Speed speed, in m / s. Acceleration acceleration, in m / s^2. Steering angle steering_angle, in radians. The generated path planning instructions are sent to the control system of the roller for autonomous operation.
[0063] Step 3: The roller control system controls the roller's travel motor and steering system according to the received path planning instructions, so that the roller operates along the planned global travel route. During the operation, the environmental perception sensor continuously collects environmental data and sends it to the central processor in real time.
[0064] Figure 3It is an exemplary flowchart for obtaining the predicted value of an obstacle at the next moment as shown in some embodiments of this specification. Step 4: Extract the feature information of the obstacle: According to the environmental data, obtain the position coordinates (x, y) and velocity (vx, vy) of the obstacle. Compare the velocity with a preset velocity threshold to determine the motion state of the obstacle: When the velocity is less than the threshold, it is determined to be in a stationary state; when the velocity is greater than or equal to the threshold, it is determined to be in a uniform motion state. Input: The feature information of the obstacle, including the position coordinates (x, y) and velocity (vx, vy). The preset velocity threshold v_threshold for determining the motion state of the obstacle. The sampling time interval Δt of the sensor. Determine the motion state of the obstacle according to the magnitude of the obstacle's velocity: Calculate the magnitude of the velocity: speed = sqrt(vx^2 + vy^2). Compare the magnitude of the velocity with the preset velocity threshold v_threshold: If speed < v_threshold, it is determined that the obstacle is in a stationary state; otherwise, it is determined that the obstacle is in a uniform motion state. Select the corresponding kinematic model according to the motion state of the obstacle: For a stationary obstacle: The state variables are the position coordinates (x, y) of the obstacle, represented as a column vector [x; y]. The state transition equation is: [x; y] = [1, 0; 0, 1] × [x; y], where the state transition matrix is a 2×2 identity matrix. Since the obstacle is in a stationary state, its position coordinates remain unchanged over time, so the state transition equation means that the position coordinates at the current moment are equal to the position coordinates at the previous moment. For a uniformly moving obstacle: The state variables are the position coordinates (x, y) and velocity (vx, vy) of the obstacle, represented as a column vector [x; y; vx; vy]. The state transition equation is: [x; y; vx; vy] = [1, 0, Δt, 0; 0, 1, 0, Δt; 0, 0, 1, 0; 0, 0, 0, 1] × [x; y; vx; vy], where the state transition matrix is a 4×4 matrix. The Δt in the state transition matrix represents the sampling time interval of the sensor, which is used to describe the relationship between the position change of the obstacle within Δt time and the velocity. The state transition equation means that the position coordinates at the current moment are equal to the position coordinates at the previous moment plus the product of the velocity and the time interval, that is, the displacement formula for uniform motion. Generate the kinematic model of the obstacle: If the obstacle is in a stationary state, generate the kinematic model of the stationary obstacle, including the state variables [x; y] and the state transition equation [x; y] = [1, 0; 0, 1] × [x; y]. If the obstacle is in a uniform motion state, generate the kinematic model of the uniformly moving obstacle, including the state variables [x; y; vx; vy] and the state transition equation [x; y; vx; vy] = [1, 0, Δt, 0; 0, 1, 0, Δt; 0, 0, 1, 0; 0, 0, 0, 1] × [x; y; vx; vy]. Output: The kinematic model of the obstacle, including the state variables and the state transition equation.Establish the observation model of the obstacle: The observation variables are the position coordinates of the obstacle (x_obss, y_obss). The observation equation is: [x_obss; y_obss] = [1, 0, 0, 0; 0, 1, 0, 0] × [x; y; vx; vy] + [nx; ny], where [nx; ny] is the observation noise. The observation noise obeys a Gaussian distribution with a mean of zero and a covariance matrix of R, R = [σx^2, 0; 0, σy^2], σx and σy are the standard deviations of the sensor's measurement errors in the x and y directions.
[0065] Use the Kalman filter algorithm to predict the position and speed of obstacles. Input: The kinematic model of the obstacle, including the state variables [x; y; vx; vy] and the state transfer equation. The observation model of the obstacle, including the observation variables [x_obss; y_obss] and the observation equation. The observation value at the current moment [x_obss_t; y_obss_t]. The state estimation value at the previous moment [x_t-1; y_t-1; vx_t-1; vy_t-1] and the state estimation covariance matrix P_t-1. The parameters of the Kalman filter, including the process noise covariance matrix Q and the observation noise covariance matrix R. Prediction steps: According to the uniform motion model, establish the state transfer equation of Kalman filter: [x_t; y_t; vx_t; vy_t] = [1, 0, Δt, 0; 0, 1, 0, Δt; 0, 0, 1, 0; 0, 0, 0, 1] × [x_t-1; y_t-1; vx_t-1; vy_t-1]. Calculate the state transfer matrix F: F = [1, 0, Δt, 0; 0, 1, 0, Δt; 0, 0, 1, 0; 0, 0, 0, 1]. Predict the current state estimate [x_t|t-1; y_t|t-1; vx_t|t-1; vy_t|t-1]: [x_t|t-1; y_t|t-1; vx_t|t-1; vy_t|t-1] = F × [x_t-1; y_t-1; vx_t-1; vy_t-1]. Predict the current state estimate covariance matrix P_t|t-1: P_t|t-1 = F × P_t-1 × F^T + Q. Update step: According to the observation model, establish the observation equation of the Kalman filter: [x_obss_t; y_obss_t] = [1, 0, 0, 0; 0, 1, 0, 0] × [x_t; y_t; vx_t; vy_t] + [nx; ny]. Calculate the observation matrix H: H = [1, 0, 0, 0; 0, 1, 0, 0]. Calculate the Kalman gain K_t: K_t = P_t|t-1×H^T×(H×P_t|t-1×H^T+R)^(-1). Calculate the observation residual [z_t]: [z_t] = [x_obss_t; y_obss_t]-H×[x_t|t-1; y_t|t-1; vx_t|t-1; vy_t|t-1]. Update the current state estimate [x_t; y_t; vx_t; vy_t]: [x_t; y_t; vx_t; vy_t] = [x_t|t-1; y_t|t-1; vx_t|t-1;
[0066] vy_t|t-1]+K_t×[z_t]. Update the state estimation covariance matrix P_t at the current moment: P_t=(I-K_t×H)×P_t|t-1. Output: The state estimation value at the current moment [x_t;y_t;vx_t;vy_t], which represents the predicted position and speed of the obstacle. The state estimation covariance matrix P_t at the current moment.
[0067] Figure 4This is an exemplary flow chart for obtaining the predicted coordinates of an obstacle according to some embodiments of this specification, illustrating the use of a particle filter algorithm to correct the obstacle position predicted by a Kalman filter, and inputs: the obstacle prediction value [x_t; y_t; vx_t; vy_t] obtained by the Kalman filter. The observation model of the obstacle includes the observed variables [x_obss_t; y_obss_t] and the probability distribution of the observation noise. The parameters of the particle filter include the number of particles N, the prior map information, and the resampling threshold. Initialize the particle set: initialize a set of particles through the observation model according to the obstacle prediction value obtained by the Kalman filter. Each particle contains position, velocity, and weight information, expressed as [x_i; y_i; vx_i; vy_i; w_i], where i is the number of the particle. The position and velocity of the particle are sampled according to the probability distribution of the predicted value and the observation noise, and the weight is initialized to 1 / N. According to the prior map information, the position of the particle is constrained, and the particles located in the infeasible area are eliminated. Predict the particle set: predict the position and velocity of each particle according to the uniform motion model. For the ith particle, predict its position and velocity [x_i_t+1; y_i_t+1; vx_i_t+1; vy_i_t+1] at the next moment: [x_i_t+1; y_i_t+1; vx_i_t+1; vy_i_t+1] = [1, 0, Δt, 0; 0, 1, 0, Δt; 0, 0, 1, 0; 0, 0, 0, 1] × [x_i_t; y_i_t; vx_i_t; vy_i_t] + [nx; ny; nvx; nvy]. Among them, [nx; ny; nvx; nvy] is process noise, which obeys the preset probability distribution. The predicted particle set {[x_i_t+1; y_i_t+1; vx_i_t+1; vy_i_t+1; w_i]} is obtained. Update particle weights: According to the observation model, calculate the likelihood probability between each predicted particle and the actual observed variable. For the i-th particle, calculate the likelihood probability p_i between it and the observed variable [x_obss_t+1; y_obss_t+1]: p_i = p([x_obss_t+1; y_obss_t+1]|[x_i_t+1; y_i_t+1]), and update the weight of each particle according to the likelihood probability: w_i_t+1 = w_i_t×p_i, and normalize the updated particle weights. Resample particle set: Calculate the effective sample number N_eff of particle weight: N_eff = 1 / (Σ_iw_i^2), if N_eff is less than the preset resampling threshold, resampling is performed. The system resampling algorithm is used to resample the particle set according to the particle weight, and the resampled particle set {[x_i_t+1; y_i_t+1; vx_i_t+1; vy_i_t+1; 1 / N]} is obtained.Calculate the predicted coordinates of the obstacle: Calculate the weighted mean of the resampled particle set as the predicted coordinates of the obstacle [x_t+1; y_t+1]: [x_t+1; y_t+1] = Σ_i(w_i×[x_i_t+1; y_i_t+1]) / Σ_i*w_i. The predicted coordinates of the obstacle [x_t+1; y_t+1] include the position information. Step 5: The CPU compares the predicted coordinates of the obstacle with the current position of the roller and calculates the distance between the two. If the distance is less than the set safety distance threshold, the local obstacle avoidance module is triggered and step 6 is entered; otherwise, step 3 is continued.
[0068] Figure 5 This is an exemplary flow chart for obtaining a local path according to some embodiments of this specification. Step 6: With the current position of the roller as the origin, an artificial potential field model is established, and a local path that avoids obstacles is searched in the potential field through the A* algorithm. Input: the predicted coordinates of the obstacle (xobs, yobs). The coordinates of the target point on the global path (xg, yg). The current position coordinates of the roller (xam, yam). The parameters of the artificial potential field model include the repulsive potential energy gain coefficient k1, the repulsive potential energy attenuation coefficient σ, and the gravitational potential energy gain coefficient k2. The distance threshold d_threshold that triggers the generation of the local path. Calculate the distance between the obstacle and the roller: Calculate the Euclidean distance d between the predicted position of the obstacle (xobs, yobs) and the current position of the roller (xam, yam): d = √((xobs-xam)^2+(yobs-yam)^2). If the distance d is less than the distance threshold d_threshold, the local path generation step is triggered; otherwise, the global path is continued. Establish an artificial potential field model: Take the current position coordinates of the roller (xam, yam) as the origin and establish a two-dimensional artificial potential field model. The potential energy function U(x, y) of the artificial potential field model includes the repulsive potential energy function Urep(x, y) and the attractive potential energy function Uatt(x, y): U(x, y) = Urep(x, y) + Uatt(x, y), and the calculation formula of the repulsive potential energy function Urep(x, y) is: Urep(x, y) = k1 × exp(-d^2 / (2σ^2)), where k1 is the gain coefficient of the repulsive potential energy, σ is the attenuation coefficient of the repulsive potential energy, and d is the Euclidean distance between the predicted obstacle position (xobs, yobs) and (x, y). The calculation formula of the gravitational potential energy function Uatt(x, y) is: Uatt(x, y)=k2×√((x-xg)^2+(y-yg)^2), where k2 is the gain coefficient of the gravitational potential energy, and (xg, yg) is the coordinates of the target point on the target path.
[0069] Search for local paths in the artificial potential field, and map the predicted coordinates of the obstacle into the artificial potential field: According to the predicted coordinates of the obstacle (xobs, yobs), map the obstacle to the corresponding position in the artificial potential field. In the artificial potential field, the position of the obstacle generates repulsion, and the magnitude of the repulsion is calculated by the repulsive potential energy function Urep (x, y). Map the target point coordinates into the artificial potential field: According to the target point coordinates (xg, yg) on the target path, map the target point to the corresponding position in the artificial potential field. In the artificial potential field, the position of the target point generates gravity, and the magnitude of the gravity is calculated by the gravitational potential energy function Uatt (x, y). Use the A* algorithm to search for multiple candidate paths: Take the current position of the roller (xam, yam) as the starting point and the target point (xg, yg) as the end point, and use the A* algorithm to search for multiple candidate paths in the artificial potential field. The search process of the A* algorithm is as follows: a. Initialize the open list and the closed list, and add the starting point to the open list. b. Select the node with the lowest cost from the open list as the current node and move it to the closed list. c. Expand the nodes in the neighborhood of the current node, calculate the cost (i.e., potential energy value) of each neighborhood node, and add it to the open list. Repeat steps b and c until the end is reached or the open list is empty. The cost function of the A* algorithm is the potential energy function U(x, y), that is, the cost of a node is equal to the potential energy value of the node in the artificial potential field. During the search process, the accuracy and efficiency of the search are controlled according to parameters such as the search step size, search direction, and search range. Calculate the sum of the potential energy of each candidate path: For each candidate path obtained by the A* algorithm, calculate the potential energy value U(x, y) of each point on the path. Add up the potential energy values of each point on the path to obtain the sum of the potential energy of the path. Select the path with the smallest sum of potential energy as the local path: Compare the sum of the potential energy of all candidate paths, and select the path with the smallest sum of potential energy as the local path to avoid obstacles. The path with the smallest total potential energy means that the combined effect of repulsion and attraction on the path is the smallest, and it is the optimal avoidance path. The local path for avoiding obstacles consists of a series of coordinate points, representing the path from the current position of the roller to the target point.
[0070] Smoothing the local path. The local path to avoid obstacles consists of a series of coordinate points (x_i, y_i), where i is the index of the coordinate point. The kinematic constraints of the roller include the minimum turning radius R_min and the maximum steering angular velocity ω_max. Smoothing the local path According to the minimum turning radius R_min of the roller, the turning points on the local path are subjected to arc transition. For the three adjacent coordinate points (x_i, y_i), (x_{i+1}, y_{i+1}) and (x_{i+2}, y_{i+2}) on the path, the angle θ between the two line segments is calculated. If the angle θ is greater than the preset angle threshold, an arc transition path is inserted at the turning point. The radius R of the arc transition path satisfies the following conditions: aR≥R_min, ensuring that the roller can travel according to the minimum turning radius. b. The arc is tangent to the two line segments to ensure the continuity and smoothness of the path. Calculate the interpolation points on the arc transition path. The spacing of the interpolation points is determined according to the speed and control cycle of the roller. Add the interpolation points to the local path to replace the original inflection points. Repeat the above steps until all inflection points are processed to obtain a smoothed local path.
[0071] Step 7: The central processing unit smoothes the generated local path according to the kinematic constraints of the roller to obtain a smooth obstacle avoidance path. Then, based on the speed, acceleration and steering angle limits of the roller, a path adjustment instruction is generated and sent to the control system of the roller. The path adjustment instruction is generated, including: generating a path adjustment instruction based on the smoothed local path, including a target steering angle and a target speed. For each coordinate point (x_i, y_i) on the path, the target steering angle of the roller at that point is calculated. and target speed v_i. Target steering angle Calculation method: a. Calculate the slope k_i of the line connecting the current coordinate point (x_i, y_i) and the next coordinate point (x_{i+1}, y_{i+1}). b. Calculate the target steering angle c. Turn the target to an angle Limited to the steering angle range of the roller, that is Calculation method of target speed v_i: a. Calculate the distance d_i between the current coordinate point (x_i, y_i) and the next coordinate point (x_{i+1}, y_{i+1}). b. Calculate the target speed v_i based on the distance d_i and the maximum speed v_max of the roller to ensure that the roller can reach the next coordinate point within the control cycle. c. Limit the target speed v_i to the speed range of the roller, that is, [v_min, v_max]. Generate a target steering angle The path adjustment instruction with the target speed v_i is used to control the movement of the roller. The smoothed local path consists of a series of coordinate points (x_i, y_i). The path adjustment instruction includes the target steering angle and the target speed v_i, which are used to control the roller to travel along the smoothed local path.
[0072] Step 8: The roller control system receives the path adjustment instruction and extracts the target steering angle and target speed v_i. The control system is based on the target steering angle Adjust the steering mechanism of the roller so that the direction of travel of the roller is consistent with the target steering angle. The control system adjusts the travel speed of the roller according to the target speed v_i so that the roller travels at the target speed. The roller travels along the obstacle avoidance path and bypasses the obstacle. The control system monitors the position coordinates (x_t, y_t) of the roller and the position of the obstacle in real time. Calculate the distance d_t between the roller and the obstacle, and determine whether d_t is greater than the preset safety distance d_safe. If d_t>d_safe, it means that the roller has bypassed the obstacle and enters the next step; otherwise, continue to travel along the obstacle avoidance path. The control system calculates the distance d_min from the current position of the roller (x_t, y_t) to the nearest path point (X_k, Y_k) on the global path based on the global travel route. Set (X_k, Y_k) as the new target point of the roller and generate a path from the current position to the new target point. The control system controls the roller to return to the original route based on the generated path. The roller continues to travel along the global travel route to perform the working task.
[0073] Step 9: Repeat steps 3 to 8 until the roller completes all work tasks.
Claims
1. A method for identifying obstacles of a road roller, comprising: Step 1: Install sensors on the roller to collect environmental data, which includes the type and location of obstacles; Step 2: Obtain the roller's travel trajectory record and obstacle prior information, use the path planning algorithm A* to set the roller's global travel route, and generate path planning instructions; Step 3, controlling the roller to operate along the global travel route according to the path planning instruction; During the roller operation, environmental data is collected through sensors; Step 4: Based on the collected environmental data, the Kalman filter algorithm is used to predict the next position and speed of the obstacle based on the kinematic model of the obstacle. The particle filter algorithm is used to update the predicted position of the obstacle based on the observation model of the obstacle to obtain the predicted coordinates of the obstacle. Step 5, calculating the distance between the predicted coordinates of the obstacle and the roller, and comparing the distance with the distance threshold; Step 6: When the calculated distance is less than or equal to the distance threshold, an artificial potential field model is established with the roller as a reference object, the obstacle generates repulsion, and the target point generates attraction; the optimal path is searched in the artificial potential field, and the path with the minimum potential energy is selected as the local path to avoid the obstacle; according to the local path, combined with the kinematic constraints of the roller, a path adjustment instruction is generated; Step 7: According to the path adjustment instruction, the roller is controlled to move along the local path to avoid the obstacle until the obstacle is bypassed; after bypassing the obstacle, the roller returns to the global route to continue the operation; Obtain the predicted coordinates of the obstacle, including: Obtain characteristic information of obstacles based on collected environmental data; characteristic information includes the position and velocity of obstacles; establish kinematic model and observation model of obstacles; kinematic model adopts uniform motion model; state variables include the position and velocity of obstacles; observation variables are the position of obstacles; According to the uniform motion model of the obstacle, the state equation of Kalman filter is established; wherein the state transfer matrix in the state equation is the unit matrix; According to the state equation of Kalman filter, the position and speed of the obstacle at the next moment are predicted to obtain the predicted state variables; According to the observation model of the obstacle, the observation equation of Kalman filter is established; wherein the state variable is associated with the observation variable by using the observation matrix in the observation equation; The observation equation is used to calculate the residual between the predicted state variables and the actual observed variables. Based on the residual and Kalman gain, the predicted state variables are corrected to obtain the corrected position and velocity of the obstacle as the predicted value of the obstacle at the next moment. Obtaining the predicted coordinates of the obstacle also includes: According to the predicted value of the obstacle at the next moment, a group of particles are initialized through the observation model of the obstacle; each particle contains the position, speed and weight of the obstacle, and the prior map information is set to constrain the position of the particle; According to the uniform motion model of the obstacle, the position and speed of each particle are predicted to obtain a predicted particle set; According to the observation model of the obstacle, the likelihood probability between each predicted particle and the actual observed variable is calculated, and the particle weight is adjusted according to the likelihood probability; According to the adjusted particle weights, the particles are resampled using a system resampling algorithm to obtain a resampled particle set; Calculate the weighted mean of the resampled particle set and use the weighted mean as the predicted coordinates of the obstacle; Establish the kinematic model of the obstacle, including: According to the characteristic information of the obstacle, the speed of the obstacle is extracted, and the movement state of the obstacle is divided into a stationary state and a uniform speed state according to the speed; when the speed is less than the speed threshold, the obstacle is judged to be in a stationary state, and when the speed is greater than or equal to the speed threshold, the obstacle is judged to be in a uniform speed movement state; When the obstacle is in a stationary state, a kinematic model of the stationary obstacle is established, and the position coordinates (x, y) in two-dimensional space are used to describe the position state of the obstacle. The position coordinates of the obstacle are used as state variables to establish the state transfer equation; When the obstacle is in a uniform motion state, a kinematic model of the uniform motion obstacle is established, using the position coordinates (x, y) and velocity (v x ,v y ) describes the motion state of the obstacle, takes the position coordinates and velocity of the obstacle as state variables, and establishes the state transfer equation, where the state transfer matrix in the state transfer equation is as follows: Among them, Δt represents the sampling time interval of the sensor; v in the state transfer matrix x ×Δt and v y ×Δt represents the relationship between the obstacle’s position change and speed within the time Δt; The kinematic model of a stationary obstacle and the kinematic model of a uniformly moving obstacle are respectively used as two sub-models of the obstacle kinematic model; according to the motion state of the obstacle, the corresponding sub-model is selected to generate the obstacle kinematic model, and the obstacle kinematic model includes state variables, state transfer equations and state transfer matrices; Establish an observation model of obstacles, including: According to the characteristic information of the obstacle, the position coordinates (x, y) of the obstacle in the two-dimensional space are obtained as the observation variables of the obstacle observation model; According to the state variables in the obstacle kinematic model, the observation equation of the obstacle observation model is established. The observation equation is: [x obs ;y obs ]=[1,0,0,0;0,1,0,0]×[x;y;v x ;v y ]+[n x ;n y ] Among them, [x obs ;y obs ] is the observed position coordinate of the obstacle, [x; y; v x ;v y ] is the state variable in the obstacle kinematic model, namely the position coordinates and velocity of the obstacle, [n x ;n y ] is the observation noise, which obeys the Gaussian distribution with mean zero and covariance matrix R; According to the sensor type and installation location, the covariance matrix R of the observation noise is determined. The covariance matrix R is as follows: Among them, σ x and σ y are the standard deviations of the sensor’s measurement errors in the x and y directions, respectively; The covariance matrix R of the observation equation and the observation noise is used as the parameter of the observation model of the obstacle to generate the observation model of the obstacle. The observation model of the obstacle includes the probability distribution of the observation variable, the observation equation and the observation noise.
2. The obstacle recognition method for a road roller according to claim 1, characterized in that: Generate path planning instructions, including: Step 2.1, divide the working area of the road roller into grids of equal size, each grid represents a node, and the nodes are connected by edges to construct a grid map; Step 2.2: Based on the collected environmental data and the prior information of obstacles, the nodes corresponding to the obstacles are marked in the grid map as inaccessible nodes, and the other nodes except the obstacles are accessible nodes. Step 2.3, according to the roller's travel trajectory record, determine the starting node and the target node of the roller in the grid map; Step 2.4, create an open list and a closed list, the open list is used to store the nodes to be searched, and the closed list is used to store the nodes that have been searched; Step 2.5, add the starting node to the open list, calculate the actual distance and estimated distance of the starting node, the actual distance represents the actual path length from the starting node to the current node, the estimated distance uses Manhattan distance to calculate the estimated path length from the current node to the target node, and the sum of the actual distance and estimated distance of the starting node is used as the evaluation function value of the starting node.
3. The obstacle recognition method for a road roller according to claim 2, characterized in that: Generate path planning instructions, including: Step 2.6, select the node with the smallest evaluation function value from the open list as the current node, remove the current node from the open list, and add it to the closed list; Step 2.7, determine whether the current node is the target node, if yes, execute step 2.12, otherwise, execute step 2.8; Step 2.8, traverse all adjacent nodes of the current node, and for each adjacent node, calculate the evaluation function value of the current node; Step 2.9, determine whether the adjacent node is in the closed list. If so, skip the corresponding node and continue to traverse the next adjacent node; otherwise, execute step 2.10; Step 2.10, determine whether the adjacent node is in the open list, if not, add the corresponding adjacent node to the open list, calculate the estimated distance and evaluation function value of the corresponding adjacent node, and set the current node as the parent node of the adjacent node; if yes, compare the actual distance currently calculated with the actual distance stored in the open list, if the actual distance currently calculated is less than the actual distance stored, update the actual distance, estimated distance, evaluation function value and parent node of the adjacent node; Step 2.11, repeat steps 2.6 to 2.10 until the current node is the target node or the open list is empty; execute step 2.12 or step 2.13; Step 2.12, if the open list is empty, it means that the path from the start node to the target node cannot be found, and the currently searched path is used as the global travel route of the roller, and according to the end node of the global travel route, the corresponding adjacent passable nodes are added to the open list, and then return to step 2.6 to continue searching for the remaining paths; Step 2.13, starting from the target node, trace back along the parent node of the node to generate the optimal path from the start node to the target node as the global travel route of the roller.
4. The obstacle recognition method for a road roller according to claim 3, characterized in that: Generate path planning instructions, including: Step 2.14, smoothing the generated global travel route of the road roller to obtain a smoothed global travel route; Step 2.15, based on the smoothed global travel route of the roller, combined with the speed, acceleration and steering angle of the roller, a cubic spline interpolation algorithm is used to generate a path planning instruction.
5. The obstacle recognition method for a road roller according to claim 1, characterized in that: Generate path adjustment instructions, including: Calculate the distance between the obstacle and the roller based on the predicted coordinates of the obstacle and the current position of the roller; When the calculated distance is less than the distance threshold, the local path generation step is triggered; Taking the current position of the roller as the origin, a two-dimensional artificial potential field model is established, and the predicted coordinates of the obstacle are mapped to the corresponding position in the artificial potential field. The obstacle generates repulsion in the artificial potential field, and the magnitude of the repulsion is inversely proportional to the distance between the obstacle and the roller; the target point on the target path is mapped to the corresponding position in the artificial potential field; the target point generates attraction in the artificial potential field, and the magnitude of the attraction is proportional to the distance between the target point and the roller; The path planning algorithm A* is used to search for multiple candidate paths from the current position of the roller to the target point in the artificial potential field; the potential energy distribution on each candidate path is calculated, and the potential energy distribution is obtained by superimposing the repulsive potential energy and the gravitational potential energy of each point on the path; and the candidate path with the smallest total potential energy is selected as the local path to avoid obstacles; According to the kinematic constraints of the roller, the local path is smoothed to generate a smoothed local path, wherein the smoothing includes performing arc transition on the inflection points on the local path; Based on the generated local path, a path adjustment instruction including a target steering angle and a target speed is generated.
6. The obstacle recognition method for a road roller according to claim 5, characterized in that: Taking the current position of the roller as the origin, a two-dimensional artificial potential field model is established, including: The current position coordinates of the roller (x am ,y am ) as the origin, a two-dimensional artificial potential field model is established. The potential energy function U(x, y) of the artificial potential field model contains the repulsive potential energy function U rep (x,y) and the gravitational potential energy function U att (x,y); The calculation formula of the potential energy function U(x,y) is: U(x,y)=U rep (x,y)+U att (x,y) Repulsive potential energy function U rep The calculation formula for (x,y) is: Among them, k1 is the gain coefficient of repulsive potential energy, σ is the attenuation coefficient of repulsive potential energy, and d is the predicted position of the obstacle (x obs ,y obs ) and the current position of the roller (x am ,y am ), Gravitational potential energy function U att The calculation formula for (x,y) is: Where k2 is the gain coefficient of gravitational potential energy, (x g ,y g ) are the coordinates of the target point on the target path.
