A multi-sensor fusion-based intelligent obstacle avoidance navigation system for a tracked vehicle

Abstract

This invention relates to the field of intelligent robot control technology and discloses an intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion. The system includes a tracked vehicle body, a multi-source sensing mechanism, a cognitive decision-making mechanism, and a real-time control mechanism. The control mechanism establishes a time reference for heterogeneous sensors through hardware interrupts and a dual-buffered queue, fuses state data using filtering and weighting algorithms, and calculates the track slippage rate. The decision-making mechanism introduces a terrain stability penalty term in local path planning to deduce the optimal speed sequence and prevent vehicle rollover. The control mechanism uses the slippage rate to perform feedforward compensation on inverse kinematic commands and combines chassis vibration data to tune the closed-loop gain parameters online. Furthermore, the system synchronously monitors motor temperature, battery voltage, and communication status to trigger multi-level fault-tolerant degradation. This invention reduces spatiotemporal fusion errors, overcomes trajectory deviations caused by track slippage, and improves navigation accuracy and equipment reliability in complex terrain.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot control technology, specifically to an intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion. Background Technology

[0002] With the development of intelligent mobile robots, tracked vehicles, due to their strong ground adaptability, are widely used in exploration and transportation tasks in complex unstructured terrains. When performing autonomous obstacle avoidance and navigation in unknown environments, tracked vehicles need to be equipped with a variety of heterogeneous sensors such as visual cameras, ultrasonic rangefinders, and inertial measurement units. However, existing perception systems often struggle to solve the problem of time synchronization of multi-source data. Due to the significant differences in the hardware acquisition frequency and data transmission delay of different sensors, directly processing these data can lead to spatiotemporal fusion errors, resulting in inaccurate pose estimation and obstacle localization of the vehicle in complex environments.

[0003] On the other hand, when tracked vehicles travel on soft or rugged surfaces, track slippage and vehicle body swaying can occur. Existing underlying control algorithms are mostly based on ideal kinematic models, and closed-loop feedback parameters are pre-set under fixed working conditions. When the adhesion of the contact surface changes suddenly, causing the track to slip, the actual trajectory of the vehicle will deviate significantly from the path planned by the higher level. At the same time, conventional local path planning schemes only evaluate the distance to obstacles in a two-dimensional plane and ignore the influence of terrain undulations on the vehicle's pitch attitude. When facing steep slopes or potholes, the chassis may drive into dangerous areas with the risk of overturning.

[0004] Furthermore, under harsh working conditions and long-term high-load operation, the underlying hardware of tracked vehicles may experience abnormal conditions such as motor overheating, battery undervoltage, or control bus communication packet loss. The existing navigation control architecture focuses on the implementation of algorithm logic and lacks a comprehensive monitoring and interlocking protection mechanism for the multi-dimensional operating status of the underlying system. When the external environment exceeds the physical tolerance of the equipment, the system is unable to perform degraded fault-tolerant control in time before irreversible hardware damage or complete loss of vehicle control, resulting in insufficient overall operational reliability and safety. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides an intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion, which solves the technical problems of asynchronous multi-source perception data, track slippage leading to trajectory deviation, and lack of underlying hardware interlocking protection in complex terrain.

[0006] To achieve the above objectives, the present invention provides a tracked vehicle intelligent obstacle avoidance and navigation system based on multi-sensor fusion, wherein the system hardware includes:

[0007] The tracked vehicle body serves as both the physical load-bearing base and the motion execution carrier;

[0008] A multi-source heterogeneous sensing mechanism is installed on the tracked vehicle body to acquire external video sequences, motion status and ranging signals, as well as equipment temperature data;

[0009] The high-level cognitive decision-making body connects with the perception body and is used to perform semantic analysis of speech signals to determine the task mode, and combine video sequences to calculate the target linear velocity and target angular velocity of the chassis.

[0010] The underlying real-time control mechanism communicates with the high-level cognitive decision-making mechanism to perform state fusion and noise reduction on motion state and ranging signal and calculate slip rate. It performs inverse kinematic decomposition, slip compensation and parameter tuning on the target linear velocity and angular velocity of the chassis, and synthesizes the dual-side reference control linear velocity.

[0011] The distributed closed-loop actuator drives the chassis motor to operate in a closed loop and maintains the load balance based on the linear velocity of the dual-side reference control.

[0012] A high-power power distribution mechanism used to distribute electrical energy to the system.

[0013] Furthermore, the high-level cognitive decision-making mechanism includes a Raspberry Pi processor, a large model parsing unit and a path planning unit running inside the Raspberry Pi processor; the low-level real-time control mechanism includes a control microcontroller, a state fusion unit, a parallel control unit and a parameter tuning unit deployed inside the control microcontroller, and an interlocking protection unit integrated inside the control microcontroller; the multi-source heterogeneous sensing mechanism includes an inertial measurement unit, an optical encoder, an ultrasonic sensor, an infrared sensor, a vibration sensor and a temperature sensor connected to the input of the control microcontroller, and a vision camera connected to the video input of the Raspberry Pi processor.

[0014] Furthermore, the control microcontroller establishes the system's underlying sampling benchmark by configuring an internal general-purpose timer to generate a fixed-period hardware overflow interrupt signal. When the timer interrupt service routine responds, the control microcontroller synchronously acquires signals from the inertial measurement unit, photoelectric encoder, ultrasonic sensor, infrared sensor, and vibration sensor through the hardware interface. After the vision camera captures an external video sequence, it uses direct memory access to write the image frames at high speed into a circular double-buffer queue in the Raspberry Pi processor's core memory space. It also assigns a global timestamp to the image frames using a high-precision real-time clock, combining the underlying hardware timer interrupt mechanism with the high-level memory double-buffer queue.

[0015] This invention eliminates the differences in acquisition frequency and transmission delay between non-visual sensors and visual images, establishing a unified time reference for multi-source sensing data.

[0016] Furthermore, the state fusion unit establishes a linear discrete state-space model with the vehicle's true pitch angle, roll angle, and geometric centroid linear velocity as state vectors. It processes the data from the inertial measurement unit and photoelectric encoder by iteratively executing discrete Kalman filtering state prediction update operations, and outputs the vehicle's filtered pitch angle, roll angle, and geometric centroid filtered driving speed. The state fusion unit also performs real-time statistical analysis on the dynamic variance fluctuations of ranging data from multiple ultrasonic and infrared sensors within a continuous historical sampling window, calculates the data confidence weight coefficients of the overlapping field-of-view channels online, and outputs the comprehensive obstacle distance by weighted average fusion to remove environmental clutter.

[0017] This invention uses discrete Kalman filtering to weight and correct multidimensional observations, suppressing the integral drift error of a single sensor. At the same time, it uses dynamic variance to measure external environmental noise, weakening the weight of the interfered channel, and achieving effective complementarity of spatial multi-source ranging signals.

[0018] Furthermore, the state fusion unit uses the vehicle's geometric center of mass filtered driving speed as a reference benchmark for the chassis's actual translation speed. It combines the actual angular velocities of the two drive wheels converted by the photoelectric encoder with the mechanical radius parameters of the drive wheels to calculate the theoretical wheel speed. By comparing the deviation between the theoretical wheel speed and the vehicle's geometric center of mass filtered driving speed, the differential slip kinematics formula is introduced to quantitatively calculate the slip rate of the left track and the slip rate of the right track.

[0019] This invention transforms the adhesion state of the terrain contact surface into a quantitative slip rate parameter by comparing the difference between the filtered translation speed and the theoretical drive wheel speed, providing a numerical basis for the dynamic compensation of the underlying drive mechanism.

[0020] Furthermore, the large model parsing unit receives the input raw speech signal and converts it into Chinese string text commands. It uses a lightweight Transformer large language model with weighted quantization to perform offline semantic understanding of the Chinese string text commands to output global task mode command words. The path planning unit synchronously maps the visible driving boundary, comprehensive obstacle distance, and vehicle body filtered pitch angle onto a two-dimensional local cost map. It uses a multi-objective cost evaluation function to score and deduce multiple sets of simulated linear velocity and simulated angular velocity sequences sampled within the dynamic window, and selects the optimal simulated velocity sequence with the highest score as the chassis target linear velocity and chassis target angular velocity.

[0021] This invention deploys a lightweight language model at the edge to achieve offline semantic understanding, and combines multi-dimensional spatial ranging and terrain pitch status for local path planning, avoiding latency caused by reliance on external network communication.

[0022] Furthermore, the multi-objective cost evaluation function performs a weighted scoring evaluation based on the heading deviation evaluation item, obstacle distance evaluation item, speed evaluation item, and terrain stability penalty item. Among them, the evaluation constraint condition of the terrain stability penalty item is: when a sampled set of simulated speed sequences causes the future predicted pitch angle calculated based on the current vehicle body filter pitch angle to approach the limit safe pitch angle allowed by the tracked vehicle body structure, the evaluation score of the corresponding speed sequence is reduced to suppress the chassis from traveling into dangerous areas with large slopes. This invention introduces the limit constraint condition of the three-dimensional pose of the terrain into the two-dimensional speed sampling subspace to prevent the chassis from entering areas with overturning risk during obstacle avoidance and improve the movement safety under complex terrain.

