CN109808509B - An automatic identification and control system and method for an unmanned off-road vehicle crossing a trench - Google Patents

Abstract

The invention provides an automatic identification and control system for an unmanned off-road vehicle crossing a trench, which calculates the angle between the center line of the trench and the longitudinal axis of the vehicle through image processing and information fusion methods according to information on obstacles around the vehicle and combined with the vehicle posture information The judging module judges whether the vehicle can cross the ditch according to the size information of the ditch, the angle between the center line of the ditch and the longitudinal axis of the vehicle, and formulates a crossing strategy; when the judging module determines that the vehicle can cross the ditch, the path planning module according to the formulated crossing strategy , plan the path across the trench, determine the path point and the speed of the path point; the path tracking module controls the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined path point and path point speed, and controls the vehicle to automatically follow the predetermined path across the trench. across the trench. The invention judges whether there is a moat through image processing and information fusion technology, and sets a corresponding crossing strategy to ensure that the vehicle automatically crosses the moat within a certain width range.

Description

An automatic identification and control system and method for an unmanned off-road vehicle crossing a trench

technical field

The invention belongs to the technical field of unmanned off-road vehicles, and in particular relates to an automatic identification and control system and method of an unmanned off-road vehicle for crossing trenches.

Background technique

The development of computer technology and sensor technology has promoted the research of unmanned vehicles and the realization of unmanned systems. Various assisted driving functions based on unmanned hardware platforms have approached people's daily life and have gradually become a high priority for new automotive products. The technological configuration provides humans with a more convenient, efficient and safe driving experience. High-performance off-road vehicles have been widely used in resource exploration, jungle search and rescue, military operations, disaster search and rescue and other fields, but the field terrain is complex, there are many types of obstacles, and the adhesion conditions are poor, and there are various complex comprehensive off-road conditions. In terms of working conditions, under the control of the driver, traditional off-road vehicles often cannot reasonably and accurately control the operation of the vehicle because they cannot fully understand the off-road working conditions, evaluate the ability of the vehicle to cross obstacles, and perceive the running state of the vehicle. It limits the extreme off-road capability of the vehicle, and at the same time it is difficult to ensure the safety of the off-road process, and accidents that endanger personal and property safety are prone to occur. Various works are carried out. When driving in the wild, the vehicle often encounters negative obstacles such as trenches. The driver can only rely on the naked eye to observe the width of the trench, the location of surrounding obstacles, and the relative position of the vehicle and the trench. According to personal experience, the driver can choose the way to pass or make multiple attempts. , the correctness of decision-making and the feasibility of operation are relatively low, and the operation error is large, the efficiency is extremely low, and it is easy to cause accidents.

The patent "A Control Method of Unmanned All-Terrain Vehicle Based on Preview" proposes a control method of unmanned all-terrain vehicle based on preview. The all-terrain vehicle can make strategies and preparations for passing obstacles in advance and change the speed of the vehicle in advance , driving direction and adjust the hardness of the shock absorber. The paper "Analysis and Simulation Research of Submarine Mining Vehicle's Trench Obstacle Passability" is to design a seabed mining vehicle connected by four sets of composite wheel set mechanisms and an articulated sealed and pressure-resistant integral tank frame, which consists of six sets of wheels. , when crossing the trench, the first three wheels are rigidly connected and transformed into one wheel, thus crossing the trench. However, these two methods have strict requirements on the structure of the vehicle for trench-type negative obstacles. The first method uses the method of swinging over the legs, and the second method requires six sets of tires. The above-mentioned patents and documents do not refer to the strategy of crossing a trench with a vehicle driven by a four-wheeled motor in a distributed manner, nor to the detailed control method and steps of how to control the four-wheeled vehicle to cross a trench. At present, there is almost no research on the control method of fully automatic four-wheeled vehicles crossing trenches.

SUMMARY OF THE INVENTION

The technical problem to be solved by the present invention is to provide an automatic identification and control method for an unmanned off-road vehicle crossing a trench.

The technical solution adopted by the present invention to solve the above-mentioned technical problems is: an automatic identification and control system for an unmanned off-road vehicle crossing a trench, which is characterized in that: it is arranged in a four-wheel distributed drive electric vehicle, and it includes:

The environmental perception unit is used to obtain the information of obstacles around the vehicle. Through image processing and information fusion methods, the edge lines of the obstacles within the perceptible range of the vehicle are obtained, and the size information of the trench is identified and calculated; at the same time, the information of the vehicle motion status is obtained, including Pose information, combined with trench information to calculate the angle between the center line of the trench and the longitudinal axis of the vehicle;

The decision-making unit includes a judgment module, a path planning module and a path tracking module; wherein, the judgment module is used to judge whether the vehicle can cross according to the size information of the trench, the angle between the center line of the trench and the longitudinal axis of the vehicle, and combined with the size information of the vehicle itself moat, and formulate a crossing strategy; the path planning module is used to formulate a path across the moat and determine the path point and the speed of the path point according to the formulated crossing strategy and the performance parameters of the vehicle when the judgment module determines that the vehicle can cross the moat; the path tracking module , which is used to control the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined waypoint and the speed of the waypoint, so as to control the vehicle to automatically cross the moat according to the predetermined path of crossing the moat.

According to the above scheme, the environment perception unit, when it is detected that there is a horizontally elongated negative obstacle on the current path, and the depth D of the negative obstacle is greater than or equal to the wheel rolling radius r, it is judged to encounter a trench; calculate the trench width W, the angle β between the center line of the trench and the longitudinal axis of the vehicle, and the distance s from the vehicle to the edge of the trench.

According to the above scheme, the judging module specifically judges that the width W of the moat is smaller than the maximum width of the moat that the vehicle can cross, W _max , then determines that the vehicle can cross the moat, and if the width W of the moat is greater than or equal to the maximum width W _max that the vehicle can cross, then the vehicle is judged to be able to cross the moat. The current trench cannot be crossed, and the planned path bypasses the trench; W _max is calculated according to the following formula:

where B is the wheelbase of the vehicle, and L is the wheelbase of the vehicle.

According to the above scheme, the judgment module specifically formulates the leaping strategy according to the following methods:

When W < 2r, the vertical crossing strategy is adopted, that is, the relative position of the vehicle and the moat is adjusted so that the longitudinal axis of the vehicle is almost perpendicular to the local center line of the moat, and then it goes straight across the moat. If the number of environmental obstacles is greater than the threshold, the diagonal crossing strategy ; r is the wheel rolling radius;

When W≥2r, the oblique span strategy is adopted;

The oblique-span strategy described includes three stages:

In the first stage, start to cross the trench, keep the β angle and drive forward from side A to side B of the trench. One front wheel w1 crosses the trench first. At this time, it is necessary to ensure that when w1 does not touch the side B of the trench, the other front wheel w2 The A side edge of the moat shall not sink into the moat, guarantee β∈(arctan(B/L), arccos(W/B)];

In the second stage, w1 has already crossed the moat and landed on the B side, and w2 started to cross the moat. At this time, it is necessary to ensure that when w2 does not touch the B side of the moat, the rear wheel w3 on the diagonal line of the vehicle with w2 is located on the edge of the moat A side and must not sink into the moat. , guarantee β∈[arctan(B/L)+arcsin(W/sqrt(B ² +L ² )), 90°];

In the third stage, w2 has crossed the moat and landed on the B side, and w3 started to cross the moat. At this time, it is necessary to ensure that when w3 does not touch the B side of the moat, the rear wheel w4 on the diagonal line with w1 is located on the edge of the moat A side and must not sink into the moat. , the working conditions of the third stage are the same as the working conditions of the first stage, but the relative driving direction of the vehicle is opposite, so β satisfies the results of the first stage;

Therefore, if β _per is not an empty set and _β∈βper , the planned path keeps this angle straight and diagonally across the trench; otherwise, the planned path adjusts the included angle β to satisfy _β∈βper , and then the planned path keeps this angle straight and diagonally across moat;

If β _per is an empty set, plan a large-angle steering maneuver on the path, adjust the relative position of the vehicle and the trench in real time so that the β angle meets the requirements of each stage in the process of crossing the trench, and cross the trench through multiple steering maneuvers.

