Boat type lotus root harvester multi-sensor fusion adaptive motion control system and method

Abstract

The application claims a kind of boat type lotus root harvester multi-sensor fusion adaptive motion control system and method, comprising: multi-sensor array for acquiring the position, attitude, speed, inclination and motor load information of the lotus root harvester, the multi-sensor array includes GPS, IMU, current sensor and encoder;Microcontroller for running extended Kalman filter EKF data fusion algorithm, multi-modal PID adaptive control algorithm, pure tracking path planning algorithm and nonlinear model predictive control algorithm, through the rolling time domain optimization strategy, the path tracking accuracy, control smoothness and multiple constraint conditions are considered in the prediction time domain, and the optimal control instruction is generated according to state estimation;Actuator, including walking motor and steering gear, for responding to the control instruction, driving the lotus root harvester motion;Simulink hardware-in-the-loop verification system.

Description

Technical Field

[0001] This invention relates to the field of agricultural machinery control technology, specifically to a multi-sensor fusion adaptive motion control system and method for a boat-type lotus root harvester. Background Technology

[0002] Currently, most lotus root harvesting machinery on the market suffers from significant technical deficiencies in motion control, severely hindering improvements in operational efficiency and control precision. Existing technologies primarily employ the following control schemes: First, using a fixed-parameter PID controller to simply adjust the speed of the walking motor. While this control method is generally sufficient on smooth roads, it performs extremely poorly in the complex muddy environment of lotus fields, exhibiting slow system response, severe overshoot, and an inability to cope with sudden load changes. Second, using open-loop control with simple encoder feedback lacks comprehensive perception of machine attitude, position, and environmental parameters, resulting in large path tracking errors and inaccurate steering, frequently leading to problems such as yaw, slippage, and getting stuck in actual operations. Third, using linear walking control based on a preset path, relying entirely on manual intervention for steering and position adjustments, resulting in low automation and difficulty in improving operational efficiency.

[0003] These existing technologies have fundamental problems at the algorithm level. Firstly, there's the issue of the simplistic and rigid control strategy. Traditional lotus root harvesters generally use a single PID controller with parameters fixed at the factory, making adaptive adjustments impossible based on different operating environments and conditions. The lotus root field environment is highly complex and dynamic, with mud depths ranging from a few centimeters to tens of centimeters, significant viscosity variations, varying water levels, and diverse bottom materials including soft mud, hard soil, and rocks. Working in this environment, the machine's load changes drastically and irregularly, and the wheels frequently experience slippage, getting stuck, and sudden increases in resistance. Actual testing revealed that when the machine moves from shallow water into deep mud, the response time of the fixed PID control often exceeds 3 seconds. During this time, the machine has already deviated significantly from the intended path, requiring manual intervention to correct it.

[0004] In terms of path tracking control, most lotus root harvesters still use the most primitive open-loop control method. Operators directly control the speed of the left and right motors via remote control or onboard joysticks to achieve steering and forward movement. This control method relies entirely on the operator's experience and reaction speed, which is not only highly dependent on personnel but also has low accuracy and poor consistency. Slightly more advanced equipment is equipped with GPS positioning and simple path tracking functions, but generally uses a proportional control method based on heading angle error. This method is barely usable on straight paths, but performs poorly in complex scenarios such as turning and obstacle avoidance.

[0005] Problems in utilizing sensor information are equally prominent. Although modern lotus root harvesters may be equipped with multiple sensing devices such as GPS and encoders, the data from these sensors are often used in isolation, lacking an effective information fusion mechanism. GPS data is used for coarse positioning, encoder data for speed feedback, and IMU data for attitude monitoring; no closed-loop system is formed among the various sensors working collaboratively. This decentralized information processing method not only wastes sensor resources but, more seriously, fails to fully exploit the complementary advantages of multiple sensors.

[0006] The most critical issue lies in the significant technical obstacles encountered in the development, verification, and optimization of control algorithms. Traditional control system development relies entirely on physical testing. Algorithm engineers must repeatedly visit lotus fields for debugging, requiring each parameter adjustment to reprogram, run the system on-site, observe the effects, record data, and then return for analysis and modification. This development model is extremely inefficient, with a complete algorithm iteration cycle often taking days or even weeks. More seriously, lotus field operations are heavily influenced by seasons and weather, resulting in a very limited testing window. Field testing also presents safety and cost risks; immature control algorithms can lead to equipment damage, vehicle entrapment, rollovers, and other accidents, each causing economic losses and delays in development. The lack of efficient simulation verification platforms and hardware-in-the-loop testing methods makes rapid iterative optimization of control algorithms virtually impossible, severely hindering technological development and performance improvement.

[0007] A search revealed that application publication number CN109247122B discloses a multi-information fusion system for a combine harvester threshing device, including an embedded processor, a combine harvester threshing device, and a sensor network. One end of the sensor network is connected to the threshing device and is used to collect the operating parameters of the combine harvester threshing device, which is similar to the field and function covered by our patent.

[0008] This patent (CN109247122B) mainly targets the load control of the threshing device of a combine harvester, and uses a sensor adaptive weighted fusion algorithm to achieve automatic adjustment of the feed amount. Its technical limitations are: (1) The application scenario is limited, only solving the load feedback control problem of the threshing device, and not involving precise motion control and path tracking in complex environments; (2) The fusion algorithm is simple, and the adaptive weighted fusion used is mainly used for load parameter fusion, which cannot achieve optimal estimation of multi-dimensional states such as position, attitude, and speed; (3) The control strategy is fixed, and there is a lack of adaptive control mechanism for different working conditions, which cannot meet the working condition switching needs of lotus root harvesters in multi-modal environments such as mud and shallow water; (4) There is a lack of path planning capability, and it is impossible to achieve autonomous navigation and precise operation path control; (5) There is insufficient verification means, and there is a lack of a complete simulation verification platform, resulting in low algorithm development efficiency and high cost.

[0009] This invention proposes a highly superior solution to the above problems: (1) Deep fusion of multiple sensors: The extended Kalman filter (EKF) algorithm is used to fuse information from multiple sources such as GPS, IMU, and encoder to achieve optimal state estimation of position, attitude, and speed. Compared with simple weighted fusion, the positioning accuracy is improved by 55% and the attitude estimation accuracy is improved by 48%; (2) Multimodal adaptive control: The PID parameters are dynamically adjusted by real-time working condition identification (shallow water stable area, deep mud area, hard bottom bumpy area), reducing the path tracking error by 62% and improving the response speed by 70%; (3) Precise path planning: A pure tracking algorithm is introduced to achieve precise path tracking of the boat-type lotus root harvester, improving the tracking accuracy by about 60%; (4) Hardware-in-the-loop verification platform: A digital twin model and automatic parameter optimization system based on Simulink are constructed, shortening the algorithm development cycle from 6 weeks to 1.5 weeks and reducing the time cost by 75%; (5) Complete control architecture: A complete closed-loop system is formed from sensor fusion, working condition identification, adaptive control to path planning, systematically solving the motion control problem of the boat-type lotus root harvester in complex paddy field environment. Summary of the Invention

[0010] This invention aims to solve the problems of the prior art mentioned above. It proposes a multi-sensor fusion adaptive motion control system and method for a boat-type lotus root harvester. The technical solution of this invention is as follows:

[0011] A multi-sensor fusion adaptive motion control system for a boat-type lotus root harvester, comprising:

[0012] A multi-sensor array is used to acquire the position, attitude, speed, tilt angle and motor load information of the lotus root harvester. The multi-sensor array includes GPS, IMU and encoder.

[0013] The microcontroller is used to run the Extended Kalman Filter (EKF) data fusion algorithm, the multimodal PID adaptive control algorithm, and the pure tracking path planning algorithm, and to generate control commands based on the state estimation.

[0014] The actuator, including a walking motor and a servo motor, is used to drive the lotus root harvester to move in response to the control command;

