CN114812581B - An Off-Road Environment Navigation Method Based on Multi-sensor Fusion - Google Patents

Abstract

The invention discloses an off-road environment navigation method based on multi-sensor fusion. Collect vehicle-mounted multi-sensor data for data fusion, input the SLAM algorithm after fusion, add various constraints, and use a graph-based optimization framework to solve to obtain accurate pose; then splicing multiple environmental observations, based on filter fusion algorithm to construct off-road topographic map and global Occupy the grid map; extract the current drivable area of the vehicle in real time according to the obtained global reference path; use the RRT path planning algorithm based on multi-attribute evaluation in the current drivable area to generate a local optimal path for obstacle avoidance navigation. The present invention achieves fast and accurate obstacle avoidance navigation in an unknown off-road environment by using multi-sensor fusion for precise positioning and constructing a global occupancy grid map.

Description

An Off-Road Environment Navigation Method Based on Multi-sensor Fusion

technical field

The invention relates to the technical field of multi-sensor fusion navigation, in particular to an off-road environment navigation method based on multi-sensor fusion.

Background technique

SLAM (simultaneous localization and mapping, simultaneous localization and mapping) technology can simultaneously perceive the surrounding environment and estimate its own pose, and can realize incremental map construction in unknown environments, so that autonomous vehicles can significantly reduce satellite positioning during autonomous navigation. and prior map dependencies. Due to the influence of factors such as unstructured roads, irregular environmental features, and unstable satellite signals in the off-road environment, the use of single sensor data for autonomous vehicle positioning will face huge challenges; in terms of map construction, off-road environment information is complex , Road boundaries are uneven, and it is difficult to extract the passable area, which increases the difficulty of building a navigation map.

Different from the two-dimensional navigation in the urban road, when the unmanned vehicle navigates autonomously in the off-road environment, the common path planning algorithm cannot generate the path quickly and effectively due to the lack of information such as lane lines and boundaries on the off-road road. RRT (rapidly exploring random tree, rapidly expanding random tree) is a path planning algorithm that can quickly generate safe and feasible paths. It has been widely used in the direction of robots and autonomous driving. It is very useful in realizing fast and accurate obstacle avoidance navigation on off-road roads. great potential.

The disadvantage of the existing technology is that the current autonomous navigation of unmanned vehicles in an off-road environment, due to unstructured roads, lack of navigation information, and unstable satellite information, will lead to the inability to construct maps normally, complete navigation tasks, and achieve Fast and accurate obstacle avoidance work.

SUMMARY OF THE INVENTION

The purpose of the present invention is to overcome the deficiencies of the prior art. In order to achieve the above purpose, an off-road environment navigation method based on multi-sensor fusion is adopted to solve the problems raised in the above background technology.

An off-road environment navigation method based on multi-sensor fusion, the specific steps include:

Step S1, establishing a global road network based on a high-resolution satellite map, using the Astar algorithm to carry out global path planning according to the coordinate information of the task file, and obtaining a global reference path;

Step S2, using vehicle-mounted equipment to collect vehicle-mounted multi-sensor data for data fusion, input the fused multi-modal data into the SLAM algorithm, add multiple sensor constraints, and use a graph-based optimization framework to solve to obtain accurate poses;

Step S3, splicing multiple environmental observations using the obtained precise pose, and constructing an off-road terrain map and a global occupancy grid map based on a filtering fusion algorithm;

Step S4, extracting the current drivable area of the vehicle in real time according to the obtained global reference path;

Step S5: In the current drivable area, the RRT path planning algorithm based on multi-attribute evaluation is used to generate a local optimal path for obstacle avoidance navigation.

As a further scheme of the present invention: the specific steps of the step S1 include:

First, the mission area of the autonomous unmanned vehicle is preset, and the mission map corresponding to the mission area is obtained according to the image based on the high-resolution satellite map;

According to the obtained task map, select the GPS coordinates of the reference point, and extract all the road information in the task map to establish a global road network;

According to the coordinates of the starting point, the task point, and the ending point in the navigation task file, use the Astar algorithm to plan the global path, and obtain the node topology information from the starting point to the ending point in the global road network;

