CN109808509B

CN109808509B - Automatic identification and control system and method for crossing trench of unmanned off-road vehicle

Info

Publication number: CN109808509B
Application number: CN201910138854.9A
Authority: CN
Inventors: 付翔; 杨鹏; 杨凤举; 赵熙金; 刘道远; 吴森
Original assignee: Wuhan University of Technology WUT
Current assignee: Wuhan University of Technology WUT
Priority date: 2019-02-25
Filing date: 2019-02-25
Publication date: 2022-03-11
Anticipated expiration: 2039-02-25
Also published as: CN109808509A

Abstract

The invention provides an automatic identification and control system for crossing a trench by an unmanned off-road vehicle, which calculates the included angle between the center line of the trench and the longitudinal axis of the vehicle by combining vehicle position and attitude information through an image processing and information fusion method according to the information of obstacles around the vehicle; the judging module judges whether the vehicle can cross the trench or not according to the size information of the trench and the included angle between the trench center line and the vehicle longitudinal axis, and formulates a crossing strategy; when the judging module judges that the vehicle can cross the trench, the path planning module plans a path of the trench according to the formulated crossing strategy and determines path points and path point speeds; and 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 the path point speed, and controls the vehicle to automatically cross the trench according to a preset trench crossing path. The method judges whether the trench exists or not through image processing and information fusion technology, and sets a corresponding crossing strategy to ensure that the vehicle automatically crosses the trench within a certain width range.

Description

Automatic identification and control system and method for crossing trench of unmanned off-road vehicle

Technical Field

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

Background

The development of computer technology and sensor technology has promoted the research of unmanned car and the realization of unmanned system, and various driving assistance functions based on unmanned hardware platform have approached people's daily life, become the high-tech configuration of novel car product gradually, provide more convenient, high-efficient, safe driving experience for the human. The high-performance off-road vehicle is widely applied to the fields of resource exploration, jungle search and rescue, military operation, disaster search and rescue and the like, but the field terrain is complex, the types of obstacles are multiple, the attachment condition is poor, and various complex comprehensive off-road working conditions exist. When the vehicle runs in the field, the vehicle often encounters negative obstacles such as the trench, a driver can only observe the width of the trench, the positions of the peripheral obstacles and the relative positions of the vehicle and the trench by the naked eyes in the environment, and selects a passing mode or performs multiple attempts according to personal experience, so that the decision correctness and the operation feasibility are relatively low, the operation error is large, the efficiency is extremely low, and accidents are easily caused.

The patent 'a control method of unmanned all-terrain vehicle based on preview' provides a control method of unmanned all-terrain vehicle based on preview, and the all-terrain vehicle can make a strategy and preparation for passing obstacles in advance, change the speed and the driving direction in advance and adjust the hardness of a shock absorber. The thesis "obstacle crossing trafficability analysis and simulation research for trench of undersea mining vehicle" is to design an undersea mining vehicle connected by four sets of composite wheel set mechanisms and an articulated sealed pressure-resistant integral tank type frame, which consists of six sets of wheels, wherein the front three wheels are rigidly connected and converted into one wheel when crossing the trench, so as to cross the trench. However, the two methods have strict requirements on the structure of the vehicle for the negative obstacles such as the trench, the first method adopts a mode of swinging legs to stride, and the second method needs six groups of tires. The above patent and document do not relate to the strategy of crossing the trench for the four-wheel hub motor distributed drive vehicle, and do not relate to the detailed control method and steps of how to control the four-wheel vehicle crossing the trench. At present, the research on the fully automatic control method for the four-wheel vehicle crossing the trench is almost blank.

Disclosure of Invention

The technical problem to be solved by the invention is as follows: provides an automatic identification and control method for the trench crossing of an unmanned off-road vehicle.

The technical scheme adopted by the invention for solving the technical problems is as follows: an automatic identification and control system for a trench crossing of an unmanned off-road vehicle is characterized in that: provided in a four-wheel distributed drive electric vehicle, comprising:

the environment perception unit is used for acquiring obstacle information around the vehicle, obtaining obstacle sidelines in a vehicle perception range through an image processing and information fusion method, and identifying and calculating size information of the trench; meanwhile, vehicle motion state information including pose information is obtained, and an included angle between a trench center line and a vehicle longitudinal axis is calculated by combining trench information;

the decision unit comprises a judgment module, a path planning module and a path tracking module; the judgment module is used for judging whether the vehicle can cross the trench or not according to the size information of the trench, the included angle between the center line of the trench and the longitudinal axis of the vehicle and the size information of the vehicle, and formulating a crossing strategy; the path planning module is used for formulating a path crossing the trench according to the formulated crossing strategy and by combining the performance parameters of the vehicle when the judging module judges that the vehicle can cross the trench, and determining a path point and a path point speed; and the path tracking module is used for controlling the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined path point and the path point speed, so that the vehicle is controlled to automatically cross the trench according to a preset trench crossing path.

According to the scheme, when the environment sensing unit detects that the transverse strip-shaped negative barrier exists on the current path and the depth D of the negative barrier is larger than or equal to the rolling radius r of the wheel, the environment sensing unit judges that the trench is encountered; and calculating the width W of the trench, the included angle beta between the center line of the trench and the longitudinal axis of the vehicle and the distance s between the vehicle and the edge line of the trench.

According to the scheme, the judgment module specifically judges that the width W of the trench is smaller than the maximum width W of the trench which can be spanned by the vehicle_maxThen it is determined that the vehicle is able to cross the trench if the trench width W is greater than or equal to the maximum trench width W that the vehicle can cross_maxJudging that the vehicle cannot cross the current trench, and planning a path to bypass the trench; w_maxCalculated according to the following formula:

