A curtain wall cleaning robot body stabilizing control method and related equipment

Abstract

The application provides a curtain wall cleaning robot body stability control method and related equipment, and relates to the technical field of curtain wall cleaning robot control. The method comprises the following steps: obtaining the initial pose of the robot; obtaining the first pose data of the robot through the pose model; obtaining the actual motion data of the robot through the mathematical model; obtaining the second pose data of the robot according to the motion data; obtaining the fusion pose of the robot according to the first pose data and the second pose data; calculating the deviation between the fusion pose and the initial pose; calculating the control quantity according to the deviation; and adjusting the pose of the robot by controlling the duct fan based on the control quantity. The method aims to solve the problem that the existing curtain wall cleaning robot is unstable in the working process due to external disturbance, thereby affecting the cleaning effect and the working quality, and realizes accurate estimation and stable control of the pose of the robot body, which is beneficial to improving the cleaning effect and the working quality.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of curtain wall cleaning robot control, in particular to a curtain wall cleaning robot body stability control method and related equipment. BACKGROUND

[0002] To improve the cleaning efficiency of the curtain wall surface, reduce the operation difficulty, reduce the cleaning cost, and improve the operation safety, curtain wall cleaning robots are widely used for curtain wall cleaning. The existing curtain wall cleaning robots are generally provided with a cleaning device. When the robot climbs along the vertical direction of the curtain wall surface, the cleaning device cleans the curtain wall surface along the way. The execution components in the cleaning device generally include a water spraying strip, a rolling brush, and a water wiping strip. The execution components align the curtain wall surface to be cleaned and sequentially perform water spraying, washing, and water wiping steps, thereby achieving the cleaning of the curtain wall surface.

[0003] However, in actual operation, the robot is often affected by external forces (such as wind) and causes the body to deviate left and right or shake left and right, so that the execution components cannot align the area to be cleaned, and thus the local position cannot be cleaned completely, which cannot meet the higher cleaning degree requirement.

[0004] At present, there is no effective technical solution to the above problems. SUMMARY

[0005] The present application aims to provide a curtain wall cleaning robot body stability control method and related equipment, which solves the problem of unstable robot body caused by external disturbance during the operation of the existing curtain wall cleaning robot, thereby affecting the cleaning effect and operation quality, and realizes accurate estimation and stable control of the robot body pose, which is beneficial to improve the cleaning effect and operation quality.

[0006] In a first aspect, the present application provides a curtain wall cleaning robot body stability control method applied to the control system of a curtain wall cleaning robot. The left and right sides of the curtain wall cleaning robot are provided with a ducted fan, which is used to adjust the left and right positions of the robot body.

[0007] The curtain wall cleaning robot body stability control method includes the following steps:

[0008] S1. Obtain the initial pose of the robot;

[0009] S2. After establishing the pose model of the robot based on the conversion relationship between the camera coordinate system and the world coordinate system of the robot, obtain the first pose data of the robot through the pose model;

[0010] S3. After the conversion relationship between the robot-based sensor coordinate system and the world coordinate system is established, a mathematical model of the sensor measurement output data is established, and the actual motion data of the robot is obtained through the mathematical model;

[0011] S4. The second pose data of the robot is obtained according to the motion data;

[0012] S5. The fusion pose of the robot is obtained according to the first pose data and the second pose data;

[0013] S6. The deviation between the fusion pose and the initial pose is calculated;

[0014] S7. The control amount is calculated according to the deviation;

[0015] S8. Based on the control amount, the pose of the robot is adjusted by controlling the duct fan.

[0016] The curtain cleaning robot body stable control method provided by the application accurately obtains the pose of the robot by fusing camera vision and sensor perception data, calculates the control amount according to the pose deviation, and then controls the duct fan to adjust the pose of the robot, effectively solving the problem of unstable robot body caused by external disturbance during the operation of the curtain cleaning robot, and improving the cleaning effect and operation quality.

[0017] Further, the motion data includes angular velocity and acceleration;

[0018] The specific expression of the mathematical model is:

[0019] ;

[0020] ;

[0021] wherein, is the angular velocity measurement value of the robot based on the sensor coordinate system at time t, is the actual angular velocity value of the robot based on the sensor coordinate system at time t, is the angular velocity zero offset at time t, is the measurement noise of the angular velocity at time t, is the acceleration measurement value of the robot based on the sensor coordinate system at time t, is the rotation matrix converted from the world coordinate system to the sensor coordinate system, is the actual acceleration value of the robot based on the world coordinate system at time t, is the gravitational acceleration constant of the robot based on the world coordinate system, is the acceleration zero offset at time t, is the measurement noise of the acceleration at time t, indicates the sensor coordinate system, represents a world coordinate system.

[0022] Further, the specific steps in step S4 include:

[0023] S41. Calculate the second pose data according to the following formula:

[0024] ;

[0025] ;

[0026] ;

[0027] ;

[0028] ;

[0029] ;

[0030] wherein, is the position of the robot from the sensor coordinate system to the world coordinate system when the camera collects the k+1th frame of image, k+1 represents the k+1th frame of image collected by the camera, is the position of the robot from the sensor coordinate system to the world coordinate system when the camera collects the kth frame of image, k represents the kth frame of image collected by the camera, is the velocity of the robot from the sensor coordinate system to the world coordinate system when the camera collects the kth frame of image, is the time difference between the kth frame of image and the k+1th frame of image collected by the camera, is the sensor attitude angle at s time, is the acceleration zero offset at s time, represents the integral with respect to s, represents the integral with respect to t, s and t are both time, is the collection time when the camera collects the k+1th frame of image, is the collection time when the camera collects the kth frame of image, is the velocity of the robot from the sensor coordinate system to the world coordinate system when the camera collects the k+1th frame of image, is the sensor attitude angle at t time, is the acceleration zero offset at t time, is the attitude angle of the robot from the sensor coordinate system to the world coordinate system when the camera collects the k+1th frame of image, is the attitude angle of the robot from the sensor coordinate system to the world coordinate system when the camera collects the kth frame of image, the second pose data includes and .

[0031] Further, the specific steps in step S5 include:

[0032] S51. Obtain a predicted pose according to the second pose data;

[0033] S52. Calculate a residual according to the predicted pose and the first pose data;

[0034] S53. Obtain a fused pose of the robot according to the residual.

[0035] Further, the specific steps in step S51 include:

[0036] The predicted pose is calculated according to the following formula:

[0037] ;

[0038] ;

[0039] wherein, is a predicted position of the robot from the camera coordinate system to the world coordinate system when the camera collects the k+1th frame of image, is a rotation matrix from the sensor coordinate system to the world coordinate system, is a position of the robot from the camera coordinate system to the sensor coordinate system when the camera collects the k+1th frame of image, is a predicted attitude angle of the robot from the camera coordinate system to the world coordinate system when the camera collects the k+1th frame of image, is an attitude angle of the robot from the camera coordinate system to the sensor coordinate system when the camera collects the k+1th frame of image.

