Multi-track synchronous walking control system and method based on multi-sensor information fusion

Abstract

The present invention relates to the technical field of multi-track synchronous walking control, and in particular to a multi-sensor information fusion multi-track synchronous walking control system and method, the system comprising: an industrial computer, a master PLC controller, a plurality of slave PLC controllers and a plurality of multi-modal track sensor devices. A set of multi-modal track sensor devices is installed on each track assembly, and the multi-modal track sensor device includes a positioning module, a posture sensor, a speed detection device, a pressure sensor and a current sensor; the industrial computer has a built-in model predictive control algorithm, and the model predictive control algorithm integrates the data of the multi-modal track sensor device to dynamically adjust the motion state of each track. The master PLC controller receives the adjusted target speed instruction and distributes it to the corresponding slave PLC controller. Each slave PLC controller realizes the synchronous walking of multiple groups of track assemblies. The advantage of the present invention is that the control system can perform differential adjustment and independent optimization according to the different working conditions of each track.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-track synchronous walking control, and in particular to a multi-sensor information fusion multi-track synchronous walking control system and method. Background Art

[0002] Tracked vehicles are widely used in military equipment, construction machinery, agricultural machinery, and specialized transportation applications due to their excellent roadworthiness, load-bearing capacity, and terrain adaptability. Compared to wheeled vehicles, tracked vehicles reduce ground contact pressure by increasing their contact area, enabling stable operation on soft, muddy, and rugged surfaces. They also possess greater traction and anti-skid capabilities.

[0003] With the development of heavy equipment and large transport platforms, vehicles with multiple tracks working in coordination have gradually become a research hotspot. Such vehicles typically use multiple independently driven track systems to improve load balancing and mobility. Currently, most research focuses on controlling the synchronous movement of dual-track systems, while research on the synchronous movement of multiple tracks is very limited. In practical applications, a single sensor is generally used to control the theoretical speed of each track equally. By controlling the current of each track, the drive speed of each track is guaranteed to be consistent. However, synchronous movement can only be achieved when the load on each track is strictly the same. Sloping ground, slippage, and different loads can cause the vehicle to slip and lose control.

[0004] Obviously, each set of tracks must strictly maintain synchronous movement, otherwise it will cause uneven mechanical stress, resulting in distortion of the vehicle structure and accelerated wear of the tracks and transmission mechanisms; the movement accuracy will be reduced. When precise steering or path tracking is required, the track speed or position deviation will cause the vehicle to deviate from the target trajectory, affecting the operation accuracy. Summary of the Invention

[0005] Based on this, the present invention proposes a multi-track synchronous motion control system and method based on multi-sensor information fusion. Each track assembly is equipped with a multimodal sensor device to collect real-time motion data from each track. Using a model predictive control algorithm to fuse multi-source sensor information, the system dynamically adjusts the actual and target speeds of each track, achieving high-precision synchronous control. This system features fast dynamic response, strong robustness, and excellent real-time performance.

[0006] To achieve the above-mentioned purpose, the technical solution created by the present invention is implemented as follows: a multi-sensor information fusion multi-group crawler synchronous walking control system is used to control a walking mechanism composed of multiple crawler components, each crawler component includes a crawler, at least one driving wheel and a driving motor, including: an industrial computer, a main PLC controller, multiple slave PLC controllers and multiple groups of multi-modal crawler sensor devices; the number of multiple slave PLC controllers and multiple groups of multi-modal crawler sensor devices is equal to the number of multiple groups of crawler components; each group of crawler components is installed with a group of multi-modal crawler sensor devices, the multi-modal crawler sensor device includes a positioning module, a posture sensor, a speed detection device, a pressure sensor and a current sensor; the signals of the positioning module, the posture sensor, the speed detection device and the pressure sensor are input to the industrial computer; the current sensor is connected to the corresponding slave PLC controller controller connection; wherein, the positioning module is used to obtain the plane position coordinates, actual speed and heading angle of the corresponding track; the attitude sensor is used to obtain the pitch angle of the corresponding track; the speed detection device is used to obtain the theoretical speed of the corresponding track; the pressure sensor is used to obtain the actual load mass of the corresponding track; the current sensor is used to measure the actual current value of the corresponding drive motor; the industrial computer has a built-in model predictive control algorithm, the industrial computer receives the actual data obtained by each group of multimodal track sensor devices, and integrates the actual data obtained by each group of multimodal track sensor devices through the model predictive control algorithm to dynamically adjust the motion state of each track; the main PLC controller receives the adjusted target speed instruction issued by the model predictive control algorithm, and distributes it to the corresponding slave PLC controller; each slave PLC controller executes a dual closed-loop control strategy to achieve synchronous walking of multiple groups of track components.

