Swimming pool cleaning robot obstacle avoidance control system based on Kalman filter and control method thereof

Abstract

The invention relates to a swimming pool cleaning robot obstacle avoidance control system and method based on a Kalman filter, and the system comprises an ultrasonic sensor and an attitude sensor, the ultrasonic sensor is used for detecting the distance information of an obstacle, and the attitude sensor is used for measuring the attitude information of a robot in real time; the micro-control unit is connected with the sensor module and is used for receiving and processing data of the sensor module; and the motor driving module is connected with the micro-control unit and is used for receiving the driving instruction issued by the micro-control unit. According to the swimming pool cleaning robot obstacle avoidance control system, the high-precision, high-stability and high-reliability obstacle avoidance function is achieved through the Kalman filter. Various sensor data are fused, a fault diagnosis module is introduced, and the obstacle avoidance performance and safety of the robot in a complex environment can be effectively improved. In addition, the design of the system has good adaptability and flexibility, and the requirements of different application scenes can be met.

Description

Technical Field

[0001] The invention relates to a Kalman filter-based obstacle avoidance control system and a method for a swimming pool cleaning robot, and belongs to the field of obstacle avoidance. Background Art

[0002] With the continuous progress and development of science and technology, the quality of life of people is improving day by day and the number of swimming pools is increasing, and swimming pool cleaning robots are being used. At present, existing swimming pool cleaning robots have many shortcomings in obstacle avoidance. For example, ultrasonic sensors are easily disturbed by water waves and misjudge the distance of obstacles when they rely solely on them; infrared sensors are easily disturbed by the lighting environment. For example, in strong outdoor light, infrared sensors may misjudge the location of obstacles or fail to accurately detect obstacles because the infrared radiation in the surrounding environment is enhanced, interfering with the infrared signals emitted and received by the sensor itself; and most cleaning robots fail to effectively integrate information from multiple sensors, resulting in inaccurate and untimely obstacle avoidance decisions, affecting cleaning efficiency. Summary of the invention

[0003] In order to overcome the defects of the prior art, the present invention provides a swimming pool cleaning robot obstacle avoidance control system based on a Kalman filter and a control method thereof. The technical solution of the present invention is:

[0004] A swimming pool cleaning robot obstacle avoidance control system based on Kalman filter, comprising:

[0005] A sensor module, wherein the sensor module includes an ultrasonic sensor and a posture sensor, wherein the ultrasonic sensor is used to detect distance information of obstacles, and the posture sensor is used to measure the posture information of the robot in real time;

[0006] A microcontroller unit is connected to the sensor module and is used to receive and process data from the sensor module, and to calculate an obstacle avoidance control equation based on the sensor data by executing a Kalman filter algorithm;

[0007] The motor drive module is connected to the micro control unit and is used to receive the drive command issued by the micro control unit and convert it into a drive signal of the thruster to realize the obstacle avoidance action of the robot.

[0008] The ultrasonic sensor includes a left ultrasonic sensor and a right ultrasonic sensor, which are respectively used to detect the distance information of obstacles on the left and right sides of the robot. The attitude sensor is used to measure the attitude information of the robot's acceleration, angular velocity, heading angle, roll angle and pitch angle. The distance information fed back by the ultrasonic sensor is integrated with the attitude sensor data to obtain distance data and attitude data.

[0009] The microcontroller unit implements obstacle avoidance control by the following steps:

[0010] Step 1: Send a forward command to the pool cleaning robot, perform ultrasonic wide-field matrix measurement, receive the detection distance at time t, and record the distance information fed back by the ultrasonic wave in real time;

[0011] Step 2: Determine whether to avoid obstacles based on real-time distance information. If not, continue to execute the forward command. If yes, initialize the state vector of the Kalman filter. The state vector in the two-dimensional coordinate system is x k =[x,y,v x ,v y ] T , where (x, y) is the position of the robot in the plane coordinate system, (v x ,v y ) is the speed of the robot in the x and y axis directions, and the state vector x at the current k moment k Through the state X at the last moment k-1 The state equation X is inferred k-1 =Ax k-1 +w k-1 , the state transfer matrix is ​​created according to the uniform motion model, and the process model is used in the prediction stage to generate a priori estimation equation for the current estimate and the priori covariance formula is as follows:

[0012]

[0013] is the prior estimate at time k, A is the conjecture model for state transition, is the posterior estimate at time kl, is the optimal estimate obtained from the measurement at time k-1, B is the state input matrix, u k-1 is the control input, P k - is the prior error covariance, A is the state transfer matrix, P k-1 The state estimation covariance matrix of the previous moment (k-1), AP k-1 A T The meaning is to infer the covariance of the current moment from the previous moment, which is equal to the covariance of the previous moment multiplied by the state transfer matrix on both sides. It is obtained according to the properties of the covariance matrix. The Q process noise covariance matrix represents the uncertainty of the system model.