[0023] Furthermore, the parallel control unit, in conjunction with the fixed mechanical wheel track parameters of the tracked vehicle body, uses the differential drive inverse kinematics model to initially decompose the target linear velocity and target angular velocity of the chassis into the ideal linear velocity of the left track and the ideal linear velocity of the right track. The parallel control unit retrieves the slip rate of the left track and the slip rate of the right track, and performs nonlinear amplification slip compensation on the ideal linear velocity of the left track and the ideal linear velocity of the right track respectively, and calculates and outputs the compensation target velocity of the left track and the compensation target velocity of the right track as the feedforward speed reference value.

[0024] This invention uses the calculated slip rate to feedforward compensation of motion commands, counteracting track slippage caused by soft terrain and reducing the deviation between the actual motion trajectory and the theoretically planned path.

[0025] Furthermore, the parallel control unit calculates the transient error sequence between the feedforward speed reference value and the actual linear velocity state value fed back by the photoelectric encoder, and inputs it into the incremental proportional-integral-derivative algorithm unit for closed-loop tracking; the parameter tuning unit extracts the real-time vibration amplitude of the chassis collected by the vibration sensor and the slip rate of the tracks on both sides, performs online real-time tuning of the proportional gain, integral gain and derivative gain parameters, iteratively calculates the incremental control quantity and accumulates the clamping to generate the absolute control voltage percentage, controls the internal peripherals to generate a complementary pulse width modulation signal with a corresponding duty cycle and outputs it to the motor drive unit.

[0026] This invention employs an incremental algorithm to output control changes, preventing voltage spikes caused by computational anomalies. It also combines vibration amplitude and slippage rate to adjust feedback gain online, thereby improving the system's adaptability to terrain disturbances.

[0027] Furthermore, within a discrete evaluation cycle, the interlocking protection unit synchronously collects real-time temperature data of the DC motor fed back by the temperature sensor and real-time terminal voltage of the vehicle battery read by the analog-to-digital converter, and counts the number of consecutive packet losses on the full-duplex serial bus. The interlocking protection unit maps the temperature data, real-time terminal voltage, and number of consecutive packet losses to a unified dimensionless interval using a linear normalization method, and calculates the comprehensive safety risk index by combining the penalty weight coefficient. The interlocking protection unit compares the comprehensive safety risk index with the preset system safety degradation threshold matrix in real time, and triggers a safety fault-tolerant control strategy based on the evaluation and grading results, which involves maintaining normal operation, reducing the maximum absolute control voltage percentage, and forcibly disconnecting the enable pin of the motor drive unit to perform physical braking.

[0028] This invention establishes a quantitative anomaly monitoring mechanism by integrating multi-dimensional state parameters, and executes a multi-level fault-tolerant degradation strategy before hardware failure, thereby ensuring the reliability of equipment operation under harsh conditions.

[0029] This invention provides an intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion. It has the following beneficial effects:

[0030] 1. This invention assigns global timestamps to multi-source physical signals and high-frequency visual images by configuring hardware timer interrupts and dual-buffer queues, thus unifying the sampling time reference of multi-source heterogeneous sensors. On this basis, it combines discrete Kalman filtering and dynamic variance weighting algorithms to perform state fusion on multi-dimensional data, suppressing the integral drift error of a single sensor and eliminating environmental clutter interference, thereby improving the accuracy of the system in perceiving complex environments and estimating vehicle pose.

[0031] 2. This invention calculates the track slip rate by comparing the theoretical driving wheel speed with the filtered actual translational speed of the vehicle body, and uses this slip rate to perform feedforward slip compensation on the ideal linear velocity obtained from kinematic decomposition; at the same time, it combines the chassis vibration amplitude and slip state to perform online real-time tuning of the gain parameters of the incremental proportional-integral-differential algorithm, overcoming track slippage caused by soft terrain and external mechanical disturbances, and reducing the deviation between the actual motion trajectory and the theoretically planned path.

[0032] 3. This invention introduces a terrain stability penalty term into the cost function of local path planning. When the predicted pitch angle approaches the physical limit of the vehicle body, the score of the corresponding speed sequence is reduced, thus avoiding the chassis from entering steep slope areas prone to overturning at the decision-making level. At the same time, the interlocking protection unit synchronously monitors the motor temperature, battery voltage and communication packet loss rate, calculates the comprehensive safety risk index and matches the corresponding safety degradation threshold matrix, and triggers the amplitude limiting degradation or physical brake in time before hardware damage or loss of control, ensuring the reliability of the system under harsh conditions. Attached Figure Description

[0033] Figure 1This is a perspective view of the present invention;

[0034] Figure 2 This is a schematic diagram of the bottom of the tracked vehicle body of the present invention;

[0035] Figure 3 This is a cross-sectional view of the inside of the tracked vehicle body of the present invention;

[0036] Figure 4 This is a system architecture diagram of the present invention;

[0037] Figure 5 This is a flowchart of the method of the present invention;

[0038] Figure 6 This is a hardware timing diagram for the multi-source data high-frequency sensing and synchronous acquisition of the present invention;

[0039] Figure 7 This is a schematic diagram illustrating the calculation principle of spatiotemporal state fusion of multi-source heterogeneous data in this invention.

[0040] Figure 8 This is the data flow diagram of the hierarchical edge-side decision guidance and path planning of the present invention;

[0041] Figure 9 This is a structural diagram of the underlying parallel motion control and drive execution control system of the present invention;

[0042] Figure 10 This is a logic diagram of the system-level anomaly monitoring and security degradation handling of the present invention;

[0043] Figure 11 This is a comparison test chart of the maximum yaw error of the present invention;

[0044] Figure 12 This is a comparison test diagram of the maximum pitch angle fluctuation of the chassis of the present invention;

[0045] Figure 13 This is a comprehensive comparison diagram of the dynamic performance and physical security of the underlying drive system of this invention.

[0046] Among them, 100 is the tracked vehicle body; 200 is the high-level cognitive decision-making mechanism; 300 is the low-level real-time control mechanism; 400 is the multi-source heterogeneous sensing mechanism; 500 is the distributed closed-loop execution mechanism; 600 is the high-power power distribution mechanism; 210 is the Raspberry Pi processor; 220 is the large model parsing unit; 230 is the path planning unit; 240 is the voice input unit; 250 is the voice output unit; 260 is the display interaction unit; 310 is the control microcontroller; and 320 is the state fusion unit. 330 Parallel control unit; 340 Parameter tuning unit; 350 Interlocking protection unit; 410 Inertial measurement unit; 420 Photoelectric encoder; 430 Ultrasonic sensor; 440 Infrared sensor; 450 Vibration sensor; 460 Temperature sensor; 470 Vision camera; 510 DC motor; 520 Motor drive unit; 530 Four sets of electric push rod units; 540 Load balancing platform; 610 Lithium battery pack; 620 Power distribution unit. Detailed Implementation

[0047] 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.

[0048] See attached document Figure 1 -Appendix Figure 4 This invention provides an intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion. The system hardware may include: tracked vehicle body 100, high-level cognitive decision-making mechanism 200, low-level real-time control mechanism 300, multi-source heterogeneous sensing mechanism 400, distributed closed-loop execution mechanism 500, and high-power power distribution mechanism 600.

[0049] The high-level cognitive decision-making unit 200 is installed in the upper electronic equipment compartment of the tracked vehicle body 100. The high-level cognitive decision-making unit 200 includes a Raspberry Pi processor 210, a large model parsing unit 220 and a path planning unit 230 running inside the Raspberry Pi processor 210.

[0050] The audio input terminal of the Raspberry Pi processor 210 is connected to a voice input unit 240, the audio output terminal of the Raspberry Pi processor 210 is connected to a voice output unit 250, and the digital video output interface of the Raspberry Pi processor 210 is connected to a display interaction unit 260. The voice input unit 240, the voice output unit 250, and the display interaction unit 260 are all fixed to the outer shell of the tracked vehicle body 100.

[0051] The underlying real-time control mechanism 300 is installed in the chassis electronic equipment compartment of the tracked vehicle body 100. The underlying real-time control mechanism 300 includes a control microcontroller 310, a state fusion unit 320, a parallel control unit 330 and a parameter tuning unit 340 deployed inside the control microcontroller 310. The control microcontroller 310 integrates an interlocking protection unit 350. The Raspberry Pi processor 210 and the control microcontroller 310 establish a data communication connection through a bidirectional full-duplex serial bus.

