A method and system for autonomous navigation and obstacle avoidance of a field unmanned vehicle

Abstract

The application provides a kind of field unmanned vehicle autonomous navigation and obstacle avoidance method and system, the method comprises: real-time acquisition hub motor driven bottom signal;Based on bottom signal and Bekker soil mechanics model, the state space equation between driving current, wheel speed and yaw angular velocity deviation and adhesion coefficient and bearing modulus is established;Real-time estimation value of adhesion coefficient is obtained by solving through extended Kalman filtering;The real-time estimation value is used to construct physical passability label map;Based on tracking error cost, control quantity smoothing cost and stability margin cost, a target function is constructed, combined with vehicle kinematics and dynamics model and real-time estimation value to establish model predictive controller;When the real-time estimation value in front of prediction is lower than safety threshold, search alternative path and adjust front wheel steering angle and hub motor torque distribution.The application reduces the risk of unmanned vehicle slipping, sinking or overturning in soft and wet terrain, improves autonomous traffic capacity and safety and reliability.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a method and system for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild. Background Technology

[0002] With the continuous evolution of intelligent technologies in the high-end equipment manufacturing sector and the expanding scope of industrial applications, the demand for autonomous operation of unmanned vehicles (UAVs) in diverse and complex unstructured environments such as mineral exploration, military reconnaissance, agricultural plant protection, disaster relief, and border patrol is becoming increasingly urgent. Compared with traditional structured roads, unstructured terrain in the wild is characterized by varied road surface materials, uneven load-bearing capacity, and significant differences in adhesion conditions, posing more stringent technical requirements for the autonomous navigation and obstacle avoidance capabilities of UAVs. Autonomous navigation and obstacle avoidance capabilities, as the core foundational technologies for the efficient and reliable autonomous operation of UAVs, directly determine their mission completion and survivability in complex wild environments, and have always been a hot topic in cutting-edge research and a key engineering challenge.

[0003] Currently, existing navigation and obstacle avoidance systems for unmanned vehicles in the wild typically use lidar, cameras, or millimeter-wave radar to perceive rigid obstacles in the environment and plan geometric travel paths by constructing an occupation grid map. For unmanned vehicles driven by four-wheel hub motors, their control system simultaneously collects the drive current and wheel speed signals of the hub motors to achieve motor torque control and closed-loop speed regulation. However, geometric-based perception methods cannot acquire physical and mechanical parameters such as the ground adhesion coefficient. This leads to the misjudgment of geometrically open but physically impassable surfaces, such as soft sand and muddy areas, as passable areas and guides vehicles into them. At the same time, the underlying signals such as drive current and wheel speed are only used for the motor drive closed loop and lack information interaction with the upper-level planning module, failing to provide ground mechanical state references for navigation and obstacle avoidance. Since the upper-level controller uses pure geometric paths as input, it lacks perception and constraints on the actual bearing capacity of the ground and still outputs drive commands according to conventional adhesion conditions. The wheel-end drive torque far exceeds the adhesion limit that soft and slippery ground can provide, causing unmanned vehicles to frequently slip, get stuck, or overturn in soft, slippery, and other unstructured terrains in the wild, resulting in poor autonomous passage capability and safety reliability. Summary of the Invention

[0004] The purpose of this invention is to provide an autonomous navigation and obstacle avoidance method and system for unmanned vehicles in the wild, to solve the problem that unmanned vehicles in the prior art frequently slip, get stuck or overturn in soft, slippery or other unstructured terrain in the wild, resulting in poor autonomous passage ability and safety reliability.

[0005] To address the aforementioned technical problems, the embodiments of the present invention provide the following technical solutions: The first aspect of this invention provides a method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild, comprising: Real-time acquisition of underlying signals driven by the hub motor; Based on the underlying signals and the Bekker soil mechanics model, state-space equations were established between the driving current deviation, wheel speed deviation, and yaw rate deviation and the adhesion coefficient and bearing modulus. An extended Kalman filter algorithm is used, with the adhesion coefficient and bearing modulus as state vectors and the drive current deviation, wheel speed deviation and yaw rate deviation as observation vectors, to solve the state space equation and obtain the real-time estimate of the adhesion coefficient. Based on the correlation between real-time estimates and the vehicle's current location, a physical accessibility label map is constructed using a preset resolution, and each grid cell of the physical accessibility label map stores the real-time estimate. The objective function is constructed based on the tracking error cost, control quantity smoothing cost, and stability margin cost. A model predictive controller is established based on the vehicle kinematics and dynamics model, real-time estimates, and the objective function. When the model predictive controller predicts that the real-time estimated value ahead is lower than the safety threshold, it searches for alternative paths based on the physical passability map and adjusts the front wheel steering angle and wheel hub motor torque distribution according to the alternative paths to achieve autonomous navigation and obstacle avoidance of the unmanned vehicle in the wild. The alternative paths are those with real-time estimated values ​​higher than the safety threshold.

[0006] A second aspect of the present invention provides an autonomous navigation and obstacle avoidance system for unmanned vehicles in the wild, comprising: The real-time acquisition module is used to acquire the underlying signals of the hub motor drive in real time. The first module is used to establish the state-space equations between the driving current deviation, wheel speed deviation, and yaw rate deviation and the adhesion coefficient and bearing modulus based on the underlying signals and the Bekker soil mechanics model. The solution module is used to solve the state space equation using the extended Kalman filter algorithm, with the adhesion coefficient and bearing modulus as the state vectors and the drive current deviation, wheel speed deviation and yaw rate deviation as the observation vectors, to obtain the real-time estimate of the adhesion coefficient. The construction module is used to build a physical accessibility label map with a preset resolution based on the correlation between real-time estimated values ​​and the vehicle's current location. Each grid cell of the physical accessibility label map stores the real-time estimated value. The second module is used to construct the objective function based on the tracking error cost, the control quantity smoothing cost, and the stability margin cost, and to establish the model predictive controller based on the vehicle kinematics and dynamics model, real-time estimates, and the objective function. The adjustment module is used to search for alternative paths based on the physical passability map when the real-time estimated value of the vehicle ahead is lower than the safety threshold, based on the model prediction controller. The module then adjusts the front wheel steering angle and wheel hub motor torque distribution based on the alternative paths to achieve autonomous navigation and obstacle avoidance for the unmanned vehicle in the wild. The alternative paths are those with real-time estimated values ​​higher than the safety threshold.

[0007] Compared to existing technologies, this invention provides an autonomous navigation and obstacle avoidance method and system for unmanned vehicles in the wild. It collects underlying signals driven by hub motors in real time; based on the underlying signals and the Bekker soil mechanics model, it establishes state-space equations relating drive current deviation, wheel speed deviation, yaw rate deviation, adhesion coefficient, and bearing modulus; using an extended Kalman filter algorithm, with adhesion coefficient and bearing modulus as state vectors and drive current deviation, wheel speed deviation, and yaw rate deviation as observation vectors, it solves the state-space equations to obtain a real-time estimate of the adhesion coefficient; based on the correlation between the real-time estimate and the vehicle's current position, it utilizes a preset... A physical accessibility map is constructed using a resolution system, with each grid cell storing real-time estimates. An objective function is constructed based on tracking error cost, control smoothing cost, and stability margin cost. A model predictive controller is then established based on the vehicle's kinematics and dynamics model, real-time estimates, and the objective function. When the model predictive controller predicts that the real-time estimate ahead is lower than a safety threshold, alternative paths are searched based on the physical accessibility map. The front wheel steering angle and hub motor torque distribution are adjusted according to the alternative paths to achieve autonomous navigation and obstacle avoidance for the unmanned vehicle in the wild. Alternative paths are those with real-time estimates higher than the safety threshold. This invention acquires the underlying signals driven by the hub motor in real time and establishes a state-space equation. The extended Kalman filter algorithm is used to obtain a real-time estimate of the adhesion coefficient, thus achieving online perception of the ground's physical and mechanical state. Based on this, the real-time estimate is used to construct a physical passability map, and the adhesion coefficient is used as a hard safety constraint in the model predictive controller. This ensures that navigation and obstacle avoidance decisions no longer rely solely on geometric information but are also constrained by the actual bearing capacity of the ground. When the predicted real-time estimate ahead is lower than a safety threshold, the system can proactively switch to an alternative path with a higher adhesion coefficient, effectively preventing the autonomous vehicle from entering soft, slippery, or unstructured terrain. This significantly reduces the risk of skidding, getting stuck, or overturning accidents, improving autonomous passage capability and safety reliability. Attached Figure Description