[0014] Step 3: Obtain the observation equations and matrices, observation noise and matrices of the left ultrasonic sensor and the right ultrasonic sensor, and establish the two-dimensional coordinate system of the robot. The center position of the robot head is taken as the origin, the forward direction is taken as the x-axis, and the right direction perpendicular to the x-axis is taken as the y-axis. The position of the robot in the coordinate system is (x i ,y i ), the observation vector z of the left ultrasonic sensor k =[d 1 ]T , the observation vector of the right ultrasonic sensor is z k =[d 2 ] T , and then get the observation equation Z k =HX k +v k , the observation value zk is the linear transformation of the system state xk through the observation matrix Hk, plus the observation noise vk. Due to the existence of noise vk, the observation value zk is not completely accurate, but fluctuates randomly around the real state, the observation matrix Hk and the system state xk. The noise vk introduces the observation noise covariance matrix R, and the parameters are adjusted according to the observation noise matrix of the left and right ultrasonic sensors;

[0015] Step 4: Obtain the equation for the posterior estimation by fusion of the data in step (3): By Kalman gain K k A priori estimate and z k The measurements are fused to calculate the posterior estimate, where H is the measurement matrix that links the system state to the measurements;

[0016] Step 5: Based on the Kalman filter, the left and right ultrasonic sensors are processed by adjusting the two parameters of the Q matrix and the R matrix. Q is the process noise covariance matrix, and R is the observation noise covariance matrix. By adjusting the Q matrix and the R matrix, a balance point is found between the uncertainty of the system model and the unreliability of the observed value. If the Q matrix is ​​larger, it means that the uncertainty of the system model is higher, and the filter is more dependent on the observed data update. The smaller the Q matrix is, the more accurate the system model is, and the filter will rely on the model prediction. If the R matrix is ​​larger, it means that the observed value is unreliable, and the system relies on the model prediction. The smaller the R matrix is, the more accurate the observed data is, and the filter will rely more on the observed data. The Q matrix and the R matrix are adjusted to balance the model and the observation weights, so that the state estimation does not rely too much on the model or the observed data, and the stability and accuracy of the obstacle avoidance are increased; it also includes a fault diagnosis module, which is used to establish a statistical model and a threshold range of normal measurement data through long-term observation of the ultrasonic sensor measurement data and the state estimation after the Kalman filter. When the sensor fails or the measurement data is abnormal, the estimation result after the Kalman filter will exceed the normal range, thereby timely detecting the sensor failure or abnormality, and providing a basis for the fault diagnosis of the system;

[0017] The observation equation of step (3) is as follows:

[0018]

[0019] Written in matrix form: Zk = HXk + Vk; therefore, the Hl observation matrix and Hr observation matrix of the left and right ultrasonic sensors are as follows:

[0020]

[0021] Among them, Dl is the distance measured by the left ultrasound, Dr is the distance measured by the right ultrasound, the center position of the robot is (x, y), the coordinates of the obstacle are (xi, yi), Hl is the left observation covariance matrix, and Hr is the right observation covariance matrix.

[0022] The matrix form of step (5) is as follows:

[0023] The H matrix is:

[0024]

[0025]

[0026] The Q matrix is:

[0027]

[0028] The R matrix is: R = diag (σ 2 dl , σ 2 dr); where Hl and Hr are the state transfer matrices of the left and right ultrasonic sensors, σ 2 , σ 2 y position noise variance; σ 2 v x , σ 2 v y Speed ​​noise variance; σxv x , σ yvy is the covariance of position and velocity;

[0029] Also included is step (6), specifically:

[0030] 6.1 Based on the data after Kalman filtering, the position of the obstacle in the coordinate system (Xob, Yob) and the center position of the robot (x, y) are calculated through the triangulation positioning algorithm;

[0031] 6.2 Calculate the distance d between the robot and the obstacle, where d = √((Xob-x)^2+(Yob-y)^2);

[0032] 6.3 Define a safety distance dv. When d≤dv, execute the obstacle avoidance strategy to control the robot to avoid obstacles.

[0033] A Kalman filter-based obstacle avoidance control method for a swimming pool cleaning robot, characterized in that it comprises the following steps:

[0034] (a) acquiring environmental data through a sensor module, wherein the sensor module includes an ultrasonic sensor and a posture sensor, wherein the ultrasonic sensor is used to detect distance information of obstacles, and the posture sensor is used to measure the posture information of the robot in real time;

[0035] (b) The distance information fed back by the ultrasonic sensor is integrated with the data of the attitude sensor to obtain stable distance data and attitude data;

[0036] (c) using a microcontroller unit to receive the fused data, and processing the data by executing a Kalman filter algorithm to calculate an obstacle avoidance control equation;

[0037] (d) Based on the results of the Kalman filter algorithm, the robot’s obstacle avoidance action is controlled by the motor drive module.

[0038] The specific implementation steps of the Kalman filter algorithm in step (c) include:

[0039] (c1) Sending a forward command to the pool cleaning robot, measuring through an ultrasonic sensor, and recording the distance information fed back by the ultrasonic wave in real time;

[0040] (c2) Determine whether obstacle avoidance is required based on real-time distance information. If necessary, initialize the state vector of the Kalman filter, which is x k =[x,y,v x ,v y ] T , where (x, y) is the position of the robot in the plane coordinate system, (v x ,v y ) is the speed of the robot in the x and y axis directions;