According to the above scheme, the path planning module performs path planning under corresponding operating conditions and strategies according to the result of the judgment module:

If the vertical crossing strategy is adopted, the planned path is maneuvered in front of the trench to adjust the beta angle to 90°±3°, and the environment perception unit detects that the beta angle is within the range of 90°±3° and starts to go straight through the trench;

When adopting the diagonal-span strategy, if the straight-line diagonal-span method is adopted, the planned path adjusts the included angle β to make it satisfy _β∈βper , and then the planned path maintains this angle to go straight to the diagonal-span trench; , then the planned path is turned to maneuver at a large angle, and the relative position of the vehicle and the trench is adjusted so that the β angle meets the requirements of each stage in the process of crossing the trench, and the crossing is gradually completed;

Meanwhile, the path planning module adopts the Reeds-Shepp curve to make the path shortest and satisfy the Ackerman steering model.

According to the above scheme, the specific planning steps of the path planning module are as follows:

(1) Judging the adopted spanning strategy and spanning method, and then select the critical path point, the critical path point includes the starting path point, the stage split point and the end point;

When the vertical leaping strategy is adopted, the current position is selected as the starting path point, the estimated point of any front wheel overhang is the stage split point 1, the end point of any rear wheel overhang is the stage split point 2, and the position 3m away from the trench in the current route angle direction is the end point; The obstacle avoidance path point is selected by the algorithm according to the actual obstacle situation;

When adopting the oblique spanning strategy, the starting and ending path points and obstacle avoidance path points are selected as above; when using the straight-line spanning method, the first front wheel suspension end point is selected as the stage split point 1, and the second front wheel suspension end point is selected as the stage split point 2 , the end point of the first rear wheel suspension is the stage split point 3; the step-by-step steering and spanning method is adopted, and the first front wheel suspension end point is selected as the stage split point 1, and the first rear wheel suspension start point is the stage split point 2 , the allowable passing point is stage split point 3, and the end point of the first rear wheel overhang is stage split point 4; among them, the estimated point of the front wheel overhang is the path point where the front wheel reaches the edge of the moat, and the end point of the rear wheel overhang is the rear crossover point. The moat is in contact with the path point on the edge of the moat, and the first and the second are divided in the order of time sequence when passing through the moat;

(2) Path planning between critical path points;

After determining the critical path point, between two adjacent critical path points, the aforementioned constraint condition for safely crossing the trench, that is, the allowable angle β for crossing the trench, is the constraint condition, and the allowable angle for crossing the trench between the next critical path point β is the path planning target. In the vertical spanning strategy, the β angle is within the range of 90°±3°, and in the oblique spanning strategy, it is within 2 to 3° of the critical value that the β angle can easily reach in the corresponding stage; The Reeds-Shepp curve connects the critical path points in series, and the planned path satisfies the Ackerman corner model of the vehicle;

For no-solution planning, the reverse path planning and path planning time sequence superposition method are used to achieve the planning goal. For example, when crossing a trench, the steering and driving under a single path planning cannot make the vehicle adjust to the range of the constraint angle β of the next stage. Steering and reversing planning, the target value of the included angle β can be reached by superimposing one or more front and rear steering maneuvers.

(3) The selection of the intermediate path points is related to the sampling frequency of the vehicle motion state and is multiplied to meet the requirements of path tracking and path correction, thereby satisfying the path following accuracy.

(4) The planned paths between the key path points are connected in series to generate a path planning control instruction, which is sent to the path tracking module.

According to the above scheme, the system also includes a vehicle controller, and the vehicle controller includes a driving anti-skid module and a fault handling module; wherein,

Drive the anti-skid module, which is used to issue a torque limit command when an electric wheel is about to reach the edge of the trench, to prevent the electric wheel from slipping due to insufficient adhesion, and to avoid excessive torque output from breaking the edge of the trench and increasing the difficulty of crossing; When an electric wheel is about to hang above the trench, a zero torque output command is issued, so that the electric wheel hanging above the trench has no power output;

The fault processing module is used to judge the software and hardware faults of the vehicle according to the monitoring information, and control the vehicle according to the corresponding fault level.

A trench identification and spanning method realized by the automatic identification and control system of the described unmanned off-road vehicle spanning trenches, it is characterized in that: this method comprises the following steps:

S1. When it is detected that there is a horizontal long negative obstacle with a depth D greater than the wheel rolling radius r on the current path, the function of crossing the trench is automatically activated to limit the vehicle speed below the preset low speed threshold;

S2, perform information fusion calculation processing to obtain the width of the trench W, the angle β between the center line of the trench and the longitudinal axis of the vehicle, and the distance between the center of the front axle of the vehicle and the trench;

S3. According to the size information of the trench, the angle between the center line of the trench and the longitudinal axis of the vehicle, and combined with the size information of the vehicle itself, determine whether the vehicle can cross the trench, and formulate a crossing strategy;

3.1. It is judged that the trench width _W is less than the maximum trench width Wmax that the vehicle can cross, then it is determined that the vehicle can cross the trench. If the trench width _W is greater than or equal to the maximum trench width Wmax that the vehicle can cross, it is determined that the vehicle cannot cross the current trench, and the planned path is Open the trench; W _max is calculated according to the following formula:

where B is the wheelbase of the vehicle, and L is the wheelbase of the vehicle;

3.2, when it is determined that the vehicle can cross the trench, formulate the crossing strategy as follows:

When W < 2r, the vertical crossing strategy is adopted, that is, the relative position of the vehicle and the moat is adjusted so that the longitudinal axis of the vehicle is almost perpendicular to the local center line of the moat, and then it goes straight across the moat. If the number of environmental obstacles is greater than the threshold, the diagonal crossing strategy ; r is the wheel rolling radius;

When W≥2r, the oblique span strategy is adopted;

The oblique-span strategy described includes three stages:

In the first stage, start to cross the trench, keep the β angle and drive forward from side A to side B of the trench. One front wheel w1 crosses the trench first. At this time, it is necessary to ensure that when w1 does not touch the side B of the trench, the other front wheel w2 The A side edge of the moat shall not sink into the moat, guarantee β∈(arctan(B/L), arccos(W/B)];

In the second stage, w1 has already crossed the moat and landed on the B side, and w2 started to cross the moat. At this time, it is necessary to ensure that when w2 does not touch the B side of the moat, the rear wheel w3 on the diagonal line of the vehicle with w2 is located on the edge of the moat A side and must not sink into the moat. , guarantee β∈[arctan(B/L)+arcsin(W/sqrt(B ² +L ² )), 90°];

In the third stage, w2 has crossed the moat and landed on the B side, and w3 started to cross the moat. At this time, it is necessary to ensure that when w3 does not touch the B side of the moat, the rear wheel w4 on the diagonal line with w1 is located on the edge of the moat A side and must not sink into the moat. , the working conditions of the third stage are the same as the working conditions of the first stage, but the relative driving direction of the vehicle is opposite, so β satisfies the results of the first stage;

Therefore, if β _per is not an empty set and _β∈βper , the planned path keeps this angle straight and diagonally across the trench; otherwise, the planned path adjusts the included angle β to satisfy _β∈βper , and then the planned path keeps this angle straight and diagonally across moat;

If β _per is an empty set, plan a large-angle steering maneuver on the path, adjust the relative position of the vehicle and the trench in real time so that the β angle meets the requirements of each stage in the process of crossing the trench, and cross the trench through multiple steering maneuvers;

S4. When the judging module determines that the vehicle can cross the ditch, according to the formulated crossing strategy, combined with the performance parameters of the vehicle, formulate a path for crossing the ditch, and determine the waypoint and the speed of the waypoint;

If the vertical crossing strategy is adopted, the planned path is maneuvered in front of the trench to adjust the beta angle to 90°±3°, and the environment perception unit detects that the beta angle is within the range of 90°±3° and starts to go straight through the trench;

When adopting the diagonal-span strategy, if the straight-line diagonal-span method is adopted, the planned path adjusts the included angle β to make it satisfy _β∈βper , and then the planned path maintains this angle to go straight to the diagonal-span trench; , then the planned path is turned to maneuver at a large angle, and the relative position of the vehicle and the trench is adjusted so that the β angle meets the requirements of each stage in the process of crossing the trench, and the crossing is gradually completed;

