Mobile robot path attribute judgment method based on TBM evidence theory fusion

Abstract

The invention discloses a mobile robot path attribute judgment method based on TBM evidence theory fusion. The method comprises the following steps: collecting multi-source data and executing space-time registration; constructing an identification framework, and mapping the geometric features of the point cloud and the semantic probability of the image into a basic probability assignment; performing orthogonal fusion on the heterogeneous evidences by using a TBM conjunctive rule, and extracting empty set quality to construct a local conflict energy spectrum; according to the method, a quadtree is driven to dynamically split based on an energy spectrum, a variable-resolution map is generated, an attribute semantic map is generated through Pignistic transformation, in addition, curved surface three-dimensional equivalent expansion path planning is executed by the method, anisotropy cost correction is carried out by combining a gravity acting principle, and anti-interference trajectory tracking is realized by utilizing a self-adaptive dynamic model and an extended state observer. According to the method, the problems of perception conflict processing deficiency and two-dimensional planning distortion in an unstructured environment are solved, and the environmental adaptability and traffic safety of the robot in a complex terrain are improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and control technology for mobile robots, specifically a method for determining the path attributes of mobile robots based on the fusion of TBM evidence theory. Background Technology

[0002] When mobile robots perform tasks in unstructured environments such as field exploration, disaster relief, and complex industrial parks, they face severe challenges such as rugged terrain, soft soil, and irregularly distributed obstacles. To ensure safe operation and complete their missions, mobile robots need to possess the ability to accurately perceive the physical properties of their surroundings. This means not only recognizing the geometric contours of obstacles but also accurately determining the material properties of the road surface, such as rigid surfaces, soft mud, or impassable obstacle areas. This ability to determine environmental properties is fundamental to achieving autonomous path planning and stable motion control for robots.

[0003] In existing mobile robot environmental perception technologies, a multi-sensor fusion architecture is typically used to acquire environmental information. The mainstream approach utilizes LiDAR to acquire 3D geometric point cloud data and combines it with a visual camera to acquire 2D texture image data. To fuse these two types of heterogeneous data, existing solutions often employ Bayesian estimation or the traditional Dempster-Shafer evidence theory to construct probabilistic grid maps. The system transforms observation data from different sensors into occupancy probabilities or confidence levels, and maps multi-source information to the same grid cell using weighted averaging or simple product rules. This infers the accessibility or obstacle attributes of the area, ultimately generating a global map for navigation.

[0004] However, existing probabilistic fusion methods have limitations in handling sensor conflicts in unstructured environments. In practical operations, factors such as lighting variations, water surface reflection, or vegetation camouflage can cause LiDAR and visual cameras to produce diametrically opposed observations of the same area. For example, LiDAR might identify a flat area as a dangerous puddle, while a visual camera might identify it as such. Traditional Bayesian or Dempster normalization-based evidence fusion methods, when faced with highly conflicting data, typically perform forced normalization, proportionally redistributing the probability of conflict to other hypotheses. While this mathematical processing smooths the data, it masks logical contradictions between sensors, making the system unable to effectively distinguish between confirmed safety and potential perceptual conflicts. This makes robots prone to misjudgments when faced with complex or deceptive environmental features, reducing the robustness of the environmental perception system and the safety of navigation decisions. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a method for determining the path attributes of mobile robots based on the fusion of TBM evidence theory, which solves the problems of insufficient environmental attribute identification capability, lack of multi-source perception conflict handling mechanism, and inaccurate three-dimensional terrain path planning in existing technologies.

[0006] To achieve the above objectives, the present invention is implemented through the following technical solution: The method first performs multi-source data acquisition and spatiotemporal rigid registration, synchronously reads the three-dimensional point cloud data of the lidar, the image frame data of the vision camera and the pose data of the positioning system, performs rigid coordinate transformation according to the pre-calibrated extrinsic parameters, and aligns the heterogeneous data based on the hardware timestamp, and outputs the spatiotemporally synchronized original observation dataset.

[0007] After acquiring the raw observation data, an unstructured environment identification framework and basic confidence functions are constructed. An unstructured environment identification framework incorporating mutually exclusive elements such as rigid pavement, flexible pavement, and obstacles is established. Based on the raw observation dataset, the geometric statistical features of the 3D point cloud data are mapped to basic probability assignments for geometric evidence, and the semantic segmentation probabilities of the image frame data are mapped to basic probability assignments for visual evidence. Specifically, for geometric features, the 3D coordinate covariance matrix of the point cloud set is calculated, and the eigenvector corresponding to the minimum eigenvalue is extracted as the normal vector estimate. Combined with slope and roughness features, a comprehensive geometric feature index is generated and transformed into confidence levels for each hypothesis through nonlinear mapping. For visual features, a semantic segmentation network and inverse perspective projection transformation are used to statistically summarize the probability of each semantic category within the target raster and assign it to the corresponding attribute hypothesis. To improve the reliability of the evidence, a discount factor is calculated based on point cloud density and semantic information entropy, and the original basic probability assignments are corrected based on uncertainty-based cognitive discounting, transferring unreliable evidence shares to the entire set representing a state of complete ignorance.

[0008] Evidence fusion and local conflict energy spectrum extraction are performed. A fusion logic based on the Transmitted Belief Model (TBM) is constructed, and geometric and visual evidence are orthogonally fused using the TBM conjunctive combination rule. During the fusion process, normalization is not performed; instead, the sum of the joint confidence components of all focal element pairs leading to logical contradictions is calculated, and this value is defined as the local conflict energy. This conflict energy is mapped onto a map, constructing a local conflict energy spectrum reflecting the degree of consistency in environmental perception.

[0009] Based on the local conflict energy spectrum, conflict-driven map resolution updates are performed. It is determined whether the local conflict energy of each grid exceeds a preset threshold. If the conflict energy is too high and has not reached the maximum level, it indicates a perceptual conflict at the current resolution, triggering a quadtree topology split. The grid is subdivided into child nodes, and the original observation data is redistributed to the child nodes for further discrimination. During the splitting process, a maximum entropy discount strategy is used to inherit evidence information from the parent node to the child nodes, ensuring that the initial state contains reasonable uncertainty.

[0010] Path attribute determination and semantic generation are performed. Using the Pignistic probability transformation logic, the confidence level of the fused data, which contains composite hypotheses and unknown components, is transformed into the deterministic attribute probabilities of each grid, generating a semantic map. To cope with dynamic environmental changes, a time-domain decay mechanism based on timestamps is introduced, which dynamically adjusts the confidence level according to the freshness of the data, so that old information gradually returns to an unknown state.

[0011] As a further improvement, this invention also performs unstructured environment surface unfolding path planning. A path search graph structure is constructed, and the surface connectivity between nodes is verified using normal vector data. When calculating the passage cost, a three-dimensional equivalent surface unfolding algorithm is executed, calculating the three-dimensional Euclidean distance between nodes as the basic unfolding distance. Simultaneously, based on the principle of gravity work and combined with the pitch slope angle, a slope correction coefficient is calculated to correct the anisotropic cost of the basic unfolding distance. The corrected equivalent geometric distance and road surface attribute passage cost are then integrated to search for the globally optimal path on a two-dimensional equivalent plane, and a three-dimensional reference trajectory is generated through inverse mapping.

[0012] This invention also integrates an anti-interference robust trajectory tracking and perception feedback mechanism, constructs a vehicle dynamics model, and adaptively adjusts the equivalent tire lateral stiffness and road adhesion coefficient parameters in the model using the generated rigid and flexible road surface probabilities to match the current road surface characteristics. The controller uses an extended state observer to estimate the total external disturbance in real time, solves the optimal steering command through model predictive control, and when wheel slippage or abnormal vibration is detected, it generates a body perception evidence package containing road surface physical properties, which is then injected back into the perception fusion step to correct the previous environmental judgment, forming a closed-loop optimization of perception and control.

[0013] This invention provides a method for determining the path attributes of mobile robots based on the fusion of TBM evidence theory. It has the following beneficial effects: 1. This invention constructs a fusion logic based on a transmission belief model and uses the conjunctive combination rule to orthogonally fuse point cloud geometric evidence and image visual evidence. While preserving the quality of the empty set, it constructs a local conflict energy spectrum that reflects the degree of perceptual consistency. This avoids the defect of traditional probabilistic fusion that masks logical contradictions between sensors due to forced normalization. It can effectively identify observation conflicts between lidar and visual cameras under conditions of changing illumination or texture camouflage, thereby significantly improving the perception robustness and decision reliability of mobile robots in unstructured environments.

[0014] 2. This invention employs a map resolution update mechanism driven by local conflict energy spectrum. When the conflict energy of a grid node exceeds a threshold, a recursive quadtree split is triggered to improve spatial resolution, while maintaining low-resolution representation in regions with consistent features. This changes the indiscriminate consumption of storage and computing resources by traditional uniform grid maps. While ensuring the accuracy of detecting the edges of complex terrain and the contours of obstacles, it significantly reduces the amount of data and processing latency of the global map, thereby achieving an adaptive balance between computational efficiency and perception accuracy in the environmental modeling process.

[0015] 3. This invention calculates the three-dimensional Euclidean distance between nodes by executing the three-dimensional equivalent unfolding algorithm of unstructured environmental surface, and performs anisotropic cost correction on the basic distance based on the principle of gravity work and the pitch slope angle, including uphill impedance and downhill braking penalty. This breaks through the limitation of traditional two-dimensional planning ignoring the influence of terrain undulation, and can generate a three-dimensional reference trajectory that conforms to the dynamic characteristics of the robot and accurately quantify the actual driving energy consumption and control risks caused by undulating terrain, thereby effectively reducing the probability of rollover and getting stuck due to excessive slope or insufficient power in field operations. Attached Figure Description

