STM32-based autonomous underwater robot intelligent attitude control system and method

Abstract

The invention provides an STM32-based autonomous underwater robot intelligent attitude control system and method, and relates to the technical field of underwater robot control, and the system comprises a sensor integration module which is used for collecting robot state data and environment data, a model construction module which is used for outputting a state estimation value and a disturbance force prediction value, and a control module which is used for controlling the state estimation value and the disturbance force prediction value. The fuzzy control module is used for determining the PID parameter adjustment amount according to the target attitude and the state estimation value, performing feed-forward compensation in combination with the disturbance force prediction value, and generating an attitude control instruction; the electric quantity monitoring module is used for sending a charging trigger signal, the fuzzy control module is further used for controlling the underwater robot to be in a charging posture according to the charging trigger signal, and the charging module is used for charging the power supply module when the wave energy power generation device reaches the optimal power generation position and sending a re-navigation signal to the fuzzy control module after charging is completed. According to the invention, the problems of low attitude control precision and low reliability of the underwater robot in the prior art are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robot control, and in particular to an STM32-based autonomous underwater robot intelligent posture control system and method. Background Art

[0002] Underwater robots are increasingly used for underwater environmental monitoring, river silt removal, and marine resource exploration. Currently, the main method for controlling the attitude of underwater robots is still based on traditional PID. Sensors such as gyroscopes and accelerometers are used to understand the robot's attitude, and thrusters are used to adjust the robot's attitude.

[0003] The power supply methods for underwater robots mainly include wire power supply and battery power supply. The current power supply methods often cannot guarantee the long-term operation of underwater robots.

[0004] With increasing attention paid to the ocean, the need for underwater tasks is increasing. Embedded control systems are gradually becoming mainstream due to their compact size, low power consumption, and low cost. At the same time, new energy technologies such as wave power generation and solar charging offer new hope for solving the endurance problem of underwater robots. Furthermore, continuous advancements in intelligent control algorithms and sensor fusion technology are enabling underwater robots to achieve more precise posture control in complex water environments.

[0005] However, the current underwater robot technology still has the following problems:

[0006] In areas with fast-flowing water, existing attitude control methods are unstable and have low control accuracy; the lack of effective energy management methods prevents underwater robots from working autonomously for long periods of time, resulting in low reliability. Summary of the Invention

[0007] In order to overcome the deficiencies of the prior art, the purpose of the present invention is to provide an autonomous underwater robot intelligent attitude control system and method based on STM32, which solves the problems of low accuracy and low reliability of underwater robot attitude control in the prior art.

[0008] To achieve the above object, the present invention provides the following solutions:

[0009] An intelligent attitude control system for an autonomous underwater robot based on STM32, comprising:

[0010] A power supply module and a sensor integration module, a model building module, a charging module, a fuzzy control module and a power monitoring module, all of which are connected to the power supply module;

[0011] The sensor integration module is used to collect robot state data and its environmental data; the model construction module is used to respectively construct a kinematic model and a disturbance force model according to the state data and its environmental data and output a state estimation value and a disturbance force prediction value through an extended Kalman filter; the fuzzy control module is used to calculate the posture error and the error change rate according to the target posture and the state estimation value and determine the PID parameter adjustment amount through three-layer fuzzy reasoning, and perform feedforward compensation in combination with the disturbance force prediction value to generate a posture control instruction; the power monitoring module is used to monitor the remaining battery power and the remaining heading to obtain a monitoring result, and send a charging trigger signal to the fuzzy control module and the charging module when the monitoring result reaches a threshold; the fuzzy control module is also used to control the underwater robot to a charging posture according to the charging trigger signal; the charging module is used to charge the power supply module when the wave energy power generation device reaches the optimal power generation position, and send a resumption signal to the fuzzy control module after charging is completed.

[0012] Preferably, the state data includes: attitude angle data, angular velocity data and depth information data; the environmental data includes water flow velocity data, battery voltage data and battery current data.

[0013] Preferably, the sensor integration module includes:

[0014] Nine-axis attitude sensor, used to output three-axis acceleration, three-axis angular velocity and three-axis magnetic field data;

[0015] Pressure sensor, used to obtain real-time depth information of underwater robots;

[0016] Water flow velocity sensor, used to obtain surrounding water flow velocity data;

[0017] The power monitoring sensor is used to detect battery voltage data and battery current data.

[0018] Preferably, the model building module includes:

[0019] A dynamic model building unit is used to build a kinematic model of the underwater robot based on the six-degree-of-freedom rigid body dynamics equation and the robot state data;

[0020] a disturbance force model building unit, configured to calculate the water flow disturbance force according to the water flow velocity data to build a disturbance torque model;

[0021] A model parameter updating unit, configured to update the damping coefficient and additional mass parameter in the dynamic model and the disturbance force model in real time;

[0022] A state fusion estimation unit is used to perform extended Kalman filtering fusion on the dynamic model prediction result with the current attitude angle data, angular velocity data and depth information data, and output a state estimation value;

[0023] A disturbance prediction unit, configured to calculate a disturbance force prediction value based on the disturbance force model and real-time water flow velocity data;

[0024] The expression of the kinematic model is:

[0025]

[0026] The expression of the disturbance torque model is:

[0027]

[0028] The calculation expression of the state estimate is:

[0029]

[0030] The calculation expression of the disturbance force prediction value is:

[0031]