At the same time, the Reeds-Shepp curve is adopted to make the path shortest and satisfy the Ackerman steering model;

The specific planning steps are as follows:

(1) Judging the adopted spanning strategy and spanning method, and then select the critical path point, the critical path point includes the starting path point, the stage split point and the end point;

When the vertical leaping strategy is adopted, the current position is selected as the starting path point, the estimated point of any front wheel overhang is the stage split point 1, the end point of any rear wheel overhang is the stage split point 2, and the position 3m away from the trench in the current route angle direction is the end point; The obstacle avoidance path point is selected by the algorithm according to the actual obstacle situation;

When adopting the oblique spanning strategy, the starting and ending path points and obstacle avoidance path points are selected as above; when using the straight-line spanning method, the first front wheel suspension end point is selected as the stage split point 1, and the second front wheel suspension end point is selected as the stage split point 2 , the end point of the first rear wheel suspension is the stage split point 3; the step-by-step steering and spanning method is adopted, and the first front wheel suspension end point is selected as the stage split point 1, and the first rear wheel suspension start point is the stage split point 2 , the allowable passing point is stage split point 3, and the end point of the first rear wheel overhang is stage split point 4; among them, the estimated point of the front wheel overhang is the path point where the front wheel reaches the edge of the moat, and the end point of the rear wheel overhang is the rear crossover point. The moat is in contact with the path point on the edge of the moat, and the first and the second are divided in the order of time sequence when passing through the moat;

(2) Path planning between critical path points;

After determining the critical path point, between two adjacent critical path points, the aforementioned constraint condition for safely crossing the trench, that is, the allowable angle β for crossing the trench, is the constraint condition, and the allowable angle for crossing the trench between the next critical path point β is the path planning target. In the vertical spanning strategy, the β angle is within the range of 90°±3°, and in the oblique spanning strategy, it is within 2 to 3° of the critical value that the β angle can easily reach in the corresponding stage; The Reeds-Shepp curve connects the critical path points in series, and the planned path satisfies the Ackerman corner model of the vehicle;

For no-solution planning, the reverse path planning and path planning time sequence superposition method are used to achieve the planning goal. For example, when crossing a trench, the steering and driving under a single path planning cannot make the vehicle adjust to the range of the constraint angle β of the next stage. Steering and reversing planning, the target value of the included angle β can be reached by superimposing one or more front and rear steering maneuvers.

(3) The selection of the intermediate path points is related to the sampling frequency of the vehicle motion state and is multiplied to meet the requirements of path tracking and path correction, thereby satisfying the path following accuracy.

(4) The planned paths between the key path points are connected in series to generate path planning control instructions;

S5. Control the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined waypoint and the speed of the waypoint, so as to control the vehicle to automatically cross the moat according to the predetermined moat crossing path.

According to the above method, the method further includes a fault processing step, monitoring the fault information of each part of the vehicle in real time, judging the software and hardware faults of the vehicle according to the monitoring information, and controlling the vehicle according to the corresponding fault level.

According to the above method, the method also includes the step of driving anti-skidding, when a certain electric wheel is about to reach the edge of the trench, a torque limit command is issued to prevent the electric wheel from slipping due to insufficient adhesion, and at the same time to avoid excessive torque output from damaging the edge of the trench And increase the difficulty of crossing; when an electric wheel is about to hang above the trench, a zero torque output command is issued, so that the electric wheel hanging above the trench has no power output.

The beneficial effects of the invention are as follows: whether there is a trench is judged by image processing and information fusion technology, and corresponding crossing strategies are set for different situations, thereby ensuring that the vehicle automatically crosses over the trench within a certain width, and is adaptable to complex and changeable field environments. it is good.

Description of drawings

FIG. 1 is a system composition and information flow diagram of an embodiment of the present invention.

FIG. 2 is a control flow chart of an embodiment of the present invention.

Figure 3 is a schematic diagram of the installation of the lidar and the detection of raised obstacles.

Figure 4 is a schematic diagram of the parameters of the moat (negative obstacle) measured by the lidar.

Figure 5(a), (b), (c) are schematic diagrams of three stages of vehicles crossing the trench.

FIG. 6 is a schematic diagram of a phased turn to cross a critical way point and a path.

Detailed ways

The present invention will be further described below with reference to specific examples and accompanying drawings.

The present invention provides an automatic identification and control system for an unmanned off-road vehicle crossing a trench, which is arranged in a four-wheel drive electric vehicle, as shown in FIG. 1 , which includes:

The environment perception unit is used to obtain information about obstacles around the vehicle. The environment perception unit includes a sensor, a vehicle parameter collection module, and a trench identification and parameter extraction module.

Sensors include lidar, multiple ultrasonic sensors, and binocular cameras. Lidar is arranged at a higher position in the front and rear of the vehicle as active vision to collect information on major obstacles; four ultrasonic radars are arranged on the left and right sides of the front and rear bumpers, mainly collecting information on other surrounding obstacles; binocular cameras are used to form vehicle environmental vision.

The vehicle parameter collection module is used to obtain the vehicle running state parameters, which are mainly obtained from the vehicle controller.

The trench identification and parameter extraction module is used to obtain the edge lines of obstacles within the perceptible range of the vehicle through image processing and information fusion methods based on the information of obstacles around the vehicle, identify and calculate the size information of the trench, and combine the vehicle pose The information calculates the angle between the centerline of the moat and the longitudinal axis of the vehicle. Described environment perception unit, when detecting that there is a horizontal elongated negative obstacle on the current path, and the depth D of the negative obstacle is greater than or equal to the wheel rolling radius r, then it is judged to encounter a trench; Calculate the trench width W, the trench center The angle β between the line and the longitudinal axis of the vehicle, and the distance s from the vehicle to the edge of the trench.

The decision-making unit includes a judgment module, a path planning module and a path tracking module; wherein, the judgment module is used to judge whether the vehicle can cross according to the size information of the trench, the angle between the center line of the trench and the longitudinal axis of the vehicle, and the size information of the vehicle itself moat, and formulate a crossing strategy; the path planning module is used to formulate a path across the moat and determine the path point and the speed of the path point according to the formulated crossing strategy and the performance parameters of the vehicle when the judgment module determines that the vehicle can cross the moat; the path tracking module , which is used to control the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined waypoint and the speed of the waypoint, so as to control the vehicle to automatically cross the moat according to the predetermined path of crossing the moat.

The length of the built-in vehicle in the judgment module is I, the width is w, the wheelbase is B, the wheelbase is L, the wheel rolling radius r, the wheel diameter d, the minimum turning radius of the vehicle R _min , the maximum turning angle of the steering wheel α _max and other information , it is judged that the trench width _W is less than the maximum trench width Wmax that the vehicle can cross, then it is determined that the vehicle can cross the trench, if the trench width _W is greater than or equal to the maximum trench width Wmax that the vehicle can cross, then it is determined that the vehicle cannot span the current trench, and the planned path is bypassed. Moat; _Wmax is calculated by the following formula:

where B is the wheelbase of the vehicle, and L is the wheelbase of the vehicle.

The judgment module formulates the leaping strategy according to the following methods:

When W < 2r, the vertical crossing strategy is adopted, that is, the relative position of the vehicle and the moat is adjusted so that the longitudinal axis of the vehicle is almost perpendicular to the local center line of the moat, and then it goes straight across the moat. If the number of environmental obstacles is greater than the threshold, the diagonal crossing strategy ; r is the wheel rolling radius;

When W≥2r, the oblique span strategy is adopted;

The oblique-span strategy described includes three stages:

In the first stage, start to cross the trench, keep the β angle and drive forward from side A to side B of the trench. One front wheel w1 crosses the trench first. At this time, it is necessary to ensure that when w1 does not touch the side B of the trench, the other front wheel w2 The A side edge of the moat shall not sink into the moat, guarantee β∈(arctan(B/L), arccos(W/B)];