[0052] The multi-source heterogeneous sensing mechanism 400 includes an inertial measurement unit 410, an optical encoder 420, an ultrasonic sensor 430, an infrared sensor 440, a vibration sensor 450, and a temperature sensor 460 connected to the signal input terminal of the control microcontroller 310, and a vision camera 470 connected to the video input terminal of the Raspberry Pi processor 210. The inertial measurement unit 410 is fixed at the geometric center of mass of the tracked vehicle body 100.

[0053] The photoelectric encoder 420 is coaxially mounted on the output shaft of the drive motor. The ultrasonic sensor 430 is arranged in a matrix on the front end and side shells of the tracked vehicle body 100. The infrared sensor 440 is symmetrically fixed on the lower front part of the chassis of the tracked vehicle body 100 with the detection window facing the ground. The vibration sensor 450 is fixed on the metal liner under the chassis of the tracked vehicle body 100. The temperature sensor 460 is respectively attached to the motor shell and the battery pack shell. The vision camera 470 is vertically fixed on the top of the support mast at the front of the tracked vehicle body 100.

[0054] The distributed closed-loop actuator 500 includes a DC motor 510 installed inside the rear drive wheel of the tracked vehicle body 100, a motor drive unit 520 installed in the chassis electronic equipment compartment, four sets of electric push rod units 530, and a load balancing platform 540 supported by the four sets of electric push rod units 530. The four sets of electric push rod units 530 are respectively installed between the suspension linkages at the four corners of the tracked vehicle body 100 chassis and the bottom surface of the load balancing platform 540. The control input terminal of the motor drive unit 520 is connected to the pulse width modulation signal output port of the control microcontroller 310, and the power output terminal of the motor drive unit 520 is connected to the excitation coil of the DC motor 510. The stroke control terminals of the four sets of electric push rod units 530 are connected to the analog signal acquisition input port of the control microcontroller 310 through potentiometer feedback lines.

[0055] The high-power power distribution mechanism 600 includes a lithium battery pack 610 located at the bottom of the chassis electronic equipment compartment and a power distribution unit 620 located adjacent to the lithium battery pack 610. The power input terminal of the power distribution unit 620 is connected to the lithium battery pack 610. The low-voltage DC output terminal of the power distribution unit 620 is connected to the Raspberry Pi processor 210 and the control microcontroller 310, respectively. The high-voltage high-current output terminal of the power distribution unit 620 is connected to the motor drive unit 520 and four sets of electric actuator units 530, respectively.

[0056] See attached document Figure 5 This invention provides an intelligent obstacle avoidance and navigation method for tracked vehicles based on multi-sensor fusion, the method comprising the following steps:

[0057] S100 controls the microcontroller 310 to synchronously acquire physical signals from the inertial measurement unit 410, photoelectric encoder 420, ultrasonic sensor 430, infrared sensor 440 and vibration sensor 450 at a frequency of 100Hz, and uses an asynchronous polling mechanism to acquire data from the temperature sensor 460. At the same time, the vision camera 470 captures external video sequences and inputs them to the Raspberry Pi processor 210.

[0058] S200, the control microcontroller 310 uses the state fusion unit 320 to perform discrete Kalman filtering on the data of the inertial measurement unit 410 and the photoelectric encoder 420, and outputs the denoised vehicle body filtered pitch angle, vehicle body roll angle and geometric center of mass filtered speed. Simultaneously, it performs dynamic variance weighted fusion on the ranging data of the ultrasonic sensor 430 and the infrared sensor 440 to output the comprehensive obstacle distance, and calculates the double track slip rate in combination with the drive wheel speed.

[0059] S300, Raspberry Pi processor 210 uses large model parsing unit 220 to perform offline semantic parsing of the signal of voice input unit 240 to determine the task mode. Path planning unit 230 combines the image of vision camera 470, the distance of comprehensive obstacles and the vehicle body filtered pitch angle to run local dynamic window method algorithm, calculate the target linear velocity and target angular velocity of chassis and send them to control microcontroller 310 through serial bus.

[0060] S400, the control microcontroller 310 uses the parallel control unit 330 to perform inverse kinematic decomposition of the target speed to obtain the basic target linear velocity of both tracks. The parameter tuning unit 340 tunes the control gain online based on the slip rate of both tracks and the amplitude of the vibration sensor 450 and generates a feedforward speed compensation amount. The feedforward speed compensation amount and the basic target linear velocity are superimposed to synthesize the reference control linear velocity of both sides. Then, the signal is output to the motor drive unit 520 through the dual-channel incremental pulse width modulation control loop to adjust the speed of the DC motor 510.

[0061] S500 controls the parallel operation of the balance control microcontroller 310. Based on the vehicle body's filtered pitch angle, it calculates the target stroke of the four electric push rod units 530 using a position-based proportional-differential algorithm. The drive linearly extends and retracts within 0.5 seconds to keep the load balance platform 540 level. When the temperature sensor 460 detects that the temperature of the DC motor 510 or the lithium battery pack 610 exceeds 80°C, the interlock protection unit 350 blocks the pulse width modulation signal to de-energize the motor drive unit 520. At the same time, it sends an alarm packet to the Raspberry Pi processor 210 to trigger the display interaction unit 260 to alarm and schedule the voice output unit 250 to broadcast the voice.

[0062] See attached document Figure 6 For a specific implementation of step S100, the following sub-steps can be used:

[0063] S110, the hardware timer sampling interrupt configuration, controls the microcontroller 310 to initialize the register parameters of the internal general-purpose timer, sets the prescaler coefficient to 16 to 64 and calculates and sets the value of the auto-reload register according to the internal system clock frequency, so that the general-purpose timer generates a hardware overflow interrupt signal with a fixed period of 10 milliseconds, thereby establishing a 100Hz system-level physical sampling benchmark. At the same time, the preemption priority and response priority of the general-purpose timer interrupt are configured in the kernel nested vector interrupt controller to ensure the highest time determinism and execution continuity of the data acquisition task at the underlying hardware level, fundamentally avoiding sudden time jitter caused by external interference in the underlying control loop.

[0064] S120: Motion and environmental ranging data are acquired synchronously. When the timer interrupt service routine responds, the microcontroller 310 reads the digital values ​​of the 3-axis angular velocity and 3-axis acceleration of the inertial measurement unit 410 through the integrated circuit bus physical interface. The microcontroller 310 reads the pulse increment of the photoelectric encoder 420 in the current sampling period using the quadrature encoder interface counter register. The microcontroller 310 measures the high-level echo time of the ultrasonic sensor 430 through the input capture channel. The microcontroller 310 starts the analog-to-digital converter and uses the direct memory access channel to perform multi-channel cyclic scanning on the analog channel of the infrared sensor 440 to obtain the ground reflected voltage signal. The microcontroller 310 reads the chassis vertical axis acceleration data of the vibration sensor 450 through the serial peripheral interface bus.

[0065] For calculating the physical detection distance of the multi-channel ultrasonic sensor 430, the control microcontroller 310 uses the time difference of the high-level pulses measured by the capture input channel to perform acoustic wave dynamics conversion. The formula for calculating the physical detection distance of the multi-channel ultrasonic sensor 430 is as follows: In the formula, For the first The local detection distance collected by the ultrasonic sensor 430 is fixed at 0.02 meters to 4.50 meters due to the limitations of the hardware physical structure. This is the index number for the multi-channel ultrasonic sensor 430; For the first The high-level echo pulse time difference measured by the input capture channel corresponding to each ultrasonic sensor 430 has a physical measurement range of 115 microseconds to 26470 microseconds. The current propagation speed constant of sound waves in standard air is determined by linear temperature compensation based on the real-time ambient temperature measured by the multi-channel temperature sensor 460 between 331.5 m / s and 343.5 m / s.

[0066] For the conversion of the drive wheel speed of the photoelectric encoder 420, the control microcontroller 310 uses the pulse count increment within the current fixed sampling period to convert the rotational angular velocity. The formula for converting the drive wheel speed is as follows: ; In the formula, The angular velocity of the left drive wheel is defined in radians per second. The angular velocity of the right drive wheel is defined in radians per second. This is the pulse count increment on the left side, and its value range is limited to -5000 to 5000 based on the speed of the DC motor 510. This is the pulse count increment on the right side, and its value range is limited to -5000 to 5000 according to the speed of the DC motor 510. The single-turn line count constant of the photoelectric encoder 420 is fixed as an integer between 100 and 1024 depending on the physical selection. This is the fixed sampling period value corresponding to the timer overflow interrupt, which is fixed at 0.01 seconds throughout the entire process.

[0067] For the object distance conversion calculation of infrared sensor 440, the control microcontroller 310 uses the read multi-channel analog voltage signal to perform inverse transformation calculation of road surface reflection object distance. The object distance conversion calculation formula of infrared sensor 440 is as follows: In the formula: For the first The measurement distance corresponding to each infrared sensor 440 is calculated to be between 0.10 meters and 0.80 meters. This is the index number for the multi-channel infrared sensor 440; The voltage-distance conversion ratio of the infrared sensor 440 is determined to be between 12.5 and 15.8 volt-meters based on the sensor's physical calibration experiment. For the first The real-time reflected voltage signal value of the analog channel corresponding to each infrared sensor 440 has a physical measurement range of 0.4 volts to 3.2 volts. The static bias voltage constant of the infrared sensor 440 is determined to be between 0.05 volts and 0.15 volts based on ambient background light intensity noise calibration.