[0041] (c3) Create a state transfer matrix based on the uniform motion model, and use the state equation X k-1 =Ax k-1 +w k-1 Predict the state vector at the current moment and calculate the prior covariance matrix Pk-; (c4) Obtain the observation equations and matrices of the left ultrasonic sensor and the right ultrasonic sensor, update the observation data, and adjust the parameters according to the observation noise matrix;

[0042] (c5) obtaining the posterior estimated state vector through data fusion and updating the covariance matrix according to the Kalman gain;

[0043] The calculation formulas of the state transfer matrix A, the process noise covariance matrix Q and the prior covariance matrix Pk- in the step (c3) are as follows:

[0044] The state transfer matrix A is constructed according to the uniform motion model; the process noise covariance matrix Q is used to describe the statistical characteristics of the system process noise;

[0045] Prior covariance matrix P k - The update formula is: k - =AP k-1 A T +Q.

[0046] The state update formula of the Kalman filter in step (c5) is as follows:

[0047] The state vector update formula is:

[0048] The covariance matrix update formula is: The advantages of the present invention are:

[0049] High-precision obstacle avoidance: The Kalman filter can effectively improve the accuracy and reliability of obstacle detection by fusing the data of ultrasonic sensors and attitude sensors. The Kalman filter can process sensor data in real time, dynamically adjust obstacle avoidance strategies, and adapt to complex swimming pool environments.

[0050] Stable obstacle avoidance performance: The Kalman filter can effectively suppress sensor noise and system errors through state estimation and covariance update, and improve the stability of obstacle avoidance control.

[0051] By adjusting the H matrix, Q matrix and R matrix, the obstacle avoidance performance can be further optimized.

[0052] Fault diagnosis capability: It can detect sensor failure or measurement anomaly in a timely manner through long-term observation and state estimation after Kalman filtering. The fault diagnosis module ensures the reliable operation of the system and reduces the safety risks caused by sensor failure.

[0053] Flexible obstacle avoidance strategy: Based on the data after Kalman filtering, the obstacle position is calculated through the triangulation positioning algorithm, and the obstacle avoidance strategy is executed according to the safe distance; the obstacle avoidance strategy can be flexibly adjusted according to actual needs to adapt to different swimming pool environments and task requirements.

[0054] Efficient data fusion: The data fusion of ultrasonic sensors and attitude sensors can provide more comprehensive environmental information and improve the efficiency of obstacle avoidance control. Data fusion reduces the limitations of a single sensor and enhances the robustness of the system.

[0055] The obstacle avoidance control system of the pool cleaning robot of the present invention realizes the obstacle avoidance function with high precision, high stability and high reliability through the Kalman filter. It integrates multiple sensor data and introduces a fault diagnosis module, which can effectively improve the obstacle avoidance performance and safety of the robot in complex environments. In addition, the design of the system has good adaptability and flexibility, and can meet the needs of different application scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a schematic diagram of the main structure of the system of the present invention.

[0057] Figure 2 is a flow chart of the control method of the present invention. DETAILED DESCRIPTION

[0058] The present invention will be further described below in conjunction with specific embodiments, and the advantages and features of the present invention will become clearer as the description proceeds. However, these embodiments are exemplary only and do not constitute any limitation to the scope of the present invention. It should be understood by those skilled in the art that the details and forms of the technical solution of the present invention may be modified or replaced without departing from the spirit and scope of the present invention, but these modifications and replacements all fall within the scope of protection of the present invention.

[0059] See also Figure 1 and Figure 2 The present invention relates to an obstacle avoidance control system for a swimming pool cleaning robot based on a Kalman filter, comprising:

[0060] Sensor module 1, the sensor module includes an ultrasonic sensor and a posture sensor, the ultrasonic sensor is used to detect the distance information of obstacles, and the posture sensor is used to measure the posture information of the robot in real time;

[0061] A micro control unit 2 is connected to the sensor module, and is used to receive and process the data from the sensor module, and to calculate the obstacle avoidance control equation according to the sensor data by executing the Kalman filter algorithm;

[0062] The motor driving module 3 is connected to the micro control unit, and is used to receive the driving instructions issued by the micro control unit and convert them into driving signals of the thrusters to realize the obstacle avoidance action of the robot.

[0063] The Kalman filter-based swimming pool cleaning robot obstacle avoidance control system has the following structural advantages, which are mainly reflected in the reliability, accuracy, flexibility and adaptability of the system:

[0064] 1. High-precision obstacle avoidance capability

[0065] Sensor fusion: The system combines ultrasonic sensors and attitude sensors. Ultrasonic sensors can provide distance information of obstacles, while attitude sensors can measure the robot's acceleration, angular velocity, heading angle, roll angle, pitch angle and other attitude information in real time. The Kalman filter algorithm is used to fuse these data, which can effectively improve the accuracy and reliability of obstacle detection.