Then, the initial global path is spliced according to the topological relationship, and the Bezier curve fitting algorithm is used to smooth the initial global path to obtain the global reference path in the task area.

As a further solution of the present invention: in the step S2, the specific steps of using the vehicle-mounted device to collect the vehicle-mounted multi-sensor data for data fusion include:

The multi-modal data collected and read in real time by the vehicle-mounted industrial computer, the multi-modal data includes 3D point cloud data obtained by collecting surrounding environment information by 3D lidar, high-frequency linear acceleration and angular velocity of the vehicle collected by IMU, The speed of the vehicle collected by the odometer, and the real-time latitude and longitude coordinates of the current vehicle collected by GPS;

Preprocessing the collected multimodal data, including: calculating the curvature of each laser point separately, assigning the feature information of the line or surface of the laser point according to the curvature, and clustering the point cloud to eliminate discrete points; The acceleration and angular velocity of the vehicle and the vehicle speed data collected by the wheel odometer are used to construct pre-integration observations using batch processing; the real-time latitude and longitude coordinates are converted into spatial position coordinates;

The preprocessed multimodal data is fused.

As a further solution of the present invention: in the step S2, the fused multi-modal data is input into the SLAM algorithm, and multiple sensor constraints are added, and the specific steps of using a graph optimization-based framework to solve to obtain an accurate pose include:

Construct lidar point cloud residuals, IMU and wheeled odometry pre-integration residuals, GPS prior residuals, and establish nonlinear least squares problem based on the obtained multimodal data;

Then, it is solved through the framework of graph optimization to obtain the precise vehicle pose. Finally, the point cloud is spliced according to the vehicle pose to obtain the environment point cloud map.

As a further scheme of the present invention: the specific steps of the step S3 include:

Obtain a single frame of lidar point cloud data and mark obstacles according to the slope, obtain passable area data in a single frame raster map, use Gaussian process to infer unobservable point cloud data, and finally pass the single frame point cloud observation through The binary Bayesian filter is updated to the global occupancy grid map;

Based on the elevation data of the single frame elevation raster map marked with the maximum height, the global elevation raster map is updated using Kalman filtering.

As a further scheme of the present invention: the specific steps of the step S4 include:

The vehicle is driven according to the obtained global reference path, and the passable road area is detected according to the point cloud data, and the discontinuous road surface is discarded according to the distance continuity index and the angle continuity index;

The distance continuity index c _d is:

；

;

In the formula, n is the number of points that need to be calculated before and after the current iteration point of the index, and the coordinates of k from xn to x+n indicate that the index considers the current point p _x the first n points and the last n points, p _k is the kth point, p _{k+ 1} is the k+ 1th point;

The angular continuity index c _a is:

；

;

In the formula, n is the number of points that need to be calculated before and after the current iteration point of the indicator, p _x indicates the point at the current iteration position, p _xk indicates the k -th point before the current iteration position, and p _x+k indicates the back of the current iteration position the kth point;

When c _d is less than the distance continuity index threshold c ^th _d , and c _a is less than the angle continuity index threshold c ^th _a , the ground points are continuous, and the ground points are included in the drivable area, and the road points extracted at the same time The area of is denoted as the drivable area Ω ₁ ;

Since the radar cannot directly detect all the drivable roads, the real-time 3D point cloud around the vehicle is firstly clustered using the Euclidean distance method to obtain the obstacle clusters around the vehicle; the obstacle clusters are used as barriers, along the global reference path In the vertical direction, the flood filling algorithm is used to spread the front area to both sides to form the drivable area Ω ₂ of the vehicle;

Finally, the union of the two drivable areas Ω ₁ ∪ Ω ₂ is calculated as the final drivable area.

As a further scheme of the present invention: the specific steps of the step S5 include:

According to the obtained drivable area Ω as the feasible area Φ of path planning, the longitudinal length L _Φ of the feasible area Φ in the direction of the global reference path is calculated, and then according to the length L _Ω of the drivable area Ω and the minimum planning distance L _{p, min} selects the preview length L _p of path planning, where L _{p , min} ≤ L _p < L _Ω ;