[0068] For the initialization control of the hardware registers at the bottom layer of the inertial measurement unit 410 and the vibration sensor 450, the configuration control words can be written according to the datasheet of the selected electronic component chip.

[0069] S130, large inertia temperature data asynchronous polling, the control microcontroller 310 runs single-bus protocol timing in a low-priority main loop thread outside the timer interrupt. The control microcontroller 310 periodically sends reset and skip read-only memory instructions through general-purpose input / output pins to send temperature conversion requests to the multi-channel temperature sensors 460 attached to the housing of DC motor 510 and lithium battery pack 610. After a preset conversion delay of 750 milliseconds, it reads the data registers of each channel to dynamically update the core temperature monitoring value. Since the temperature signal is physically a slow-changing large inertia low-frequency physical quantity, completely separating the temperature signal from the 100Hz high-frequency interrupt service routine can reduce the high-frequency interrupt service overhead of the control microcontroller 310, thereby ensuring that the execution delay of the high-frequency control task is not disturbed.

[0070] For single-bus microsecond-level waveform timing delay control, master-slave response signal capture, and multi-point temperature cycle retrieval, the general single-bus standard software driver code library can be directly called, or precise microsecond-level timing matching can be achieved by controlling the hardware timer delay function inside the microcontroller 310.

[0071] S140, High-level image capture and timestamp alignment: The visual camera 470 in the high-level cognitive decision-making unit 200 synchronously performs external image capture, and transmits the image data stream to the Raspberry Pi processor 210 at high frequency through the mobile industrial processor interface camera serial interface. The Raspberry Pi processor 210 uses the video driver layer to open a dedicated circular double buffer queue in the core memory space. After the visual camera 470 captures the current video frame, it uses direct memory access to write the data to the circular double buffer queue at high speed. When reading image data, the Raspberry Pi processor 210 calls the internal high-precision real-time clock to assign a global timestamp to the current image frame, so that it can establish a high-precision fusion correspondence with the non-visual sensor data reported by the control microcontroller 310 through the serial bus on a unified time axis.

[0072] For the physical memory allocation of the circular buffer, the atomic-level mutex lock control of internal read-write pointer conflicts, and the kernel compilation and loading of the physical driver for the mobile industry processor interface, the general video multimedia underlying driver framework built into the embedded operating system kernel can be directly called for high-level application layer calls, or the common multi-threaded bounded buffer synchronization development mechanism can be used.

[0073] After the high-frequency sensing and synchronous acquisition steps of multi-source data are completed, the control microcontroller 310 and the Raspberry Pi processor 210 work together to complete the pipeline data readiness preparation of the low-level non-visual physical state quantities and the high-level multimedia visual frame queue. Based on this highly deterministic multi-source sensing dataset, the present invention drives the control system to smoothly enter the subsequent multi-source heterogeneous data spatiotemporal state fusion and solution steps, thereby providing real-time chassis full-dimensional motion characteristics and multi-dimensional obstacle space detection and ranging benchmark parameters with high confidence and zero logic dead zone timeliness for the subsequent hierarchical parallel decoupling control, online tuning of adaptive parameters for complex terrain features, and active attitude level compensation of the load balance platform 540.

[0074] See attached document Figure 7 For a specific implementation of step S200, the following sub-steps can be used:

[0075] S210, based on the optimal estimation of vehicle attitude and geometric center of mass velocity using discrete Kalman filtering, the control microcontroller 310 calls the internal state fusion unit 320 to establish a linear discrete state space model with the vehicle's true pitch angle, roll angle, and geometric center of mass linear velocity as state vectors. The state fusion unit 320 inputs the acceleration and angular velocity data output by the inertial measurement unit 410 and the drive wheel speed data output by the photoelectric encoder 420 as observation variables into the linear discrete state space model. Through iterative execution of state prediction update operations, it outputs the filtered pitch angle, roll angle, and geometric center of mass filtered driving speed of the vehicle after removing hardware thermal drift noise.

[0076] In actual operation, relying solely on the inertial measurement unit 410 for integration will accumulate drift errors, while relying solely on the photoelectric encoder 420 for speed calculation will result in serious deviations when the track slips. The state fusion unit 320 introduces a Kalman filter framework to predict the theoretical value at the current moment based on the system state at the previous moment and combines it with the actual observation values ​​of multiple sensors at the current moment for weighted correction. This state prediction update operation can suppress the divergence phenomenon caused by the long-term operation of a single sensor by finding the optimal balance between prediction uncertainty and observation uncertainty.

[0077] For the state prediction update operation, the state fusion unit 320 performs the following state prediction and observation equations using discrete Kalman filtering: ; ; In the formula, For the system at discrete time steps The optimal state vector at any given time includes three physical dimensions: vehicle body filtered pitch angle, vehicle body roll angle, and vehicle body geometric centroid filtered driving speed. The numerical range of the optimal state vector is limited by the chassis physical limit configuration, which is -1.5 to 1.5. This is the index number of the current discrete time step, and its value is a positive integer greater than 0. For the system at discrete time steps The optimal state vector at any given time includes three physical dimensions: vehicle body filtered pitch angle, vehicle body roll angle, and vehicle body geometric centroid filtered driving speed. The numerical range is limited to -1.5 to 1.5 by the chassis physical limit configuration. For the system at discrete time steps The chassis linear velocity and angular velocity control input vector at each moment; For the system at discrete time steps The process noise vector at time t follows a Gaussian distribution with a mean of 0, and the covariance parameter of the process noise vector is set between 0.001 and 0.05 based on the static calibration data of the inertial measurement unit 410. For the system at discrete time steps The actual sensor observation vector at any given time; The observation transformation matrix is ​​a constant identity matrix with a fixed dimension of 3 rows and 3 columns; For the system at discrete time steps The measurement noise vector at any given time follows a Gaussian distribution with a mean of 0. The covariance parameter of the measurement noise vector is set between 0.01 and 0.1 based on the quantization error calibration results of the photoelectric encoder 420.

[0078] In order to ensure the physical executability of the discrete state-space model, The system state transition matrix is ​​a constant square matrix with a fixed dimension of 3 rows and 3 columns; To control the input matrix, a constant matrix with a fixed dimension of 3 rows and 2 columns is used. The specific matrix expressions are defined as follows: ;

[0079] For the mathematical processes of matrix multiplication and inverse matrix solving required for error covariance matrix prediction, Kalman gain matrix calculation, and posterior update of state covariance matrix in the Kalman filtering algorithm, a corresponding microcontroller low-level calculation function library can be written according to standard linear algebra arithmetic rules.

[0080] S220, based on dynamic variance weighted multi-channel ultrasonic and infrared ranging spatial fusion, the state fusion unit 320 statistically analyzes the dynamic variance fluctuation of the ranging data returned by the multi-channel ultrasonic sensor 430 and infrared sensor 440 in real time within multiple consecutive historical sampling periods. The state fusion unit 320 calculates the data confidence weight coefficient of each sensor channel in the spatially overlapping field of view online according to the dynamic variance fluctuation. The state fusion unit 320 uses the data confidence weight coefficient to perform weighted average fusion of the ranging data of different channels to remove external environmental clutter interference and finally output a unified comprehensive obstacle distance.

[0081] Because the ultrasonic sensor 430 is subject to multipath reflection interference and produces abrupt ranging data in complex terrain environments, while the infrared sensor 440 is affected by changes in ambient light, the state fusion unit 320 uses dynamic variance as a data quality metric. When the fluctuation variance of the data returned by a certain sensor channel increases sharply, the system determines that the current sensor channel is severely interfered with and automatically weakens the weight of the corresponding channel in the fusion algorithm. Conversely, it increases the weight of the stable channel, thereby achieving complementary advantages of multiple environmental ranging signals.

[0082] The modified dynamic variance weighted calculation and distance fusion formula, which addresses the division-to-zero overflow dead zone issue, is as follows: ; ; In the formula, For the first The dynamic variance of the ranging sensor channel within a continuous sampling window fluctuates between 0 and 1.5 square meters. The historical sample capacity of the dynamic statistical sampling window is fixed as an integer constant between 20 and 50, based on the microcontroller's memory resources. This refers to the index number of the historical sampling sequence within the dynamic statistical sampling window; For the first The ranging sensor channel in the first The local detection distance at each historical sampling moment is limited to a range of 0.02 meters to 4.50 meters. For the first The average local detection distance of each ranging sensor channel within the entire historical sampling window is limited to a range of 0.02 meters to 4.50 meters. For the first The normalized weighting coefficient of the data confidence obtained from the allocation of each ranging sensor channel is strictly limited to the range of 0 to 1. To prevent zero overflow in the denominator, the regularization minimum constant value is fixed at 0.0001 internally. The total number of overlapping field-of-view sensor channels participating in spatial data fusion is fixed as an integer constant between 3 and 8 based on the 100 sensor mounting matrix of the tracked vehicle body; This refers to the index number of the ranging sensor channel in the summation operation term; For the summation term, the first... The dynamic variance of the ranging sensor channel fluctuates between 0 and 1.5 square meters. The unified comprehensive obstacle distance output after variance-weighted fusion calculation is limited to a value range of 0.1 meters to 4.0 meters. This is the index number of the ranging sensor channel, with a value ranging from 1 to... Integers.