[0066] Dynamic adjustment: The Kalman filter can dynamically adjust the obstacle avoidance strategy based on real-time sensor data. Even if there is noise or error in the sensor data, the system can make corrections through the filtering algorithm to achieve high-precision obstacle avoidance.

[0067] 2. System stability and reliability

[0068] Advantages of Kalman filter: Kalman filter is a recursive algorithm that can process sensor data in real time and suppress sensor noise and system errors through state estimation and covariance update. The introduction of this algorithm significantly improves the stability of the obstacle avoidance control system and reduces misjudgments caused by sensor failure or environmental interference.

[0069] Fault diagnosis capability: The system can establish a statistical model and threshold range for normal measurement data through long-term observation and state estimation after Kalman filtering. When a sensor fails or measurement data is abnormal, the system can detect and issue an alarm in time, thereby improving the overall reliability of the system.

[0070] 3. Flexible obstacle avoidance strategy

[0071] Dynamic obstacle avoidance control: The microcontroller unit calculates the obstacle avoidance control equation based on the output of the Kalman filter and implements the robot's obstacle avoidance action through the motor drive module. This dynamic obstacle avoidance strategy can be flexibly adjusted according to real-time environmental information to adapt to different obstacle positions and motion states.

[0072] Safety distance setting: The system can set a safety distance according to actual needs. When the distance between the robot and the obstacle is less than the safety distance, the obstacle avoidance strategy is automatically executed. This strategy not only improves the flexibility of obstacle avoidance, but also enhances the safety of the system.

[0073] 4. Strong adaptability

[0074] Multiple sensor support: The system combines ultrasonic sensors and gesture sensors to provide more comprehensive environmental information. This multi-sensor fusion design enables the system to adapt to different types of swimming pool environments and obstacle types.

[0075] Parameter adjustability: The key parameters in the Kalman filter (such as the process noise covariance matrix Q, the observation noise covariance matrix R, etc.) can be adjusted according to actual needs to optimize the obstacle avoidance performance. This parameter adjustability enables the system to adapt to different application scenarios and task requirements.

[0076] 5. Efficient data processing capabilities

[0077] Real-time: Kalman filter can complete data processing and state estimation in a short time, ensuring the real-time response capability of the system. This is especially important for fast-moving robots, which can effectively avoid collisions.

[0078] Data fusion: The system uses the Kalman filter algorithm to fuse the data of the ultrasonic sensor and the attitude sensor, reducing the limitations of a single sensor and improving the overall performance of the system.

[0079] 6. Improve cleaning efficiency

[0080] Reduced collision risk: Through efficient obstacle avoidance control, the robot is able to avoid collisions with pool walls, steps or other obstacles, thereby reducing cleaning interruptions caused by collisions.

[0081] Optimize cleaning path: The system can dynamically adjust the cleaning path according to the location of obstacles to ensure the continuity and efficiency of the cleaning process.

[0082] This Kalman filter-based obstacle avoidance control system for a pool cleaning robot significantly improves the accuracy and reliability of obstacle avoidance through sensor fusion, dynamic obstacle avoidance strategy, and fault diagnosis capabilities. Its modular design and parameter adjustability make the system easy to expand and maintain, and can adapt to a variety of application scenarios. This system not only improves the intelligence level of the pool cleaning robot, but also provides a useful reference for the design of similar robot systems.

[0083] The ultrasonic sensor includes a left ultrasonic sensor and a right ultrasonic sensor, which are respectively used to detect the distance information of obstacles on the left and right sides of the robot. The attitude sensor is used to measure the attitude information of the robot's acceleration, angular velocity, heading angle, roll angle and pitch angle. The distance information fed back by the ultrasonic sensor is integrated with the attitude sensor data to obtain distance data and attitude data.

[0084] Based on the above ultrasonic sensor settings, the following advantages are achieved:

[0085] Multi-angle obstacle detection: The left and right ultrasonic sensors detect the distance information of obstacles on both sides of the robot respectively, and can perceive the horizontal obstacle distribution of the robot in real time. This two-sided detection method makes up for the visual limitation of a single sensor, avoids blind spots, and ensures that the robot can fully perceive potential collision risks in complex swimming pool environments.

[0086] Attitude information supplement: The attitude sensor measures the robot's acceleration, angular velocity, heading angle, roll angle, and pitch angle, providing information about the robot's motion state in three-dimensional space. These data can help the system determine whether the robot is in a tilted, rotating, or accelerated state, thereby more accurately predicting the relative motion relationship between the robot and obstacles.

[0087] Advantages of data fusion: Fusion of the distance information of the ultrasonic sensor and the attitude information of the attitude sensor can comprehensively consider the position of the obstacle and the motion state of the robot. For example, when the robot is tilted or rotated, relying solely on distance information may lead to misjudgment, while the addition of attitude information can correct this error, thereby improving the accuracy of obstacle avoidance decisions.

[0088] Reduce misjudgment and missed judgment: Ultrasonic sensors may be disturbed by the underwater environment (such as bubbles, water flow, etc.) and produce noise or errors. The data of the attitude sensor can be used as auxiliary information to help the system filter out these noises, reduce the possibility of misjudgment and missed judgment, and enhance the robustness of the system.