wherein B is the wheel base of the vehicle and L is the wheel base of the vehicle.

According to the scheme, the judgment module specifically makes a crossing strategy according to the following method:

when W is less than 2r, a vertical crossing strategy is adopted, namely the relative positions of the vehicle and the trench are adjusted to enable the longitudinal axis of the vehicle to be approximately vertical to the local central line of the trench, then the vehicle moves straight to cross the trench, and if the number of the environmental obstacles is greater than a threshold value, an inclined crossing strategy is switched to; r is the rolling radius of the wheel;

when W is larger than or equal to 2r, adopting an inclined span strategy;

the skew strategy comprises 3 stages:

the first stage, the trench is crossed, the angle beta is kept, the front wheel W1 on one side is driven forwards from the side of the trench A to the side B, and when the condition that W1 does not touch the side of the trench B is ensured, the front wheel W2 on the other side is positioned on the side edge of the trench A and cannot be sunk into the trench, and the beta is ensured to be (arctan (B/L) and arccos (W/B));

in the second stage, W1 lands on the side B after crossing the trench, W2 starts to cross the trench, and when ensuring that W2 does not touch the side B of the trench, the rear wheel W3 on the diagonal of the vehicle with W2 is positioned at the side edge of the trench A and cannot be sunk into the trench, and the condition that beta is in the form of [ arctan (B/L) + arcsin (W/sqrt (B/L) +²+L²))，90°]；

In the third stage, w2 lands on the side B after crossing over the trench, w3 starts to cross over the trench, and at the moment, when the w3 does not touch the side B of the trench, the rear wheel w4 on the diagonal line of the vehicle, which is the w1, is positioned on the side edge of the trench A and cannot fall into the trench, the working condition of the third stage is consistent with the working condition of the first stage, but the relative driving direction of the vehicle is opposite, so that the beta meets the result of the first stage;

therefore, if beta_perNon-empty set and beta ∈ beta_perThe planned path keeps the angle to go straight and obliquely across the trench, otherwise, the planned path adjusts the included angle beta to ensure that the included angle beta belongs to beta_perFurther planning a path to keep the angle to move straight and obliquely across the trench;

if beta is_perPlanning a path to carry out large-angle steering maneuver for the empty set, adjusting the relative positions of the vehicle and the trench in real time to enable the beta angles to respectively meet the requirements of each stage in the process of crossing the trench, and crossing the trench through multiple steering maneuvers.

According to the scheme, the path planning module carries out path planning under corresponding working conditions and strategies according to the result of the judging module:

if a vertical crossing strategy is adopted, the planned path is steered and maneuvered in front of the trench to adjust the beta angle to be within the range of 90 degrees +/-3 degrees, and the environment sensing unit detects that the beta angle is within the range of 90 degrees +/-3 degrees and then starts to move straight through the trench;

when the inclined span strategy is adopted, if a straight inclined span mode is adopted for spanning, the included angle beta is adjusted by planning a path, so that the included angle beta meets the requirement that beta belongs to beta_perFurther planning a path to keep the angle to move straight and obliquely across the trench; if a staged steering and inclined crossing mode is required, planning a path to steer and maneuver at a large angle, adjusting the relative positions of the vehicle and the trench to ensure that the beta angles respectively meet the requirements of each stage in the process of crossing the trench, and gradually finishing crossing;

meanwhile, the path planning module adopts a Reeds-Shepp curve to make the path shortest and meet the Ackerman steering model.

According to the scheme, the path planning module specifically comprises the following steps:

(1) judging the adopted crossing strategy and crossing mode, and further selecting key path points, wherein the key path points comprise initial path points, stage division points and end points;

when a vertical crossing strategy is adopted, selecting the current position as an initial path point, taking any front wheel suspension estimation point as a stage division point 1, taking any rear wheel suspension end point as a stage division point 2, and taking the position 3m away from the trench in the current fairway angle direction as an end point; selecting obstacle avoidance path points through an algorithm according to the actual obstacle condition;

when the inclined span strategy is adopted, the starting and stopping path points and the obstacle avoidance path points are selected as above; selecting a first front wheel suspension end point as a stage division point 1, a second front wheel suspension end point as a stage division point 2 and a first rear wheel suspension end point as a stage division point 3 in a straight crossing mode; selecting a first front wheel suspension end point as a stage division point 1, a first rear wheel suspension end point as a stage division point 2, a permitted point as a stage division point 3 and a first rear wheel suspension end point as a stage division point 4 by adopting a staged turning crossing mode; the first wheel suspension estimation point and the second wheel suspension estimation point are divided into a front wheel suspension estimation point and a rear wheel suspension end point, wherein the front wheel suspension estimation point is a path point of the front wheel reaching the edge of the trench, the rear wheel suspension end point is a path point of the rear wheel crossing the trench and contacting the edge of the trench, and the first wheel suspension estimation point and the second wheel suspension estimation point start suspension and end suspension division in time sequence when passing through the trench;

(2) planning the paths among the key path points;

after the key path points are determined, in the space between two adjacent key path points, the constraint condition of safely crossing the trench is used, namely the included angle beta allowed by crossing the trench is used as the constraint condition, the included angle beta allowed by crossing the trench between the next key path point is used as a path planning target, the target is a beta angle in the range of 90 degrees +/-3 degrees in a vertical crossing strategy, and the target is a critical value within which the beta angle allowed range of the corresponding stage is easy to reach in an oblique crossing strategy and is within 2-3 degrees; connecting key path points in series by using a Reeds-Shepp curve through an algorithm, wherein the planned path meets the Ackerman corner model of the vehicle;