[0015] The Simulink hardware-in-the-loop verification system includes a digital twin model, an environmental interaction model, a sensor simulation model, and an interface for real-time communication with the microcontroller. It is used for simulation based on the state estimation and control commands to optimize the multimodal PID adaptive control algorithm and the pure tracking path planning algorithm.

[0016] Furthermore, the multimodal PID adaptive control algorithm includes dynamically adjusting the forward look-ahead distance L. dThe strategy, based on the speed v and steering performance of the lotus root harvester, uses a speed coefficient k and a minimum forward sight distance L. d Adjusting _min improves path tracking accuracy.

[0017] Furthermore, it also includes a boundary constraint processing mechanism, which uses a virtual force field method to generate a repulsive force F_rep when the lotus harvester approaches the boundary of the lotus field or an obstacle, and converts it into an additional expected heading angle correction Δθ to guide the lotus harvester away from the danger zone.

[0018] Furthermore, it also includes a shallow water effect compensation algorithm. When the ratio of the water depth h to the ship's draft T is less than 1.5, indicating that the ship is in shallow water, the forward sight distance L is increased. d And reduce the steering angle gain k_δ.

[0019] Furthermore, it also includes a thrust constraint optimization method, which uses a saturation function (Saturation) to limit the calculated thrust to the range of the maximum thrust T_max of the thruster. If the constrained thrust cannot meet the desired motion, the thrust of the left and right thrusters is recalculated through an optimization strategy that minimizes the force and torque errors. and .

[0020] Furthermore, it also includes a rudder angle limiting and rate of change limiting mechanism, which constrains the steering angle θ to be within the range of [-30°, 30°], and the rudder angle change rate |Δθ(t) / Δt| does not exceed 15° / s for two consecutive control cycles.

[0021] Furthermore, the EKF data fusion algorithm includes setting initial values ​​for the process noise covariance matrix Q and the observation noise covariance matrix R, and adaptively adjusting them according to the actual operating conditions to improve the accuracy of state estimation and the fusion efficiency of the sensor data.

[0022] A motion control method for a lotus root harvester employing any one of the systems described above, comprising the following steps:

[0023] Step 1: Apply the Extended Kalman Filter (EKF) data fusion algorithm to fuse the GPS, IMU, and encoder data to obtain the state estimate of the lotus root harvester;

[0024] Step 2: Implement the working mode recognition algorithm to dynamically adjust the parameters of the PID controller to adapt to the current state of the lotus root harvester;

[0025] Step 3: Based on the current position and target path, use a pure tracking path planning algorithm to calculate the desired steering angle and thrust distribution, and generate control commands;

[0026] Step 4: Real-time hardware-in-the-loop simulation verification with the microcontroller is achieved through the digital twin model in the Simulink environment, including real-time data interaction and automatic optimization of the algorithm parameters based on the simulation data.

[0027] Furthermore, the working mode recognition algorithm uses the ratio of the speed v measured by the encoder to the theoretical speed v_th, the standard deviation σ_a of the IMU acceleration a, and the ratio of the motor current I to the rated current I_nom to identify the working mode and trigger parameter switching to adapt to different working environments.

[0028] Furthermore, in the pure tracking path planning algorithm, the calculation of the desired steering angle θ_des is based on the current speed v of the lotus root harvester and the position of the forward target point relative to the coordinate system of the lotus root harvester, ensuring precise steering control under complex paths;

[0029] In the Simulink hardware-in-the-loop simulation verification, genetic algorithms and particle swarm optimization algorithms are used to automatically search for the optimal configuration in the parameter space, optimize the performance indicators of the multimodal PID adaptive control algorithm and the pure tracking path planning algorithm, and improve the efficiency of parameter tuning.

[0030] The advantages and beneficial effects of this invention are as follows:

[0031] After long-term research and development and systematic testing and verification, this invention has shown significant technical advantages and practical value in many aspects compared with the existing technology, and provides a practical solution for the intelligent development of motion control of lotus root harvesters.

[0032] This invention represents a breakthrough in control precision and environmental adaptability. Traditional lotus root harvesters generally employ fixed-parameter PID control, which is barely usable in ideal environments but performs poorly under the complex and ever-changing working conditions of lotus fields, frequently exhibiting problems such as path deviation, slow response, and severe overshoot. The multimodal PID adaptive control algorithm proposed in this invention fundamentally solves this problem. By identifying the working environment in real time and dynamically adjusting control parameters, the system can quickly adapt to environmental changes from shallow water to deep mud. Actual test data shows that under extreme conditions where the mud depth abruptly increases from 5 cm to 30 cm, the control system of this invention can complete parameter adjustment and restore stable control within 1.5 seconds, while the traditional fixed PID system requires more than 5 seconds, and the path deviation reaches 1.2 meters during the adjustment process. In terms of overall performance, the root mean square error of path tracking of this invention is reduced by 62% compared to traditional methods, speed control precision is improved by 45%, overshoot is reduced by 58%, and system response speed is improved by 70%.

[0033] In terms of path tracking performance, the pure tracking algorithm introduced in this invention exhibits significant advantages. Traditional methods mostly use simple proportional control based on heading angle error. This method is barely usable on straight paths, but performs poorly on curved paths, exhibiting inaccurate turning radius control and frequently resulting in path oscillations and difficulties in approaching the target point. The pure tracking algorithm of this invention fully considers the kinematic characteristics of the ship, accurately calculating the required steering angle and achieving smooth path tracking. Comparative tests show that when executing a standard S-curve path, the average tracking error of traditional heading angle control reaches 0.85 meters, with a maximum error exceeding 1.5 meters, while the average error of the pure tracking control of this invention is only 0.32 meters, with a maximum error not exceeding 0.6 meters, improving tracking accuracy by approximately 60%. More importantly, the pure tracking algorithm, combined with a speed-adaptive forward look-ahead distance mechanism, maintains good tracking performance at different speeds. At high speeds, it anticipates and avoids understeer, while at low speeds, it shortens the forward look-ahead distance to improve accuracy, resulting in an overall improvement in operational efficiency of approximately 35%.

[0034] The application of multi-sensor fusion technology significantly improves the reliability and robustness of the system. Traditional systems often use data from individual sensors in isolation; when a sensor malfunctions or has a large error, the entire system fails. This invention employs an extended Kalman filter algorithm to optimally fuse information from multiple sources, including GPS, IMU, and encoder, which not only improves the accuracy of state estimation but also enhances the system's fault tolerance. Tests show that when GPS signal obstruction reduces accuracy to the 5-meter level, the positioning error of traditional methods deteriorates to the same level. However, the fusion algorithm of this invention can utilize the short-term high-precision characteristics of the IMU and encoder to control the positioning error within 1.2 meters, with a performance degradation of less than 25%. When the encoder produces a large error due to wheel slippage, the fusion algorithm can detect and correct it in a timely manner using information from GPS and IMU, avoiding the impact of erroneous information on the control system. Overall, multi-sensor fusion improves the system's positioning accuracy by 55%, attitude estimation accuracy by 48%, and single-point fault tolerance by more than 80%. Attached Figure Description

[0035] Figure 1 This is a schematic diagram of the multi-sensor fusion adaptive motion control system for a boat-type lotus root harvester, provided by a preferred embodiment of the present invention. Detailed Implementation

[0036] The technical solutions of the embodiments of the present invention will be clearly and thoroughly described below with reference to the accompanying drawings. The described embodiments are merely some embodiments of the present invention.

[0037] The technical solution of the present invention to solve the above-mentioned technical problems is:

[0038] The core technical problems to be solved by this invention include the following aspects:

[0039] First, the problem of multi-condition adaptive control. Traditional fixed-parameter PID controllers cannot adapt to the complex and variable characteristics of lotus pond environments. This invention proposes a multi-modal PID adaptive control strategy, which dynamically switches or merges different PID parameter sets by identifying the machine's operating mode in real time (shallow water area, deep mud area, hard bottom area, etc.) to achieve adaptive control for different operating conditions. The key lies in designing a reasonable modal identification algorithm to accurately determine the current operating environment and designing a smooth parameter switching strategy to avoid control jumps during the switching process. At the same time, it is necessary to establish a load estimation model based on multi-source information such as encoder speed feedback, IMU acceleration information, and motor current changes to provide a basis for parameter adaptation.