[0089] The microcontroller unit implements obstacle avoidance control by the following steps:

[0090] Step 1: Send a forward command to the pool cleaning robot, perform ultrasonic wide-field matrix measurement, receive the detection distance after time t, and record the distance information fed back by the ultrasonic wave in real time; this real-time data collection ensures the robot's rapid response to the environment.

[0091] Step 2: Determine whether to avoid obstacles based on real-time distance information. If not, continue to execute the forward command. If yes, initialize the state vector of the Kalman filter. The state vector in the two-dimensional coordinate system is x k =[x,y,v x , v y ] T , where (x, y) is the position of the robot in the plane coordinate system, (v x , v y ) is the speed of the robot in the x and y axes, and the state vector x at the current k moment k Through the state X at the last moment k-1 The state equation X is inferred k-1 =Ax k-1 +w k-1, the state transfer matrix is ​​created according to the uniform motion model, and the process model is used in the prediction stage to generate a priori estimation equation for the current estimate and the priori covariance formula is as follows:

[0092]

[0093] is the prior estimate at time k, A is the conjecture model for state transition, is the posterior estimate at time k-1, that is, the optimal estimate obtained by measuring the value at time k-1, B is the state input matrix, u k-1 is the control input, P k - is the prior error covariance, A is the state transfer matrix, P k-1 The state estimation covariance matrix of the previous k-1 time, AP k-1 A T The meaning is to infer the covariance of the current moment from the previous moment, which is equal to the covariance of the previous moment multiplied by the state transfer matrix on both sides. It is obtained according to the properties of the covariance matrix. The Q process noise covariance matrix represents the uncertainty of the system model.

[0094] By using real-time distance information to determine whether obstacle avoidance is needed, the robot can make proactive decisions rather than passively waiting for a collision to occur.

[0095] Step 3: Obtain the observation equations and matrices, observation noise and matrices of the left ultrasonic sensor and the right ultrasonic sensor, and establish the two-dimensional coordinate system of the robot. The center position of the robot head is taken as the origin, the forward direction is taken as the x-axis, and the right direction perpendicular to the x-axis is taken as the y-axis. The position of the robot in the coordinate system is (x i ,y i ), the observation vector z of the left ultrasonic sensor k =[d 1 ] T , the observation vector of the right ultrasonic sensor is z k =[d 2 ] T , and then get the observation equation Z k =HX k +v k , the observation value zk is the linear transformation of the system state xk through the observation matrix Hk, plus the observation noise vk. Due to the existence of noise vk, the observation value zk is not completely accurate, but fluctuates randomly around the real state, the observation matrix Hk and the system state xk. The noise vk introduces the observation noise covariance matrix R, and the parameters are adjusted according to the observation noise matrix of the left and right ultrasonic sensors;

[0096] Step 4: Obtain the state update equation of the posterior estimate by data fusion in step (3): is the posterior state estimate, is the prior state estimate, K k The Kalman gain is the weight matrix that determines the influence of the observation value on the state estimation. k The observation value represents the data obtained from the sensor at time k. Let H be the observation matrix Mapping to the observation space, data fusion obtains the equation for posterior estimation, which enables the robot to comprehensively consider the data from multiple sensors and improve the accuracy and reliability of state estimation.

[0097] Step 5: Based on the Kalman filter, the left and right ultrasonic sensors are processed. The state update formula is as follows:

[0098] This formula is the a posteriori state estimation equation; This formula is the posterior covariance update equation; where Kk represents the Kalman gain. The Kalman gain formula is derived from the Kalman gain.

[0099] is the posterior state estimate; represents the prior state estimate, Zk is the observation, The observation matrix H will be Mapped to the observation space, Pk represents the posterior covariance matrix at time k, I is the unit matrix, which is used to keep the structure of other matrices unchanged in matrix operations, Kk is the Kalman gain matrix, P k - is the prior covariance matrix, H is the observation matrix, H T is the transpose of the observation matrix, R is the observation noise, and by adjusting the two parameters of the Q matrix and the R matrix, Q is the process noise covariance matrix, and R is the observation noise covariance matrix. By adjusting Q and R, a balance can be found between the uncertainty of the system model and the unreliability of the observation. If Q is larger, the uncertainty of the system model is higher, and the filter is more dependent on the observation data update. If Q is smaller, the system model is more accurate, and the filter will rely on the model prediction. If R is larger, the observation is unreliable, and the system relies on the model prediction. The smaller R is, the more accurate the observation data is, and the filter will rely more on the observation data. Adjust Q and R to balance the model and observation weights, so that the state estimation does not rely too much on the model or the observation data, and the stability and accuracy of obstacle avoidance are increased. Based on the processing of Kalman filtering, the robot can effectively filter out noise and improve the stability of obstacle avoidance control.

[0100] It also includes a fault diagnosis module, which is used to establish a statistical model and threshold range of normal measurement data through long-term observation of ultrasonic sensor measurement data and state estimation after Kalman filtering. When the sensor fails or the measurement data is abnormal, the estimation result after Kalman filtering will exceed the normal range, thereby timely detecting the sensor failure or abnormality, providing a basis for system fault diagnosis. Through the fault diagnosis module, the robot can detect sensor failure or abnormality in a timely manner, thereby ensuring the reliability of the system.