and (3) adopting a reverse path planning and path planning time sequence superposition method for the non-solution planning to achieve a planning target, for example, when the steering driving under the single path planning can not adjust the vehicle to the range of the included angle beta of the constraint condition in the next stage when the trench is crossed, performing steering and back driving planning, and superposing the vehicle moving through one or more times of forward and backward steering to reach the target value of the included angle beta.

(3) The selection of the middle path point is related to the sampling frequency of the vehicle motion state and is multiplied, and the requirements of path tracking and path correction are met, so that the path following precision is met.

(4) And the planning paths among all the key path points are connected in series to generate a path planning control instruction and send the path planning control instruction to the path tracking module.

According to the scheme, the system further comprises a vehicle control unit, wherein the vehicle control unit comprises a driving anti-skidding module and a fault processing module; wherein,

the driving anti-skid module is used for sending a torque limiting instruction when a certain electric wheel is about to reach the edge of the trench, preventing the electric wheel from slipping due to insufficient adhesive force, and avoiding the phenomenon that the edge of the trench is damaged by excessive torque output to increase the crossing difficulty; when one electric wheel is about to hang above the trench, a zero-torque output instruction is sent out, so that the electric wheel hanging above the trench does not have power output;

and the fault processing module is used for judging the faults of the software and the hardware of the vehicle according to the monitoring information and controlling the vehicle according to the corresponding fault level.

A trench identification and crossing method realized by the automatic identification and control system for crossing trenches by using the unmanned off-road vehicle is characterized in that: the method comprises the following steps:

s1, when a transverse strip-shaped negative obstacle with the depth D larger than the rolling radius r of the wheel exists on the current path, automatically starting a trench crossing function, and limiting the speed of the vehicle to be below a preset low-speed threshold;

s2, carrying out information fusion calculation processing to obtain the width W of the trench, the included angle beta 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, judging whether the vehicle can cross the trench or not according to the size information of the trench, the included angle between the center line of the trench and the longitudinal axis of the vehicle and the size information of the vehicle, and making a crossing strategy;

3.1, judging that the width W of the trench is smaller than the maximum trench width W which can be spanned by the vehicle_maxThen it is determined that the vehicle is able to cross the trench if the trench width W is greater than or equal to the maximum trench width W that the vehicle can cross_maxJudging that the vehicle cannot cross the current trench, and planning a path to bypass the trench; w_maxCalculated according to the following formula:

wherein B is the wheel track of the vehicle, and L is the wheel base of the vehicle;

3.2, when judging that the vehicle can cross the trench, making a crossing strategy according to the following method:

when W is larger than or equal to 2r, adopting an inclined span strategy;

the skew strategy comprises 3 stages:

therefore, if beta_perNon-empty set and beta ∈ beta_perThe planned path keeps the angle to go straight and obliquely across the trench, otherwise, the planned path adjusts the included angle beta to ensure that the included angle beta belongs to beta_perFurther planning the path to keep the straight-going and inclined-crossing trench of the angle；

If beta is_perPlanning a path to carry out large-angle steering maneuver for the empty set, adjusting the relative positions of the vehicle and the trench in real time to enable the beta angles to respectively meet the requirements of each stage in the process of crossing the trench, and crossing the trench through multiple steering maneuvers;

s4, when the judgment module judges that the vehicle can cross the trench, according to the established crossing strategy and in combination with the performance parameters of the vehicle, establishing a path of crossing the trench, and determining a path point and a path point speed;

meanwhile, a Reeds-Shepp curve is adopted to enable the path to be shortest and meet the Ackerman steering model;

the specific planning steps are as follows:

(2) planning the paths among the key path points;

(4) The planning paths among all the key path points are connected in series to generate a path planning control instruction;

and S5, controlling the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined path point and the determined path point speed, so as to control the vehicle to automatically cross the trench according to the preset path for crossing the trench.

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

According to the method, the method also comprises a driving anti-skidding step, when one electric wheel is about to reach the edge of the trench, a torque limiting command is sent out to prevent the electric wheel from skidding due to insufficient adhesive force, and the phenomenon that the edge of the trench is damaged by overlarge torque output to increase the crossing difficulty is avoided; when one electric wheel is about to hang above the trench, a zero-torque output instruction is sent out, so that the electric wheel hanging above the trench does not have power output.

The invention has the beneficial effects that: whether the trench exists is judged through image processing and information fusion technology, and corresponding crossing strategies are set according to different situations, so that the vehicle can automatically cross the trench within a certain width range, and the adaptability to complex and variable field environments is good.

Drawings

Fig. 1 is a system component and an information flow diagram according to an embodiment of the present invention.

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

Fig. 3 is a schematic diagram illustrating installation of a laser radar and detection of a protruding obstacle.

Fig. 4 is a diagram of laser radar measurement trench (negative obstacle) parameters.

FIGS. 5(a), (b), and (c) are schematic views of the vehicle at three stages of crossing the trench.

FIG. 6 is a schematic diagram of a phased steering across critical path points and paths.

Detailed Description

The invention is further illustrated by the following specific examples and figures.

The invention provides an automatic identification and control system for a trench crossing of an unmanned off-road vehicle, which is arranged in a four-wheel drive electric vehicle and comprises the following components as shown in figure 1:

and the environment sensing unit is used for acquiring the obstacle information around the vehicle. The environment sensing unit comprises a sensor, a vehicle parameter acquisition module and a trench identification and parameter extraction module.

The sensor includes laser radar, a plurality of ultrasonic sensor and binocular camera. The laser radar is arranged at the higher positions of the front part and the rear part of the vehicle as active vision to collect main obstacle information; 4 ultrasonic radars are arranged on the left side and the right side of the front bumper and the rear bumper and mainly used for collecting information of other surrounding obstacles; the binocular camera is used for forming vehicle environment vision.