[0040] Further, the specific steps in step S7 include:

[0041] S71. Obtain an internal disturbance of the robot;

[0042] S72. Calculate a control amount according to the internal disturbance and the bias.

[0043] Further, the specific steps in step S8 include:

[0044] S81. Based on the control amount, determine the rotational speed of the left and right duct fans respectively according to a preset duct fan thrust reference table, and adjust the pose of the robot by controlling the rotational speed of the duct fan.

[0045] In a second aspect, the present application provides a curtain wall cleaning robot body stabilizing control device, which is applied to a control system of a curtain wall cleaning robot, and the left and right sides of the curtain wall cleaning robot are provided with duct fans, which are used to adjust the left and right positions of the robot body.

[0046] The curtain wall cleaning robot body stabilizing control device comprises:

[0047] The first acquisition module is configured to acquire an initial pose of the robot.

[0048] The second acquisition module is configured to, after establishing a pose model of the robot based on a conversion relationship between a camera coordinate system of the robot and a world coordinate system, obtain first pose data of the robot through the pose model.

[0049] The third acquisition module is configured to, after establishing a mathematical model of sensor measurement output data based on a conversion relationship between a sensor coordinate system of the robot and the world coordinate system, obtain actual motion data of the robot through the mathematical model.

[0050] The first calculation module is configured to obtain second pose data of the robot according to the motion data.

[0051] The second calculation module is configured to obtain a fused pose of the robot according to the first pose data and the second pose data.

[0052] The third calculation module is configured to calculate a deviation between the fused pose and the initial pose.

[0053] The fourth calculation module is configured to calculate a control quantity according to the deviation.

[0054] The control module is configured to adjust the pose of the robot by controlling the duct fan based on the control quantity.

[0055] The curtain wall cleaning robot body stabilizing control device provided by the application improves the accuracy and robustness of pose estimation through multi-source data fusion, and performs accurate control quantity calculation based on this, and then adjusts the robot body in real time through the duct fan to ensure that it can still work stably in a complex environment.

[0056] In a third aspect, the application provides an electronic device comprising a processor and a memory, wherein the memory stores computer readable instructions, and when the computer readable instructions are executed by the processor, the steps of the curtain wall cleaning robot body stabilizing control method provided in the first aspect are executed.

[0057] In a fourth aspect, the application provides a computer readable storage medium, which stores a computer program, and when the computer program is executed by a processor, the steps of the curtain wall cleaning robot body stabilizing control method provided in the first aspect are executed.

[0058] From the above, the curtain wall cleaning robot body stable control method provided by the application, by setting the duct fan on the left and right sides of the curtain wall cleaning robot, and adjusting the left and right positions of the robot body by using the fan, effectively solves the problem that the curtain wall cleaning robot is unstable due to external disturbance in the prior art, and further affects the cleaning effect and operation quality. Specifically, the method first acquires the initial pose of the robot, and obtains first pose data and second pose data through camera vision and IMU sensor sensing respectively. Then, the two kinds of pose data are fused to obtain a more accurate robot fusion pose. By calculating the deviation between the fusion pose and the initial pose, and calculating the control amount according to the deviation, the duct fan is finally controlled accurately based on the control amount, so as to adjust the pose of the robot. This fusion of visual and sensor sensing pose estimation method overcomes the limitations of single sensor in complex environment, improves the accuracy and robustness of pose estimation. Through accurate pose control, the robot can effectively resist external disturbance, keep the body stable, ensure that the execution component of the cleaning device is accurately aligned with the area to be cleaned, thereby significantly improving the uniformity and thoroughness of curtain wall cleaning, meeting the higher cleaning degree requirement, and effectively solving the technical problems of incomplete cleaning and poor operation quality in the prior art.

[0059] Other features and advantages of the present application will be set forth in the following description, and in part will become apparent from the description, or can be learned by practice of the present application according to the embodiments. The objects and other advantages of the present application can be achieved and obtained by the structure particularly pointed out in the written description and the accompanying drawings. BRIEF DESCRIPTION OF DRAWINGS

[0060] Figure 1 A flow chart of the curtain wall cleaning robot body stable control method provided by the embodiment of the present application.

[0061] Figure 2 A structure schematic view of the curtain wall cleaning robot body stable control device provided by the embodiment of the present application.

[0062] Figure 3 A structure schematic view of the electronic device provided by the embodiment of the present application.

[0063] Label explanation:

[0064] 100, first acquisition module; 200, second acquisition module; 300, third acquisition module; 400, first calculation module; 500, second calculation module; 600, third calculation module; 700, fourth calculation module; 800, control module; 13, electronic device; 1301, processor; 1302, memory; 1303, communication bus. DETAILED DESCRIPTION

[0065] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0066] It should be noted that similar reference numerals and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures. Furthermore, in the description of this invention, terms such as "first," "second," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0067] Please refer to Figure 1 , Figure 1 This is a flowchart of a method for stabilizing the body of a curtain wall cleaning robot. This method is applied to the control system of the curtain wall cleaning robot, which has ducted fans installed on both its left and right sides. These fans are used to adjust the left and right positions of the robot's body.

[0068] The method for stabilizing the body of a curtain wall cleaning robot includes the following steps:

[0069] S1. Obtain the robot's initial pose; acquire a current image using a camera as a reference image for straight-line feature matching, and use the robot's pose at the time of acquiring the reference image as the initial pose (the initial pose includes the initial position, denoted as...). Subsequent pose changes are all relative to the position changes of the reference image;

[0070] S2. After establishing the robot's pose model based on the transformation relationship between the robot's camera coordinate system and world coordinate system, the robot's first pose data is obtained through the pose model; the first pose data is pose estimation based on camera vision; specifically, the pose estimation is expressed as:

[0071] ;

[0072] in, Indicates the camera coordinate system. Indicates the world coordinate system. for Pose estimation for a robot that transitions from camera coordinate system to world coordinate system. is the position at which the robot converts from the camera coordinate system to the world coordinate system at time t, is the angle of pose at which the robot converts from the camera coordinate system to the world coordinate system at time t; is the acquisition time of the kth image in the adjacent two image frames acquired by the camera;

[0073] S3. After the conversion relationship between the robot-based sensor coordinate system and the world coordinate system is established, the mathematical model of the sensor measurement output data and the pose model of the robot are established, the actual motion data of the robot is obtained through the mathematical model, and the pose data of the robot is obtained through the pose model;

[0074] S4. The second pose data of the robot is obtained according to the motion data and the pose data; the second pose data is a pose estimation based on sensor perception; the sensor is an IMU sensor;

[0075] S5. The fusion pose of the robot (fusion of camera vision and sensor perception) is obtained according to the first pose data and the second pose data;

[0076] S6. The deviation between the fusion pose and the initial pose is calculated; specifically, the fusion pose includes a fusion position, and the deviation is calculated according to the following formula:

[0077] ;