[0083] S230, a mathematical analytical model of the slip rate of the two-sided tracks is established. The state fusion unit 320 uses the filtered driving speed of the vehicle body geometric centroid output by the filter calculation as the reference benchmark for the actual translation speed of the chassis. The state fusion unit 320 calculates the theoretical wheel speed by combining the actual angular velocity of the two-sided drive wheels and the mechanical physical radius parameters of the drive wheels calculated by the photoelectric encoder 420. When the tracked vehicle body 100 travels on soft sand or wet mud, the wheel spin will cause the encoder's theoretical wheel speed to be greater than the actual forward speed of the chassis. The state fusion unit 320 introduces the differential slip kinematic formula by comparing the deviation between the theoretical wheel speed and the filtered driving speed to quantitatively calculate the slip rate of the left track and the slip rate of the right track, and directly characterizes the physical adhesion state between the chassis and the current contact ground in a numerical way.

[0084] The formula for calculating the slip rate of both tracks is as follows: ; In the formula, To calculate the slip rate of the left track, the physical quantity is a dimensionless parameter with a value range strictly defined between 0 and 1. To calculate the slip rate of the right track, the physical quantity is a dimensionless parameter with a value range strictly defined between 0 and 1. This is the actual rotational angular velocity of the left drive wheel, with the physical unit defined as radians per second. The value range is based on the speed limit of the DC motor 510, which is between 0 and 100. The actual rotational angular velocity of the right drive wheel is defined in radians per second, and the value range is based on the speed limit of the DC motor 510, which is between 0 and 100. The mechanical physical radius parameter of the rear drive wheel of the tracked vehicle body is fixed at 0.08 meters to 0.15 meters based on the metal machining dimensions; The vehicle's geometric centroid is the filtered speed output by the Kalman filter. The physical measurement unit is meters per second, and the value range is limited to 0 to 3.0 meters per second. To prevent the algorithm's denominator from overflowing to zero, the speed lower limit compensation minimum constant is fixed at 0.01 meters per second internally in the software program.

[0085] The multi-source heterogeneous data spatiotemporal state fusion solution step executed by the state fusion unit 320 can control the microcontroller 310 to transform the low-level raw sensor data containing environmental noise and hardware drift error into stable chassis motion state parameters and external obstacle boundary distance parameters. This pre-filtering mechanism, which preprocesses and aligns multi-source data within the microcontroller, helps to reduce the edge computing load of the high-level cognitive decision-making agency 200. The spatiotemporal state parameters, after mathematical mapping and noise reduction, are seamlessly transmitted to the subsequent high-level decision guidance stage and parallel feedforward control stage, which can reduce the control loop oscillation phenomenon caused by raw data jitter.

[0086] See attached document Figure 8 For a specific implementation of step S300, the following sub-steps can be used:

[0087] S310, based on an edge-lightweight large language model for offline semantic task parsing, the large model parsing unit 220 inside the Raspberry Pi processor 210 receives the raw speech signal collected by the speech input unit 240. The large model parsing unit 220 uses the built-in Hidden Markov Acoustic Model to convert the raw speech signal into Chinese string text instructions. The large model parsing unit 220 uses a lightweight Transformer large language model with weight quantization to perform offline semantic understanding of the Chinese string text instructions to output global task mode instruction words.

[0088] The lightweight Transformer large language model consists of a word embedding layer, a stacked 6-layer multi-head self-attention encoder module, and a feedforward fully connected layer with a Softmax normalized exponential function. The front-end input data is a Chinese string text instruction, which is mapped by a lookup word segmenter to generate a 1-dimensional integer token tensor of fixed length 64. The 1-dimensional integer token tensor flows sequentially through the word embedding layer and the multi-head self-attention encoder module to extract long-distance contextual dependency features from the Chinese string text instruction. The feedforward fully connected layer receives the long-distance contextual dependency features and finally calculates and outputs a classification probability distribution vector representing the specific physical task state. The specific business meaning corresponding to the output results covers three discrete global task mode instruction words: autonomous obstacle avoidance and inspection, target following, and on-the-spot standby.

[0089] For the construction and training steps of the lightweight Transformer large language model, a training sample dataset of 5000 manually recorded texts containing spoken language of tracked vehicle operation in a real-world work scenario was used as a pre-trained dataset. Manually labeled standard task category indices were used as supervised training labels. The lightweight Transformer large language model was trained under supervised fine-tuning on a cloud graphics processing unit server using the cross-entropy loss function and an adaptive moment estimation optimizer with an initial learning rate of 0.001. After training convergence, the system compressed the model weight parameters using an 8-bit integer quantization algorithm and compiled and exported the weight parameters as a static inference graph file, which was then deployed to the solid-state storage of a Raspberry Pi 210 processor. The training process of the lightweight Transformer large language model can be reproduced according to the above sample definition and network topology.

[0090] S320, a local dynamic window method path planning that integrates multi-source spatiotemporal constraints, the path planning unit 230 receives the drivable area boundary point array extracted by the visual camera 470 through the image segmentation algorithm, the path planning unit 230 synchronously maps the visible driving boundary, the comprehensive obstacle distance reported by the state fusion unit 320 and the vehicle body filtered pitch angle to the 2D local cost map, the path planning unit 230 randomly generates multiple sets of simulated linear velocity and simulated angular velocity sequences in the 2D velocity sampling subspace constrained by the current kinematic limit of the tracked vehicle body 100.

[0091] The path planning unit 230 uses a specially designed multi-objective cost evaluation function to perform simulation and scoring on each set of simulated speed sequences. The multi-objective cost evaluation function not only performs conventional evaluation of heading deviation and obstacle distance, but also introduces the vehicle body filtering pitch angle as a terrain stability penalty. When a set of simulated speed sequences causes the chassis to drive onto a steep slope, resulting in the predicted pitch angle approaching the physical rollover limit, the multi-objective cost evaluation function will lower the score of this speed combination. The path planning unit 230 finally selects the optimal simulated speed sequence with the highest score as the current chassis target linear velocity and chassis target angular velocity.

[0092] The calculation formulas for the multi-objective cost evaluation function and the terrain stability penalty term are as follows: ; In the formula, This is the total score of the multi-objective cost evaluation corresponding to the currently sampled combination of simulated linear velocity and simulated angular velocity, with a numerical range of 0 to 100; The simulated linear velocity is sampled within a dynamic window, with the physical measurement unit being meters per second, and the value range limited to 0 to 2.0 meters per second; The simulated angular velocity is sampled within a dynamic window, with the physical unit being radians per second, and the value range is limited to -1.5 to 1.5 radians per second; The weighting coefficient for the heading deviation evaluation item is fixed at 0.2 within the program. The heading deviation evaluation item is calculated by normalizing the angle between the end of the simulated trajectory and the global target point by subtracting 180° from the angle between the end of the simulated trajectory and the global target point. The value range is from 0 to 1. This is the weighting coefficient for the obstacle distance evaluation item, which is fixed at 0.4 internally. This is the obstacle distance evaluation item, which corresponds to the shortest geometric interval between the simulated trajectory and the overall obstacle distance. The value range is limited to 0 to 4.0 meters. The weighting coefficient for the speed evaluation item is fixed at 0.1 internally within the program. This is a velocity evaluation term, equal to the ratio of the simulated linear velocity to the maximum permissible linear velocity of the system, with a value ranging from 0 to 1. This is the weighting coefficient for the terrain stability penalty term, which is fixed at 0.3 internally. This is a terrain stability penalty, used to use terrain physical information to prevent the chassis from moving into dangerous areas with steep slopes; The future predicted pitch angle is mathematically calculated using the current vehicle body filtered pitch angle under the current simulated speed, with a value ranging from -1.5 to 1.5 radians; The limit safe pitch angle constant allowed by the physical structure of the tracked vehicle body 100 is fixed at 0.78 radians based on the chassis center of gravity height parameter.

[0093] The S330, a dual-core heterogeneous platform, features efficient serial instruction encapsulation and delivery. After selecting the optimal target linear velocity and angular velocity of the chassis, the Raspberry Pi processor 210 calls its internal hardware general asynchronous transceiver interface to convert the floating-point velocity data into a fixed-byte hexadecimal communication payload. The Raspberry Pi processor 210 then packages the task mode instruction word, the target linear velocity, and the target angular velocity into a complete data packet containing a 2-byte synchronization frame header, a 1-byte data length, a 2-byte cyclic redundancy check code, and a 1-byte frame tail, according to a predefined custom communication protocol.