And the vehicle parameter acquisition module is used for acquiring vehicle running state parameters and mainly acquiring the vehicle running state parameters from the whole vehicle controller.

And the trench identification and parameter extraction module is used for obtaining the boundary line of the obstacles in the vehicle sensing range through image processing and information fusion methods according to the information of the obstacles around the vehicle, identifying and calculating the size information of the trench, and calculating the included angle between the center line of the trench and the longitudinal axis of the vehicle by combining the position and posture information of the vehicle. When the environment sensing unit detects that a transverse strip-shaped negative barrier exists on the current path and the depth D of the negative barrier is greater than or equal to the rolling radius r of the wheel, the environment sensing unit judges that the trench is encountered; and calculating the width W of the trench, the included angle beta between the center line of the trench and the longitudinal axis of the vehicle and the distance s between the vehicle and the edge line of the trench.

The length of the vehicle with the built-in judgment module is I, the width of the vehicle is w, the wheel track is B, the wheel base is L, the rolling radius r of the wheel, the diameter d of the wheel and the minimum rotation of the vehicleRadius of curvature R_minMaximum turning angle alpha of steering wheel_maxJudging that the width W of the trench is smaller than the maximum trench width W which can be spanned by the vehicle_maxThen it is determined that the vehicle is able to cross the trench if the trench width W is greater than or equal to the maximum trench width W that the vehicle can cross_maxJudging that the vehicle cannot cross the current trench, and planning a path to bypass the trench; w_maxCalculated according to the following formula:

The judgment module specifically makes a crossing strategy according to the following method:

when W is larger than or equal to 2r, adopting an inclined span strategy;

the skew strategy comprises 3 stages:

And the path planning module performs path planning under corresponding working conditions and strategies according to the result of the judging module:

The path planning module specifically plans the steps as follows:

(2) planning the paths among the key path points;

The direction control function is designed according to a pure tracking algorithm, the actual running track of the vehicle is a section of circular arc, meanwhile, a straight path can be seen as a circular arc with an infinite radius, the speed of the vehicle is considered to be constant within a short time, and once a steering angle with a proper size is planned, the reference path can be accurately tracked. When the vehicle crosses the trench, the speed is very low, a closer forward-looking path point on the path is selected for tracking, and a tracking error which generates two errors and is the distance is called as a transverse error, wherein the error is the absolute distance between the position of the vehicle and an expected path; another error is the angle between the longitudinal axis of the vehicle and the tangent of a point on the reference path is the heading error. And controlling the real-time steering angle of the vehicle by adopting a fuzzy controller so as to enable the error to trend to 0 as much as possible.

The speed control function receives a path point speed expected value instruction of path planning, the instruction changes and corrects in real time along with the change of working conditions and path points, and the speed control function takes the current speed of the vehicle as feedback information to carry out closed-loop control on the torque output of the four-hub motor and the pressure of a brake system. Based on the experience and process of human speed control on vehicles, a set of incremental fuzzy PID controllers are respectively adopted for driving and braking a hub motor and a braking system, the driving and the braking do not work simultaneously, when the current speed is greater than a certain value of a command speed, the driving control flag bit is stopped, the driving PID control is skipped, and the braking PID control algorithm is started, and conversely, when the current speed is less than the certain value of the command speed, the driving PID control algorithm is started.

The system also comprises a vehicle controller, wherein a Vehicle Control Unit (VCU) is used as a vehicle bottom layer control unit, comprises a functional control strategy and a bottom layer driving strategy, and is divided into a driving anti-skid module, a fault processing module and other control modules. Since the invention of this patent in the VCU section is mainly focused on driving the antiskid and fault handling modules, the other control modules are not described in detail. The anti-skid module is driven to ensure that the electric wheel suspended above the trench or with insufficient adhesive force flies and rotates in the process of crossing the trench by the vehicle; the fault processing module is responsible for monitoring and analyzing the working state of each unit of the system, and processes the fault when the fault occurs, so that the safety of the whole trench crossing process is ensured.

The driving anti-skid module can judge the adhesion condition of the wheels in advance according to the real-time environment sensing information, the planned path, the path point where the vehicle is located, the vehicle pose and the motion parameters, so that a corresponding control strategy is adopted to prevent the electric wheels from slipping. When the system judges that a certain electric wheel is about to reach the edge of the trench, the driving anti-skid unit sends out a proper torque limiting instruction to prevent the electric wheel from slipping due to insufficient adhesive force and avoid the phenomenon that the edge of the trench is damaged by excessive torque output to increase the crossing difficulty; when the system judges that the electric wheel is about to hang above the trench, the anti-skid module is driven to send out a zero-torque output instruction, so that the electric wheel hanging above the trench is in unpowered output; meanwhile, the drive anti-skid module also has a conventional drive anti-skid function, and when the system detects that any electric wheel has slip, the system sends out corresponding torque limit to ensure that the wheel output torque in a non-suspended state drives the vehicle to cross the trench.

The fault processing module has the functions of fault monitoring, fault grading processing, limping control and the like, the faults of software and hardware of the vehicle are judged according to the monitoring information, the faults are divided into three stages, the high-voltage electricity is immediately cut off/the running is stopped by the first-stage fault, the power output and the running speed are limited by the second-stage fault, low-speed limping is carried out, and the third-stage fault is used for alarming and prompting but normally running.

A method for identifying and crossing a trench by using the system for automatically identifying and controlling a crossing trench of an off-road unmanned vehicle as shown in fig. 2 comprises the following steps:

s1, when a transverse strip-shaped negative obstacle with the depth D larger than the rolling radius r of the wheels exists on the current path, automatically starting a trench crossing function, and limiting the speed of the vehicle to be below a preset low-speed threshold (10 km/h); and then, fault information confirmation is carried out, the fault information of each system of the current vehicle is read, and the current fault or the fault which does not influence the safety of the cross trench is confirmed.