According to the preview length L _p of the path planning, find the corresponding global path coordinate point in the global reference path, take the coordinate point as the preview point of the path planning, and explore the surroundings with the preview point as the center of the circle. the largest circle C _p,goal in the feasible region Φ ;

Initialize the growth step ρ of the random tree T , take the center of the rear axle of the vehicle as the initial node q _start of the random tree T , randomly select N coordinate points in the circle C _p,goal as the target node q _goal of the random tree, and calculate get a collision-free path;

Based on the random tree corresponding to each path, the node of the random tree is used as the control point of the Bezier curve, and the Bezier curve is used for smoothing to obtain a smooth path; the curve fitting algorithm is used to smooth the path, which is effective Optimized the quality of paths.

For each candidate path, by using a multi-attribute evaluation function and setting reasonable weights, the path with the least cost is selected as the current optimal path, wherein the multi-attribute evaluation function includes the cost of the lateral offset, the curvature of the candidate path cost, and the distance cost of the candidate path to the obstacle.

As a further scheme of the present invention: the specific steps of calculating the target node q _goal of the random tree to obtain a collision-free path include:

For each goal node qi _goal , randomly sample the random node q ⁱ _rand in the planning feasible region Φ with a specific probability, select the nearest node q ^{i near of the random node q i} _rand , ^calculate _the ^parent node of the nearest node q ⁱ _near and The angle θ ⁱ _near formed between the nearest node q ⁱ _near and the angle θ ⁱ _randnear formed between the nearest node q ⁱ _near and the random node q ⁱ _rand ; the RRT takes the connection angle between nodes into consideration when generating new nodes, so that The path generated by RRT is more gradual.

Determine whether | θ ⁱ _near - θ ⁱ _randnear | is less than the node angle threshold θ ^th , if it is greater than θ ^th , find another nearest node until it is less than the node angle threshold θ ^th , then the calculation method of the new node q ⁱ _new is:

；

;

If there is no obstacle in the connection between q ⁱ _near and q ⁱ _new , take q ⁱ _near as the parent node of q ⁱ _new , and q ⁱ _new as the child node of q ⁱ _near , and then continue to randomly sample and expand the random tree T ; Otherwise, ^discard the current new node qi _new and re-sample randomly;

Cyclic search, when the new node q ⁱ _new of the random tree T expands and reaches the vicinity of the target point q ⁱ _goal , from the target node q ⁱ _goal through the corresponding parent node back to the root node q _start , a connection q _start to The _collision -free path of the qi ^goal .

As a further solution of the present invention: the specific steps of setting reasonable weights by adopting the multi-attribute evaluation function include:

The cost Q _d of the lateral offset is:

；

;

The cost Q _k of the candidate path curvature is:

；

;

The distance cost Q _obs of the candidate path to the obstacle is:

；

;

；

;

The total cost is:

J _cost = ω ₁ Q _d + ω ₂ Q _k + ω ₃ Q _obs ;

where d _i is the lateral offset of the discrete candidate path point relative to the reference path _, ki is the curvature value of the discrete candidate path point, l _mix and l _max are the minimum and maximum lateral safety distances of obstacles relative to the candidate path, respectively , l _obs,i is the lateral distance of the obstacle i relative to the candidate path, d _obs,i is the distance cost of the obstacle i relative to the candidate path, ω ₁ , ω ₂ , ω ₃ are the lateral offset cost and the candidate path, respectively The curvature cost, and the weight parameter of the distance cost of the candidate path to the obstacle.

Compared with the prior art, the present invention has the following technical effects:

Using the above technical solutions, by extracting sensor data such as IMU, wheel odometer, GPS, lidar, etc., and adding constraints, the autonomous vehicle can obtain accurate and robust positioning in complex off-road environments, adapt to a variety of scene; using a two-dimensional grid map construction algorithm, the point cloud observation of a single frame is updated to the global map through Bayesian filtering, which significantly reduces the perception blind area and improves the map accuracy; using the improved RRT algorithm is only used in planning Sampling in the drivable area simplifies the sampling space of the RRT and greatly improves the sampling efficiency.

Description of drawings

Below in conjunction with the accompanying drawings, the specific embodiments of the present invention are described in detail:

FIG. 1 is a flowchart of an off-road environment navigation method according to some embodiments disclosed in the present application;

FIG. 2 is a block diagram of the system structure of multi-sensor fusion for SLAM and navigation according to some embodiments disclosed in the present application;

FIG. 3 is a result diagram of global path planning according to some embodiments disclosed in the present application;

4 is an environmental point cloud map constructed when performing SLAM according to some embodiments disclosed in the present application;

5 is a constructed global occupancy grid map of some embodiments disclosed in the present application;

6 is a constructed global elevation grid map of some embodiments disclosed in the present application;

FIG. 7 is a schematic diagram of the drivable area of the extracted current position of the vehicle according to some embodiments disclosed in the present application;

8 is a flowchart of an RRT path planning algorithm of some embodiments disclosed in the present application;

FIG. 9 is a schematic diagram of RRT node expansion according to some embodiments disclosed in the present application;

FIG. 10 is a schematic diagram of real-time path planning in an off-road environment according to some embodiments disclosed in this application.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

Referring to FIG. 1 , in an embodiment of the present invention, a method for navigating an off-road environment based on multi-sensor fusion, the specific steps include:

As shown in FIG. 2 , it is a block diagram of the system structure of the multi-sensor fusion for SLAM and navigation according to the present invention;

Step S1, establishing a global road network based on a high-resolution satellite map, using the Astar algorithm to perform global path planning according to the coordinate information of the task file, and obtaining a global reference path, the specific steps include:

First, the mission area of the autonomous unmanned vehicle is preset, and the mission map corresponding to the mission area is obtained according to the image based on the high-resolution satellite map;

According to the obtained task map, select the GPS coordinates of the reference point, and extract all the road information in the task map to establish a global road network;

According to the coordinates of the starting point, the task point, and the ending point in the navigation task file, use the Astar algorithm to plan the global path, and obtain the node topology information from the starting point to the ending point in the global road network;

Then stitch the initial global path according to the topological relationship, and use the Bezier curve fitting algorithm to smooth the initial global path to obtain the global reference path in the task area, as shown in Figure 3, which is the result of global path planning .

Step S2, using vehicle-mounted equipment to collect vehicle-mounted multi-sensor data for data fusion, input the fused multi-modal data into the SLAM algorithm, add multiple sensor constraints, and use a graph-based optimization framework to solve to obtain accurate poses;

In the specific implementation manner, in step S2, the specific steps of using the vehicle-mounted device to collect vehicle-mounted multi-sensor data for data fusion include:

Firstly, the multi-modal data is collected and read in real time through the vehicle-mounted industrial computer. The collection method of the multi-modal data specifically includes 3D point cloud data obtained by collecting surrounding environment information through 3D lidar, and collecting high-frequency linear acceleration of the vehicle through IMU. and angular velocity, the vehicle speed collected by the wheel odometer, and the real-time latitude and longitude coordinates of the current vehicle collected by GPS;

The collected multimodal data is preprocessed, and the preprocessing methods include:

3D point cloud data of 3D lidar: Calculate the curvature of each laser point separately, assign the feature information of the line or surface of the laser point according to the curvature, and cluster the point cloud to eliminate discrete points;

High-frequency linear acceleration and angular velocity of IMU, vehicle velocity of wheel odometer: build pre-integrated observations through batch processing;

GPS real-time latitude and longitude coordinates: convert latitude and longitude coordinates into spatial position coordinates;

The preprocessed multimodal data is fused and used as the input of the subsequent SLAM algorithm.

In the specific embodiment, in the step S2, the fused multi-modal data is input into the SLAM algorithm, and multiple sensor constraints are added, and the specific steps of using the graph optimization-based framework to solve and obtain the precise pose include:

Construct lidar point cloud residuals, IMU and wheeled odometry pre-integration residuals, GPS prior residuals, and establish nonlinear least squares problem based on the obtained multimodal data;

Then solve the problem through the graph optimization framework to obtain the precise vehicle pose, and finally stitch the point clouds according to the vehicle pose to obtain the environment point cloud map, as shown in Figure 4, which shows the environment point cloud constructed during SLAM. map. A and B marked by the rectangular box in the figure are the rod-shaped markers on the roadside, and A and B in the uppermost figure are the effects of the rod-shaped markers in the global point cloud image. In the four pictures below, the two pictures on the right are the shapes of rod-shaped markers A and B in the vehicle camera, and the left is the corresponding effect in the 3D point cloud.