[0101] The observation equation of step (3) is as follows:

[0102]

[0103] Written in matrix form: Zk = HXk + Vk; therefore, the Hl observation matrix and Hr observation matrix of the left and right ultrasonic sensors are as follows:

[0104]

[0105]

[0106] Where D1 is the distance measured by the left ultrasonic wave, Dr is the distance measured by the right ultrasonic wave, the center position of the robot is (x, y), the coordinates of the obstacle are (xi, yi), H1 is the left observation covariance matrix, and Hr is the right observation covariance matrix;

[0107] The matrix form of step (5) is as follows:

[0108] The H matrix is:

[0109]

[0110]

[0111] The Q matrix is:

[0112]

[0113] The R matrix is: R = diag (σ 2 dl,σ 2 dr); where Hl and Hr are the state transfer matrices of the left and right ultrasonic sensors, σ 2 x,σ 2 y is the position noise variance; σ 2 v x , σ 2 v y is the speed noise variance; σxv x , σyvy is the covariance of position and velocity;

[0114] The meaning of the Q matrix represents the estimation of random noise in the system model, and the R matrix represents the estimation of measurement noise.

[0115] Also included is step (6), specifically:

[0116] 6.1 Based on the data after Kalman filtering, the position of the obstacle in the coordinate system (Xob, Yob) and the center position of the robot (x, y) are calculated through the triangulation positioning algorithm;

[0117] 6.2 Calculate the distance d between the robot and the obstacle, where d = √((Xob-x)^2+(Yob-y)^2);

[0118] 6.3 Define a safety distance dv. When d≤dv, execute the obstacle avoidance strategy to control the robot to avoid obstacles.

[0119] The steps of the Kalman filter-based swimming pool cleaning robot obstacle avoidance control system of the present invention significantly improve the obstacle avoidance performance and overall efficiency of the swimming pool cleaning robot through real-time data acquisition, high-precision state estimation, data fusion and filtering, fault diagnosis and anomaly detection, flexible obstacle avoidance strategy, optimized parameter adjustment, safety considerations, efficient data processing, system intelligence and scalability.

[0120] The present invention also relates to an obstacle avoidance control method for a swimming pool cleaning robot based on a Kalman filter, comprising the following steps:

[0121] (a) acquiring environmental data through a sensor module, wherein the sensor module includes an ultrasonic sensor and a posture sensor, wherein the ultrasonic sensor is used to detect distance information of obstacles, and the posture sensor is used to measure the posture information of the robot in real time;

[0122] (b) The distance information fed back by the ultrasonic sensor is integrated with the data of the attitude sensor to obtain stable distance data and attitude data;

[0123] (c) using a microcontroller unit to receive the fused data, and processing the data by executing a Kalman filter algorithm to calculate an obstacle avoidance control equation;

[0124] (d) Based on the results of the Kalman filter algorithm, the robot’s obstacle avoidance action is controlled by the motor drive module.

[0125] The specific implementation steps of the Kalman filter algorithm in step (c) include:

[0126] (c1) Sending a forward command to the pool cleaning robot, measuring through an ultrasonic sensor, and recording the distance information fed back by the ultrasonic wave in real time;

[0127] (c2) Determine whether obstacle avoidance is required based on real-time distance information. If necessary, initialize the state vector of the Kalman filter, which is x k =[x,y,v x , v y ] T , where (x, y) is the position of the robot in the plane coordinate system, (v x , v y ) is the speed of the robot in the x and y axis directions;

[0128] (c3) Create a state transfer matrix based on the uniform motion model, and use the state equation X k-1 =Ax k-1 +w k-1 Predict the state vector at the current moment and calculate the prior covariance matrix Pk-;

[0129] (c4) obtaining the observation equations and matrices of the left ultrasonic sensor and the right ultrasonic sensor, updating the observation data, and adjusting the parameters according to the observation noise matrix;

[0130] (c5) obtaining the posterior estimated state vector through data fusion and updating the covariance matrix according to the Kalman gain;

[0131] The calculation formulas of the state transfer matrix A, the process noise covariance matrix Q and the prior covariance matrix Pk- in the step (c3) are as follows:

[0132] The state transfer matrix A is constructed according to the uniform motion model; the process noise covariance matrix Q is used to describe the statistical characteristics of the system process noise;

[0133] The update formula of the prior covariance matrix Pk is:

[0134] The state update formula of the Kalman filter in step (c5) is as follows:

[0135] The state vector update formula is:

[0136] The covariance matrix update formula is: Among them, Kk is the Kalman gain, H is the observation matrix, Zk is the observation value, and I is the unit matrix.

[0137] Based on the configuration of the system and method of the present invention, the following is further achieved:

[0138] Improve obstacle avoidance accuracy: The Kalman filter can be used to fuse the data of the ultrasonic sensor and the attitude sensor, effectively filtering out noise and improving the estimation accuracy of the robot's position, speed and other state information. This enables the robot to more accurately judge the position and distance of obstacles in complex environments and avoid misjudgment caused by sensor noise.

[0139] Enhanced system robustness: The Kalman filter can adapt to changes in the system model and noise statistics, and can provide the minimum mean square error state estimate of the system through recursive estimation even in the presence of noise and uncertainty in the measured data. This allows the pool cleaning robot to maintain stable obstacle avoidance performance when facing complex underwater environments and dynamic changes.

[0140] Optimize data processing efficiency: The recursive nature of the Kalman filter enables it to process input data in real time without storing a large amount of historical data, thereby improving the system's response speed and data processing efficiency. This can significantly improve the operating efficiency of a pool cleaning robot that needs to avoid obstacles in real time.

[0141] Realize multi-sensor data fusion: The present invention fully utilizes the advantages of different types of sensors by fusing the data of ultrasonic sensors and attitude sensors. This fusion method not only improves the reliability of data, but also provides more comprehensive information support for subsequent obstacle avoidance decisions.

[0142] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. A swimming pool cleaning robot obstacle avoidance control system based on Kalman filter, characterized in that: include: A sensor module, wherein the sensor module includes an ultrasonic sensor and a posture sensor, wherein the ultrasonic sensor is used to detect distance information of obstacles, and the posture sensor is used to measure the posture information of the robot in real time; A microcontroller unit is connected to the sensor module and is used to receive and process data from the sensor module, and to calculate an obstacle avoidance control equation based on the sensor data by executing a Kalman filter algorithm; The motor drive module is connected to the micro control unit and is used to receive the drive command issued by the micro control unit and convert it into a drive signal of the thruster to realize the obstacle avoidance action of the robot.

2. The swimming pool cleaning robot obstacle avoidance control system according to claim 1, characterized in that: The ultrasonic sensor includes a left ultrasonic sensor and a right ultrasonic sensor, which are respectively used to detect the distance information of obstacles on the left and right sides of the robot. The attitude sensor is used to measure the attitude information of the robot's acceleration, angular velocity, heading angle, roll angle and pitch angle. The distance information fed back by the ultrasonic sensor is integrated with the attitude sensor data to obtain distance data and attitude data.

3. The swimming pool cleaning robot obstacle avoidance control system according to claim 1, characterized in that: The microcontroller unit implements obstacle avoidance control by the following steps: Step 1: Send a forward command to the pool cleaning robot, perform ultrasonic wide-field matrix measurement, receive the detection distance at time t, and record the distance information fed back by the ultrasonic wave in real time; Step 2: Determine whether to avoid obstacles based on real-time distance information. If not, continue to execute the forward command. If yes, initialize the state vector of the Kalman filter. The state vector in the two-dimensional coordinate system is x k =[x,y,v x ,v y ] T , where (x, y) is the position of the robot in the plane coordinate system, (v x ,v y ) is the speed of the robot in the x and y axis directions, and the state vector x at the current k moment k Through the state X at the last moment k-1 The state equation X is inferred k-1 =Ax k-1 +w k-1 , the state transfer matrix is ​​created according to the uniform motion model, and the process model is used in the prediction stage to generate a priori estimation equation for the current estimate and the priori covariance formula is as follows: is the prior estimate at time k, A is the conjecture model for state transition, is the posterior estimate at time k-1, that is, the optimal estimate obtained by the measurement value at time k-1, B is the state input matrix, u k-1 is the control input, is the prior error covariance, A is the state transfer matrix, P k-1 The state estimation covariance matrix of the previous k-1 time, AP k-1 A T The meaning is to infer the covariance of the current moment from the previous moment, which is equal to the covariance of the previous moment multiplied by the state transfer matrix on both sides. It is obtained according to the properties of the covariance matrix. The Q process noise covariance matrix represents the uncertainty of the system model. Step 3: Obtain the observation equations and matrices, observation noise and matrices of the left ultrasonic sensor and the right ultrasonic sensor, and establish the two-dimensional coordinate system of the robot. The center position of the robot head is taken as the origin, the forward direction is taken as the x-axis, and the right direction perpendicular to the x-axis is taken as the y-axis. The position of the robot in the coordinate system is (x i ,y i ), the observation vector z of the left ultrasonic sensor k =[d1] T , the observation vector of the right ultrasonic sensor is z k =[d2] T , and then get the observation equation Z k =HX k +v k , the observation value zk is the linear transformation of the system state xk through the observation matrix Hk, plus the observation noise vk. Due to the existence of noise vk, the observation value zk is not completely accurate, but fluctuates randomly around the real state, the observation matrix Hk and the system state xk. The noise vk introduces the observation noise covariance matrix R, and the parameters are adjusted according to the observation noise matrix of the left and right ultrasonic sensors; Step 4: Obtain the equation for the posterior estimation by data fusion in step (3): By Kalman gain K k A priori estimate and z k The measurements are fused to calculate the posterior estimate, where H is the measurement matrix that links the system state to the measurements; Step 5: Based on the Kalman filter, the left and right ultrasonic sensors are processed by adjusting the two parameters of the Q matrix and the R matrix. Q is the process noise covariance matrix, and R is the observation noise covariance matrix. By adjusting the Q matrix and the R matrix, a balance is found between the uncertainty of the system model and the unreliability of the observation. If the Q matrix is ​​larger, it means that the uncertainty of the system model is higher, and the filter is more dependent on the observation data update. The smaller the Q matrix, the more accurate the system model is, and the filter will rely on the model prediction. If the R matrix is ​​larger, it means that the observation is unreliable and the system relies on the model prediction. The smaller the R matrix, the more accurate the observation data is, and the filter will rely more on the observation data. Adjust the Q matrix and the R matrix to balance the model and observation weights.