[0008] The above and other objects, features, and advantages of exemplary embodiments of the present invention will become readily apparent upon reading the following detailed description with reference to the accompanying drawings. In the drawings, several embodiments of the invention are illustrated by way of example and not limitation, with the same or corresponding reference numerals denoteing the same or corresponding parts, wherein: Figure 1The diagram schematically illustrates the hardware system architecture used in the autonomous navigation and obstacle avoidance methods of unmanned vehicles in the wild; Figure 2 A flowchart illustrating the autonomous navigation and obstacle avoidance method for unmanned vehicles in the wild is shown in the diagram. Figure 3 A flowchart illustrating the attachment coefficient estimation based on the extended Kalman filter algorithm is shown schematically. Figure 4 A path tracking comparison diagram of an embodiment of the present invention and a comparative example is shown schematically; Figure 5 A schematic diagram illustrating the real-time estimation curve of the adhesion coefficient according to an embodiment of the present invention is shown. Figure 6 The diagram schematically illustrates the structure of an autonomous navigation and obstacle avoidance system for unmanned vehicles in the wild. Detailed Implementation

[0009] Exemplary embodiments of the invention will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the invention are shown in the drawings, it should be understood that the invention can be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided to enable a more thorough understanding of the invention and to fully convey the scope of the invention to those skilled in the art.

[0010] It should be noted that, unless otherwise stated, the technical or scientific terms used in this invention should have the ordinary meaning as understood by one of ordinary skill in the art.

[0011] The methods described in the embodiments of the present invention will be explained in detail below.

[0012] Figure 1 The diagram schematically illustrates the hardware system architecture used in the autonomous navigation and obstacle avoidance methods for unmanned vehicles in the wild. (See attached image.) Figure 1As shown, the autonomous navigation and obstacle avoidance method for unmanned vehicles in the wild according to the present invention is implemented using a hardware system architecture. This hardware system architecture includes a wheel-ground mechanics perception sensor kit, a vehicle controller, a controller area network (CLAN) bus, and a power system. The wheel-ground mechanics perception sensor kit consists of a drive current sensor, a wheel speed sensor, and an inertial measurement unit (IMU), used to collect low-level signals from the hub motor drive in real time. These low-level signals include drive current, wheel speed, and yaw rate. The drive current sensor is located in the drive circuit of the hub motor, the wheel speed sensor is located at the wheel hub, and the IMU is located at the center of the vehicle body. The vehicle controller is communicatively connected to the drive current sensor, wheel speed sensor, and IMU of the wheel-ground mechanics perception sensor kit, used to receive, process, and make decisions based on the low-level signals. The power system consists of four hub motors: a first hub motor, a second hub motor, a third hub motor, and a fourth hub motor, each driving its corresponding wheel. The CLAN bus connects the vehicle controller and the power system, used to transmit control commands generated by the vehicle controller to the power system. The vehicle controller communicates with the first, second, third, and fourth hub motors of the powertrain via a controller area network (CLAN) bus. Control commands include front wheel steering angle adjustment and hub motor torque distribution.

[0013] In this embodiment, the drive current sensor is a Hall current sensor, which is installed in the drive circuit of each wheel hub motor to collect the drive current signal flowing through the wheel hub motor in real time; the wheel speed sensor is a magnetoelectric speed sensor, which is installed at the wheel hub of each wheel to collect the wheel speed signal of each wheel in real time; the inertial measurement unit is integrated at the center of the vehicle body to collect the yaw rate signal of the vehicle body in real time.

[0014] Figure 2 The flowchart illustrating the autonomous navigation and obstacle avoidance method for unmanned vehicles in the wild according to an embodiment of the present invention is shown in the figure. See also: Figure 2 As shown, the autonomous navigation and obstacle avoidance method for unmanned vehicles in the wild may include: S201, Real-time acquisition of underlying signals from the hub motor drive.

[0015] The underlying signals are collected by the wheel-ground mechanics sensing sensor kit, which includes a drive current sensor, a wheel speed sensor, and an inertial measurement unit. The underlying signals include drive current, wheel speed, and yaw rate.

[0016] During data acquisition, a wheel-ground mechanics sensing sensor suite is used to collect the underlying signals driven by the wheel hub motor in real time. Specifically, a drive current sensor collects the signal corresponding to the drive current flowing through the wheel hub motor; a wheel speed sensor collects the signal corresponding to the wheel speed; and an inertial measurement unit collects the signal corresponding to the vehicle's yaw rate. The vehicle controller performs preprocessing on the signals corresponding to the drive current, wheel speed, and yaw rate. Preprocessing includes filtering for noise reduction and outlier removal. For example, a low-pass filter is used to filter and denoise the signals corresponding to the drive current, wheel speed, and yaw rate. A three-standard-deviation criterion is used to remove outliers from these signals.

[0017] S202. Based on the underlying signals and the Bekker soil mechanics model, establish the state-space equations between the driving current deviation, wheel speed deviation, and yaw rate deviation, and the adhesion coefficient and bearing modulus.

[0018] The Bekker soil mechanics model includes a first equation and a second equation. The first equation is used to characterize the nonlinear relationship between the normal bearing pressure and the settlement, and includes the bearing modulus parameter and the settlement index. The second equation is used to characterize the bilinear relationship between the tangential shear force and the slip ratio, and includes the soil cohesion and the soil internal friction angle.

[0019] Specifically, step S202 includes: Step A1: Calculate the drive current deviation, wheel speed deviation, and yaw rate deviation based on the drive current, wheel speed, and yaw rate.

[0020] The drive current deviation is the difference between the actual drive current and the steady-state reference current; the wheel speed deviation is the difference between the actual wheel speed and the reference wheel speed; and the yaw rate deviation is the difference between the actual yaw rate and the reference yaw rate.

[0021] Step A2: Linearize the first equation and the second equation, and based on the linearized first equation, the linearized second equation, the drive current deviation, wheel speed deviation, yaw rate deviation, and vehicle dynamics constraints, establish the state-space equations between the drive current deviation, wheel speed deviation, yaw rate deviation, and the adhesion coefficient and bearing modulus.

[0022] In this embodiment, the first equation in the Bekker soil mechanics model is the Bekker compaction equation, which describes the nonlinear relationship between the normal bearing pressure and the settlement. This nonlinear relationship reflects the soil's bearing characteristics and is related to factors such as soil density and moisture content. By measuring the tire settlement of vehicles and the corresponding vertical load, the soil's bearing modulus parameter can be inferred. The second equation is the Bekker shear equation, which describes the bilinear relationship between the tangential shear force and the slip ratio. The slip ratio is defined as the ratio of the difference between the actual and theoretical rotational speeds of the wheel to the theoretical rotational speed. The soil cohesion parameter and the soil internal friction angle parameter in the shear equation together determine the maximum adhesion force that the ground can provide.

[0023] Specifically, the first and second equations are linearized near the current operating point. The current operating point consists of the estimated values ​​of the adhesion coefficient and bearing modulus at the current moment. During the linearization process, a first-order Taylor expansion is performed on the first and second equations respectively, and higher-order terms are ignored to obtain approximately linear equations. Then, using the adhesion coefficient and bearing modulus as state vectors, and the drive current deviation, wheel speed deviation, and yaw rate deviation as observation vectors, combined with vehicle dynamics constraints, a state-space equation is established.