The detection of an obstacle is described as follows: the lidar is mounted on the roof of the vehicle with the scan line inclined downwardly as shown in figure 3. In the figure, h is the height of the laser radar, p is the lowest height of the obstacle to be detected, and gamma is the pitch angle of the laser radar. Protruding objects 0.4m above the ground are determined as protruding obstacles, a depressed area of the road surface with a depth greater than 0.4m and a wheel rolling radius r is determined as negative obstacles, and a transverse strip-shaped negative obstacle with a depth D greater than the wheel rolling radius r triggers an automatic trench crossing function.

In the actual detection of the trench, the distances of the discontinuous adjacent masses in the horizontal and vertical directions are mainly detected. If the distance is greater than a given threshold, it is an indication that a moat obstacle is detected. The distances in the horizontal and vertical directions are now defined as the moat width W and depth D, respectively. As shown in FIG. 4, h_hdlDetermining the installation height of the laser radar, wherein theta is the maximum depression angle of a ray emitted by the laser radar, d is the horizontal distance between the ground reflection point of the maximum depression angle ray and the laser radar, gamma is the included angle between any laser ray and the maximum depression angle laser ray, delta d is the distance between any laser ray and the maximum depression angle ray reflection point on the flat ground, the included angle between two adjacent laser rays is delta theta, and then defining the distance between two adjacent laser reflection points scanned on the flat ground as delta d_idealWhen the trench exists, the distance between the reflection point of the light beam entering the trench and the reflection point of the adjacent storefront is increased to delta d_true. The presence of a negative obstacle can be detected by the change in deltad caused by the discontinuous adjacent masses and an estimate of the depth of the negative obstacle can be made.

S2, acquiring detailed environment information by sensors such as laser radar and the like, and performing information fusion calculation processing by the information processing module to obtain the local width W of the trench, the included angle beta (less than or equal to 90 degrees) 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. The method specifically comprises the following steps:

2.1, establishing a plane coordinate system by taking the position of the current automatic trench crossing function as a coordinate origin, the longitudinal direction of the automobile as an X axis and the transverse direction of the automobile as a Y axis;

2.2, processing environment information sensed by the radar and the camera according to a pre-input algorithm, calculating and identifying the outline and the direction of the trench, processing an image to fit the edge line of the trench, and establishing a parameterized edge and position of the trench in the plane coordinate system; calculating and identifying other protruding obstacles around the trench, and representing the obstacles in a coordinate system;

2.3, according to vehicle running state parameters fed back by the vehicle controller, including the current running speed v, the running time t and the steering wheel rotation angle alpha, calculating the relative position of the current vehicle to be tested and the trench by combining the trench parameters, and carrying out parametric representation in a coordinate system to obtain key parameters required by the judgment module and the path planning module: the vehicle is at a distance s and β from the trench edge line.

And S3, judging whether the vehicle can cross the trench or not according to the size information of the trench, the included angle between the center line of the trench and the longitudinal axis of the vehicle and the size information of the vehicle, and making a crossing strategy.

3.1, the basic requirement for safe crossing of the trench ensures that at least three wheels of the vehicle land (i.e. do not overhang the trench) at any time. W_maxWhen the vehicle obliquely crosses the trench, the wheels on the diagonal line of the vehicle are positioned at the edge of the trench at the same time and continue to cross the trench, no matter how the vehicle is operated, the two wheels on the diagonal line are simultaneously suspended on the trench, and the crossing of the trench fails, namely the limit value of the vehicle crossing the trench at this moment

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

When W is more than or equal to W_maxAnd when the vehicle cannot cross the current trench, the planned path bypasses the trench and the automatic trench crossing function is exited.

When W is less than W_maxThe vehicle is able to cross the current trench.

when W is larger than or equal to 2r, adopting an inclined span strategy;

the skew strategy comprises 3 stages, as shown in fig. 5:

the first stage, the trench is crossed, the angle beta is kept, the front wheel w1 on one side is driven forwards from the side of the trench A to the side B, the front wheel w2 on the other side is ensured not to be sunk into the trench when the side w1 does not touch the side of the trench B, and the angle beta is obtained by geometric analysis

B×cosβ≥W (2)

β≤arccos(W/B) (3)

The limit angle beta allowed at this time_per1Arccos (W/B) occurs at W1 at the side edge of trench a and W2 just at the side edge of trench B. On the other hand, β should be greater than W_maxThe angle at which the wheels on the diagonal of the vehicle are at the edge of the trench at the same time, beta being satisfied by geometric analysis

β＞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 landed on side B across the trench, w2 begins to cross the trench, and when it is ensured that w2 does not touch side B of the trench, the rear wheel w3 on the diagonal of the vehicle is located on the side edge of trench A and cannot be sunk into the trench, and beta is satisfied by geometric analysis

β≥∠1+∠2，

Namely, it is

At the allowable limit angle beta

Occurs when w3 is located at the side edge of trench a and w2 is located just above trench BSide edges.

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

in summary, the allowable β range β of the trench width is determined when the vehicle is inclined straight across the width_per＝[arctan(B/L)+arcsin(W/sqrt(B²+L²))，arccos(W/B)]If not, if β ∈ β_perThe planned path keeps the angle to go straight and obliquely across the trench, otherwise, the planned path adjusts the included angle beta to ensure that the included angle beta belongs to beta_perFurther planning a path to keep the angle to move straight and obliquely across the trench; when beta is_perWhen the set is empty, the planned path adjustment beta is needed to respectively meet the safe crossing angle ranges required by the three crossing stages.