[0007] Furthermore, the positioning module is a GPS module with a dual-antenna RTK structure, which collects the latitude and longitude coordinates of the corresponding track and performs low-pass filtering on them, then outputs the filtered longitude and latitude coordinates at a fixed frequency, converts the filtered longitude and latitude coordinates into position coordinates in the local coordinate system of the walking mechanism in real time, and finally calculates the heading angle and actual speed of the track based on the converted longitude and latitude coordinates.

[0008] Furthermore, the calculation formulas for the actual speed and heading angle are:

[0009] ;

[0010] ;

[0011] in, ; ( 、 )、( 、 ) represent the time 、 The longitude and latitude coordinates of the center point of the crawler track in the unified coordinate system of the walking mechanism; For crawler tracks 、 The difference in displacement between two moments; For 、 The time interval between two moments; is the actual speed of the track; is the heading angle of the track.

[0012] Furthermore, the speed detection device is an absolute encoder, which achieves time synchronization with the positioning module through a precise time protocol; the speed detection device collects the driving wheel speed pulse signal and calculates the theoretical speed of the crawler in combination with the driving wheel radius.

[0013] Furthermore, the model predictive control algorithm calculates the slip ratio based on the actual speed and theoretical speed of the track, and adjusts the actual speed of the track by the slip ratio so that the actual speed reaches the theoretical speed. The slip ratio formula is:

[0014] ;

[0015] ;

[0016] in, is the slip rate; is the theoretical speed of the track; is the angular velocity of the driving wheel; r is the driving wheel radius.

[0017] Furthermore, the model predictive control algorithm establishes a slope resistance model based on the pitch angle and combined with the vehicle dynamics model to obtain the corrected target speed of the track; the corrected target speed formula is:

[0018] ;

[0019] in, is the corrected target speed of the track; k is the slope correction factor, its unit is meter / second / degree; is the pitch angle in degrees.

[0020] Furthermore, the model predictive control algorithm calculates the target speed compensation of the track based on the pitch angle, gravity acceleration, and the calculation cycle of the model predictive control algorithm. The target speed compensation is added to the target speed command output by the model predictive control algorithm as a feedforward control variable to offset the influence of the gravity component of the walking mechanism on the slope. The calculation formulas for the target speed compensation and the target speed are:

[0021] ;

[0022] ;

[0023] in, Indicates the target speed compensation of the crawler; C indicates the calibration compensation coefficient; is the target speed of the track; T represents the calculation period of the model predictive control algorithm, and its unit is second.

[0024] Furthermore, a dual closed-loop control strategy of a speed loop and a current loop is executed from the PLC controller; in the process of executing the closed-loop control strategy of the speed loop, the actual speed is adjusted by comparing the target speed of the track with the actual speed; in the process of executing the closed-loop control strategy of the current loop, the actual current is adjusted by comparing the target current of the drive motor with the actual current.

[0025] A method for controlling synchronous movement of multiple crawlers using multi-sensor information fusion is implemented based on the multi-sensor information fusion multi-crawler synchronous movement control system, comprising the following steps:

[0026] S1: Set the theoretical speed and slip rate deviation value.

[0027] S2: Obtain the plane position coordinates, actual speed and heading angle of the corresponding track through the positioning module; obtain the pitch angle of the corresponding track through the attitude sensor; obtain the theoretical speed of the track through the speed detection device; obtain the actual load mass of the corresponding track through the pressure sensor; and measure the actual current value of the drive motor through the current sensor.

[0028] S3: The actual data of each crawler obtained in step S2 is transmitted to the industrial computer, and the model predictive control algorithm dynamically adjusts the motion state of each crawler based on the actual data of each crawler.