[0024] S203. Using the extended Kalman filter algorithm, with the adhesion coefficient and bearing modulus as the state vectors and the drive current deviation, wheel speed deviation and yaw rate deviation as the observation vectors, the state space equation is solved to obtain the real-time estimate of the adhesion coefficient.

[0025] Specifically, Figure 3 A flowchart illustrating the attachment coefficient estimation based on the extended Kalman filter algorithm is shown below. Figure 3 As shown, step S203 includes: Step S2031: The extended Kalman filter algorithm is used to solve the state space equation. The adhesion coefficient and bearing modulus are used as the state vectors, and the driving current deviation, wheel speed deviation and yaw rate deviation are used as the observation vectors. The Jacobian matrix of the Bekker soil mechanics model is used to construct the state transition matrix and the observation matrix. The process noise covariance matrix and the observation noise covariance matrix are determined by offline tuning according to the sensor characteristics.

[0026] The state transition matrix describes the evolution of the state vector between adjacent time points, while the observation matrix describes the linear relationship between the observation vector and the state vector. Both the state transition matrix and the observation matrix are constructed from the values ​​of the Jacobian matrix of the Bekker soil mechanics model at the current operating point. The process noise covariance matrix and the observation noise covariance matrix are determined offline based on the sensor characteristics. Process noise originates from model uncertainties and external disturbances, while observation noise originates from sensor measurement errors.

[0027] Step S2032: In the state prediction stage, state prediction is performed based on the estimated value of the state vector at the previous moment and the vehicle kinematics and dynamics model to obtain the state prior value at the current moment.

[0028] Vehicle kinematics and dynamics models include vehicle kinematics models and vehicle dynamics models. Vehicle kinematics models describe the geometric relationships between vehicle position, velocity, and acceleration, while vehicle dynamics models describe the physical relationships between forces acting on the vehicle and changes in its motion state. The combination of the two models predicts the vehicle's state at future moments.

[0029] Step S2033: In the observation update phase, using the actual measured values ​​of the drive current, wheel speed, and yaw rate collected at the current moment, calculate the drive current deviation, wheel speed deviation, and yaw rate deviation at the current moment, respectively. Use the drive current deviation, wheel speed deviation, and yaw rate deviation at the current moment as the observation vector at the current moment, and correct the prior state value at the current moment based on the observation vector at the current moment to obtain the posterior state estimate at the current moment.

[0030] Among them, the posterior state estimates include the estimated values ​​of the adhesion coefficient and the bearing modulus.

[0031] Step S2034: Using the independent estimator module under each wheel, perform the state prediction stage and the observation update stage, and estimate the local adhesion coefficient under each wheel based on the drive current and wheel speed corresponding to each wheel.

[0032] Among them, the local adhesion coefficient estimate is the adhesion coefficient component of the posterior state estimate at the current time.

[0033] In the vehicle controller, an independent estimator module is configured for each wheel. Each estimator module is a processing logic that runs independently in the vehicle controller. A total of four independent estimator modules run in parallel. Each estimator module takes the driving current and wheel speed of the corresponding wheel as input, independently executes the state prediction stage and the observation update stage, and outputs the estimated value of the local adhesion coefficient under the wheel.

[0034] Since the ground adhesion conditions at the locations of each wheel may differ, the spatial distribution characteristics of the ground adhesion conditions can be captured by estimating the local adhesion coefficient under each wheel separately.

[0035] Step S2035: Perform a weighted average fusion of the local adhesion coefficient estimates of all estimator modules to obtain the average adhesion coefficient estimate under the vehicle body, and use the average adhesion coefficient estimate as the real-time estimate of the adhesion coefficient.

[0036] S204. Based on the correlation between real-time estimated values ​​and the vehicle's current location, construct a physical passability label map using a preset resolution.

[0037] Among them, each grid cell of the physical accessibility label map stores real-time estimates.

[0038] Specifically, step S204 includes: Step B1: Associate the real-time estimated value of the adhesion coefficient with the vehicle's current position to determine the map grid position corresponding to the real-time estimated value.

[0039] The real-time estimated value of the adhesion coefficient is collected at the current location of the vehicle. The real-time estimated value of the adhesion coefficient is bound to the current location of the vehicle at the time of collection. Based on the preset resolution of the physical accessibility label map and the location of the map origin, the geographical location is converted into row and column indices in the raster map, thereby determining the map raster location corresponding to the real-time estimated value of the adhesion coefficient.

[0040] Step B2: Centered on the vehicle's current location, divide the vehicle's surrounding environment into multiple discrete cells according to a preset resolution. Based on the map grid position, use the real-time estimated value as the real-time estimated value of the attachment coefficient of the discrete cells, and calculate the confidence level of multiple discrete cells.

[0041] For example, the preset resolution is set to 0.5m × 0.5m, that is, the environment around the vehicle is divided into multiple discrete square cells with a side length of 0.5m. Based on the map grid position, the real-time estimated value is used as the real-time estimated value of the attachment coefficient of the discrete square cell, stored in the discrete square cell, and the confidence level of the discrete square cell is calculated.

[0042] The confidence level is determined based on two factors: temporal continuity and spatial consistency. Temporal continuity refers to the number of times the estimated value is written to the discrete cell; the more times it is written, the higher the confidence level. Spatial consistency refers to the degree of difference between the real-time estimated value of the adhesion coefficient of the discrete cell and the real-time estimated values ​​of the adhesion coefficients of its surrounding discrete cells; the smaller the difference, the higher the confidence level.

[0043] For example, the confidence level can be calculated using a weighted summation method, i.e. ,in, The first preset weighting coefficient and This is the second preset weighting coefficient. The time continuity factor is obtained by normalizing the historical write count of the discrete cell, and the spatial consistency factor is obtained by normalizing the inverse of the absolute value of the difference between the real-time estimated value of the attachment coefficient of the discrete cell and the real-time estimated value of the attachment coefficient of its surrounding discrete cells.

[0044] Step B3: Collect new real-time estimates during vehicle movement, determine the new map grid position corresponding to the new real-time estimates, update the real-time estimates of the adhesion coefficients in the discrete cells at the new map grid position, and update the confidence level of the discrete cells at the new map grid position.

[0045] A weighted average method can be used to update the real-time estimate of the adhesion coefficient in discrete cells at new map grid locations. Specifically, new real-time estimates are collected during vehicle movement, the corresponding new map grid location is determined, and a weighted average is used to merge the new real-time estimate with the existing real-time estimates of the adhesion coefficient in the discrete cells at the new map grid location to obtain an updated real-time estimate of the adhesion coefficient, which is then stored in the discrete cell. Simultaneously, based on the new real-time estimate and the historical write count and spatial consistency information of the discrete cell, the confidence level of the discrete cell is recalculated and used as the updated confidence level.

[0046] Step B3: Construct a physical passability label map using the updated real-time estimates of the adhesion coefficients of each discrete cell and the updated confidence levels.

[0047] Once the physical accessibility label map is constructed, it stores ground adhesion coefficient information in the form of a raster map. Each raster stores the estimated adhesion coefficient and the corresponding confidence level at that location.

[0048] S205. Construct an objective function based on the tracking error cost, control quantity smoothing cost, and stability margin cost, and establish a model predictive controller based on the vehicle kinematics and dynamics model, real-time estimates, and objective function.

[0049] Specifically, step S205 includes: Step C1: Use the vehicle kinematics and dynamics model as the prediction model for the predictive controller to be established.

[0050] The vehicle kinematics model describes the geometric relationship between the vehicle's position, velocity, and acceleration in a planar coordinate system, while the vehicle dynamics model describes the relationship between the forces acting on the vehicle and changes in its motion state.

[0051] Step C2: Construct the objective function of the predictive controller for the model to be built based on the tracking error cost, control quantity smoothing cost, and stability margin cost.

