Measurement Method for Swing Angle of Crane Load Based on Multi-Sensor Information Fusion

Abstract

The present invention discloses a method for measuring the swing angle of a hoisted object of a crane based on multi-sensor information fusion. By fusing the data of an inertial measurement unit (IMU) and an industrial camera, and combining the advantages of both, high-precision measurement of the swing angle of the hoisted object of the crane is achieved in a dynamic environment. Through a real-time correction mechanism for the IMU swing angle based on the visual swing angle, the cumulative error of the IMU is eliminated, ensuring the accuracy of the measurement of the swing angle of the hoisted object. This mechanism can trigger corrections in real time during the dynamic swing of the hoisted object of the crane, greatly improving the reliability, accuracy, and real-time performance of the system. The multi-sensor fusion mechanism is adopted to effectively reduce the problems caused by the drift or failure of a single sensor, ensuring that the system can still operate reliably and output accurate swing angle results even when visual data is lost or the IMU drifts.

Description

Technical Field

[0001] The present invention relates to the field of computer vision technology applications, and particularly to a method for measuring the swing angle of a hoisted object of a crane based on multi-sensor information fusion. Background Art

[0002] Existing crane swing angle detection methods generally rely on single-sensor data for angle measurement, such as IMU or vision sensors. Although these methods can complete swing angle detection to a certain extent, they have the following defects:

[0003] Limitations of sensors: IMU can provide real-time angle change information, but it is affected by long-term drift, and the measurement error will accumulate over time, resulting in an increase in deviation; measuring the swing angle of a hoisted object with an industrial camera is easily affected by factors such as environmental light changes and occlusion, resulting in the loss of key information and thus affecting the measurement accuracy.

[0004] Difficulty in data fusion: The data fusion in the prior art does not fully consider the advantages and disadvantages of each sensor, nor does it dynamically adjust for the errors of different sensors, so the accuracy and reliability are insufficient.

[0005] In summary, there is an urgent need to provide a method for measuring the swing angle of a hoisted object of a crane based on multi-sensor information fusion. Summary of the Invention

[0006] To solve the above technical problems, the present invention provides a method for measuring the swing angle of a hoisted object of a crane based on multi-sensor information fusion, which improves the accuracy and reliability of measuring the swing angle of a hoisted object of a crane by fusing the visual swing angle and the IMU swing angle to obtain the final swing angle.

[0007] The technical solution of the present invention is: A method for measuring the swing angle of a hoisted object of a crane based on multi-sensor information fusion, comprising the following steps:

[0008] Step 1: The staff sends a work instruction to the crane, and the crane is still in a stationary state. Complete the initialization task including setting the error correction angle , the effective measurement times T of the visual swing angle is equal to 0, the continuous failure times H of the vision is equal to 0, and calculate the IMU standard deviation ;

[0009] Step 2: After the initialization task is completed, the crane starts to move, and the camera continuously acquires image frames of the hook plate;

[0010] Step 3: Identify the two-dimensional code on the hook plate, calculate the number n of detected two-dimensional codes, and determine whether the number n of two-dimensional codes is equal to 3. If n is equal to 3, enter Step 4; if n is not equal to 3, return to Step 2;

[0011] Step 4: Determine whether the relationship of the QR codes satisfies geometric constraints:

[0012] If the geometric constraints are satisfied, it is considered that the visual measurement is valid, and the visual swing angle is calculated , set the correction flag bit F equal to 1, increment the effective measurement times T of the visual swing angle by 1, set the visual continuous failure times H to 0, and enter Step 5. Otherwise, it is considered that the visual measurement is invalid, set the correction flag bit F equal to 0, increment the visual continuous failure times H by 1, and enter Step 6;

[0013] Step 5: Determine whether the effective measurement times T of the visual swing angle is equal to N. If it is equal to N, calculate the visual standard deviation , and at the same time reset T to 0. Otherwise, return to Step 2;

[0014] Step 6: Determine whether the visual continuous failure times H is greater than the threshold , if it is greater than it is considered that the visual measurement system fails and an alarm signal is sent to notify the staff to stop for maintenance. If it is not greater than then enter Step 7;

[0015] Step 7: Measure the swing angle using the IMU ; Calculate the IMU corrected swing angle ;