[0029] S4: The master PLC controller receives the adjusted target speed command issued by the model predictive control algorithm and distributes it to the corresponding slave PLC controllers; each slave PLC controller executes a dual closed-loop control strategy to achieve synchronous movement of multiple groups of crawler assemblies.

[0030] Furthermore, the actual data include actual speed, heading angle, theoretical speed, slip rate, pitch angle, and target speed compensation; the theoretical speed is obtained by collecting the driving wheel speed pulse signal from the speed detection device and combining it with the driving wheel radius; the model predictive control algorithm dynamically coordinates the motion state of each track, including: changing the heading of the walking mechanism by adjusting the heading angle of each track; calculating the slip rate of the track based on the actual speed and theoretical speed of the track, and adjusting the target speed of the track by the slip rate so that the actual speed of the track reaches the target speed; calculating the target speed compensation caused by slope gravity based on the pitch angle, gravity acceleration and driving wheel radius; the target speed compensation is superimposed as a feedforward control quantity on the actual target speed instruction output by the model predictive control algorithm.

[0031] The invention can achieve the following beneficial effects:

[0032] 1) A multi-track synchronous travel control system based on multi-sensor information fusion enables differentiated adjustment and independent optimization for each track's varying operating conditions. It adaptively adjusts each track's actual and target speeds based on terrain and load conditions, while improving the travelability and steering stability of the travel mechanism through heading correction.

[0033] 2) By fusing data from various sets of multimodal track sensors through the Model Predictive Control (MPC) algorithm, the impact of single sensor anomalies or transient failures on control decisions can be effectively eliminated, thereby improving the consistency, robustness, reliability, and accuracy of the overall data.

[0034] 3) Execute a dual closed-loop control strategy from the PLC controller, taking into account both speed tracking and current response, to achieve high-precision, high-bandwidth control of the drive motor speed.

[0035] 4) The PTP protocol can achieve nanosecond-level synchronization accuracy, and the time sequence of multi-source data obtained under the unified clock is consistent, avoiding misjudgment of the fusion algorithm due to time deviation, and ensuring the accuracy of the MPC decision-making basis.

[0036] 5) By pre-compensating for grade resistance, it improves uphill climbing ability and enables smoother braking on downhill slopes. This helps reduce vibration, protects structural components, and reduces control system response delays, thereby improving overall energy efficiency and driving comfort. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] The accompanying drawings, which constitute part of the present invention, are intended to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are intended to explain the present invention and do not constitute an undue limitation of the present invention. In the accompanying drawings:

[0038] Figure 1 This is a flow chart of a multi-track synchronous walking control system based on multi-sensor information fusion provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0039] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and do not constitute a limitation of the present invention.

[0040] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other.

[0041] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention. In addition, the terms "first", "second" and the like are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, features defined as "first", "second" and the like may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, "multiple" means two or more.

[0042] In the description of the present invention, it should be noted that, unless otherwise expressly specified or limited, the terms "installed," "connected," and "connected" should be understood in a broad sense. For example, they can refer to fixed connections, detachable connections, or integral connections; they can refer to mechanical connections or electrical connections; they can refer to direct connections or indirect connections through an intermediate medium; and they can refer to internal connections between two components. Those skilled in the art can understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0043] The present invention will be described in detail below with reference to the embodiments.

[0044] like Figure 1 As shown, an embodiment of the present invention provides a multi-sensor information fusion multi-track synchronous travel control system for controlling a travel mechanism composed of multiple track assemblies, each of which includes a track, at least one drive wheel, and a drive motor. The multi-sensor information fusion multi-track synchronous travel control system includes: an industrial computer, a master PLC controller, multiple slave PLC controllers, and multiple multimodal track sensor devices. The number of multimodal track sensor devices and slave PLC controllers is equal to the number of track assemblies. In this embodiment, the travel mechanism includes at least four track assemblies.

[0045] Each track assembly is equipped with a multimodal track sensor system, which includes a positioning module, an attitude sensor, a speed detection device, a pressure sensor, and a current sensor. Signals from these sensors are fed into an industrial computer, while the current sensor is connected to the corresponding slave PLC controller.

