Multi-sensor intelligent motion posture recognition method of photovoltaic intelligent cleaning robot

Abstract

This invention discloses a multi-sensor intelligent motion attitude recognition method for a photovoltaic intelligent cleaning robot. First, a three-axis accelerometer and a three-axis gyroscope are added to the photovoltaic intelligent cleaning robot to construct a multi-sensor intelligent motion attitude recognition system. Then, the robot's attitude data is calculated and updated in real time using an extended Kalman filter algorithm. The gravity components on the XYZ axes of the cleaning robot are measured to obtain the robot's heading angle, pitch angle, and roll angle. Finally, the measured values ​​of the heading angle, pitch angle, and roll angle are converted into attitude quaternions in real time and participated in each iteration of the extended Kalman filter algorithm to obtain the real-time attitude of the cleaning robot. This invention can achieve real-time measurement of the cleaning robot's attitude, enabling timely adjustment of the attitude angles and ensuring good cleaning results.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology, specifically relating to a multi-sensor intelligent motion posture recognition method for robots. Background Technology

[0002] Photovoltaic power generation has a wide range of applications, and the construction speed of power plants in China is very fast, but the operation and maintenance of power plants has been neglected. This is especially true for photovoltaic power plants in deserts and Gobi areas with harsh weather conditions, and for commercial and industrial rooftop photovoltaic power plants with serious environmental pollution. In the windy and dusty areas of Northwest China, a sandstorm can affect the efficiency of photovoltaic power generation by as much as 15-30%. For photovoltaic power generation systems that have been operating for a long time, the impact of dust accumulation on photovoltaic modules cannot be underestimated. Dust on the surface of the modules reflects, scatters, and absorbs solar radiation, which can reduce the transmittance of sunlight, resulting in less solar radiation received by the modules and a corresponding decrease in output power. Its effect is directly proportional to the thickness of the accumulated dust. Currently, photovoltaic modules can be cleaned manually, using large cleaning trucks, or using track-mounted cleaning robots. When using water for manual cleaning, there are problems such as wastewater treatment, personnel management, difficulty in controlling the cleaning process, and insufficient cleaning depth. When dry cleaning is used by personnel, there are problems such as module cracks, poor cleaning effect, and dust residue. Using large cleaning trucks is only suitable for flat ground, the non-linear movement of the vehicle can cause uneven stress, affecting the life of the modules, requires high technical skills from operators, and the vehicle requires routine maintenance. Therefore, photovoltaic intelligent cleaning robots are now widely used for cleaning. They require minimal human intervention, offer excellent cleaning results, and are one of the mainstream solutions. The accuracy of the robot's posture recognition directly impacts its overall accuracy and cleaning performance. Therefore, an effective motion posture recognition method needs to be designed. Summary of the Invention

[0003] To overcome the shortcomings of existing technologies, this invention provides a multi-sensor intelligent motion attitude recognition method for photovoltaic intelligent cleaning robots. First, a three-axis accelerometer and a three-axis gyroscope are added to the photovoltaic intelligent cleaning robot to construct a multi-sensor intelligent motion attitude recognition system. Then, the robot's attitude data is calculated and updated in real time using an extended Kalman filter algorithm. The gravity components on the XYZ axes of the cleaning robot are measured to obtain the robot's heading angle, pitch angle, and roll angle. Finally, the measured values ​​of the heading angle, pitch angle, and roll angle are converted into attitude quaternions in real time and participated in each iteration of the extended Kalman filter algorithm to obtain the real-time attitude of the cleaning robot. This invention can achieve real-time measurement of the cleaning robot's attitude, enabling timely adjustment of attitude angles and ensuring good cleaning results.

[0004] The technical solution adopted by this invention to solve its technical problem includes the following steps:

[0005] Step 1: Add a three-axis accelerometer and a three-axis gyroscope to the photovoltaic intelligent cleaning robot to build a multi-sensor intelligent motion attitude recognition system; the three-axis gyroscope serves as a horizontal, vertical, pitch, heading, and angular velocity sensor; the three-axis accelerometer is used to measure linear acceleration;

[0006] Step 2: The three Euler angles representing the robot's attitude are the yaw angle ψ, pitch angle θ, and roll angle.

[0007] Step 3: During the operation of the cleaning robot, the robot's posture data is calculated and updated in real time using the extended Kalman filter algorithm;

[0008] The extended Kalman filter algorithm consists of two parts: time update (prediction) and measurement update (correction).

[0009] Step 3-1: Prediction; the data comes from a three-axis gyroscope;