4. The swimming pool cleaning robot obstacle avoidance control system according to claim 1, characterized in that: It also includes a fault diagnosis module, which is used to establish a statistical model and threshold range for normal measurement data through long-term observation of ultrasonic sensor measurement data and state estimation after Kalman filtering. When the sensor fails or the measurement data is abnormal, the estimation result after Kalman filtering will exceed the normal range, thereby detecting the sensor failure or abnormality in time and providing a basis for system fault diagnosis.

5. The obstacle avoidance control system of the swimming pool cleaning robot according to claim 3, characterized in that: The observation equation of step (3) is as follows: Written in matrix form: Zk = HXk + Vk; therefore, the Hl observation matrix and Hr observation matrix of the left and right ultrasonic sensors are as follows: Among them, Dl is the distance measured by the left ultrasound, Dr is the distance measured by the right ultrasound, the center position of the robot is (x, y), the coordinates of the obstacle are (xi, yi), Hl is the left observation covariance matrix, and Hr is the right observation covariance matrix.

6. The obstacle avoidance control system of the swimming pool cleaning robot according to claim 3, characterized in that: The matrix form of step (5) is as follows: The H matrix is: The Q matrix is: The R matrix is: R = diag (σ 2 dl ,σ 2 dr); where Hl and Hr are the state transfer matrices of the left and right ultrasonic sensors, σ 2 x,σ 2 y position noise variance; σ 2 v x ,σ 2 vy velocity noise variance); σxv x ,σyvy is the covariance of position and velocity.

7. The swimming pool cleaning robot obstacle avoidance control system according to claim 3, characterized in that: Also included is step (6), specifically: 6.1 Based on the data after Kalman filtering, the position of the obstacle in the coordinate system (Xob, Yob) and the center position of the robot (x, y) are calculated through the triangulation positioning algorithm; 6.2 Calculate the distance d between the robot and the obstacle, where 6.3 Define a safety distance dv. When d≤dv, execute the obstacle avoidance strategy to control the robot to avoid obstacles.

8. An obstacle avoidance control method for a swimming pool cleaning robot based on a Kalman filter according to any one of claims 1 to 7, characterized in that: The following steps are involved: (a) acquiring environmental data through a sensor module, wherein the sensor module includes an ultrasonic sensor and a posture sensor, wherein the ultrasonic sensor is used to detect distance information of obstacles, and the posture sensor is used to measure the posture information of the robot in real time; (b) The distance information fed back by the ultrasonic sensor is integrated with the data of the attitude sensor to obtain stable distance data and attitude data; (c) using a microcontroller unit to receive the fused data, and processing the data by executing a Kalman filter algorithm to calculate an obstacle avoidance control equation; (d) Based on the results of the Kalman filter algorithm, the robot’s obstacle avoidance action is controlled by the motor drive module.

9. The obstacle avoidance control method according to claim 8, characterized in that: The specific implementation steps of the Kalman filter algorithm in step (c) include: (c1) Sending a forward command to the pool cleaning robot, measuring through an ultrasonic sensor, and recording the distance information fed back by the ultrasonic wave in real time; (c2) Determine whether obstacle avoidance is required based on real-time distance information. If necessary, initialize the state vector of the Kalman filter, which is x k =[x,y,v x ,v y ] T , where (x, y) is the position of the robot in the plane coordinate system, (v x ,v y ) is the speed of the robot in the x and y axis directions; (c3) Create a state transfer matrix based on the uniform motion model, and use the state equation X k-1 =Ax k-1 +w k-1 Predict the state vector at the current moment and calculate the prior covariance matrix Pk-; (c4) obtain the observation equations and matrices of the left ultrasonic sensor and the right ultrasonic sensor, update the observation data, and adjust the parameters according to the observation noise matrix; (c5) obtaining the posterior estimated state vector through data fusion and updating the covariance matrix according to the Kalman gain; The calculation formulas of the state transfer matrix A, the process noise covariance matrix Q and the prior covariance matrix Pk- in the step (c3) are as follows: The state transfer matrix A is constructed according to the uniform motion model; the process noise covariance matrix Q is used to describe the statistical characteristics of the system process noise; Prior covariance matrix The update formula is:

10. The obstacle avoidance control method according to claim 8, characterized in that: The state update formula of the Kalman filter in step (c5) is as follows: The state vector update formula is: The covariance matrix update formula is:

Images