[0016] Figure 1 This is a block diagram of the overall logical architecture of the present invention. Detailed Implementation

[0017] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] Please see the appendix Figure 1This invention provides a method for determining the path attributes of a mobile robot based on the fusion of TBM evidence theory. The mobile robot hardware platform constructed in this embodiment adopts a modular design and can adapt to the driving needs of unstructured roads such as the wilderness, disaster sites, and complex industrial parks. The hardware system mainly consists of four physical subsystems: motion drive subsystem, environmental perception subsystem, integrated navigation and positioning subsystem, and central computing and control subsystem. Each subsystem is physically connected through CAN bus, gigabit Ethernet, and serial communication interface to build a hardware foundation for high-speed data transmission and real-time command execution.

[0019] The motion drive subsystem is responsible for the robot's power output and posture execution. The chassis structure is equipped with a power drive unit, which consists of a high-torque brushless DC motor and a matching servo driver. To achieve high real-time performance of the underlying motion control, the drive subsystem integrates an embedded motion controller (such as an MCU based on the ARM Cortex-M core). The embedded motion controller receives speed commands from the upper layer via the CAN bus and performs PID closed-loop regulation on the motor. Each drive wheel is coaxially connected to a high-resolution incremental wheel encoder. The wheel encoder collects the wheel's rotational speed pulses and rotation angle increments in real time and sends the data back to the embedded motion controller for dead reckoning, thereby providing high-frequency odometer data. To address the bumpy characteristics of unstructured road surfaces, the chassis adopts an independent suspension structure to mechanically isolate high-frequency ground vibrations and protect the upper-level precision sensors.

[0020] The environmental perception subsystem acquires three-dimensional geometric and physical property information of the external environment. For complex terrain, the main sensor is a multi-line mechanical rotating LiDAR (e.g., 16-line or 32-line LiDAR), which is mounted horizontally on the top of the robot via a shock-absorbing bracket to acquire omnidirectional three-dimensional point cloud data. The three-dimensional point cloud data includes information on the distance, azimuth, height, and surface reflection intensity of obstacles. To overcome the limitations of a single sensor, the environmental perception subsystem deploys a near-field detection sensor array around the robot body, including ultrasonic sensors and infrared distance sensors. The ultrasonic sensors are used to cover the near-field blind spots of the LiDAR and detect transparent media such as glass. The infrared distance sensors are installed at a downward angle to detect the risk of falling from nearby curbs. The data from these heterogeneous sensors serve as auxiliary observation signals and are input into the subsequent attribute determination module to address sensor failure issues.

[0021] The integrated navigation and positioning subsystem provides the robot with all-weather pose reference. The core components of this subsystem include a microelectromechanical system micro inertial measurement unit (MEMS IMU) and a global navigation satellite system (GNSS) receiver. The MEMS IMU is rigidly fixed at the geometric center of the robot and contains a three-axis gyroscope and a three-axis accelerometer to output the three-axis angular velocity and three-axis specific force information of the vehicle. In order to meet the algorithm's modeling requirements for zero-bias instability characteristics, the selected IMU has a high-frequency raw data output interface. The GNSS receiver is equipped with dual antennas and supports RTK differential positioning mode, providing absolute position coordinates in open outdoor environments.

[0022] The central computing and control subsystem is the core of data processing and decision-making. This subsystem adopts a two-layer computing architecture of host computer + slave computer. The host computer is a high-performance industrial control computer (IPC) or an edge computing module with GPU acceleration, running a real-time operating system (RTOS) or a robot operating system (ROS). The host computer is connected to the LiDAR via a gigabit Ethernet interface and to the IMU and GNSS module via USB or serial port. It is responsible for executing TBM-based attribute determination, path planning, and SLAM algorithms. To ensure time alignment of multi-source data, the central computing and control subsystem is equipped with a hardware time synchronization module. It uses the PPS (pulse per second) signal output by the GNSS receiver or the hardware trigger signal generated by the FPGA to simultaneously trigger the data acquisition of the LiDAR and IMU, ensuring strict synchronization of all sensor data on the time axis. In addition, the system is equipped with an intelligent power management module (BMS) to provide stable voltage output for each subsystem and prevent sensor data noise caused by voltage fluctuations.

[0023] In the aforementioned hardware architecture, lidar and vision sensors provide environmental observation data, while IMU and wheel encoders provide body motion state data. These two types of data converge in a central computing and control subsystem with hardware time synchronization capabilities, providing accurate physical data input for subsequent unstructured environmental perception and control methods.

[0024] The system functional architecture described in this embodiment runs on the computing platform of the central computing and control subsystem. It constructs a closed-loop processing channel for perception, positioning, decision-making, and control through software logic. The system is logically divided into five core processing units: multi-source data acquisition and spatiotemporal calibration unit (S100), integrated navigation and positioning unit (S200), TBM-based variable resolution perception and mapping unit (S300), surface unfolding path planning unit (S400), and anti-interference robust trajectory tracking control unit (S500). The units interact with each other through a shared memory mechanism or a message middleware based on a publish-subscribe pattern for high-bandwidth data exchange.

[0025] S100: Multi-source data acquisition and spatiotemporal rigid registration. The multi-source data acquisition and spatiotemporal calibration unit synchronously reads the 3D point cloud data of the lidar, the image frame data of the vision camera, the acceleration and angular velocity data of the micro inertial measurement unit (IMU), the latitude and longitude data of the GNSS, and the pulse count data of the wheel encoder through the underlying hardware driver interface. Considering the differences in the physical installation positions of each sensor, the multi-source data acquisition and spatiotemporal calibration unit performs rigid coordinate transformation based on the pre-calibrated extrinsic parameter rotation matrix R and translation vector T. The observation data in the lidar coordinate system, camera coordinate system, and IMU coordinate system are uniformly transformed into the robot body reference coordinate system. At the same time, the multi-source data acquisition and spatiotemporal calibration unit performs time axis alignment on the heterogeneous data based on the hardware-triggered timestamp and outputs the spatiotemporally synchronized original observation dataset.

[0026] S200: Integrated Navigation and Positioning Based on Error Parameter Identification. The integrated navigation and positioning unit is responsible for calculating the robot's six-degree-of-freedom global pose in unstructured terrain. The integrated navigation and positioning unit first executes the online identification algorithm for random error parameters of the strapdown inertial navigation system. For the zero-bias instability characteristics of the MEMS IMU, it constructs a state equation that includes gyroscope zero bias, accelerometer zero bias, and scale factor error. By observing the sensor outputs during stationary or uniform speed segments, the integrated navigation and positioning unit identifies the above error parameters in real time and dynamically corrects the original measurement values ​​of the IMU. The corrected inertial data and wheel encoder data are used for dead reckoning to generate a high-frequency prior pose. At the same time, the integrated navigation and positioning unit uses the Normal Distribution Transform (NDT) or Iterative Closest Point (ICP) algorithm to scan and match the real-time laser point cloud with the preset global point cloud map to obtain the low-frequency absolute observation pose. Using the Extended Kalman Filter (EKF) or Factor Graph Optimizer, the integrated navigation and positioning unit fuses the high-frequency prior pose with the low-frequency absolute observation pose and outputs the optimal pose estimate after eliminating cumulative errors and random drift.

[0027] S300: Variable resolution environmental perception and mapping based on TBM conflict energy spectrum. The TBM-based variable resolution perception and mapping unit is the logical core of environmental understanding and is used to generate multi-level raster maps containing physical attribute determinations.

[0028] S310, Feature Extraction and Projection: Based on the global pose, the perception mapping unit projects the LiDAR point cloud and visual image onto the local map region within the current field of view. For each basic grid unit, the perception mapping unit extracts the geometric statistical features (height variance, normal vector covariance matrix) of the point cloud and the texture semantic features of the image.

[0029] S320. Evidence Construction and Fusion: The perceptual mapping unit is based on the unstructured environment identification framework (rigid road surface, flexible road surface, obstacles). It maps the extracted geometric features and texture features to basic probability assignments (BPAs) respectively. It uses the TBM conjunctive combination rule to orthogonally fuse the multi-source BPAs and explicitly calculates the mass of the empty set generated by the fusion, i.e., the local conflict energy.

[0030] S340, Conflict-Driven Quadtree Splitting: The perception mapping unit maintains a quadtree map structure based on a hash table index. When the local conflict energy of a certain grid node exceeds the preset splitting threshold, the perception mapping unit triggers recursive splitting logic to decompose the parent node grid into four child node grids, thereby improving the spatial resolution of high-conflict areas. The original point cloud data is redistributed to the child node grids for secondary fusion until the conflict energy converges or reaches the maximum preset depth.

[0031] S350, Spatiotemporal Correction and Semantic Generation: The perception mapping unit introduces a time decay factor coupled with the robot's velocity vector to discount the confidence of historical evidence. Finally, the Pengistic probability transformation is used to convert the TBM confidence into the attribute probability of each grid, generating a road surface attribute semantic map with dynamic resolution characteristics.