[0052] The objective function uses the front wheel steering angle and hub motor torque as optimization variables, with the goal of minimizing the tracking error cost, control quantity smoothing cost, and stability margin cost.

[0053] The tracking error cost measures the deviation between the vehicle's actual driving path and the desired reference path; the smaller the deviation, the lower the cost. The control input smoothing cost measures the drasticness of changes in the control input; the smoother the change, the lower the cost. The stability margin cost measures the vehicle's stability reserve under the current adhesion coefficient; the higher the stability reserve, the lower the cost. Within each control cycle, while satisfying hard safety constraints, the front wheel steering angle and hub motor torque that minimize the objective function are calculated.

[0054] The expression for the objective function is: ; ; ; in, For the first The objective function value at each sampling time. To predict the step index, To predict the time domain, In the first The predicted number at the sampling time is... The system output at each sampling time. In the first The prediction at the nth sampling time The expected reference trajectory output at each sampling time point, For the tracking error weight matrix, To control the step index, To control the time domain, For the first Changes in control quantity at each sampling time To smooth the weight matrix of the control quantity, For the first Stability state parameters at each sampling time. This serves as a reference value for the stability safety boundary. This is the stability margin weight matrix. To track the squared weighted Euclidean norm of the error weight matrix, To smooth the square of the weighted Euclidean norm of the control quantity weight matrix, The square of the weighted Euclidean norm of the stability margin weight matrix. To account for the cost of tracking errors, To control the cost of smoothing out the quantity, For the sake of stability margin, For control vectors, This refers to the front wheel steering angle. This refers to the torque of the left front wheel hub motor. This refers to the torque of the right front wheel hub motor. This refers to the torque of the left rear wheel hub motor. This refers to the torque of the right rear wheel hub motor. This is the matrix transpose symbol. The adhesion coefficient, For the first The front / rear axle, the first The torque of the left / right wheel hub motors The adhesion coefficient is The first time The front / rear axle, the first The maximum permissible driving torque for each left / right wheel. , For the front axle, For the rear axle, Left side On the right side, The adhesion coefficient is The maximum permissible front wheel steering angle at that time The yaw rate is angular velocity. The adhesion coefficient is The maximum permissible yaw rate at that time For the maximum longitudinal driving force constraint, For maximum lateral force constraint, This is a constraint for the maximum yaw moment.

[0055] Step C3: Determine hard safety constraints based on real-time estimates.

[0056] Among them, the rigid safety constraints include the maximum longitudinal driving force constraint, the maximum lateral force constraint, and the maximum yaw moment constraint.

[0057] The maximum longitudinal driving force constraint is determined by real-time estimates and the vehicle's vertical load; the maximum lateral force constraint is determined by real-time estimates and the vehicle's vertical load; and the maximum yaw moment constraint is determined by real-time estimates and the lateral forces on the front and rear axles of the vehicle.

[0058] Step C4: Add hard safety constraints to the constraint system of the model predictive controller to be built, and build the model predictive controller based on the predictive model, objective function and constraint system.

[0059] Once the model predictive controller is established, it is used to calculate the optimal front wheel steering angle and hub motor torque distribution in real time based on the current vehicle state and the real-time estimated value ahead.

[0060] S206. When the model predictive controller predicts that the real-time estimated value ahead is lower than the safety threshold, alternative paths are searched based on the physical passability label map. The front wheel steering angle and wheel hub motor torque distribution are adjusted according to the alternative paths to achieve autonomous navigation and obstacle avoidance of the unmanned vehicle in the wild.

[0061] The alternative paths are those whose real-time estimated values ​​are higher than the safety threshold.

[0062] Specifically, step S206 includes: Step D1: When the model predictive controller predicts that the real-time estimated value ahead is lower than the safety threshold, then based on the physical passability label map, search for all feasible paths in the area ahead and extract the real-time estimated value of the adhesion coefficient of the grid through which each feasible path passes.

[0063] For example, the safety threshold is set to 0.3. When the model prediction controller predicts that the real-time estimate of the road ahead is lower than 0.3, it indicates that the road surface is soft or slippery, and the risk of the vehicle skidding or getting stuck increases significantly. At this time, a path search operation is triggered.

[0064] The model predictive controller predicts the vehicle state at multiple sampling times in each control cycle and queries the physical passability label map during the prediction process to obtain real-time estimates of the adhesion coefficient of each grid on the path ahead.

[0065] Step D2: Compare the real-time estimated value of the adhesion coefficient with the safety threshold, filter out the preset paths where the real-time estimated value of the adhesion coefficient of all grids is higher than the safety threshold, and use the preset paths as alternative paths.

[0066] The preset path is the path where the real-time estimated value of the adhesion coefficient of all grids is higher than the safety threshold.

[0067] Step D3: Calculate the path switching cost based on the difference between the alternative path and the current path, and select the optimal alternative path from the alternative paths based on the path switching cost.

[0068] This difference includes the lateral offset between the candidate path and the current path, the heading angle deviation between the candidate path and the current path, the change in the front wheel steering angle between the candidate path and the current path, and the change in the in-wheel motor torque between the candidate path and the current path. The path switching cost is a weighted sum of the lateral offset, heading angle deviation, change in front wheel steering angle, and change in in-wheel motor torque.

[0069] The smaller the path switching cost of the alternative path, the less adjustment is required to switch from the current path to the alternative path. The alternative path with the smallest path switching cost is selected as the optimal alternative path.

[0070] The expression for the path switching cost is: ; in, For the cost of path switching, As the first weighting coefficient, This represents the lateral offset between the candidate path and the current path. This is the second weighting coefficient. The heading angle deviation between the alternative path and the current path. This is the third weighting coefficient. This represents the change in the front wheel steering angle between the alternative path and the current path. It is the fourth weighting coefficient. This represents the change in hub motor torque between the alternative path and the current path.

[0071] Step D4: Adjust the front wheel steering angle and hub motor torque distribution according to the optimal alternative path to control the vehicle to travel along the optimal alternative path.

[0072] The optimal alternative path is used as the desired reference trajectory for the model predictive controller (MMC). The MMC then uses the deviation between the current vehicle state and the desired reference trajectory, combined with the vehicle's kinematics and dynamics model, to solve for the front wheel steering angle and hub motor torque that minimize the objective function, while satisfying strict safety constraints. These values ​​serve as the control commands for the current control cycle. The front wheel steering angle control command is sent to the steering actuator to control the vehicle's front wheel steering angle to track the desired trajectory of the optimal alternative path. The hub motor torque distribution command is sent to each hub motor driver to control the distribution of driving torque to each wheel, ensuring the vehicle travels along the optimal alternative path.

[0073] This invention acquires real-time underlying signals such as the drive current, wheel speed, and yaw rate of the hub motor. Using the Bekker soil mechanics model, a state-space equation is established. An extended Kalman filter algorithm is employed, with the adhesion coefficient and bearing modulus as state vectors and drive current deviation, wheel speed deviation, and yaw rate deviation as observation vectors, for real-time estimation. This achieves the technical effect of extracting ground physical and mechanical parameters from the vehicle's underlying signals, effectively overcoming the technical deficiency of existing technologies that rely solely on geometric perception to obtain the ground adhesion coefficient. By associating the real-time estimated value of the adhesion coefficient with the vehicle's current position and constructing a physical passability map at a preset resolution, the invention achieves discrete representation and continuous updating of ground physical passability information in the spatial dimension, providing rich ground mechanical state reference information for navigation decisions.

[0074] This invention constructs an objective function that integrates tracking error cost, control smoothing cost, and stability margin cost. It then establishes a model predictive controller (MMC) based on vehicle kinematics and dynamics models. The real-time estimate of the adhesion coefficient is incorporated into the MMC's constraint system as a hard safety constraint, enabling the controller to dynamically adjust its control strategy based on the real-time estimate ahead. When the MMC predicts that the real-time estimate ahead is below a safety threshold, the system automatically searches and switches to an alternative path with a real-time adhesion coefficient above the safety threshold using a physical passability map. Simultaneously, it adjusts the front wheel steering angle and hub motor torque distribution, effectively preventing the autonomous vehicle from entering soft sand, mud, or other areas with extremely poor physical passability, significantly reducing the risk of skidding, getting stuck, or overturning.