[0078] wherein, is the deviation at time t, is the fusion position at time t; is the acquisition time of the k+1th image in the adjacent two image frames acquired by the camera;

[0079] S7. The control quantity is calculated according to the deviation;

[0080] S8. Based on the control quantity, the pose of the robot is adjusted by controlling the duct fan.

[0081] The curtain cleaning robot body stability control method proposed in the present application aims to realize accurate estimation and stable control of the robot body pose by fusing camera vision and sensor perception data, thereby effectively solving the problem of unstable robot body caused by external disturbance during the operation of the curtain cleaning robot, and improving the cleaning effect and operation quality. The method improves the accuracy and robustness of pose estimation through multi-source data fusion, and performs accurate control quantity calculation based on this, and then adjusts the robot body in real time through the duct fan, so that it can still maintain stable operation in complex environment.

[0082] The curtain wall cleaning robot body stability control method proposed in the present application is applied to the control system of the curtain wall cleaning robot. In the control system, a duct fan is arranged on the left and right sides of the curtain wall cleaning robot. The duct fan is a device capable of generating thrust, which sucks air into the duct and discharges it from the duct through the high-speed rotating blades, thereby generating a reaction force. In the present application, the duct fan is used to adjust the left and right positions of the robot body to offset the influence of external disturbances on the stability of the robot body.

[0083] Specifically, the curtain wall cleaning robot body stability control method of the present application comprises the following steps:

[0084] In step S1, the initial pose of the robot needs to be obtained. The initial pose is the basis for the robot to calculate the subsequent pose change. There are various ways to obtain the initial pose. For example, the robot can be placed at a predetermined starting position by manual operation, and the pose data at this time is recorded as the initial pose. Alternatively, the positioning system inside the robot can be used to automatically record the current pose of the robot as the initial pose when the robot is started. As a preferred embodiment, the present application acquires a current image as a straight line feature matching reference image through a camera, and records the pose of the robot when the reference image is acquired as the initial pose. The subsequent pose change is relative to the position change of the reference image.

[0085] In step S2, the first pose data of the robot needs to be obtained. The first pose data is the pose estimation based on camera vision. To achieve this goal, it is necessary to first establish a pose model of the robot, which is based on the conversion relationship between the camera coordinate system C and the world coordinate system W. The camera coordinate system C is a right-handed coordinate system with the camera optical center as the origin and the camera optical axis as the Z axis. The world coordinate system W is a global coordinate system used to describe the robot and its environment. The pose model describes the position and attitude of the camera coordinate system C relative to the world coordinate system W. For example, the pose model can be established by calibrating the camera intrinsic and extrinsic parameters. After establishing the pose model, the first pose data of the robot can be obtained through the pose model.

[0086] In step S3, the actual motion data and pose data of the robot need to be obtained. To achieve this goal, first, a mathematical model of the sensor measurement output data and a pose model of the robot need to be established, which are based on the transformation relationship between the sensor coordinate system B and the world coordinate system W. The sensor coordinate system B is a coordinate system with the center of the sensor (e.g., IMU sensor) as the origin. The mathematical model describes the relationship between the sensor measurement output data (e.g., angular velocity and acceleration) and the actual motion data of the robot. The pose model describes the change of the pose of the robot. For example, these models can be established by calibrating the sensor. After establishing the mathematical model and the pose model, the actual motion data of the robot can be obtained through the mathematical model, and the pose data of the robot can be obtained through the pose model.

[0087] In step S4, the second pose data of the robot is obtained based on the motion data and the pose data. The second pose data is a pose estimation based on sensor perception. The sensor can be an IMU sensor. The IMU sensor can measure the angular velocity and acceleration of the robot, which can be used to calculate the change of the pose of the robot. For example, by integrating the IMU sensor data, the position, velocity and attitude angle of the robot can be obtained, and then the second pose data can be obtained.

[0088] In step S5, the fusion pose of the robot is obtained based on the first pose data and the second pose data. The fusion pose is the result of fusing camera vision and sensor perception, which combines the advantages of the two sensors, thereby improving the accuracy and robustness of pose estimation. For example, Kalman filtering, extended Kalman filtering or unscented Kalman filtering algorithms can be used to fuse the first pose data and the second pose data, thereby obtaining a more accurate fusion pose.

[0089] In step S6, the deviation between the fusion pose and the initial pose is calculated. The deviation is an indicator to measure the difference between the current pose of the robot and the expected pose (i.e., the initial pose).

[0090] In step S7, the control amount is calculated based on the deviation. The control amount is an instruction for adjusting the output of the ducted fan to restore the robot body to a stable state. For example, a PID controller can be used to calculate the corresponding control amount based on the size and trend of the deviation.

[0091] In step S8, based on the control amount, the pose of the robot is adjusted by controlling the ducted fan. The ducted fan can generate different sizes and directions of thrust by changing its speed or direction, thereby exerting force on the robot body to adjust its pose. For example, when the robot body deviates to the left, the speed of the right ducted fan can be increased or the speed of the left ducted fan can be decreased, thereby generating a rightward thrust to restore the robot body to the center position.

[0092] The curtain cleaning robot body stability control method proposed in the present application realizes accurate estimation of the robot body pose by fusing camera vision and sensor perception data. Specifically, the method first obtains the initial pose of the robot as a reference, then obtains first pose data through camera vision and second pose data through IMU sensor perception. The two kinds of pose data are effectively fused to obtain more accurate and robust fused pose. Based on the deviation between the fused pose and the initial pose, the system can accurately calculate the required control amount. Finally, through the control of the duct fan, the robot body can be adjusted in real time to offset the influence of external disturbances, ensuring its stability during the cleaning operation.

[0093] Compared with the lack of effective solutions in the prior art, the core innovation of the present application lies in introducing a multi-source data fusion pose estimation method and applying it to the body stability control of the curtain cleaning robot. Traditional methods may rely only on a single sensor (such as vision or IMU), which is easily affected by noise or occlusion in complex environments, leading to inaccurate pose estimation and affecting the control effect. By fusing camera vision and IMU sensor data, the present application can effectively compensate for the shortcomings of a single sensor. For example, camera vision may fail when there is insufficient texture feature or when light changes drastically, while IMU sensors are easily affected by cumulative errors. By combining the advantages of both, the present application can obtain more accurate and robust robot pose information. The accuracy of this fused pose estimation directly improves the accuracy of subsequent control amount calculation, enabling the duct fan to more effectively adjust the robot body, thereby significantly improving the body stability of the curtain cleaning robot under external disturbances and ensuring the quality and efficiency of the cleaning operation.

[0094] In some embodiments, the motion data includes angular velocity and acceleration;

[0095] The specific expression of the mathematical model is:

[0096] ;

[0097] ;