[0040] Second, the problem of high-precision path tracking control. As a boat-like mobile robot, the kinematic characteristics of the lotus root harvester make it difficult for simple PID control to achieve accurate path tracking. This invention introduces a pure tracking algorithm into the control of the lotus root harvester, which can calculate the optimal steering angle based on information such as the current position, target path, and vehicle parameters. Key technologies include calculating the corresponding rudder angle and thrust distribution based on the hull geometry and hydrodynamic characteristics to improve tracking accuracy at different speeds; and coordinated integration with a multimodal PID controller. The pure tracking algorithm provides steering commands, while the PID controller is responsible for speed adjustment. The two need to work closely together to achieve optimal control results.

[0041] Third, the problem of deep fusion of multi-sensor information and closed-loop control. This invention proposes a multi-sensor fusion algorithm based on extended Kalman filtering, which optimally fuses absolute position information provided by GPS, attitude and acceleration information provided by IMU, and relative displacement information provided by encoder to obtain more accurate and reliable state estimates. The key lies in establishing accurate system state equations and observation equations, rationally designing the covariance matrix of process noise and observation noise, and introducing an adaptive mechanism to dynamically adjust filter parameters. Furthermore, a complete multi-layer closed-loop control architecture is constructed, including outer-loop control based on position error, middle-loop control based on velocity error, and inner-loop control based on current. The closed loops are nested and work collaboratively to form a complete information flow from sensors to actuators.

[0042] Fourth, the issue of rapid verification and parameter optimization. This invention constructs a hardware-in-the-loop verification system based on the Simulink platform. By establishing a precise digital twin model of a lotus root harvester, including vehicle dynamics, environmental interaction, and sensor models, it simulates real-world operating scenarios in a simulation environment. The key technology lies in achieving real-time bidirectional communication between the STM32 microcontroller and the Simulink, Unity, and Gazebo simulation environments. The controller runs real control algorithm code, acquires sensor data from the simulation environment, and sends control commands to the simulation environment, forming a closed-loop hardware-in-the-loop system. This ensures the realism of the controller while avoiding the limitations of physical testing, allowing for thorough testing of algorithm performance under various extreme conditions. More importantly, it designs an automatic parameter optimization mechanism based on simulation data. Through intelligent optimization methods such as genetic algorithms and particle swarm optimization, it automatically searches for the optimal parameter combination based on preset performance indicators (path tracking error, response time, overshoot, etc.), significantly improving the efficiency and effectiveness of parameter tuning.

[0043] By systematically solving the aforementioned technical problems, the main objective of this invention is to provide a technologically advanced, reliable, and easy-to-implement motion control system for lotus root harvesters. This system not only significantly improves path tracking accuracy and operational automation, but also adapts to the complex and ever-changing environment of lotus root fields, providing a practical technical solution for the intelligent development of lotus root harvesting machinery.

[0044] 1. Clearly and completely describe the technical solution of the invention.

[0045] The present invention relates to a multi-sensor fusion adaptive motion control system for a wheeled lotus root harvester based on Simulink hardware-in-the-loop. In terms of overall architecture design, it adopts a hierarchical and nested closed-loop design concept, dividing the entire control system into four core layers: a multi-sensor information fusion layer, a multi-modal adaptive control layer, a pure tracking path planning layer, and a hardware-in-the-loop verification and optimization layer. Through the Simulink platform, it realizes close collaboration and information interaction between each layer, forming a complete control closed loop from perception to decision-making to execution.

[0046] 4.1 Core Algorithm of Multimodal PID Adaptive Control

[0047] Multimodal PID control is the fundamental control algorithm of this invention. Addressing the diversity and dynamism of the lotus pond working environment, the system no longer uses a single fixed-parameter PID controller, but dynamically adjusts the control parameters according to the machine's operating mode. Operating mode identification is a key step in algorithm implementation. This invention comprehensively utilizes information from multiple sensors to establish a mode recognition model. The wheel speed signal fed back by the encoder can reflect slippage; when the actual wheel speed is much greater than the theoretical wheel speed calculated based on motor commands, it indicates severe slippage, and the machine may be in a deep mud area. IMU accelerometer data can reflect the machine's motion state; large acceleration fluctuations indicate bumpy road surfaces or drastic changes in resistance. Changes in motor current directly reflect the load size; a sudden increase in current indicates significant resistance.

[0048] 4.1.1 Working Mode Recognition Mechanism

[0049] In Simulink, the modal recognition algorithm is implemented through the Function module. This function receives multiple input signals, including encoder speed, IMU acceleration, and motor current, and internally defines several key judgment thresholds. First, the speed slip ratio is calculated:

[0050]

[0051] in This represents the wheel linear velocity (m / s) measured based on the encoder, while This represents the actual forward speed (m / s) of the vehicle obtained based on the fusion of the positioning system and IMU. A slip ratio s greater than 0.3 is considered a slipping state. Simultaneously, the variance of acceleration is monitored, and the standard deviation of acceleration over the most recent N sampling points is calculated. A bumpy state is identified when the standard deviation exceeds a preset threshold. The acceleration variance is calculated as follows:

[0052]

[0053] The use of "N-1" instead of "N" is due to unbiased estimation. The ratio of motor current to rated current reflects the load level, i.e., the motor load ratio. , This represents the current actual current of the motor. This refers to the motor's rated current. When the ratio is greater than 1.5, it is determined to be a heavy load condition. Based on the combination of these conditions, the system identifies the current operating mode: shallow water stable zone, deep mud zone, hard bottom bumpy zone, etc.

[0054] For different operating modes, the system presets corresponding PID parameter sets. In the shallow, stable water zone, the road surface resistance is small and uniform. In this case, a larger proportional and derivative coefficient is needed to ensure rapid response and stability, while the integral coefficient can be appropriately reduced to avoid integral saturation. In the deep mud zone, due to severe slippage and irregular resistance, the proportional coefficient needs to be reduced to avoid overreaction, the integral coefficient increased to overcome continuous resistance, and the derivative coefficient appropriately reduced to filter out noise in the velocity signal. In the hard-bottom, bumpy zone, the main problems are sudden changes in resistance and impacts. In this case, the derivative coefficient should be set relatively large to suppress overshoot, while the proportional and integral coefficients should be kept at a moderate level.

[0055] 4.1.2 Working Mode Parameter Switching Strategy

[0056] The design of the parameter switching strategy is crucial. Direct parameter jumps can lead to abrupt changes in control output, causing machine jitter or even instability. This invention employs a smooth switching strategy with gradual parameter changes. When a mode switch is detected, the parameters are not immediately switched from the old value to the new value, but rather gradually transitioned within a time window. In Simulink, a first-order inertial element is used to achieve the smooth transition of parameters. The time constant is adaptively adjusted according to the severity of the mode switch; a larger mode difference results in a longer transition time, and a smaller mode difference results in a shorter transition time. Specifically, a Memory module is used to store the parameter values ​​from the previous time step, and the parameter values ​​at the current time step are:

[0057]

[0058] The PID proportional coefficient at the current time k is where the smoothing coefficient α is determined by both the modal difference and the time constant. When the modal difference is large, the transition time is long, and when the modal difference is small, the transition time is short.

[0059] 4.1.3 Operating Mode Control Algorithm

[0060] In the internal implementation of the PID controller, this invention employs an incremental PID algorithm, which offers better resistance to integral saturation compared to positional PID. The output of the incremental PID is:

[0061]