[0032] Among them, M is the six-degree-of-freedom mass inertia matrix; v is the six-dimensional velocity vector composed of the underwater robot's linear velocity and angular velocity, is the time derivative of the six-dimensional velocity vector; C(v) is the Coriolis and centrifugal term matrix; D1 and D2 are the linear and quadratic damping matrices, respectively; g(η) is the gravity-buoyancy vector, η is the position-attitude vector; τ is the control force / torque generated by the thruster and the rudder; τ d and are the actual disturbance moment and the next predicted disturbance moment respectively; ρ is the water density; C d is the dimensionless resistance coefficient; A is the equivalent flow area matrix; v w 、v w,k is the water velocity vector between the current and k-th step; and are the prediction state and correction state of the extended Karl explicit filter respectively; P k∣k-1 is the prediction covariance matrix; H k is the observation Jacobian matrix; R is the observation noise covariance matrix; z k is the sensor observation vector at step k; h(·) is the observation model; is the body velocity estimate output by EKF; is the relative water velocity estimation vector;

[0033] The expression of the observation model is:

[0034]

[0035] Among them, q0, q1, q2, q3 are the first, second, third and fourth components of the unit quaternion respectively; φ, θ, ψ are the roll angle, pitch angle and heading angle respectively; p, q, r are the first, second and third components of the angular velocity respectively; d is the depth, u, v, w are the first, second and third components of the linear velocity respectively.

[0036] Preferably, the fuzzy control module includes:

[0037] The error calculation unit is used to calculate the attitude error e and the error change rate e according to the target attitude and the state estimation value c ;

[0038] The three-layer fuzzy inference unit is used to compare e and e c After the membership function μ(e), μ(e c ) is fuzzified and N If-Then rules are used to obtain the rule activation strength ω i =μ i (e)μ i (e c ) and calculate the PID parameter adjustment amount through Sugeno type reasoning;

[0039]

[0040] Parameter defuzzification unit is used to convert ΔK p ,ΔK i ,ΔK d With the benchmark parameter K p0 ,K i0 ,K d0 The sum is used to obtain the real-time PID coefficients:

[0041] K p =K p0 +ΔK p ,K i =K i0 +ΔK i ,K d =K d0 +ΔK d ;

[0042] Feedforward compensation unit, used to generate compensation control value u according to the disturbance force prediction value ff ;

[0043] An instruction synthesis unit is used to calculate attitude control instructions according to the instruction synthesis formula and output them to the propulsion / steering gear actuator;

[0044] The expression of the instruction synthesis formula is:

[0045]

[0046] Where, e is the deviation between the target posture and the current posture; is the error change rate; K p0 ,K i0 ,K d0 is the initial PID coefficient of the system offline tuning; a i ,b i ,c i are the consequent constants of the i-th fuzzy rule for the proportional, integral, and differential links; μ i (e),μ i (e c ) is e, e c Membership degree on the fuzzy set of the antecedent of the i-th rule; ω i is the rule activation strength; is the disturbance force / torque vector output by the disturbance prediction unit; u ff is the feedforward compensation; u c (t) is the final attitude control command.

[0047] Preferably, the power monitoring module includes:

[0048] Voltage acquisition unit, used to collect battery terminal voltage in real time and complete analog-to-digital conversion;

[0049] Current acquisition unit, used to collect battery charge and discharge current in real time and complete analog-to-digital conversion;

[0050] The remaining power estimation unit is used to integrate the outputs of the voltage acquisition unit and the current acquisition unit and estimate the remaining battery power percentage using an improved Kalman filter algorithm;

[0051] A remaining flight time prediction unit is used to calculate the remaining time for the underwater robot to continue working based on the historical power consumption curve and the current power consumption rate;

[0052] A threshold determination unit is used to compare the remaining battery percentage and the remaining flight time with a preset threshold value and generate a charging trigger signal when either indicator is below the threshold value;

[0053] The communication interface unit is used to send the charging trigger signal to the fuzzy control module and the charging module respectively, and receive the resumption signal after the charging module completes charging and forward it to the fuzzy control module.

[0054] Preferably, the remaining power estimation unit includes:

[0055] The Coulomb integrator unit is used to perform time integration on the instantaneous current output by the current acquisition unit to obtain the battery power consumption;

[0056] An open-circuit voltage correction subunit is configured to measure the open-circuit voltage of the battery during a stationary or low-power consumption phase of the underwater robot, and correct the open-circuit voltage according to an electrochemical characteristic curve to obtain an open-circuit voltage correction amount;

[0057] A temperature compensation subunit is configured to collect temperature data of the water flow and determine a temperature compensation amount;

[0058] A health status evaluation subunit is configured to evaluate the aging degree of the battery by counting the number of charge and discharge cycles and detecting the change rate of the internal resistance to generate a capacity attenuation coefficient;

[0059] A fusion filtering subunit is configured to use an unscented Kalman filtering algorithm to fuse the Coulomb integral result, the open-circuit voltage correction amount, the temperature compensation amount, and the capacity attenuation coefficient, and output a percentage of the remaining battery capacity.

[0060] Preferably, the charging module comprises:

[0061] A wave energy capturing unit is configured to convert the periodic up-and-down motion of waves into rotational mechanical energy through a float-link mechanism;

[0062] A generator set unit is configured to receive the rotational mechanical energy and output alternating current energy;

[0063] A rectification and voltage stabilization unit is configured to rectify the alternating current energy into stable direct current and perform constant voltage control on the output voltage;

[0064] A maximum power point tracking unit is configured to adjust the working parameters of the rectification and voltage stabilization unit in real time to ensure that the wave energy capturing unit is in an optimal energy conversion state under different wave heights and frequencies;