In the second stage, w1 has already crossed the moat and landed on the B side, and w2 started to cross the moat. At this time, it is necessary to ensure that when w2 does not touch the B side of the moat, the rear wheel w3 on the diagonal line of the vehicle with w2 is located on the edge of the moat A side and must not sink into the moat. , guarantee β∈[arctan(B/L)+arcsin(W/sqrt(B ² +L ² )), 90°];

In the third stage, w2 has crossed the moat and landed on the B side, and w3 started to cross the moat. At this time, it is necessary to ensure that when w3 does not touch the B side of the moat, the rear wheel w4 on the diagonal line with w1 is located on the edge of the moat A side and must not sink into the moat. , the working conditions of the third stage are the same as the working conditions of the first stage, but the relative driving direction of the vehicle is opposite, so β satisfies the results of the first stage;

Therefore, if β _per is not an empty set and _β∈βper , the planned path keeps this angle straight and diagonally across the trench; otherwise, the planned path adjusts the included angle β to satisfy _β∈βper , and then the planned path keeps this angle straight and diagonally across moat;

If β _per is an empty set, plan a large-angle steering maneuver on the path, adjust the relative position of the vehicle and the trench in real time so that the β angle meets the requirements of each stage in the process of crossing the trench, and cross the trench through multiple steering maneuvers.

The path planning module performs path planning under corresponding operating conditions and strategies according to the results of the judgment module:

If the vertical crossing strategy is adopted, the planned path is maneuvered in front of the trench to adjust the beta angle to 90°±3°, and the environment perception unit detects that the beta angle is within the range of 90°±3° and starts to go straight through the trench;

When adopting the diagonal-span strategy, if the straight-line diagonal-span method is adopted, the planned path adjusts the included angle β to make it satisfy _β∈βper , and then the planned path maintains this angle to go straight to the diagonal-span trench; , then the planned path is turned to maneuver at a large angle, and the relative position of the vehicle and the trench is adjusted so that the β angle meets the requirements of each stage in the process of crossing the trench, and the crossing is gradually completed;

Meanwhile, the path planning module adopts the Reeds-Shepp curve to make the path shortest and satisfy the Ackerman steering model.

The specific planning steps of the path planning module are as follows:

(1) Judging the adopted spanning strategy and spanning method, and then select the critical path point, the critical path point includes the starting path point, the stage split point and the end point;

When the vertical leaping strategy is adopted, the current position is selected as the starting path point, the estimated point of any front wheel overhang is the stage split point 1, the end point of any rear wheel overhang is the stage split point 2, and the position 3m away from the trench in the current route angle direction is the end point; The obstacle avoidance path point is selected by the algorithm according to the actual obstacle situation;

When adopting the oblique spanning strategy, the starting and ending path points and obstacle avoidance path points are selected as above; when using the straight-line spanning method, the first front wheel suspension end point is selected as the stage split point 1, and the second front wheel suspension end point is selected as the stage split point 2 , the end point of the first rear wheel suspension is the stage split point 3; the step-by-step steering and spanning method is adopted, and the first front wheel suspension end point is selected as the stage split point 1, and the first rear wheel suspension start point is the stage split point 2 , the allowable passing point is stage split point 3, and the end point of the first rear wheel overhang is stage split point 4; among them, the estimated point of the front wheel overhang is the path point where the front wheel reaches the edge of the moat, and the end point of the rear wheel overhang is the rear crossover point. The moat is in contact with the path point on the edge of the moat, and the first and the second are divided in the order of time sequence when passing through the moat;

(2) Path planning between critical path points;

After determining the critical path point, between two adjacent critical path points, the aforementioned constraint condition for safely crossing the trench, that is, the allowable angle β for crossing the trench, is the constraint condition, and the allowable angle for crossing the trench between the next critical path point β is the path planning target. In the vertical spanning strategy, the β angle is within the range of 90°±3°, and in the oblique spanning strategy, it is within 2 to 3° of the critical value that the β angle can easily reach in the corresponding stage; The Reeds-Shepp curve connects the critical path points in series, and the planned path satisfies the Ackerman corner model of the vehicle;

For no-solution planning, the reverse path planning and path planning time sequence superposition method are used to achieve the planning goal. For example, when crossing a trench, the steering and driving under a single path planning cannot make the vehicle adjust to the range of the constraint angle β of the next stage. Steering and reversing planning, the target value of the included angle β can be reached by superimposing one or more front and rear steering maneuvers.

(3) The selection of the intermediate path points is related to the sampling frequency of the vehicle motion state and is multiplied to meet the requirements of path tracking and path correction, thereby satisfying the path following accuracy.

(4) The planned paths between the key path points are connected in series to generate a path planning control instruction, which is sent to the path tracking module.

The direction control function is designed based on pure tracking algorithm. The actual trajectory of the vehicle is an arc, and the straight line path can be seen as an arc with an infinite radius. The vehicle considers the speed to remain unchanged for a short period of time. Once a suitable size is planned The steering angle can more accurately track the reference path. When the vehicle crosses the ditch, the speed is very low, and the forward-looking path point that is closer on the path is selected for tracking, and two errors will be generated. One is the tracking error in the distance, which is called the lateral error, and this error is the absolute distance between the vehicle position and the desired path. ; Another error is that the angle between the longitudinal axis of the vehicle and the tangent of a point on the reference path is the heading error. The fuzzy controller is used to control the real-time steering angle of the vehicle, so that the above error tends to be zero as much as possible.

The speed control function receives the expected value command of the path point speed of the path planning, and the command changes and corrects in real time with the change of the working conditions and the way point. The pressure of the system is closed-loop controlled. Based on human's experience and process of vehicle speed control, a set of incremental fuzzy PID controllers are respectively used for in-wheel motor driving and braking system braking. Driving and braking do not work at the same time. When the current speed is greater than a certain value of the command speed, Stop the drive control flag bit set, jump out of the drive PID control, enter the brake PID control algorithm, on the contrary, when the current speed is less than a certain value of the command speed, enter the drive PID control algorithm.

The system also includes a vehicle controller, and the vehicle control unit (VCU) is the bottom control unit of the vehicle, including functional control strategies and bottom driving strategies. In the present invention, it is divided into a driving anti-skid module, a fault processing module, and other control units. module. Since the inventions of this patent in the VCU part mainly focus on the drive anti-skid and fault handling modules, other control modules will not be described in detail. The driving anti-skid module ensures that the electric wheels suspended above the trench or with insufficient adhesion will fly when the vehicle crosses the trench; the fault handling module is responsible for monitoring and analyzing the working status of each unit of the system, and handling the failure in case of failure to ensure the entire process of crossing the trench. Safety.

The driving anti-skid module can pre-judg the attachment of the wheels according to real-time environmental perception information, planned paths, path points where the vehicle is located, vehicle posture and motion parameters, so as to adopt corresponding control strategies to prevent the electric wheels from slipping. When the system judges that an electric wheel is about to reach the edge of the trench, the drive anti-skid unit will issue an appropriate limit torque command to prevent the electric wheel from slipping due to insufficient adhesion, and to avoid excessive torque output from damaging the edge of the trench and increasing the difficulty of crossing. ; When the system judges that the electric wheel is about to hang above the trench, the drive anti-skid module sends a zero torque output command, so that the electric wheel hanging above the trench has no power output; at the same time, the drive anti-skid module also has a conventional drive anti-skid function, the system detects When any electric wheel is slipping, the corresponding torque limit will be issued to ensure that the output torque of the wheel in the non-suspended state drives the vehicle to cross the trench.

The fault processing module has the functions of fault monitoring, fault classification processing, limp control, etc. According to the monitoring information, the vehicle software and hardware faults are judged, and the faults are divided into three levels. The first-level fault will immediately cut off the high-voltage power/stop operation, and the second-level fault will be cut off immediately. Limit power output and driving speed, perform low-speed limp, three-level fault alarm prompts but normal driving.

A trench identification and spanning method realized by the automatic identification and control system of the described unmanned off-road vehicle spanning trenches, as shown in Figure 2, comprises the following steps:

S1. When it is detected that there is a horizontally long negative obstacle with a depth D greater than the wheel rolling radius r on the current path, the function of crossing the trench is automatically activated to limit the vehicle speed below the preset low speed threshold (10km/h); then fault Information confirmation, read the fault information of each system of the current vehicle, and confirm that there is no fault or any fault that affects the safety of crossing trenches.