[0062] Where e(k) is the error at the current time, and e(k-1) and e(k-2) are the errors at the previous two time points. This represents the increment of the control quantity at time k (such as thrust increment, rudder angle increment, etc.), with units depending on the specific controlled object. In Simulink, a Memory module is used to store historical error values, ensuring that correct historical data is obtained in each control cycle. Additionally, an output limiting circuit is implemented to prevent excessive control quantity from causing actuator saturation. Integral separation technology is also introduced: when the error is large, integral action is disabled, and only proportional and derivative control is used to quickly eliminate the error; integral action is only enabled when the error decreases to a certain range. This avoids integral saturation and improves the system's response speed.

[0063] 4.2 Implementation of Pure Tracking Algorithm and Path Control

[0064] The pure tracking algorithm is the core method of this invention for path tracking of a ship-type lotus root dredger. Based on a ship kinematics model, this algorithm fully considers the motion characteristics of surface vehicles. Unlike wheeled vehicles, the ship-type lotus root dredger achieves motion control through thrust generated by propellers and rudder control of heading, making its kinematic characteristics closer to those of unmanned surface vessels. The basic principle of the algorithm is to select a target point at a predetermined distance in front of the hull, calculate the heading angle and thrust required to reach that target point from the current position, and then calculate the corresponding rudder angle and thrust distribution based on the hull's geometric parameters and hydrodynamic characteristics. This method offers better path tracking performance compared to simple heading angle control, especially in curves and complex waterways.

[0065] 4.2.1 Establishment of the ship's kinematic model

[0066] The first step in implementing the algorithm is to establish a kinematic model of the ship's hull. In Simulink, the forward kinematics solution is implemented using the MATLABFunction module, with the current thrust as the input. The rudder angle δ is used as the output, which is the rate of change of the ship's position in the world coordinate system, dx / dt and dy / dt, and the rate of change of the heading angle, dθ / dt.

[0067] The ship's kinematic equations take into account the effects of hydrodynamic damping and environmental disturbances, with the longitudinal motion equation being:

[0068]

[0069] Where v is the linear velocity of the hull, ψ is the heading angle of the hull (rad), and represents the angle between the longitudinal axis of the hull and the x-axis of the world coordinate system; the upper point indicates that it is the reciprocal of time.

[0070] δ is the rudder angle (rad), representing the rudder deflection angle; The thrust (N) generated by the thruster; The moment of inertia of the hull about the x-axis (roll axis) )

[0071] and and These represent the velocity components of the water flow in the x and y directions, respectively. These differential equations are used by an integrator to obtain the actual position and attitude of the ship, forming a ship kinematics simulation model.

[0072] 4.2.2 Forward-looking target point algorithm

[0073] The core of a pure tracking controller lies in the selection of the forward look-ahead point and the calculation of the heading angle. For a ship-type lotus root digger, the forward look-ahead distance... The design needs to consider the inertial characteristics of the hull and the hydrodynamic response delay. This invention employs a forward look-ahead distance strategy that adapts to speed and steering performance.

[0074]

[0075] in For speed coefficient, ,

[0076] This is to prevent insufficient forward visibility from causing delayed steering. Where ρ is the coefficient of inertia and ρ is the density of water. This represents the wetted surface area.

[0077] This increases the forward look-ahead distance at high speeds to compensate for the ship's inertia, and also increases it when steering performance is poor to allow for earlier prediction. At low speeds, the forward look-ahead distance is shortened to improve tracking accuracy. The algorithm first finds the point on the path closest to the current position.

[0078] Then proceed along the path. The distance is taken into account, and the effect of water flow drift is also considered to obtain the corrected forward target point.

[0079] When calculating the forward-looking target point, the forward-looking distance from the hull is searched along the current path. The target point. Assume the path consists of a series of waypoints: .in .

[0080] First, find the path point closest to the current position:

[0081]

[0082] Then search for the forward target point along the path direction. :

[0083]

[0084] in From arrive The cumulative distance.

[0085] These represent the forward-looking target points respectively. x and y coordinates (m) in the world coordinate system.

[0086] When the cumulative distance First time greater than or equal to forward sight distance At that time, the corresponding path point This is the target point we are looking for.

[0087] 4.2.3 Calculation of Desired Heading Angle and Rudder Angle Control

[0088] Let the coordinates of the forward-looking target point in the ship's coordinate system be... Desired heading angle The calculation requires a coordinate transformation first:

[0089]

[0090] The desired heading angle is:

[0091] The calculation of rudder angle needs to take into account the ship's maneuverability. According to ship maneuvering theory, the desired angular velocity r... des Calculated using a PID controller based on the heading angle error:

[0092]

[0093] The relationship between the rudder angle δ and the desired angular velocity is as follows: ; It is the rudder angle fine adjustment, used to counteract fixed disturbances such as water flow and hull eccentricity, to ensure straight navigation when there is no need to turn. It is the steering angle gain, which characterizes the proportional relationship between the desired angular velocity and the actual rudder angle, and controls the rudder angle response sensitivity.

[0094] 4.2.4 Propulsion Distribution Strategy

[0095] The propulsion distribution strategy is another crucial aspect of the motion control of ship-type lotus root dredgers. Unlike single-propeller ships, many ship-type lotus root dredgers are equipped with dual propellers or a propeller plus a side thruster configuration, requiring the rational distribution of propulsion force to achieve the desired motion. This invention employs a thrust distribution algorithm based on virtual force and torque, decomposing the desired motion of the hull into longitudinal forces. lateral force and steering torque Three components. For a dual-thruster configuration, the thrust of the left and right thrusters... and Solve using the following system of equations:

[0096]

[0097] in This is the distance between the two thrusters. If side thrusters are configured, the lateral force... By side thruster supply: = This allocation strategy is implemented in Simulink using the Matrix Multiply and Gain modules to form the thrust allocation matrix. The solution yields:

[0098]

[0099]

[0100] in:

[0101] · The thrust (N) of the left and right thrusters.

[0102] · Desired longitudinal force (N)

[0103] · The desired steering torque (N·m)

[0104] · The distance between the two thrusters (m) is the core geometric parameter for calculating the steering torque (torque = thrust difference × distance / 2).

[0105] 4.2.5 Innovation Points of Path Planning Constraints

[0106] Considering the operational characteristics of boat-type lotus root harvesters in complex waterways within lotus fields, the algorithm also needs to address shallow water effects and boundary constraints. In shallow water areas, the hydrodynamic coefficients of the hull change, affecting maneuverability. This invention adjusts algorithm parameters based on real-time water depth information. When the ratio of water depth h to hull draft T, h / T, is less than 1.5, it is considered to have entered a shallow water zone. In this case, the forward sight distance is increased and the rudder angle gain is decreased to compensate for the reduced maneuverability.

[0107] Boundary constraint handling is implemented using a virtual force field method, which generates a repulsive force when the ship approaches the boundary of the lotus field or an obstacle.

[0108]

[0109] Where d is the distance to the boundary, For a safe distance, This represents the repulsive force coefficient. This repulsive force is converted into an additional desired heading angle correction, guiding the ship away from dangerous areas. Through these improvements, the pure tracking algorithm can achieve stable and reliable path tracking control in complex lotus pond environments.

[0110] When the calculated thrust exceeds the thruster's capability, constraint optimization is required. Let the maximum thrust of the thruster be... Using the saturation function:

[0111]

[0112]

[0113] If the constrained thrust cannot achieve the desired motion, an optimized allocation strategy is adopted to minimize the error between force and torque:

[0114]

[0115] The constraints are: ; It is the maximum output thrust of the thruster, determined by the rated power of the motor and the efficiency of the thruster, limiting the thrust to not exceed the physical limit.

[0116] 4.2.6 Algorithm Fusion and Coordination

[0117] The fusion of pure tracking algorithm and multimodal PID control is achieved through a motion coordinator:

[0118]

[0119] in: The path tracking error is (m). This is the path error adjustment coefficient; This refers to the modally dependent rudder angle adjustment coefficient;