[0065] A posture adjustment driving unit is configured to drive the underwater robot to adjust the posture according to the charging posture instructions sent by the fuzzy control module, so that the wave energy capturing unit maintains an optimal energy capturing angle and water surface height;

[0066] A charging completion determination unit is configured to generate a homeward-bound signal when it is detected that the battery capacity reaches a set threshold and the charging current drops to a safe range, and send the signal to the fuzzy control module through the communication interface unit.

[0067] Preferably, the fuzzy control module is an STM32F4 series microcontroller.

[0068] An intelligent posture control method for an autonomous underwater robot based on an STM32, comprising:

[0069] Collecting state data of the robot and its environment data;

[0070] Constructing a kinematic model and a disturbance force model according to the state data and its environment data, respectively, and outputting state estimation values and disturbance force prediction values through an extended Kalman filter;

[0071] Calculate the attitude error and error change rate based on the target attitude and the state estimation value, determine the PID parameter adjustment amount through three-layer fuzzy reasoning, and perform feedforward compensation in combination with the disturbance force prediction value to generate an attitude control instruction;

[0072] monitoring the remaining battery power and the remaining heading to obtain a monitoring result, and sending a charging trigger signal to the fuzzy control module and the charging module when the monitoring result reaches a threshold;

[0073] When the wave energy power generation device reaches the optimal power generation position, the power supply module is charged, and after charging is completed, a resumption signal is sent to the fuzzy control module.

[0074] The present invention discloses the following technical effects:

[0075] The present invention provides an STM32-based intelligent attitude control system and method for an autonomous underwater robot. The system includes: a power supply module and a sensor integration module, a model construction module, a charging module, a fuzzy control module, and a power monitoring module, all of which are connected to the power supply module; the sensor integration module is used to collect robot state data and environmental data thereof; the model construction module is used to respectively construct a kinematic model and a disturbance force model according to the state data and the environmental data thereof, and output a state estimation value and a disturbance force prediction value through an extended Kalman filter; the fuzzy control module is used to calculate an attitude error and an error change rate according to a target attitude and the state estimation value, determine a PID parameter adjustment amount through three-layer fuzzy reasoning, and perform feedforward compensation in combination with the disturbance force prediction value to generate an attitude control instruction; the power monitoring module is used to monitor the remaining battery power and the remaining heading to obtain a monitoring result, and send a charging trigger signal to the fuzzy control module and the charging module when the monitoring result reaches a threshold value; the fuzzy control module is further used to control the underwater robot to a charging posture according to the charging trigger signal; the charging module is used to charge the power supply module when a wave energy power generation device reaches an optimal power generation position, and send a resumption signal to the fuzzy control module after charging is completed. The present invention uses STM32 as the core and organically combines multi-sensor fusion, extended Kalman filtering, three-layer fuzzy-PID dynamic tuning and feedforward disturbance force compensation, so that the underwater robot can obtain more accurate and real-time attitude control in complex water flow environments. At the same time, the invention introduces a collaborative mechanism of power monitoring and wave energy self-charging, which can automatically switch to the charging attitude and complete energy replenishment when the remaining power is lower than the threshold, and seamlessly resume the task after charging is completed. Overall, it not only ensures the accuracy and robustness of attitude control, but also significantly improves the endurance and system autonomy. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0077] Figure 1 This is a schematic diagram of the structure of an autonomous underwater robot intelligent attitude control system based on STM32 provided in an embodiment of the present invention.

[0078] Reference numerals:

[0079] 1-power supply module, 2-sensor integration module, 3-model building module, 4-fuzzy control module, 5-charging module, 6-power monitoring module. DETAILED DESCRIPTION

[0080] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0081] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0082] like Figure 1 As shown, the present invention provides an autonomous underwater robot intelligent attitude control system based on STM32, comprising:

[0083] A power supply module 1 and a sensor integration module 2, a model building module 3, a charging module 5, a fuzzy control module 4 and a power monitoring module 6, all of which are connected to the power supply module 1;

[0084] The sensor integration module 2 is used to collect robot state data and its environmental data, the model construction module 3 is used to respectively construct a kinematic model and a disturbance force model according to the state data and its environmental data, and output a state estimation value and a disturbance force prediction value through an extended Kalman filter, the fuzzy control module 4 is used to calculate the posture error and the error change rate according to the target posture and the state estimation value, and determine the PID parameter adjustment amount through three-layer fuzzy reasoning, and perform feedforward compensation in combination with the disturbance force prediction value to generate a posture control instruction; the power monitoring module 6 is used to monitor the remaining battery power and the remaining heading to obtain a monitoring result, and send a charging trigger signal to the fuzzy control module 4 and the charging module 5 when the monitoring result reaches a threshold value, the fuzzy control module 4 is also used to control the underwater robot to a charging posture according to the charging trigger signal, and the charging module 5 is used to charge the power supply module 1 when the wave energy power generation device reaches the optimal power generation position, and send a resumption signal to the fuzzy control module 4 after charging is completed.

[0085] Furthermore, the state data includes: attitude angle data, angular velocity data and depth information data; the environmental data includes water flow velocity data, battery voltage data and battery current data.

[0086] Furthermore, the sensor integration module 2 includes:

[0087] Nine-axis attitude sensor, used to output three-axis acceleration, three-axis angular velocity and three-axis magnetic field data;

[0088] Pressure sensor, used to obtain real-time depth information of underwater robots;

[0089] Water flow velocity sensor, used to obtain surrounding water flow velocity data;

[0090] The power monitoring sensor is used to detect battery voltage data and battery current data.

[0091] Specifically, the sampling hardware configuration:

[0092] Attitude angle and angular velocity: The nine-axis inertial measurement unit is placed near the center of mass of the robot, and the gyroscope and accelerometer adopt a two-level range adaptive mode; the magnetometer is placed in a weak magnetic interference area, and the three-axis winding is evenly distributed to reduce hard iron distortion.

[0093] In-depth information: The high-resolution silicon piezoresistive pressure chip is connected to the STM32 via a four-wire SPI interface, and the on-chip temperature compensation coefficient is written into a read-only register.

[0094] Water flow velocity: The miniature acoustic Doppler flow measurement probe is arranged at the bow of the fuselage, with an angle of less than 5° to the velocity direction of the fuselage. The probe outputs the instantaneous flow velocity vector in a 4Hz fan-scan mode.

[0095] Voltage and current: The power monitoring chip directly outputs I after sampling 2 C digital quantity, avoiding the voltage drop error introduced by long-distance high-current wires.

[0096] Furthermore, the model building module 3 includes:

[0097] A dynamic model building unit is used to build a kinematic model of the underwater robot based on the six-degree-of-freedom rigid body dynamics equation and the robot state data;

[0098] a disturbance force model building unit, configured to calculate the water flow disturbance force according to the water flow velocity data to build a disturbance torque model;

[0099] A model parameter updating unit, configured to update the damping coefficient and the additional mass parameter in the dynamic model and the disturbance force model in real time;

[0100] Specifically, the initial baseline and parameter vector are constructed:

[0101] The benchmark damping coefficient and added mass matrix are obtained through still water towing tank experiments, and the parameter vectors are written in order according to the three categories of linear damping, quadratic damping, and added mass.

[0102] The unitization process is used to normalize the damping and mass elements of different dimensions to the floating point range of 0 to 1 to ensure the consistency of the dynamic range of STM32 floating point operations.

[0103] Real-time observation equation establishment:

[0104] Observables: The six-dimensional velocity and acceleration provided by the extended Kalman filter, together with the real-time thrust of the propeller and the rudder torque, form the observation equation input;

[0105] Regression vector: Based on the six-degree-of-freedom rigid body motion equation, the velocity and acceleration terms are rearranged to form a sparse regression vector, which corresponds to a one-to-one mapping relationship with the reference parameter vector;

[0106] Equation residual: "Dynamic prediction ability - actual execution ability" is used as the identification residual to directly reflect the degree of model inconsistency.

[0107] Adaptive recursive least squares identification:

[0108] A recursive least squares algorithm with an exponential forgetting factor is used. The forgetting factor is dynamically adjusted between 0.98 and 0.995 according to the intensity of the water disturbance, which improves the response speed to sudden changes in working conditions.

[0109] The covariance matrix is ​​stored in blocks in the STM32 on-chip SRAM, and DMA double buffering is used to improve the efficiency of matrix operations;

[0110] To avoid high-frequency noise amplification, a "gain limiter" is introduced: when the single-step update amount of any parameter exceeds 10% of its baseline value, the update step size is automatically halved and an abnormal flag is recorded.

[0111] Smooth update and model reconstruction:

[0112] The latest identification results are smoothed by a first-order low-pass filter to suppress high-frequency jitter;

[0113] The smoothed parameters are directly written into the damping matrix and additional mass matrix of the dynamic model and the disturbance force model, and then a "model refresh" interrupt is broadcast to the state fusion estimation unit to ensure that the new parameters are used in the next cycle prediction;

[0114] To prevent numerical explosion, upper and lower limits are set for each parameter: the damping coefficient is not less than 50% of the experimental baseline, and the additional mass is not higher than 150% of the theoretical mass.

[0115] All matrix operations call CMSIS-DSP single-precision library functions, which can complete a six-dimensional parameter update within 2ms at a 120MHz main frequency;

[0116] The update period is fixed at 50ms, which is a multiple of the EKF prediction step to ensure decoupling.

[0117] When the power monitoring module 6 enters the low power mode, the system automatically reduces the update cycle to 200ms to reduce the processor load.

[0118] A state fusion estimation unit is used to perform extended Kalman filtering fusion on the dynamic model prediction result with the current attitude angle data, angular velocity data and depth information data, and output a state estimation value;

[0119] A disturbance prediction unit, configured to calculate a disturbance force prediction value based on the disturbance force model and real-time water flow velocity data;

[0120] The expression of the kinematic model is:

[0121]

[0122] The expression of the disturbance torque model is:

[0123]

[0124] The calculation expression of the state estimate is:

[0125]

[0126] The calculation expression of the disturbance force prediction value is:

[0127]

[0128] Among them, M is the six-degree-of-freedom mass inertia matrix; v is the six-dimensional velocity vector composed of the underwater robot's linear velocity and angular velocity, is the time derivative of the six-dimensional velocity vector; C(v) is the Coriolis and centrifugal term matrix; D1 and D2 are the linear and quadratic damping matrices, respectively; g(η) is the gravity-buoyancy vector, η is the position-attitude vector; τ is the control force / torque generated by the thruster and the rudder; τ d and are the actual disturbance moment and the next predicted disturbance moment respectively; ρ is the water density; C d is the dimensionless resistance coefficient; A is the equivalent flow area matrix; v w 、v w,k is the water velocity vector between the current and k-th step; and are the prediction state and correction state of the extended Karl explicit filter respectively; P k∣k-1 is the prediction covariance matrix; H k is the observation Jacobian matrix; R is the observation noise covariance matrix; z k is the sensor observation vector at step k; h(·) is the observation model; is the body velocity estimate output by EKF; is the relative water velocity estimation vector;

[0129] The expression of the observation model is:

[0130]

[0131] Among them, q0, q1, q2, q3 are the first, second, third and fourth components of the unit quaternion respectively; φ, θ, ψ are the roll angle, pitch angle and heading angle respectively; p, q, r are the first, second and third components of the angular velocity respectively; d is the depth, u, v, w are the first, second and third components of the linear velocity respectively.

[0132] Furthermore, the fuzzy control module 4 includes:

[0133] The error calculation unit is used to calculate the attitude error e and the error change rate e according to the target attitude and the state estimation value c ;

[0134] The three-layer fuzzy inference unit is used to compare e and e c After the membership function μ(e), μ(e c ) is fuzzified and N If-Then rules are used to obtain the rule activation strength ω i =μ i (e)μ i (e c) and calculate the PID parameter adjustment amount through Sugeno type reasoning;

[0135]

[0136] Parameter defuzzification unit is used to convert ΔK p ,ΔK i ,ΔK d With the benchmark parameter K p0 ,K i0 ,K d0 The sum is used to obtain the real-time PID coefficients:

[0137] K p =K p0 +ΔK p ,K i =K i0 +ΔK i ,K d =K d0 +ΔK d ;

[0138] Feedforward compensation unit, used to generate compensation control value u according to the disturbance force prediction value ff ;

[0139] An instruction synthesis unit is used to calculate attitude control instructions according to the instruction synthesis formula and output them to the propulsion / steering gear actuator;

[0140] The expression of the instruction synthesis formula is:

[0141]

[0142] Where, e is the deviation between the target posture and the current posture; is the error change rate; K p0 ,K i0 ,K d0 is the initial PID coefficient of the system offline tuning; a i ,b i ,c i are the consequent constants of the i-th fuzzy rule for the proportional, integral, and differential links; μ i (e),μ i (e c ) is e, e c Membership degree on the fuzzy set of the antecedent of the i-th rule; ω i is the rule activation strength; is the disturbance force / torque vector output by the disturbance prediction unit; u ff is the feedforward compensation; u c (t) is the final attitude control command.

[0143] More specifically, to maintain a stable closed-loop 500Hz refresh rate despite wave and current disturbances, this implementation decomposes the fuzzy-PID parameter tuning process into a three-layer serial pipeline: membership degree self-scaling layer, rule activation matrix layer, and Sugeno linear consequence layer. Each layer incorporates dedicated anti-shake and real-time performance mechanisms. Missing any one of these layers will not guarantee real-time adjustability and numerical stability of the PID coefficients under significant operating conditions. Therefore, the following key steps are essential.

[0144] Membership self-scaling layer:

[0145] Input variables: attitude error e and error change rate ē, both limited to the interval [-e_sat, e_sat],

[0146] Dynamic scaling factors S_e, S_e: obtained by linear interpolation of the damping coefficient β output by the model parameter update unit and the additional mass μ, formula S_e=f1(β), The membership function is automatically contracted when the water flow rate increases to improve sensitivity.

[0147] Membership function: Adopt five symmetrical triangle sets NB, NS, ZO, PS, PB; base point coordinates are reloaded into LUT every 20ms through S_e, S_e.

[0148] On-chip implementation: STM32F4's FPU plus LUT direct lookup table, single fuzzification takes < 3μs.

[0149] Rule activation matrix layer:

[0150] Rule base size: M × N = 5 × 5 = 25 If-Then rules, forming a two-dimensional matrix with errors e as rows and error change rates e as columns;

[0151] Calculation of activation strength λij: Take the minimum value of the two input membership μe_i∧μe-j, and then send it to the "sparse threshold gate". When λij<0.05, it is considered as 0 and directly eliminated, which can reduce the average number of participating rules from 25 to ≤9

[0152] Parallel accumulation: Utilize the dual MAC instructions of the STM32F4 to implement 4-way unroll, and use DMA-PingPong to complete λij aggregation within one system beat, ensuring high-frequency response.

[0153] Sugeno linear post-layer:

[0154] Post-processing function: Similarly;

[0155] All coefficients a, b, and c are calibrated offline by the whole ship identification in the towing tank, stored in the on-chip Flash, and can be dynamically refreshed with firmware upgrades.

[0156] The numerator and denominator are accumulated synchronously:

[0157] Σ(λij·ΔKPij),Σλij…→ΔKP;

[0158] The other two links are isomorphic, with a set quantitative resolution of 2×10 -3 .

[0159] Explosion-proof mechanism: If Σλij<0.2 (extremely low activation), maintain the previous cycle ΔK * The "low credibility" flag is set; the fuzzy control module 4 maintains saturation protection in this cycle.

[0160] Output cutoff: ΔKP, ΔKI, and ΔKD are smoothed by first-order Butterworth smoothing (fc = 10Hz) and then combined with the offline reference KO to synthesize the new PID coefficients. These coefficients are then written directly to the servo drive PWM registers via DMA. The entire Layer-3 calculation latency is ≤ 7μs.

[0161] Real-time and robustness guarantee:

[0162] Three-layer pipeline: FreeRTOS priority 3, 2, and 1 tasks are bound to the core for execution, with a maximum end-to-end latency of 32μs.

[0163] Abnormal hot switching: When the power monitoring module 6 triggers low power mode, Layer-1 / 2 switches to a simplified library with 3-level triangle membership and 9 rules, instantly reducing the amount of calculation by 60%;

[0164] Closed-loop verification: Compare the latest PID coefficients with the expected convergence bandwidth every 10 seconds. If the deviation is greater than 25%, "parameter freeze" is automatically enabled, and the model parameter update unit is recalibrated to recalibrate the damping before resuming automatic tuning.