[0032] S400: Unstructured Environment Surface Unfolding Path Planning. The surface unfolding path planning unit plans traversable paths based on attribute semantic maps. Addressing the three-dimensional undulation characteristics of outdoor terrain, the unit abandons traditional two-dimensional projection planning and executes a three-dimensional equivalent surface unfolding algorithm. This algorithm models the three-dimensional terrain surface as a Riemannian manifold and unfolds the curved physical surface into a two-dimensional equivalent plane using isometric or conformal mapping techniques, minimizing the metric distortion of path length or angle. On this two-dimensional equivalent plane, the surface unfolding path planning unit combines road surface attribute costs (avoiding flexible or dangerous areas) and slope costs to search for the globally optimal path point sequence. Subsequently, through inverse mapping transformation, it generates a three-dimensional reference trajectory containing spatial location coordinates (x, y, z) and tangential guide angles.

[0033] S500: Anti-interference robust trajectory tracking and perception feedback. The anti-interference robust trajectory tracking control unit is responsible for driving the actuator to accurately track the three-dimensional reference trajectory. The anti-interference robust trajectory tracking control unit establishes a vehicle dynamics model including lateral slip stiffness and longitudinal slip ratio. It adopts a model predictive control (MPC) strategy, treating the contact force changes caused by unstructured terrain as external disturbances. The controller solves the optimal control law online and outputs the desired linear velocity and angular velocity commands to the underlying driver.

[0034] Furthermore, the anti-interference robust trajectory tracking control unit constructs a perception-control closed-loop feedback mechanism. When the deviation between the actual wheel speed and the theoretical wheel speed (slip ratio) is detected, or when the IMU vibration spectrum exceeds the normal threshold, the anti-interference robust trajectory tracking control unit generates a propagation perception evidence package. The propagation perception evidence package contains BPAs characterizing the physical properties of the road surface (such as slippery or soft), which are back-injected into the perception mapping unit in S300 to participate in evidence fusion at subsequent moments, thereby correcting the prior judgment errors generated by relying solely on vision and radar.

[0035] In this embodiment, the micro inertial system error modeling and parameter identification logic is integrated inside the integrated navigation and positioning unit. Given that low-cost microelectromechanical systems (MEMS) inertial measurement units (IMUs) have significant temperature drift characteristics, zero bias instability, and inter-axis coupling effects, the integrated navigation and positioning unit adopts a hierarchical compensation strategy: using factory calibration parameters to compensate for deterministic scale factors and installation errors, and using online filtering algorithms to estimate and compensate for random zero bias errors in real time.

[0036] S210: Constructing a hybrid error compensation model for a micro inertial system. The integrated navigation and positioning unit first reads pre-calibrated sensor parameters from a non-volatile memory. These parameters include a scale factor error parameter for correcting input-output ratio deviation and an installation error parameter for correcting inter-axis coupling components caused by triaxial non-orthogonality.

[0037] To address random errors that vary with time and temperature, the integrated navigation and positioning unit establishes a high-order hybrid error model. In this model, the sensor's measurement output is represented as the superposition of the real physical quantity and various errors. The integrated navigation and positioning unit decouples the sensor's zero bias error into two parts: one part is the static constant zero bias determined by power-on initialization; the other part is the dynamic random zero bias that drifts over time.

[0038] To provide a mathematical description of dynamic random zero bias, the integrated navigation and positioning unit models it as a first-order Gauss-Markov process. By using the relevant time constant and driving white noise, its random walk characteristics on the time axis are characterized, which can effectively reflect the zero bias instability of MEMS devices under long-term operation.

[0039] S220: Zero-velocity interval detection based on multidimensional feature constraints. In order to obtain the observation benchmark for Kalman filtering, the integrated navigation and positioning unit performs zero-velocity state detection (ZUPT). The integrated navigation and positioning unit collects raw IMU data within a sliding window and constructs a multidimensional decision criterion that includes acceleration statistical features, angular velocity energy features and local gravity consistency.

[0040] Specifically, the integrated navigation and positioning unit calculates the fluctuation amplitude (such as variance or standard deviation) of the acceleration data within the sliding window and the energy mean of the angular velocity data. The robot is determined to be in a zero-speed state when the system simultaneously meets the following conditions: The fluctuation amplitude of the acceleration data is less than the preset vibration threshold, indicating that the carrier has not undergone violent displacement; The average energy of the angular velocity data is less than the preset static threshold, indicating that the carrier has not undergone rotational motion; The deviation between the current average acceleration and the modulus of local gravitational acceleration is within a reasonable range, eliminating interference from non-gravitational acceleration.

[0041] S230: Online identification of zero-bias parameters based on error-state Kalman filter. During the detection of zero-speed state, the integrated navigation and positioning unit activates the error-state Kalman filter (ESKF) to identify parameters.

[0042] The integrated navigation and positioning unit constructs a state vector containing multi-dimensional error states, which includes attitude error, velocity error, position error, and the core parameters to be estimated: gyroscope zero-bias residual and accelerometer zero-bias residual.

[0043] Based on the error propagation mechanism of inertial navigation systems, the integrated navigation and positioning unit constructs a state transition relationship, which describes how attitude, velocity, and position errors evolve over time, and how sensor zero bias affects the navigation solution results.

[0044] In the observation phase, given that the robot's true speed in the zero-speed state is known to be zero, the integrated navigation and positioning unit directly inputs the speed value calculated by the inertial navigation system into the filter as the observation residual. Using the closed-loop correction mechanism of the Kalman filter, the filter inversely calculates the source of the speed divergence based on the speed residual, namely the current zero bias error of the gyroscope and accelerometer. Through continuous iterative updates, the filter outputs a converged zero bias estimate.

[0045] S240: Full-parameter decoupling error compensation utilizes the zero-bias parameter estimate updated in real time by S230, combined with the pre-calibration scale factor and installation error parameter loaded in S210, to perform full-parameter inverse decoupling compensation by the combined navigation and positioning unit.

[0046] In this step, the integrated navigation and positioning unit constructs inverse correction logic to subtract the estimated zero-bias component from the original measurement data and eliminates the scale factor deviation and inter-axis coupling effect through matrix operations. After the above compensation processing, the angular velocity and specific force data have long-term zero-bias stability. The integrated navigation and positioning unit inputs them into the strapdown inertial navigation algorithm for pose update, or as high-confidence measurement values ​​input into the subsequent perception fusion module to help determine the vibration and slippage state of the robot body.

[0047] In this embodiment, the multi-source positioning information fusion logic is executed by the integrated navigation and positioning unit. The integrated navigation and positioning unit utilizes the complementary characteristics of heterogeneous sensor data to fuse high-frequency dead reckoning data with accumulated errors and low-frequency laser scanning matching data with absolute position information, thereby outputting a six-degree-of-freedom pose estimate that has both high dynamic response and global consistency.

[0048] S250: The high-frequency dead reckoning integrated navigation and positioning unit based on kinematic constraints and adaptive covariance performs dead reckoning based on a three-dimensional kinematic model to obtain the robot's high-frequency pose prediction value. The input to the dead reckoning is the error-compensated IMU angular velocity and force data, as well as the mileage increment fed back by the wheel encoder.

[0049] The integrated navigation and positioning unit uses the compensated gyroscope angular velocity data and a quaternion update algorithm to calculate the robot's three-dimensional attitude angles in real time. In terms of position update, the integrated navigation and positioning unit combines the vehicle's non-holonomic constraints, projects the longitudinal linear velocity measured by the wheel encoder onto the current three-dimensional attitude direction, and calculates the three-axis displacement increment of the carrier in the global coordinate system.

[0050] To address wheel slippage or suspension caused by unstructured road surfaces, the integrated navigation and positioning unit (IMU) introduces an adaptive process noise covariance mechanism. The IMU monitors the vertical acceleration component output by the IMU in real time. When the variance of the vertical acceleration fluctuation exceeds a preset stability threshold, the vehicle is determined to be in a bumpy or suspended state. The IMU amplifies the corresponding terms in the position process noise covariance matrix (Q matrix) during dead reckoning using an exponential function. By expanding the process noise covariance, the IMU reduces the filter's reliance on wheel odometer data, thus relying more on the short-term accuracy of inertial navigation for pose recursion when slippage occurs.

[0051] S260: Laser scanning matching observation based on normal distribution transformation. In order to correct the cumulative error of dead reckoning over time, the integrated navigation and positioning unit uses lidar data to perform absolute positioning observation. The integrated navigation and positioning unit adopts the normal distribution transformation (NDT) algorithm to register the real-time acquired laser point cloud with the real-time maintained incremental global environmental point cloud map.

[0052] The integrated navigation and positioning unit first performs voxelization downsampling on the real-time laser point cloud. Then, the integrated navigation and positioning unit divides the global map into regular grid cells and calculates the probability density function of the point cloud distribution in each grid, transforming the discrete point cloud map into a continuously differentiable probability distribution field.

[0053] Using the prior pose output by S250 as initial value, the integrated navigation and localization unit constructs the maximum likelihood estimation problem and iteratively solves the pose transformation matrix that maximizes the matching degree between the real-time point cloud and the global distribution through Newton's method. This pose transformation matrix is ​​the robot's absolute position and attitude observation value. At the same time, the integrated navigation and localization unit calculates the inverse of the Hessian matrix in the NDT matching process and uses it as the benchmark of the measurement noise covariance matrix of the observation value of this frame, thereby quantifying the uncertainty of the current laser matching observation.

[0054] S270: The extended Kalman filter (EKF) is constructed as an algorithm carrier for multi-source data fusion in the spatiotemporal data fusion integrated navigation and positioning unit. The state vector of the extended Kalman filter contains the robot's position, velocity and attitude quaternions.