[0120] The integration of pure tracking algorithms and multimodal PID control is key to achieving complete motion control. The pure tracking algorithm calculates the desired heading angle and propulsion strategy, addressing the questions of "where to go" and "how to go." The multimodal PID controller adjusts the thruster speed and rudder angle, addressing the question of "how to execute precisely." In system design, the pure tracking controller's output of the desired heading angle and thrust distribution command acts on the underlying actuators, while the thruster speed control is determined by the PID controller based on thrust error and environmental conditions. Specifically, when the path tracking error is large, the PID controller appropriately increases the thrust to accelerate the correction of path deviations; when a sharp turn is required at a curve or forward target point, the longitudinal thrust is appropriately reduced and the steering torque is increased to ensure steering flexibility and safety. This coordination strategy is implemented in Simulink through a ship motion coordinator module. This module receives inputs such as path error, heading angle error, target point curvature, and environmental disturbances, and outputs thrust and rudder angle adjustment coefficients. These are multiplied by the reference command to obtain the final execution command, which is then precisely tracked by the respective PID controllers.

[0121] 4.3 Multi-sensor information fusion and closed-loop control architecture

[0122] 4.3.1 Multi-sensor fusion system design

[0123] Multi-sensor information fusion is fundamental to improving the accuracy and reliability of control systems. The sensors equipped in a lotus root harvester include a GPS positioning module, an inertial measurement unit (IMU), wheel encoders, tilt sensors, and current sensors. Each of these sensors has its own advantages and disadvantages, requiring information fusion algorithms to achieve complementary strengths. GPS can provide absolute position information, but its update frequency is low (1–10 Hz), and its accuracy decreases when the signal is blocked. The IMU can provide high-frequency attitude and acceleration information, but it suffers from zero drift and error accumulation. The encoder can provide relative displacement information, but it generates significant errors when slipping.

[0124] This invention employs the Extended Kalman Filter (EKF) algorithm to fuse information from multiple sensors. For a lotus root harvester, the state vector is defined as:

[0125]

[0126] Where (x, y) represents the position, For heading angle, For velocity. The state equation describes the evolution of the state over time:

[0127]

[0128] The state transition function based on the vehicle kinematics model is:

[0129]

[0130] Implementing EKF in Simulink requires writing functions for the state equations and observation equations, as well as functions for calculating the Jacobian matrix. The state equations are based on the vehicle's kinematics model, and their specific form has been given previously. The observation equations describe the relationship between sensor measurements and the state:

[0131]

[0132] in The observation vector includes GPS position, IMU attitude angular velocity, encoder velocity, etc. To observe noise.

[0133] 4.3.2 Extended Kalman Filter Core Algorithm

[0134] The EKF recursive process includes two steps: time update (prediction) and measurement update (correction). In the time update phase, based on the state estimate from the previous time step and the current control input, the state at the current time step is predicted using the state equation:

[0135]

[0136]

[0137] The measurement update phase is similar to the above:

[0138]

[0139]

[0140] in and The Jacobian matrices for the state equation and the observation equation are respectively. and These are the covariance matrices of process noise and observation noise, respectively.

[0141] In Simulink, the EKF algorithm is implemented through a dedicated MATLAB Function module. This module maintains persistent variables for the state estimates and covariance matrix, performing a complete prediction and update process once per sampling period. To improve the algorithm's real-time performance, efficient algorithm libraries are used for matrix operations to avoid unnecessary matrix inversion calculations. Furthermore, an outlier detection mechanism is introduced; when the measured value of a sensor deviates significantly from the predicted value, the reliability of that sensor is reduced, and its weight in the fusion process is correspondingly decreased.

[0142] 4.3.3 Multi-layer closed-loop control architecture

[0143] The design of a multi-layered closed-loop control architecture is crucial for ensuring system performance. This invention employs a three-layer nested closed-loop structure, consisting of a position loop, a speed loop, and a current loop from the outside in. The outermost position loop uses the fused position state as feedback, compares it with the desired path, and calculates the desired steering angle and speed using a pure tracking algorithm. The control cycle of this layer is relatively slow, typically 50-100ms. The middle speed loop uses the fused speed state as feedback, compares it with the desired speed output from the position loop, and calculates the desired motor torque or current using a multi-modal PID controller. The control cycle of this layer is 10-20ms. The innermost current loop uses the measurement value from the current sensor as feedback, and adjusts the PWM duty cycle using a fast PID or PI controller to control the actual motor current. This layer has the fastest control cycle, typically 1-5ms.

[0144] 1. Position Loop Controller: The outer loop controller receives the reference path and the current position estimate. The position error is calculated as follows:

[0145]

[0146] 2. Speed ​​Loop Controller: The multimodal PID control law of the middle loop controller is:

[0147]

[0148] 3. Current Loop Controller: The inner loop uses a PI controller to achieve fast and accurate current tracking.

[0149]

[0150] Coordination between the three closed loops is achieved through the design of control cycles. In Simulink, different control loops use different sampling periods, and the data rate conversion is implemented through the Rate Transition module. The output of the position loop serves as the setpoint for the velocity loop, and the output of the velocity loop serves as the setpoint for the current loop. Each layer focuses only on error elimination within its own range, forming a cascaded control structure. This multi-layered nested design not only improves control accuracy but also enhances the system's anti-interference capability. External interference is eliminated by the outer loop, and internal interference is eliminated by the inner loop, each performing its specific function.

[0151] 4.4 Design and Implementation of Nonlinear Model Predictive Control Algorithm

[0152] To address the problem that traditional feedback control struggles to handle complex constraints, this invention adds a nonlinear model predictive control (NMPC) layer to the multimodal PID control and pure tracking algorithm. Through online optimization, it comprehensively considers future states and multiple constraints in the prediction time domain to generate predictive, smooth, and optimal control sequences.

[0153] 4.4.1 Prediction Model and Optimization Problem

[0154] The hull kinematic model is described as follows:

[0155] Establish a ship nonlinear kinematics prediction model:

[0156]

[0157] Where x and y are position coordinates (in meters), θ is the heading angle (in radians), v is the linear velocity (in meters per second), δ is the rudder angle (in radians), and L is the wheelbase (in meters); Euler discretization is used (during sampling). =0.1 seconds) used to predict the future Step state.

[0158] Simultaneously construct the rolling NMPC time-domain optimization objective function:

[0159]

[0160] Where Q, R, and P are the weight matrices for state tracking, control energy consumption, and terminal constraint, respectively, and are adaptively adjusted according to the working mode: shallow water stable zone setting. =20 steps =5 steps, high-position tracking weight (Q main diagonal element = 50); deep mud zone settings =15 steps =3 steps, high control smoothing weight (R main diagonal element = 20), reducing the rudder angle change rate limit to 10° / s.

[0161] Multiple constraints include:

[0162] 1. Rudder angle amplitude:

[0163] 2. Rudder angle change rate: (Deep mud zone descends to) )

[0164] 3. Speed ​​range: Adjusted according to the mode, shallow water area Deep mud zone

[0165] 4. Obstacle avoidance distance: meters

[0166] 4.4.2 Solving Sequence Quadratic Programming and Rolling Optimization

[0167] The Sequential Quadratic Programming (SQP) algorithm is used to solve the aforementioned nonlinear constrained optimization problem. SQP transforms the original problem into a quadratic programming (QP) subproblem by iteratively linearizing the state equations and constraints near the current control sequence through a first-order Taylor expansion. The optimal control sequence is obtained by iteratively updating the control sequence until convergence.

[0168]

[0169] A warm-start strategy is adopted, using the optimal solution from the previous cycle as the initial value, which significantly reduces the number of iterations. The maximum number of iterations is set to 5-10 to ensure that the solution time for a single iteration is controlled within 30-50ms, meeting the real-time requirement of a 100ms control cycle.

[0170] Implement a rolling time-domain strategy: execute only the first control variable in each control cycle. The desired speed and rudder angle are sent to the multimodal PID controller; in the next cycle, the controller is re-optimized based on the latest state to achieve closed-loop feedback control.

[0171] 4.4.3 Three-layer collaborative control architecture