[0165] Through the above-mentioned "three-layer pipeline + double protection" mechanism, this implementation method can adaptively complete a full-range adjustment of the PID coefficient within 20ms within the water flow velocity range of 0-1.5m / s and the attitude disturbance range of ±30°, ensuring that the control loop phase margin is ≥45° and the overshoot rate is ≤8%, significantly improving the attitude stability of the autonomous underwater robot in complex flow fields.

[0166] Furthermore, the power monitoring module 6 includes:

[0167] Voltage acquisition unit, used to collect battery terminal voltage in real time and complete analog-to-digital conversion;

[0168] Current acquisition unit, used to collect battery charge and discharge current in real time and complete analog-to-digital conversion;

[0169] The remaining power estimation unit is used to integrate the outputs of the voltage acquisition unit and the current acquisition unit and estimate the remaining battery power percentage using an improved Kalman filter algorithm;

[0170] A remaining flight time prediction unit is used to calculate the remaining time for the underwater robot to continue working based on the historical power consumption curve and the current power consumption rate;

[0171] A threshold determination unit is used to compare the remaining battery percentage and the remaining flight time with a preset threshold value and generate a charging trigger signal when either indicator is below the threshold value;

[0172] The communication interface unit is used to send the charging trigger signal to the fuzzy control module 4 and the charging module 5 respectively, and receive the resumption signal after the charging module 5 completes charging and forward it to the fuzzy control module 4.

[0173] Furthermore, the remaining power estimation unit includes:

[0174] The Coulomb integrator unit is used to perform time integration on the instantaneous current output by the current acquisition unit to obtain the battery power consumption;

[0175] The open circuit voltage correction subunit is used to measure the open circuit voltage of the battery when the underwater robot is stationary or in a low power consumption stage, and to correct the open circuit voltage according to the electrochemical characteristic curve to obtain an open circuit voltage correction value;

[0176] The temperature compensation subunit is used to collect the temperature data of the water flow and determine the temperature compensation amount;

[0177] The health status assessment subunit is used to assess the battery aging by counting the number of charge and discharge cycles and detecting the internal resistance change rate to generate a capacity attenuation coefficient;

[0178] The fusion filter subunit is used to use the unscented Kalman filter algorithm to fuse the Coulomb integral result, the open circuit voltage correction amount, the temperature compensation amount and the capacity attenuation coefficient, and output the percentage of the remaining battery power.

[0179] Specifically, in this embodiment, the remaining battery life estimation unit runs as a separate thread on an STM32 microcontroller. Its workflow is as follows: First, the Coulomb integrator integrates the instantaneous current captured by the dual-channel Hall effect current sensor at a sampling frequency of one thousand times per second, accumulating the battery's power consumption in real time. When the robot detects that it has entered the constant-voltage charging phase, the integration result is immediately reset according to the battery's rated capacity and state-of-health coefficient to eliminate long-term drift. Subsequently, the open-circuit voltage correction unit, after determining the static condition of "the absolute value of the current is less than 0.05 current multipliers for two minutes," measures the battery terminal voltage and infers the state-of-charge value by combining the open-circuit voltage referenced at 25 degrees Celsius with the state-of-charge curve. Simultaneously, the temperature compensation unit calculates the cell node temperature using three temperature probes located in the center of the battery compartment, the inner wall of the hull, and the external water. The open-circuit voltage slope and Coulomb efficiency are then simultaneously corrected based on the temperature differential coefficient. The health assessment unit records the changes in terminal voltage and current at the moment of a sudden drop in thruster load, calculates the internal resistance online, and updates the battery health coefficient and cycle count using an autoregressive model. These four data points, along with the integral bias, serve as observational inputs to an unscented Kalman filter, which executes every 50 milliseconds. The filter completes the prediction-correction process in approximately 1.8 milliseconds, outputting the instantaneous state of charge. If the state of charge falls below 15% or the predicted remaining flight time is less than 30 minutes, the module sends a charge trigger signal via the bus to fuzzy control module 4. When the state of charge reaches 95% and the charging current drops below 0.05 current multiplier for five minutes, a resume signal is automatically issued and the integral counter is reset. This five-stage closed-loop system has been tested under ambient temperatures of 0 to 40 degrees Celsius, current velocities of 0 to 1.5 meters per second, and 500 complete charge and discharge cycles to ensure a maximum state of charge error of no more than 2.7%.

[0180] Furthermore, the charging module 5 includes:

[0181] a wave energy capture unit for converting the periodic up-and-down motion of waves into rotational mechanical energy through a buoy-linkage mechanism;

[0182] a generator unit, configured to receive the rotational mechanical energy and output AC electrical energy;

[0183] A rectifier and voltage stabilization unit, configured to rectify the AC power into stable DC power and perform constant voltage control on the output voltage;

[0184] The maximum power point tracking unit is used to adjust the working parameters of the rectifier and voltage regulator unit in real time to ensure that the wave energy capture unit is in the optimal energy conversion state under different wave heights and frequencies;

[0185] A posture adjustment driving unit is configured to drive the underwater robot to adjust the posture according to the charging posture instruction sent by the fuzzy control module 4, so that the wave energy capturing unit can keep the optimal energy capturing angle and water surface height.

[0186] A charging completion determination unit is configured to generate a homeward navigation signal when the battery power reaches a set threshold and the charging current falls within a safe range, and send the signal to the fuzzy control module 4 through the communication interface unit.

[0187] Specifically, the fuzzy control module 4 is an STM32F4 series microcontroller.