Beta is as defined above_perNot empty set, beta does not satisfy beta_perThe reason why the program does not enter the steering inclined span program is that the straight inclined span passing efficiency is high, the time of the straight inclined span passing above the trench is short, the number of vehicle operation control programs is small, and the safety is guaranteed to be higher.

If beta is_perNon-empty set and beta ∈ beta_perThe planned path keeps the angle to go straight and obliquely across the trench, otherwise, the planned path adjusts the included angle beta to ensure that the included angle beta belongs to beta_perFurther planning a path to keep the angle to move straight and obliquely across the trench;

And the judging unit sends the judging result, namely the strategy of the vehicle crossing the trench obtained through calculation and logic judgment, to the path planning module.

S4, when the judgment module judges that the vehicle can cross the trench, according to the established crossing strategy and by combining the vehicle performance parameters, establishing a path for crossing the trench, and determining a path point and a path point speed; if a vertical crossing strategy is adopted, the planned path is steered and maneuvered in front of the trench to adjust the beta angle to be within the range of 90 degrees +/-3 degrees, and the environment sensing unit detects that the beta angle is within the range of 90 degrees +/-3 degrees and then starts to move straight through the trench; when the inclined span strategy is adopted, if a straight inclined span mode is adopted for spanning, the included angle beta is adjusted by planning a path, so that the included angle beta meets the requirement that beta belongs to beta_perFurther planning a path to keep the angle to move straight and obliquely across the trench; if a staged steering and inclined crossing mode is needed, a path is planned to steer and maneuver at a large angle, and the relative positions of the vehicle and the trench are adjusted to ensure that the beta angles respectively meet the requirements of each stage in the process of crossing the trench, so that the crossing is gradually completed.

The path planning has the following points:

4.1, the path plan under the control method is under the global path plan of the unmanned vehicle, is different from the local path plan, and is a special working condition path plan. The planning method is based on geometric planning, namely relative positions of four wheels and the trench and the constraint condition of safe crossing of the trench, the path adopts a Reeds-Shepp curve to be shortest, and an Ackerman steering model is met, so that passing efficiency and an adjustment space of an included angle beta required by a reserved staged steering crossing mode are ensured.

And 4.2, planning the path, including path points (or paths) and ideal speeds (including stopping and starting) of the path points. The path points include a starting path point, an obstacle avoidance path point, a dividing point of the trench crossing stage (i.e. a path point estimated according to a coordinate system and a vehicle motion state that a certain wheel starts to hang above the trench or finishes hanging), a passing point (which satisfies the constraint condition of safe trench crossing in the second stage when the path point is located), an intermediate path point (a path point which is inserted between adjacent special path points and is convenient to track), and an end point. The ideal speed of the path point is planned as follows: the ideal speed of the path point in the whole process is lower than 10 km/h; the speed of the intermediate path point between the stage division points and the adjacent stage division points is limited below 5 km/h; the speeds of the starting point, the end point and the other path points are kept at the current speed, and special control is not carried out; starting and stopping adopt a linear speed plan, namely, starting acceleration and braking deceleration are set to be small constant values.

4.3, basic steps of path planning:

(1) and judging the adopted crossing strategy and crossing mode, and further selecting the key path point.

And adopting a vertical crossing strategy, selecting the current position as an initial path point, taking any front wheel suspension estimation point (namely the path point of the front wheel reaching the edge of the trench) as a stage division point 1, taking any rear wheel suspension end point (namely the path point of the rear wheel crossing the trench and contacting the edge of the trench) as a stage division point 2, and taking the position of the current navigation angle direction 3m away from the trench as an end point. And selecting the obstacle avoidance path points through an algorithm according to the actual obstacle condition. When the oblique crossing strategy is adopted, as shown in fig. 6, the starting and stopping path points and the obstacle avoidance path points are selected as above; selecting a first front wheel suspension end point as a stage division point 1, a second front wheel suspension end point as a stage division point 2 and a first rear wheel suspension end point as a stage division point 3 in a straight crossing mode; and selecting a first front wheel suspension end point as a stage division point 1, a first rear wheel suspension end point as a stage division point 2, a permitted point as a stage division point 3 and a first rear wheel suspension end point as a stage division point 4 by adopting a staged turning crossing mode.

Note: the first wheel and the second wheel are wheels which start to suspend and finish suspending in sequence when passing through the trench.

(2) And planning the path between the key path points.

After the key path points are determined, the constraint condition of safely crossing the trench between two adjacent key path points, namely the included angle beta allowed by crossing the trench is taken as the constraint condition, the included angle beta allowed by crossing the trench between the next key path point is taken as a path planning target, the target is that the beta angle is within the range of 90 degrees +/-3 degrees in the vertical crossing strategy, and the target is within the range of 2-3 degrees of the critical value easily reached by the beta angle in the corresponding stage in the oblique crossing strategy. And connecting the key path points in series by using a Reeds-Shepp curve through an algorithm, wherein the planned path meets the Ackerman corner model of the vehicle. And (3) adopting a reverse path planning and path planning time sequence superposition method for the non-solution planning to achieve a planning target, for example, when the steering driving under the single path planning can not adjust the vehicle to the range of the included angle beta of the constraint condition in the next stage when the trench is crossed, performing steering and back driving planning, and superposing the vehicle moving through one or more times of forward and backward steering to reach the target value of the included angle beta.

And 4.4, tracking the current trench by fusing trench coordinate information and vehicle kinematic information, and correcting a course angle, namely a path planning result. Errors caused by path tracking and chassis actuator control are reduced by a method of planning and correcting for multiple times, so that the planned path is in a real-time tracking and correcting state. And selecting the intermediate path point through an algorithm according to the path tracking requirement.