The detection of obstacles is described as follows: the lidar is installed on the top of the car, and the scanning line has a certain slope downward, as shown in Figure 3. In the figure, h is the height of the lidar, p is the minimum height of the obstacle to be detected, and γ is the pitch angle of the lidar. A protruding object with a height of 0.4m above the ground is determined as a raised obstacle, a concave area of the road surface with a depth greater than 0.4m and greater than the wheel rolling radius r is determined as a negative obstacle, and a horizontally elongated negative obstacle with a depth D greater than the wheel rolling radius r. Obstacles trigger automatic trench crossing function.

In the actual detection of moat, the distance between discontinuous adjacent clumps in the horizontal and vertical directions is mainly detected. If the distance is greater than the given threshold, it means that a moat obstacle is detected. At this time, the distances in the horizontal and vertical directions are defined as the width W and depth D of the moat, respectively. As shown in Figure 4, h _hdl is the installation height of the radar, θ is the maximum depression angle of the ray emitted by the lidar, d is the horizontal distance from the ground reflection point of the maximum depression angle ray to the lidar, and γ is any laser ray and the maximum depression angle laser ray Δd is the distance between any laser ray on the flat ground and the reflection point of the maximum depression angle, and the included angle between two adjacent laser rays is Δθ, which defines the scanning of two adjacent laser reflection points on the flat ground. The distance is Δd _ideal . When the moat exists, the distance between the reflection point of the beam entering the moat and the reflection point of the adjacent storefront increases to Δd _true . The presence of negative obstacles can be detected by the Δd changes caused by discontinuous adjacent clumps, and the estimation of the depth of the negative obstacles can be performed.

S2. Sensors such as lidar collect detailed environmental information and perform information fusion calculation processing by the information processing module to obtain the local width of the trench W, the angle β (≤90°) between the center line of the trench and the longitudinal axis of the vehicle, the center of the front axle of the vehicle and the trench the distance. Specifically include the following steps:

2.1. Take the position where the automatic trench crossing function is currently enabled as the coordinate origin, take the longitudinal direction of the car as the X axis, and the transverse direction of the car as the Y axis, to establish a plane coordinate system;

2.2. Process the environmental information perceived by the radar and camera according to the pre-input algorithm, calculate and identify the outline and orientation of the moat, fit the edge line of the moat by image processing, and establish the parameterized moat edge and position in the above-mentioned plane coordinate system; Prominent obstacles, represented in the coordinate system;

2.3. According to the vehicle running state parameters fed back by the vehicle controller, including the current driving speed v, the driving time t and the steering wheel angle α, the relative position of the current waiting vehicle and the trench is calculated in combination with the above-mentioned trench parameters, and parameterized in the coordinate system represents the key parameters required by the judgment module and the path planning module: the distance s and β from the vehicle to the edge of the trench.

S3. According to the size information of the trench, the angle between the center line of the trench and the longitudinal axis of the vehicle, and combined with the size information of the vehicle itself, determine whether the vehicle can cross the trench, and formulate a crossing strategy.

3.1, the basic requirements for safely crossing the trench At any time, ensure that the vehicle has at least three wheels on the ground (that is, not overhanging the trench). When W _max appears in a diagonally crossing ditch, the wheels on the diagonal line of the vehicle are at the edge of the ditch at the same time, and continue to cross the ditch. No matter how the vehicle is manipulated, the two wheels on the diagonal line will hang on the ditch at the same time and fail to cross the ditch. is the limit value of the vehicle crossing the trench

In formula (1), B is the wheelbase of the vehicle, and L is the wheelbase of the vehicle.

When W ≥ W _max , the vehicle cannot cross the current trench, and the planned path bypasses the trench and exits the automatic trench crossing function.

When _W <Wmax, the vehicle can cross the current moat.

3.2, when it is determined that the vehicle can cross the trench, formulate the crossing strategy as follows:

When W < 2r, the vertical crossing strategy is adopted, that is, the relative position of the vehicle and the moat is adjusted so that the longitudinal axis of the vehicle is almost perpendicular to the local center line of the moat, and then it goes straight across the moat. If the number of environmental obstacles is greater than the threshold, the diagonal crossing strategy ; r is the wheel rolling radius;

When W≥2r, the oblique span strategy is adopted;

The oblique-span strategy includes three stages, as shown in Figure 5:

In the first stage, start to cross the trench, keep the β angle and drive forward from side A to side B of the trench. One front wheel w1 crosses the trench first. At this time, it is necessary to ensure that when w1 does not touch the side B of the trench, the other front wheel w2 It is not allowed to sink into the trench on the side edge of trench A, and β is satisfied by geometric analysis

B×cosβ≥W (2)

β≤arccos(W/B) (3)

At this time, the allowable limit β angle β _per1 =arccos(W/B) occurs when w1 is located at the edge of the A side of the moat and w2 is just located at the edge of the B side of the moat. On the other hand, β should be larger than the included angle when W _max appears, that is, the wheels on the diagonal line of the vehicle are at the edge of the trench at the same time. From the geometric analysis, β satisfies

β＞arctan(B/L) (4)

Therefore, at this stage β∈(arctan(B/L), arccos(W/B)], the vehicle can safely cross the trench.

In the second stage, w1 has already crossed the trench and landed on the B side, and w2 started to cross the trench. At this time, it is necessary to ensure that when w2 does not touch the B side of the trench, the rear wheel w3 on the diagonal line of the vehicle is located on the edge of the trench A side and must not fall into the trench. Analyzed and found that β is satisfied

β≥∠1+∠2,

which is

出现在w3位于壕沟A侧边缘且w2刚好位于壕沟B侧边缘。The allowable limit β angle at this time

Occurs when w3 is at the edge of the A side of the moat and w2 is just at the edge of the B side of the moat.

Therefore, at this stage β∈[arctan(B/L)+arcsin(W/sqrt(B ² +L ² )), 90°], the vehicle can safely cross the trench.

In the third stage, w2 has crossed the moat and landed on the B side, and w3 started to cross the moat. At this time, it is necessary to ensure that when w3 does not touch the B side of the moat, the rear wheel w4 on the diagonal line with w1 is located on the edge of the moat A side and must not sink into the moat. , the working conditions of the third stage are the same as the working conditions of the first stage, but the relative driving direction of the vehicle is opposite, so β satisfies the results of the first stage;

To sum up, the allowable β angle range β _per = [arctan(B/L)+arcsin(W/sqrt(B ² +L ² )), arccos(W/B)] is not empty when the vehicle goes straight and diagonally across the trench of this width When set, if _β∈βper , the planned path keeps the angle straight and diagonally across the trench; otherwise, the planned path adjusts the included angle β to satisfy _β∈βper , and then the planned path keeps the angle straight and diagonally across the trench; when _βper When it is an empty set, it is necessary to plan the path adjustment β to satisfy the safe spanning angle range required by the three spanning stages.

The above β _per is not an empty set. When β does not satisfy β _per , the program does not enter the steering ramp program because the straight-line ramp pass has high efficiency, the time above the trench is short, the vehicle manipulation control program is less, and the safety is more guaranteed. .

If β _per is not an empty set and _β∈βper , the planned path keeps this angle straight and diagonally across the trench; otherwise, the planned path adjusts the included angle β to satisfy _β∈βper , and then the planned path keeps this angle straight and diagonally across the trench;

If β _per is an empty set, plan a large-angle steering maneuver on the path, adjust the relative position of the vehicle and the trench in real time so that the β angle meets the requirements of each stage in the process of crossing the trench, and cross the trench through multiple steering maneuvers.

The judging unit sends the judging result, that is, the vehicle crossing the trench strategy obtained through calculation and logical judgment, to the path planning module.