[0055] During the prediction phase of the filter, the integrated navigation and positioning unit uses the IMU data and odometer data in S250 as control inputs, deduces the state vector at the next moment based on the discretized kinematic equations, and updates the prediction error covariance matrix in combination with the adaptive process noise covariance matrix generated in S250.

[0056] During the filter update phase, when the S260 outputs a valid laser scan matching pose, the integrated navigation and positioning unit introduces it as an observation vector. The integrated navigation and positioning unit calculates the observation residual and calculates the Kalman gain by combining it with the measurement noise covariance matrix based on the Hessian matrix output by the S260. Using the Kalman gain, the extended Kalman filter performs a posteriori correction on the predicted state vector and converges the state covariance matrix.

[0057] To prevent erroneous laser matching results from compromising positioning stability, the integrated navigation and positioning unit (INP) introduces a degradation detection-based gating mechanism during the update phase. The INP performs eigenvalue decomposition on the Hessian matrix output by NDT matching. If the ratio of the minimum eigenvalue to the maximum eigenvalue is less than a preset degradation threshold (indicating insufficient constraint in a certain direction, such as in a corridor environment), or if the matching score is lower than a preset confidence threshold, the INP determines that the current observation is degraded, refuses to execute the Kalman measurement update for that frame, and only retains the dead reckoning result.

[0058] S280: Time delay compensation based on backtracking-reintegration. Considering the time consumption of lidar data processing and scanning matching calculation, which causes the observation pose output by S260 to be lagging, the integrated navigation and positioning unit maintains a state history buffer queue with timestamps. This queue stores the state vector and IMU measurement values ​​over a period of time.

[0059] When a delayed laser observation frame arrives, the integrated navigation and positioning unit retrieves the historical state corresponding to that moment from the buffer queue based on the timestamp of the frame. The integrated navigation and positioning unit performs a Kalman measurement update at that historical moment to correct the historical state vector. Subsequently, the integrated navigation and positioning unit uses the corrected historical state as a starting point and uses all IMU data after that moment stored in the buffer queue to perform forward Euler integration or Runge-Kutta integration (Re-propagation) until it is recursively pushed to the current system moment. Through the backtracking-reintegration mechanism, the global pose output by the integrated navigation and positioning unit combines the accuracy of laser positioning with the low latency of inertial navigation.

[0060] In this embodiment, the unstructured environment identification framework and the hierarchical map initialization are performed by a TBM-based variable resolution perceptual mapping unit. In order to overcome the limitation that traditional Bayesian occupancy grid maps can only express the binary state of occupancy and vacancy, the perceptual mapping unit introduces evidence theory and its variant Transmitted Belief Model (TBM) to construct a semantic map architecture that can simultaneously express the physical properties of the environment (rigidity, flexibility) and cognitive uncertainty (unknown, conflict).

[0061] S311: Constructing an Unstructured Environment Identification Framework The perceptual mapping unit first defines an identification framework for unstructured environments, denoted as... To address the accessibility needs of mobile robots in the field, the perception and mapping unit defines the set of basic mutually exclusive elements of the environment as follows: .

[0062] In the formula: (Rigid): Represents rigid road surfaces, including asphalt roads, hard dirt roads, and dry riverbeds. In such areas, robots have high mobility and low risk of slipping.

[0063] (Soft / Flexible): This refers to flexible or semi-rigid road surfaces, including grass, mud, sand, and gravel roads. In such areas, robots can pass through but there is a risk of sinking or wheel slippage, requiring adjustments to the motion control strategy.