[0046] The positioning module is used to obtain the planar position coordinates, actual speed, and heading angle of the corresponding track. The attitude sensor is used to obtain the pitch angle of the corresponding track. The speed detection device is used to obtain the theoretical speed of the corresponding track. The pressure sensor is used to obtain the actual load mass of the corresponding track. The current sensor is used to measure the actual current value of the corresponding drive motor. The industrial computer has a built-in Model Predictive Control (MPC) algorithm. The industrial computer receives the actual data obtained by each group of multimodal track sensors, integrates the actual data obtained by each group of multimodal track sensors through the MPC algorithm, and dynamically adjusts the motion state of each track.

[0047] The master PLC receives the actual and target speeds of the adjusted crawler tracks from the model predictive control algorithm and distributes them to the corresponding slave PLCs. Each slave PLC executes a dual closed-loop control strategy to synchronize the movement of multiple crawler tracks.

[0048] The multi-track synchronous travel control system, powered by multi-sensor information fusion, enables differentiated adjustment and independent optimization for each track's varying operating conditions. It adaptively adjusts each track's actual and target speeds based on terrain and load conditions, while improving the travelability and steering stability of the travel mechanism through heading corrections.

[0049] In addition, the multi-sensor information fusion multi-group track synchronous walking control system fuses the actual data obtained from each group of multimodal track sensor devices through the model predictive control algorithm (MPC), which can effectively eliminate the impact of single sensor abnormality or temporary failure on control decisions, thereby improving the consistency, robustness, reliability and accuracy of the overall data.

[0050] Each slave PLC controller implements a dual closed-loop control strategy for speed and current. During the speed closed-loop control process, the actual speed is adjusted by comparing the target speed of the crawler track with the actual speed. Specifically, when the actual speed of the crawler track is lower than the target speed, the slave PLC controller increases the actual speed. When the actual speed is higher than the target speed, the slave PLC controller decreases the actual speed.

[0051] During the closed-loop current control strategy, the actual current is adjusted by comparing the target current of the drive motor with the actual current. Specifically, when the actual current is lower than the target current, the PLC controller increases the actual current; when the actual current is higher than the target current, the PLC controller decreases the actual current.

[0052] The dual closed-loop control strategy is executed from the PLC controller, taking into account both speed tracking and current response, to achieve high-precision, high-bandwidth control of the drive motor speed.

[0053] In some embodiments, the industrial computer has built-in robust control or LQR (Linear Quadratic Regulator), fuzzy control, reinforcement learning control, PID control, or sliding mode control. In addition to dual closed-loop control, the PLC controller can also choose full closed-loop control, field-oriented control, adaptive control, single closed-loop control, direct force control, and highly integrated motion control chips.

[0054] The positioning module uses a GPS module with a dual-antenna RTK structure to collect the longitude and latitude coordinates of the track and perform low-pass filtering on them to effectively eliminate instantaneous fluctuation errors. The filtered longitude and latitude coordinates are then output at a fixed frequency and converted into position coordinates in the local coordinate system of the walking mechanism in real time. Finally, the heading angle and actual speed are calculated based on the converted longitude and latitude coordinates, and the heading angle and actual speed are transmitted to the industrial computer.

[0055] The positioning module uses a dual-antenna RTK GPS module with centimeter-level displacement accuracy, providing more stable and accurate heading angle data. This further improves the reliability of the walking mechanism's positioning and attitude estimation in complex terrain.

[0056] The calculation formulas for actual speed and heading angle are:

[0057] ;

[0058] ;

[0059] in, ; ( 、 ) indicates the time The longitude and latitude coordinates of the track center point in the unified coordinate system of the walking mechanism; ( 、 ) indicates the time The longitude and latitude coordinates of the track center point in the unified coordinate system of the walking mechanism; For crawler tracks 、 The difference in displacement between two moments; For 、 The time interval between two moments; is the actual speed of the track; is the heading angle of the track.

[0060] In some embodiments, the positioning module adopts single-frequency GPS, dual-frequency multi-mode GPS, differential GPS, or precise point positioning (PPP).