[0010] The prior state estimate of the cleaning robot is:

[0011]

[0012] The prior error covariance of the cleaning robot's position is:

[0013]

[0014] Where x is the attitude state variable, representing the three-axis attitude angle, and k is the k-th iteration. This is the posterior estimate of the attitude state variable x from the previous cycle. and u k-1 For the control matrix; ω k-1 The system noise vector is of dimension l×1, extracted from a zero-mean multivariate normal distribution N; F is a nonlinear function estimated a priori. k-1 Let F and P be the state transition matrices of the previous cycle. k-1∣k-1 G is the posterior estimate of the covariance from the previous iteration. k-1 For control input matrix; Q k-1 Let be the covariance matrix, which is assumed to be a multivariate zero-mean normal distribution; To control the input matrix;

[0015] Step 3-2: Calibration; the data comes from the triaxial accelerometer;

[0016] The optimal Kalman gain is defined as:

[0017]

[0018] The posterior state estimate of the updated robot vacuum's location is:

[0019]

[0020] The posterior error covariance of the updated robot vacuum's position is:

[0021] P k∣k =(I n -K k H k )P k∣k-1

[0022] Among them, P k∣k-1 For the prior error covariance, For the measurement matrix, H k Let R′ be the observation matrix. k For observation noise with zero mean and Gaussian covariance, z k z is the state measurement value for the current cycle. k =H(X) k ,v k ), I n G is the identity matrix; k For the input control matrix; F k H is the state transition matrix; k ω is the observation matrix; k and ν k All follow a normal distribution, Q k :ω k ~N(0,Q) k ), R k :v k ~N(0,R k ), Q k It is the system noise variance matrix, R k It is the measurement noise variance matrix;

[0023] Step 4: Attitude state variable x = [q] T q is an attitude quaternion, and the attitude quaternion can be related to three attitude angles ψ, θ, ... Mutual conversion; the conversion equation is:

[0024]

[0025] θ = arcsin(2q0q2 - 2q1q3)

[0026]

[0027] Where q0, q1, q2, and q3 represent attitude quaternions;

[0028] Step 5: The data for the prediction section is updated as follows:

[0029] Δθ truth =Δθ meas -Δθ b

[0030]

[0031] q k+1 =q k ·Δq

[0032]

[0033] Where, Δθ truth Δθ represents the actual change in angle. meas This represents the change in angle obtained by integrating the angular velocity ω read from the gyroscope; Δθ b Δq0, Δq1, Δq2, and Δq3 represent the process quantities of the quaternion, used to calculate the attitude quaternion.

[0034]

[0035]

[0036] Δθ truth =Δθ meas -Δθ bias

[0037] Where ω is a matrix representing the real-time angular velocity values ​​of the three axes measured by the gyroscope, the calculated angle values ​​of the three axes are obtained by integrating over time. Write it as Δθ meas ; This represents the angular offset state of the three-axis gyroscope, which is an inherent error; Δθ truth The true value of the three-axis rotation angle is obtained by subtracting its own angular offset value from the calculated three-axis rotation angle;

[0038] Step 6: Measure the gravity components A on the XYZ axes of the cleaning robot respectively. Xout A Yout A Zout The magnitudes of these parameters are used to calculate the robot's heading angle ψ, pitch angle θ, and roll angle. The size is:

[0039]

[0040]

[0041]

[0042] The cleaning robot's heading angle ψ, pitch angle θ, and roll angle The measured values ​​are converted into attitude quaternions q in real time, which participate in each iteration of the extended Kalman filter algorithm to obtain the real-time attitude of the cleaning robot.

[0043] The beneficial effects of this invention are as follows:

[0044] This invention enables real-time measurement of the robot's posture, allowing for timely adjustments to the posture angle and ensuring optimal cleaning results. Attached Figure Description

[0045] Figure 1 This is a flowchart illustrating the real-time tracking process of the method of this invention.

[0046] Figure 2 This is a block diagram illustrating the principle of the multi-sensor intelligent motion posture recognition system of the present invention. Detailed Implementation

[0047] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0048] like Figure 1 As shown, this invention provides a multi-sensor intelligent motion posture recognition method for photovoltaic intelligent cleaning robots. It can automatically track and calculate the posture of the cleaning robot in real time using sensors. Using this multi-sensor posture recognition system, the posture of the cleaning robot can be measured in real time, and the posture angle can be adjusted in a timely manner to ensure good cleaning effect.

[0049] This invention is achieved using the following technical solution:

[0050] like Figure 2 As shown, this invention primarily utilizes a three-axis accelerometer and a three-axis gyroscope to perform attitude recognition on static objects. Using these two sensors for attitude calculation is an accurate and reliable method. It not only has the advantages of low cost and simple structure, but also easy installation and maintenance.

[0051] Gyroscope sensors are used in automatic control systems of various moving devices as level, vertical, pitch, heading, and angular velocity sensors in driving and navigation instruments. Accelerometer sensors are used to measure the linear acceleration of a moving object, which can be converted into corresponding attitude angles through different components on three axes.

[0052] Gyroscopes have good dynamic response characteristics, but they introduce cumulative errors when calculating attitude. Accelerometers do not have cumulative errors when measuring attitude, but their dynamic response is poor. Since their frequency domain characteristics are complementary, fusing their data using the EKF filtering algorithm reduces the impact of their respective shortcomings, improves the accuracy and effectiveness of attitude angle estimation, and enhances measurement precision and system dynamic performance.

[0053] The three Euler angles representing the attitude state of the cleaning robot are the yaw, pitch, and roll angles, denoted by ψ; θ; and θ, respectively. To express.

[0054] In the actual operation of a cleaning robot, the Extended Kalman Filter (EKF) algorithm is mainly used to continuously calculate and update the robot's real-time attitude data. The EKF equation mainly consists of two parts: time update (prediction) and measurement update (correction).

[0055] (1) Prediction: (Data source: gyroscope)

[0056] Predictive (prior) state estimation of the cleaning robot

[0057]

[0058] Error covariance of predicting (prior) the position of the cleaning robot

[0059]

[0060] Where the state variable x is the three-axis attitude angle, and k is the k-th iteration. This is the posterior estimate from the previous iteration. and u k-1 For the control matrix, ω k-1 Let l×1 be the system noise vector. F is a nonlinear function estimated a priori. k-1 Let F and P be the state transition matrices of the previous cycle. k-1∣k-1 G is the posterior estimate of the covariance from the previous iteration. k-1 To control the input matrix, Q k-1 The covariance matrix is ​​assumed to follow a multivariate zero-mean normal distribution. To control the input matrix.

[0061] (2) Calibration: (Data source: accelerometer)

[0062] Optimal Kalman gain

[0063]

[0064] Updated (posterior) state estimation of the robot vacuum's position

[0065]

[0066] Updated (posterior) error covariance of the robot vacuum's position

[0067] P k∣k =(I n-K k H k )P k∣k-1

[0068] Among them, P k∣k-1 For the prior error covariance, For the measurement matrix, H k Let R′ be the observation matrix. k Observation noise with zero mean Gaussian covariance. For prior state estimation, and z′ k z is the state measurement value for the current cycle. k For the current cycle's state measurement value, note that in EKF filtering calculations for discrete nonlinear systems, the state-space system transfer function for the measurement part is:

[0069] z k =H(X) k ,v k )

[0070] I n It is the identity matrix; ω k-1 G is an ι×1 dimensional system noise vector, assumed to be extracted from a zero-mean multivariate normal distribution N; k For the input control matrix; F k H is the state transition matrix; k The observation matrix;

[0071] x = [q] T The algorithm primarily filters and calculates attitude state variables, where q is the attitude quaternion. The attitude quaternion can be correlated with the three actual attitude angles ψ, θ, ... They can be converted to each other. The conversion equation is:

[0072]

[0073] θ = arcsin(2q0q2 - 2q1q3)

[0074]

[0075] The data updates for the forecast section are as follows:

[0076] Δθ truth =Δθ meas -Δθ b

[0077]

[0078] q k+1 =q k ·Δq

[0079]

[0080] Where, Δθ truth Δθ represents the actual change in angle. meas This represents the change in angle obtained by integrating the angular velocity ω read from the gyroscope; Δθ b Δq0, Δq1, Δq2, and Δq3 represent the process quantities of the quaternion, used to calculate the attitude quaternion.

[0081]

[0082]

[0083] Δθ truth =Δθ meas -Δθ bias

[0084] Where ω is a matrix representing the real-time angular velocity values ​​of the three axes measured by the gyroscope, the calculated angle values ​​of the three axes are obtained by integrating over time. Write it as Δθ meas ; This represents the angular offset state of the three-axis gyroscope, which is an inherent error; Δθ truth The true value of the three-axis rotation angle is obtained by subtracting its own angular offset value from the calculated three-axis rotation angle;