[0075] Furthermore, this invention estimates the local adhesion coefficient under each wheel separately and obtains an average adhesion coefficient estimate under the vehicle body through weighted averaging fusion. This average estimate serves as the real-time value of the adhesion coefficient. This architecture, combining distributed estimation with centralized fusion, effectively improves the accuracy and reliability of the adhesion coefficient estimate. Simultaneously, this invention assigns a confidence level to each discrete cell in the physical accessibility label map and dynamically updates the confidence level based on temporal continuity and spatial consistency. This allows the map to accurately reflect the reliability of ground adhesion conditions in each area, providing a more reliable decision-making basis for path planning.

[0076] The autonomous navigation and obstacle avoidance method and system for unmanned vehicles in the wild provided by this invention effectively solves the technical problem that geometric navigation cannot perceive ground adhesion conditions in the prior art by deeply integrating the perception of ground physical and mechanical state with upper-level path planning and motion control. It significantly improves the autonomous passage capability and safety reliability of unmanned vehicles in complex unstructured terrain, and has broad application prospects and important engineering practical value.

[0077] To verify the technical effects of the present invention, a specific embodiment and comparative analysis are provided below.

[0078] In a specific embodiment, the autonomous navigation and obstacle avoidance method and system for unmanned vehicles in the wild, as described in this invention, were tested and verified. The test site was selected in a wild area with diverse terrain features, including hard grass, soft sand, muddy sections, and ordinary hardened roads. The test vehicle was a four-wheel hub motor-driven unmanned vehicle with a total weight of 500 kg, a wheelbase of 1.8 meters, and a track width of 1.4 meters.

[0079] During the test, the vehicle autonomously traveled at a speed of 20 kilometers per hour, and the path planning system set a reference path from the starting point to the end point. The reference path traversed different road surface areas, with approximately 20% of the route located on soft sand or muddy sections. When the vehicle reached the soft sand area, the drive current, wheel speed, and yaw rate were collected in real time using a wheel-ground mechanics sensing sensor suite. The extended Kalman filter algorithm was used to estimate the real-time adhesion coefficient of this area to be approximately 0.25, significantly lower than the safety threshold (set to 0.3). The model predictive controller predicted that the real-time estimate ahead would remain below the safety threshold, triggering a path switching operation.

[0080] A detour route was found based on the physical passability map. This route bypasses the soft sand area, and the real-time estimated adhesion coefficients of each grid cell along the route are all above 0.4, meeting safety requirements. Calculations of the route switching cost show that this detour route has the lowest switching cost. Adjusting the front wheel steering angle and hub motor torque distribution based on this optimal alternative route, the vehicle successfully completed the route switch and smoothly entered the safe path, avoiding the risk of getting stuck in the soft sand.

[0081] A comparative experiment was conducted using existing geometric navigation methods. The same test site and driving task were used, employing an unmanned vehicle system that relies solely on LiDAR and visual sensors for geometric perception and path planning. This system only considers the road's geometric information when planning its path and cannot obtain the ground adhesion coefficient, thus continuing to follow the original planned path. When the vehicle entered a soft sandy area, due to poor ground adhesion, the drive wheels severely slipped, preventing the vehicle from passing normally, ultimately requiring manual intervention to extricate it from the predicament.

[0082] Table 1. Comparison of experimental results between the embodiments of the present invention and the comparative examples.

[0083] Table 1 compares the test results of the embodiments of the present invention with those of the comparative examples. As can be seen from the comparative data in Table 1, under the same test road length and the same proportion of soft road sections, the embodiments of the present invention can estimate the ground adhesion coefficient in real time with an estimation accuracy of 85%. During the journey, it successfully completed three path switches based on changes in the adhesion conditions ahead, ultimately safely traversing soft sand and muddy sections without getting stuck. In contrast, the comparative examples, relying solely on geometric information, could not obtain the ground adhesion coefficient. After entering soft road sections, the drive wheels slipped, preventing normal passage and ultimately requiring manual intervention, resulting in three instances of getting stuck. This demonstrates that the present invention, by estimating the ground adhesion coefficient in real time and constructing a physical passability map, enables the system to perceive the physical and mechanical state of the road surface. Therefore, when a low-adhesion area is predicted ahead, it automatically switches to a safe path, effectively preventing the vehicle from entering dangerous areas. Compared with traditional methods relying solely on geometric information, this significantly improves the autonomous vehicle's ability to navigate complex unstructured terrain and enhances its safety and reliability.

[0084] Figure 4 The diagram schematically illustrates a path tracking comparison between embodiments of the present invention and comparative examples. See also... Figure 4 As shown, the horizontal axis represents the direction of travel in meters, and the vertical axis represents the lateral position in meters. Figure 4 The areas marked by the dashed rectangles represent soft sand (coefficient of adhesion 0.25) and muddy sections (coefficient of adhesion 0.20). These two areas together account for approximately 20% of the total travel distance, consistent with the approximately 20% proportion of soft road sections in Table 1. The squares on the vertical axis represent the starting point, and the squares within the axes represent the ending point. The safety threshold is 0.3; areas below this threshold (marked by the dashed rectangles) are considered danger zones. The dashed lines represent a comparative path using the geometric navigation method. This path maintains a lateral position of approximately 50 meters throughout the journey, directly traversing soft sand and muddy sections, causing vehicles to slip and get stuck, making it impossible to safely reach the destination. The solid line represents the path after switching using the method of this invention. Starting from the starting point (approximately 40 meters laterally), the path actively veers to the right after approximately 20 meters, gradually adjusting to a lateral position of approximately 56 meters, completely bypassing the soft sand and muddy sections. It then gradually returns to a lateral position of approximately 40 meters and reaches the endpoint. The adhesion coefficient on the path is approximately 0.45–0.55, higher than the safety threshold of 0.3. The pentagram marks the path switching points.

[0085] from Figure 4As can be seen, the method of this invention successfully bypassed the low-adhesion area by actively switching paths in real time to sense the ground adhesion coefficient ahead; while the comparative example, unable to sense the ground adhesion coefficient, directly crossed the low-adhesion area, resulting in skidding and getting stuck. Based on the data in Table 1, the embodiment of this invention completed 3 path switches throughout the entire test, achieving an estimation accuracy of 85%, with 2 instances of skidding and 0 instances of getting stuck; while the comparative example, unable to estimate the adhesion coefficient, experienced more than 15 instances of skidding and 3 instances of getting stuck, failing to pass the test section. Figure 4 The handover shown is a typical path handover process among the three handovers in Table 1.

[0086] Figure 5 A schematic diagram illustrating the real-time estimation curve of the adhesion coefficient according to an embodiment of the present invention is shown below. Figure 5 As shown, the horizontal axis represents the travel time in seconds, and the vertical axis represents the adhesion coefficient. Figure 5 The rectangular area adjacent to the pentagram marker represents a low-adhesion zone, namely soft sand and muddy sections. The adhesion coefficient for soft sand is 0.25, and for muddy sections it is 0.20. The dashed line with a long dash at an adhesion coefficient of 0.3 represents the safety threshold line, with a value of 0.3. The dashed line with a short dash represents the actual adhesion coefficient value of the ground, which drops to a minimum of approximately 0.30 after about 30 seconds. The solid line represents the adhesion coefficient curve estimated in real time by the method of this invention, which can track the changing trend of the actual value. In the low-adhesion zone (approximately 25-35 seconds), the estimated value drops to approximately 0.28, approaching the safety threshold of 0.3, triggering a path switch. The dashed line with a long dash at an adhesion coefficient of 0.7 is a comparative example. Because it relies solely on geometric perception and cannot obtain the adhesion coefficient, it remains at a constant value of 0.7 throughout the experiment, completely failing to perceive the actual changes in the adhesion of the road surface ahead. The pentagram marks the switching moment, corresponding to an adhesion coefficient of approximately 0.28. When the estimated value of the method of this invention drops to approximately 0.28 (below the safety threshold of 0.3), the path switching procedure is triggered. This switching moment is related to... Figure 4 The path switching points in the sequence correspond perfectly in terms of timing. Figure 5 and Figure 4 Together, they verified the effectiveness and superiority of the present invention in complex terrain in the field by sensing the adhesion coefficient in real time and actively switching paths.

