Front-wheel steering vehicle autonomous exploration method and system suitable for complex terrain

Abstract

The invention discloses a front-wheel steering vehicle autonomous exploration method and system suitable for a complex terrain, and the method comprises the steps: 1, obtaining a global rasterized terrain plane map according to original point cloud data; step 2, carrying out trafficability cost evaluation on the global rasterized terrain plane map to obtain a global trafficability cost map and a leading edge point; step 3, dividing a set direction area according to the orientation and steering capability of the vehicle, calculating leading edge gathering points in the set direction by using a geometric analysis algorithm, and obtaining a sparse topological graph of the leading edge points; step 4, local planning: selecting a frontier aggregation point with the lowest navigation comprehensive cost from local frontier aggregation points in the frontier point sparse topological graph as a local planning target point of the current period, and obtaining an initial path by using an improved A * algorithm; and 5, performing global planning. According to the method, the interaction between the vehicle and the terrain can be fully considered, a real local terrain traversal map is generated, and the vehicle can completely and autonomously explore a large outdoor unknown area.

Description

Technical Field

[0001] The present invention relates to the technical field of robot active exploration, and particularly to an autonomous exploration method and system for a front-wheel steering vehicle applicable to complex terrains. Background Art

[0002] Autonomous exploration is a key technology that enables a robot to autonomously move within a specific area and construct a complete map. Its core goal is to achieve rapid map construction while ensuring the quality of map building. This technology has been widely applied in fields such as post-disaster rescue, cave exploration, military confrontation, home service, and 3D reconstruction, and has formed three major technical routes based on frontier theory, view-point theory, and learning.

[0003] These technical routes and their derivative methods have effectively solved the exploration requirements of wheeled robots and unmanned aerial vehicles. However, as the most common form of front-wheel steering robot - the car, due to its special kinematic constraints, it is difficult to directly apply existing exploration algorithms. For example, the historical path construction technology based on Voronoi diagrams cannot be directly adopted by front-wheel steering vehicles. Considering the large number of front-wheel steering vehicles in existence and their applicability in different scenarios, if they can be made to have autonomous exploration capabilities, it will greatly improve the emergency response efficiency. Therefore, researching autonomous exploration algorithms applicable to front-wheel steering vehicles has important practical significance.

[0004] The autonomous exploration method based on frontier theory is based on a limited field of view. By setting the boundary of the sensing domain, candidate frontier points are generated, and combined with planning and control technologies to guide the movement of the robot. However, in complex outdoor environments, traditional methods face challenges such as difficult path tracking and insufficient assessment of terrain traversability, especially for front-wheel steering vehicles. Therefore, combining autonomous exploration with terrain traversability assessment has become an inevitable choice to solve the problem of complex terrain exploration.

[0005] In response to the above problems, some researchers have proposed various improvement schemes: one is an autonomous exploration method for unknown environments based on a frontier search tree. By constructing a frontier tree through an evaluation criterion, depth-first search is used to reduce the path coverage rate, and combined with the A* algorithm and path optimization to achieve real-time exploration; the other is an autonomous exploration method for mobile robots facing rugged terrains. Terrain feasibility assessment is introduced into the traditional exploration framework, and the main branch benefits are evaluated through a view-point gain tree.

[0006] However, in practical applications, there are still two key challenges: one is how to accurately evaluate the traversability of a specific vehicle model on irregular terrains based on existing point cloud data, considering the impact of different vehicle chassis and suspension configurations on terrain adaptability; the other is how to effectively integrate a planning method considering kinematic constraints into the autonomous exploration framework. The solution of these problems is of great significance for improving the practicality of the system.

[0007] Currently, a technical solution of the prior art is as follows: First, update the environmental voxel grid according to the real-time point cloud data, extract the ground grid points for path planning, obtain the frontiers by the perceived boundaries, construct a frontier information set, and form a growing frontier search tree; create a benefit scoring criterion for the frontier points, and use depth-first search to obtain the best target node; furthermore, use A* for path planning in the rasterized map; finally, use the initial solution provided by A*, construct an unconstrained nonlinear optimization problem, build an objective function based on kinematics, ground safety, and smoothness, and solve to obtain the optimal autonomous exploration path. This technology considers the autonomous exploration navigation problem of a vehicle with a motion-limited configuration in autonomous exploration and implements vehicle autonomous exploration by following the classical frontier theory. However, this technology does not consider the impact of the drivable terrain on the vehicle's driving ability, defaults that there are only feasible and infeasible paths for the vehicle to travel, and cannot obtain a relatively flat exploration path. When analyzing the vehicle characteristics, since the ground characteristics are not considered, it is impossible to judge whether the vehicle pose on the actual trajectory meets the requirements. The frontier search tree structure is used to judge the required frontier points, but the growth process of the frontier tree cannot truly reflect the transfer cost between the vehicle's frontier points, and the cost analysis of the frontier points does not consider the comprehensive steering cost of the vehicle.

[0008] Another technical solution of the prior art uses a plane fitting method based on laser point cloud to measure the traversability of a certain area, and defines evaluation indicators such as slope, flatness, sparsity, and height change to analyze the passability of the terrain. A method of hierarchical local exploration and global exploration is adopted to realize the construction of the global map. Among them, the exploration architecture adopts a sampling-based view point method, analyzes the orientation cost, distance cost, and environmental exploration gain cost of each path, and selects the branch with the highest benefit for exploration, and sets the transfer between local exploration and global exploration. However, when analyzing the terrain traversability, this technology does not consider the actual contact situation between the wheels and the ground, cannot truly reflect the vehicle pose, and it is unreasonable to directly measure the traversability through simple plane fitting data. The path generated by the exploration method is not smooth, has poor path tracking performance, and is not suitable for front-wheel steering vehicles. Summary of the Invention

[0009] The purpose of the present invention is to provide an autonomous exploration method and system for a front-wheel steering vehicle applicable to complex terrains to overcome or at least mitigate at least one of the above-mentioned defects of the prior art.

[0010] To achieve the above object, the present invention provides an autonomous exploration method for a front-wheel steering vehicle applicable to complex terrains, which includes:

[0011] Step 1, obtain a global rasterized terrain plane map according to the original point cloud data of the exploration space;

[0012] Step 2: Evaluate the traversability cost of the globally rasterized terrain plane map obtained in Step 1 to obtain a global passability cost map. In the global passability cost map, the leading grids within the sensing boundary of the vehicle-mounted sensor are regarded as leading points;

[0013] Step 3: Divide a number of preset direction areas according to the current vehicle's orientation and steering ability, and calculate the leading aggregation points in each preset direction using a geometric analysis algorithm based on the leading points in Step 2. Update the root node of the topological map to the node corresponding to the current vehicle position in real time, with the vehicle orientation as the root node direction. Connect the updated root node and the leading aggregation point corresponding to this root node using a curve to obtain a sparse topological map of leading points, where the newly generated leading aggregation points at the current moment are defined as local leading aggregation points in the topological map, and other points are defined as global leading aggregation points in the topological map;

[0014] Step 4: Local planning, which specifically includes:

[0015] Select the leading aggregation point with the lowest first navigation comprehensive cost from the local leading aggregation points in the sparse topological map of leading points in Step 3 as the local planning target point for the current cycle. According to the index and orientation of the grid corresponding to the local planning target point for the current cycle, use the improved A* algorithm to expand on the corresponding grid of the global passability cost map, and combine the real-time pose of the vehicle to evaluate the safety of the expanded grid to obtain an initial path, and optimize the initial path using a B-spline curve;

[0016] Step 5: Global planning: Combine the sparse topological structure diagram in Step 3, select the leading aggregation points in the entire sparse topological map, select the leading aggregation point with the lowest second navigation comprehensive cost as the global planning target point for the current cycle, search for a path according to the method in Step 4, and when a local leading aggregation point is rediscovered, enter Step 4 until there are no unexplored leading points in the entire area to be explored.

[0017] Furthermore, the "traversability cost" is determined by the inclination degree I of the fitting plane corresponding to the plane to be evaluated, the surface roughness S a , the vacancy degree V of the fitting plane, and the estimated traversability degree τ: In the case where I, S a , V, and τ are all lower than their respective thresholds, it is regarded as traversable;

[0018] Where:

[0019] τ = w I (I / I max ) + w S (S a / S amax ) + w V (V / V max )), w I, w S , w V are the weight coefficients corresponding to I, S a , V respectively, and the sum of w I , w S and w V is 1. I max , S amax , V max are the maximum values of I, S a , V in the current planning area respectively;

[0020] is the normal vector of the plane to be evaluated relative to the world coordinate system. The superscript z represents the direction of the vector, and n i represents the standard unit normal vector, and the subscript i represents the unit vector;

[0021] n represents the number of point clouds in the grid, m represents the index of the grid, dist(·) represents the absolute value of the Euclidean distance between two spatial point clouds, and p m represents the point cloud point in grid m, and p ref represents the projection point of the point cloud point on the plane to be evaluated;

[0022] represents the reference clustering center.

[0023] Furthermore, in step 4, the cost J when the improved A* algorithm expands to a certain grid includes the distance cost J dis and the heuristic function cost J h , and also includes the backward cost J back , the direction-keeping cost J turn , the estimated traversability τ, and the exploration distance cost J explore of at least one or more;

[0024]

[0025] Among them, turn represents that the vehicle backs up. When the vehicle backs up, the backward cost J back is 1, otherwise it is 0;

[0026]

[0027] Among them, direction1 represents the orientation of the parent node, and direction2 represents the orientation of the child node. When the driving direction changes during the vehicle direction expansion, the direction-keeping cost J turn is 1, otherwise it is 0;

[0028]

[0029] Among them, R represents the sensing range of vehicle-mounted sensors, and dis min represents the closest distance from the center of gravity of the vehicle to the currently observed grid.

[0030] Furthermore, the method of "evaluating the safety of the extended grid in combination with the real-time pose of the vehicle" in step 4 specifically includes:

[0031] Calculate the roll angle β and the front / rear roll chamfer angle α of the vehicle at this time according to the following formula:

[0032]

[0033] Among them, |·| represents taking the absolute value of the vector, respectively represent the first, second, and third terms of respectively represent the first, second, and third terms of. If α < α max and β < β max , then the state of the extended grid is safe. and respectively represent the spatial vectors of the vehicle coordinate system xyz on the plane corresponding to the extended grid, represents the unit normal vector of the plane corresponding to the extended grid, represents the vehicle's orientation vector, α max represents the maximum front / rear roll chamfer angle of the vehicle, and β max represents the maximum roll angle of the vehicle.

[0034] Furthermore, the maximum front / rear roll chamfer angle α max and the maximum roll angle β max of the vehicle are obtained through the following chassis plane acquisition process:

[0035] Step a: Obtain the ground contact center coordinates of the four wheels;

[0036] Step b: According to any three of the ground contact center coordinates of the four wheels, obtain a fitting plane equation according to formula (13) as the chassis plane of the vehicle;

[0037] A 1 X + A 2 Y + A 3 Z + A 4 = 0 (13)

[0038] Among them, A 1 , A 2 , A 3 and A 4 are all constants;

[0039] Step c: According to formula (13), obtain the spatial position coordinates z of the wheel center in the world coordinate system0 = where r is the tire radius of the vehicle;

[0040] Step d, determine the chassis plane stability, according to α of the chassis plane within the stable range max and β max .

[0041] The present invention also provides an autonomous exploration system for a front-wheel steering vehicle applicable to complex terrains, which includes:

[0042] A global rasterized terrain plane map generation module, which is used to obtain a global rasterized terrain plane map according to the original point cloud data of the exploration space;

[0043] A front point generation module, which is used to evaluate the traversability cost of the global rasterized terrain plane map to obtain a global passability cost map. In the global passability cost map, the front grids within the perception boundary of the vehicle-mounted sensor are used as front points;

[0044] A front point topology map acquisition module, which is used to divide several set direction areas according to the orientation and steering ability of the current vehicle, calculate the front aggregation points in each set direction using a geometric analysis algorithm, update the root node of the topology map to the node corresponding to the current position of the vehicle in real time, use the vehicle orientation as the root node direction, and connect the updated root node and the front aggregation point corresponding to the root node with a curve to obtain a sparse front point topology map, where the newly generated front aggregation points at the current moment are defined as local front aggregation points in the topology map, and other points are defined as global front aggregation points in the topology map;

[0045] A local path planning module, which is used to select the front aggregation point with the lowest first navigation comprehensive cost from the local front aggregation points in the sparse front point topology map as the local planning target point for the current cycle. According to the index and orientation of the grid corresponding to the local planning target point for the current cycle, use the improved A* algorithm to expand on the grid corresponding to the global passability cost map, and combine the real-time pose of the vehicle to evaluate the safety of the expanded grid to obtain an initial path, and optimize the initial path using a B-spline curve;

[0046] A global path planning module, which is used to combine the sparse topology structure diagram, select the front aggregation points in the entire sparse topology map, select the front aggregation point with the lowest second navigation comprehensive cost as the global planning target point for the current cycle, search for a path according to the method of the local path planning module, and when a local front aggregation point is rediscovered, the local path planning module re-obtains the optimized initial path until there are no unexplored front points in the entire area to be explored.

[0047] Furthermore, the "traversability cost" is determined by the inclination degree I of the fitting plane corresponding to the plane to be evaluated, the surface roughness S of the fitting plane a , the vacancy degree V of the fitting plane, and the estimated traversability degree τ: in the case where I, S a , V, and τ are all lower than their respective thresholds, it is considered traversable;

[0048] Among them:

[0049] τ = w I (I / I max ) + w S (S a / S amax ) + w V (V / V max ), where w I , w S , and w V are the weight coefficients corresponding to I, S a , and V respectively, and the sum of w I , w S , and w V is 1, and I max , S amax , and V max are the maximum values of I, S a , and V respectively in the current planning area;

[0050] is the normal vector of the plane to be evaluated relative to the world coordinate system, the superscript z represents the direction of the vector, and n i represents the standard unit normal vector, and the subscript i represents the unit vector;

[0051] n represents the number of point clouds in the grid, m represents the index of the grid, dist(·) represents the absolute value of the Euclidean distance between two spatial point clouds, p m represents the point cloud point in the grid m, and p ref represents the projection point of the point cloud point on the plane to be evaluated;