[0172] NMPC forms a three-tier collaborative architecture with the existing control system:

[0173] • First layer (pure tracking algorithm): Generates a reference trajectory sequence based on the global path and current position. ... It provides global navigation information.

[0174] • Second layer (NMPC optimization): Based on the reference trajectory and prediction model, solve for the optimal control sequence under multiple constraints, and output the desired velocity. and expected rudder .

[0175] • Third layer (multimodal PID): The desired value output by NMPC is used as the setpoint for low-level tracking control, compensating for model errors and external disturbances, and outputting motor PWM and servo control signals.

[0176] The advantage of this architecture lies in its layered decoupling and complementary strengths. To ensure robustness, a degraded control strategy is designed: when the NMPC solver times out (>70ms) or fails, the system automatically bypasses the NMPC layer and degrades to a pure tracking + PID mode to ensure uninterrupted control.

[0177] 4.4.4 Implementation and Performance Verification

[0178] The NMPC algorithm is implemented in Simulink using the MATLAB Function module, and the fmincon function from the optimization toolbox (configured in SQP mode) is called to solve it. For STM32 deployment, the Simulink Coder is used to generate efficient C code, and real-time performance is ensured through sparse matrix optimization and warm-start techniques.

[0179] Hardware-in-the-loop test results show that, compared to the traditional pure tracking + fixed PID method: path tracking error is reduced by approximately 50% (from 0.68m to 0.34m); the number of constraint violations is reduced by approximately 85%; and the standard deviation of the rudder angle change rate is reduced by approximately 60% (from...). Down to Especially in complex scenarios such as sharp turns and dense obstacles, NMPC's predictive and constraint processing advantages are significant, systematically improving the automated operation performance and safety of the boat-type lotus root harvester.

[0180] 4.5 Explanation and Physical Meaning of Other Symbols

[0181] For ease of understanding, the physical meanings of the main symbols in this invention are explained uniformly below:

[0182] 1. Error amount : No. The control error at any given time, in units that depend on the specific controlled object. Path tracking error The increment of the control quantity, the unit depends on the specific controlled object.

[0183] EKF algorithm related: State vector (bold italics indicate vector) The state estimation error covariance matrix (in bold italics) is an intermediate variable. Kalman gain matrix (in bold italics) is an intermediate variable; Process noise covariance matrix (bolded italics indicate the matrix); : Observation noise covariance matrix; The state transition Jacobian matrix (in bold italics) is an intermediate variable. The observed Jacobian matrix (in bold italics) is an intermediate variable. Process noise vector; Observation noise vector (bold italics indicate the vector)

[0184] Other intermediate variables: : Saturation function (mathematical operator, dimensionless); Path error adjustment coefficient (dimensionless); : Modal-dependent rudder angle adjustment coefficient (dimensionless); Steering angle gain (dimensionless); Shallow water effect compensation coefficient (dimensionless).

[0185] 4.6 Construction of a Hardware-in-the-Loop Verification Platform Based on Simulink

[0186] Hardware-in-the-loop verification is a key innovation of this invention. By building a digital twin model of a lotus root harvester in Simulink and communicating with a real STM32 controller in real time, rapid verification and optimization of the control algorithm are achieved. The construction of the hardware-in-the-loop system includes multiple stages such as digital twin model building, communication interface design, and real-time simulation configuration.

[0187] The environmental interaction model simulates the impact of the complex environment of lotus ponds on vehicle motion. Mud depth affects the rolling resistance coefficient; the greater the depth, the greater the resistance. This relationship was obtained through experimental fitting and implemented in the model using tables or polynomial functions. Water level affects buoyancy; when the water level is high, the effective weight of the vehicle decreases, and the ground contact pressure decreases. The ground adhesion coefficient affects the friction between the tires and the ground. Slippage is determined in the model by comparing the driving force with the maximum static friction force. Slippage occurs when the driving force exceeds the maximum static friction force; the actual driving force is calculated based on kinetic friction.

[0188] The sensor models simulate the characteristics of real sensors, including measurement noise, delay, and quantization error. GPS module errors include random and systematic errors. Random errors are simulated using Gaussian white noise, while systematic errors, related to satellite geometry, can be simplified to slowly varying biases. In Simulink, the GPS model is implemented by superimposing Band-Limited White Noise and Transport Delay onto the actual location. The IMU model needs to consider zero drift, scale factor error, and installation error. Zero drift is described using a random walk model, superimposed with integrated white noise on the actual angular velocity. Encoder errors mainly arise from quantization and installation deviations, simulated using uniformly distributed noise. By establishing these sensor models, hardware-in-the-loop simulation can realistically reflect the imperfections of the sensors and test the robustness of the control algorithm.

[0189] The design of the communication interface is a key technology for achieving hardware-in-the-loop (HIL) implementation. This invention uses serial communication to realize data interaction between Simulink and STM32. To ensure real-time performance and reliability, data packets adopt a fixed-length structure to avoid complex parsing processes. To address communication latency and data loss, a timestamp mechanism is introduced, with each data packet carrying a timestamp. The receiving end can determine the freshness of the data and discard outdated data. Simultaneously, a timeout protection is implemented; if no data is received for a certain period, the controller automatically enters a safe mode, stopping output or retaining the previous control command.

[0190] Real-time simulation configuration is crucial for ensuring hardware-in-the-loop performance. Simulink models need to be configured in Real-Time mode, using the Pacing module to synchronize simulation speed with real-time, preventing simulations from being too fast or too slow. The solver should be the fixed-step ode4 (Runge-Kutta) algorithm, with the step size set to match the control cycle (e.g., 0.01s), ensuring one step is simulated per control cycle. To reduce computational latency, complex algebraic loops should be avoided in the model, and explicit state-space representation should be used whenever possible. For computationally intensive modules (such as EKF), consider reducing their execution frequency or using C code generation for acceleration.

[0191] Parameter optimization is another important application of hardware-in-the-loop platforms. Control algorithms contain a large number of parameters, such as the proportional, integral, and derivative coefficients of PID controllers, the look-forward distance coefficient for pure tracking, and the noise covariance of EKF. The proper setting of these parameters is crucial to control performance. Traditional manual parameter tuning methods are not only time-consuming and laborious, but also struggle to find the globally optimal solution. This invention achieves automatic parameter optimization based on intelligent optimization algorithms, primarily employing genetic algorithms and particle swarm optimization. The genetic algorithm simulates the natural evolutionary process, searching for the optimal solution in the parameter space through operations such as selection, crossover, and mutation.

[0192] In the Simulink environment, the following modules are used to implement this multi-sensor fusion and closed-loop control system:

[0193] Sensor simulation module: The Band-Limited White Noise and Transport Delay modules are used to simulate the noise and delay characteristics of each sensor.

[0194] EKF fusion module: Implements the extended Kalman filter algorithm using MATLAB Functions, and uses persistent variables to store state estimates and covariance matrices.

[0195] Multi-layer control module: Each controller is encapsulated as an independent subsystem, and data conversion between different sampling rates is achieved through the Rate Transition module.

[0196] Communication interface module: Real-time communication with STM32 hardware is achieved using the Serial Configuration, Serial Send, and Serial Receive modules.

[0197] The technological innovations and patent protection focus of this invention are mainly concentrated on three core aspects: adaptive motion control algorithm, multi-sensor fusion technology, and hardware-in-the-loop verification and optimization. The organic combination of these key technologies constitutes a complete technology protection system.