[0016] Wherein, represents the value that needs to be compensated for the IMU data due to the drift of the IMU sensor;

[0017] Step 8: Determine whether the correction flag bit F is equal to 1. If F is equal to 1 and the visual swing angle is considered valid, enter Step 9; if F is equal to 0 and the visual swing angle is considered invalid, enter Step 11;

[0018] Step 9: Calculate the deviation between the visual swing angle and the IMU corrected swing angle , and determine whether it exceeds the threshold . If it does not exceed the threshold then no re-correction is performed , otherwise re-correction is performed ;

[0019] Step 10: Fuse the visual swing angle and the IMU swing angle according to the following strategy, calculate the final swing angle and enter Step 12:

[0020]

[0021] Among them, the confidence weights of the IMU and vision are respectively and , and the calculation is as follows:

[0022]

[0023]

[0024] k is the balance coefficient;

[0025] Step 11: Only use the IMU to correct the swing angle The output is the final swing angle , and enter Step 12;

[0026] Step 12: From the start of the crane to the completion of the lifting and handling of the load, repeat Steps 2 - 11 until the crane stops working.

[0027] Furthermore, in Step 1, calculate the standard deviation of the IMU :

[0028]

[0029] Among them , is the swing angle value measured by the IMU, is the true swing angle value, when the load is stationary , and N is the number of swing angle measurements.

[0030] Furthermore, in Step 4, the calculation process of the geometric constraint is as follows:

[0031] Detect the coordinates of the center of the th QR code in the camera coordinate system ;

[0032] Calculate the side length between the centers of the th QR code and the th QR code measured by the camera;

[0033] Calculate the difference between and the true side length : ;

[0034] The differences between the three actually measured side length values of the QR code array and the true side length are less than the set threshold , then the geometric constraint is satisfied, otherwise it is not satisfied.

[0035] Furthermore, the specific calculation method of the vision swing angle is as follows:

[0036] A. Obtain the coordinate system of the i-th two-dimensional code in the camera coordinate system through the camera, including the translation vector and the rotation vector ;

[0037] B. Calculate the average value of the translation vectors of the three two-dimensional codes as the central coordinate of the two-dimensional code array, and obtain the geometric center coordinate of the two-dimensional code array in the camera coordinate system ;

[0038] The calculation of the geometric center p coordinate is as follows:

[0039]

[0040]

[0041]

[0042] Finally, the p coordinate is ;

[0043] C. Convert the rotation vector of each two-dimensional code into the corresponding rotation matrix ; Since the poses of the three two-dimensional codes satisfy the constraints, indicating that the poses of the three two-dimensional codes are the same, in order to simplify the calculation, average all the rotation matrices and orthogonalize them to obtain , and then convert it back to the rotation vector ;

[0044] The modulus of the rotation vector

[0045] The Rodriguez formula converts the rotation vector into the rotation matrix :

[0046]

[0047] Where: is the 3*3 identity matrix, is the rotation angle, is the unit vector of the rotation axis, is composed of constitute the skew-symmetric matrix:

[0048]

[0049] Finally, average and use singular value decomposition for orthogonalization to obtain ;

[0050] D. The connection point between the suspension rope and the hook plate is denoted as point c. Starting from point p and offsetting along the negative x-axis direction by , that is, reaching point c. Therefore, it can be obtained through Calculation Furthermore, a translation vector is obtained , and the visual pendulum angle is calculated through trigonometric functions :

[0051]

[0052] Furthermore, in step 5, the visual standard deviation is calculated as follows:

[0053] The centers of three QR codes are detected, and the centers of the three QR codes are connected to obtain three sides, and the measured values of the three sides are calculated :

[0054] For each side , the standard deviation

[0055]

[0056] where ;

[0057] Based on the standard deviation of each side, the visual standard deviation is calculated:

[0058]

[0059] Furthermore, in step 9

[0060] When , it is considered that the deviation is within the normal range at this time, and the IMU pendulum angle is credible, so the correction angle of the IMU does not need to be updated ;

[0061] When , it is considered that a large cumulative error has occurred in the IMU data at this moment, so the cumulative error correction angle of the IMU is updated, and the IMU pendulum angle is corrected again.

[0062] The beneficial technical effects of the present invention are:

[0063] By fusing the data of the inertial measurement unit (IMU) and the industrial camera, combining the advantages of both, high-precision measurement of the pendulum angle of the crane load is achieved in a dynamic environment. By fusing the visual and IMU information, this solution makes full use of the complementarity of the two, mutually correcting and supplementing, and significantly improving the measurement accuracy and reliability of the system.

[0064] The multi-sensor fusion mechanism is adopted to effectively reduce the problems caused by the drift or failure of a single sensor, ensuring that the system can still operate reliably and output accurate swing angle results even when visual data is lost or the IMU drifts.

[0065] Through a real-time correction mechanism for the IMU swing angle based on the visual swing angle, the cumulative error of the IMU is eliminated, and the accuracy of the suspended object swing angle measurement is improved. This mechanism does not need to wait for the device to return to a fixed position or a stationary state, and can correct the IMU swing angle in real time during the dynamic swing process, greatly improving the real-time performance and flexibility of the system.

[0066] The above description is only an overview of the technical solution of the present invention. In order to understand the technical means of the present invention more clearly and implement it according to the content of the specification, the following describes the preferred embodiments of the present invention in detail with reference to the accompanying drawings. Brief Description of the Drawings

[0067] Figure 1 It is a schematic diagram of the swing angle measurement architecture of the present invention;

[0068] Figure 2 It is the specific position of the two-dimensional code of the present invention;

[0069] Figure 3 It is a schematic diagram of the process of the present invention. Detailed Embodiment

[0070] In order to understand the technical means of the present invention more clearly and implement it according to the content of the specification, the following further describes the detailed embodiments of the present invention in combination with the accompanying drawings and embodiments. The following embodiments are used to illustrate the present invention, but do not limit the scope of the present invention.

[0071] It should be noted that the terms "first", "second", etc. in the specification and claims of this application and the above-mentioned drawings are used to distinguish similar objects, and do not have to be used to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so as to implement the embodiments of this application described herein.

[0072] In the description of the present invention, it should be noted that the orientation or positional relationship indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. is based on the orientation or positional relationship recorded in the embodiments and shown in the accompanying drawings, or the orientation or positional relationship in which the invention product is usually placed when in use. It is only for the convenience of describing the present invention and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention.

[0073] Such as Figure 1As shown, the present invention specifically relates to a method for measuring the swing angle of a hoisted object of a crane based on multi-sensor information fusion, comprising the following steps:

[0074] Step 1: The staff sends a work instruction to the crane, and the crane remains stationary. The initialization task is completed, including setting , the effective measurement times T of the visual swing angle is equal to 0, the continuous failure times H of the vision is equal to 0, and the standard deviation of the IMU is calculated ;

[0075] Furthermore, in step 1, the standard deviation of the IMU is calculated :

[0076]

[0077] Wherein , is the swing angle value measured by the IMU, is the true swing angle value. When the hoisted object is stationary , and N is the number of swing angle measurements.

[0078] By calculating the standard deviation of the IMU, the noise level of the IMU can be determined, providing a reference for subsequent data fusion. This helps to dynamically adjust the fusion ratio of the IMU and the vision sensor in practical applications.

[0079] After the initialization task is completed, the crane starts to move, and the camera acquires the image frames of the hook plate in real time;

[0080] Step 3: Identify the two-dimensional codes on the hook plate, calculate the number n of detected two-dimensional codes, and determine whether the number n of two-dimensional codes is equal to 3; if n is equal to 3, go to step 4, if n is not equal to 3, return to step 2;

[0081] Here, it is hoped that all two-dimensional codes can be detected to avoid the situation of being blocked.

[0082] Before calculating the swing angle, ensure that at least 3 two-dimensional codes are detected. If the number of detected two-dimensional codes does not match the expected value, it indicates that there may be visual data loss, and at this time, the system should re-collect the data.

[0083] Step 4: Determine whether the relationship of the two-dimensional codes satisfies the geometric constraint. The calculation process of the geometric constraint is as follows:

[0084] Detect the coordinate of the center of the th two-dimensional code in the camera coordinate system;

[0085] Calculate the side length between the centers of the th two-dimensional code and the th two-dimensional code measured by the camera;

[0086] Calculate The difference from the true side length is: ;