[0098] wherein, is the angular velocity measurement value of the robot based on the sensor coordinate system at time t, is the actual angular velocity value of the robot based on the sensor coordinate system at time t, is the angular velocity zero offset at time t, is the measurement noise of the angular velocity at time t, is the acceleration measurement value of the robot based on the sensor coordinate system at time t, RwB, the rotation matrix for converting from the world coordinate system to the sensor coordinate system, at, the actual acceleration of the robot at time t based on the world coordinate system, g, the gravitational acceleration constant for the robot based on the world coordinate system, b, the acceleration bias at time t, n, the measurement noise of acceleration at time t, B represents the sensor coordinate system, W represents the world coordinate system, and the measurement noise in subsequent calculations is implicitly handled, so it does not appear in the subsequent formulas.

[0099] Specifically, motion data refers to physical quantities used to describe the instantaneous motion state of a robot, which in this embodiment specifically refers to angular velocity and acceleration. Angular velocity is a physical quantity that describes the speed of rotation of an object, while acceleration is a physical quantity that describes the rate of change of an object's speed. These data are usually measured by sensors such as inertial measurement units (IMU).

[0100] where the mathematical model is a mathematical expression used to convert sensor measurement output data into actual motion data of the robot. This model takes into account possible error sources in the sensor measurement process, such as bias and measurement noise.

[0101] Specifically, b, the angular velocity sensor bias at time t, which is a non-zero value that the sensor may still output in a stationary state. n, the random noise introduced in the angular velocity measurement process at time t (which can be determined through experimental testing). By subtracting the bias and noise from the measured value, the actual angular velocity can be more accurately obtained.

[0102] Similarly, RwB, the rotation matrix for converting from the world coordinate system W to the sensor coordinate system B, used to convert the acceleration in the sensor coordinate system to the sensor coordinate system. b, the acceleration sensor bias at time t, n, the random noise introduced in the acceleration measurement process at time t (which can be determined through experimental testing).

[0103] The solution in this application defines motion data as angular velocity and acceleration, and introduces the aforementioned mathematical model. This enables precise correction and processing of measurement output data from IMU sensors and other sources in step S3. Specifically, sensor measurements often contain errors such as bias and noise. Directly using these raw measurements would lead to inaccurate acquisition of the robot's actual motion data. The aforementioned mathematical model effectively removes the influence of bias and measurement noise from sensor measurements and performs gravity compensation on acceleration data, thereby obtaining more realistic and reliable actual angular velocity and acceleration of the robot. This precisely processed motion data forms the basis for calculating the robot's second pose data (based on sensor perception) in the subsequent step S4, and is crucial for improving the accuracy of overall pose estimation.

[0104] Through the above technical solution, this application can significantly improve the accuracy of acquiring actual robot motion data. By considering and compensating for zero bias, noise, and the effects of gravity in sensor measurements, the obtained angular velocity and acceleration data are closer to the robot's actual motion state. This high-precision motion data provides a solid foundation for subsequent pose estimation based on sensor perception, thereby improving the overall accuracy and robustness of the robot's body stability control. This enables the curtain wall cleaning robot to maintain a more stable posture and position in complex and changing environments, effectively avoiding control deviations caused by inaccurate pose estimation.

[0105] In some embodiments, the attitude data includes attitude angles;

[0106] The specific expression for the pose model is:

[0107] ;(Formula 1);

[0108] in, The pose angle of the robot when it transforms from the sensor coordinate system to the world coordinate system to acquire the (k+1)th frame image for the camera. It is the identity matrix for quaternion multiplication. express Right multiplication of a matrix by a quaternion The actual value of the robot's angular velocity based on the sensor coordinate system when the camera acquires the k-th frame image ( The calculation can be directly referenced. (calculation) The time difference between the (k+1)th frame and the kth frame acquired by the camera. The pose angle of the robot when it transforms from the sensor coordinate system to the world coordinate system to acquire the k-th frame image for the camera.

[0109] Specifically, the attitude data refers to information describing the direction and orientation of the robot body in a three-dimensional space, and the core element is the attitude angle. The attitude angle can be understood as the rotation angle of the robot relative to a certain reference coordinate system (for example, the world coordinate system W), which is usually represented in the form of Euler angles, quaternions or rotation matrices. In this application, the attitude angle is represented in the form of quaternions, which can effectively avoid the gimbal lock problem and simplify the rotation operation. Among them, the attitude model is a mathematical relationship used to describe and predict the change of the robot attitude over time. The specific expression of the above attitude model aims to estimate the attitude angle of the robot after a short time interval based on the angular velocity measured by the sensor (for example, the IMU sensor) at time t. This model uses the principle of quaternion calculus, where I represents the unit matrix of quaternion multiplication, represents the angular velocity corresponding quaternion right multiplication matrix, which is used to apply the rotation effect of the angular velocity to the current attitude quaternion . Usually a very small time step to ensure the accuracy of the approximate calculation. In this way, the real-time attitude of the robot can be iteratively updated and estimated based on continuous angular velocity measurements.

[0110] It should be noted that the result of formula 1 of the embodiment is the approximate result of formula 2 below. Although the calculation using formula 1 is fast but low in accuracy, it can be used in some cases where the accuracy requirement is not high.

[0111] The scheme of the present application defines the attitude data as the attitude angle and provides a specific mathematical expression of the attitude model, so that the robot control system can accurately calculate and update the attitude of the robot based on the motion data (especially the angular velocity) output by the sensor (for example, the IMU sensor). In the above curtain cleaning robot body stabilization control method, step S3 obtains the robot attitude data by the attitude model. The attitude model takes advantage of the advantages of quaternions in representing three-dimensional rotations, and obtains the attitude angle by performing a compound operation on the attitude angle at time t and the attitude change caused by the angular velocity within a time interval . This quaternion-based attitude updating mechanism can effectively avoid the gimbal lock problem that may occur in traditional Euler angle representation, ensuring the continuity and stability of attitude estimation. At the same time, by introducing the right multiplication matrix of the angular velocity and the time difference, the updating process of the attitude can accurately reflect the actual rotation motion of the robot, providing accurate attitude information for subsequent pose fusion and stable control.

[0112] By the technical solution, it is clear that the attitude data is an attitude angle, and a specific mathematical expression of the attitude model is provided, so that the calculation process of the robot attitude is more accurate and reliable. Compared with only mentioning the "attitude model" generally, the application provides a specific quaternion attitude updating formula, which not only improves the mathematical rigor of attitude estimation, but also effectively avoids the singularity problem that may be caused by the traditional attitude representation method (such as Euler angle), thereby ensuring the continuity and stability of the attitude data when the robot performs complex motion. This is crucial for the stable control of the robot body in a complex external environment (such as wind disturbance), and can provide high-quality attitude input for subsequent pose fusion and control quantity calculation, thereby improving the accuracy and robustness of the entire robot body stable control method.

[0113] In some embodiments, the specific steps in step S4 include:

[0114] S41. Calculate the second pose data (for the k+1th frame of image) according to the following formula:

[0115] ;

[0116] ;

[0117] ;

[0118] ;

[0119] ;

[0120] ; (Formula 2)