[0198] The primary protection point is the multimodal PID adaptive control algorithm and its implementation mechanism. This is the core technological innovation of this invention, proposing for the first time in the field of lotus root harvesters a multimodal parameter adaptive strategy based on working condition identification. Specific protection includes a working mode identification algorithm based on multi-sensor information. This algorithm integrates multiple characteristic parameters such as encoder slip ratio, IMU acceleration variance, and motor current ratio to establish a criterion set for working condition identification, accurately distinguishing different operating environments such as shallow water stable zones, deep mud zones, and hard-bottom bumpy zones. Multiple sets of PID parameter libraries and their optimization methods are designed for different working conditions. Each set of parameters is specifically tuned for a particular working condition, and the parameter values ​​are determined based on a large amount of test data from hardware-in-the-loop simulation. The parameter smooth switching strategy is a key focus of the technical protection. A first-order inertial element is used to achieve a gradual transition of parameters from old to new values. The switching time constant is adaptively adjusted according to modal differences, ensuring both control continuity and rapid environmental adaptation. The specific implementation of the incremental PID algorithm combined with integral separation technology effectively avoids integral saturation, improving the system's response speed and control accuracy. This multimodal adaptive control architecture organically integrates environmental perception, modality recognition, parameter adjustment, and control execution, forming a complete adaptive control closed loop.

[0199] The second protection point is the improvement of the pure tracking algorithm and its coordination mechanism with speed control. While the pure tracking algorithm has applications in mobile robotics, the improvement made for the specific needs of the lotus root harvester is the innovation of this invention. The key technical protections include the speed-adaptive forward-looking distance strategy, and the forward-looking distance L... d =k*v+L d _min, through the velocity coefficient k and the minimum forward sight distance L dThe reasonable setting of _min ensures optimal tracking performance at different speeds. Robust implementation methods for coordinate transformation and steering angle calculation, especially strategies for handling special cases such as the target point being behind the vehicle or too close, guarantee the algorithm's reliability in various scenarios. Steering angle amplitude and rate-of-change constraint mechanisms, based on the vehicle's physical limitations, constrain the amplitude and rate of change of the steering angle, avoiding unrealizable control commands. The coordination strategy between the pure tracking algorithm and multimodal PID control is the core of the technical protection. The pure tracking algorithm outputs the steering angle, solving the "which way to go" problem, while the multimodal PID controller adjusts the speed, solving the "how fast to go" problem. The two work together organically through a coordinator module. The coordinator comprehensively determines the speed adjustment coefficient based on information such as path tracking error, forward curvature, and operating mode, achieving optimal matching between steering and speed. This algorithm architecture fully leverages the advantages of the pure tracking algorithm in path tracking while maintaining the adaptive capability of the multimodal PID in speed control.

[0200] The third protection point is the multi-sensor fusion algorithm and multi-layer closed-loop control architecture based on EKF. Multi-sensor fusion is the foundation for improving control accuracy and reliability. The innovation of this invention lies in the design of a specialized fusion algorithm and closed-loop architecture tailored to the characteristics of lotus root harvesters. Key aspects of the technical protection include the establishment of a state-space model, where the state vector is defined as position, heading angle, and velocity; the state equation is established based on vehicle kinematics; and the observation equation correlates the sensor measurements with the state variables. The specific implementation of the EKF algorithm includes recursive formulas for time and measurement updates, the calculation method for the Jacobian matrix, and the initialization and update strategy for the covariance matrix. An outlier detection and sensor reliability assessment mechanisms are implemented by analyzing the deviation between measured and predicted values ​​to identify sensor faults or abnormal data and dynamically adjust the weights of each sensor in the fusion process. The design of the three-layer nested closed-loop control architecture involves an outer position loop based on the fused position state, outputting the desired speed and steering angle through a pure tracking algorithm; a middle speed loop based on the fused speed state, outputting the desired current through a multi-modal PID controller; and an inner current loop based on a current sensor to rapidly adjust the PWM duty cycle. The control cycle design and data rate conversion mechanism of each closed loop layer achieve effective coordination between layers by rationally configuring the sampling period of each layer. The application strategy of multi-sensor information in each closed loop not only uses the fused main state information, but also introduces auxiliary information such as IMU attitude and motor current as feedforward or compensation, fully exploring the value of the sensors.

[0201] The fourth protection point is the hardware-in-the-loop verification platform and automatic parameter optimization method based on Simulink. Hardware-in-the-loop technology is a key innovation of this invention, providing a powerful tool for the rapid development and optimization of control algorithms. The focus of technical protection lies in the method of establishing the digital twin model, including the vehicle dynamics model, considering parameters such as mass, moment of inertia, wheelbase, and track width, and establishing the equation of motion based on Newton's second law; the environmental interaction model, simulating the impact of environmental factors such as mud resistance, ground adhesion, and slippage on vehicle motion; and the sensor model, simulating the real-world characteristics of sensors such as GPS, IMU, and encoders, including noise, delay, and quantization. The design of the Simulink-STM32 communication interface includes the definition of the serial communication protocol, the structure design of the data packets, measures to ensure real-time performance and reliability, and timestamp and timeout protection mechanisms. The configuration method for real-time simulation includes setting the Real-Time mode, using Pacing control to synchronize simulation with real time, selecting and setting the fixed-step solver, avoiding algebraic loops, and optimizing computational efficiency. The automatic parameter optimization strategy based on intelligent optimization algorithms utilizes genetic algorithms and particle swarm optimization to search for the optimal configuration in the parameter space, optimizes the design of the objective function, and comprehensively considers multiple performance indicators such as path tracking accuracy, response speed, and control smoothness. The application of parallel computing technology accelerates the optimization process and shortens the development cycle. The hardware-in-the-loop platform not only enables the safe verification of the control algorithm but also supports testing under various extreme conditions and fault scenarios, greatly improving the robustness of the algorithm.

[0202] The fifth protection point is the specific implementation technology of the control algorithm on the Simulink platform. The Simulink-based implementation is a distinctive feature of this invention, involving numerous engineering implementation details. Technical protection includes the MATLAB function implementation for modal recognition, receiving multi-sensor inputs, determining characteristics such as slip ratio, acceleration variance, and current ratio by setting thresholds, and outputting a working mode identifier. The pure tracking algorithm function implementation includes a coordinate transformation function to convert the target point from the world coordinate system to the vehicle coordinate system, a forward look-ahead calculation function to search for the target point on the path based on the forward look-ahead distance, and a steering angle calculation function, implemented based on geometric derivation formulas. The recursive implementation of the EKF filter uses persistent variables to maintain the state estimate and covariance matrix, performing prediction and update steps in each control cycle, and employing efficient algorithms for matrix operations to avoid inversion calculations. A modular design of multi-layer closed loops is used, with each control loop independently encapsulated as a subsystem. Data rate conversion is achieved through Rate Transition, and signal routing uses a Bus bus to improve readability. The implementation of smooth parameter switching uses a Memory module to store historical parameter values, achieving gradual transition through weighted averaging, with the transition coefficient dynamically adjusted according to modal differences. These implementation techniques not only ensure the correctness of the algorithm's functionality, but also optimize computational efficiency and code maintainability, laying the foundation for the engineering application of the algorithm.

[0203] These key technologies support and collaborate with each other, forming a complete and implementable intelligent motion control system. Each protection point is designed based on the actual capabilities of the Simulink platform and the application requirements of the lotus root harvester, ensuring both technological advancement and feasibility. Patent protection of these core technologies not only safeguards the technological advantages of this invention but also provides a feasible technical path and industrial application foundation for the intelligent development of agricultural machinery motion control. The technical system of this invention has strong versatility and scalability, applicable not only to lotus root harvesters but also to other types of agricultural machinery and mobile robots, making a significant contribution to the technological advancement of the entire intelligent agricultural machinery industry.