[0188] The embodiment also provides an intelligent posture control method for an autonomous underwater robot based on an STM32, which comprises the following steps:

[0189] Collecting state data and environment data of the robot;

[0190] Building a kinematic model and a disturbance force model according to the state data and the environment data, and outputting state estimation values and disturbance force prediction values through extended Kalman filtering;

[0191] Calculating posture error and error change rate according to a target posture and the state estimation values, determining a PID parameter adjustment amount through three-layer fuzzy reasoning, and combining the disturbance force prediction values to perform feedforward compensation, so as to generate a posture control instruction;

[0192] Obtaining a monitoring result by monitoring the remaining battery power and the remaining navigation time, and sending a charging trigger signal to the fuzzy control module and the charging module when the monitoring result reaches a threshold;

[0193] Charging the power supply module when the wave energy power generation device reaches the optimal power generation position, and sending a homeward navigation signal to the fuzzy control module after the charging is completed.

[0194] The embodiments in the specification are described in a progressive manner, and each embodiment focuses on the difference from other embodiments. The same or similar parts of each embodiment can be referred to each other.

[0195] The principles and implementation manners of the present application are described by using specific examples in the specification. The above description of the embodiments is only used to help understand the method and the core idea of the present application; meanwhile, for those skilled in the art, the specific implementation manners and application ranges can be changed according to the idea of the present application. In conclusion, the content of the specification should not be understood as a limitation of the present application.

Claims

1. An autonomous underwater robot intelligent attitude control system based on STM32, characterized in that: include: A power supply module and a sensor integration module, a model building module, a charging module, a fuzzy control module and a power monitoring module, all of which are connected to the power supply module; The sensor integration module is used to collect robot state data and its environmental data; the model construction module is used to respectively construct a kinematic model and a disturbance force model according to the state data and its environmental data and output a state estimation value and a disturbance force prediction value through an extended Kalman filter; the fuzzy control module is used to calculate the posture error and the error change rate according to the target posture and the state estimation value and determine the PID parameter adjustment amount through three-layer fuzzy reasoning, and perform feedforward compensation in combination with the disturbance force prediction value to generate a posture control instruction; the power monitoring module is used to monitor the remaining battery power and the remaining heading to obtain a monitoring result, and send a charging trigger signal to the fuzzy control module and the charging module when the monitoring result reaches a threshold; the fuzzy control module is also used to control the underwater robot to a charging posture according to the charging trigger signal; the charging module is used to charge the power supply module when the wave energy power generation device reaches the optimal power generation position, and send a resumption signal to the fuzzy control module after charging is completed.

2. The STM32-based autonomous underwater robot intelligent attitude control system according to claim 1, characterized in that: The state data includes: attitude angle data, angular velocity data and depth information data; the environmental data includes water flow velocity data, battery voltage data and battery current data.

3. The STM32-based autonomous underwater robot intelligent attitude control system according to claim 1 is characterized in that: The sensor integrated module includes: Nine-axis attitude sensor, used to output three-axis acceleration, three-axis angular velocity and three-axis magnetic field data; Pressure sensor, used to obtain real-time depth information of underwater robots; Water flow velocity sensor, used to obtain surrounding water flow velocity data; The power monitoring sensor is used to detect battery voltage data and battery current data.

4. The STM32-based autonomous underwater robot intelligent attitude control system according to claim 2, characterized in that: The model building module includes: A dynamic model building unit is used to build a kinematic model of the underwater robot based on the six-degree-of-freedom rigid body dynamics equation and the robot state data; a disturbance force model building unit, configured to calculate the water flow disturbance force according to the water flow velocity data to build a disturbance torque model; A model parameter updating unit, configured to update the damping coefficient and additional mass parameter in the dynamic model and the disturbance force model in real time; A state fusion estimation unit is used to perform extended Kalman filtering fusion on the dynamic model prediction result with the current attitude angle data, angular velocity data and depth information data, and output a state estimation value; A disturbance prediction unit, configured to calculate a disturbance force prediction value based on the disturbance force model and real-time water flow velocity data; The expression of the kinematic model is: The expression of the disturbance torque model is: The calculation expression of the state estimate is: The calculation expression of the disturbance force prediction value is: Among them, M is the six-degree-of-freedom mass inertia matrix; v is the six-dimensional velocity vector composed of the underwater robot's linear velocity and angular velocity, is the time derivative of the six-dimensional velocity vector; C(v) is the Coriolis and centrifugal term matrix; D1 and D2 are the linear and quadratic damping matrices, respectively; g(η) is the gravity-buoyancy vector, η is the position-attitude vector; τ is the control force / torque generated by the thruster and the rudder; τ d and are the actual disturbance moment and the next predicted disturbance moment respectively; ρ is the water density; C d is the dimensionless resistance coefficient; A is the equivalent flow area matrix; v w 、v w,k is the water velocity vector between the current and k-th step; and are the prediction state and correction state of the extended Karl explicit filter respectively; P k∣k-1 is the prediction covariance matrix; H k is the observation Jacobian matrix; R is the observation noise covariance matrix; z k is the sensor observation vector at step k; h(·) is the observation model; is the body velocity estimate output by EKF; is the relative water velocity estimation vector; The expression of the observation model is: Among them, q0, q1, q2, q3 are the first, second, third and fourth components of the unit quaternion respectively; φ, θ, ψ are the roll angle, pitch angle and heading angle respectively; p, q, r are the first, second and third components of the angular velocity respectively; d is the depth, u, v, w are the first, second and third components of the linear velocity respectively.