[0087] The three side length values actually measured for the QR code array and the true side length have a difference less than the set threshold , then the geometric constraint is satisfied, otherwise the geometric constraint is not satisfied.

[0088] If the geometric constraint is satisfied, the visual measurement is considered valid, calculate the visual swing angle , set the correction flag bit F equal to 1, increment the effective measurement times T of the visual swing angle by 1, set the visual continuous failure times H to 0, and enter step 5. Otherwise, the visual measurement is considered invalid, set the correction flag bit F equal to 0, increment the visual continuous failure times H by 1, and enter step 6;

[0089] This process helps to detect the validity of visual data in real time, ensure that the system performs subsequent swing angle calculations based on reliable visual data, and avoid incorrect operation data calculations caused by inaccurate data.

[0090] The geometric structure and position of the QR code array are relatively fixed. Therefore, errors caused by image distortion, illumination changes, or QR code recognition errors can be effectively excluded in this way. This constraint can improve the robustness of the system in practical applications.

[0091] When in the static state, i.e., when the hook angle is 0, there are three QR codes on the hook plate that satisfy the same attitude angle. The three QR codes form a geometric array and form an equilateral triangle on the plane. The geometric constraint of the array is the fixed side length is known. At the same time, the line segment formed by the center point of the equilateral triangle and the connection point of the suspension rope on the hook plate is perpendicular to the corresponding edge of the hook plate, and the length of this line segment is known.

[0092] Furthermore, in step 4, if the geometric constraint is satisfied: it is considered that the visual swing angle calculated using the QR code is valid, calculate the visual swing angle , increment the effective measurement times T of the visual swing angle by 1, set the correction flag bit F equal to 1, and subsequently use the visual swing angle to correct the IMU swing angle.

[0093] Among them, T is incremented by 1 each time the constraint is satisfied. When the number of times reaches N, T will be cleared. Since multiple sets of data are required to calculate the standard deviation, multiple sets of data that satisfy the constraint need to be collected here. If the geometric constraint is not satisfied, the currently detected visual angle Invalid. Set the correction flag bit F to 0, and the subsequent IMU swing angle will not use the visual swing angle correction.

[0094] Furthermore, the visual swing angle is calculated as follows:

[0095] A. Obtain the coordinate system of the th QR code in the camera coordinate system through the camera, including the translation vector and the rotation vector ;

[0096] B. Calculate the average value of the translation vectors of the three QR codes as the center coordinates of the QR code array, and obtain the geometric center coordinates of the QR code array in the camera coordinate system ;

[0097] The calculation of the geometric center p coordinates is as follows:

[0098]

[0099]

[0100]

[0101] Finally, the p coordinate is ;

[0102] C. Convert the rotation vector of each QR code into the corresponding rotation matrix ; Since the postures of the three QR codes satisfy the constraints, indicating that the postures of the three QR codes are the same, for simplicity of calculation, average all the rotation matrices and orthogonalize them to obtain , and then convert it back to the rotation vector ;

[0103] The modulus of the rotation vector

[0104] The Rodriguez formula converts the rotation vector into the rotation matrix :

[0105]

[0106] Where: is the 3*3 identity matrix, is the rotation angle, is the unit vector of the rotation axis, is composed of to form the skew-symmetric matrix:

[0107]

[0108] Finally, perform Calculate the mean value and orthogonalize it using singular value decomposition to obtain ;

[0109] D. The connection point between the suspension rope and the hook plate is denoted as point c. Starting from point p and offsetting along the negative x-axis by , that is, reaching point c. Therefore, the translation vector can be obtained by calculating and then obtaining the translation vector . Calculate the visual measurement pendulum angle through trigonometric functions :

[0110]

[0111] Step 5: Determine whether the effective measurement times T of the visual pendulum angle is equal to N. If it is equal to N, calculate the visual standard deviation , and at the same time reset T to 0. Otherwise, return to Step 2.

[0112] The standard deviation obtained by calculating multiple sets of valid data provides a quantitative index for the visual data fluctuation range, helping the system evaluate the stability and consistency of the visual measurement results. This calculation provides a basis for subsequent sensor data fusion.

[0113] Furthermore, in Step 5, the visual standard deviation is calculated as follows:

[0114] For each edge , calculate the standard deviation :

[0115]