[0064] (Obstacle): Represents an impassable obstacle, including trees, rocks, deep pits, and steep slopes. Based on TBM theory, a recognition framework is constructed using perceptual mapping units. The power set is used to describe uncertain assumptions; the power set contains... All possible subsets of elements, except for single subsets, are used by the perceptual mapping unit to describe the fuzzy states in navigation using the following composite assumptions: Passable area assumption { When geometric sensors such as LiDAR detect a flat ground but lack texture information to distinguish materials, the perception mapping unit assigns the evidence to the set { This indicates that the perception mapping unit confirms that the area is not an obstacle, but there is ambiguity in the judgment between rigid and flexible properties.

[0065] The assumption of complete ignorance { }: Represents the complete set When the detection area is in the sensor's blind spot or has not yet been detected, the system state corresponds to the entire set.

[0066] Conflict state (empty set): When different sensors give completely contradictory observations of the same area (e.g., LiDAR determines it to be a flat surface, while vision determines it to be a puddle obstacle), the perception mapping unit will generate a non-zero empty set mass in the fusion calculation, which is used to characterize the degree of conflict between perception sources.

[0067] S312: Establishing a Hash-Index-Based Hierarchical Pyramid Map. To support variable resolution mapping and dynamic quadtree splitting mechanisms, the perceptual mapping unit abandons the single-resolution planar grid and establishes a hierarchical raster map system based on a pyramid structure. The perceptual mapping unit defines a multi-level resolution hierarchy from the basic coarse-grained layer (e.g., side length 1.0 meter) to the highest fine-grained layer (e.g., side length 0.05 meter).

[0068] In terms of memory storage and indexing, the perceptual mapping unit uses a spatial hash table containing hierarchical information as the underlying data structure. The perceptual mapping unit uses spatial coordinates (x, y, z) combined with the current resolution hierarchy depth to generate a unique hash key, and maps the key to the corresponding raster object. This indexing method enables the perceptual mapping unit to store parent and child raster nodes of different resolutions in the same data container at the same time, and allocates memory space only for the actually observed areas, achieving efficient storage in sparse scenes.

[0069] Each grid object stores a Basic Probability Assignment (BPA) vector. The dimension of the BPA vector corresponds to the cardinality of the identification frame power set (8 dimensions for a three-element frame). Each component in the BPA vector corresponds to the trust level of rigidity, flexibility, obstacles, and their respective combination subsets, and the sum of the values ​​of all components is strictly constrained to 1.

[0070] S313: Map State Initialization and Empty Confidence Assignment Before the system starts or enters a new area, the perception and mapping unit performs map initialization operations. Since the robot has not yet acquired any environmental observation data at this time, the perception and mapping unit must accurately reflect the completely unknown cognitive state in the map.

[0071] The perceptual mapping unit uses a null confidence assignment strategy to initialize unexplored regions. Specifically, the perceptual mapping unit assigns the BPA vectors of all unexplored rasters corresponding to the entire set. The component corresponding to BPA is assigned a value of 1, while the components corresponding to all other subsets (including single subsets and composite subsets) are assigned a value of 0. This initialization setting ensures that the initial state of the map does not contain any prior bias, so that the weak sensor evidence received later can directly drive the update of the BPA value, thereby ensuring the objectivity of the environmental attribute determination.

[0072] In this embodiment, the basic confidence function (BPA) construction logic is executed by the perception mapping unit, which is responsible for transforming the raw observation data of multi-source sensors into a confidence distribution under the evidence theory framework. The perception mapping unit establishes a quantitative mapping relationship between the sensor physical measurement values ​​and the identification framework subset, providing input for subsequent evidence fusion.

[0073] S321: The perceptual mapping unit for constructing point cloud evidence based on geometric statistical features first processes the 3D point cloud data acquired by the LiDAR. For each target grid cell in the local grid map, the perceptual mapping unit extracts the set of point clouds that fall within the target grid cell.

[0074] To quantify the geometric properties of the ground, the perceptual mapping unit calculates the three-dimensional coordinate covariance matrix of the point cloud set and uses principal component analysis (PCA) to obtain the principal direction of the point cloud distribution. The perceptual mapping unit selects the eigenvector corresponding to the minimum eigenvalue as the normal vector estimate of the target raster surface, calculates the angle between the normal vector and the gravity direction, and defines the angle value as the slope feature. At the same time, the perceptual mapping unit calculates the standard deviation of the coordinate distribution of the point cloud set in the vertical direction and defines the standard deviation value as the roughness feature.

[0075] After obtaining the slope and roughness features, the perception mapping unit uses a weighted summation method, combined with preset weight coefficients, to integrate the slope and roughness features into a comprehensive geometric feature index.

[0076] Since geometric information alone is insufficient to distinguish between a flat, rigid road surface and a flat, flexible road surface, but can effectively differentiate between passable areas and obstacles, the perception mapping unit primarily assigns the evidence from the geometric sensors to the composite hypothesis (i.e., a passable area without obstacles) and the single obstacle hypothesis. The perception mapping unit constructs a nonlinear mapping relationship (e.g., an S-shaped function mapping) to transform the comprehensive geometric feature indicators into basic probability assignments: As the value of the comprehensive geometric feature index increases (meaning the terrain is steeper or more complex), the confidence of the perception mapping unit in assigning the obstacle hypothesis increases monotonically. As the value of the comprehensive geometric feature index decreases (meaning the terrain is flatter), the confidence of the perceptual mapping unit in the feasible composite hypothesis increases monotonically. This mapping relationship includes adjustable sensitivity and threshold parameters to adapt to the terrain undulation characteristics of different field environments.

[0077] S322: The visual evidence construction perception mapping unit based on semantic segmentation probability processes the image data acquired by the visual camera to supplement texture and material information. The perception mapping unit uses a pre-trained semantic segmentation network to perform pixel-level classification of the two-dimensional image and outputs a probability vector of each pixel in the image belonging to a preset semantic category (including asphalt, grass, rock, etc.).

[0078] To map two-dimensional image features to three-dimensional space, the perceptual mapping unit performs inverse perspective projection transformation (IPM). The perceptual mapping unit uses the camera intrinsic and extrinsic parameter matrices to establish the correspondence between the image pixel coordinate system and the local raster map coordinate system. Since there are cases where multiple pixels are projected onto the same target raster, the perceptual mapping unit performs statistical aggregation processing (e.g., calculates the arithmetic mean) on the probability vectors of all pixels falling into the target raster, thereby obtaining the average semantic probability vector of the target raster.

[0079] The perceptual mapping unit constructs a visual BPA based on the correspondence between semantic categories and recognition frames: The perception mapping unit accumulates the probability values ​​of belonging to the rigid pavement category (such as asphalt, cement, hard soil) in the average semantic probability vector and assigns the accumulated result to the rigidity hypothesis. The perception mapping unit accumulates the probability values ​​of belonging to the flexible road surface category (such as grass, soil, and gravel) in the average semantic probability vector and assigns the accumulated result to the flexible hypothesis. The perceptual mapping unit accumulates the probability values ​​of belonging to obstacle categories (such as trees and rocks) in the average semantic probability vector and assigns the accumulated result to the obstacle hypothesis.

[0080] S323: Uncertainty-based cognitive discount. Considering the noise in sensor observations and the classification uncertainty caused by changes in ambient light, the perception mapping unit performs a confidence discount operation on the generated original BPA.

[0081] The perception mapping unit calculates a discount factor between 0 and 1 based on the physical observation conditions.

[0082] For lidar evidence, the sensing mapping unit counts the number of point clouds within the target grid. The discount factor is negatively correlated with the number of point clouds: the fewer the number of point clouds, the larger the discount factor, indicating that the reliability of geometric observation is lower.

[0083] Specifically, the discount factor for geometric evidence The negative exponential function can be used for calculation:

[0084] In the formula: The number of effective point clouds within the target grid. This represents a preset point cloud density adjustment constant used to control the sensitivity of the discount factor to changes in the number of point clouds (e.g., a value of 20 is possible). When it increases, A decrease indicates a reduction in the confidence level that needs to be deducted, meaning an increase in the reliability of the evidence. This formula shows that when the number of point clouds... When it approaches 0, the discount factor Approaching 1 (complete distrust); as the number of point clouds increases, the discount factor rapidly decays and approaches 0 (complete preservation).

[0085] For visual evidence, the perceptual mapping unit calculates the information entropy of the average semantic probability vector of the target raster. The discount factor is positively correlated with the information entropy: the more chaotic the probability distribution (the higher the entropy), the larger the discount factor, indicating that the reliability of the semantic classification result is lower.

[0086] Specifically, the discount factor for visual evidence It can be calculated using a normalization function based on the maximum information entropy:

[0087] In the formula: The information entropy of the current raster average semantic probability vector. To identify the number of mutually exclusive elements in the frame (here) (corresponding to rigid, flexible, and obstacle) Let be the probability value of each element. This formula ensures that the discount factor takes values ​​in the interval [0,1] and is linearly positively correlated with the information entropy. This represents the index number of the mutually exclusive element, with a value range of 1. to .

[0088] Using the calculated discount factor, the perception mapping unit corrects the original BPA: the perception mapping unit deducts the confidence of each specific hypothesis (rigid, flexible, obstacle) proportionally, and reassigns the deducted confidence share to the whole set representing the state of complete ignorance. When the sensor data quality is poor, the confidence is forcibly transferred to the whole set, thereby reducing the risk of misjudgment of the system under weak observation conditions and reflecting the system's conservative estimate of the uncertainty of environmental cognition.

[0089] In this embodiment, the basic confidence function (BPA) construction logic is executed by the perception mapping unit, which is responsible for transforming the raw observation data of multi-source sensors into a confidence distribution under the evidence theory framework. The perception mapping unit establishes a quantitative mapping relationship between the sensor physical measurement values ​​and the identification framework subset, providing input for subsequent evidence fusion.

[0090] In this embodiment, the evidence fusion and conflict energy spectrum extraction logic is executed by the perception mapping unit. After acquiring geometric evidence based on point cloud geometric features and visual evidence based on image semantic features, the perception mapping unit uses the conjunction combination rule of the Transmission Belief Model (TBM) to orthogonally fuse these two types of heterogeneous evidence. While reducing the uncertainty of environmental cognition, the perception mapping unit explicitly extracts the conflict information between sources and constructs a local conflict energy spectrum that reflects the complexity of the environment.

[0091] S331: The perceptual mapping unit based on the TBM conjunction rule receives the geometric evidence basic probability assignment (Geometric BPA) and the visual evidence basic probability assignment (Visual BPA). Under the same identification framework, the perceptual mapping unit assumes that the lidar observation and the visual observation are statistically independent and performs orthogonal summation operation.

[0092] The perceptual mapping unit iterates through every focal element with non-zero confidence in the geometric BPA and every focal element with non-zero confidence in the visual BPA, constructing all possible focal element pairs. For each focal element pair, the perceptual mapping unit executes the following fusion logic: Set intersection operation: The perceptual mapping unit calculates the intersection of two focal elements. For example, when the geometric focal element is a collapsible composite hypothesis (including rigidity and flexibility) and the visual focal element is a rigid single hypothesis, the intersection of the two is the rigid single hypothesis, thus realizing the precision of fuzzy information; when the geometric focal element is an obstacle and the visual focal element is a collapsible composite hypothesis, the intersection of the two is an empty set, indicating that there is a logical contradiction between the two.

[0093] Joint confidence calculation: The perceptual mapping unit calculates the product of the confidence values ​​corresponding to the two focal elements, and uses this product as the joint confidence component of the above intersection result.

[0094] Accumulation of similar terms: The perceptual mapping unit accumulates and sums all joint confidence components that produce the same intersection result, and uses the accumulated result as the confidence of that subset in the fused new BPA vector.

[0095] S332: The extraction and quantification of local conflict energy spectrum differs from the traditional Dempster-Shafer evidence theory, which forces normalization after fusion. The perceptual mapping unit deliberately retains the joint confidence of the generated empty set during the fusion process and defines the confidence of the empty set as the conflict energy.

[0096] During the orthogonal fusion traversal, the perceptual mapping unit identifies all focal element pairs whose intersection is an empty set. These focal element pairs correspond to perceptual conflict scenarios in the physical environment. For example, geometric features show that the ground is flat (supporting passability), but texture features show that it is a puddle or trap (supporting obstacles); or geometric features show that there are bumps (supporting obstacles), but visual features show that they are tall weeds (supporting flexible passability).

[0097] The perceptual mapping unit sums the joint confidence components of all focal element pairs that lead to the empty set to obtain the total conflict quality of the current raster cell. The perceptual mapping unit treats this total conflict quality as an independent scalar attribute, maps it to and stores it in the spatial hash table of the hierarchical raster map. The conflict quality of all rasters together constitutes a conflict energy spectrum aligned with the map resolution. The magnitude of this energy spectrum quantifies the degree of inconsistency of multi-source observations in the current area, serving as the core driving variable for subsequent dynamic splitting of map resolution.

[0098] S333: Reconstruction and Output of the Fusion BPA After extracting the conflict energy, the perception mapping unit outputs a new fused BPA vector, which includes updated confidence for rigid, flexible, obstacles and their combined subsets, as well as a non-zero empty set confidence (i.e., conflict energy).

[0099] The perception mapping unit does not execute the Dempster normalization rule (i.e., it does not proportionally allocate the quality of the empty set to other hypotheses). This avoids the erroneous decision paradox caused by forced normalization in cases of severe conflict (e.g., conflict energy close to 1). Preserving the confidence of the empty set allows the system to identify the special state of severe logical paradox in the current observation results, thereby providing a clear risk confidence for the subsequent path planning module, or serving as a basis for triggering active detection behavior (such as reducing movement speed or requesting manual confirmation).

[0100] In this embodiment, the variable resolution splitting mechanism is executed by the perception mapping unit. The perception mapping unit uses the local conflict energy spectrum generated in the aforementioned evidence fusion step as a driving signal to dynamically adjust the spatial resolution of the raster map. The variable resolution splitting logic follows the principle of on-demand subdivision: in areas with simple environmental features or no conflict, it maintains a coarse-grained low resolution to save storage and computing resources; in areas with high conflict energy or environmental edge areas, it automatically subdivides into high resolution to accurately capture obstacle outlines and terrain boundaries.

[0101] S341: The split decision sensing and mapping unit based on the conflict energy spectrum traverses all active leaf node grids in the current map. For each target leaf node grid, the sensing and mapping unit reads the conflict energy (i.e., empty set confidence) of the target leaf node grid.

[0102] The perception mapping unit executes the following split decision logic: Conflict threshold determination: If the conflict energy of the target leaf node grid is greater than the preset splitting threshold, it indicates that there is a strong perceptual contradiction inside the target leaf node grid (for example, covering the boundary between grass and rocks, causing the laser and visual features to be irreconcilable at the coarse-grained level). At this time, the perceptual mapping unit determines that the target leaf node grid needs a higher spatial resolution to decouple the physical conflict.

[0103] Hierarchical limitation determination: The perception mapping unit checks the hierarchical depth of the target leaf node raster. If the current hierarchical depth is less than the maximum resolution hierarchical level set by the system, splitting is allowed; otherwise, even if the conflict energy is too high, the perception mapping unit will not perform the splitting operation to prevent computational overflow caused by infinite subdivision.

[0104] S342: Quadtree Topology Splitting and Hash Index Update When the splitting condition is met, the perceptual mapping unit performs a quadtree topology splitting operation. The perceptual mapping unit uses the geometric center of the target leaf node grid (parent node) as a reference to divide the two-dimensional space plane where the parent node is located into four quadrant regions (child nodes) of equal area.

[0105] The perception mapping unit calculates the geometric parameters of the child nodes: the grid step size of the child node is set to half of the grid step size of the parent node; the geometric center coordinates of the four child nodes are the coordinates of the parent node center after being offset by one-quarter of the parent node step size in the X and Y directions.

[0106] The perceptual mapping unit uses the mapping rules defined in the aforementioned spatial hash table to generate hash keys for four child nodes based on the calculated center coordinates of the four child nodes and the depth value obtained by adding 1 to the layer depth. The perceptual mapping unit allocates memory objects for these four new keys in the spatial hash table and marks the parent node as a non-leaf node, thereby completing the topology update from coarse-grained to fine-grained.

[0107] S343: Evidence inheritance based on maximum entropy discount After generating child node objects, the perception mapping unit performs the downward transmission initialization of parent node information. Since the child nodes have just been generated and there is no direct sensor observation data, the perception mapping unit adopts the maximum entropy discount strategy for data initialization to avoid false information gain caused by direct copying.

[0108] The perception mapping unit acquires the BPA vector of the parent node and sets an inheritance discount factor (typically ranging from 0.6 to 0.8). The perception mapping unit multiplies the confidence of each specific hypothesis (rigid, flexible, obstacle) in the parent node's BPA vector by the inheritance discount factor, and uses the calculation result as the initial confidence of the corresponding hypothesis for the four child nodes. At the same time, the perception mapping unit calculates the sum of the confidence shares that are deducted from the parent node's BPA vector and adds this sum to the total (completely unknown) confidence of the four child nodes. This ensures that while the child nodes inherit the general attributes of the parent node, they also add uncertainty descriptions, waiting for subsequent higher-resolution sensor measurements to update and correct them.

[0109] S344: Reverse Merging Based on Euclidean Distance Similarity. To prevent the map data from expanding indefinitely, the perceptual mapping unit periodically performs a reverse merging (pruning) operation. The perceptual mapping unit checks four sibling child nodes with the same parent node. If these four sibling child nodes simultaneously meet the following conditions: Low-conflict state: The conflict energy of all four sibling child nodes is lower than the preset merging threshold; Semantic consistency: The BPA vectors of the four sibling child nodes are statistically highly similar. The perceptual mapping unit calculates the Euclidean distance between each pair of the BPA vectors of the four sibling child nodes. If the calculated maximum Euclidean distance is less than the preset consistency threshold, it indicates that the four sub-regions are uniform in physical properties.

[0110] When the above conditions are met, the perception mapping unit performs a merging operation: the perception mapping unit calculates the arithmetic mean of the BPA vectors of the four sibling child nodes, uses this arithmetic mean as the BPA vector of the parent node, and then destroys the memory objects of the four sibling child nodes and restores the parent node as an active leaf node. This step dynamically releases redundant storage space while ensuring map accuracy.

[0111] In this embodiment, the spatiotemporal dynamic attenuation and path attribute determination logic is executed by the perception mapping unit. This logic aims to solve the timeliness problem of environmental perception and transform the fuzzy confidence level under the evidence theory framework into a deterministic passage cost that can be directly used by the path planning module.

[0112] S351: Time-domain decay of confidence based on timestamps. In order to cope with dynamic obstacles in unstructured environments and changes in environmental attributes over time, the perception mapping unit introduces a time dimension management mechanism in the raster map.

[0113] In the spatial hash table structure described in S312, each target raster object stores a last update timestamp in addition to the basic probability assignment (BPA) vector. Whenever the perception mapping unit uses new sensor data to fuse and update the target raster object, the perception mapping unit synchronously writes the current system time into the last update timestamp of the target raster object.

[0114] The perception mapping unit periodically performs a confidence decay operation. The perception mapping unit reads the last update timestamp of the target raster object and calculates the time difference between the current system time and the last update timestamp. The perception mapping unit calculates the time decay coefficient based on this time difference. The time decay coefficient is negatively correlated with the time difference: the larger the time difference (indicating that the data is older), the smaller the decay coefficient and the closer it is to 0; the smaller the time difference (indicating that the data is fresher), the closer the decay coefficient is to 1.

[0115] As a preferred embodiment, the time decay coefficient The calculation is performed using an exponential decay model:

[0116] In the formula, This represents the time decay coefficient at the current moment, with a value range of (0, 1]. This represents the preset time forgetting factor, used to adjust the rate at which reliability decays over time (e.g., a value of 0.5 is possible). This represents the time difference between the current system time and the last update timestamp stored in the raster object, in seconds.

[0117] The perceptual mapping unit uses a time decay coefficient to correct the BPA vector of the target raster object: The perception mapping unit multiplies the confidence values ​​of all specific hypotheses (including rigidity, flexibility, obstacles and their composite subsets) in the BPA vector, except for the whole set (complete ignorance hypothesis), by a time decay coefficient, thereby reducing the degree of trust in outdated information. The perception mapping unit calculates the sum of the confidence shares that were cut off in the above multiplication operations; The perception mapping unit adds the sum of the reduced confidence shares to the total confidence of the entire set. Through this step, the confidence of the specific attributes of regions that have not been observed for a long time will gradually decrease over time and return to a completely unknown state, thereby preventing outdated historical data from misleading the current path decision.

[0118] S352: The BPA vector generated by the probabilistic projection evidence theory based on the Pignistic transformation contains composite assumptions (e.g., passable but uncertain material) and conflict information, which cannot be directly used by standard path planning algorithms. Therefore, the perception mapping unit performs the Pignistic probabilistic transformation to convert the fuzzy evidence distribution into a deterministic probability distribution for decision-making.

[0119] The perception mapping unit traverses each single-element hypothesis (i.e., rigid road surface, flexible road surface, obstacle) in the recognition framework. For each single-element hypothesis, the perception mapping unit calculates its corresponding Pignistic probability, and the calculation logic follows the cardinality average distribution principle: Conflict redistribution: The perceptual mapping unit first removes the empty set confidence (conflict energy) from the BPA vector and normalizes the remaining confidence to ensure that the sum of the confidence of all non-empty set hypotheses is 1.

[0120] Composite set decomposition: For each composite hypothesis set containing the target single-element hypothesis, the perception mapping unit divides the confidence value of the composite hypothesis set by the number of single elements contained in the set (i.e., the set cardinality) to obtain the average distribution.

[0121] Probability aggregation: The perception mapping unit adds the confidence level of the target single-element hypothesis itself to the average distribution obtained from all composite hypothesis sets containing that element, and sums them to obtain the final probability value of the target single-element hypothesis.

[0122] Through the Pignistic probability transformation, the perception mapping unit forces the unknown or ambiguous composite confidence to be evenly distributed among the possibilities, thereby providing an equally distributed probability estimate at the decision-making level.

[0123] S353: The perceptual mapping unit for generating multi-level access cost maps uses the calculated probabilities of three single elements (rigid road surface probability, flexible road surface probability, and obstacle probability) and combines them with the robot's own motion performance parameters to synthesize the comprehensive access cost of the target grid object.

[0124] The perception mapping unit calculates the overall passage cost using a weighted summation method: The perception mapping unit assigns a low-generation value weight to the rigid road surface probability, representing an ideal road surface; The perception mapping unit assigns a medium value weight to the probability of flexible road surfaces, representing road surfaces that are passable but have high energy consumption or a risk of slippage. The perception mapping unit assigns high-value weights to obstacles, representing prohibited or high-risk areas.

[0125] The perception and mapping unit further sets an impassable threshold. If the obstacle probability of the target grid object exceeds the impassable threshold, the perception and mapping unit directly marks the target grid object as impassable.

[0126] Specifically, if the target grid object is in a completely unknown state (i.e., the confidence level of the entire set is close to 1), after the Pengistic transformation described in S352, the probabilities of its rigid road surface, flexible road surface, and obstacles will be equally distributed (each about one-third). At this time, the comprehensive passage cost calculated by the perception mapping unit will be higher than the cost of the rigid road surface but significantly lower than the cost of the obstacle. This cost setting prompts the path planning algorithm to follow the following strategy: when there is a clear rigid road, priority is given to selecting the road; when there is no clear road, it is allowed to try to cross the unknown area for exploration, thereby realizing the automatic balance between exploration and utilization in path planning.

[0127] In this embodiment, the path planning logic for the three-dimensional equivalent unfolding of the curved surface is executed by the path planning unit. The path planning logic for the three-dimensional equivalent unfolding of the curved surface aims to establish a metric mapping relationship between the three-dimensional surface manifold and the two-dimensional search map, and to convert the geometric distance and gravity work in the three-dimensional space into equivalent two-dimensional plane values.

[0128] S411: The local surface connectivity verification path planning unit first constructs a graph structure for path search based on the layered raster map with height information generated by the aforementioned perception mapping unit. In the graph structure, each raster center is a graph node, and the connection relationship between adjacent rasteres is a graph edge.

[0129] To obtain accurate terrain geometry parameters, the path planning unit performs local connectivity checks on each current graph node and its adjacent neighboring graph nodes. The path planning unit reads the elevation values ​​and normal vector data of the current graph node and neighboring graph nodes stored in the raster map. Using the three-dimensional spatial coordinates of the current graph node and neighboring graph nodes, the path planning unit constructs a three-dimensional displacement vector connecting the current graph node and neighboring graph nodes. The path planning unit calculates the dot product of the three-dimensional displacement vector and the normal vector at the current graph node. If the dot product value exceeds a preset step threshold, the path planning unit determines that there are physical discontinuities (such as cliffs or steps) between the current graph node and neighboring graph nodes, and directly removes the edge connecting the current graph node and neighboring graph nodes in the search graph.

[0130] S412: After confirming the continuity of the local surface, the path planning unit calculates the physical travel distance between the current graph node and neighboring graph nodes. The path planning unit uses the three-dimensional Euclidean distance algorithm to calculate the spatial straight-line distance to replace the traditional two-dimensional horizontal projection distance. The path planning unit calculates the projection distance between the current graph node and neighboring graph nodes on the horizontal plane. The path planning unit calculates the elevation difference between the current graph node and neighboring graph nodes. Based on the Pythagorean theorem, the path planning unit uses the horizontal projection distance and elevation difference to calculate the spatial straight-line distance connecting the current graph node and neighboring graph nodes. The path planning unit defines the spatial straight-line distance as the basic unfolded distance, which represents the physical distance that the robot actually needs to travel on the three-dimensional curved surface.

[0131] S413: Anisotropic cost correction based on gravity work. Terrain undulations will cause changes in robot motion energy consumption. The path planning unit performs anisotropic cost correction on the basic deployment distance based on the principle of gravity work.

[0132] The path planning unit calculates the angle between the 3D displacement vector and the horizontal plane to obtain the pitch slope angle. Based on the robot's movement direction (from the current graph node to neighboring graph nodes), the path planning unit determines the slope type and calculates the slope correction factor. Uphill condition: When the elevation increment in the direction of movement is positive, the path planning unit introduces the uphill resistance coefficient (a constant greater than 1.0). The path planning unit calculates the sine value of the pitch slope angle and multiplies this sine value by the uphill resistance coefficient to generate a slope correction coefficient greater than 1, which represents the additional energy consumption caused by overcoming gravity.

[0133] Downhill condition: When the elevation increment in the direction of movement is negative, the path planning unit introduces a safe slope threshold.

[0134] If the pitch angle is less than the safe slope threshold, the path planning unit sets a smooth road surface gain coefficient with a value between 0 and 1 as the slope correction coefficient to characterize the energy consumption reduction under gravity assistance. If the pitch slope angle is greater than the safe slope threshold, the path planning unit introduces a braking penalty coefficient. The path planning unit calculates and generates a slope correction coefficient greater than 1 based on the pitch slope angle to characterize the control risk cost caused by forced operation.

[0135] The path planning unit multiplies the basic unfolded distance by the slope correction factor to obtain the equivalent geometric distance after slope correction. Through the anisotropic cost correction step, the cost of the same path is not equal when there is bidirectional traffic, thus constructing directed weighted edges in the search graph.

[0136] S414: The multidimensional cost fusion and search graph update path planning unit ultimately fuses the cost of the geometric dimension and the risk of the semantic dimension to generate comprehensive connection edge weights for heuristic search algorithms.

[0137] The path planning unit reads the comprehensive passage cost generated in the previous steps. Here, the path planning unit defines the comprehensive passage cost as the passage impedance coefficient per unit distance. The path planning unit reads the equivalent geometric distance after slope correction calculated in step S413. The path planning unit calculates the product of the equivalent geometric distance after slope correction and the comprehensive passage cost to obtain the final edge weight connecting the current graph node and the neighboring graph nodes.

[0138] The path planning unit updates the adjacency matrix of the search graph using the updated final edge weights. In this search graph, the connection weights between nodes incorporate the three-dimensional path extension, gravity work loss, road slippage risk, and environmental unknowns. This multi-dimensional cost fusion mechanism enables the heuristic search algorithm to plan a path that is optimal in terms of both energy consumption and travel risk in three-dimensional undulating terrain.

[0139] In this embodiment, the anti-interference trajectory tracking control logic is executed by the motion control unit. The anti-interference trajectory tracking control logic aims to control the mobile robot to accurately track the target trajectory based on the three-dimensional reference path containing terrain semantic information generated in the aforementioned steps, and to perform real-time compensation for dynamic disturbances caused by complex terrain (such as muddy slippery or bumpy terrain).

[0140] S510: Tracking error linearization motion control unit based on Frenet coordinate system receives the three-dimensional reference path output by the path planning unit. The motion control unit uses the vehicle's current positioning information (including position coordinates, heading angle, and longitudinal velocity) to search for the path point on the three-dimensional reference path that is closest to the vehicle's current position in Euclidean distance, and defines the searched path point as the projection matching point.

[0141] The motion control unit establishes the Frenet coordinate system, calculates the geometric relationship between the vehicle's current position and the projected matching point, and solves for two key tracking error quantities: Lateral error: The perpendicular distance from the vehicle's center of gravity to the tangent direction of the projected matching point.

[0142] Heading error: The difference between the vehicle's current heading angle and the path tangent angle at the projection matching point.

[0143] Considering the impact of three-dimensional terrain on control safety, the motion control unit reads the slope correction coefficient at the projection matching point (from the aforementioned S413 step). The motion control unit uses the slope correction coefficient to dynamically limit the vehicle's reference cruise speed: the motion control unit maps the preset maximum cruise speed with the slope correction coefficient in a negative correlation (e.g., by dividing by the coefficient) to obtain the dynamic speed limit value at the current moment. When the slope correction coefficient is large (indicating steep slopes or high-risk road sections), the dynamic speed limit value is reduced accordingly to prevent the vehicle from experiencing power saturation or overturning during the tracking process.

[0144] S520: Adaptive Dynamic Model Parameters Based on Semantic Probability To achieve high-precision model predictive control, the motion control unit needs to rely on the vehicle dynamics model for state prediction. The motion control unit uses the environmental semantic information provided by the perception layer to correct the key parameters in the vehicle dynamics model in real time to match the physical properties of the current road surface.

[0145] The motion control unit reads the pignistic probability distribution of the current grid cell where the vehicle is located to obtain the probability of rigid road surface and the probability of flexible road surface. Based on the probability distribution, the motion control unit dynamically adjusts the equivalent tire lateral stiffness and road adhesion coefficient in the vehicle dynamics model. The adjustment logic follows the principle of probability weighted combination: using the rigid road surface parameters and flexible road surface parameters as baseline values, a weighted sum is performed with the rigid road surface probability and the flexible road surface probability as weights, respectively. As a specific calculation example, the parameter adjustment formula can be expressed as:

[0146]

[0147] In the formula: For equivalent tire lateral stiffness, The road surface adhesion coefficient; , These are the pre-calibrated rigid pavement parameters; , These are the pre-calibrated parameters for flexible pavement. For rigid pavement probability, The probability of a flexible road surface.

[0148] Through the adaptive step of dynamic model parameters, when the vehicle enters a flexible road surface area (where the probability of a flexible road surface is relatively high), the equivalent tire lateral stiffness value is automatically reduced. In subsequent model prediction calculations, the reduced equivalent tire lateral stiffness will prompt the controller to calculate a larger front wheel steering angle command to compensate for the insufficient lateral force caused by the soft ground, thereby avoiding understeer.

[0149] S530: Total disturbance estimation based on extended state observer. In actual driving, there are disturbances that cannot be fully described by the theoretical model, such as wind resistance, road potholes and impacts, and model parameter mismatch. The motion control unit runs an extended state observer (ESO) to estimate the total disturbance in real time.

[0150] The motion control unit defines the total disturbance as an extended state variable to characterize the deviation between the actual yaw rate dynamics of the vehicle and the dynamics of the theoretical model. The motion control unit takes the front wheel steering angle command as the system input and the actual yaw rate measured by the sensor as the observation value. The motion control unit calculates the estimation error between the actual yaw rate and the yaw rate estimated by the observer. The motion control unit uses a preset observer gain matrix to amplify the estimation error and uses the amplified error value to update the estimated value of the total disturbance in real time. The value of the total disturbance reflects the sum of the unmodeled external torque and internal parameter perturbations experienced by the vehicle at the current moment.

[0151] S540: Solving and Executing Multi-Objective Optimization Control Quantities. The motion control unit constructs a quadratic programming (QP) problem for model predictive control (MPC). The motion control unit sets an objective cost function, which aims to minimize tracking error and smooth control input. The construction logic of the objective cost function includes the accumulation of the following weighted terms: Lateral accuracy term: Weighted by the square of the lateral error in the prediction time domain to ensure that the vehicle stays close to the path; Heading stability term: Weighted average of the squares of the heading error in the prediction time domain to ensure vehicle body attitude alignment; Control smoothing term: The square of the rate of change of the front wheel steering angle in the prediction time domain is weighted to avoid drastic fluctuations in the control quantity.

[0152] As a concrete example of mathematical expression, the objective cost function It can be represented as:

[0153] In the formula: To predict the length of the time domain; These are the prediction errors; To control the increment; For the corresponding weighting coefficients, This is the index for the predicted time.

[0154] To achieve anti-interference control, the motion control unit incorporates the total disturbance estimated in step S530 into the state prediction equation of the MPC. When constructing the prediction model, the motion control unit treats the total disturbance as an exogenous disturbance term that remains constant in the prediction time domain and adds it to the state update equation of the yaw rate. The motion control unit calls the numerical optimization solver to solve the quadratic programming problem containing the total disturbance term and calculates a series of optimal front wheel steering angle change rates. Since the total disturbance has been explicitly included in the prediction model, the front wheel steering angle change rate calculated by the optimization solver has automatically included the component required to counteract the total disturbance.

[0155] The motion control unit updates the final front wheel steering angle command based on the first control increment in the sequence and sends it to the underlying actuator via the CAN bus to drive the vehicle steering motor to operate, repeating the steps S510 to S540 above to achieve closed-loop trajectory tracking control.

[0156] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for determining the path attributes of mobile robots based on the fusion of TBM evidence theory, characterized in that, Includes the following steps: Step S1: Collect multi-source heterogeneous data, establish a raw observation dataset, align the heterogeneous data based on hardware timestamps, and output a spatiotemporally synchronized raw observation dataset. Step S2: Construct an unstructured environment identification framework and a basic confidence function, and establish an unstructured environment identification framework that includes mutually exclusive elements of rigid road surface, flexible road surface and obstacles; based on the original observation dataset output in Step S1, map the geometric statistical features of the three-dimensional point cloud data to geometric evidence basic probability assignments, map the semantic segmentation probability of the image frame data to visual evidence basic probability assignments, and construct an evidence grid map of the environment, which is composed of multiple grid units; Step S3: Evidence fusion and local conflict energy spectrum extraction. Construct fusion logic based on Transmission Belief Model (TBM). Use the TBM conjunction rule to orthogonally fuse the basic probability assignments of the geometric evidence and the basic probability assignments of the visual evidence generated in Step S2. Calculate the empty set mass generated by the fusion and define the empty set mass as the local conflict energy. Construct the local conflict energy spectrum. At the same time, redistribute the original observation dataset from Step S1 to the grids of the grid map and perform data fusion calculation to obtain the fusion confidence of each grid. Step S4: Conflict-driven map resolution update. Based on the local conflict energy spectrum constructed in step S3, determine whether to dynamically split or merge the quadtree map structure based on hash table index to generate a variable resolution raster map, and redistribute the original observation dataset in step S1 to the updated raster. Step S5: Path attribute determination and semantic generation. The confidence level after fusion in step S3 is converted into the attribute probability of each grid in the grid map using the Pignistic probability transformation to determine the road surface attributes and generate a semantic map.

2. The method for determining the path attributes of a mobile robot based on TBM evidence theory fusion according to claim 1, characterized in that, The pose data in step S1 is provided by the integrated navigation and positioning unit, which performs the following processing: An online algorithm for identifying random error parameters of strapdown inertial navigation is implemented. To address the zero-bias instability characteristics of micro-inertial measurement units, a high-order hybrid error model incorporating gyroscope and accelerometer zero-bias is constructed. The observation benchmark is obtained by using zero-velocity state detection, the error state Kalman filter is activated to identify parameters, the converged zero-bias estimate is output and full-parameter decoupling error compensation is performed. The real-time laser point cloud is scanned and matched with the global point cloud map using the normal distribution transformation algorithm to obtain the absolute observation pose. The dead reckoning pose and the absolute observation pose are then fused using an extended Kalman filter.

3. The method for determining the path attributes of a mobile robot based on TBM evidence theory fusion according to claim 1, characterized in that, The specific process of mapping geometric statistical features to geometric evidence basic probability assignments in step S2 includes: Calculate the three-dimensional coordinate covariance matrix of the point cloud set within the target raster cell, and obtain the eigenvectors of the three-dimensional coordinate covariance matrix using principal component analysis. The eigenvector corresponding to the minimum eigenvalue is selected as the normal vector estimate of the target grid surface. The angle between the normal vector estimate and the gravity direction is calculated, and the angle value is defined as the slope feature. Calculate the standard deviation of the coordinate distribution of the point cloud set in the vertical direction, and define the standard deviation value as the roughness feature; The slope feature and the roughness feature are combined by weighted summation to obtain a comprehensive geometric feature index. The comprehensive geometric feature index is then transformed into a basic probability assignment for each hypothesis in the unstructured environment identification framework using a nonlinear mapping relationship.

4. The method for determining the path attributes of a mobile robot based on TBM evidence theory fusion according to claim 1, characterized in that, The specific process of mapping semantic segmentation probabilities to basic visual evidence probabilities in step S2 includes: The image frame data is classified at the pixel level using a semantic segmentation network, and a pixel probability vector is output. The pixel probability vector is mapped to a local grid map by inverse perspective projection transformation. The probability vectors of all pixels falling into the target grid are statistically aggregated to obtain the average semantic probability vector of the target grid. The probability values ​​belonging to the rigid road surface category, flexible road surface category, and obstacle category in the average semantic probability vector are summed respectively, and the summation results are assigned to the rigid hypothesis, flexible hypothesis, and obstacle hypothesis in the unstructured environment identification framework respectively.

5. The method for determining the path attributes of a mobile robot based on TBM evidence theory fusion according to claim 1, characterized in that, Step S2 further includes a cognitive discounting operation based on uncertainty: Calculate a discount factor with a value between 0 and 1: for geometric evidence, the discount factor is negatively correlated with the number of point clouds in the target raster; for visual evidence, the discount factor is positively correlated with the information entropy of the average semantic probability vector of the target raster. The basic probability assignment of the geometric evidence or the basic probability assignment of the visual evidence is corrected using the discount factor, the confidence level of each specific hypothesis is deducted proportionally, and the deducted confidence level share is reassigned to the whole set representing the state of complete ignorance.

6. The method for determining the path attributes of a mobile robot based on TBM evidence theory fusion according to claim 1, characterized in that, The specific process of extracting the local conflict energy spectrum in step S3 is as follows: when performing the orthogonal fusion, identify all focal element pairs whose intersection is an empty set; The total collision quality of the current grid cell is obtained by summing the joint confidence components of all focal pairs that lead to an empty set. The total conflict quality is mapped as an independent scalar attribute to the spatial hash table of the quadtree map structure, and the local conflict energy spectrum, which is aligned with the map resolution, is composed of the conflict quality of all grid cells. When outputting the new basic probability assignment vector after fusion, the quality of the empty set is preserved to characterize the degree of conflict between the sensing sources.

7. The method for determining the path attributes of a mobile robot based on TBM evidence theory fusion according to claim 1, characterized in that, The specific process of conflict-driven map resolution update in step S4 includes: Splitting decision based on conflict energy spectrum: If the local conflict energy of the target leaf node grid is greater than the preset splitting threshold, and the current level depth is less than the maximum resolution level set by the system, then it is determined that splitting is required. Quadtree topology splitting: Based on the geometric center of the target leaf node grid, it is divided into four child node grids of equal area, and the index is updated based on the spatial hash table; Evidence inheritance based on maximum entropy discount: Obtain the basic probability assignment vector of the parent node, set an inheritance discount factor, multiply the confidence of each specific hypothesis of the parent node by the inheritance discount factor to obtain the initial confidence of the child node grid, and add the deducted confidence share to the total confidence of the child node grid.

8. The method for determining the path attributes of a mobile robot based on TBM evidence theory fusion according to claim 1, characterized in that, The path attribute determination in step S5 also includes confidence time-domain decay based on timestamps: The last update timestamp is stored in each raster object of the raster map; The time difference between the current system time and the last updated timestamp is periodically calculated, and a time decay coefficient is calculated based on the time difference. The time decay coefficient is negatively correlated with the time difference. The basic probability assignment vector of the raster object is corrected using the time decay coefficient. The confidence values ​​of all specific hypotheses other than the entire set are multiplied by the time decay coefficient, and the reduced confidence share is added to the confidence of the entire set.

9. The method for determining the path attributes of a mobile robot based on TBM evidence theory fusion according to claim 1, characterized in that, It also includes performing unstructured environment surface unfolding path planning based on the semantic map generated in step S5: A graph structure for path search is constructed, which consists of multiple graph nodes and edges connecting adjacent graph nodes. The current graph node and its neighboring graph nodes are selected, and the normal vector data of the current graph node and the neighboring graph nodes are used to perform local surface connectivity verification. Execute the surface three-dimensional equivalent unfolding algorithm to calculate the three-dimensional Euclidean distance between the current graph node and the neighboring graph nodes, and define the calculated three-dimensional Euclidean distance as the basic unfolding distance; Based on the principle of gravity doing work, the slope correction coefficient is calculated by combining the pitch slope angle between the current graph node and the neighboring graph nodes. The slope correction coefficient is then used to correct the anisotropic cost of the foundation unfolding distance, generating the equivalent geometric distance after slope correction. By integrating the equivalent geometric distance with the access cost corresponding to the attribute probability generated in step S5, a globally optimal path point sequence is searched on the two-dimensional equivalent plane, and a three-dimensional reference trajectory containing spatial location coordinates and tangential guidance angle is generated through inverse mapping transformation.

10. The method for determining the path attributes of a mobile robot based on TBM evidence theory fusion according to claim 9, characterized in that, It also includes performing robust trajectory tracking and perception feedback to withstand interference: A vehicle dynamics model including lateral slip stiffness and longitudinal slip ratio is constructed. Using the rigid road surface probability and flexible road surface probability generated in step S5, the equivalent tire lateral stiffness parameter and road surface adhesion coefficient parameter in the vehicle dynamics model are adaptively weighted and adjusted. The total disturbance is estimated in real time using an extended state observer, and a model predictive control quadratic programming problem that incorporates the total disturbance is constructed to solve the problem. The front wheel steering angle command is output to control the robot to track the three-dimensional reference trajectory. During the tracking process, when the deviation between the actual wheel speed and the theoretical wheel speed exceeds a threshold or the vibration spectrum of the inertial measurement unit exceeds a threshold, an ontological perception evidence package containing basic probability assignments characterizing the physical properties of the road surface is generated, and the ontological perception evidence package is injected back into step S3 to participate in evidence fusion.

Images