[0087] In practical engineering applications, this invention can be widely used in unmanned vehicles for fields such as field exploration, agricultural operations, and disaster relief. These application scenarios share the common characteristic of complex and variable road conditions, making traditional methods relying solely on geometric information for navigation and obstacle avoidance insufficient to meet the requirements of autonomous passage capability and safety reliability. This invention effectively solves these problems by sensing the ground adhesion coefficient in real time and making path planning and obstacle avoidance decisions based on the real-time estimate of the adhesion coefficient. It has broad application prospects and significant engineering practical value.

[0088] Based on the above Figure 2 As can be seen from the implementation method, the embodiments of the present invention acquire the underlying signals of the hub motor drive in real time; based on the underlying signals and the Bekker soil mechanics model, a state-space equation is established between the drive current deviation, wheel speed deviation, and yaw rate deviation, and the adhesion coefficient and bearing modulus; using the extended Kalman filter algorithm, with the adhesion coefficient and bearing modulus as the state vector and the drive current deviation, wheel speed deviation, and yaw rate deviation as the observation vector, the state-space equation is solved to obtain a real-time estimate of the adhesion coefficient; based on the correlation between the real-time estimate and the vehicle's current position, a physical passthrough is constructed using a preset resolution. The physical accessibility map stores real-time estimates in each grid cell. An objective function is constructed based on tracking error cost, control smoothing cost, and stability margin cost. A model predictive controller is then established based on the vehicle's kinematics and dynamics model, real-time estimates, and the objective function. When the model predictive controller predicts that the real-time estimate ahead is lower than a safety threshold, alternative paths are searched based on the physical accessibility map. The front wheel steering angle and hub motor torque distribution are adjusted according to the alternative paths to achieve autonomous navigation and obstacle avoidance for the unmanned vehicle in the wild. Alternative paths are those with real-time estimates higher than the safety threshold. This invention acquires the underlying signals driven by the hub motor in real time and establishes a state-space equation. The extended Kalman filter algorithm is used to obtain a real-time estimate of the adhesion coefficient, thus achieving online perception of the ground's physical and mechanical state. Based on this, the real-time estimate is used to construct a physical passability map, and the adhesion coefficient is used as a hard safety constraint in the model predictive controller. This ensures that navigation and obstacle avoidance decisions no longer rely solely on geometric information but are also constrained by the actual bearing capacity of the ground. When the predicted real-time estimate ahead is lower than a safety threshold, the system can proactively switch to an alternative path with a higher adhesion coefficient, effectively preventing the autonomous vehicle from entering soft, slippery, or unstructured terrain. This significantly reduces the risk of skidding, getting stuck, or overturning accidents, improving autonomous passage capability and safety reliability.

[0089] Based on the same inventive concept, as an implementation of the above-mentioned method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild, this embodiment of the invention also provides an autonomous navigation and obstacle avoidance system for unmanned vehicles in the wild. Figure 6 This is a structural diagram of the autonomous navigation and obstacle avoidance system for unmanned vehicles in the wild, as described in this embodiment of the invention. (See also...) Figure 6 As shown, the autonomous navigation and obstacle avoidance system for unmanned vehicles in the wild may include: The real-time acquisition module 601 is used to acquire the underlying signals of the hub motor drive in real time. The first module 602 is used to establish the state-space equations between the driving current deviation, wheel speed deviation, and yaw rate deviation and the adhesion coefficient and bearing modulus based on the underlying signal and the Bekker soil mechanics model. The solver module 603 is used to solve the state space equation using the extended Kalman filter algorithm, with the adhesion coefficient and bearing modulus as state vectors and the drive current deviation, wheel speed deviation and yaw rate deviation as observation vectors, to obtain the real-time estimate of the adhesion coefficient. The construction module 604 is used to construct a physical accessibility label map using a preset resolution based on the correlation between the real-time estimated value and the current position of the vehicle. Each grid of the physical accessibility label map stores the real-time estimated value. The second module 605 is used to construct an objective function based on the tracking error cost, the control quantity smoothing cost, and the stability margin cost, and to establish a model predictive controller based on the vehicle kinematics and dynamics model, real-time estimates, and the objective function. The adjustment module 606 is used to search for alternative paths based on the physical passability label map when the real-time estimated value of the vehicle ahead is predicted to be lower than the safety threshold by the model prediction controller. The adjustment module then adjusts the front wheel steering angle and the wheel hub motor torque distribution based on the alternative paths to achieve autonomous navigation and obstacle avoidance of the unmanned vehicle in the wild. The alternative paths are those with real-time estimated values ​​higher than the safety threshold.

[0090] The first module 602 is specifically used to calculate the driving current deviation, wheel speed deviation, and yaw rate deviation based on the driving current, wheel speed, and yaw rate, respectively. It linearizes the first and second equations and, based on the linearized first and second equations, the driving current deviation, wheel speed deviation, yaw rate deviation, and vehicle dynamics constraints, establishes state-space equations relating the driving current deviation, wheel speed deviation, and yaw rate deviation to the adhesion coefficient and bearing modulus. The Bekker soil mechanics model includes the first and second equations. The first equation characterizes the nonlinear relationship between the normal bearing pressure and subsidence, and includes the bearing modulus parameter and subsidence index. The second equation characterizes the bilinear relationship between the tangential shear force and slip ratio, and includes soil cohesion and soil internal friction angle.

[0091] The solver module 603 specifically employs the extended Kalman filter algorithm to solve the state-space equations. It uses the adhesion coefficient and bearing modulus as state vectors, and the drive current deviation, wheel speed deviation, and yaw rate deviation as observation vectors. The state transition matrix and observation matrix are constructed using the Jacobian matrix of the Bekker soil mechanics model. Offline tuning based on sensor characteristics is performed to determine the process noise covariance matrix and observation noise covariance matrix. In the state prediction stage, state prediction is performed based on the estimated state vector from the previous moment and the vehicle kinematics and dynamics model to obtain the prior state value for the current moment. In the observation update stage, the actual measured values ​​of the drive current, wheel speed, and yaw rate collected at the current moment are used to calculate the drive current deviation, wheel speed deviation, and yaw rate deviation for the current moment, respectively. The deviation is calculated by using the current drive current deviation, wheel speed deviation, and yaw rate deviation as the current observation vector. Based on the current observation vector, the prior state value at the current moment is corrected to obtain the posterior state estimate. The posterior state estimate includes the estimated values ​​of the adhesion coefficient and bearing modulus. Using an independent estimator module under each wheel, the state prediction stage and observation update stage are performed. Based on the drive current and wheel speed corresponding to each wheel, the local adhesion coefficient estimate under each wheel is estimated. The local adhesion coefficient estimate is the adhesion coefficient component of the posterior state estimate at the current moment. The local adhesion coefficient estimates of all estimator modules are weighted and averaged to obtain the average adhesion coefficient estimate under the vehicle body, and the average adhesion coefficient estimate is used as the real-time estimate of the adhesion coefficient.