[0121] wherein, is the position of the robot from the sensor coordinate system to the world coordinate system when the camera collects the k+1th frame of image, k+1 represents the k+1th frame of image collected by the camera, is the position of the robot from the sensor coordinate system to the world coordinate system when the camera collects the kth frame of image, k represents the kth frame of image collected by the camera, is the velocity of the robot from the sensor coordinate system to the world coordinate system when the camera collects the kth frame of image, is the time difference between the kth frame of image and the k+1th frame of image collected by the camera, is the sensor attitude angle at s, is the acceleration zero offset at s, represents the integral with respect to s, represents the integral with respect to t, s and t are both time, ​This refers to the acquisition time when the camera acquires the (k+1)th frame of the image. The acquisition time is when the camera acquires the k-th frame of the image. The speed at which the robot transforms from the sensor coordinate system to the world coordinate system when the camera acquires the (k+1)th frame image. Let be the sensor attitude angle at time t. The acceleration at time t is zero bias. The pose angle of the robot when it transforms from the sensor coordinate system to the world coordinate system to acquire the (k+1)th frame image for the camera. The second pose data includes the robot's pose angles when it transforms from the sensor coordinate system to the world coordinate system during the acquisition of the k-th frame image by the camera. and .

[0122] Specifically, step S41 above aims to improve the IMU sensor (inertial measurement unit) performance at time intervals. The measurement data within the specified time period is integrated to accurately calculate the robot's position, velocity, and attitude angles during that time period. Among these, position... The calculation takes into account the initial position. Initial velocity And in Acceleration during the time period (After zero bias) After correcting and transforming to the world coordinate system, subtract the gravitational acceleration; The calculation can be directly referenced. The position is obtained by double integration (calculation). Velocity The calculation is based on the initial velocity. Based on this, acceleration (Also after zero bias) After correction and transformation to the world coordinate system, and then subtracting the gravitational acceleration, an integral is performed to obtain the attitude angle. The calculation is performed by adjusting the initial attitude angle. With angular velocity (After zero bias) (Correction) and After performing integration The Kronecker product (direct product) is performed to obtain the data. These calculations involve transformations between the sensor coordinate system B and the world coordinate system W to ensure data accuracy and consistency. This represents the time difference between the k-th frame and the (k+1)-th frame in two adjacent image frames captured by the camera. This indicates that the integration of the sensor data is performed between camera frame rates to provide continuous pose estimation.

[0123] The scheme of the present application can effectively convert the high-frequency measurement data (including angular velocity and acceleration) of the IMU sensor in a short time interval into the position, velocity and attitude angle information of the robot through the integral calculation defined in detail in step S41. This pose estimation method based on sensor data integration takes advantage of the sensitivity of the IMU sensor to instantaneous motion changes, and can provide continuous and relatively accurate robot motion state information even in the case of low camera vision data update frequency or temporary failure. By double integrating the acceleration to obtain the position, integrating the acceleration once to obtain the velocity, and integrating the angular velocity to obtain the attitude angle, accurate calculation from the original sensor measurement value to the kinematic state of the robot is realized. This method provides reliable sensor perception pose data for subsequent pose fusion, thereby enhancing the robustness of the robot body stability control.

[0124] Through the above technical scheme, the specific calculation process of the second pose data is clarified, that is, the position, velocity and attitude angle are obtained by integrating the IMU sensor data. This accurate mathematical model and calculation step makes the sensor-based pose estimation more accurate and reliable. Compared with the general mention of "obtaining the second pose data of the robot according to the motion data and the attitude data", the present scheme effectively avoids the uncertainty in the pose estimation process by providing specific integral formulas, thereby improving the accuracy and continuity of the pose data. Especially when the camera vision data may be affected by environmental light, occlusion and other factors, this detailed sensor data integration method can provide stable pose information, thereby providing a solid foundation for subsequent pose fusion and body stability control, and significantly improving the stability and control accuracy of the curtain cleaning robot in complex environments.

[0125] In some embodiments, the specific steps in step S5 include:

[0126] S51. Obtain a predicted pose (i.e., predict the pose data of the robot obtained from camera vision using sensor data) according to the second pose data; specifically, the predicted pose includes a predicted position and a predicted attitude angle, and the predicted pose is calculated according to the following formula:

[0127] ;

[0128] ;

[0129] wherein, is the predicted position of the robot converted from the camera coordinate system to the world coordinate system when the camera collects the k+1th frame of image, is the rotation matrix converted from the sensor coordinate system to the world coordinate system, is the position of the robot converted from the camera coordinate system to the sensor coordinate system when the camera collects the k+1th frame of image, is the predicted pose angle of the robot from the camera coordinate system C to the world coordinate system W when the camera captures the k+1th frame of image, is the pose angle of the robot from the camera coordinate system C to the sensor coordinate system B when the camera captures the k+1th frame of image;

[0130] S52. According to the predicted pose and the first pose data, a residual error is calculated (i.e. comparing the predicted pose data with the first pose data actually calculated based on camera vision); specifically, the residual error is calculated according to the following formula:

[0131] ;

[0132] wherein, is the residual error when the camera captures the k+1th frame of image, is the position of the robot from the camera coordinate system C to the world coordinate system W when the camera captures the k+1th frame of image;

[0133] S53. According to the residual error, a fused pose of the robot is obtained; specifically, the fused pose includes a fused position, which is calculated according to the following formula:

[0134] ;

[0135] wherein, is the fused position when the camera captures the k+1th frame of image, is a preset weight coefficient.

[0136] Specifically, in step S51, the predicted pose is obtained to make a forward prediction of the camera vision data (first pose data) by using the sensor data (second pose data). The predicted position is obtained by converting the robot position in the sensor coordinate system B by the rotation matrix between the sensor coordinate system B and the world coordinate system W and the position of the camera coordinate system C to the sensor coordinate system B Similarly, the predicted pose angle is obtained by performing a quaternion multiplication operation on the pose angle in the sensor coordinate system B and the pose angle of the camera coordinate system C to the sensor coordinate system B The purpose of this step is to continuously estimate the pose of the robot by using the high-frequency sensor data between the vision data updates, so as to make up for the sparseness of the vision data in time.

[0137] Further, in step S52, the calculation of the residual error is to compare the actual position obtained based on camera vision with the predicted position obtained by the sensor data The comparison is made. The residual reflects the inconsistency between visual measurement and inertial prediction, which is the key information in the fusion algorithm for correcting sensor drift and optimizing pose estimation.

[0138] On this basis, in step S53, the fused position is obtained by correcting the visual measurement position and the residual by a preset weight coefficient . The weight coefficient is used to balance the weight of visual measurement and residual correction, and its value can be adjusted according to the actual application scene and sensor characteristics to optimize the accuracy and stability of the fusion result. The fused position represents the position of the robot after comprehensive correction of visual and sensor data.