[0052] represents the reference clustering center.

[0053] Furthermore, the cost J when the improved A* algorithm expands to a certain grid includes the distance cost J dis and the heuristic function cost J h , and also includes at least one of the backward cost J back , the direction retention cost J turn , the estimated traversability degree τ, and the exploration distance cost J explore ;

[0054]

[0055] Among them, "turn" indicates that the vehicle is reversing. When the vehicle is reversing, the reverse cost J back is 1; otherwise, it is 0.

[0056]

[0057] Among them, "direction1" represents the orientation of the parent node, and "direction2" represents the orientation of the child node. When the driving direction changes during vehicle direction expansion, the direction maintenance cost J turn is 1; otherwise, it is 0.

[0058]

[0059] Among them, "R" represents the sensing range of the vehicle-mounted sensor, and "dis" min represents the distance from the current grid to the closest point to the vehicle's center of gravity that has been observed.

[0060] Furthermore, when evaluating the safety of the expanded grid in combination with the vehicle's real-time pose, the roll angle β and the front / rear roll chamfer angle α of the vehicle at this time are calculated according to the following formula:

[0061]

[0062] Among them, "|·|" represents taking the absolute value of the vector, respectively represent the first, second, and third terms of respectively represent the first, second, and third terms of. If α < α max and β < β max , then the state of the expanded grid is safe. and respectively represent the spatial vectors of the vehicle coordinate system xyz on the plane corresponding to the expanded grid, represents the unit normal vector of the plane corresponding to the expanded grid, represents the orientation vector of the vehicle, α max represents the maximum front / rear roll chamfer angle of the vehicle, and β max represents the maximum roll angle of the vehicle.

[0063] Furthermore, the maximum front / rear roll chamfer angle α max and the maximum roll angle β max of the vehicle are obtained through the following chassis plane acquisition process:

[0064] Step a, obtain the ground contact center coordinates of the four wheels;

[0065] Step b: According to any three of the ground contact center coordinates of the four wheels, obtain a fitting plane equation according to Equation (13) as the chassis plane of the vehicle;

[0066] A 1 X + A 2 Y + A 3 Z + A 4 = 0 (13)

[0067] where A 1 、A 2 、A 3 and A 4 are all constants;

[0068] Step c: According to Equation (13), obtain the spatial position coordinates of the wheel center in the world coordinate system where r is the tire radius of the vehicle;

[0069] Step d: Judge the stability of the chassis plane, according to α max and β max of the chassis plane within the stable range.

[0070] Due to the above technical solutions, the present invention has the following advantages:

[0071] The present invention can fully consider the interaction between the vehicle and the terrain, generate a real local terrain traversability map, and realize the vehicle's complete autonomous exploration of large outdoor unknown areas. Description of the Drawings

[0072] Figure 1 is a schematic diagram of a working scenario and a vehicle of an embodiment of the present invention.

[0073] Figure 2 is a schematic diagram of an autonomous exploration system for a vehicle with front-wheel steering on complex terrain according to an embodiment of the present invention.

[0074] Figure 3 is a schematic diagram of a leading point recognition strategy according to an embodiment of the present invention.

[0075] Figure 4 is a schematic diagram of a single expansion process of an improved A* algorithm according to an embodiment of the present invention.

[0076] Figure 5 is a schematic diagram of the relationship between roll angle and flip angle according to an embodiment of the present invention. Detailed Embodiments

[0077] In the drawings, the same or similar reference numerals are used to represent the same or similar elements or elements with the same or similar functions. The embodiments of the present invention will be described in detail below with reference to the drawings.

[0078] In the description of the present invention, the orientation or positional relationship indicated by the terms "center", "longitudinal", "lateral", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings. It is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the protection scope of the present invention.

[0079] The scenario in which the present invention is used can be as Figure 1 shown. The working environment is a rugged terrain, and the vehicle is a front-wheel steering vehicle. The vehicle uses lidar to obtain the point cloud data around the vehicle. The current task is to complete the construction of a complete area map within a specified area.

[0080] The autonomous exploration method for a front-wheel steering vehicle applicable to complex terrains provided by the embodiments of the present invention includes:

[0081] Step 1: According to the original point cloud data of the exploration space, perform rasterization zoning processing on the perceived point cloud to obtain a global occupancy grid map, and there is only point cloud information in each grid area. Perform plane fitting on the space corresponding to each grid of the global occupancy grid map to generate a global rasterized terrain plane map. At this time, the global rasterized terrain plane map contains point cloud information and the parameters of the point cloud fitting plane.

[0082] Among them, the method of "generating a global occupancy grid map" is specifically as follows:

[0083] Rasterize the exploration space according to a preset accuracy and update the grid status in the perceived area in real time. The grid status includes four types: free, obstacle, front, and unknown. Among them, the obstacle grid represents an area where the traversability is lower than the safety threshold, and the free grid is an area where the traversability is higher than the safety threshold. The known area refers to the area where the grids in the free and obstacle states are located. The unknown state is both the initial state of the grid and includes areas where the number of point clouds during the exploration process is insufficient for evaluation. The unknown area refers to the area where the grids in the unknown state are located. The front grid is defined as the boundary between the known area and the unknown area. As Figure 3 shown in the figure, in the figure, the red circle indicates the sensing boundary of the vehicle sensor, and the area within the boundary is the sensing range of the vehicle sensor. There is a blue-black rectangle at the center of the red circle, representing the current position of the vehicle. The red dots indicate the current frame point cloud information, the gray grids correspond to the unknown grids, the white grids correspond to the free grids, the black grids correspond to the obstacle grids, and the green grids correspond to the front grids.

[0084] "Plane fitting" can be achieved using existing methods. For example, for each grid space in the generated global occupancy grid map, in this embodiment, the RANSAC algorithm is used to perform point cloud plane fitting to obtain the RANSAC plane. The RANSAC algorithm has the advantages of strong anti-noise and outlier resistance.

[0085] Step 2: Evaluate the traversability cost of the global rasterized terrain plane map in Step 1 to obtain the global passability cost map. The global passability cost map contains point cloud information, point cloud fitting plane parameters, and evaluation indicators transformed from the plane parameters.

[0086] In one embodiment, the evaluation of the vehicle's ground traversability cost mainly depends on the upstream data input of the perception, map, and positioning modules, as well as the environmental scan data of the on-vehicle radar. In areas with limited GPS signals and mapless environments, the system relies only on the raw point cloud or depth data collected by the on-vehicle radar or camera, and performs point cloud fusion through the SLAM algorithm to construct a real-time environmental point cloud map. To achieve the traversability map required for path planning, the system maps the three-dimensional world information into a two-dimensional raster map, where the raster accuracy is initially set based on the vehicle wheel diameter and can be adjusted according to actual needs.

[0087] The "traversability cost" is determined by the traversability evaluation indicators such as the inclination degree I of the fitting plane corresponding to the plane to be evaluated, the surface roughness S of the fitting plane a , the vacancy degree V of the fitting plane, and the estimated traversability degree τ. For example, in the case where I, S a , V, and τ are all lower than their respective thresholds, it is considered traversable;