[0092] The construction module 604 is specifically used to associate the real-time estimated value of the adhesion coefficient with the current position of the vehicle to determine the map grid position corresponding to the real-time estimated value; with the current position of the vehicle as the center, the surrounding environment of the vehicle is divided into multiple discrete cells according to a preset resolution; based on the map grid position, the real-time estimated value is used as the real-time estimated value of the adhesion coefficient of the discrete cell, and the confidence of multiple discrete cells is calculated; new real-time estimated values ​​are collected during the vehicle's movement, the new map grid position corresponding to the new real-time estimated value is determined, the real-time estimated value of the adhesion coefficient in the discrete cell at the new map grid position is updated, and the confidence of the discrete cell at the new map grid position is updated; using the updated real-time estimated value of the adhesion coefficient of each discrete cell and the updated confidence, a physical passability label map is constructed.

[0093] The second module 605 is specifically used to establish a predictive model for the predictive controller of the model to be established, based on the vehicle kinematics and dynamics model; to construct the objective function of the predictive controller of the model to be established based on the tracking error cost, control quantity smoothing cost, and stability margin cost, with the front wheel steering angle and hub motor torque as optimization variables, and minimizing the tracking error cost, control quantity smoothing cost, and stability margin cost as the optimization objective; to determine hard safety constraints based on real-time estimates, including maximum longitudinal driving force constraint, maximum lateral force constraint, and maximum yaw moment constraint; to add the hard safety constraints to the constraint system of the predictive controller of the model to be established, and to establish the model predictive controller based on the predictive model, objective function, and constraint system.

[0094] In the second module 605, the expression for the objective function is: ; ; ; in, For the first The objective function value at each sampling time. To predict the step index, To predict the time domain, In the first The prediction at the nth sampling time The system output at each sampling time. In the first The prediction at the nth sampling time The expected reference trajectory output at each sampling time point, For the tracking error weight matrix, To control the step index, To control the time domain, For the first Changes in control quantity at each sampling time To smooth the weight matrix of the control quantity, For the first Stability state parameters at each sampling time. This serves as a reference value for the stability safety boundary. This is the stability margin weight matrix. To track the squared weighted Euclidean norm of the error weight matrix, To smooth the square of the weighted Euclidean norm of the control quantity weight matrix, The square of the weighted Euclidean norm of the stability margin weight matrix. To account for the cost of tracking errors, To control the cost of smoothing out the quantity, For the sake of stability margin, For control vectors, This refers to the front wheel steering angle. This refers to the torque of the left front wheel hub motor. This refers to the torque of the right front wheel hub motor. This refers to the torque of the left rear wheel hub motor. This refers to the torque of the right rear wheel hub motor. This is the matrix transpose symbol. The adhesion coefficient, For the first The front / rear axle, the first The torque of the left / right wheel hub motors The adhesion coefficient is The first time The front / rear axle, the first The maximum permissible driving torque for each left / right wheel. , For the front axle, For the rear axle, Left side On the right side, The adhesion coefficient is The maximum permissible front wheel steering angle at that time The yaw rate is angular velocity. The adhesion coefficient is The maximum permissible yaw rate at that time For the maximum longitudinal driving force constraint, For maximum lateral force constraint, This is a constraint for the maximum yaw moment.

[0095] The adjustment module 606 is specifically used to: when the model predictive controller predicts that the real-time estimated value ahead is lower than the safety threshold, search all feasible paths in the area ahead based on the physical passability label map, and extract the real-time estimated value of the adhesion coefficient of each grid cell traversed by the feasible path; compare the real-time estimated value of the adhesion coefficient with the safety threshold, filter out the preset path where the real-time estimated value of the adhesion coefficient of all grid cells is higher than the safety threshold, and use the preset path as the candidate path; calculate the path switching cost based on the difference between the candidate path and the current path, and select the optimal candidate path from the candidate paths based on the path switching cost; adjust the front wheel steering angle and wheel hub motor torque distribution based on the optimal candidate path to control the vehicle to travel along the optimal candidate path.

[0096] In adjustment module 606, the expression for the path switching cost is: ; in, For the cost of path switching, As the first weighting coefficient, This represents the lateral offset between the candidate path and the current path. This is the second weighting coefficient. The heading angle deviation between the alternative path and the current path. This is the third weighting coefficient. This represents the change in the front wheel steering angle between the alternative path and the current path. It is the fourth weighting coefficient. This represents the change in hub motor torque between the alternative path and the current path.

[0097] It should be noted that the above description of the autonomous navigation and obstacle avoidance system for unmanned vehicles in the wild is similar to the description of the above-described autonomous navigation and obstacle avoidance method for unmanned vehicles in the wild, and has similar beneficial effects. For technical details not disclosed in the embodiments of the autonomous navigation and obstacle avoidance system for unmanned vehicles in the wild of this invention, please refer to the description of the embodiments of the autonomous navigation and obstacle avoidance method for unmanned vehicles in the wild of this invention for understanding.

[0098] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild, characterized in that, include: Real-time acquisition of underlying signals driven by the hub motor; Based on the underlying signals and the Bekker soil mechanics model, state-space equations are established between the driving current deviation, wheel speed deviation, and yaw rate deviation and the adhesion coefficient and bearing modulus. An extended Kalman filter algorithm is used, with the adhesion coefficient and the bearing modulus as the state vectors, and the drive current deviation, the wheel speed deviation and the yaw rate deviation as the observation vectors, to solve the state space equation and obtain a real-time estimate of the adhesion coefficient. Based on the correlation between the real-time estimated value and the vehicle's current location, a physical passability label map is constructed using a preset resolution, and each grid cell of the physical passability label map stores the real-time estimated value; An objective function is constructed based on the tracking error cost, control quantity smoothing cost, and stability margin cost. A model predictive controller is established based on the vehicle kinematics and dynamics model, the real-time estimated value, and the objective function. When the model prediction controller predicts that the real-time estimated value ahead is lower than the safety threshold, it searches for alternative paths based on the physical passability label map, and adjusts the front wheel steering angle and wheel hub motor torque distribution based on the alternative paths to achieve autonomous navigation and obstacle avoidance of the unmanned vehicle in the wild. The alternative paths are those where the real-time estimated value is higher than the safety threshold.

2. The method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild according to claim 1, characterized in that, The underlying signals are collected by a wheel-ground mechanics sensing sensor kit, which includes a drive current sensor, a wheel speed sensor, and an inertial measurement unit. The underlying signals include drive current, wheel speed, and yaw rate.

3. The method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild according to claim 2, characterized in that, The Bekker soil mechanics model includes a first equation and a second equation. The first equation is used to characterize the nonlinear relationship between the normal bearing pressure and the settlement. The first equation includes the bearing modulus parameter and the settlement index. The second equation is used to characterize the bilinear relationship between the tangential shear force and the slip ratio. The second equation includes the soil cohesion and the soil internal friction angle. The process of establishing state-space equations relating driving current deviation, wheel speed deviation, and yaw rate deviation to adhesion coefficient and bearing modulus based on the underlying signal and the Bekker soil mechanics model includes: Based on the drive current, the wheel speed, and the yaw rate, calculate the drive current deviation, the wheel speed deviation, and the yaw rate deviation, respectively. The first equation and the second equation are linearized, and based on the linearized first equation, the linearized second equation, the drive current deviation, the wheel speed deviation, the yaw rate deviation, and the vehicle dynamics constraints, a state-space equation is established between the drive current deviation, the wheel speed deviation, the yaw rate deviation, the adhesion coefficient, and the bearing modulus.