[0139] The scheme of the present application effectively solves the problems of time inconsistency between visual data and sensor data, noise characteristic difference and sensor drift accumulation in the traditional pose fusion method by introducing prediction, residual calculation and weighted correction mechanism based on residual. Specifically, step S51 uses high-frequency sensor data to continuously predict the pose of the robot, providing smooth and continuous pose estimation for the period between visual data updates, thereby making up for the limitation of low visual data update frequency. Then, step S52 calculates the residual between the predicted pose and the actual visual pose, quantifying the difference between the two data sources, which contains information of sensor drift and visual measurement error. Finally, step S53 uses the residual to correct the visual pose, so that the final fused pose not only inherits the global accuracy of visual data, but also effectively suppresses the sensor drift and improves the real-time and smoothness of pose estimation through prediction and residual correction of sensor data. This mechanism enables the fused pose to more robustly cope with the instantaneous errors or long-term drifts that may exist in single sensor data, thereby providing more accurate and reliable pose input for the body stability control of the curtain cleaning robot.

[0140] Through the above technical scheme, the present application can significantly improve the accuracy and stability of the body pose estimation of the curtain cleaning robot. Compared with simple pose fusion, the present application introduces sensor data prediction of visual pose, calculation of residual between predicted and actual visual pose, and weighted correction of visual pose using the residual, effectively overcoming the inherent defects of sparse visual positioning and inertial sensor drift. Therefore, even in an environment where visual features are not obvious or sensor data has noise, the robot can still obtain high-precision and low-delay fused pose, thereby providing a more reliable basis for subsequent control quantity calculation and duct fan adjustment, and finally ensuring that the body stability control performance of the curtain cleaning robot in complex working environment is greatly improved, and the control error and operation risk caused by inaccurate pose estimation are reduced.

[0141] In some embodiments, the specific steps in step S7 include:

[0142] S71. Obtain the internal disturbance of the robot; specifically, calculate the internal disturbance according to the following formula:

[0143] ;

[0144] Wherein, is the internal disturbance when the camera collects the k+1th frame of image, is the external environmental disturbance when the camera collects the k+1th frame of image, is the internal gyroscopic torque of the robot;

[0145] S72. Calculate the control variable according to the internal disturbance and the deviation; specifically, calculate the control variable according to the following formula:

[0146] ;

[0147] ;

[0148] Wherein, is the control variable when the camera collects the k+1th frame of image, is the deviation when the camera collects the k+1th frame of image, is the known control variable obtained from the sensor when the camera collects the k+1th frame of image, is the torque coefficient of the duct.

[0149] Specifically, the internal disturbance refers to the sum of all non-controlled forces or torques acting on the robot body, which includes external environmental disturbances and internal gyroscopic torques of the robot. The external environmental disturbance can be understood as a random or non-random force from the external environment of the robot that affects the pose of the robot, such as wind force, water flow impact, uneven surface contact force, etc. These disturbances can be measured by additional environmental sensors (such as wind speed sensors, pressure sensors) or estimated by disturbance observers. The internal gyroscopic torque of the robot refers to the reaction torque generated by the internal moving parts of the robot (for example, the rotation of the cleaning brush, the swing of the mechanical arm, the rotation of the duct fan itself). These torques can usually be calculated in advance or estimated in real time according to the internal structure and kinematics model of the robot.

[0150] Wherein, the control variable is a compensation variable for compensating for the left-right deviation of the robot (its positive and negative values can represent the direction of pose adjustment, for example, when the compensation variable is positive, it means that the robot pose needs to be adjusted to the left, and when the compensation variable is negative, it means that the robot pose needs to be adjusted to the right), which is used to drive the duct fan to adjust the pose of the robot. In this formula, represents the deviation between the fusion pose when the camera captures the k+1 frame of image and the initial pose, which is the main error signal that the control system needs to eliminate. For the known control variable obtained from the sensor, it can be understood as a control input preset by the system or obtained through feedforward control, which is used to realize a specific motion or compensate for known system dynamics. The torque coefficient of the duct is a proportional coefficient for the duct fan to convert the control signal into actual thrust or torque, and its value is usually calibrated by experiment or determined by the design parameters of the duct fan. By comprehensively considering the deviation, the known control variable and the internal disturbance, and dividing by the torque coefficient of the duct, a more accurate and comprehensive control variable can be obtained.

[0151] The scheme of the present application explicitly includes the internal disturbance in the calculation of the control variable, so that the control system can more comprehensively perceive and respond to the dynamic environment faced by the robot. Traditionally, the control system may only calculate the control variable according to the pose deviation, which may cause a lag or inaccuracy in the control response when there are significant internal disturbances or external environmental disturbances. By introducing the internal disturbance, the control system can actively compensate for these disturbances, for example, when external wind or internal components generate gyroscopic torque, the control variable will be immediately adjusted to offset the impact of these disturbances on the robot pose, so as to pre-compensate or real-time compensate before the pose deviation fully appears. This active compensation mechanism significantly improves the robustness and response speed of the control system.

[0152] By the above technical scheme, the internal disturbance is included in the calculation of the control variable, so that the curtain cleaning robot body stable control method can more accurately and robustly cope with complex working environment. Specifically, by considering external environmental disturbances and internal gyroscopic torque of the robot, the control system can compensate for these disturbances in advance or in real time, significantly improving the stability of the robot body under the influence of wind, uneven surface contact or internal motion induced torque changes. Compared with the scheme of controlling only based on the pose deviation, the scheme of the present application can effectively reduce the control lag and oscillation caused by not considering the disturbance, thereby improving the smoothness and work efficiency of the robot in actual work, and reducing the risk of accidents.

[0153] In some embodiments, the specific steps in step S8 include:

[0154] S81. Based on the control variable, determine the rotation speed of the left and right duct fans respectively according to the preset duct fan thrust reference table, and adjust the pose of the robot by controlling the rotation speed of the duct fan.

[0155] The preset duct fan thrust reference table can be understood as a data structure that stores the corresponding relationship between the duct fan speed and the thrust generated thereby. The reference table is usually obtained through experimental calibration or theoretical modeling, and is intended to provide a basis for the control system to convert abstract control quantities into specific physical execution parameters. For example, the reference table can be a two-dimensional lookup table, with one column being the speed of the duct fan (such as RPM) and the other column being the thrust generated by the duct fan at that speed (such as Newton). Specifically, after receiving the control quantity calculated in step S7, the control system will query or calculate the required speed of the left and right duct fans according to the control quantity in combination with the duct fan thrust reference table. To ensure cleaning effect, when the robot deviates to the left and right, it is required to complete the pose adjustment in a short time, for example, it is required to complete the pose adjustment within 1 second, and the duct fan thrust reference table presets the speed required for different control quantities to complete the pose adjustment within 1 second, if the control quantity indicates that the robot needs to move to the left, the left duct fan may need to reduce the speed or maintain a lower speed, and the right duct fan needs to increase the speed to generate greater thrust (the speed and thrust are proportional, and the duct fan thrust reference table lists the speed and thrust corresponding to different control quantities). By accurately determining and controlling the speed of the left and right duct fans, fine adjustment of the left and right positions of the robot body can be achieved, and the overall pose of the robot can be adjusted.