[0088] Among them:

[0089] τ = w I (I / I max ) + w S (S a / S amax ) + w V (V / V max )), where w I , w S , and w V are the weight coefficients corresponding to I, S a , and V respectively. The sum of w I , w S , and w V is 1. I max , S amax , and V max are the maximum values of I, S a , and V respectively in the current planning area;

[0090] $\vec{n}$ is the normal vector of the plane to be evaluated with respect to the world coordinate system. The superscript $z$ represents the direction in which the vector points. i $\vec{i}$ represents the standard unit normal vector, whose value is $(0, 0, 1)$. The subscript $i$ represents the unit vector.

[0091] $n$ represents the number of point clouds within the grid, $m$ represents the index of the grid, $dist(·)$ represents the absolute value of the Euclidean distance calculated between two spatial point clouds, and $\vec{p}$ m represents the point cloud point within grid $m$, which can be obtained through the cluster center after clustering the point cloud points within grid $m$ using the K - means algorithm. $\vec{p}$ ref represents the projection point $\vec{p}$ of the point cloud point on the plane to be evaluated ref , that is, $\vec{p}$ ref $(x, y, z)$. The acquisition method is as follows: After fitting the plane in step 1, substitute the original $x$ and $y$ coordinates of the point cloud to obtain the new $z$ value. At this time, $\vec{p}$ ref $(x, y, z)$ constitutes the point which is the projection of the original point cloud on the plane.

[0092] $\vec{p}_{ref}$ represents the reference cluster center. The ideal point cloud distribution should be uniform. A large deviation of the cluster center may be due to the missing and accumulated point clouds caused by pits and peaks. The degree of clustering drift is used to measure the plane vacancy. By fitting the plane vacancy $V$, it is possible to directly determine whether there is a point cloud occlusion situation caused by peaks or a blank point cloud area caused by pits in the grid area.

[0093] In this embodiment, for the coefficient degree standard in traversability assessment, a clustering algorithm is used to calculate the point cloud aggregation difference to measure the point cloud vacancy attribute, which is used as one of the traversability measurement indicators. Of course, those skilled in the art can also add other traversability assessment indicators in the prior art.

[0094] In the global traversability cost map, the leading edge grids within the sensing boundary of the vehicle - mounted sensor are used as leading edge points. These leading edge points are used as local leading edge points and global leading edge points, and are respectively put into the local leading edge point set and the global leading edge point set. As Figure 3 shown, according to the current sensing boundary of the vehicle - mounted sensor, in the figure, the red circle indicates the sensing boundary of the vehicle sensor, and the green grids correspond to the leading edge grids. These green grids are the leading edge points. It should be noted that due to the occlusion of obstacles, some areas cannot be observed. Therefore, the occluded part of the leading edge is replaced by the obstacle boundary.

[0095] The so-called local refers to the grid within the perception range of the current frame. Each frame updates the grid status to the global map. The difference between the local and the global is not significant, as both are updated in real-time. However, the local refers to a smaller range, which is a small part of the global. It should be noted that the local map is obtained first, and then the global map is gradually updated and established based on the local map.

[0096] Step 3: Divide several preset direction areas according to the orientation and steering ability of the current vehicle. For example, Figure 3 as shown by the six directions in the schematic. The orange straight line represents the dividing line for the front grid according to the vehicle's steering ability. The circular area covered by the vehicle sensor's perception boundary, which is represented by the red circle, is divided into 6 equal-area fan-shaped areas by four diameters passing through the vehicle center. The division into 6 equal-area blocks is determined based on the vehicle's maximum steering angle of 30°. Of course, according to actual needs and the vehicle's steering ability, it can also be divided into other equal-numbered fan-shaped areas.

[0097] According to the front points in Step 2, use geometric analysis algorithms such as Weiszfeld to calculate the front aggregation points in each preset direction. For example, Figure 3 the blue points shown. Use the front aggregation points to represent the grids in their respective areas. The newly added front aggregation points at the current moment are local front aggregation points. After they leave the perception range, these aggregation points become global front aggregation points and are respectively placed in the local front aggregation point set and the global front aggregation point set.

[0098] Update the root node of the topological map to the node corresponding to the current position of the vehicle in real-time, with the vehicle orientation as the root node direction. Use curves such as RS (the full English name is Reeds-Shepp Curve) to connect the updated root node and the front aggregation points corresponding to this root node, and store the front aggregation points in the local and global front point sets respectively to obtain a sparse topological map of front points. Among them, the newly generated front aggregation points at the current moment are defined as local front aggregation points in the topological map, and other points are defined as global front aggregation points in the topological map. In this embodiment, other curves considering vehicle kinematics, such as the dubins curve, can also be used for the RS curve. However, the advantage of the RS curve is that it can definitely generate a path that meets the requirements of the starting point and the ending point. In this way, the generated topological path map can measure the true motion performance of an Ackermann vehicle, and compared with the dubins curve, the RS curve can measure the vehicle's reversing ability.

[0099] Step 4: Local planning, which specifically includes:

[0100] Judge whether the local front aggregation point set is empty. If it is, screen the local planning target points for the current cycle from the global front aggregation point set; otherwise, screen in the current local front aggregation point set.

[0101] The method for screening local planning target points in the current cycle specifically includes:

[0102] Select the front aggregation point with the lowest first navigation comprehensive cost Cost from the local front aggregation points in the front point sparse topology map as the local planning target point in the current cycle.

[0103] For example, the first navigation comprehensive cost Cost can be evaluated through multiple different cost values. For example: traversability cost Cost tra , exploration gain Cost explore , steering cost gain Cost turn , distance cost Cost dis The sum is shown in Equation (1):

[0104] Cost = w e Cost explore + w t1 Cost turn + w t2 Cost tra + w d Cost dis (1)

[0105] Among them, w e , w t1 , w t2 , w d Are the weight coefficients corresponding to Cost explore , Cost turn , Cost tra , Cost dis respectively. The sum of w e , w t1 , w t2 and w d is 1, and their specific values are determined according to the current actual scenario and work requirements. Different requirements for efficiency, safety, tracking error, etc. can increase or decrease the proportion of different costs respectively. It should be noted that the total cost Cost can also be evaluated through one or two or three of the traversability cost Cost tra , exploration gain Cost explore , steering cost gain Cost turn , distance cost Cost dis , or even additional other indicators in the prior art can be obtained.

[0106] In one embodiment, the traversability cost Cost tra can be set as Equation (2):

[0107]

[0108] Among them, τ represents the traversability of the grid, and the traversability cost in this embodiment is represented by the average traversability within a rectangular range of a*a around the leading edge aggregation point.

[0109] In one embodiment, the exploration gain Cost explore can be set as Equation (3):

[0110]

[0111] where State un represents the grid with an unknown grid state. is(·) is used to judge the number of grids that do not conform to the predetermined state, and r and count respectively represent the observation range and the number of grids.

[0112] In one embodiment, the turning cost gain Cost turn can be set as Equation (4):

[0113]

[0114] where Δθ represents the angle between the vector formed by the leading edge aggregation point and the current vehicle's unknown and the vehicle's orientation vector, and θ max represents the maximum deviation angle that can be traveled within the maximum turning angle in the vehicle's planning range.