4. The method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild according to claim 1, characterized in that, The extended Kalman filter algorithm is used, with the adhesion coefficient and the bearing modulus as the state vectors, and the drive current deviation, wheel speed deviation, and yaw rate deviation as the observation vectors, to solve the state-space equations and obtain a real-time estimate of the adhesion coefficient, including: The extended Kalman filter algorithm is used to solve the state space equation. The adhesion coefficient and the bearing modulus are used as the state vector, and the driving current deviation, the wheel speed deviation and the yaw rate deviation are used as the observation vector. The Jacobian matrix of the Bekker soil mechanics model is used to construct the state transition matrix and the observation matrix. The process noise covariance matrix and the observation noise covariance matrix are determined by offline tuning according to the sensor characteristics. In the state prediction phase, state prediction is performed based on the estimated value of the state vector at the previous moment and the vehicle kinematics and dynamics model to obtain the state prior value at the current moment. During the observation update phase, using the actual measured values ​​of the drive current, wheel speed, and yaw rate collected at the current moment, the drive current deviation, wheel speed deviation, and yaw rate deviation at the current moment are calculated respectively. These deviations are then used as the observation vector for the current moment. Based on this observation vector, the prior state value at the current moment is corrected to obtain the posterior state estimate. The posterior state estimate includes estimates of the adhesion coefficient and the bearing modulus. Using an independent estimator module under each wheel, the state prediction stage and the observation update stage are executed. Based on the driving current and wheel speed corresponding to each wheel, the local adhesion coefficient estimate under each wheel is estimated. The local adhesion coefficient estimate is the adhesion coefficient component of the posterior state estimate at the current moment. The local adhesion coefficient estimates of all estimator modules are weighted and averaged to obtain the average adhesion coefficient estimate under the vehicle body, and the average adhesion coefficient estimate is used as the real-time estimate of the adhesion coefficient.

5. The method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild according to claim 1, characterized in that, The step of constructing a physical passability label map using a preset resolution based on the correlation between the real-time estimated value and the vehicle's current location includes: The real-time estimated value of the adhesion coefficient is correlated with the vehicle's current position to determine the map grid position corresponding to the real-time estimated value; Centered on the current location of the vehicle, the surrounding environment of the vehicle is divided into multiple discrete cells according to the preset resolution. Based on the map grid position, the real-time estimated value is used as the real-time estimated value of the attachment coefficient of the discrete cell, and the confidence level of the multiple discrete cells is calculated. Collect new real-time estimates during vehicle movement, determine the new map grid position corresponding to the new real-time estimates, update the real-time estimates of the adhesion coefficients in the discrete cells at the new map grid position, and update the confidence level of the discrete cells at the new map grid position. The physical accessibility label map is constructed using the updated real-time estimates of the adhesion coefficients of each discrete cell and the updated confidence level.

6. The method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild according to claim 1, characterized in that, The step of constructing an objective function based on tracking error cost, control quantity smoothing cost, and stability margin cost, and establishing a model predictive controller based on the vehicle kinematics and dynamics model, the real-time estimated value, and the objective function, includes: The vehicle kinematics and dynamics model is used as the prediction model for the model prediction controller to be established; Based on the tracking error cost, the control quantity smoothing cost, and the stability margin cost, an objective function for the predictive controller of the model to be established is constructed. The objective function uses the front wheel steering angle and the hub motor torque as optimization variables, and minimizes the tracking error cost, the control quantity smoothing cost, and the stability margin cost as the optimization objective. Hard safety constraints are determined based on the real-time estimated values. These hard safety constraints include the maximum longitudinal driving force constraint, the maximum lateral force constraint, and the maximum yaw moment constraint. The hard safety constraints are added to the constraint system of the model predictive controller to be established, and the model predictive controller is established based on the predictive model, the objective function, and the constraint system.

7. The method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild according to claim 6, characterized in that, The expression for the objective function is: ； ； ； in, For the first The objective function value at each sampling time. To predict the step index, To predict the time domain, In the first The prediction at the nth sampling time The system output at each sampling time. In the first The prediction at the nth sampling time The expected reference trajectory output at each sampling time point, For the tracking error weight matrix, To control the step index, To control the time domain, For the first Changes in control quantity at each sampling time To smooth the weight matrix of the control quantity, For the first Stability state parameters at each sampling time. This serves as a reference value for the stability safety boundary. This is the stability margin weight matrix. The weighted Euclidean norm of the tracking error weight matrix is ​​the square of the weighted Euclidean norm. The square of the weighted Euclidean norm of the smoothing weight matrix of the control quantity is given. The square of the weighted Euclidean norm of the stability margin weight matrix is ​​given by... The tracking error cost, The cost of smoothing the control quantity. For the stability margin cost, For control vectors, This refers to the front wheel steering angle. This refers to the torque of the left front wheel hub motor. This refers to the torque of the right front wheel hub motor. This refers to the torque of the left rear wheel hub motor. This refers to the torque of the right rear wheel hub motor. This is the matrix transpose symbol. The adhesion coefficient is... For the first The front / rear axle, the first The torque of the left / right wheel hub motors The adhesion coefficient is The first time The front / rear axle, the first The maximum permissible driving torque for each left / right wheel. , For the front axle, For the rear axle, Left side On the right side, The adhesion coefficient is The maximum permissible front wheel steering angle at that time The yaw rate is angular velocity. The adhesion coefficient is The maximum permissible yaw rate at that time The maximum longitudinal driving force constraint, For the maximum lateral force constraint, The maximum yaw moment constraint is defined as follows.

8. The method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild according to claim 1, characterized in that, When the model prediction controller predicts that the real-time estimated value ahead is lower than the safety threshold, the process of searching for alternative paths based on the physical passability map and adjusting the front wheel steering angle and hub motor torque distribution based on the alternative paths includes: When the model prediction controller predicts that the real-time estimated value ahead is lower than the safety threshold, then according to the physical passability label map, all feasible paths in the area ahead are searched, and the real-time estimated value of the adhesion coefficient of the grid passed by each feasible path is extracted. The real-time estimated value of the adhesion coefficient is compared with the safety threshold, and a preset path in which the real-time estimated value of the adhesion coefficient of all grids is higher than the safety threshold is selected, and the preset path is used as the candidate path. Calculate the path switching cost based on the difference between the alternative path and the current path, and select the optimal alternative path from the alternative paths based on the path switching cost; The front wheel steering angle and the wheel hub motor torque distribution are adjusted according to the optimal alternative path to control the vehicle to travel along the optimal alternative path.

9. The method for autonomous navigation and obstacle avoidance of unmanned vehicles in the wild according to claim 8, characterized in that, The expression for the path switching cost is: ； in, The cost of the path switching, As the first weighting coefficient, The horizontal offset between the candidate path and the current path. This is the second weighting coefficient. The heading angle deviation between the candidate path and the current path. This is the third weighting coefficient. The change in the front wheel steering angle between the candidate path and the current path. It is the fourth weighting coefficient. The change in hub motor torque between the alternative path and the current path.

10. An autonomous navigation and obstacle avoidance system for unmanned vehicles in the wild, characterized in that, include: The real-time acquisition module is used to acquire the underlying signals of the hub motor drive in real time. The first module is used to establish the state-space equations between the driving current deviation, wheel speed deviation, and yaw rate deviation and the adhesion coefficient and bearing modulus based on the underlying signal and the Bekker soil mechanics model. The solution module is used to solve the state space equation using the extended Kalman filter algorithm, with the adhesion coefficient and the bearing modulus as the state vectors and the drive current deviation, the wheel speed deviation and the yaw rate deviation as the observation vectors, to obtain a real-time estimate of the adhesion coefficient. A construction module is used to construct a physical accessibility label map using a preset resolution based on the correlation between the real-time estimated value and the vehicle's current location. Each grid cell of the physical accessibility label map stores the real-time estimated value. The second module is used to construct an objective function based on the tracking error cost, the control quantity smoothing cost, and the stability margin cost, and to establish a model predictive controller based on the vehicle kinematics and dynamics model, the real-time estimated value, and the objective function. The adjustment module is used to search for alternative paths based on the physical passability label map when the real-time estimated value of the vehicle ahead is predicted to be lower than the safety threshold by the model prediction controller, and to adjust the front wheel steering angle and wheel hub motor torque distribution based on the alternative paths to achieve autonomous navigation and obstacle avoidance of the unmanned vehicle in the wild. The alternative paths are those where the real-time estimated value is higher than the safety threshold.

Images