[0094] The Raspberry Pi processor 210 transmits the assembled complete data message at a standard baud rate of 115,200 bits per second to the receive buffer of the control microcontroller 310 via a bidirectional full-duplex serial bus. The cross-core communication protocol with strict byte boundary alignment and cyclic redundancy check mechanism can avoid instruction error and misalignment caused by electromagnetic interference. The control microcontroller 310 parses the complete data message to ensure that the high-level decision instructions are accurately transmitted to the subsequent low-level parallel control loop.

[0095] See attached document Figure 9 The specific implementation of step S400 can be processed through the following sub-steps:

[0096] S410, based on the feedforward kinematic compensation calculation of the independent slip rate on both sides, the parallel control unit 330 inside the control microcontroller 310 extracts the target linear velocity and target angular velocity of the chassis from the received data message. The parallel control unit 330, combined with the fixed mechanical wheel track parameters of the tracked vehicle body 100, uses the differential drive inverse kinematic model to preliminarily calculate the ideal linear velocity of the left track and the ideal linear velocity of the right track.

[0097] To overcome the problem of actuator slippage and step loss caused by soft terrain, which leads to the actual trajectory deviating from the theoretically planned path, the parallel control unit 330 calls the state fusion unit 320 to calculate and output the slip rate of the left track and the slip rate of the right track in real time. The left track and the right track are nonlinearly amplified and compensated to output the compensation target speed of the left track and the compensation target speed of the right track. This slippage compensation mechanism is essentially to physically compensate for the traction lost by the system by amplifying the speed feedforward.

[0098] The formula for solving feedforward kinematic compensation is as follows:

[0099] ;

[0100] ;

[0101] ;

[0102] ; In the formula, The ideal linear velocity of the left track, calculated in the preliminary calculation, is measured in meters per second, with a range limited to -2.5 to 2.5 meters per second. The ideal linear velocity of the right track, calculated in the preliminary step, is measured in meters per second, with a range limited to -2.5 to 2.5 meters per second. The target linear velocity of the chassis, determined by the Raspberry Pi processor 210, is measured in meters per second and its range is limited to 0 to 2.0 meters per second. The target angular velocity of the chassis, determined by the Raspberry Pi processor 210, is measured in radians per second and is limited to a range of -1.5 to 1.5 radians per second. The fixed mechanical wheel track parameter between the center lines of the tracks on both sides of the tracked vehicle body is set to 0.45 meters based on the chassis structure dimensions. The output is the target speed of the left track after slippage compensation. The physical measurement unit is meters per second, and the value range is limited to -3.0 to 3.0 meters per second. The output is the right track compensation target speed after slippage compensation. The physical measurement unit is meters per second, and the value range is limited to -3.0 to 3.0 meters per second. The left track slip rate is input to the state fusion unit 320. The physical quantity is a dimensionless parameter with a value range between 0 and 1. The slip rate of the right track is input to the state fusion unit 320. The physical quantity is a dimensionless parameter with a value range between 0 and 1. The slippage rate safety cutoff upper limit constant, which is set to prevent the denominator of the formula from overflowing into the dead zone, is fixed at 0.85 internally.

[0103] The S420 is a low-level speed closed-loop follower based on the incremental proportional-integral-derivative (PID) algorithm. The parallel control unit 330 uses the target speeds of the left and right tracks as the given reference values ​​for the two independent control loops, respectively. The parallel control unit 330 synchronously acquires the actual linear speeds of the left and right tracks fed back by the photoelectric encoder 420 as the state feedback values. The parallel control unit 330 calculates the transient error sequence between the given reference values ​​and the state feedback values ​​and inputs it into the incremental PID controller.

[0104] To enable the underlying control gain to adapt to unstructured soft terrain and severe chassis vibration environments, the parameter tuning unit 340 retrieves and processes the real-time chassis vibration amplitude from the vibration sensor 450 and the bilateral track slippage rate reported by the state fusion unit 320, and performs online real-time tuning of the proportional, integral, and derivative gain parameters. Taking the left track loop as an example, the online gain tuning calculation formula is as follows: ; ; In the formula, The adaptive proportional gain is calculated in real time; The reference constant is set to 12.5, and the range of engineering adjustment values ​​is limited to between 10.0 and 20.0. The first slippage correction factor for the proportional term is fixed at 0.3, with a value range limited to between 0.1 and 0.5. The first vibration correction factor for the proportional term is fixed at 0.1, with its value limited to between 0.05 and 0.2. The real-time vibration amplitude of the chassis is collected and extracted by the vibration sensor 450. The physical unit is meters per second squared, and the value ranges from 0 to 5.0 meters per second squared. The reference constant is used to tune the adaptive integral gain calculated in real time. The calibration value is set at 0.8, and the range of engineering adjustment values ​​is limited to between 0.1 and 2.0. The second slippage correction factor for the integral term is fixed at 0.5, with a value range limited to between 0.2 and 0.8. The reference constant is used to tune the adaptive differential gain calculated in real time. The calibration value is set at 2.1, and the range of engineering adjustment values ​​is limited to between 0.5 and 5.0. The second vibration correction factor for the differential term is fixed at 0.15, with its value limited to between 0.05 and 0.3.

[0105] The incremental control quantity used to adjust the power output of the DC motor 510 is obtained through iteration, and the incremental control quantity is accumulated to generate the absolute control voltage percentage.

[0106] Compared to the traditional positional proportional-integral-derivative (PID) algorithm which directly calculates the full control output, the incremental PID algorithm only outputs the change in the control quantity. When the processor experiences a computational anomaly, it will not produce a large voltage surge across the entire range. This inherent advantage in the algorithm structure helps protect hardware resources such as the DC motor 510 from the impact of instantaneous overload current, thereby improving the overall operational reliability of the underlying electronic control system.

[0107] The incremental control formula, taking the left track closed-loop control as an example, is as follows: ; ; In the formula, For the current number The speed tracking error value of the left track within a discrete control cycle, with the physical measurement unit being meters per second; This is the discrete control step size index number triggered by the timer interrupt of the 310 microcontroller at the lower level, and its value range is a positive integer greater than 2; The output is the target speed of the left track after slippage compensation. The physical measurement unit is meters per second, and the value range is limited to -3.0 to 3.0 meters per second. For the photoelectric encoder 420 in the first The actual linear velocity of the left track is obtained by collecting and converting data in one discrete control cycle. The physical measurement unit is meters per second, and the value ranges from -3.0 to 3.0 meters per second. For the incremental proportional-integral-differential algorithm in the th... The incremental control quantity calculated from each discrete control cycle, expressed as a control percentage. This is the proportional gain constant parameter, used to adjust the system's response to the current speed tracking error value. It is calibrated to 12.5 based on the motor load characteristics. This is the integral gain constant parameter, used to eliminate the steady-state error of the system, and is calibrated to 0.8 based on the chassis drag characteristics; The differential gain constant parameter is used to suppress velocity overshoot oscillations and is calibrated to 2.1 based on the system's dynamic response characteristics. The left track speed tracking error value of the previous discrete control cycle is measured in meters per second. This is the index number corresponding to the previous discrete control cycle, and its value range is a positive integer greater than 1; The left track speed tracking error value is the result of the previous two discrete control cycles, and the physical measurement unit is meters per second. This is the index number corresponding to the previous two discrete control cycles, and its value range is a positive integer greater than 0; For the first The percentage of the left-hand absolute control voltage output at the end of each discrete control cycle is limited to a range of -100 to 100. It is the percentage of the left-hand absolute control voltage of the previous discrete control cycle, and its value is limited to -100 to 100. The maximum absolute control percentage threshold allowed by the physical drive is set to 100 to prevent integral saturation and motor burnout. The range clamping mathematical function is used to strictly limit the accumulated absolute control voltage percentage within the positive and negative maximum absolute control percentage boundary range.

[0108] S430, pulse width modulation signal generation and power bridge arm physical drive, the timer peripheral inside the parallel control unit 330 configures the comparison register of the advanced control timer according to the absolute control voltage percentage to generate a complementary pulse width modulation signal waveform with a specific duty cycle. The complementary pulse width modulation signal waveform is transmitted to the H-bridge power conversion circuit of the motor drive unit 520 through the optocoupler isolation circuit to control the on and off states of the high-power metal oxide semiconductor field-effect transistor.

[0109] The motor drive unit 520 inverts the 24V DC power from the vehicle battery into phase current that drives the dual DC motors 510 to rotate according to the duty cycle ratio, thereby completing the final conversion from digital decision-making instructions to physical mechanical actions. In order to ensure electrical stability during power bridge arm switching, the dead time of the H-bridge power conversion circuit is fixed between 1.5 microseconds and 3.0 microseconds, and the capacitance of the bootstrap capacitor of the complementary pulse width modulation signal is configured between 0.1 microfarads and 1.0 microfarads.