S4. When the judging module determines that the vehicle can cross the ditch, according to the formulated crossing strategy, combined with the vehicle's component energy parameters, formulate the path across the ditch, and determine the path point and the speed of the path point; if the vertical crossing strategy is adopted, the planned path is carried out in front of the ditch. The steering maneuver adjusts the β angle to the range of 90°±3°, and the environment perception unit detects that the β angle is within the range of 90°±3° and starts to go straight through the trench; when the oblique-span strategy is adopted, if the straight-travel and oblique-span method is used to cross, plan Adjust the included angle β of the path to satisfy _β∈βper , and then plan the path to keep this angle straight and diagonally across the trench; if a phased steering diagonally-span method is required, the planned path is turned to maneuver at a large angle, and the relative position of the vehicle and the trench is adjusted. Make the β angle meet the requirements of each stage in the process of crossing the trench, and gradually complete the crossing.

The path planning described has the following points:

4.1. The path planning under this control method is located under the global path planning of the unmanned vehicle, and is different from the local path planning. It is a special working condition path planning. The planning method is based on geometric planning, that is, the relative positions of the four wheels and the trench and the aforementioned constraints for safely crossing the trench. The path adopts the Reeds-Shepp curve to make the path the shortest, and satisfies the Ackerman steering model to ensure the efficient and safe passing. Reserve the adjustment space for the included angle β required by the step-by-step steering method.

4.2. The path planning includes the planning of the path point (or trajectory) and the ideal speed of the path point (including stopping and starting). The path points include the starting path point, the obstacle avoidance path point, the split point in the cross-ditch stage (that is, the path point where a certain wheel starts to hang above the moat or ends up hanging according to the coordinate system and the vehicle motion state), the allowable passage point ( When it is at this waypoint, it satisfies the constraints of safely crossing the moat in the second stage), the middle waypoint (the waypoint inserted between the adjacent special waypoints that is easy to track), and the end point. The ideal speed of the waypoint is planned as follows: the ideal speed of the waypoint in the whole process is lower than 10km/h; the speed of the stage split point and the intermediate waypoint between adjacent stage split points is limited to less than 5km/h; The current vehicle speed is maintained, and no special control is performed; the starting and stopping use linear speed planning, that is, the starting acceleration and braking deceleration are set to small fixed values.

4.3. Basic steps of path planning:

(1) Judging the adopted leaping strategy and leaping method, and then select the key way point.

The vertical leaping strategy is adopted, the current position is selected as the starting path point, the estimated point of any front wheel overhang (that is, the path point where the front wheel reaches the edge of the trench) is the stage division point 1, and the end point of any rear wheel overhang (that is, the contact point of the rear over the trench) The path point on the edge of the moat) is the stage split point 2, and the position 3m away from the moat in the angular direction of the current route is the end point. The obstacle avoidance path point is selected by the algorithm according to the actual obstacle situation. When adopting the oblique spanning strategy, as shown in Figure 6, the starting and ending path points and the obstacle avoidance path points are selected as above; when the straight spanning method is adopted, the end point of the first front wheel suspension is selected as the stage split point 1, and the second front wheel suspension ends The point is the stage split point 2, and the end point of the first rear wheel suspension is the stage split point 3; adopt the step-by-step steering and spanning method, select the first front wheel suspension end point as the stage split point 1, and the first rear wheel suspension starts. The point is the stage split point 2, the pass-through point is the stage split point 3, and the end point of the first rear wheel suspension is the stage split point 4.

Note: The above "first" and "second" are the wheels that start to be suspended and end to be suspended in sequence when passing through the trench.

(2) Path planning between critical path points.

After determining the critical path point, between two adjacent critical path points, the aforementioned constraint condition for safely crossing the trench, that is, the allowable angle β for crossing the trench, is the constraint condition, and the allowable angle for crossing the trench between the next critical path point β is the path planning target. In the vertical spanning strategy, the β angle is within the range of 90°±3°, and in the oblique spanning strategy, it is within 2-3° of the critical value that the allowable range of the β angle in the corresponding stage is easy to reach. The critical path points are connected in series with the Reeds-Shepp curve through the algorithm, and the planned path satisfies the Ackerman angle model of the vehicle. For no-solution planning, the reverse path planning and path planning time sequence superposition method are used to achieve the planning goal. For example, when crossing a trench, the steering and driving under a single path planning cannot make the vehicle adjust to the range of the constraint angle β of the next stage. Steering and reversing planning, the target value of the included angle β can be reached by superimposing one or more front and rear steering maneuvers.

(3) The selection of the intermediate path points is related to the sampling frequency of the vehicle motion state and is multiplied to meet the requirements of path tracking and path correction, thereby satisfying the path following accuracy.

(4) The planned paths between the key path points are connected in series to generate a path planning control instruction, which is sent to the path tracking module.

4.4. The current trench is tracked by fusing the trench coordinate information and the vehicle kinematics information, and the heading angle, that is, the path planning result, is corrected at the same time. The errors caused by path tracking and chassis actuator control are reduced through multiple planning and correction methods, so the planned path is in a real-time tracking and correction state. The intermediate path points are selected by algorithm according to the path tracking requirements.

S5. Control the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined waypoint and the speed of the waypoint, so as to control the vehicle to automatically cross the moat according to the predetermined moat crossing path. The direction control function and speed control all use closed-loop control, and the algorithm and control principle are as described above. The direction control function outputs the steering angle adjustment amount through the fuzzy control algorithm to control the vehicle to drive along the ideal trajectory; the speed control function outputs the acceleration, braking signal and gear signal through the fuzzy PID algorithm to control the vehicle speed as close to the ideal speed as possible.

The above control signal is equivalent to the driver signal, which is sent by the decision-making unit to the vehicle control unit. The vehicle control unit acts as the underlying driver and controls the corresponding actuators (steering system, braking system, four-wheel motor drive system, etc.) after receiving the signal. Execute the control command, and finally the vehicle crosses the trench according to the path. During the whole driving process of the vehicle, the driving anti-skid module in the vehicle control unit controls the driving state of the four-wheel motor in real time to prevent slipping or even flying; the fault handling module is in real-time working condition. The fault information of each system of the vehicle is confirmed, and then the fault information is monitored in real time, and the fault occurred in the process of crossing the trench is dealt with in time to ensure the safety of crossing the trench.

The advantages of the present invention are mainly reflected in the following aspects:

(1) The environment perception module composed of sensors such as lidar and cameras installed at the front and rear of the vehicle body detects obstacles in the process of vehicle travel, which well solves the limited field of vision, large blind spots, and distances of traditional off-road vehicle drivers. Inaccurate perception, loss of vision in bad weather conditions, etc. Through the fusion calculation of the information collected by sensors such as radar and cameras, the exact position of the obstacle, the specific shape parameters of the trench and the relative position of each wheel and the trench when crossing the trench can be obtained. The control unit can make reasonable analysis and decision-making, and formulate the best Control Strategy. Good adaptability to complex and changeable field environments.

(2) The unmanned off-road vehicle performs fusion calculation according to the information collected by each sensor, and the decision-making unit performs comprehensive analysis and decision-making according to the calculated obstacle parameters, the relative position parameters of the vehicle and the obstacle, and the vehicle geometric parameters. The process of judgment and decision-making is rational, and it can be determined through accurate calculation whether the vehicle can pass through the trench and in what way, and the safety is fully guaranteed.

(3) The control unit will plan the path according to the key environmental information, formulate the best control strategy for passing through the trench, and control the vehicle to execute the control command to achieve path tracking, which realizes precise control and can automatically adjust in real time according to the information during the driving process. It is ensured that the vehicle can cross the trench in the best way, saving time when passing obstacles and improving efficiency.

(4) The present invention is a distributed drive off-road vehicle, each wheel of the vehicle is directly driven by a motor, its torque is independently controllable, and can be flexibly and coordinated to control, realize differential power steering and reduce the minimum turning radius of the vehicle when off-road, which is very large. To a certain extent, the mobility and flexibility of the off-road vehicle in the field are improved, the attitude adjustment of the vehicle is convenient, and the passing efficiency is improved.

(5) The control system and control method can be transplanted and replicated on the distributed drive vehicles of the same level of hardware facilities, which have portability and replication, and can be widely used in distributed drive off-road vehicles of the same hardware level.