[0061] The speed detection device is an absolute encoder, which collects the driving wheel speed pulse signal at a fixed frequency and calculates the angular velocity by combining the timestamp difference. A low-pass filter is then used to remove high-frequency noise caused by transmission system vibration. Finally, the theoretical track speed is calculated based on the drive wheel radius (the theoretical track speed is equal to the theoretical drive wheel speed). To achieve cross-sensor time synchronization, the PTP (Precision Time Protocol) protocol is used to uniformly calibrate the timestamps of the positioning module and the speed detection device to ensure data time consistency.

[0062] The PTP protocol can achieve nanosecond-level synchronization accuracy, ensuring the timing consistency of multi-source data acquired under a unified clock, avoiding misjudgment of the fusion algorithm due to time deviation, and ensuring the accuracy of the MPC decision-making basis.

[0063] The theoretical speed calculation formula is:

[0064] ;

[0065] in, is the theoretical speed of the track; is the angular velocity of the driving wheel; r is the radius of the driving wheel.

[0066] In some embodiments, the speed detection device is an incremental encoder, a rotary transformer, a magnetic encoder, or a visual encoder. To achieve cross-sensor time synchronization, in addition to the PTP protocol, other options include NTP (Network Time Protocol), IRIG-B (Inter-Range Instrumentation Group) bus time synchronization, GPS PPS (Pulse Per Second) signal synchronization, CAN (Controller Area Network) bus time synchronization, or software logic time synchronization.

[0067] The model predictive control algorithm calculates the slip ratio based on the actual and theoretical track speeds. The slip ratio indicates the degree of slippage of the track relative to the ground and reflects idling or slipping conditions. The slip ratio is transmitted to the industrial computer, where the model predictive control algorithm adjusts the actual track speed based on the slip ratio, ensuring that the actual speed approaches and equals the theoretical speed. Furthermore, the model predictive control algorithm limits the actual and theoretical speeds of each track based on the current slip ratio to ensure that the slip ratio remains within the slip ratio deviation value.

[0068] The slip ratio formula is:

[0069] ;in, is the slip rate.

[0070] Real-time calculation and feedback of each track's slip rate allows for instant detection of track idling or slipping. Feeding the slip rate back to the MPC as a control variable, rather than manually setting a threshold for slip rate, allows for rapid suppression of slip and adaptive distribution of traction, resulting in a fast response and high accuracy.

[0071] It should be noted that the slip ratio deviation setting needs to be adjusted based on the actual application of the control method. The specific value depends on the track design parameters, operating environment, and load conditions. Generally, the greater the rigidity of the track material, the harder the working surface, and the lighter the load, the smaller the deviation setting should be. For example, when a metal crawler excavator is traveling unloaded on a hard surface, the slip ratio deviation should be controlled within 10%.

[0072] The attitude sensor is a six-axis inertial measurement unit (MIU). It collects acceleration and angular velocity data at the corresponding track position at a fixed frequency. After filtering through a low-pass filter, the data is input into an industrial computer for real-time calculation of the track's pitch angle. The pitch angle is used for slope compensation and dynamic correction of the target speed.

[0073] In some embodiments, the attitude sensor is a three-axis gyroscope, a nine-axis gyroscope, a combination of a fiber optic gyroscope and a magnetometer, or a combination of an accelerometer and a gyroscope.

[0074] The model predictive control algorithm is based on the pitch angle and combined with the vehicle dynamics model to establish a slope resistance model to obtain the corrected target speed of the track. The corrected target speed formula is:

[0075] ;

[0076] in, is the corrected target speed of the track; k is the slope correction factor, its unit is meter / second / degree; is the pitch angle in degrees.

[0077] The model predictive control algorithm uses pitch angle information and a vehicle dynamics model to modify the target speed. This correction reduces control errors and oscillations, ensuring consistent coordination among the tracks, thereby improving vehicle ride smoothness and efficiency. In this way, the model predictive control algorithm can more accurately predict the dynamic behavior of the walking mechanism under different operating conditions and make adjustments based on real-time feedback to achieve even better control performance.