[0110] See attached document Figure 10 The specific implementation of step S500 can be handled through the following sub-steps:

[0111] S510, based on high-frequency sampling of hardware operating status of multi-dimensional physical parameters, controls the microcontroller 310 to call the internal interlocking protection unit 350 to collect the real-time temperature data of the motor through the thermistor integrated on the housing of the DC motor 510 and read the real-time terminal voltage of the vehicle battery through the analog-to-digital converter. The interlocking protection unit 350 synchronously monitors the communication data frame stream between the control microcontroller 310 and the Raspberry Pi processor 210 and counts the number of consecutive packet losses as a communication health feature.

[0112] The interlocking protection unit 350 synchronizes the real-time temperature data, real-time terminal voltage and the number of consecutive packet losses of the motor into a unified discrete evaluation period, which can provide a reliable data benchmark for subsequent risk quantification calculation, thereby avoiding the phenomenon of system false triggering of shutdown protection caused by asynchronous failure of single sensor data.

[0113] S520, based on the comprehensive safety risk index calculation using linear normalization weighted fusion, the interlocking protection unit 350 inputs the real-time motor temperature data, real-time terminal voltage, and number of consecutive packet losses into the preset risk assessment mathematical analysis model for quantitative calculation. Due to the differences in the physical dimensions and range of change of the multi-dimensional physical parameters, the interlocking protection unit 350 uses the linear normalization method to map the multi-dimensional physical parameters to a unified dimensionless interval and combines the penalty weight coefficient calibrated by the engineering to calculate the comprehensive safety risk index that characterizes the overall health level of the current chassis system.

[0114] This mathematical evaluation mechanism, which transforms discrete underlying physical states into continuous numerical sequences, helps the underlying electronic control system identify potential performance degradation trends before irreversible hardware burn-out occurs, thus providing sufficient processing time to trigger safety fault-tolerant control.

[0115] To prevent the excellent physical parameters under normal operating conditions from generating negative values ​​and thus offsetting the actual dangerous conditions, the comprehensive safety risk index calculation formula after adding the maximum value clamping function is as follows: ; In the formula, The comprehensive safety risk index is calculated quantitatively. The physical dimension is a dimensionless parameter, and the value range is limited to 0 to 10. The weighting coefficient for the motor temperature risk item is fixed at 4.0 internally in the program. The maximum value mathematical function is used to ensure that the input parameters output non-negative results to eliminate the risk of negative output and offset the dead zone. 0 is the lower limit of the maximum value mathematical function compared to the zero value constant. The real-time temperature data of the motor is collected by the thermistor during the current discrete evaluation cycle. The physical unit is ℃, and the value range is limited to -20 to 120℃. The reference temperature constant for normal operation of DC motor 510 is fixed at 45℃ based on the motor's no-load heat dissipation calibration test results. The maximum permissible physical temperature threshold for DC motor 510 is 85℃, which is fixed according to the physical parameters of the motor coil insulation class. The weighting coefficient for the battery undervoltage risk item is fixed at 3.0 internally. This is the reference constant for the rated terminal voltage of the vehicle battery when fully charged, with the physical unit being volts, and it is fixed at 24.0 volts. This is the real-time terminal voltage of the vehicle battery read by the analog-to-digital converter during the current discrete evaluation cycle, in volts, with a range limited to 18.0 to 25.2 volts. This is the permissible discharge cutoff voltage threshold for the vehicle battery, measured in volts, and is fixed at 19.5 volts. The weighting coefficient for the packet loss risk item in the communication link is fixed at 3.0 internally. This refers to the number of consecutive packet losses between the Raspberry Pi processor 210 and the control microcontroller 310 within the current discrete evaluation period. The physical unit is a data frame, and the value ranges from 0 to 100 data frames. The maximum number of consecutive packet losses that can trigger a communication link disconnection is fixed at 50 data frames within the program.

[0116] S530, based on the risk index threshold classification, implements safety degradation control and status reporting. The interlocking protection unit 350 compares the comprehensive safety risk index with the preset system safety degradation threshold matrix in real time and triggers the corresponding hardware control degradation strategy according to the comparison result. When the comprehensive safety risk index is in the normal safety range of 0 to 1.5, the microcontroller 310 is controlled to maintain the normal parallel motion control and underlying drive execution process of the chassis system.

[0117] When the comprehensive safety risk index exceeds 1.5 but is below 3.0, the interlocking protection unit 350 determines that the chassis system is in a mild overload decay stage and forcibly limits the percentage of the absolute control voltage output by the parallel control unit 330 to 50% of the normal maximum value in order to reduce the power output of the DC motor 510 and reduce the physical heating rate of the actuator.

[0118] When the comprehensive safety risk index exceeds 3.0, the interlocking protection unit 350 determines that the chassis system has a serious operational fault and immediately sends a hardware emergency stop level signal to the motor drive unit 520 to cut off the enable pin of the H-bridge power conversion circuit, thereby forcing the tracked vehicle body 100 into a physical brake safety state.

[0119] After triggering the emergency stop protection action, the control microcontroller 310 will upload a status feedback message containing real-time motor temperature data, real-time terminal voltage and corresponding fault diagnosis code to the higher-level Raspberry Pi processor 210 via a full-duplex serial bus so that the higher-level cognitive decision-making body 200 can terminate the global navigation task in time and issue an external audible and visual alarm. The design of the hardware emergency stop signal anti-jitter filter circuit of the motor drive unit 520 can be implemented using a typical RC filter network.

[0120] To aid in understanding the technical solution of this invention, the following is an application example of autonomous inspection based on an outdoor unstructured geological exploration scenario.

[0121] Tracked vehicles are deployed at exploration sites that include sandy and gravelly roads as well as steep slopes. Operators issue voice commands to the tracked vehicles that include environmental inspection information of the target rock area.

[0122] After receiving the voice command, the large model parsing unit inside the Raspberry Pi processor parses the autonomous obstacle avoidance and inspection task mode offline and sends out the target coordinates. During the journey, the visual camera, ultrasonic sensor and infrared sensor detect the presence of a group of gravel ahead and a steep sandy slope on the local path. The control microcontroller processes the ultrasonic data and infrared data through variance weighted fusion and outputs the distance to the obstacle. When running the local dynamic window method algorithm, the path planning unit calculates the terrain stability penalty score by combining the predicted pitch angle of the steep slope direction. The system plans the trajectory along the gentle slope based on the terrain stability penalty score.

[0123] When the tracked vehicle enters the sand, the left track slips relative to the ground. The state fusion unit compares the wheel speed measured by the photoelectric encoder with the actual linear velocity of the geometric center of mass calculated by the inertial measurement unit and calculates the slip rate of the left track. The parallel control unit starts the feedforward compensation program based on the slip rate of the left track and increases the target drive speed command value of the left motor. After the vehicle enters the gravel road section, the chassis vibration sensor collects vertical vibration data. The parameter tuning unit adjusts the differential gain parameter and proportional gain parameter of the incremental controller online based on the vibration amplitude data.

[0124] During the hill climbing and obstacle crossing phase, the chassis resistance of the tracked vehicle increases and the heat generated by the on-board DC motor increases. The interlocking protection unit collects motor housing temperature data and battery terminal voltage data through an asynchronous polling program. The interlocking protection unit inputs the motor housing temperature data and battery terminal voltage data into the risk assessment mathematical analysis model and calculates the comprehensive safety risk index. When the comprehensive safety risk index exceeds the set slight overload threshold, the control microcontroller triggers a degradation control strategy and limits the maximum travel linear speed to 50% of the rated speed. After the control microcontroller limits the speed, the motor housing temperature data decreases and the tracked vehicle reaches the target coordinate point.

[0125] Field experiments were conducted based on the above application scenario. The field included a sand slippage area, a gravel vibration zone, a continuous obstacle placement area, and an asymmetric continuous slope with an inclination angle of 22 degrees. The control group adopted an operation scheme that disabled multi-source variance weighted fusion, slippage feedforward compensation, and online parameter tuning, while retaining 90-degree Celsius power-off protection. The proposed solution adopted an operation scheme that enabled multi-sensor spatiotemporal state fusion, feedforward control, and risk index hierarchical interlocking protection. The two control programs ran alternately on the same tracked vehicle hardware and performed eight autonomous cruise missions consecutively. The system data recording module recorded the track coordinates, motor temperature, and chassis attitude parameters during the test at a frequency of 50 Hz. The system extracted the raw recorded data from the eight cruise missions for quantitative summary and analysis.

[0126] Table 1: Test Sample Data Table Comparing Overall Navigation Performance and Track Following Error