And S5, controlling the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined path point and the determined path point speed, so as to control the vehicle to automatically cross the trench according to the preset path for crossing the trench. The direction control function and the speed control adopt closed-loop control, and the algorithm and the control principle are as described above. The direction control function outputs steering angle adjustment quantity through a fuzzy control algorithm to control the vehicle to run along an ideal track; the speed control function outputs acceleration and braking signals and gear signals through a fuzzy PID algorithm, and the vehicle speed is controlled to be as close to an ideal vehicle speed as possible.

The control signals are equivalent to driver signals and are sent to the whole vehicle control unit by the decision unit, the whole vehicle control unit serves as a bottom driver, corresponding actuators (a steering system, a braking system, a four-hub motor driving system and the like) are controlled to execute control instructions after the signals are received, and finally vehicles cross the trench according to the path. In the whole running process of the vehicle, a driving antiskid module in a whole vehicle control unit controls the driving state of the four-hub motor in real time to prevent the occurrence of slip and even fly; and when the fault processing module is in a real-time working condition, after the trench crossing function is started, firstly confirming fault information of each system of the vehicle, then monitoring the fault information in real time, and processing faults generated in the process of crossing the trench in time to ensure the safety of crossing the trench.

The advantages of the invention are mainly embodied in the following aspects:

(1) the environment sensing module consisting of the sensors such as the laser radar, the camera and the like arranged at the front and the rear of the vehicle body detects obstacles in the advancing process of the vehicle, and well solves the problems that the traditional off-road vehicle driver has limited visual field, large blind area, inaccurate distance sensing, lost visual field under severe weather conditions and the like. The accurate position of the barrier, the specific shape parameters of the trench and the relative positions of the wheels and the trench when the trench is crossed can be obtained by performing fusion calculation on information acquired by sensors such as a radar and a camera, and the control unit can perform reasonable analysis and decision to make an optimal control strategy. The method has good adaptability to complex and variable field environments.

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

(3) The control unit plans a path according to the key environment information, formulates a control strategy of the best trench, controls the vehicle to execute a control instruction to realize path tracking, realizes accurate control and can automatically adjust in real time according to the information in the driving process. The vehicle can cross the trench in an optimal mode, the barrier passing time is saved, and the efficiency is improved.

(4) The invention relates to a distributed driving off-road vehicle, wherein each wheel of the vehicle is directly driven by a motor, the torque of the vehicle is independently controllable, the vehicle can be flexibly and coordinately controlled, differential power-assisted steering is realized, the minimum turning radius of the vehicle in off-road is reduced, the maneuverability and flexibility of the off-road vehicle in the field are improved to a great extent, the attitude adjustment of the vehicle is facilitated, and the passing efficiency is improved.

(5) The control system and the control method can be transplanted and copied on the distributed driving vehicles with the same level of hardware facilities, have portability and duplicability, and can be widely applied to the distributed driving off-road vehicles with the same level of hardware.

The above embodiments are only used for illustrating the design idea and features of the present invention, and the purpose of the present invention is to enable those skilled in the art to understand the content of the present invention and implement the present invention accordingly, and the protection scope of the present invention is not limited to the above embodiments. Therefore, all equivalent changes and modifications made in accordance with the principles and concepts disclosed herein are intended to be included within the scope of the present invention.

Claims

1. An automatic identification and control system for a trench crossing of an unmanned off-road vehicle is characterized in that: provided in a four-wheel distributed drive electric vehicle, comprising:

the environment perception unit is used for acquiring obstacle information around the vehicle, obtaining obstacle sidelines in a vehicle perception range through an image processing and information fusion method, and identifying and calculating size information of the trench; meanwhile, vehicle motion state information including pose information is received, and an included angle between a trench center line and a vehicle longitudinal axis is calculated by combining trench information;

the decision unit comprises a judgment module, a path planning module and a path tracking module; the judgment module is used for judging whether the vehicle can cross the trench or not according to the size information of the trench, the included angle between the center line of the trench and the longitudinal axis of the vehicle and the size information of the vehicle, and formulating a crossing strategy; the path planning module is used for formulating a path crossing the trench according to the formulated crossing strategy and by combining the performance parameters of the vehicle when the judging module judges that the vehicle can cross the trench, and determining a path point and a path point speed; the path tracking module is used for controlling the steering angle of the steering wheel and the torque of the driving wheel in real time according to the determined path point and the path point speed, so that the vehicle is controlled to automatically cross the trench according to a preset trench crossing path;

the judgment module specifically judges that the width W of the trench is smaller than the maximum width W of the trench which can be spanned by the vehicle_maxThen it is determined that the vehicle is able to cross the trench if the trench width W is greater than or equal to the maximum trench width W that the vehicle can cross_maxJudging that the vehicle cannot cross the current trench, and planning a path to bypass the trench; w_maxCalculated according to the following formula:

when W is less than 2r, a vertical crossing strategy is adopted, namely the relative positions of the vehicle and the trench are adjusted to ensure that the longitudinal axis of the vehicle is vertical to the local central line of the trench, and then the vehicle is moved straight to cross the trench, wherein the vertical allowance is less than 3 degrees of deviation; if the environmental barrier distribution causes no straight crossing path, switching to an inclined crossing strategy; r is the rolling radius of the wheel;

when W is larger than or equal to 2r, adopting an inclined span strategy;

the skew strategy comprises 3 stages:

the first stage, the trench starts to be spanned, an included angle beta is kept, the front wheel W1 on one side is spanned to the trench from the trench A side to the trench B side, at the moment, when it is ensured that W1 does not touch the trench B side, the front wheel W2 on the other side is positioned on the side edge of the trench A and cannot be sunk into the trench, and the beta is ensured to be in the form of [ arctan (B/L) and arccos (W/B) ]; beta is the included angle between the center line of the trench and the longitudinal axis of the vehicle;