[0115] In one embodiment, the distance cost Cost dis can be set as Equation (5):

[0116] Cost dis = Dijkstra(P goal )(5)

[0117] where P goal represents the central position corresponding to the selected leading edge point grid, and the formula Dijskstra(·) represents the total distance length of the route obtained from the current position as the root node to the target point using the Dijkstra algorithm.

[0118] Of course, in addition to the traversability cost Cost tra provided by Equation (2), the exploration gain Cost explore provided by Equation (3), the turning cost gain Cost turn provided by Equation (4), and the distance cost Cost dis provided by Equation (5), other methods in the prior art can also be used to measure their respective costs.

[0119] After screening out the local planning target points in the current cycle, the method for obtaining the initial path according to the index and orientation of the grid corresponding to the planning target points in the current cycle specifically includes:

[0120] ① Obtain the index of the planned target point in the current cycle, as well as the grid index and starting orientation of the starting point.

[0121] ② Enter the loop iteration. Take the starting point as the current node and add it to the open list. In each iteration, pop the grid index with the lowest cost J from the open list. Expand the grids in six states according to the expansion rules. If the grid point has been expanded as the current node or the expanded grid fails the safety assessment, add it to the close list. Judge whether the newly expanded grid state exists in the open list and the close list. If it exists in the open list, compare the cost of the newly expanded grid with the original cost. If the new cost is lower than the original cost, update the grid state; if it exists in the close list, abandon the expansion. Before reaching the maximum expansion times or iteration time, repeat the iteration until the target index point is added to the open list.

[0122] ③ Start from the planned target point in the current cycle, perform backtracking of the parent node, and return the path list.

[0123] The grid state is defined in 8 states, including the position index and orientation. The orientation is divided into eight directions: east, south, west, north, southeast, southwest, northwest, and northeast. Multiple states can coexist in the same grid. For example: for the existing grids A, B, and C, for a certain state in A, a new state with the south direction is expanded to C, and for another state in B, a new state with the north direction is expanded to C. At this time, there are two states in C, one north and one south, sharing this grid without interfering with each other. However, if there is another grid D, and for a certain state in D, a new state with the north direction is also expanded to C. At this time, the new state expanded from the state at D is in the same grid and the same direction as the state expanded from the state at A before, resulting in a conflict, and only one can be retained, and the one with the minimum cost is retained.

[0124] Use the improved A* algorithm to expand on the grid corresponding to the global passable cost map. Compared with the traditional A* algorithm that expands the surrounding eight grids, this algorithm restricts the state of the expanded grid according to the steering limit rules, such as Figure 4As shown in the figure, the grid expansion can select three expansion states, forward and backward. Taking the current orientation as rightward as an example, due to the vehicle's steering angle limitation, the grid expansion only allows the vehicle to execute according to the current orientation. Turning left and right correspond to the right grid facing east, the upper right grid facing northeast, and the lower right grid facing southeast respectively. When the vehicle reverses, it is also restricted by steering and orientation, corresponding to the left grid facing east, the upper left grid facing southeast, and the lower left grid facing northeast respectively. The red grid represents the currently expanded grid, the green grids are the six new states expanded this time, the black arrows represent the orientations corresponding to the grids, the blue arrows represent the process of orientation change, and the yellow arrows represent the possible grids and corresponding orientations for the next forward expansion of the state corresponding to the newly expanded grid in the upper right corner.

[0125] When the improved A* algorithm expands to a certain grid, the cost J includes the distance cost J dis and the heuristic function cost J h , the distance cost function J dis , records the total distance length from the starting point to the current point; the heuristic function cost J h , records the Euclidean distance from the current position to the end point.

[0126] The cost J also includes the reverse cost J back , the direction maintainability cost J turn , the estimated traversability τ, and the exploration distance cost J explore of at least one or more;

[0127]

[0128] Among them, turn represents that the vehicle reverses. When the vehicle reverses, the reverse cost J back is 1, otherwise it is 0. That is to say, when the vehicle reverses compared to the parent grid state during the expansion process, the reverse cost is introduced. Here, turn indicates that the current grid point is obtained from the parent node using the reverse strategy. Reversing is not desirable and is only a strategy chosen when a solution cannot be found by going straight, or when the cost of executing the non-reverse strategy is much greater than that with the reverse strategy.

[0129]

[0130] Among them, direction1 represents the orientation of the parent node, and direction2 represents the orientation of the child node. When the driving direction changes during the vehicle's direction expansion, the direction maintainability cost J turn is 1, otherwise it is 0;

[0131]

[0132] Among them, R represents the sensing range of on-vehicle sensors, and dis min represents the closest distance from the center of gravity of the vehicle to the currently observed grid. For example, if R is set to 20m, the vehicle passes by location A for the first time, and the distance from A is 10m. During the second exploration process, the vehicle passes by location A again, and this time the distance from A is 5m. Then this 5m is retained as dis min .

[0133] In one embodiment, the cost J when the improved A* algorithm expands to a certain grid is expressed as Equation (9):

[0134] J = w d1 J dis + w h J h + w b J back + w t J turn + w e J explore (9)

[0135] Among them, J represents the cost when expanding to a certain grid, and w d1 , w h , w b , w t , w e are coefficients, and their sum is 1. And when the current exploration state is global exploration, w e is 0. J dis ensures that the total exploration route is as short as possible, J h ensures that the exploration route reaches the end point quickly, J back tries to avoid reversing in the exploration route, J turn avoids frequent steering operations during exploration, J explore reduces the degree of path repetition during exploration.

[0136] By expanding the grid and combining the real-time pose of the vehicle to evaluate the safety of the expanded grid, an initial path is obtained, and the B-spline curve is used to optimize the initial path. In this embodiment, the improved A* algorithm is introduced. Compared with HybridA*, the improved A* significantly reduces the calculation speed, and the solution searched by the improved A* provides the node direction of the B-spline curve because it considers part of the steering performance.

[0137] The cost value initially calculated in the grid map is only used for cost evaluation and is directly used to judge whether the vehicle is feasible, which is unreliable. The real vehicle state is related to the contact situation between the vehicle wheels and the ground. When expanding the grid, the center position of the vehicle's center of gravity is used to replace the expansion point. When the vehicle cannot be in a safe posture at the measured position, this expansion is discarded.

[0138] The method of "evaluating the safety of the extended grid in combination with the real-time pose of the vehicle" in step 4 specifically includes:

[0139] Calculate the roll angle β and the front / rear roll chamfer angle α of the vehicle at this time according to the following formula:

[0140]

[0141] Where, |·| represents taking the absolute value of the vector, respectively represent the first, second, and third terms of, respectively represent the first, second, and third terms of. If α < α max and β < β max , then the state of the extended grid is safe, and the angular relationship is as Figure 5 shown, and respectively represent the spatial vectors of the vehicle coordinate system xyz on the plane corresponding to the extended grid, represents the unit normal vector of the plane corresponding to the extended grid, represents the vehicle's orientation vector, α max represents the maximum front / rear roll chamfer angle of the vehicle, β max represents the maximum roll angle of the vehicle.