[0085] The real-time conversion measurement attitude data in the correction section comes from the real-time conversion of accelerometer data. The calculation principle is that the three-axis accelerometer chip, in a static or uniform motion state, only experiences gravity. The components of gravity g along the X, Y, and Z axes of the chip can represent the object's attitude. The X, Y, and Z axes are parallel and perpendicular to the three-axis edges of the chip package. When the chip is in a horizontal orientation, the X and Y axes coincide with the horizontal plane, and its gravity component is 0g. The Z axis is perpendicular to the horizontal plane, and its gravity component is 1g, in the opposite direction to gravity. When representing the chip's attitude, the three-axis coordinate system in the horizontal plane state is used as the reference attitude. When the chip's attitude changes, the directions of the X, Y, and Z axes also change. At this time, the angle between the chip's X axis and the reference X axis is set as θ, the angle between the Y axis and the reference Y axis is set as ψ, and the angle between the Z axis and the reference Z axis is set as... Keeping the chip stationary or in uniform motion, the gravitational component A on the XYZ axes is measured. Xout A Yout A Zout The magnitude of the angles θ and ψ along the XYZ axes can be used to calculate the changes in these angles. The size is

[0086]

[0087]

[0088]

[0089] The measured values ​​of the three included angles are converted into attitude quaternions in real time and participated in each iteration of EKF calculation to calculate the real-time attitude of the cleaning robot, ensuring the validity of the data.

Claims

1. A multi-sensor intelligent motion posture recognition method for a photovoltaic intelligent cleaning robot, characterized in that, Includes the following steps: Step 1: Add a three-axis accelerometer and a three-axis gyroscope to the photovoltaic intelligent cleaning robot to build a multi-sensor intelligent motion attitude recognition system; the three-axis gyroscope serves as a horizontal, vertical, pitch, heading, and angular velocity sensor; the three-axis accelerometer is used to measure linear acceleration; Step 2: The three Euler angles representing the robot's attitude are the heading angle and the yaw angle. Pitch angle and roll angle ; Step 3: During the operation of the cleaning robot, the robot's posture data is calculated and updated in real time using the extended Kalman filter algorithm; The extended Kalman filter algorithm consists of two parts: time update (prediction) and measurement update (correction). Step 3-1: Prediction; the data comes from a three-axis gyroscope; The prior state estimate of the cleaning robot is: The prior error covariance of the cleaning robot's position is: Among them, attitude state quantity For three-axis attitude angles, k For the first k The next loop. The attitude state of the previous cycle The posterior estimate, and state variables , For control matrix; for The system noise vector of dimension is extracted from a zero-mean multivariate normal distribution N; For the prior estimate of the nonlinear function, Let F be the state transition matrix of the previous cycle. The covariance is estimated posteriorly for the previous iteration. To control the input matrix; Let be the covariance matrix, which is assumed to be a multivariate zero-mean normal distribution; To control the input matrix; Step 3-2: Calibration; the data comes from the triaxial accelerometer; The optimal Kalman gain is defined as: The posterior state estimate of the updated robot vacuum's location is: The posterior error covariance of the updated robot vacuum's position is: in, For the prior error covariance, For the measurement matrix, For the observation matrix, Observation noise with zero mean Gaussian covariance. , It is the identity matrix; For input control matrix; This is the state transition matrix; The observation matrix; and All follow a normal distribution. , , It is the system noise variance matrix. It is the measurement noise variance matrix; Step 4: Attitude State Quantities , q For attitude quaternions, attitude quaternions can be combined with three attitude angles. Mutual conversion; the conversion equation is: in, Represents attitude quaternions; Step 5: The data for the prediction section is updated as follows: in, This means that the angular velocity is read from the gyroscope and integrated to obtain the change in angle; This indicates the inherent error of the sensor itself; These represent the process quantities of quaternions, used to calculate attitude quaternions; in, Let be a matrix representing the real-time angular velocity values ​​of the three axes measured by the gyroscope. By integrating over time, the calculated angle values ​​of the three axes are obtained. Write it as ; This is the angular offset state of the three-axis gyroscope, which is an inherent error. The true value of the three-axis rotation angle is obtained by subtracting its own angular offset value from the calculated three-axis rotation angle; Step 6: Measure the gravity components on the XYZ axes of the cleaning robot. The size of the value, i.e., the calculated heading angle of the cleaning robot. Pitch angle and roll angle The size is: The cleaning robot's heading angle Pitch angle and roll angle The measured values ​​are converted into attitude quaternions in real time. q It participates in each iteration of the extended Kalman filter algorithm to obtain the real-time posture of the cleaning robot.

Images