[0156] The scheme of the present application establishes a direct and quantitative corresponding relationship between the abstract control quantity and the actual physical output (i.e. thrust) of the duct fan by introducing a preset duct fan thrust reference table. When the control system calculates the control quantity according to the deviation between the fused pose and the initial pose, it is no longer simply a vague "control duct fan" instruction, but can accurately determine the speed that the left and right duct fans should reach according to the reference table. The duct fan adjusts the thrust generated by changing the speed, thereby achieving accurate control of the left and right positions of the robot body. This fine control based on speed enables the robot to generate just the right amount of thrust to offset the deviation and adjust to the target pose according to actual needs.

[0157] Through the above technical scheme, since the control quantity can be accurately converted into a specific speed instruction of the duct fan, the duct fan can generate more accurate thrust, effectively avoiding the problem of inaccurate pose adjustment or delayed response caused by vague control instructions. This significantly improves the accuracy and stability of the pose adjustment of the curtain wall cleaning robot, enabling it to recover to a stable state more quickly and accurately when facing external environmental disturbances, thereby improving the efficiency and safety of the cleaning operation.

[0158] Please refer to Figure 2 , Figure 2The application discloses a curtain wall cleaning robot body stabilizing control device, which is applied to a control system of a curtain wall cleaning robot.

[0159] The first acquisition module 100 is used for acquiring an initial pose of the robot.

[0160] The second acquisition module 200 is used for obtaining first pose data of the robot through a pose model after the pose model of the robot is established based on a conversion relationship between a camera coordinate system of the robot and a world coordinate system.

[0161] The third acquisition module 300 is used for obtaining actual motion data of the robot through a mathematical model of sensor measurement output data after the mathematical model is established based on a conversion relationship between a sensor coordinate system of the robot and the world coordinate system.

[0162] The first calculation module 400 is used for obtaining second pose data of the robot according to the motion data.

[0163] The second calculation module 500 is used for obtaining a fused pose of the robot according to the first pose data and the second pose data.

[0164] The third calculation module 600 is used for calculating a deviation between the fused pose and the initial pose.

[0165] The fourth calculation module 700 is used for calculating a control amount according to the deviation.

[0166] The control module 800 is used for adjusting the pose of the robot by controlling the ducted fan based on the control amount.

[0167] Further, the first calculation module 400 is used for executing the following steps when obtaining the second pose data of the robot according to the motion data.

[0168] S41. The second pose data is calculated according to the following formula:

[0169] ;

[0170] ;

[0171] ;

[0172] ;

[0173] ;

[0174] ;

[0175] wherein, is a position of the robot from the sensor coordinate system to the world coordinate system when the k+1th image is captured by the camera, k+1 represents the k+1th image captured by the camera, is a position of the robot from the sensor coordinate system to the world coordinate system when the kth image is captured by the camera, k represents the kth image captured by the camera, is a velocity of the robot from the sensor coordinate system to the world coordinate system when the kth image is captured by the camera, is a time difference between the kth image and the k+1th image captured by the camera, is a sensor attitude angle at s time, is an acceleration bias at s time, represents an integral with respect to s, represents an integral with respect to t, s and t are both time, is a capture time when the k+1th image is captured by the camera, is a capture time when the kth image is captured by the camera, is a velocity of the robot from the sensor coordinate system to the world coordinate system when the k+1th image is captured by the camera, is a sensor attitude angle at t time, is an acceleration bias at t time, is a position of the robot from the sensor coordinate system to the world coordinate system when the k+1th image is captured by the camera, is a position of the robot from the sensor coordinate system to the world coordinate system when the kth image is captured by the camera, the second pose data comprises and .

[0176] Further, the second calculation module 500 is configured to, when obtaining the fusion pose of the robot according to the first pose data and the second pose data, perform:

[0177] S51. obtaining a predicted pose according to the second pose data;

[0178] S52. calculating a residual according to the predicted pose and the first pose data;

[0179] S53. obtaining the fusion pose of the robot according to the residual.

[0180] Further, the fourth calculation module 700 is configured to, when calculating the control quantity according to the deviation, perform:

[0181] S71. obtaining an internal disturbance of the robot;

[0182] S72. Calculate the control quantity according to the internal disturbance and the deviation.

[0183] Further, the control module 800 executes when adjusting the pose of the robot by controlling the ducted fan based on the control quantity:

[0184] S81. Based on the control quantity, determine the rotation speed of the left and right ducted fans respectively according to the preset ducted fan thrust reference table, and adjust the pose of the robot by controlling the rotation speed of the ducted fan.

[0185] Please refer to Figure 3 , Figure 3 A structural schematic diagram of an electronic device provided by an embodiment of the present application, the present application provides an electronic device 13, comprising: a processor 1301 and a memory 1302, the processor 1301 and the memory 1302 are interconnected and communicate with each other through a communication bus 1303 and / or other forms of connection mechanism (not marked), the memory 1302 stores computer readable instructions executable by the processor 1301, when the electronic device runs, the processor 1301 executes the computer readable instructions to execute the curtain cleaning robot body stability control method in any optional implementation manner of the above-mentioned embodiments, to realize the following functions: obtaining the initial pose of the robot; after establishing the pose model of the robot based on the conversion relationship between the camera coordinate system of the robot and the world coordinate system, obtaining the first pose data of the robot through the pose model; after establishing the mathematical model of the sensor measurement output data based on the conversion relationship between the sensor coordinate system of the robot and the world coordinate system, obtaining the actual motion data of the robot through the mathematical model; obtaining the second pose data of the robot according to the motion data; obtaining the fusion pose of the robot according to the first pose data and the second pose data; calculating the deviation between the fusion pose and the initial pose; calculating the control quantity according to the deviation; based on the control quantity, adjusting the pose of the robot by controlling the ducted fan.

[0186] The embodiment of the present application provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to execute the curtain cleaning robot body stability control method in any optional implementation manner of the above embodiment, so as to realize the following functions: obtaining an initial pose of a robot; after a pose model of the robot is established based on a conversion relationship between a camera coordinate system of the robot and a world coordinate system, obtaining first pose data of the robot through the pose model; after a mathematical model of sensor measurement output data is established based on a conversion relationship between a sensor coordinate system of the robot and the world coordinate system, obtaining actual motion data of the robot through the mathematical model; obtaining second pose data of the robot according to the motion data; obtaining a fusion pose of the robot according to the first pose data and the second pose data; calculating a deviation between the fusion pose and the initial pose; calculating a control quantity according to the deviation; and adjusting the pose of the robot by controlling a ducted fan based on the control quantity.

[0187] The computer readable storage medium can be implemented by any type of volatile or nonvolatile storage devices or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk or optical disk.

[0188] In the embodiments of the present application, it should be understood that the disclosed device and method can be implemented in other ways. The device embodiments described above are only schematic. For example, the division of the units is only a logical function division. In actual implementation, other division manners can be used. For example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed coupling or direct coupling or communication connection between each other can be indirect coupling or communication connection through some communication interfaces, devices or units, and can be electrical, mechanical or other forms.