therefore, if beta_perNon-empty set and beta ∈ beta_perThe planned path keeps the included angle beta to move straightly and obliquely across the trench, otherwise, the planned path adjusts the included angle beta to ensure that the included angle beta belongs to beta_perFurther planning a path to keep an included angle beta to move straight and obliquely across the trench;

if beta is_perPlanning a path to carry out large-angle steering maneuver for the empty set, adjusting the relative positions of the vehicle and the trench in real time to enable the included angle beta to respectively meet the requirements of each stage in the process of crossing the trench, and crossing the trench through multiple steering maneuvers.

2. The system of claim 1, wherein: when the environment sensing unit detects that a transverse strip-shaped negative barrier exists on the current path and the depth D of the negative barrier is greater than or equal to the rolling radius r of the wheel, the environment sensing unit judges that the trench is encountered; and calculating the width W of the trench, the included angle beta between the center line of the trench and the longitudinal axis of the vehicle and the distance s between the vehicle and the edge line of the trench.

3. The system of claim 1, wherein: the path planning module carries out path planning under corresponding working conditions and strategies according to the result of the judging module:

if a vertical crossing strategy is adopted, the planned path is steered and maneuvered in the range of the included angle beta to 90 degrees +/-3 degrees in front of the trench, and the environment sensing unit detects that the included angle beta is in the range of 90 degrees +/-3 degrees and then starts to move straight through the trench;

when the inclined span strategy is adopted, if a straight inclined span mode is adopted for spanning, the included angle beta is adjusted by planning a path, so that the included angle beta meets the requirement that beta belongs to beta_perFurther planning a path to keep an included angle beta to move straight and obliquely across the trench; if a staged steering inclined span mode is required, planning a path and steering maneuver at a large angle, and adjusting the vehicleThe included angles beta respectively meet the requirements of each stage in the process of crossing the trench by the relative position of the included angles beta and the trench, and the crossing is gradually completed;

meanwhile, the path planning module adopts a Reeds-Shepp curve to enable the path to be shortest, and takes an Ackerman steering model as a constraint condition.

4. The system of claim 3, wherein: the path planning module concretely comprises the following planning steps:

(2) planning the paths among the key path points;

after the key path points are determined, taking an included angle beta allowed by crossing the trench between two adjacent key path points as a constraint condition, and taking the included angle beta allowed by crossing the trench between the next key path point as a path planning target, wherein the target is within a range of 90 degrees +/-3 degrees for the included angle beta in a vertical crossing strategy, and is within 2-3 degrees of a critical value which is easily reached by the included angle beta allowed range of a corresponding stage in an oblique crossing strategy; connecting key path points in series by using a Reeds-Shepp curve through an algorithm, wherein the planned path meets the Ackerman corner model of the vehicle;

if the non-solution planning adopts a reverse path planning and path planning time sequence superposition method to reach a planning target, the steering and backward driving planning is carried out, and the target value of the included angle beta is reached through one or more times of forward and backward steering vehicle moving superposition;

(3) the selection of the middle path point is related to the sampling frequency of the vehicle motion state and is multiplied, and the requirements of path tracking and path correction are met, so that the path following precision is met;

5. The system of any of claims 1 to 4, wherein: the system also comprises a vehicle control unit, wherein the vehicle control unit comprises a driving anti-skid module and a fault processing module; wherein,

6. A trench identification and crossing method implemented using the automated trench crossing identification and control system of an unmanned off-road vehicle of claim 1, wherein: the method comprises the following steps:

when W is larger than or equal to 2r, adopting an inclined span strategy;

the skew strategy comprises 3 stages:

the first stage, the trench starts to be spanned, an included angle beta is kept, the front wheel W1 on one side is spanned to the trench from the trench A side to the trench B side, at the moment, when it is ensured that W1 does not touch the trench B side, the front wheel W2 on the other side is positioned on the side edge of the trench A and cannot be sunk into the trench, and the beta is ensured to be in the form of [ arctan (B/L) and arccos (W/B) ];

if beta is_perPlanning a path to carry out large-angle steering maneuver for an empty set, adjusting the relative positions of the vehicle and the trench in real time to enable the included angle beta to respectively meet the requirements of each stage in the process of crossing the trench, and crossing the trench through multiple steering maneuvers;

when the inclined span strategy is adopted, if a straight inclined span mode is adopted for spanning, the included angle beta is adjusted by planning a path, so that the included angle beta meets the requirement that beta belongs to beta_perFurther planning a path to keep an included angle beta to move straight and obliquely across the trench; if a staged steering and inclined crossing mode is required, planning a path to steer at a large angle to maneuver, adjusting the relative positions of the vehicle and the trench to enable the included angle beta to respectively meet the requirements of each stage in the process of crossing the trench, and gradually finishing crossing;

the specific planning steps are as follows:

(2) planning the paths among the key path points;

7. The moat identification and spanning method of claim 6, wherein: the method also comprises a fault processing step, namely monitoring the fault information of each part of the vehicle in real time, judging the faults of software and hardware of the vehicle according to the monitoring information, and controlling the vehicle according to the corresponding fault level.

8. The moat identification and spanning method of claim 6, wherein: the method also comprises a driving anti-skid step, wherein when one electric wheel is about to reach the edge of the trench, a torque limiting instruction is sent out to prevent the electric wheel from slipping due to insufficient adhesive force, and the phenomenon that the edge of the trench is damaged by excessive torque output to increase the crossing difficulty is avoided; when one electric wheel is about to hang above the trench, a zero-torque output instruction is sent out, so that the electric wheel hanging above the trench does not have power output.