[0116] where ;

[0117] Based on the standard deviation of each edge, calculate the visual standard deviation :

[0118]

[0119] Step 6: Determine whether the continuous failure times H of the vision is greater than the threshold . If it is greater than , it is considered that the visual measurement system has a fault and an alarm signal is sent to notify the staff to stop for maintenance. If it is not greater than , then go to Step 7;

[0120] Further, in step 6, when the number of consecutive visual failures H is 20, by setting a threshold, such as 20 consecutive visual failures, an alarm signal is sent to indicate that the visual system may be abnormal or interfered, which can effectively monitor the health status of the visual system. After the operator receives the alarm signal in time, the visual system can be inspected or recalibrated, thereby improving the operation safety. This processing ensures that when the visual data is inaccurate, the system can still rely on the data provided by the IMU to avoid the negative impact of visual failure on the system operation.

[0121] Step 7: Measure the swing angle using the IMU ; Calculate the IMU-corrected swing angle .

[0122] Step 8: Determine whether the correction flag bit F is equal to 1. If F is equal to 1, it is considered that the visual swing angle is valid, and then enter step 9; if F is equal to 0, it is considered that the visual swing angle is invalid, and then enter step 11.

[0123] The problem with IMU data lies in the cumulative deviation. If it is not in the case of long-term operation, the deviation will not be too large. When the correction flag bit is 0, it only represents the failure of some frames of data, that is, the visual data is abnormal in the short term. If the visual data has problems for a long time, an alarm signal is issued.

[0124] Step 9: Calculate the deviation between the visual swing angle and the IMU-corrected swing angle , and determine whether it exceeds the threshold ; if it does not exceed, no re-correction is performed , otherwise re-correction is performed . .

[0125] Further, when , it is considered that the deviation is within the normal range at this time, and the IMU swing angle is credible and does not need to be corrected, so the corrected angle of the IMU does not need to be updated .

[0126] When , it is considered that a large cumulative error has occurred in the IMU data at this moment, and the cumulative error correction angle of the IMU is updated , and the IMU swing angle is re-corrected .

[0127] Step 10: Fuse the visual swing angle and the IMU swing angle according to the following strategy, calculate the final swing angle and enter step 12:

[0128]

[0129] where the confidence weights of the IMU and vision are and respectively, and are calculated as follows:

[0130]

[0131]

[0132] k is the balance coefficient;

[0133] Through the dynamic adjustment of the weights, optimal decisions can be made under different data quality conditions. For example, when the IMU data is accurate, the weight of the IMU is higher, and the system will rely more on the IMU data; when the vision data is stable, the weight of the vision data is higher, and the system will rely more on the vision data.

[0134] By calculating and adjusting the standard deviation in real time, the system can adaptively select the weights according to the actual conditions of the sensors, and thus always make decisions based on the most favorable sensor data. This enables the system to adaptively change under different environmental conditions and provide stable performance.

[0135] Step 11: Only use the IMU to correct the swing angle The output is the final swing angle and enter Step 12.

[0136] At this time, the vision swing angle is invalid, and only the IMU swing angle is used and the current swing angle is output.

[0137] Step 12: Between the crane starting and completing the handling of the lifted object, repeat Steps 2 - 11 until the crane stops working.

[0138] The above embodiments are only specific implementation manners of the present invention, used to illustrate the technical solutions of the present invention, rather than limiting it. The protection scope of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: Any person skilled in the art within the technical scope disclosed by the present invention can still modify the technical solutions recorded in the foregoing embodiments, or can easily think of changes, or perform equivalent replacements on some of the technical features; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the protection scope of the present invention.

Claims