[0078] The model predictive control algorithm calculates the target speed compensation for the track caused by the slope's gravity based on the pitch angle α, gravitational acceleration g, and the model predictive control (MPC) calculation cycle. This target speed compensation is added to the target speed command output by the MPC algorithm as a feedforward control variable to offset the influence of the gravity component on the walking mechanism on the slope.

[0079] ;

[0080] ;

[0081] in, Indicates the target speed compensation amount; C indicates the calibration compensation coefficient; is the target speed of the track; T represents the calculation period of the model predictive control algorithm, and its unit is second.

[0082] By compensating for grade resistance in advance, it improves uphill climbing ability and enables smoother braking on downhill slopes. This helps reduce vibration, protects structural components, and reduces control system response delay, thereby improving overall energy efficiency and driving comfort.

[0083] The actual load of the crawler is measured by a pressure sensor. In this embodiment, the pressure sensor is a piezoelectric film. In some embodiments, the pressure sensor is a strain gauge sensor, a fiber grating sensor, a capacitive pressure sensor, a hydraulic force sensor, or a MEMS pressure sensor array.

[0084] In terms of control logic, the MPC internally establishes a dynamic prediction model for the walking mechanism, which consists of multiple track assemblies. This model predicts the actual speed and slip rate trends of each track assembly over the next several days. Combining actual speed, heading angle, theoretical speed, slip rate, pitch angle, and speed compensation, the MPC optimizes the target speed distribution in real time, ensuring that the actual speed approaches and equals the target speed as much as possible. The MPC proactively adjusts the target speed distribution before any sudden data changes. Simultaneously, the MPC adjusts the theoretical speed of each track based on the current slip rate to ensure that the slip rate remains within the allowable range. The speed difference between the two tracks is adjusted based on the heading angle to ensure the walking mechanism's heading.

[0085] The MPC sends target speeds and target speed commands to the master PLC in real time via the CANopen bus (an open communication protocol based on the Controller Area Network). The master PLC then distributes these commands to each slave PLC via the CANopen bus. After receiving the target speed, the slave PLCs execute a dual closed-loop control strategy for speed and current. The speed loop uses the target speed issued by the MPC and the actual speed feedback from the positioning module to ensure that the actual speed of the crawler tracks approaches and equals the target speed (within a preset range). The current loop calculates the PWM duty cycle signal based on the deviation between the target current set for the drive motor and the actual current measured by the current sensor. This signal is then output to the drive motor to ensure that the actual speed of the drive motor approaches and equals the target speed. The speed loop ensures target speed stability, while the current loop enhances the dynamic performance of the current response, collaboratively achieving precise control of the crawler track's motion.

[0086] In some embodiments, when MPC data is transmitted to PLC, in addition to CANopen, PROFINET IRT (isochronous real-time communication) or POWERLINK (open real-time Ethernet) or Modbus TCP (communication protocol based on TCP / IP protocol) or Modbus RTU (Remote Terminal Unit) or TSN (Time Sensitive Network) can also be selected.

[0087] When transmitting data from the master PLC controller to the slave PLC controller, in addition to CANopen, you can also choose CAN-FD (Controller Area Network Flexible Data-Rate), Modbus TCP (a communication protocol based on TCP / IP), Modbus RTU (Remote Terminal Unit), RS-485, IO-Link (Input / Output Link), Ethernet / IP (EtherNet / Industrial Protocol), or PROFINET (an open communication protocol based on Industrial Ethernet).

[0088] A method for controlling synchronous movement of multiple crawlers using multi-sensor information fusion is implemented based on the multi-sensor information fusion multi-crawler synchronous movement control system, comprising the following steps:

[0089] S1: Set the theoretical speed and slip rate deviation value.

[0090] S2: The positioning module obtains the corresponding track's planar position coordinates, actual speed, and heading angle. The attitude sensor obtains the corresponding track's pitch angle. The speed detector obtains the track's theoretical speed. The pressure sensor obtains the corresponding track's actual load mass. The current sensor measures the actual current of the drive motor.

[0091] S3: The actual data of each crawler obtained in step S2 is transmitted to the industrial computer, and the model predictive control algorithm dynamically adjusts the motion state of each crawler based on the actual data of each crawler.