Step S3, using the obtained precise poses to splicing multiple environmental observations, and constructing an off-road terrain map and a global occupancy grid map based on a filtering fusion algorithm, the specific steps include:

As shown in Figure 5, the diagram shows the constructed global occupancy grid map.

Obtain a single frame of lidar point cloud data and mark obstacles according to the slope, obtain passable area data in a single frame raster map, use Gaussian process to infer unobservable point cloud data, and finally pass the single frame point cloud observation through The binary Bayesian filter is updated to the global occupancy grid map;

As shown in Figure 6, the figure shows the constructed global elevation grid map.

Based on the elevation data of the single frame elevation raster map marked with the maximum height, the global elevation raster map is updated using Kalman filtering.

Step S4, extracting the current drivable area of the vehicle in real time according to the obtained global reference path, and the specific steps include:

The vehicle is driven according to the forward direction along the obtained global reference path, and the passable road surface area is detected according to the point cloud data, and the discontinuous road surface is discarded according to the distance continuity index _cd and the angle continuity index c a _;

The distance continuity index c _d is:

；

;

In the formula, n is the number of points that need to be calculated before and after the current iteration point of the index, and the coordinates of k from xn to x+n indicate that the index considers the current point p _x the first n points and the last n points, p _k is the kth point, p _{k+ 1} is the k+ 1th point;

The angular continuity index c _a is:

；

;

In the formula, n is the number of points that need to be calculated before and after the current iteration point of the indicator, p _x indicates the point at the current iteration position, p _xk indicates the k -th point before the current iteration position, and p _x+k indicates the back of the current iteration position the kth point;

Specifically, when c _d is less than the distance continuity index threshold c ^th _d , and c _a is less than the angle continuity index threshold c ^th _a , then the ground point is continuous, the ground point is included in the drivable area, and the extraction The area of the road surface point is recorded as the drivable area Ω ₁ ;

Considering that due to the occlusion of obstacles, etc., the radar cannot directly detect all the drivable roads. In view of this situation, first use the Euclidean distance method to cluster the real-time 3D point cloud around the vehicle to obtain the obstacles around the vehicle. cluster;

Taking the obstacle cluster as the barrier, along the vertical direction of the global reference path, the flood filling algorithm is used to spread the front area to both sides to form the drivable area Ω ₂ of the vehicle;

Finally, the union of the two drivable areas Ω ₁ ∪ Ω ₂ is calculated as the final drivable area, as shown in FIG.

Step S5, in the current drivable area, use the RRT path planning algorithm based on multi-attribute evaluation to generate a local optimal path for obstacle avoidance navigation, and the specific steps include:

First, according to the drivable area Ω obtained in step S4 as the feasible region Φ of path planning, the longitudinal length L _Φ of the feasible region Φ in the direction of the global reference path is calculated, and then according to the length L _Ω of the drivable area Ω and the minimum The planning distance L _p,min selects the preview length L _p of the path planning, where L _p satisfies L _p,min ≤ L _p < L _Ω ;

According to the preview length L _p of the path planning, find the corresponding global path coordinate point in the global reference path, take the global path coordinate point as the preview point of the path planning, and use the preview point as the center of the circle to explore around, and the calculation is obtained. The largest circle C _p,goal in the planning feasible region Φ ;

As shown in Figure 8, which is a flow chart of the RRT path planning algorithm, first initialize the growth step ρ of the random tree T , and take the center of the rear axle of the vehicle as the initial node q _start of the random tree T , in the circle C _p, Randomly select N coordinate points in the _goal as the target node q _goal of the random tree, and calculate to obtain a collision-free path;

The specific calculation steps are:

As shown in FIG. 9 , a schematic diagram of RRT node expansion is shown. Specifically, when RRT is used to generate a new node, the connection angle between nodes is considered, so that the path generated by RRT is smoother.