The above embodiments are only used to illustrate the design ideas and features of the present invention, and the purpose is to enable those skilled in the art to understand the contents of the present invention and implement them accordingly, and the protection scope of the present invention is not limited to the above embodiments. Therefore, all equivalent changes or modifications made according to the principles and design ideas disclosed in the present invention fall within the protection scope of the present invention.

Claims (8)

1. the automatic identification and control system of an unmanned off-road vehicle spanning trenches, it is characterized in that: be arranged in the four-wheel distributed drive electric vehicle, it comprises: The environmental perception unit is used to obtain the information of obstacles around the vehicle. Through image processing and information fusion methods, the edge lines of the obstacles within the perceptible range of the vehicle are obtained, and the size information of the trench is identified and calculated; at the same time, it receives the vehicle motion state information, including Pose information, combined with trench information to calculate the angle between the center line of the trench and the longitudinal axis of the vehicle; The decision-making unit includes a judgment module, a path planning module and a path tracking module; wherein, the judgment module is used to judge whether the vehicle can cross according to the size information of the trench, the angle between the center line of the trench and the longitudinal axis of the vehicle, and combined with the size information of the vehicle itself moat, and formulate a crossing strategy; the path planning module is used to formulate a path across the moat, and determine the path point and the speed of the path point according to the formulated crossing strategy and the performance parameters of the vehicle when the judgment module determines that the vehicle can cross the moat; the path tracking module , which is used to control the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined waypoint and the speed of the waypoint, so as to control the vehicle to automatically cross the moat according to the predetermined path of crossing the moat; The judging module specifically judges that the trench width _W is less than the maximum trench width Wmax that the vehicle can cross, then it is determined that the vehicle can cross the trench, and if the trench width _W is greater than or equal to the maximum trench width Wmax that the vehicle can cross, then it is determined that the vehicle cannot span the current trench. , the planned path bypasses the trench; W _max is calculated according to the following formula:

where B is the wheelbase of the vehicle, and L is the wheelbase of the vehicle; The judging module specifically formulates the leaping strategy according to the following methods: When W < 2r, the vertical crossing strategy is adopted, that is, the relative position of the vehicle and the moat is adjusted so that the longitudinal axis of the vehicle is perpendicular to the local center line of the moat, and then goes straight across the moat, the vertical deviation of less than 3° is allowed; if environmental obstacles If the distribution leads to no straight crossing path, then the diagonal crossing strategy is turned; r is the wheel rolling radius; When W≥2r, the oblique span strategy is adopted; The oblique-span strategy described includes three stages: In the first stage, start to cross the ditch, keep the included angle β and drive forward from the side A of the ditch to the side B. The front wheel w1 on one side crosses the ditch first. At this time, it must be ensured that when w1 does not touch the side B of the ditch, the front wheel on the other side will cross the ditch first. w2 is located on the side edge of the trench A and must not fall into the trench, to ensure β∈[arctan(B/L), arccos(W/B)]; β is the angle between the centerline of the trench and the longitudinal axis of the vehicle; In the second stage, w1 has already crossed the moat and landed on the B side, and w2 started to cross the moat. At this time, it is necessary to ensure that when w2 does not touch the B side of the moat, the rear wheel w3 on the diagonal line of the vehicle with w2 is located on the edge of the moat A side and must not sink into the moat. , guarantee β∈[arctan(B/L)+arcsin(W/sqrt(B ² +L ² )), 90°]; In the third stage, w2 has crossed the moat and landed on the B side, and w3 started to cross the moat. At this time, it is necessary to ensure that when w3 does not touch the B side of the moat, the rear wheel w4 on the diagonal line with w1 is located on the edge of the moat A side and must not sink into the moat. , the working conditions of the third stage are the same as the working conditions of the first stage, but the relative driving direction of the vehicle is opposite, so β satisfies the results of the first stage; Therefore, if β _per is not an empty set and _β∈βper , the planned path keeps the included angle β straight across the trench, otherwise, the planned path adjusts the included angle β to satisfy β∈β _per , and then the planned path keeps the included angle β straight. diagonally across the trench; If _βper is an empty set, plan a large-angle steering maneuver, adjust the relative position of the vehicle and the trench in real time so that the included angle β meets the requirements of each stage in the process of crossing the trench, and cross the trench through multiple steering maneuvers. 2. the automatic identification and control system of the unmanned off-road vehicle according to claim 1, it is characterized in that: described environment perception unit, when detecting that there is a horizontal elongated negative obstacle on the current path, and If the depth D of the negative obstacle is greater than or equal to the wheel rolling radius r, it is judged that a moat is encountered; the width W of the moat, the angle β between the center line of the moat and the longitudinal axis of the vehicle, and the distance s between the vehicle and the edge of the moat are calculated. 3. the automatic identification and control system of unmanned off-road vehicle according to claim 1 across the trench, it is characterized in that: described path planning module carries out the path planning under corresponding working condition and strategy according to the result of judging module: If the vertical leaping strategy is adopted, the planned path is steered in front of the trench to adjust the angle β to 90°±3°, and the environment perception unit detects that the angle β is within the range of 90°±3° and starts to go straight through the trench; When adopting the diagonal-span strategy, if the straight-line diagonal-span method is adopted, the planned path adjusts the included angle β to satisfy _β∈βper , and then the planned path maintains the included angle β to go straight and diagonally across the trench; In this way, the planned path turns to maneuver at a large angle, and the relative position of the vehicle and the trench is adjusted so that the included angle β meets the requirements of each stage in the process of crossing the trench, and the crossing is gradually completed; At the same time, the path planning module adopts the Reeds-Shepp curve to make the path shortest and takes the Ackerman steering model as the constraint. 4. the automatic identification and control system of unmanned off-road vehicle according to claim 3 across the trench, it is characterized in that: the concrete planning step of described path planning module is as follows: (1) Judging the adopted spanning strategy and spanning method, and then select the critical path point, the critical path point includes the starting path point, the stage split point and the end point; When the vertical leaping strategy is adopted, the current position is selected as the starting path point, the estimated point of any front wheel overhang is the stage split point 1, the end point of any rear wheel overhang is the stage split point 2, and the position 3m away from the trench in the current route angle direction is the end point; The obstacle avoidance path point is selected by the algorithm according to the actual obstacle situation; When adopting the oblique spanning strategy, the starting and ending path points and obstacle avoidance path points are selected as above; when using the straight-line spanning method, the first front wheel suspension end point is selected as the stage split point 1, and the second front wheel suspension end point is selected as the stage split point 2 , the end point of the first rear wheel suspension is the stage split point 3; the step-by-step steering and spanning method is adopted, and the first front wheel suspension end point is selected as the stage split point 1, and the first rear wheel suspension start point is the stage split point 2 , the allowable passing point is stage split point 3, and the end point of the first rear wheel overhang is stage split point 4; among them, the estimated point of the front wheel overhang is the path point where the front wheel reaches the edge of the moat, and the end point of the rear wheel overhang is the rear crossover point. The moat is in contact with the path point on the edge of the moat, and the first and the second are divided in the order of time sequence when passing through the moat; (2) Path planning between critical path points; After determining the critical path point, between two adjacent critical path points, the allowable angle β of crossing the moat is used as the constraint condition, and the allowable angle β of crossing the moat between the next critical path point is the path planning target. In the vertical spanning strategy, the included angle β is in the range of 90°±3°, and in the oblique spanning strategy, the included angle β of the corresponding stage is within 2-3° of the critical value that can easily be reached; the critical path is defined by the Reeds-Shepp curve through the algorithm. The points are connected in series, and the planned path satisfies the Ackerman corner model of the vehicle; For no-solution planning, the reverse path planning and path planning time sequence superposition method are used to achieve the planning goal, then the steering and reverse driving planning is carried out, and the target value of the included angle β is reached by one or more forward and backward steering and moving vehicles superimposed; (3) The selection of the intermediate path point is related to the sampling frequency of the vehicle motion state and is multiplied, so as to meet the requirements of path tracking and path correction, so as to meet the path following accuracy; (4) The planned paths between the key path points are connected in series to generate a path planning control instruction, which is sent to the path tracking module. 5. The automatic identification and control system of the unmanned off-road vehicle crossing a trench according to any one of claims 1 to 4, wherein the system also comprises a vehicle controller, and the vehicle controller comprises a driving anti-skid module and a fault handling module; where, Drive the anti-skid module, which is used to issue a torque limit command when an electric wheel is about to reach the edge of the trench, to prevent the electric wheel from slipping due to insufficient adhesion, and to avoid excessive torque output from breaking the edge of the trench and increasing the difficulty of crossing; When an electric wheel is about to hang above the trench, a zero torque output command is issued, so that the electric wheel hanging above the trench has no power output; The fault processing module is used to judge the software and hardware faults of the vehicle according to the monitoring information, and control the vehicle according to the corresponding fault level. 6. a trench identification and spanning method realized by the automatic identification and control system of utilizing the unmanned off-road vehicle described in claim 1 to cross trenches, it is characterized in that: this method may further comprise the steps: S1. When it is detected that there is a horizontal long negative obstacle with a depth D greater than the wheel rolling radius r on the current path, the function of crossing the trench is automatically activated to limit the vehicle speed below the preset low speed threshold; S2, perform information fusion calculation processing to obtain the width of the trench W, the angle β between the center line of the trench and the longitudinal axis of the vehicle, and the distance between the center of the front axle of the vehicle and the trench; S3. According to the size information of the trench, the angle between the center line of the trench and the longitudinal axis of the vehicle, and combined with the size information of the vehicle itself, determine whether the vehicle can cross the trench, and formulate a crossing strategy; 3.1. It is judged that the trench width _W is less than the maximum trench width Wmax that the vehicle can cross, then it is determined that the vehicle can cross the trench. If the trench width _W is greater than or equal to the maximum trench width Wmax that the vehicle can cross, it is determined that the vehicle cannot cross the current trench, and the planned path is Open the trench; W _max is calculated according to the following formula:

where B is the wheelbase of the vehicle, and L is the wheelbase of the vehicle; 3.2, when it is determined that the vehicle can cross the trench, formulate the crossing strategy as follows: When W < 2r, the vertical crossing strategy is adopted, that is, the relative position of the vehicle and the moat is adjusted so that the longitudinal axis of the vehicle is almost perpendicular to the local center line of the moat, and then it goes straight across the moat. If the number of environmental obstacles is greater than the threshold, the diagonal crossing strategy ; r is the wheel rolling radius; When W≥2r, the oblique span strategy is adopted; The oblique-span strategy described includes three stages: In the first stage, start to cross the ditch, keep the included angle β and drive forward from the side A of the ditch to the side B. The front wheel w1 on one side crosses the ditch first. At this time, it must be ensured that when w1 does not touch the side B of the ditch, the front wheel on the other side will cross the ditch first. w2 is located on the side edge of the trench A and must not fall into the trench, ensuring β∈[arctan(B/L), arccos(W/B)]; In the second stage, w1 has already crossed the moat and landed on the B side, and w2 started to cross the moat. At this time, it is necessary to ensure that when w2 does not touch the B side of the moat, the rear wheel w3 on the diagonal line of the vehicle with w2 is located on the edge of the moat A side and must not sink into the moat. , guarantee β∈[arctan(B/L)+arcsin(W/sqrt(B ² +L ² )), 90°]; In the third stage, w2 has crossed the moat and landed on the B side, and w3 started to cross the moat. At this time, it is necessary to ensure that when w3 does not touch the B side of the moat, the rear wheel w4 on the diagonal line with w1 is located on the edge of the moat A side and must not sink into the moat. , the working conditions of the third stage are the same as the working conditions of the first stage, but the relative driving direction of the vehicle is opposite, so β satisfies the results of the first stage; Therefore, if β _per is not an empty set and _β∈βper , the planned path keeps the included angle β straight across the trench, otherwise, the planned path adjusts the included angle β to satisfy β∈β _per , and then the planned path keeps the included angle β straight. diagonally across the trench; If β _per is an empty set, plan a large-angle steering maneuver on the path, adjust the relative position of the vehicle and the trench in real time so that the included angle β meets the requirements of each stage in the process of crossing the trench, and cross the trench through multiple steering maneuvers; S4. When the judging module determines that the vehicle can cross the ditch, according to the formulated crossing strategy, combined with the performance parameters of the vehicle, formulate a path for crossing the ditch, and determine the waypoint and the speed of the waypoint; If the vertical leaping strategy is adopted, the planned path is steered in front of the trench to adjust the angle β to 90°±3°, and the environment perception unit detects that the angle β is within the range of 90°±3° and starts to go straight through the trench; When adopting the diagonal-span strategy, if the straight-line diagonal-span method is adopted, the planned path adjusts the included angle β to satisfy _β∈βper , and then the planned path maintains the included angle β to go straight and diagonally across the trench; In this way, the planned path turns to maneuver at a large angle, and the relative position of the vehicle and the trench is adjusted so that the included angle β meets the requirements of each stage in the process of crossing the trench, and the crossing is gradually completed; At the same time, the Reeds-Shepp curve is adopted to make the path shortest and satisfy the Ackerman steering model; The specific planning steps are as follows: (1) Judging the adopted spanning strategy and spanning method, and then select the critical path point, the critical path point includes the starting path point, the stage split point and the end point; When the vertical leaping strategy is adopted, the current position is selected as the starting path point, the estimated point of any front wheel overhang is the stage split point 1, the end point of any rear wheel overhang is the stage split point 2, and the position 3m away from the trench in the current route angle direction is the end point; The obstacle avoidance path point is selected by the algorithm according to the actual obstacle situation; When adopting the oblique spanning strategy, the starting and ending path points and obstacle avoidance path points are selected as above; when using the straight-line spanning method, the first front wheel suspension end point is selected as the stage split point 1, and the second front wheel suspension end point is selected as the stage split point 2 , the end point of the first rear wheel suspension is the stage split point 3; the step-by-step steering and spanning method is adopted, and the first front wheel suspension end point is selected as the stage split point 1, and the first rear wheel suspension start point is the stage split point 2 , the allowable passing point is stage split point 3, and the end point of the first rear wheel overhang is stage split point 4; among them, the estimated point of the front wheel overhang is the path point where the front wheel reaches the edge of the moat, and the end point of the rear wheel overhang is the rear crossover point. The moat is in contact with the path point on the edge of the moat, and the first and the second are divided in the order of time sequence when passing through the moat; (2) Path planning between critical path points; After determining the critical path point, between two adjacent critical path points, the allowable angle β of crossing the moat is used as the constraint condition, and the allowable angle β of crossing the moat between the next critical path point is the path planning target. In the vertical spanning strategy, the included angle β is in the range of 90°±3°, and in the oblique spanning strategy, the included angle β of the corresponding stage is within 2-3° of the critical value that can easily be reached; the critical path is defined by the Reeds-Shepp curve through the algorithm. The points are connected in series, and the planned path satisfies the Ackerman corner model of the vehicle; For no-solution planning, the reverse path planning and path planning time sequence superposition method are used to achieve the planning goal, then the steering and reverse driving planning is carried out, and the target value of the included angle β is reached by one or more forward and backward steering and moving vehicles superimposed; (3) The selection of the intermediate path point is related to the sampling frequency of the vehicle motion state and is multiplied, so as to meet the requirements of path tracking and path correction, so as to meet the path following accuracy; (4) The planned paths between the key path points are connected in series to generate path planning control instructions; S5. Control the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined waypoint and the speed of the waypoint, so as to control the vehicle to automatically cross the moat according to the predetermined moat crossing path. 7. trench identification according to claim 6 and spanning method, it is characterized in that: this method also comprises fault handling step, the fault information of each part of real-time monitoring vehicle, judges vehicle hardware and software fault according to monitoring information, according to corresponding fault level Take control of the vehicle. 8. trench identification according to claim 6 and spanning method, it is characterized in that: this method also comprises driving anti-skid step, when a certain electric wheel is about to arrive at trench edge, sends out limit torque command, prevents insufficient adhesion and electric The wheel slips, and at the same time avoids excessive torque output to damage the edge of the trench and increase the difficulty of crossing; when an electric wheel is about to hang above the trench, a zero torque output command is issued, so that the electric wheel hanging above the trench has no power output.

Images