1. A method for measuring the swing angle of a crane hoist based on multi-sensor information fusion, characterized in that: The following steps are involved: Step 1: The staff sends a work instruction to the crane. The crane is still in a stationary state and completes the initialization task including setting the error correction angle. , the number of effective visual swing angle measurements T is equal to 0, the number of consecutive visual failures H is equal to 0, and the IMU standard deviation is calculated ; Step 2: After the initialization task is completed, the crane starts to move, and the camera acquires the image frame of the hook plate in real time; Step 3, identify the QR code on the hook plate, calculate the number n of QR codes detected, and determine whether the number n of QR codes is equal to 3. If n is equal to 3, proceed to step 4; if n is not equal to 3, return to step 2; Step 4: Determine whether the relationship of the QR code satisfies the geometric constraints; If the geometric constraints are met, the visual measurement is considered valid and the visual swing angle is calculated. , set the correction flag F to 1, increase the number of effective visual swing angle measurements T by 1, set the number of consecutive visual failures H to 0, and go to step 5; otherwise, the visual measurement is considered invalid, set the correction flag F to 0, increase the number of consecutive visual failures H by 1, and go to step 6; Step 5: Determine whether the number of effective visual swing angle measurements T is equal to N. If it is equal to N, calculate the visual standard deviation , and reset T to 0, otherwise return to step 2; Step 6: Determine whether the number of consecutive visual failures H is greater than the threshold , if greater than It is considered that the visual measurement system has failed and an alarm signal is issued to notify the staff to shut down for maintenance. If it is not greater than Then proceed to step 7; Step 7: Use IMU to measure the swing angle ; Calculate IMU corrected swing angle ; in, Indicates that the IMU sensor has drift and the IMU data needs to be compensated. Step 8, determine whether the correction flag F is equal to 1, if F is equal to 1, it is considered that the visual swing angle is valid and go to step 9; if F is equal to 0, it is considered that the visual swing angle is invalid and go to step 11; Step 9: Calculate the visual swing angle Correcting the swing angle with IMU Deviation ,judge Whether it exceeds the threshold If the threshold is not exceeded No re-correction Otherwise, re-correct ; Step 10: Adjust the visual angle and IMU swing angle According to the following strategy, the fusion is performed to calculate the final swing angle And proceed to step 12: ； The confidence weights of IMU and vision are and , calculated as follows: ； ； k is the balance coefficient; Step 11: Correct the swing angle using only the IMU Output is the final swing angle , and go to step 12; Step 12: From the time the crane is started to the time the lifting object is transported, steps 2 to 11 are repeated until the crane stops working; Calculate the IMU standard deviation in step 1 : ； in , is the swing angle value measured by IMU, is the actual swing angle value, when the hanging object is stationary , N is the number of swing angle measurements; In step 4, the geometric constraints are calculated as follows: Detection The coordinates of the center of the QR code in the camera coordinate system ; Calculate the camera's measured QR code and The length of the side between the centers of the QR codes ; calculate The real side length The difference: ; The three side lengths actually measured by the QR code array The real side length The difference is less than the set threshold , then the geometric constraint is satisfied, otherwise it is not satisfied; Visual swing angle The specific calculation method is as follows: A. Obtain the first i The coordinate system of the QR code, including the translation vector and the rotation vector ; B. Calculate the average of the translation vectors of the three QR codes as the center coordinates of the QR code array to obtain the geometric center coordinates of the QR code array in the camera coordinate system ; Geometric Center p The coordinates are calculated as follows: ； ； ； at last p The coordinates are ; C. Rotate the rotation vector of each QR code Convert to the corresponding rotation matrix ; Since the postures of the three QR codes meet the constraints, indicating that the postures of the three QR codes are the same, to simplify the calculation, all rotation matrices are averaged and orthogonalized to obtain , and then converted back to the rotation vector ; Magnitude of the rotation vector ; Rodriguez formula converts a rotation vector into a rotation matrix : ； in: is the 3*3 identity matrix, is the rotation angle, is the unit vector of the rotation axis, Is The antisymmetric matrix formed is: ； Finally Find the mean and orthogonalize using singular value decomposition to get ; D. The point where the lifting rope and the hook plate are connected is marked as point c ,from p Starting from point, offset along the negative direction of x axis , i.e. the arrival point c , so, through calculate Then we get the translation vector , calculate the visual swing angle through trigonometric functions : ； In step 5, the visual standard deviation The calculation is as follows: For each edge , calculate the standard deviation ; ； in ; Calculate the visual standard deviation based on the standard deviation of each edge : ； In step 9, when When the deviation is considered to be within the normal range, the IMU swing angle If it is reliable and does not require correction, then there is no need to update the IMU correction angle. ; when When , it is considered that the IMU data has a large cumulative error at this moment, and the IMU cumulative error correction angle is updated , re-correct the IMU swing angle .

Images