[0092] Actual data includes actual speed, heading angle, theoretical speed, slip ratio, pitch angle, and target speed compensation. The theoretical speed is calculated based on the drive wheel radius and the drive wheel speed pulse signal collected by the speed detection device.

[0093] The model predictive control algorithm dynamically coordinates the motion states of each track including:

[0094] The heading of the walking mechanism is maintained by adjusting the heading angle of each track. The slip ratio is calculated based on the actual and theoretical track speeds, and the actual track speed is adjusted based on the slip ratio to bring it within the theoretical speed. The target speed compensation for the track caused by the slope's gravity is calculated based on the pitch angle, gravitational acceleration, and drive wheel radius. This target speed compensation is added as a feedforward control variable to the actual target speed command output by the model predictive control algorithm.

[0095] S4: The master PLC receives the adjusted target speed command from the industrial computer and distributes it to the corresponding slave PLCs. Each slave PLC executes a dual closed-loop control strategy to achieve synchronous movement of multiple track assemblies.

[0096] The above specific embodiments do not limit the scope of protection of the present invention. Those skilled in the art will appreciate that various modifications, combinations, sub-combinations, and substitutions may be made based on design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention are intended to be included within the scope of protection of the present invention.

Claims

1. A multi-track synchronous walking control system with multi-sensor information fusion, used to control a walking mechanism composed of multiple track assemblies, each track assembly including a track, at least one drive wheel and a drive motor, characterized in that: include: Industrial computer, master PLC controller, multiple slave PLC controllers and multiple sets of multi-modal crawler sensor devices; The number of the plurality of slave PLC controllers and the plurality of sets of multi-modal crawler sensor devices is equal to the number of the plurality of sets of crawler assemblies; Each set of the crawler assembly is equipped with a set of multi-modal crawler sensor devices, which include a positioning module, a posture sensor, a speed detection device, a pressure sensor and a current sensor; the signals of the positioning module, the posture sensor, the speed detection device and the pressure sensor are input to the industrial computer; the current sensor is connected to the corresponding slave PLC controller; wherein, The positioning module is used to obtain the plane position coordinates, actual speed and heading angle corresponding to the crawler; The attitude sensor is used to obtain the pitch angle corresponding to the crawler; The rotation speed detection device is used to obtain the theoretical speed of the corresponding crawler; The pressure sensor is used to obtain the actual load mass corresponding to the crawler; The current sensor is used to measure the actual current value corresponding to the drive motor; The industrial computer has a built-in model predictive control algorithm, which receives actual data obtained by each group of the multimodal crawler sensor devices, fuses the actual data obtained by each group of the multimodal crawler sensor devices through the model predictive control algorithm, and dynamically adjusts the motion state of each crawler; The master PLC controller receives the adjusted target speed instruction issued by the model predictive control algorithm and distributes it to the corresponding slave PLC controller; Each of the slave PLC controllers executes a double closed-loop control strategy to achieve synchronous movement of the multiple groups of crawler assemblies.

2. The multi-track synchronous walking control system based on multi-sensor information fusion according to claim 1 is characterized in that: The positioning module is a GPS module with a dual-antenna RTK structure. It collects the longitude and latitude coordinates corresponding to the track and performs low-pass filtering on them. It then outputs the filtered longitude and latitude coordinates at a fixed frequency. The filtered longitude and latitude coordinates are converted into position coordinates in the local coordinate system of the walking mechanism in real time. Finally, the heading angle and actual speed of the track are calculated based on the converted longitude and latitude coordinates.

3. The multi-track synchronous walking control system based on multi-sensor information fusion according to claim 2 is characterized in that: The calculation formulas for the actual speed and the heading angle are respectively: ； ； in, ; ( 、 )、( 、 ) represent the time 、 The longitude and latitude coordinates of the center point of the crawler track in the unified coordinate system of the walking mechanism; For crawler tracks 、 The difference in displacement between two moments; For 、 The time interval between two moments; is the actual speed of the track; is the heading angle of the track.