[0142] This embodiment combines the estimated pose of the vehicle to measure the real pose of the vehicle's movement, judges the real traversability of the vehicle, and improves the feasibility of traditional traversability estimation.

[0143] When the vehicle is on an uneven terrain, due to the tilt of the vehicle body, the positions of the wheels change, and the positions of the four wheels are calculated according to the following formula (12):

[0144]

[0145] Where, P 1 , P 2 , P 3 , P 4 respectively represent the position coordinates of the ground contact points of the vehicle's left rear wheel, right rear wheel, right front wheel, and left front wheel, and P 34 represents the position of the front suspension center of the vehicle, P G represents the position of the center of gravity of the vehicle, and a represents the distance from the center of gravity of the vehicle to the center of gravity of the front suspension, represents the spatial vector representation of the vehicle coordinate system xyz on the fitting plane in step 1, represents the unit normal vector of the fitting plane in step 1, It represents the orientation vector of the vehicle, L represents the suspension distance of the vehicle, and B represents the wheelbase of the vehicle. Pab represents the mid-coordinate of the line segment connecting position Pa and position Pb.

[0146] When the vehicle is traveling on uneven terrain, most of the time, the four wheels of the vehicle do not contact the ground simultaneously. Therefore, it is necessary to consider which three wheels are in contact with the ground at a certain position of the vehicle and determine whether the chassis plane formed at this time is reasonable. Therefore, the combination methods are P 1 P 2 P 3 、P 1 P 2 P 4 、P 1 P 3 P 4 、P 2 P 3 P 4 There are four kinds. The most stable existing mode is selected from the four combination cases. Therefore, it is necessary to obtain the centroid positions of the chassis planes of these four contact cases respectively, and the plane with the lowest centroid is the most stable plane.

[0147] The maximum front / rear roll chamfer α max and the maximum roll angle β max of the vehicle are preferably obtained through the following chassis plane acquisition process. Of course, α max and β max can also be obtained by existing methods:

[0148] Step a: Obtain the ground contact center coordinates of the four wheels, which specifically include:

[0149] Simplify the tire model to a rigid body, and approximately consider the contact point between the wheel and the fitting plane in Step 1 as the contact position when the vehicle is on the plane. Then, the ground contact center coordinates of each wheel of the vehicle can be obtained through P.

[0150] Step b: According to any three of the ground contact center coordinates of the four wheels, a fitting plane equation can be obtained according to Equation (13), and this fitting plane equation is regarded as the chassis plane of the vehicle.

[0151] A 1 X + A 2 Y + A 3 Z + A 4 =0 (13)

[0152] where A 1 、A 2 、A 3 and A 4 are all constants.

[0153] Step c, according to Equation (13), the spatial position coordinates in the world coordinate system of the wheel center can be obtained where r is the tire radius of the vehicle.

[0154] Step d is to judge the stability of the chassis plane.

[0155] Step d1, taking P(x 1 , y 1 , z 1 ), P(x 2 , y 2 , z 2 ), and P(x 3 , y 3 , z 3 ) as examples, the fitting plane equation is expressed as Equation (14):

[0156] A 1 (x - x 1 ) + A 2 (y - y 1 ) + A 3 (z - z 1 ) + A 4 = 0 (14)

[0157] Then, based on P(x 1 , y 1 , z 1 ), P(x 2 , y 2 , z 2 ), and P(x 3 , y 3 , z 3 ), the A 1 , A 2 , A 3 and A 4 expressed by Equations (15) - (18) can be obtained:[[]]

[0158] A 1 = (y 2 - y 1 )(z 3 - z 1 ) - (z 2 - z 1 )(y 3 - y 1 ) (15)

[0159] A 2 = (z 2 - z 1 )(x 3 - x 1 ) - (x 2 - x1 )(z 3 -z 1 )(16)

[0160] A 3 =(x 2 -x 1 )(y 3 -y 1 )-(y 2 -y 1 )(x 3 -x 1 )(17)

[0161] A 4 =-(A 1 x 1 +A 2 y 1 +A 3 z 1 )(18)

[0162] Step d2: calculate the obtained A 1 , A 2 , A 3 and A 4 and the vehicle center of gravity position (x g ,y g ,z g ) into formula (14), we can obtain the projection position z of the center of gravity of the vehicle on the chassis plane g , compare the projection position z of the center of gravity of the chassis of the above four combinations g , determine the most stable chassis plane. x g and g Each car has its own factory configuration, and parameters will be provided or measured using measuring instruments.

[0163] Step d3, obtain the vehicle rollover angle and rollover angle according to the chassis plane to ensure that the roll pitch angle is within the vehicle steady-state range. According to the static stability of the vehicle, the maximum rollover angle of the vehicle is The maximum tipping angle is Flip forward to Where: h g is the height of the vehicle's center of mass, B is the vehicle's wheelbase, b is the distance from the vehicle's rear axle center of gravity to the center of mass, and a is the distance from the vehicle's front axle center of gravity to the center of mass.

[0164] In one embodiment, the B-spline curve optimization method is as follows:

[0165] The path generated by improving the A* algorithm will have sharp inflection points. To make the planned path more in line with the driving mode of the vehicle, the path generated by the improved A* is smoothed. Compared with other spline curves (Bezier, Hermite, Carmull-Rom), the B-spline curve has C 2 continuity, which can ensure the smoothness of curvature and acceleration. The initial path points are obtained according to the improved A* as control points, and the control point set is Q = {Q 0 , Q 1 , Q 2 ,..., Q s}, where s represents one less than the number of control points. Construct the knot vector. For the cubic B-spline curve, a continuous curve is constructed by repeating the endpoint values, and the knot vector can be defined as:

[0166] U = {u 0 , u 1 , u 2 ,..., u j ,..., u s+3}(19)

[0167]

[0168] Then the B-spline curve can be expressed as S(u), as shown in Equation (21):

[0169]

[0170] According to the path smoothness requirement, points are evenly sampled on the path generated by the continuous B-spline curve. The specific operation is to sample q discrete points t 1 , t 2 , t 3 ,…, t q within the range of [0, 1], and use Equation (22) to calculate the corresponding curve points S(t k ), which are used for path tracking or control in actual applications;

[0171]

[0172] In this embodiment, the A* expansion method is improved in combination with the characteristics of the main body's steering limitation, and path smoothing is performed in combination with the cubic B-spline curve.

[0173] Step 5, global planning: Combine the sparse topological structure diagram in Step 3, select the leading aggregation points in the entire sparse topological diagram, evaluate the second navigation comprehensive cost of the global leading aggregation points, select the leading aggregation point with the lowest navigation comprehensive cost as the global planning target point for the current cycle, search for the path according to the method in Step 4, and when a local leading aggregation point is rediscovered, enter Step 4 until there are no unexplored leading points in the entire area to be explored.

[0174] Among them, the difference between the second navigation comprehensive cost and the first navigation comprehensive cost is that when the current exploration target is a local frontier gathering point, the first navigation comprehensive cost does not need to consider the exploration distance cost. When it is a global frontier gathering point, in order to avoid path duplication, the second navigation comprehensive cost introduces the exploration distance cost to improve the exploration quality.