[0189] In addition, the units described as separate components may or may not be physically separate, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed to multiple network units. Part or all of the units can be selected according to actual needs to achieve the purpose of the embodiment scheme.

[0190] Furthermore, each functional module in various embodiments of the present application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0191] In this article, relationship terms such as first and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between the entities or operations.

[0192] The above is only an embodiment of the present application and is not used to limit the protection scope of the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method for stabilizing the body of a curtain wall cleaning robot, applied to the control system of a curtain wall cleaning robot, characterized in that, The curtain wall cleaning robot is equipped with ducted fans on both the left and right sides. The ducted fans are used to adjust the left and right position of the robot body. The method for stabilizing the body of a curtain wall cleaning robot includes the following steps: S1. Obtain the robot's initial pose; S2. After establishing the robot's pose model based on the transformation relationship between the robot's camera coordinate system and the world coordinate system, the robot's first pose data is obtained through the pose model; S3. After establishing a mathematical model for the sensor measurement output data based on the transformation relationship between the robot's sensor coordinate system and the world coordinate system, the actual motion data of the robot is obtained through the mathematical model; the motion data includes angular velocity and acceleration. The specific expression of the mathematical model is: ； ； in, Let t be the angular velocity measurement value of the robot based on the sensor coordinate system. Let be the actual angular velocity of the robot at time t based on the sensor coordinate system. The angular velocity at time t is zero bias. The measurement noise of the angular velocity at time t. Let t be the acceleration measurement value of the robot based on the sensor coordinate system. This is the rotation matrix for transforming from the world coordinate system to the sensor coordinate system. Let be the actual acceleration value of the robot at time t based on the world coordinate system. The gravitational acceleration constant of the robot based on the world coordinate system. The acceleration at time t is zero bias. The measurement noise of acceleration at time t, Indicates the sensor coordinate system. Represents the world coordinate system; S4. Based on the motion data, obtain the robot's second pose data; S5. Based on the first pose data and the second pose data, obtain the fused pose of the robot. Specific steps include: S51. Based on the second pose data, obtain the predicted pose. The specific steps include: The predicted pose is calculated using the following formula: ； ； in, The predicted position of the robot when it transforms from the camera coordinate system to the world coordinate system to acquire the (k+1)th frame image from the camera. The position of the robot when it transforms from the sensor coordinate system to the world coordinate system to capture the (k+1)th frame of the image from the camera. This is the rotation matrix for transforming from the sensor coordinate system to the world coordinate system. The position of the robot when it transforms from the camera coordinate system to the sensor coordinate system when the camera acquires the (k+1)th frame image. The predicted pose angle of the robot when transforming from the camera coordinate system to the world coordinate system during the acquisition of the (k+1)th frame image by the camera. The pose angle of the robot when it transforms from the sensor coordinate system to the world coordinate system to acquire the (k+1)th frame image for the camera. The attitude angle of the robot when it transforms from the camera coordinate system to the sensor coordinate system when the camera acquires the (k+1)th frame image; S52. Calculate the residuals based on the predicted pose and the first pose data; S53. Obtain the robot's fused pose based on the residual; S6. Calculate the deviation between the fused pose and the initial pose; S7. Calculate the control quantity based on the deviation; S8. Based on the control quantity, adjust the robot's posture by controlling the ducted fan.

2. The method for stabilizing the body of a curtain wall cleaning robot according to claim 1, characterized in that, The specific steps in step S4 include: S41. Calculate the second pose data according to the following formula: ； ； ； ； ； ； in, The position of the robot when it transforms from the sensor coordinate system to the world coordinate system during the acquisition of the (k+1)th frame image by the camera, where k+1 represents the (k+1)th frame image acquired by the camera. The position of the robot when it transforms from the sensor coordinate system to the world coordinate system during the acquisition of the k-th frame image by the camera, where k represents the k-th frame image acquired by the camera. The speed at which the robot transforms from the sensor coordinate system to the world coordinate system when the camera acquires the k-th frame image. The time difference between the k-th frame and the (k+1)-th frame captured by the camera. Let be the sensor attitude angle at time s. The acceleration at time s is zero bias. Represents the integral with respect to s. Let represent the integral with respect to t, where s and t are both time intervals. This refers to the acquisition time when the camera acquires the (k+1)th frame of the image. The acquisition time is when the camera acquires the k-th frame of the image. The speed at which the robot transforms from the sensor coordinate system to the world coordinate system when the camera acquires the (k+1)th frame image. Let be the sensor attitude angle at time t. The acceleration at time t is zero bias. The pose angle of the robot when it transforms from the sensor coordinate system to the world coordinate system to acquire the (k+1)th frame image for the camera. The second pose data includes the robot's pose angles when it transforms from the sensor coordinate system to the world coordinate system during the acquisition of the k-th frame image by the camera. and .

3. The method for stabilizing the body of a curtain wall cleaning robot according to claim 1, characterized in that, The specific steps in step S7 include: S71. Obtain the robot's internal disturbances; S72. Calculate the control quantity based on the internal disturbances and deviations.

4. The method for stabilizing the body of a curtain wall cleaning robot according to claim 1, characterized in that, The specific steps in step S8 include: S81. Based on the control input, determine the rotational speed of the left and right duct fans according to the preset duct fan thrust comparison table, and adjust the robot's posture by controlling the rotational speed of the duct fans.

5. A curtain wall cleaning robot body stabilization control device employing the curtain wall cleaning robot body stabilization control method as described in any one of claims 1-4, applied to the control system of a curtain wall cleaning robot, characterized in that, The curtain wall cleaning robot is equipped with ducted fans on both the left and right sides. The ducted fans are used to adjust the left and right position of the robot body. The curtain wall cleaning robot's body stabilization control device includes: The first acquisition module is used to acquire the robot's initial pose; The second acquisition module is used to establish the robot's pose model by transforming the relationship between the robot's camera coordinate system and the world coordinate system, and then obtain the robot's first pose data through the pose model. The third acquisition module is used to establish a mathematical model of the sensor measurement output data after establishing the transformation relationship between the robot's sensor coordinate system and the world coordinate system, and then obtain the robot's actual motion data through the mathematical model. The first calculation module is used to obtain the robot's second pose data based on the motion data; The second calculation module is used to obtain the robot's fused pose based on the first pose data and the second pose data; The third calculation module is used to calculate the deviation between the fused pose and the initial pose. The fourth calculation module is used to calculate the control quantity based on the deviation; The control module is used to adjust the robot's posture by controlling the ducted fan based on control variables.

6. An electronic device, characterized in that, It includes a processor and a memory, the memory storing computer-readable instructions, which, when executed by the processor, perform the steps of the curtain wall cleaning robot body stabilization control method as described in any one of claims 1-4.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it performs the steps in the body stabilization control method for the curtain wall cleaning robot as described in any one of claims 1-4.

Images