[0204] The above embodiments should be understood as illustrative only and not as limiting the scope of protection of the present invention. After reading the description of the present invention, those skilled in the art can make various alterations or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A multi-sensor fusion adaptive motion control system for a boat-type lotus root harvester, characterized in that, include: A multi-sensor array is used to acquire the position, attitude, speed, tilt angle and motor load information of the lotus root harvester. The multi-sensor array includes GPS, IMU, current sensor and encoder. The microcontroller is used to run the Extended Kalman Filter (EKF) data fusion algorithm, the multimodal PID adaptive control algorithm, the pure tracking path planning algorithm, and the nonlinear model predictive control (NMPC) algorithm. The NMPC algorithm is based on the ship's nonlinear kinematic model and performs roll optimization in the prediction time domain. It comprehensively considers multiple constraints, including rudder angle physical limitations, rudder angle change rate limitations, speed constraints, and obstacle avoidance, to generate the optimal control command that satisfies the constraints. The actuator, including a walking motor and a servo motor, is used to drive the lotus root harvester to move in response to the control command; The Simulink hardware-in-the-loop verification system includes a digital twin model, an environmental interaction model, a sensor simulation model, and an interface for real-time communication with the microcontroller. It is used for simulation based on state estimation and control commands to optimize the multimodal PID adaptive control algorithm and the pure tracking path planning algorithm. The multimodal PID adaptive control algorithm includes a strategy for dynamically adjusting the forward look-ahead distance Ld, which is expressed by the formula: ； Calculate the forward sight distance, where v is the current speed of the lotus root harvester. For speed coefficient, For minimum forward sight distance, The inertia coefficient, Let be the moment of inertia of the ship's hull about the z-axis. The wetted surface area, The density of water is dynamically adjusted based on the speed and steering performance of the lotus root harvester.

2. The multi-sensor fusion adaptive motion control system for the boat-type lotus root harvester according to claim 1, characterized in that, It also includes a boundary constraint handling mechanism, which uses a virtual force field method to generate a repulsive force F when the lotus harvester approaches the boundary of the lotus field or an obstacle. rep This is converted into an additional desired heading angle correction Δθ, guiding the lotus root harvester away from the danger zone.

3. The multi-sensor fusion adaptive motion control system for the boat-type lotus root harvester according to claim 2, characterized in that, It also includes a shallow water effect compensation algorithm, which increases the forward sight distance L when the ratio of water depth h to the ship's draft T is less than 1.5, indicating that the ship is in shallow water. d And reduce the steering angle gain k_δ.

4. The multi-sensor fusion adaptive motion control system for the boat-type lotus root harvester according to claim 3, characterized in that, It also includes a thrust constraint optimization method, which uses a saturation function (Saturation) to limit the calculated thrust to the maximum thrust of the thruster. Within the range, if the constrained thrust cannot satisfy the desired motion, then the objective function is minimized: ； The constraints are: ; It is the maximum output thrust of the thruster; in For the desired longitudinal force, For the desired steering torque, For the pitch of the thrusters, and These represent the thrust of the left and right thrusters, respectively.

5. The multi-sensor fusion adaptive motion control system for the boat-type lotus root harvester according to claim 4, characterized in that, It also includes a rudder angle limiting and rate of change limiting mechanism, which constrains the steering angle θ to be within the range of [-30°, 30°], and the rate of change of rudder angle |Δθ(t) / Δt| for two consecutive control cycles does not exceed 15° / s.

6. The multi-sensor fusion adaptive motion control system for the boat-type lotus root harvester according to claim 1, characterized in that, The EKF data fusion algorithm includes: State vector: ; Where (x, y) represents the position, For heading angle, For speed; State prediction equation: ; ； Measurement update equation: ; ； Where F and H are the Jacobian matrices of the state equation and observation equation with respect to the state, respectively; Q is the process noise covariance matrix; R is the observation noise covariance matrix; K is the Kalman gain matrix; Z(k) is the sensor observation value; P is the state estimation error covariance matrix; X(k|k-1) is the state prediction value at time k-1 to time k; f is the state transition function; and u(k) is the control input vector at time k. The values ​​of Q and R are adaptively adjusted according to the actual working conditions to improve the accuracy of state estimation and the fusion efficiency of the sensor data.

7. A motion control method for a lotus root harvester using the system described in any one of claims 1-6, characterized in that, Includes the following steps: Step 1: Apply the Extended Kalman Filter (EKF) data fusion algorithm to fuse the GPS, IMU, and encoder data to obtain the state estimate of the lotus root harvester; Step 2: Implement the working mode recognition algorithm. Calculate the speed slip ratio using the encoder, the acceleration variance using the IMU, and the ratio of motor current to rated current using the current sensor. Dynamically adjust the parameters of the PID controller to adapt to the current state of the lotus root harvester. Step 3: Based on the current position and target path, use a pure tracking path planning algorithm to calculate the desired steering angle and thrust distribution, and generate control commands; Step 4: Real-time hardware-in-the-loop simulation verification with the microcontroller is achieved through the digital twin model in the Simulink environment, including real-time data interaction and automatic optimization of the algorithm parameters based on the simulation data.

8. The motion control method for a lotus root harvester according to claim 7, characterized in that, The working mode recognition algorithm specifically includes: In Simulink, the modal recognition algorithm is implemented through the Function module. The function receives multiple input signals, including encoder speed, IMU acceleration, and motor current, and first calculates the speed slip ratio. : ； in This represents the wheel linear velocity measured by the encoder, while This indicates the actual forward speed of the vehicle based on the fusion of the positioning system and IMU. A slip ratio greater than 0.3 indicates a slipping state. Simultaneously, the variance of acceleration is monitored, and the standard deviation of acceleration over the most recent N sampling points is calculated. A bumpy state is identified when the standard deviation exceeds a preset threshold. (Acceleration variance...) Calculated as: ； The ratio of motor current to rated current reflects the load level. The formula is expressed as follows: ； in , These represent the motor current and the rated current, respectively. When the value is greater than 1.5, it is determined to be a heavy load state; based on the combination of these states, the system identifies the current working mode: shallow water stable zone, deep mud zone, hard bottom bumpy zone; A smooth switching strategy with gradual parameter changes is adopted. When a mode switch is detected, a first-order inertial element is used to achieve a smooth transition of parameters. The time constant is adaptively adjusted according to the severity of the mode switch. A memory module is used to store the parameter values ​​at the previous moment, and the parameter values ​​at the current moment are: ； Let k be the PID proportional coefficient at the current time k, and This indicates that the result is obtained by looking up a table based on the current identification working mode. The target value of the PID proportional coefficient, where α represents the smoothing coefficient. This represents the parameter value at the previous time step. Using an incremental PID algorithm, the output of the incremental PID... for: ； Where e(k) is the error at the current time, and e(k-1) and e(k-2) are the errors at the previous two time points; The proportional gain in a PID controller represents the proportional effect of error changes on the control input. For PID integral coefficients, These are the PID differential coefficients.

9. The motion control method for a lotus root harvester according to claim 7, characterized in that, It also includes a nonlinear model predictive control (NMPC) layer, used for predictive optimization control based on multimodal PID control and pure tracking algorithms, specifically including: Establish a ship nonlinear kinematics prediction model: ； Where x and y are position coordinates, θ is the heading angle, v is the linear velocity, δ is the rudder angle, and L is the wheelbase; simultaneously, a rolling time-domain optimization objective function is constructed: ； in To predict the number of time-domain steps, To control the number of time-domain steps, for Time prediction The state vector at time step Xref is the reference trajectory state, U(k+ik) is the control vector planned at time step k+i, and Q, R, and P are weight matrices. Describe the Euclidean norm; and set multiple constraints; 1) Rudder angle amplitude constraint: δmin ≤ δ(k+i) ≤ δmax, where δmin = -30°, δmax = 30°; 2) Rudder angle change rate constraint: ,in ; 3) Velocity constraint: vmin ≤ v (k+i) ≤ vmax, dynamically adjusted according to the working mode; 4) Obstacle avoidance safety distance constraint: d (X (k+ik), Obstacle) ≥ dsafe, where dsafe is the safety distance threshold; 5) The Sequential Quadratic Programming (SQP) algorithm is used to solve the constrained optimization problem, and the optimal control sequence U* = [u (k), u (k+1), ..., u (k+Nc-1)] is obtained; 6) Implement a rolling time-domain control strategy, which only executes the optimal control quantity u(k) at the current moment, and re-optimizes and solves the solution in the next control cycle to achieve closed-loop feedback control.

Images