[0175] The vehicle gives priority to local exploration. After the local exploration is completed, a global guidance algorithm is needed to guide the vehicle to the sub-area that has not been explored. The guidance process should pay attention to avoiding too many or too close repeated paths with the explored area to ensure the exploration quality and exploration time. When there are no unknown grids within the latest perception range of the vehicle, that is, the current node cannot expand a new leaf node, the local exploration is considered to be completed. At this time, it switches to the global layer search to search for the remaining leaf nodes in the topology map. Compared with local exploration, global exploration achieves alienation from repeated paths in the explored area by increasing the exploration cost term, improving the exploration quality and reducing the similarity of the exploration path.

[0176] This embodiment constructs a hierarchical structure of local exploration and global exploration. Local exploration focuses on local frontier gathering points within the current perception range, uses improved A* to search for preliminary paths, and uses B-spline curve optimization to obtain the final solution path. After the local path exploration is completed, it turns to the global exploration stage. The distance cost, steering cost, and exploration cost of the global frontier gathering points are evaluated in combination with the sparse topological map to construct the cost term. The path search process is the same as the local exploration. If the local frontier gathering points are re-explored in the global planning stage, the vehicle will re-enter the local exploration mode until there are no unexplored frontier points in the limited area.

[0177] The embodiment of the present invention further provides a front-wheel steering vehicle autonomous exploration system suitable for complex terrain, which includes a global rasterized terrain plane map generation module, a frontier point generation module, a frontier point topology map acquisition module, a local path planning module and a global path planning module, wherein:

[0178] The global rasterized terrain plane map generation module is used to obtain the global rasterized terrain plane map based on the original point cloud data of the exploration space.

[0179] The frontier point generation module is used to evaluate the traversability cost of the global rasterized terrain plane map to obtain a global traversability cost map. In the global traversability cost map, the frontier grid within the boundary perceived by the vehicle-mounted sensor is used as the frontier point.

[0180] The leading-edge point topology map acquisition module is used to divide several set direction regions according to the orientation and steering ability of the current vehicle, calculate the leading-edge aggregation points in each set direction using a geometric analysis algorithm, update the root node of the topology map in real time to the node corresponding to the current position of the vehicle, use the vehicle orientation as the root node direction, and connect the updated root node and the leading-edge aggregation points corresponding to this root node with curves to obtain a sparse leading-edge point topology map. Among them, the newly generated leading-edge aggregation points at the current moment are defined as local leading-edge aggregation points in the topology map, and other points are defined as global leading-edge aggregation points in the topology map.

[0181] The local path planning module is used to select the leading-edge aggregation point with the lowest first navigation comprehensive cost from the local leading-edge aggregation points in the sparse leading-edge point topology map as the local planning target point for the current cycle. According to the index and orientation of the grid corresponding to the local planning target point for the current cycle, use the improved A* algorithm to expand on the grid corresponding to the global passability cost map, and combine the real-time pose of the vehicle to evaluate the safety of the expanded grid to obtain an initial path, and optimize the initial path using a B-spline curve.

[0182] The global path planning module is used to combine the sparse topology structure diagram, select the leading-edge aggregation points in the entire sparse topology map, select the leading-edge aggregation point with the lowest second navigation comprehensive cost as the global planning target point for the current cycle, search for a path according to the method of the local path planning module, and when a local leading-edge aggregation point is rediscovered, the local path planning module re-obtains the optimized initial path until there are no unexplored leading-edge points in the entire area to be explored.

[0183] Finally, it should be pointed out that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Those of ordinary skill in the art should understand that the technical solutions recorded in the foregoing embodiments can be modified, or some of the technical features can be equivalently replaced; these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A front-wheel steering vehicle autonomous exploration method suitable for complex terrain, characterized in that: include: Step 1: Obtain a global rasterized terrain plane map based on the original point cloud data of the exploration space; Step 2, evaluating the traversability cost of the global rasterized terrain plane map of step 1 to obtain a global traversability cost map, in which the frontier grid within the perception boundary of the vehicle-mounted sensor is used as the frontier point; Step 3: Divide the area into several set directions according to the current vehicle's orientation and steering capability, and calculate the frontier aggregation points in each set direction using the geometric analysis algorithm based on the frontier points in step 2, and update the root node of the topological map to the node corresponding to the vehicle's current position in real time. The vehicle orientation is used as the root node direction, and a curve is used to connect the updated root node and the frontier aggregation points corresponding to the root node to obtain a sparse topological map of frontier points, where the frontier aggregation points newly generated at the current moment are defined as local frontier aggregation points in the topological map, and other points are defined as global frontier aggregation points in the topological map; Step 4, local planning, includes: Select the frontier aggregation point with the lowest comprehensive navigation cost from the local frontier aggregation points in the frontier point sparse topology map of step 3 as the local planning target point of the current cycle, and use the improved A* algorithm to expand the grid corresponding to the global passability cost map according to the index and orientation of the grid corresponding to the local planning target point of the current cycle, and evaluate the safety of the expanded grid in combination with the real-time posture of the vehicle to obtain the initial path and optimize the initial path; Step 5, global planning: Combined with the sparse topology structure diagram of step 3, select the frontier aggregation points in the entire sparse topology diagram, select the frontier aggregation point with the lowest second navigation comprehensive cost as the global planning target point of the current cycle, search for the path according to the method of step 4, and when the local frontier aggregation point is re-explored, enter step 4 until there are no unexplored frontier points in the entire area to be explored.

2. The front-wheel steering vehicle autonomous exploration method suitable for complex terrain as claimed in claim 1, characterized in that: The "traversability cost" is determined by the inclination degree I of the fitting plane corresponding to the plane to be evaluated, the surface roughness S of the fitting plane a , fitting plane vacancy V, and estimating the degree of traversability τ to determine: a When , V and τ are all below their respective thresholds, it is considered traversable; in: τ=w I (I / I max )+w S (S a / S amax )+w V (V / V max ), w I 、w S 、w V Corresponding to I and S respectively a , weight coefficient of V, w I 、w S and w V The sum of is 1, I max , S amax 、V max They are the corresponding I and S in the current planning area. a , the maximum value of V; is the normal vector of the plane to be evaluated relative to the world coordinate system. The superscript z represents the direction in which the vector points. n i represents the standard unit normal vector, and the subscript i represents the unit vector; n represents the number of point clouds in the grid, m represents the index of the grid, dist(·) represents the absolute value of the Euclidean distance between two spatial point clouds, and p m represents the point cloud point in the grid m, p ref Represents the projection point of the point cloud point on the plane to be evaluated; represents the reference cluster center.

3. The front-wheel steering vehicle autonomous exploration method suitable for complex terrain as claimed in claim 1, characterized in that: In step 4, the cost J of improving the A* algorithm when it is extended to a certain grid includes the distance cost J dis and the heuristic function cost J h , including the retreat cost J back , direction-maintaining cost J turn , estimated traversability τ and exploration distance cost J explore At least one of the following: Among them, turn means that the vehicle moves backward. When the vehicle moves backward, the backward cost J back is 1, otherwise it is 0; Wherein, direction1 represents the direction of the parent node, and direction2 represents the direction of the child node. When the driving direction changes during the vehicle direction expansion, the direction preservation cost J turn is 1, otherwise it is 0; Among them, R represents the sensing range of the vehicle sensor, min Indicates the closest distance to the vehicle's center of gravity observed in the current grid.