For each goal node qi _goal , randomly sample the random node q ⁱ _rand in the planning feasible region Φ with a specific probability, select the nearest node q ^{i near of the random node q i} _rand , ^calculate _the ^parent node of the nearest node q ⁱ _near and The angle θ ⁱ _near formed between the nearest node q ⁱ _near and the angle θ ⁱ _randnear formed between the nearest node q ⁱ _near and the random node q ⁱ _rand ;

Determine whether | θ ⁱ _near - θ ⁱ _randnear | is less than the node angle threshold θ ^th , if it is greater than θ ^th , find another nearest node until it is less than the node angle threshold θ ^th , then the calculation method of the new node q ⁱ _new is:

；

;

If there is no obstacle in the connection between q ⁱ _near and q ⁱ _new , take q ⁱ _near as the parent node of q ⁱ _new , and q ⁱ _new as the child node of q ⁱ _near , and then continue to randomly sample and expand the random tree T ; Otherwise, ^discard the current new node qi _new and re-sample randomly;

Cyclic search, when the new node q ⁱ _new of the random tree T expands and reaches the vicinity of the target point q ⁱ _goal , from the target node q ⁱ _goal through the corresponding parent node back to the root node q _start , a connection q _start to The _collision -free path of the qi ^goal .

Based on the random tree corresponding to each path, the node of the random tree is used as the control point of the Bezier curve, and the Bezier curve is used for smoothing to obtain a smooth path, in which N paths form a candidate path cluster; use The curve fitting algorithm smoothes the path and effectively optimizes the quality of the path.

For each candidate path, by using a multi-attribute evaluation function and setting reasonable weights, the path with the least cost is selected as the current optimal path, wherein the multi-attribute evaluation function includes the cost of the lateral offset, the curvature of the candidate path cost, and the distance cost of the candidate path to the obstacle.

As shown in FIG. 10 , it is a schematic diagram of real-time path planning in an off-road environment in this embodiment.

Specifically, the specific steps for setting reasonable weights by adopting a multi-attribute evaluation function include:

The cost Q _d of the lateral offset is:

；

;

Q _d is the cost of lateral offset, representing the degree of deviation of the candidate path relative to the reference path;

The cost Q _k of the candidate path curvature is:

；

;

Q _k is the cost of the candidate path curvature, which is used to select the path with the smallest average curvature;

The distance cost Q _obs of the candidate path to the obstacle is:

；

;

；

;

The total cost is:

J _cost = ω ₁ Q _d + ω ₂ Q _k + ω ₃ Q _obs ;

where d _i is the lateral offset of the discrete candidate path point relative to the reference path _, ki is the curvature value of the discrete candidate path point, l _mix and l _max are the minimum and maximum lateral safety distances of obstacles relative to the candidate path, respectively , l _obs,i is the lateral distance of the obstacle i relative to the candidate path, d _obs,i is the distance cost of the obstacle i relative to the candidate path, ω ₁ , ω ₂ , ω ₃ are the lateral offset cost and the candidate path, respectively The curvature cost, and the weight parameter of the distance cost of the candidate path to the obstacle.

Although embodiments of the present invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, and substitutions can be made in these embodiments without departing from the principle and spirit of the invention and modifications, the scope of the present invention is defined by the appended claims and their equivalents, and should be included within the protection scope of the present invention.

Claims (8)

1. an off-road environment navigation method based on multi-sensor fusion, is characterized in that, concrete steps comprise: Step S1, establishing a global road network based on a high-resolution satellite map, using the Astar algorithm to carry out global path planning according to the coordinate information of the task file, and obtaining a global reference path; Step S2, using vehicle-mounted equipment to collect vehicle-mounted multi-sensor data for data fusion, input the fused multi-modal data into the SLAM algorithm, add multiple sensor constraints, and use a graph-based optimization framework to solve to obtain accurate poses; Step S3, splicing multiple environmental observations using the obtained precise pose, and constructing an off-road terrain map and a global occupancy grid map based on a filtering fusion algorithm; Step S4, extracting the current drivable area of the vehicle in real time according to the obtained global reference path, and the specific steps include: The vehicle is driven according to the obtained global reference path, and the passable road area is detected according to the point cloud data, and the discontinuous road surface is discarded according to the distance continuity index and the angle continuity index; The distance continuity index c _d is:

; In the formula, n is the number of points that need to be calculated before and after the current iteration point of the index, and the coordinates of k from xn to x+n indicate that the index considers the current point p _x the first n points and the last n points, p _k is the kth point, p _{k+ 1} is the k+ 1th point; The angular continuity index c _a is:

; In the formula, n is the number of points that need to be calculated before and after the current iteration point of the indicator, p _x indicates the point at the current iteration position, p _xk indicates the k -th point before the current iteration position, and p _x+k indicates the back of the current iteration position the kth point; When c _d is less than the distance continuity index threshold c ^th _d , and c _a is less than the angle continuity index threshold c ^th _a , the ground points are continuous, and the ground points are included in the drivable area, and the road points extracted at the same time The area of is denoted as the drivable area Ω ₁ ; Since the radar cannot directly detect all the drivable roads, the real-time 3D point cloud around the vehicle is firstly clustered using the Euclidean distance method to obtain the obstacle clusters around the vehicle; the obstacle clusters are used as barriers, along the global reference path In the vertical direction, the flood filling algorithm is used to spread the front area to both sides to form the drivable area Ω ₂ of the vehicle; Finally, calculate the union of the two drivable areas Ω ₁ ∪ Ω ₂ as the final drivable area; Step S5: In the current drivable area, the RRT path planning algorithm based on multi-attribute evaluation is used to generate a local optimal path for obstacle avoidance navigation. 2. The method for off-road environment navigation based on multi-sensor fusion according to claim 1, wherein the specific steps of the step S1 comprise: First, the mission area of the autonomous unmanned vehicle is preset, and the mission map corresponding to the mission area is obtained according to the image based on the high-resolution satellite map; According to the obtained task map, select the GPS coordinates of the reference point, and extract all the road information in the task map to establish a global road network; According to the coordinates of the starting point, the task point, and the ending point in the navigation task file, use the Astar algorithm to plan the global path, and obtain the node topology information from the starting point to the ending point in the global road network; Then, the initial global path is spliced according to the topological relationship, and the Bezier curve fitting algorithm is used to smooth the initial global path to obtain the global reference path in the task area. 3. a kind of off-road environment navigation method based on multi-sensor fusion according to claim 1, is characterized in that, in described step S2, utilize vehicle-mounted equipment to collect vehicle-mounted multi-sensor data and carry out the concrete steps of data fusion comprising: The multi-modal data collected and read in real time by the vehicle-mounted industrial computer, the multi-modal data includes 3D point cloud data obtained by collecting surrounding environment information by 3D lidar, high-frequency linear acceleration and angular velocity of the vehicle collected by IMU, The speed of the vehicle collected by the odometer, and the real-time latitude and longitude coordinates of the current vehicle collected by GPS; Preprocessing the collected multimodal data, including: calculating the curvature of each laser point separately, assigning the feature information of the line or surface of the laser point according to the curvature, and clustering the point cloud to eliminate discrete points; The acceleration and angular velocity of the vehicle and the vehicle speed data collected by the wheel odometer are used to construct pre-integration observations using batch processing; the real-time latitude and longitude coordinates are converted into spatial position coordinates; The preprocessed multimodal data is fused. 4. A kind of off-road environment navigation method based on multi-sensor fusion according to claim 1, it is characterized in that, in described step S2, the multimodal data of fusion is input in SLAM algorithm, and add a variety of sensor constraints, use based on SLAM algorithm. The specific steps for obtaining accurate poses by solving the graph optimization framework include: Construct lidar point cloud residuals, IMU and wheeled odometry pre-integration residuals, GPS prior residuals, and establish nonlinear least squares problem based on the obtained multimodal data; Then, it is solved through the framework of graph optimization to obtain the precise vehicle pose. Finally, the point cloud is spliced according to the vehicle pose to obtain the environment point cloud map. 5. The method for off-road environment navigation based on multi-sensor fusion according to claim 1, wherein the specific steps of step S3 comprise: Obtain a single frame of lidar point cloud data and mark obstacles according to the slope, obtain passable area data in a single frame raster map, use Gaussian process to infer unobservable point cloud data, and finally pass the single frame point cloud observation through The binary Bayesian filter is updated to the global occupancy grid map; Based on the elevation data of the single frame elevation raster map marked with the maximum height, the global elevation raster map is updated using Kalman filtering. 6. The method for off-road environment navigation based on multi-sensor fusion according to claim 1, wherein the specific steps of step S5 comprise: According to the obtained drivable area Ω as the feasible area Φ of path planning, the longitudinal length L _Φ of the feasible area Φ in the direction of the global reference path is calculated, and then according to the length L _Ω of the drivable area Ω and the minimum planning distance L _{p, min} selects the preview length L _p of path planning, where L _{p , min} ≤ L _p < L _Ω ; According to the preview length L _p of the path planning, find the corresponding global path coordinate point in the global reference path, take the coordinate point as the preview point of the path planning, and explore the surroundings with the preview point as the center of the circle. the largest circle C _p,goal in the feasible region Φ ; Initialize the growth step ρ of the random tree T , take the center of the rear axle of the vehicle as the initial node q _start of the random tree T , randomly select N coordinate points in the circle C _p,goal as the target node q _goal of the random tree, and calculate get a collision-free path; Based on the random tree corresponding to each path, the node of the random tree is used as the control point of the Bezier curve, and the Bezier curve is used for smoothing to obtain a smooth path; For each candidate path, by using a multi-attribute evaluation function and setting reasonable weights, the path with the least cost is selected as the current optimal path, wherein the multi-attribute evaluation function includes the cost of the lateral offset, the curvature of the candidate path cost, and the distance cost of the candidate path to the obstacle. 7. a kind of off-road environment navigation method based on multi-sensor fusion according to claim 6, is characterized in that, the target node q _goal of described random tree, the concrete step that calculates and obtains a collision-free path comprises: For each goal node qi _goal , randomly sample the random node q ⁱ _rand in the planning feasible region Φ with a specific probability, select the nearest node q ^{i near of the random node q i} _rand , ^calculate _the ^parent node of the nearest node q ⁱ _near and The angle θ ⁱ _near formed between the nearest node q ⁱ _near and the angle θ ⁱ _randnear formed between the nearest node q ⁱ _near and the random node q ⁱ _rand ; Determine whether | θ ⁱ _near - θ ⁱ _randnear | is less than the node angle threshold θ ^th , if it is greater than θ ^th , find another nearest node until it is less than the node angle threshold θ ^th , then the calculation method of the new node q ⁱ _new is:

; If there is no obstacle in the connection between q ⁱ _near and q ⁱ _new , take q ⁱ _near as the parent node of q ⁱ _new , and q ⁱ _new as the child node of q ⁱ _near , and then continue to randomly sample and expand the random tree T ; Otherwise, ^discard the current new node qi _new and re-sample randomly; Cyclic search, when the new node q ⁱ _new of the random tree T expands and reaches the vicinity of the target point q ⁱ _goal , from the target node q ⁱ _goal through the corresponding parent node back to the root node q _start , a connection q _start to The _collision -free path of the qi ^goal . 8 . The method for off-road environment navigation based on multi-sensor fusion according to claim 6 , wherein the specific steps of using a multi-attribute evaluation function and setting reasonable weights include: 9 . The cost Q _d of the lateral offset is:

; The cost Q _k of the candidate path curvature is:

; The distance cost Q _obs of the candidate path to the obstacle is:

;

; The total cost is: J _cost = ω ₁ Q _d + ω ₂ Q _k + ω ₃ Q _obs ; Among them, d _i is the lateral offset of the discrete candidate path point relative to the reference path, K _max is the threshold value of the curvature of the discrete track point, k _i is the curvature value of the discrete candidate path point, l _min and l _max are the relative obstacles of the obstacle, respectively. The minimum and maximum lateral safety distance of the candidate path, l _obs,i is the lateral distance of obstacle i relative to the candidate path, d _obs,i is the distance cost of obstacle i relative to the candidate path, ω ₁ , ω ₂ , ω ₃ are the weight parameters of the lateral offset cost, the candidate path curvature cost, and the distance cost of the candidate path to the obstacle, respectively.

Images