[0127] Test-01 1.83 0.36 0.94 0.52 2 0 Test-02 2.11 0.41 1.02 0.48 3 0 Test-03 1.67 0.28 0.88 0.55 1 0 Test-04 2.45 0.33 1.15 0.49 4 0 Test-05 1.94 0.47 0.91 0.51 2 0 Test-06 1.76 0.31 0.96 0.54 1 0 Test-07 2.08 0.39 1.07 0.47 3 0 Test-08 1.92 0.26 0.99 0.53 2 0 Mean Total 1.97 0.35 0.99 0.51 18 0

[0128] Table 2: Monitoring Data of Dynamic Performance and Physical Safety Index of Underlying Drive System

[0129] Traditional solution group 14.86 453.27 91.4 284.5 37.5%

[0130] From Table 1 and Table 2 and Appendix Figure 11 Appendix Figure 12 and attached Figure 13 The data shows that the average maximum yaw error of the traditional scheme group in the sandy area was 1.97 meters and the total number of obstacle collisions was 18. The average maximum yaw error of the scheme group under the feedforward compensation mechanism was 0.35 meters and the total number of obstacle collisions was 0. After the predicted pitch angle penalty term was calculated in the dynamic window method, the average maximum pitch angle fluctuation of the chassis of the scheme group was 0.51 radians, while the average maximum pitch angle fluctuation of the chassis of the traditional scheme group was 0.99 radians.

[0131] Data shows that the driving voltage variance of the proposed solution group under the online proportional-integral-derivative tuning is 108.65, while that of the traditional solution group is 453.27. The peak motor temperature of the traditional solution group during the climbing section is 91.4 degrees Celsius, and the emergency stop failure rate is 37.5%. After the interlocking protection unit of the proposed solution group extracts the risk index and executes the downgrade speed limiting procedure, the peak motor temperature is 78.6 degrees Celsius, and the emergency stop failure rate is 0. The average time of the proposed solution group is 312.8 seconds, while that of the traditional solution group is 284.5 seconds.

[0132] 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 tracked vehicle intelligent obstacle avoidance and navigation system based on multi-sensor fusion, characterized in that, The system hardware layer includes: The tracked vehicle body serves as both the physical load-bearing base and the motion execution carrier; A multi-source heterogeneous sensing mechanism is installed on the tracked vehicle body to acquire external video sequences, motion status and ranging physical signals, as well as equipment temperature data; A high-level cognitive decision-making body, connected to the perception body, is used to perform semantic analysis on the speech signal to determine the task mode, and to calculate the target linear velocity and target angular velocity of the chassis by combining the video sequence. The underlying real-time control mechanism is connected to the high-level cognitive decision-making mechanism. It is used to perform state fusion and noise reduction on the physical signal and calculate the slip rate. It performs inverse kinematic decomposition, slip compensation and parameter tuning on the target linear velocity and angular velocity of the chassis and synthesizes the dual-side reference control linear velocity. The distributed closed-loop actuator drives the chassis motor to operate in a closed loop and maintains the load balance based on the dual-side reference control linear velocity. A high-power power distribution mechanism used to distribute electrical energy to the system.

2. The intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion according to claim 1, characterized in that, The high-level cognitive decision-making mechanism includes a Raspberry Pi processor, a large model parsing unit and a path planning unit running inside the Raspberry Pi processor; the low-level real-time control mechanism includes a control microcontroller, a state fusion unit, a parallel control unit and a parameter tuning unit deployed inside the control microcontroller, and the control microcontroller integrates an interlocking protection unit; the multi-source heterogeneous sensing mechanism includes an inertial measurement unit, an optical encoder, an ultrasonic sensor, an infrared sensor, a vibration sensor and a temperature sensor connected to the input of the control microcontroller, and a visual camera connected to the video input of the Raspberry Pi processor.

3. The intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion according to claim 2, characterized in that, The control microcontroller establishes the system's underlying physical sampling benchmark by generating fixed-period hardware overflow interrupt signals through an internal general-purpose timer. When the timer interrupt service routine responds, the control microcontroller synchronously acquires the physical signals of the inertial measurement unit, photoelectric encoder, ultrasonic sensor, infrared sensor, and vibration sensor through the hardware interface. After capturing an external video sequence, the vision camera writes the image frames at high speed into a circular double-buffer queue in the core memory space of the Raspberry Pi processor using direct memory access, and assigns a global timestamp to the image frames using a high-precision real-time clock.

4. The intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion according to claim 3, characterized in that, The state fusion unit establishes a linear discrete state-space model with the vehicle's true pitch angle, roll angle, and geometric centroid linear velocity as state vectors. It processes the data from the inertial measurement unit and photoelectric encoder by iteratively executing discrete Kalman filtering state prediction update operations, and outputs the vehicle's filtered pitch angle, roll angle, and geometric centroid filtered driving speed. The state fusion unit also performs real-time statistical analysis of the dynamic variance fluctuations of ranging data from multiple ultrasonic and infrared sensors within a continuous historical sampling window, calculates the data confidence weight coefficients of overlapping field-of-view channels online, and outputs the comprehensive obstacle distance by weighted average fusion to remove environmental clutter.

5. The intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion according to claim 4, characterized in that, The state fusion unit uses the vehicle body geometric center of mass filtered driving speed as a reference benchmark for the chassis's true translation speed. It combines the actual angular velocities of the two drive wheels calculated by the photoelectric encoder with the mechanical physical radius parameters of the drive wheels to calculate the theoretical wheel speed. By comparing the deviation between the theoretical wheel speed and the vehicle body geometric center of mass filtered driving speed, it introduces the differential slip kinematics formula to quantitatively calculate the slip rate of the left track and the slip rate of the right track to characterize the physical adhesion state.

6. The intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion according to claim 5, characterized in that, The large model parsing unit receives the input raw speech signal and converts it into Chinese string text commands. It uses a lightweight large language model with weighted quantization to perform offline semantic understanding of the Chinese string text commands to output global task mode command words. The path planning unit synchronously maps the visible driving boundary, the comprehensive obstacle distance, and the vehicle body filtered pitch angle onto a two-dimensional local cost map. It uses a multi-objective cost evaluation function to score and deduce multiple sets of simulated linear velocity and simulated angular velocity sequences sampled within the dynamic window, and selects the optimal simulated velocity sequence with the highest score as the chassis target linear velocity and chassis target angular velocity.

7. The intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion according to claim 6, characterized in that, The multi-objective cost evaluation function is based on a weighted scoring evaluation of heading deviation, obstacle distance, speed, and terrain stability penalty. The evaluation constraint of the terrain stability penalty is as follows: when a sampled set of simulated speed sequences causes the future predicted pitch angle, which is mathematically derived from the current vehicle body filter pitch angle, to approach the limit safe pitch angle allowed by the physical structure of the tracked vehicle, the evaluation score of the corresponding speed sequence is reduced to suppress the chassis from traveling into dangerous areas with large slopes.

8. The intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion according to claim 7, characterized in that, The parallel control unit, combining the fixed mechanical wheel track parameters of the tracked vehicle body, uses a differential drive inverse kinematics model to initially decompose the target linear velocity and target angular velocity of the chassis into the ideal linear velocity of the left track and the ideal linear velocity of the right track. The parallel control unit retrieves the slip rate of the left track and the slip rate of the right track, and performs nonlinear amplification slip compensation on the ideal linear velocities of the left track and the right track respectively, and calculates and outputs the compensation target velocities of the left track and the right track as feedforward speed reference values.

9. The intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion according to claim 8, characterized in that, The parallel control unit calculates the transient error sequence between the feedforward speed reference value and the actual linear speed state value fed back by the photoelectric encoder, and inputs it into the incremental proportional-integral-derivative algorithm unit for closed-loop tracking; the parameter tuning unit extracts the real-time vibration amplitude of the chassis and the slip rate of the tracks on both sides collected by the sensor, performs online real-time tuning of the proportional gain, integral gain and derivative gain parameters, iteratively calculates the incremental control quantity and accumulates the clamping to generate the absolute control voltage percentage, controls the internal peripherals to generate a complementary pulse width modulation signal with a corresponding duty cycle and outputs it to the motor drive unit.

10. The intelligent obstacle avoidance and navigation system for tracked vehicles based on multi-sensor fusion according to claim 2, characterized in that, Within a discrete evaluation cycle, the interlocking protection unit synchronously collects real-time temperature data of the DC motor fed back by the temperature sensor and real-time terminal voltage of the vehicle battery read by the analog-to-digital converter, and counts the number of consecutive packet losses on the full-duplex serial bus. The interlocking protection unit maps the temperature data, real-time terminal voltage and number of consecutive packet losses to a unified dimensionless interval through a linear normalization method, and calculates the comprehensive safety risk index by combining the penalty weight coefficient. The interlocking protection unit compares the comprehensive safety risk index with the preset system safety degradation threshold matrix in real time, and triggers a safety fault-tolerant control strategy based on the assessment and grading results, such as maintaining normal operation, reducing the maximum absolute control voltage percentage, or forcibly disconnecting the enable pin of the motor drive unit to perform physical braking.

Images