4. The autonomous exploration method for a front-wheel steering vehicle suitable for complex terrain according to any one of claims 1 to 3, characterized in that: The method of "assessing the safety of the extended grid in combination with the real-time posture of the vehicle" in step 4 specifically includes: The vehicle's rollover angle β and front / rear rollover angle α are calculated according to the following formula: Among them, |·| means taking the absolute value of the vector, Respectively The first, second, and third items, Respectively The first, second, and third terms of max And β<β max , then the extended grid state is safe, and Respectively represent the space vectors of the vehicle coordinate system xyz on the plane corresponding to the extended grid, Represents the unit normal vector of the plane corresponding to the extended grid, represents the vehicle's orientation vector, α max Indicates the maximum front / rear rollover angle of the vehicle, β max Indicates the maximum rollover angle of the vehicle.

5. The front-wheel steering vehicle autonomous exploration method applicable to complex terrain as claimed in claim 4, characterized in that: The maximum front / rear rollover angle of the vehicle max and the maximum rollover angle β max The chassis plane is obtained through the following process: Step a, obtaining the ground contact center coordinates of the four wheels; Step b, according to any three of the ground contact center coordinates of the four wheels, a fitting plane equation is obtained according to formula (13) as the chassis plane of the vehicle; A1X+A2Y+A3Z+A4=0 (13) Among them, A1, A2, A3 and A4 are all constants; Step c: According to formula (13), obtain the spatial position coordinates of the wheel center in the world coordinate system: Where r is the tire radius of the vehicle; Step d, judging the stability of the chassis plane, according to the α of the chassis plane within the stable range max and β max .

6. An autonomous exploration system for front-wheel steering vehicles suitable for complex terrain, characterized in that: include: A global rasterized terrain plane map generation module is used to obtain a global rasterized terrain plane map based on the original point cloud data of the exploration space; A frontier point generation module is used to evaluate the traversability cost of the global rasterized terrain plane map to obtain a global traversability cost map. In the global traversability cost map, the frontier grid within the perception boundary of the vehicle-mounted sensor is used as the frontier point; The frontier point topology map acquisition module is used to divide several set direction areas according to the current vehicle's direction and steering ability, calculate the frontier aggregation points in each set direction using a geometric analysis algorithm, update the root node of the topology map to the node corresponding to the vehicle's current position in real time, use the vehicle's direction as the root node direction, and use a curve to connect the updated root node and the frontier aggregation points corresponding to the root node to obtain a frontier point sparse topology map, where the frontier aggregation points newly generated at the current moment are defined as local frontier aggregation points in the topology map, and other points are defined as global frontier aggregation points in the topology map; The local path planning module is used to select the frontier aggregation point with the lowest comprehensive navigation cost from the local frontier aggregation points in the frontier point sparse topology map as the local planning target point of the current cycle, and use the improved A* algorithm to expand the grid corresponding to the global passability cost map according to the index and orientation of the grid corresponding to the local planning target point of the current cycle, and evaluate the safety of the expanded grid in combination with the real-time posture of the vehicle to obtain the initial path and optimize the initial path; The global path planning module is used to combine the sparse topological structure map, select the frontier aggregation points in the entire sparse topological map, select the frontier aggregation points with the lowest comprehensive cost of the second navigation as the global planning target point of the current cycle, search for the path according to the method of the local path planning module, and when the local frontier aggregation points are re-explored, the local path planning module re-acquires the optimized initial path until there are no unexplored frontier points in the entire area to be explored.

7. The front-wheel steering vehicle autonomous exploration system suitable for complex terrain as claimed in claim 6, characterized in that: The "traversability cost" is determined by the inclination degree I of the fitting plane corresponding to the plane to be evaluated, the surface roughness S of the fitting plane a , fitting plane vacancy V, and estimating the degree of traversability τ to determine: a When , V and τ are all below their respective thresholds, it is considered traversable; in: τ=w I (I / I max )+w S (S a / S amax )+w V (V / V max ), w I 、w S 、w V Corresponding to I and S respectively a , weight coefficient of V, w I 、w S and w V The sum of is 1, I max , S amax 、V max They are the corresponding I and S in the current planning area. a , the maximum value of V; is the normal vector of the plane to be evaluated relative to the world coordinate system. The superscript z represents the direction in which the vector points. n i represents the standard unit normal vector, and the subscript i represents the unit vector; n represents the number of point clouds in the grid, m represents the index of the grid, dist(·) represents the absolute value of the Euclidean distance between two spatial point clouds, and p m represents the point cloud point in the grid m, p ref Represents the projection point of the point cloud point on the plane to be evaluated; represents the reference cluster center.

8. The front-wheel steering vehicle autonomous exploration system suitable for complex terrain as claimed in claim 6, characterized in that: The cost J of improving the A* algorithm when it is extended to a certain grid includes the distance cost J dis and the heuristic function cost J h , including the retreat cost J back , direction-maintaining cost J turn , estimated traversability τ and exploration distance cost J explore At least one of the following: Among them, turn means that the vehicle moves backward. When the vehicle moves backward, the backward cost J back is 1, otherwise it is 0; Wherein, direction1 represents the direction of the parent node, and direction2 represents the direction of the child node. When the driving direction changes during the vehicle direction expansion, the direction preservation cost J turn is 1, otherwise it is 0; Among them, R represents the sensing range of the vehicle sensor, min Indicates the closest distance to the vehicle's center of gravity observed in the current grid.

9. The front-wheel steering vehicle autonomous exploration system suitable for complex terrain as claimed in claim 6, characterized in that: When evaluating the safety of the extended grid in combination with the real-time position of the vehicle, the rollover angle β and the front / rear rollover angle α of the vehicle at this time are calculated according to the following formula: Among them, |·| means taking the absolute value of the vector, Respectively The first, second, and third items, Respectively The first, second, and third terms of max And β<β max , then the extended grid state is safe, and Respectively represent the space vectors of the vehicle coordinate system xyz on the plane corresponding to the extended grid, Represents the unit normal vector of the plane corresponding to the extended grid, represents the vehicle's orientation vector, α max Indicates the maximum front / rear rollover angle of the vehicle, β max Indicates the maximum rollover angle of the vehicle.

10. The front-wheel steering vehicle autonomous exploration system suitable for complex terrain as claimed in claim 6, characterized in that: The maximum front / rear rollover angle of the vehicle max and the maximum rollover angle β max The chassis plane is obtained through the following process: Step a, obtaining the ground contact center coordinates of the four wheels; Step b, according to any three of the ground contact center coordinates of the four wheels, a fitting plane equation is obtained according to formula (13) as the chassis plane of the vehicle; A1X+A2Y+A3Z+A4=0 (13) Among them, A1, A2, A3 and A4 are all constants; Step c: According to formula (13), obtain the spatial position coordinates of the wheel center in the world coordinate system: Where r is the tire radius of the vehicle; Step d, judging the stability of the chassis plane, according to the α of the chassis plane within the stable range max and β max .

Images