5. The STM32-based autonomous underwater robot intelligent attitude control system according to claim 1 is characterized in that: The fuzzy control module includes: The error calculation unit is used to calculate the attitude error e and the error change rate e according to the target attitude and the state estimation value c ; The three-layer fuzzy inference unit is used to compare e and e c After the membership function μ(e), μ(e c ) is fuzzified and N If-Then rules are used to obtain the rule activation strength ω i =μ i (e)μ i (e c ) and calculate the PID parameter adjustment amount through Sugeno type reasoning; Parameter defuzzification unit is used to convert ΔK p ,ΔK i ,ΔK d With the benchmark parameter K p0 ,K i0 ,K d0 The sum is used to obtain the real-time PID coefficients: K p =K p0 +ΔK p ,K i =K i0 +ΔK i ,K d =K d0 +ΔK d ; Feedforward compensation unit, used to generate compensation control value u according to the disturbance force prediction value ff ; An instruction synthesis unit is used to calculate attitude control instructions according to the instruction synthesis formula and output them to the propulsion / steering gear actuator; The expression of the instruction synthesis formula is: Where, e is the deviation between the target posture and the current posture; is the error change rate; K p0 ,K i0 ,K d0 is the initial PID coefficient of the system offline tuning; a i ,b i ,c i are the consequent constants of the i-th fuzzy rule for the proportional, integral, and differential links; μ i (e),μ i (e c ) is e, e c Membership degree on the fuzzy set of the antecedent of the i-th rule; ω i is the rule activation strength; is the disturbance force / torque vector output by the disturbance prediction unit; u ff is the feedforward compensation; u c (t) is the final attitude control command.

6. The STM32-based autonomous underwater robot intelligent attitude control system according to claim 1, characterized in that: The power monitoring module includes: Voltage acquisition unit, used to collect battery terminal voltage in real time and complete analog-to-digital conversion; Current acquisition unit, used to collect battery charge and discharge current in real time and complete analog-to-digital conversion; The remaining power estimation unit is used to integrate the outputs of the voltage acquisition unit and the current acquisition unit and estimate the remaining battery power percentage using an improved Kalman filter algorithm; A remaining flight time prediction unit is used to calculate the remaining time for the underwater robot to continue working based on the historical power consumption curve and the current power consumption rate; A threshold determination unit is used to compare the remaining battery percentage and the remaining flight time with a preset threshold value and generate a charging trigger signal when either indicator is below the threshold value; The communication interface unit is used to send the charging trigger signal to the fuzzy control module and the charging module respectively, and receive the resumption signal after the charging module completes charging and forward it to the fuzzy control module.

7. The STM32-based autonomous underwater robot intelligent attitude control system according to claim 6, characterized in that: The remaining power estimation unit includes: The Coulomb integrator unit is used to perform time integration on the instantaneous current output by the current acquisition unit to obtain the battery power consumption; The open circuit voltage correction subunit is used to measure the open circuit voltage of the battery when the underwater robot is stationary or in a low power consumption stage, and to correct the open circuit voltage according to the electrochemical characteristic curve to obtain an open circuit voltage correction value; The temperature compensation subunit is used to collect the temperature data of the water flow and determine the temperature compensation amount; The health status assessment subunit is used to assess the battery aging by counting the number of charge and discharge cycles and detecting the internal resistance change rate to generate a capacity attenuation coefficient; The fusion filter subunit is used to use the unscented Kalman filter algorithm to fuse the Coulomb integral result, the open circuit voltage correction amount, the temperature compensation amount and the capacity attenuation coefficient, and output the percentage of the remaining battery power.

8. The STM32-based autonomous underwater robot intelligent attitude control system according to claim 6, characterized in that: The charging module includes: a wave energy capture unit for converting the periodic up-and-down motion of waves into rotational mechanical energy through a buoy-linkage mechanism; a generator unit, configured to receive the rotational mechanical energy and output AC electrical energy; A rectifier and voltage stabilization unit, configured to rectify the AC power into stable DC power and perform constant voltage control on the output voltage; The maximum power point tracking unit is used to adjust the working parameters of the rectifier and voltage regulator unit in real time to ensure that the wave energy capture unit is in the optimal energy conversion state under different wave heights and frequencies; The attitude adjustment drive unit is used to drive the underwater robot to adjust its attitude according to the charging attitude instruction sent by the fuzzy control module, so that the wave energy capture unit maintains the optimal energy extraction angle and water surface height; The charging completion determination unit is used to generate a resumption signal when it is detected that the battery power reaches a set threshold and the charging current drops to a safe range, and send it to the fuzzy control module through the communication interface unit.

9. The STM32-based autonomous underwater robot intelligent attitude control system according to claim 1, characterized in that: The fuzzy control module is a microcontroller of the STM32F4 series.

10. An intelligent attitude control method for an autonomous underwater robot based on STM32, characterized in that: include: Collect robot status data and its environment data; A kinematic model and a disturbance force model are constructed according to the state data and the environmental data thereof, and a state estimation value and a disturbance force prediction value are outputted through an extended Kalman filter; Calculate the attitude error and error change rate based on the target attitude and the state estimation value, determine the PID parameter adjustment amount through three-layer fuzzy reasoning, and perform feedforward compensation in combination with the disturbance force prediction value to generate an attitude control instruction; monitoring the remaining battery power and the remaining heading to obtain a monitoring result, and sending a charging trigger signal to the fuzzy control module and the charging module when the monitoring result reaches a threshold; When the wave energy power generation device reaches the optimal power generation position, the power supply module is charged, and after charging is completed, a resumption signal is sent to the fuzzy control module.

Images