4. The multi-track synchronous walking control system based on multi-sensor information fusion according to claim 3 is characterized in that: The speed detection device is an absolute encoder, which achieves time synchronization with the positioning module through a precise time protocol; the speed detection device collects the driving wheel speed pulse signal and calculates the theoretical speed of the crawler in combination with the driving wheel radius.

5. The multi-track synchronous walking control system based on multi-sensor information fusion according to claim 4 is characterized in that: The model predictive control algorithm calculates the slip ratio based on the actual speed and theoretical speed of the crawler, and adjusts the actual speed of the crawler by the slip ratio so that the actual speed reaches the theoretical speed. The slip ratio formula is: ； ； in, is the slip rate; is the theoretical speed of the track; is the angular velocity of the driving wheel; r is the driving wheel radius.

6. The multi-track synchronous walking control system based on multi-sensor information fusion according to claim 5, characterized in that: The model predictive control algorithm establishes a slope resistance model based on the pitch angle and in combination with the vehicle dynamics model to obtain the corrected target speed of the track; the corrected target speed formula is: ； in, is the corrected target speed of the crawler track; k is the slope correction factor, its unit is meter / second / degree; is the pitch angle in degrees.

7. The multi-track synchronous walking control system based on multi-sensor information fusion according to claim 6, characterized in that: The model predictive control algorithm calculates the target speed compensation of the crawler based on the pitch angle, gravity acceleration, and the calculation period of the model predictive control algorithm. The target speed compensation is added to the target speed command output by the model predictive control algorithm as a feedforward control variable to offset the influence of the gravity component of the walking mechanism on the slope. The calculation formulas for the target speed compensation and the target speed are respectively: ； ； in, represents the target speed compensation of the crawler; C represents the calibration compensation coefficient; is the target speed of the track; T represents the calculation period of the model predictive control algorithm, and its unit is second.

8. The multi-track synchronous walking control system based on multi-sensor information fusion according to claim 1, characterized in that: The slave PLC controller executes a dual closed-loop control strategy of a speed loop and a current loop; In the process of executing the closed-loop control strategy of the speed loop, the actual speed is adjusted by comparing the target speed of the crawler with the actual speed; During the execution of the closed-loop control strategy of the current loop, the actual current is adjusted by comparing the target current of the driving motor with the actual current.

9. A method for controlling the synchronous movement of multiple crawlers using multi-sensor information fusion, characterized in that: The multi-track synchronous walking control system based on the multi-sensor information fusion according to any one of claims 1 to 8 is implemented, comprising the following steps: S1: Set the theoretical speed and slip rate deviation value; S2: Obtain the plane position coordinates, actual speed and heading angle of the corresponding track through the positioning module; Obtain the pitch angle of the corresponding track through the attitude sensor; The theoretical speed of the crawler is obtained through the speed detection device; The actual load mass of the corresponding track is obtained through the pressure sensor; Measure the actual current value of the driving motor through the current sensor; S3: The actual data of each crawler obtained in step S2 is transmitted to the industrial computer, and the model predictive control algorithm dynamically adjusts the motion state of each crawler based on the actual data of each crawler; S4: The master PLC controller receives the adjusted target speed command issued by the model predictive control algorithm and distributes it to the corresponding slave PLC controller; Each slave PLC controller executes a double closed-loop control strategy to achieve synchronous movement of multiple groups of crawler assemblies.

10. The method for controlling synchronous movement of multiple crawlers using multi-sensor information fusion according to claim 9, characterized in that: The actual data includes actual speed, heading angle, theoretical speed, slip rate, pitch angle, and target speed compensation; the theoretical speed is calculated by combining the speed pulse signal of the driving wheel collected by the speed detection device with the radius of the driving wheel; The model predictive control algorithm dynamically coordinates the motion states of each track including: Changing the heading of the walking mechanism by adjusting the heading angle of each of the crawlers; Calculating a slip ratio of the crawler track based on an actual speed and a theoretical speed of the crawler track, and adjusting a target speed of the crawler track according to the slip ratio so that the actual speed of the crawler track reaches the target speed; Based on the pitch angle, gravity acceleration and driving wheel radius, the target speed compensation caused by the slope gravity is calculated; the target speed compensation is added to the actual target speed instruction output by the model predictive control algorithm as a feedforward control variable.

Images