Heavy-load equipment one-key alignment method under multiple scenes

Abstract

The invention provides a one-key alignment method for heavy-load equipment in multiple scenes, which comprises the following steps of: acquiring a gravity direction component of an equipment body, and establishing a magnetic attitude reference coordinate of the equipment body by combining the reading of a tilt angle sensor and a horizontal reference plane of an optical ranging unit and a calibration point. Capturing the spatial distribution information of the mark points, calculating the offset of the current attitude of the equipment relative to the horizontal reference plane according to the geometric constraint among the mark points, and generating an initial attitude mapping matrix; and reading the angular velocity and the acceleration of the inertial navigation system, determining the deviation between the inertial attitude and the visual attitude, and generating a combined attitude calculation result. The hydraulic cylinder and the rotating table are driven to be synchronously adjusted according to the deviation value between the current attitude of the equipment and the target mounting point, the change trend of the attitude angle is monitored, and suppression feedback is triggered to adjust the fusion weight of the sensor. And calling a micro-motion mechanism to execute a fine pose adjustment instruction, monitoring a pose error in real time, and generating a pose locking report when the pose error is within a tolerance range.

Description

Technical Field

[0001] This invention relates to the field of artificial intelligence technology, and more specifically, to a one-click alignment method for heavy-duty equipment in multiple scenarios. Background Technology

[0002] In the alignment process of heavy-duty equipment, existing methods for adjusting the posture of heavy-duty equipment rely heavily on geomagnetic sensors. Visual geometric constraints are then combined with these sensors. However, this existing technology is susceptible to magnetic interference during application, which can lead to slight drift and result in a small error between the actual alignment of the heavy-duty equipment and the expected posture. Summary of the Invention

[0003] To address the shortcomings of existing technologies, the present invention aims to resolve the aforementioned deficiencies and propose a one-click alignment method for heavy-duty equipment in multiple scenarios.

[0004] The present invention adopts the following technical solution.

[0005] The first aspect of this invention discloses a one-click alignment method for heavy-duty devices in multiple scenarios, the method comprising: The gravity direction component of the device body is obtained, and combined with the readings of the tilt sensor and the horizontal reference plane of the optical ranging unit and the calibration point, the magnetic attitude reference coordinates of the device body are established. The spatial distribution information of the marker points is captured by an industrial camera, and the offset of the current posture of the device relative to the horizontal reference plane is calculated based on the geometric constraints between the marker points to generate an initial posture mapping matrix. The angular velocity and acceleration of the inertial navigation system are read, and the deviation between the inertial predicted attitude and the visual mapped attitude is determined based on the angular velocity and acceleration to generate a joint attitude calculation result of inertial and vision. The hydraulic cylinder is driven to adjust synchronously with the rotary table based on the deviation between the current attitude of the equipment and the target installation point, and the trend of attitude angle change is monitored to trigger suppression feedback to adjust the sensor fusion weights. Based on the device attitude control state under the adjusted sensor fusion weights, the micro-motion mechanism is invoked to execute fine pose adjustment commands, and the attitude error is monitored in real time. When the attitude error is within the tolerance range, an attitude lock report is generated.

[0006] Furthermore, the step of acquiring the gravity direction component of the device body and, in conjunction with the tilt sensor readings and the horizontal reference plane of the optical ranging unit and the calibration point, establishing the magnetic attitude reference coordinates of the device body includes: The device body is fixed in the initial installation position, and acceleration components in different directions are collected in real time by a triaxial accelerometer. The acceleration components in different directions are the acceleration of the device along the X-axis, Y-axis and Z-axis, respectively. When the device is stationary, a gravity direction vector is constructed based on the acceleration components, and the magnitude of gravitational acceleration is calculated based on the gravity direction vector.

[0007] Furthermore, the step of obtaining the gravity direction component of the device body and establishing the magnetic attitude reference coordinates of the device body by combining the tilt sensor readings and the horizontal reference plane of the optical ranging unit and the calibration point also includes: The pitch and roll angles of the device body are calculated based on the gravity direction vector, and the angle signals of the device body are collected in real time by the tilt sensor and compared and corrected with the pitch and roll angles to generate a unified attitude reference plane. The distances between multiple preset calibration points on the installation platform and the equipment body, as well as the height differences of each calibration point in the coordinate system of the equipment body, are measured by the optical ranging unit. The horizontal reference plane is constructed with the equipment body as the origin. The horizontal reference plane is normalized, and the angle between the normal vectors of the attitude reference plane and the normalized horizontal reference plane is calculated. When the angle between the normal vectors exceeds a first threshold, the attitude reference plane is corrected.

[0008] Furthermore, the step of capturing the spatial distribution information of the marker points using an industrial camera and calculating the offset of the device's current pose relative to the horizontal reference plane based on the geometric constraints between the marker points to generate an initial pose mapping matrix includes: The set of marker point image coordinates is obtained by an industrial camera, and the two-dimensional image coordinates in the set of marker point image coordinates are back-projected to three-dimensional space by calling the pinhole imaging model. Combined with the transformation parameters of the camera coordinate system and the magnetic-free coordinate system, a set of three-dimensional coordinates of the marker points in the magnetic-free reference coordinate system is generated. The least squares method is used to fit the actual plane equation of the marker point to determine the plane normal vector coefficient and the normalized unit normal vector. The angle between the unit normal vector and the normal vector of the magnetic exemption reference plane in the magnetic exemption attitude reference coordinate is calculated to determine the attitude offset vector. The attitude offset vector is composed of the pitch offset angle, roll offset angle, and yaw offset angle of the main body of the device.

[0009] Furthermore, the step of capturing the spatial distribution information of the marker points using an industrial camera and calculating the offset of the device's current pose relative to the horizontal reference plane based on the geometric constraints between the marker points to generate an initial pose mapping matrix further includes: The attitude is compensated by the rotation matrix according to the attitude offset vector to obtain the compensated corrected attitude matrix. Based on the three-dimensional coordinate set of the marker point in the magnetic reference coordinate system, a homogeneous transformation relationship between the device body coordinate system and the magnetic reference coordinate system is established to output the initial attitude mapping matrix. In continuous multi-frame visual sampling, the difference in transformation matrices between adjacent frames based on the initial pose mapping matrix is ​​calculated, and a valid pose mapping matrix is ​​output when the difference in transformation matrices does not exceed a second threshold.

[0010] Furthermore, the step of reading the angular velocity and acceleration of the inertial navigation system, and determining the deviation between the inertial predicted attitude and the visually mapped attitude based on the angular velocity and acceleration, to generate a joint attitude calculation result of inertial and vision, includes: The inertial unit acquires the angular velocity and acceleration of the device body according to the set sampling period, and calculates the attitude change matrix of the device body per unit time according to the Euler integral principle. The inertial prediction attitude matrix is ​​then recursively output by combining the effective attitude mapping matrix. Calculate the attitude deviation matrix between the visually mapped attitude matrix and the inertial predicted attitude matrix, and convert the attitude deviation matrix into an equivalent Euler angle error to generate a deviation angle vector; The proportional suppression linear correction model is invoked to constrain the inertial prediction attitude matrix by angle gain based on the deviation angle vector to generate a drift correction coefficient vector. The visual mapping attitude matrix and the inertial prediction attitude matrix are then weighted and fused according to the drift correction coefficient vector. The joint attitude calculation result of inertial and visual methods is output by combining the angle standard deviation of attitudes in multiple consecutive frames.

[0011] Furthermore, the step of driving the hydraulic cylinder and the rotary table to adjust synchronously based on the deviation between the current attitude of the equipment and the target mounting point, and monitoring the changing trend of the attitude angle to trigger the suppression feedback adjustment of sensor fusion weights, includes: Extract the reverse vector of the device spindle from the joint attitude calculation results, and calculate the deviation vector and deviation angle between the device spindle and the target mounting point to output the attitude deviation vector; The attitude deviation vector is decomposed into the motion components of each hydraulic cylinder and the rotary table to generate a set of control commands to be executed, which includes multiple linear driving forces and rotational speeds. When executing the set of control commands, the angular velocity response direction of the main body of the device and the direction of execution control are monitored in real time. When the angular velocity response direction and the direction of execution control are opposite, the fusion weights are adaptively adjusted and the joint attitude calculation result of inertia and vision based on the adjusted weight coefficients is output.

[0012] Furthermore, based on the adjusted sensor fusion weights, the device attitude control state invokes a micro-motion mechanism to execute fine pose adjustment commands and monitors attitude errors in real time. When the attitude error is within the tolerance range, an attitude lock report is generated, including: The standard deviation of the attitude angle fluctuation of the main body of the device is statistically analyzed within multiple consecutive control cycles. When the standard deviation of the attitude angle fluctuation does not exceed the third threshold, a stable control attitude matrix is ​​output. The attitude deviation vector corresponding to the standard deviation of the attitude angle fluctuation exceeding the third threshold is assigned to the micro actuator to obtain a set of fine pose adjustment instructions. When executing the set of fine pose adjustment instructions, the distance value of the calibration point relative to the device body is updated in real time, and the updated attitude tilt angle and the residual deviation between the current device spindle and the target mounting point are calculated to output the corrected attitude matrix. The standard deviation of the distance value between the updated calibration point and the device body is calculated to generate the device position stability index and the device attitude stability index. When the device position stability index and the device attitude stability index meet the set locking conditions, the attitude lock report is generated and output.

[0013] The second aspect of this invention discloses a one-click alignment device for heavy-duty equipment in multiple scenarios, used to implement the one-click alignment method for heavy-duty equipment in multiple scenarios as described in any one of the first aspects, the device comprising: The reference coordinate establishment module is used to obtain the gravity direction component of the device body, and combine it with the tilt sensor readings and the horizontal reference plane of the optical ranging unit and the calibration point to establish the magnetic attitude reference coordinates of the device body. The attitude mapping generation module is used to capture the spatial distribution information of marker points through an industrial camera, and calculate the offset of the current attitude of the device relative to the horizontal reference plane based on the geometric constraints between the marker points, so as to generate an initial attitude mapping matrix. The attitude joint module is used to read the angular velocity and acceleration of the inertial navigation system, and determine the deviation between the inertial predicted attitude and the visual mapped attitude based on the angular velocity and acceleration, so as to generate the joint attitude calculation result of inertial and vision. The weight adjustment module is used to drive the hydraulic cylinder and the rotary table to adjust synchronously according to the deviation between the current attitude of the equipment and the target installation point, and to monitor the changing trend of the attitude angle in order to trigger the suppression feedback to adjust the sensor fusion weight. The device alignment and locking module is used to call the micro-motion mechanism to execute fine pose adjustment commands based on the device attitude control state under the adjusted sensor fusion weights, and to monitor the attitude error in real time, so as to generate an attitude locking report when the attitude error is within the tolerance range.

[0014] A third aspect of the present invention discloses a terminal, including a processor and a storage medium; The storage medium is used to store instructions; The processor is configured to operate according to the instructions to perform the steps of the method described in the first aspect.

[0015] A fourth aspect of the present invention discloses a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method described in the first aspect.

[0016] The beneficial effects of the present invention are as follows: Compared with the prior art, the present invention has the following advantages: (1) Before the operation begins, this invention no longer relies on geomagnetic sensors to establish coordinates. Instead, it uses the direction of gravity as a vertical reference, determines the gravity direction component of the equipment body through a triaxial accelerometer, and establishes an initial attitude reference plane using a high-precision tilt sensor. At the same time, a horizontal reference plane is formed by the optical ranging unit and the calibration point of the installation platform, thereby forming a magnetically independent attitude reference coordinate without magnetic input. This effectively eliminates the interference source of magnetic sensors and provides a stable and physically traceable reference direction for subsequent attitude measurements.

[0017] (2) This invention captures the spatial distribution information of fixed marker points within a scene using an industrial camera. Based on the geometric constraints between the marker points, it calculates the offset of the device's current posture relative to the reference plane and performs preliminary posture compensation to align the device's main axis direction with the visual reference. This generates an initial posture mapping matrix for real-time tracking of the relative relationship between the device's posture and the spatial coordinate system, eliminating the initial drift trend caused by magnetic interference. Simultaneously, continuous correction of the inertial system using the visual reference prevents the accumulation of posture drift over long periods of operation, achieving temporal stability of the posture reference. Attached Figure Description

[0018] Figure 1 This is a flowchart illustrating the one-click alignment method for heavy-duty devices in multiple scenarios provided by the present invention.

[0019] Figure 2 This is a schematic diagram of the structure of the one-click alignment device for heavy-duty equipment in multiple scenarios provided by the present invention. Detailed Implementation

[0020] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0021] like Figure 1 As shown in one embodiment, a one-click alignment method for heavy-duty devices in multiple scenarios includes the following steps: Step S110: Obtain the gravity direction component of the device body, and combine it with the tilt sensor readings and the horizontal reference plane of the optical ranging unit and the calibration point to establish the magnetic attitude reference coordinates of the device body.

[0022] In some embodiments, the one-click alignment method for heavy-duty devices in multiple scenarios provided by the present invention includes the following steps in step S110: Step S111: Fix the device body in the initial installation position and collect acceleration components in different directions in real time using a triaxial accelerometer. The acceleration components in different directions are the acceleration of the device along the X-axis, Y-axis and Z-axis, respectively. Step S112: When the device is stationary, construct the gravity direction vector based on the acceleration components, and calculate the magnitude of gravity acceleration based on the gravity direction vector.

[0023] In some embodiments, the one-click alignment method for heavy-duty devices in multiple scenarios provided by the present invention further includes the following steps in step S110: Step S113: Calculate the pitch and roll angles of the device body based on the gravity direction vector, and compare and correct the angle signals of the device body with the pitch and roll angles in real time through the tilt sensor to generate a unified attitude reference plane.

[0024] Step S114: Measure the distance values ​​of multiple preset calibration points on the installation platform relative to the device body and the height difference of each calibration point in the coordinate system of the device body using the optical ranging unit. Construct a horizontal reference plane with the device body as the origin.

[0025] Step S115: Normalize the horizontal reference plane, calculate the angle between the normal vectors of the attitude reference plane and the normalized horizontal reference plane, and perform attitude correction on the attitude reference plane when the angle between the normal vectors exceeds the first threshold.

[0026] The value of the first threshold (the trigger correction threshold for the angle between two horizontal directions) is affected by the platform's geometric flatness, tilt angle and acceleration noise level, device clamping tolerances, structural deflection caused by load, and the intensity of on-site vibration. The more uneven the platform, the greater the noise, and the heavier the load, the more relaxed the threshold should be. In engineering, a range of approximately 0.5° to 2.0° is recommended, with approximately 1.0° being commonly used.

[0027] In a specific embodiment, the one-click alignment method for heavy-duty devices in multiple scenarios provided by the present invention includes steps 1 to 5: Step 1: Establish magnetic immunity attitude reference and gravity direction lock.

[0028] Before the operation begins, instead of relying on geomagnetic sensors to establish coordinates, the direction of gravity is used as the vertical reference. A triaxial accelerometer determines the gravity direction component of the equipment body, and a high-precision tilt sensor establishes an initial attitude reference plane. Simultaneously, an optical ranging unit and the calibration point of the mounting platform form a horizontal reference plane, thus creating a magnetically independent attitude reference coordinate system without magnetic input. This step effectively eliminates interference from magnetic sensors, providing a stable and physically traceable reference direction for subsequent attitude measurements. It includes the following sub-steps: Sub-step 1.1: Triaxial acceleration acquisition and gravity component extraction.

[0029] Specifically, the device body is fixed in its initial installation position, and acceleration components in three directions are collected in real time using a triaxial accelerometer: acceleration along the X-axis, Y-axis, and Z-axis. In a stationary state, the acceleration measured by the device primarily originates from gravitational acceleration. Therefore, a gravitational direction vector can be constructed based on the aforementioned accelerations along the X-axis, Y-axis, and Z-axis. The magnitude of gravitational acceleration is then the square root of the sum of the squares of the accelerations along the X-axis, Y-axis, and Z-axis.

[0030] To remove electrical noise and sensor drift, the system employs a moving average filter, averaging N consecutive data sets. The value of N ranges from 10 to 50, and is automatically adjusted based on the sensor's sampling frequency (50-200Hz). The filtered output is a stable gravity direction vector.

[0031] Sub-step 1.2: Establishing the attitude plane based on the tilt sensor.

[0032] Specifically, the pitch angle of the device is calculated based on the components of the gravity direction vector. and roll angle The expression is: ; ; In the formula, , , These are the accelerations along the X-axis, Y-axis, and Z-axis, respectively.

[0033] Subsequently, the angle signal of the device is acquired in real time using a high-precision tilt sensor (resolution 0.001°) and compared and corrected with the above acceleration calculation results to output a unified attitude reference plane. , is represented as: ; Sub-step 1.3: Calibrate the horizontal reference plane of the optical ranging unit.

[0034] Specifically, first, three preset calibration points and their coordinates are determined on the installation platform. These coordinates can be obtained using an external laser rangefinder. Then, an optical ranging unit (composed of a laser rangefinder or a 3D scanner) measures the three distances between the three calibration points and the equipment body. The height difference between each calibration point in the equipment body's coordinate system is calculated. Using the equipment body as the origin, and based on the principle of three points determining a plane, a horizontal reference plane is obtained. , is represented as: ; in, , , Calculated from the coordinates of three calibration points: ; ; ; It should be noted that the coordinates of the three calibration points are as follows: , , ), ( , , )as well as( , , ).

[0035] Next, the horizontal reference plane Normalization is performed to make its normal vector length 1, so that it can be aligned with the attitude reference plane later. The integration.

[0036] Sub-step 1.4: Merge the attitude plane and the horizontal plane to establish a magnetic reference coordinate system.

[0037] Specifically, by calculating the attitude reference plane With horizontal reference plane The angle between the normal vectors is used to determine the consistency between the device's attitude and the horizontal plane of the external platform. If the angle between the normal vectors is greater than a set threshold (1°), attitude correction is performed, and the attitude reference plane is adjusted. The correction plane is generated by rotating around the intersection line by an angle equal to half the angle between the normal vectors. This makes the correction plane With horizontal reference plane By maintaining geometric symmetry, a magnetically exempt reference coordinate system is finally established.

[0038] In this system, the Z-axis of the magnetically exempt reference coordinate system is the correction plane. The direction of the normal vector, with the X-axis as the correction plane. With horizontal reference plane The direction of the intersection line, with the Y-axis being the orthogonal direction of the X-axis and Z-axis.

[0039] Sub-step 1.5: Coordinate stability verification and output buffering.

[0040] Specifically, during the stability test of the magnetically exempt reference coordinate system, N sets of attitude data (N=100~200) are continuously sampled, and the standard deviation of each basis vector in time is calculated. If the standard deviation is less than or equal to the set threshold of 0.01, the coordinate system is considered stable; otherwise, the magnetically exempt reference coordinate system is re-established.

[0041] Step S120: The spatial distribution information of the marker points is captured by an industrial camera, and the offset of the current posture of the device relative to the horizontal reference plane is calculated according to the geometric constraints between the marker points to generate an initial posture mapping matrix.

[0042] In some embodiments, the one-click alignment method for heavy-duty devices in multiple scenarios provided by the present invention includes the following steps in step S120: Step S121: Obtain the set of marker point image coordinates through an industrial camera, and call the pinhole imaging model to back-project the two-dimensional image coordinates in the set of marker point image coordinates to three-dimensional space. Combine the transformation parameters of the camera coordinate system and the magnetic-free coordinate system to generate the set of three-dimensional coordinates of the marker points in the magnetic-free reference coordinate system.

[0043] Step S122: Use the least squares method to fit the actual plane equation of the marker point to determine the plane normal vector coefficients and the normalized unit normal vector, and calculate the angle between the unit normal vector and the normal vector of the magnetic exemption reference plane in the magnetic exemption attitude reference coordinate to determine the attitude offset vector.

[0044] The attitude offset vector consists of the pitch offset angle, roll offset angle, and yaw offset angle of the main body of the equipment.

[0045] In some embodiments, the one-click alignment method for heavy-duty devices in multiple scenarios provided by the present invention further includes the following steps in step S120: Step S123: Perform attitude compensation based on the attitude offset vector using the rotation matrix to obtain the compensated corrected attitude matrix. Based on the three-dimensional coordinate set of the marker points in the magnetic reference coordinate system, establish the homogeneous transformation relationship between the device body coordinate system and the magnetic reference coordinate system to output the initial attitude mapping matrix.

[0046] Step S124: In continuous multi-frame visual sampling, calculate the difference of transformation matrix based on the initial pose mapping matrix of adjacent frames, and output the effective pose mapping matrix when the difference of transformation matrix does not exceed the second threshold.

[0047] The value of the second threshold is affected by camera resolution and focal length, lens distortion residuals, the geometric scale and layout of marker points, ranging depth noise, camera frame rate, and exposure jitter. The more stable the imaging and ranging, and the more regular the marker points, the tighter the threshold can be. An engineering recommendation is approximately 0.001 to 0.005.

[0048] In a specific embodiment, the one-click alignment method for heavy-duty equipment in multiple scenarios provided by the present invention includes step 2, local posture mapping and initial error compensation based on visual calibration. The magnetic reference coordinates output in step 1 are input to the visual calibration unit, and the spatial distribution information of fixed marker points within the scene is captured by an industrial camera. Based on the geometric constraints between the marker points, the offset of the current posture of the equipment relative to the reference plane is calculated, and preliminary posture compensation is performed to align the main axis direction of the equipment with the visual reference, thereby generating an initial posture mapping matrix for real-time tracking of the relative relationship between the equipment posture and the spatial coordinate system. This step achieves the first error correction of the equipment posture through visual geometric constraints, eliminating the initial drift trend caused by magnetic interference, and includes the following sub-steps: Sub-step 2.1: Visual landmark acquisition and spatial geometric positioning.

[0049] Specifically, using the pinhole imaging model of the camera, the two-dimensional image coordinates of the set of image coordinates of fixed markers in the scene are captured by the industrial camera. The back projection onto three-dimensional space is expressed as: ; In the formula, This represents the actual distance from the i-th marker point to the camera, obtained by binocular ranging or laser ranging, with a range of 0.5-10m; The camera intrinsic parameter matrix is ​​pre-determined by the calibration plate; Let be the three-dimensional spatial coordinates of the i-th marker point in the camera coordinate system.

[0050] Then, the coordinates of the marker point in the magnetic reference coordinate system are determined by the transformation relationship between the camera coordinate system and the magnetic exemption coordinate system.

[0051] Sub-step 2.2: Calculation of geometric constraints and determination of attitude offset of marker points.

[0052] Specifically, the correction plane in the magnetic exemption reference coordinate system is determined by fitting the actual plane equation of the set of coordinates of marker points in the magnetic exemption reference coordinate system using the least squares method. The normal vector coefficients are then normalized to obtain the unit normal vector. Next, the magnetically exempted reference correction plane is taken. The normal vector of the reference plane is used to calculate the angle between the normal vector of the reference plane and the normalized unit normal vector. Then, the attitude offset rotation axis vector is obtained through the cross product. Finally, the attitude offset vector is determined by small-angle approximation, including the device pitch offset angle, roll offset angle, and yaw offset angle, with the angle range not exceeding ±3°.

[0053] Sub-step 2.3, attitude error compensation and spindle alignment control.

[0054] Specifically, based on the aforementioned attitude offset vector, an attitude compensation correction is performed using a rotation matrix, which is expressed as: ; In the formula, The corrected attitude matrix after compensation; The attitude matrix of the current device is obtained from inertial calculation; This represents the compensation matrix for the reverse rotation about the corresponding three Euler angles.

[0055] Then, based on the compensated corrected attitude matrix, the corresponding compensation operation is performed, namely the automatic adjustment of the hydraulic cylinder or servo turntable, so that the main axis direction of the equipment is aligned with the normal vector of the visual reference plane again.

[0056] Sub-step 2.4: Construct the initial pose mapping matrix.

[0057] Specifically, the homogeneous transformation relationship between the equipment coordinate system and the magnetic immunity reference coordinate system is established, expressed as: ; ; In the formula, The corrected attitude matrix after compensation; The translation vector represents the origin of the device coordinate system. The amount of translation relative to the origin of the magnetically exempt reference coordinate system; The centroid coordinates of the set of marker points are used to determine the center position of the equipment; This is the initial attitude mapping matrix that describes the complete attitude and position mapping relationship of the device body in the magnetic reference coordinate system.

[0058] Sub-step 2.5: Verification and cached output of pose mapping results.

[0059] Specifically, the initial pose mapping matrix is ​​verified using multi-frame visual data of the marker points. The stability is determined by the following criteria: if the difference between the transformation matrices calculated in adjacent frames is less than a set threshold (the threshold value is less than or equal to 0.002) in N consecutive frames (N=50~100) of visual sampling, the attitude mapping matrix is ​​considered stable and is cached for later use; otherwise, the device attitude matrix is ​​recompensated and corrected.

[0060] Step S130: Read the angular velocity and acceleration of the inertial navigation system, and determine the deviation between the inertial predicted attitude and the visual mapped attitude based on the angular velocity and acceleration, so as to generate a joint attitude calculation result of inertial and vision. The joint attitude calculation result is used to describe the precise attitude result of the device in the three directions of pitch angle, roll angle and yaw angle in space at this moment.

[0061] In some embodiments, the one-click alignment method for heavy-duty devices in multiple scenarios provided by the present invention includes the following steps in step S130: Step S131: The angular velocity and acceleration of the device body are obtained by the inertial unit according to the set sampling period, and the attitude change matrix of the device body per unit time is calculated according to the Euler integral principle. The inertial prediction attitude matrix is ​​then recursively output by combining the effective attitude mapping matrix.

[0062] Step S132: Calculate the attitude deviation matrix between the visually mapped attitude matrix and the inertial predicted attitude matrix, and convert the attitude deviation matrix into an equivalent Euler angle error to generate a deviation angle vector.

[0063] Step S133: The proportional suppression linear correction model is invoked to constrain the angle gain of the inertial prediction attitude matrix based on the deviation angle vector to generate the drift correction coefficient vector. The visual mapping attitude matrix and the inertial prediction attitude matrix are then weighted and fused according to the drift correction coefficient vector. The joint attitude calculation result of inertial and visual is output by combining the angle standard deviation of attitudes in multiple consecutive frames.

[0064] In a specific embodiment, the one-click alignment method for heavy-duty equipment in multiple scenarios provided by the present invention includes step 3, joint drift calibration of the inertial unit and visual mapping. The initial attitude mapping matrix output in step 2 is input into the inertial unit, which simultaneously reads the angular velocity and acceleration information of the inertial navigation system. By comparing the deviation between the inertial predicted attitude and the visually mapped attitude in real time, a drift correction model is established. Using the direction of gravity as a constraint, the inertial calculation results are dynamically adjusted to gradually approach the visual reference value, forming a joint attitude calculation result of inertial and visual parameters, which is then output to subsequent steps for alignment control. This step continuously corrects the inertial system using a visual reference, preventing the accumulation of attitude drift over long periods of operation and achieving time stability of the attitude reference. It includes the following sub-steps: Sub-step 3.1, inertial attitude prediction and integral solution.

[0065] Specifically, the inertial unit outputs angular velocity and acceleration data at a set sampling period (range 0.005-0.02s), and calculates the attitude change matrix per unit time based on the Euler integral principle. The expression is: ; In the formula, , , These are single-axis rotation matrices for rotation around the X, Y, and Z axes, respectively.

[0066] To prevent excessive integration errors caused by short-term vibrations, the Z-axis directional deviation is corrected by comparing the acceleration direction with the gravity direction vector. The corrected angle is then substituted into the Euler integral principle for calculation.

[0067] Sub-step 3.2: Visual and inertial posture comparison and deviation calculation.

[0068] Specifically, the visual pose matrix (i.e., the rotated part of the initial pose mapping matrix) and the inertial predicted pose matrix (i.e., the pose change matrix per unit time) are calculated. The relative rotation matrix between the visual attitude matrix and the inertial predicted attitude matrix, i.e., the attitude deviation matrix, is equal to the product of the visual attitude matrix and the inertial predicted attitude matrix. Then, the attitude deviation matrix is ​​converted into an equivalent Euler angle error representation, resulting in a deviation angle vector composed of pitch deviation angle, roll deviation angle, and yaw deviation angle. Furthermore, to enhance matrix stability, a sliding window smoothing filter is used to perform a weighted average of the attitude deviations from the most recent N frames (N=10-20) to remove transient disturbances.

[0069] Sub-step 3.3: Establish the drift correction model and calculate the compensation coefficients.

[0070] Specifically, an angle gain constraint is applied to the integral result of the inertial attitude using a proportional suppression linear correction model, expressed as: ; ; ; In the formula, , , These are pitch deviation angles. Roll deviation angle and yaw deviation angle The upper limit of the allowable attitude deviation is 3-5°. , , The corresponding update weights for inertial attitude are reduced when the deviation increases.

[0071] Sub-step 3.4, joint attitude correction and dynamic fusion calculation.

[0072] Specifically, based on the principle of reducing the update weight of inertial attitude when the deviation increases, a weighted fusion method is adopted to jointly correct the inertial predicted attitude matrix and the visual attitude matrix, so as to output the weighted fused joint attitude matrix of inertial and visual attitude, and the joint attitude matrix is ​​orthogonalized to ensure that the column vectors of the matrix remain unit orthogonal.

[0073] Sub-step 3.5: Verification and output caching of joint attitude calculation results.

[0074] Specifically, N frames (N=50-100) of attitude calculation results are continuously collected, and the standard deviation of the angle in the time series is calculated. If the standard deviation is less than or equal to 0.05°, the attitude fusion is considered stable, and the joint attitude calculation result at this time is output and cached; otherwise, the self-calibration mechanism is triggered, and drift correction and weighted fusion are performed again.

[0075] Step S140: Drive the hydraulic cylinder and the rotary table to adjust synchronously according to the deviation between the current attitude of the equipment and the target installation point, and monitor the trend of attitude angle change to trigger the suppression feedback to adjust the sensor fusion weight.

[0076] In some embodiments, the one-click alignment method for heavy-duty devices in multiple scenarios provided by the present invention includes the following steps in step S140: Step S141: Extract the reverse vector of the device spindle from the joint attitude calculation results, and calculate the deviation vector and deviation angle between the device spindle and the target mounting point to output the attitude deviation vector.

[0077] Step S142: Decompose the attitude deviation vector into the motion components of each hydraulic cylinder and the rotary table to generate a set of control commands to be executed. The set of control commands includes multiple linear driving forces and rotational speeds.

[0078] Step S143: When executing the set of control commands, monitor the angular velocity response direction of the main body of the device and the direction of execution control in real time. When the angular velocity response direction is opposite to the direction of execution control, adaptively adjust the fusion weight and output the joint attitude calculation result of inertia and vision based on the adjusted weight coefficient.

[0079] In a specific embodiment, the one-click alignment method for heavy-duty equipment in multiple scenarios provided by the present invention includes step 4: dynamic drift suppression feedback and attitude closed-loop control. The joint attitude calculation result output in step 3 is input to the dynamic control unit. This unit drives the hydraulic cylinder and the rotary table to adjust synchronously according to the deviation between the current attitude and the target mounting point. At the same time, it monitors the drift rate of the inertial unit during the adjustment process. Once it detects that the trend of attitude angle change is inconsistent with the control direction, it immediately triggers dynamic suppression feedback to adjust the sensor fusion weights and prevent drift amplification caused by magnetic interference. This closed-loop control process keeps the main axis direction of the equipment aligned with the target mounting direction in real time. This step constructs a closed-loop adjustment of "calculation → control → feedback", which can ensure that the equipment's attitude is stable and does not deviate in a dynamic environment. It includes the following sub-steps: Sub-step 4.1: Attitude deviation calculation and target vector establishment.

[0080] Specifically, firstly, the device spindle direction vector is extracted from the cached joint attitude calculation results, which represents the acceleration components of the device's current spindle in the magnetically exempt reference coordinate system. Then, the attitude deviation vector between the device spindle and the target mounting direction, as well as the deviation angle used to determine the attitude correction magnitude, are calculated. If this deviation angle exceeds the allowable angle tolerance (range 0.2-0.5°), the attitude correction control process is initiated.

[0081] Sub-step 4.2, calculation of coordinated adjustment between hydraulic cylinder and rotary table.

[0082] Specifically, the attitude deviation vector is decomposed into the motion components of each hydraulic cylinder and the rotary table, and a set of control commands is established through a control matrix. The control matrix is ​​a 3×3 control allocation matrix, whose elements are the response coefficients of any actuator to attitude deviation in any direction. The established control commands include three linear driving forces and one rotation angle.

[0083] In this embodiment, the hydraulic cylinder provides attitude correction torque in the pitch and roll directions, the rotary table is responsible for fine-tuning the yaw angle, and the control system predicts the inertial attitude response delay in advance based on the sign change of the real-time inertial drift rate, and introduces time compensation (range 0.01-0.05s) when outputting commands to avoid execution overshoot.

[0084] Sub-step 4.3, dynamic drift detection and feedback suppression mechanism.

[0085] Specifically, the system monitors the real-time trend of the inertial unit's angular velocity change and the degree of matching with the direction of control. When the inertial response direction is detected to be opposite to the control direction (i.e., the product of any linear driving force and its weight is less than 0), it is determined to be a drift amplification trend. At this time, adaptive adjustment of the fusion weights is performed, and the expression is: ; In the formula, The initial fusion weights range from 0.6 to 0.9. The dynamic inhibition coefficient ranges from 0.1 to 0.5. This is the upper limit of the rated angular velocity of the inertial unit; The updated fusion weights are adjusted to reduce the weight of the inertial navigation components in attitude fusion, thereby mitigating the drift accumulation effect.

[0086] Sub-step 4.4: Joint attitude update and closed-loop control execution.

[0087] Specifically, the inertial and visual pose fusion results are recalculated based on the updated fusion weights to obtain the updated closed-loop pose matrix. The updated closed-loop pose matrix and the pose feedback from the actuator need to be compared in real time. When the pose error is lower than the set threshold (0.1°), the pose is automatically locked; otherwise, the control iteration continues.

[0088] Sub-step 4.5: Closed-loop control stability verification and attitude buffer output.

[0089] Specifically, within N consecutive control cycles (N=50-100), the standard deviation of attitude angle fluctuation is calculated. If the standard deviation of attitude angle fluctuation is less than or equal to 0.03°, the system is considered to have entered a stable state, the closed-loop attitude matrix at this time is output, and the current control parameters are recorded; otherwise, dynamic adjustment is performed again.

[0090] Step S150: Based on the device attitude control state under the adjusted sensor fusion weights, the micro-motion mechanism is invoked to execute fine pose adjustment commands, and the attitude error is monitored in real time. When the attitude error is within the tolerance range, an attitude lock report is generated.

[0091] In some embodiments, the one-click alignment method for heavy-duty devices in multiple scenarios provided by the present invention includes the following steps in step S150: Step S151: Statistically measure the standard deviation of the attitude angle fluctuation of the main body of the device within multiple consecutive control cycles, so as to output a stable control attitude matrix when the standard deviation of the attitude angle fluctuation does not exceed the third threshold, and allocate the attitude deviation vector corresponding to the standard deviation of the attitude angle fluctuation exceeding the third threshold to the micro actuator to obtain a set of fine pose adjustment instructions.

[0092] Step S152: When executing the set of fine pose adjustment instructions, update the distance value of the calibration point relative to the device body in real time, and calculate the updated attitude tilt angle and the residual deviation between the current device spindle and the target mounting point to output the corrected attitude matrix.

[0093] Step S153: Calculate the standard deviation of the distance value between the updated calibration point and the device body to generate the device position stability index and the device attitude stability index. When the device position stability index and the device attitude stability index meet the set locking conditions, generate and output the attitude lock report.

[0094] In a specific embodiment, the one-click alignment method for heavy-duty equipment in multiple scenarios provided by the present invention includes step 5, high-precision alignment execution and final attitude confirmation. The attitude control state output in step 4 is input to the fine alignment unit. This unit activates a micro-motion mechanism to perform fine pose adjustments, aligning the equipment's positioning holes or flange faces with the target interface. Simultaneously, a laser ranging unit confirms in real time whether the attitude error is within the tolerance range (e.g., within 0.5mm). If the detection result is stable and the repeatability error is less than a set threshold, the system automatically locks the attitude and outputs an alignment completion signal. This step achieves high-precision automatic alignment in strong magnetic interference scenarios, ensuring that the equipment's attitude does not drift systematically during long-term operation. It includes the following sub-steps: Sub-step 5.1: Calculation and allocation of displacement of the micro-actuator.

[0095] Specifically, the attitude deviation vector in the aforementioned three-dimensional coordinate system is assigned to three independent micro-actuators. A set of micro-motion displacement commands is generated through a control matrix. This control matrix is ​​a 3×3 allocation matrix, and its elements represent the proportional coefficient of any micro-actuator along any direction, with values ​​ranging from 0.3 to 1.0. The generated set of micro-motion displacement commands consists of the displacement amounts of the three micro-actuators, in mm, with a resolution better than 0.01 mm. Then, a range check is performed based on the maximum stroke of each micro-actuator (generally not exceeding 10 mm). If the limit is exceeded, the compensation amounts for each axis are automatically redistributed.

[0096] Sub-step 5.2: Attitude dynamic correction and laser ranging feedback closed loop.

[0097] Specifically, after the micro-motion mechanism executes the displacement action according to the micro-motion execution displacement command set, the laser ranging unit updates the distance values ​​of the three marker points and the new attitude tilt angle in real time, while calculating the residual deviation between the current main axis center and the target point. Then, the attitude matrix is ​​updated in real time based on the change in tilt angle, and sampling is repeated 5-10 times to ensure the system enters a stable detection state.

[0098] Sub-step 5.3, stability verification and repeatability error assessment.

[0099] Specifically, the standard deviation of the ranging data is calculated to obtain the position stability index and attitude stability index. The position stability index is the standard deviation of displacement fluctuation, and the attitude stability index is the standard deviation of attitude angle fluctuation. If the position stability index is less than or equal to 0.05 mm and the attitude stability index is less than or equal to 0.02°, the system is considered to be in stable alignment, and the locking evaluation result is output.

[0100] Sub-step 5.4: Attitude locking and alignment completion signal output.

[0101] Specifically, when the evaluation parameters meet the locking conditions, the micro-motion mechanism is controlled to enter the locking mode, driving the mechanical locking pin to insert into the positioning hole; the attitude matrix at this time is stored as the final attitude state matrix; a digital signal is generated to indicate that the alignment is complete, and the alignment process log is recorded, including execution time, final attitude deviation, and attitude angle information. If the evaluation result does not meet the standard, the micro-motion mechanism is readjusted, forming a self-circulating correction process.

[0102] The following describes the one-click alignment device for heavy-duty equipment in multiple scenarios provided by the present invention. The one-click alignment device for heavy-duty equipment in multiple scenarios described below and the one-click alignment method for heavy-duty equipment in multiple scenarios described above can be referred to in correspondence with each other.

[0103] like Figure 2 As shown in one embodiment, a one-click alignment device for heavy-duty devices in multiple scenarios includes a reference coordinate establishment module, an attitude mapping generation module, an attitude joint module, a weight adjustment module, and a device alignment locking module.

[0104] The reference coordinate establishment module is used to obtain the gravity direction component of the device body, and combine it with the tilt sensor readings and the horizontal reference plane of the optical ranging unit and the calibration point to establish the magnetic attitude reference coordinates of the device body.

[0105] The attitude mapping generation module is used to capture the spatial distribution information of marker points through an industrial camera, and calculate the offset of the current attitude of the device relative to the horizontal reference plane based on the geometric constraints between the marker points, so as to generate an initial attitude mapping matrix.

[0106] The attitude joint module is used to read the angular velocity and acceleration of the inertial navigation system, and determine the deviation between the inertial predicted attitude and the visual mapped attitude based on the angular velocity and acceleration, so as to generate the joint attitude solution results of inertial and vision.

[0107] The weight adjustment module is used to drive the hydraulic cylinder and the rotary table to adjust synchronously according to the deviation between the current attitude of the equipment and the target mounting point, and to monitor the changing trend of the attitude angle in order to trigger the suppression feedback to adjust the sensor fusion weight.

[0108] The device alignment and locking module is used to call the micro-motion mechanism to execute fine pose adjustment commands based on the device attitude control state under the adjusted sensor fusion weights, and to monitor the attitude error in real time, so as to generate an attitude locking report when the attitude error is within the tolerance range.

[0109] The embodiments described above are merely illustrative of implementation methods of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of this patent should be defined by the appended claims.

Claims

1. A one-click alignment method for heavy-duty equipment in multiple scenarios, characterized in that, include: The gravity direction component of the device body is obtained, and combined with the readings from the tilt sensor and the horizontal reference plane of the optical ranging unit and the calibration point, the magnetic attitude reference coordinates of the device body are established. The spatial distribution information of the marker points is captured by an industrial camera, and the offset of the current posture of the device relative to the horizontal reference plane is calculated based on the geometric constraints between the marker points to generate an initial posture mapping matrix. The angular velocity and acceleration of the inertial navigation system are read, and the deviation between the inertial predicted attitude and the visual mapped attitude is determined based on the angular velocity and acceleration to generate a joint attitude calculation result of inertial and vision. The hydraulic cylinder is driven to adjust synchronously with the rotary table based on the deviation between the current attitude of the equipment and the target installation point, and the trend of attitude angle change is monitored to trigger suppression feedback to adjust the sensor fusion weights. Based on the device attitude control state under the adjusted sensor fusion weights, the micro-motion mechanism is invoked to execute fine pose adjustment commands, and the attitude error is monitored in real time. When the attitude error is within the tolerance range, an attitude lock report is generated.

2. The one-click alignment method for heavy-duty equipment in multiple scenarios according to claim 1, characterized in that, The process of acquiring the gravity direction component of the device body and, in conjunction with the tilt sensor readings and the horizontal reference plane of the optical ranging unit and the calibration point, establishing the magnetic attitude reference coordinates of the device body includes: The device body is fixed in the initial installation position, and acceleration components in different directions are collected in real time by a triaxial accelerometer. The acceleration components in different directions are the acceleration of the device along the X-axis, Y-axis and Z-axis, respectively. When the device is stationary, a gravity direction vector is constructed based on the acceleration components, and the magnitude of gravitational acceleration is calculated based on the gravity direction vector.

3. The one-click alignment method for heavy-duty equipment in multiple scenarios according to claim 2, characterized in that, The process of acquiring the gravity direction component of the device body and establishing the magnetic attitude reference coordinates of the device body by combining the tilt sensor readings and the horizontal reference plane of the optical ranging unit and the calibration point, further includes: The pitch and roll angles of the device body are calculated based on the gravity direction vector, and the angle signals of the device body are collected in real time by the tilt sensor and compared and corrected with the pitch and roll angles to generate a unified attitude reference plane. The distances between multiple preset calibration points on the installation platform and the equipment body, as well as the height differences of each calibration point in the coordinate system of the equipment body, are measured by the optical ranging unit. The horizontal reference plane is constructed with the equipment body as the origin. The horizontal reference plane is normalized, and the angle between the normal vectors of the attitude reference plane and the normalized horizontal reference plane is calculated. When the angle between the normal vectors exceeds a first threshold, the attitude reference plane is corrected.

4. The one-click alignment method for heavy-duty equipment in multiple scenarios according to claim 1, characterized in that, The step of capturing the spatial distribution information of marker points using an industrial camera and calculating the offset of the device's current pose relative to the horizontal reference plane based on the geometric constraints between the marker points to generate an initial pose mapping matrix includes: The set of marker point image coordinates is obtained by an industrial camera, and the two-dimensional image coordinates in the set of marker point image coordinates are back-projected to three-dimensional space by calling the pinhole imaging model. Combined with the transformation parameters of the camera coordinate system and the magnetic-free coordinate system, a set of three-dimensional coordinates of the marker points in the magnetic-free reference coordinate system is generated. The least squares method is used to fit the actual plane equation of the marker point to determine the plane normal vector coefficient and the normalized unit normal vector. The angle between the unit normal vector and the normal vector of the magnetic exemption reference plane in the magnetic exemption attitude reference coordinate is calculated to determine the attitude offset vector. The attitude offset vector is composed of the pitch offset angle, roll offset angle, and yaw offset angle of the main body of the device.

5. The one-click alignment method for heavy-duty equipment in multiple scenarios according to claim 4, characterized in that, The step of capturing the spatial distribution information of marker points using an industrial camera and calculating the offset of the device's current pose relative to the horizontal reference plane based on the geometric constraints between the marker points to generate an initial pose mapping matrix further includes: The attitude is compensated by the rotation matrix according to the attitude offset vector to obtain the compensated corrected attitude matrix. Based on the three-dimensional coordinate set of the marker point in the magnetic reference coordinate system, a homogeneous transformation relationship between the device body coordinate system and the magnetic reference coordinate system is established to output the initial attitude mapping matrix. In continuous multi-frame visual sampling, the difference in transformation matrices between adjacent frames based on the initial pose mapping matrix is ​​calculated, and a valid pose mapping matrix is ​​output when the difference in transformation matrices does not exceed a second threshold.

6. The one-click alignment method for heavy-duty equipment in multiple scenarios according to claim 5, characterized in that, The process of reading the angular velocity and acceleration of the inertial navigation system, and determining the deviation between the inertial predicted attitude and the visual mapped attitude based on the angular velocity and acceleration, to generate a joint attitude calculation result of inertial and vision, includes: The inertial unit acquires the angular velocity and acceleration of the device body according to the set sampling period, and calculates the attitude change matrix of the device body per unit time according to the Euler integral principle. The inertial prediction attitude matrix is ​​then recursively output by combining the effective attitude mapping matrix. Calculate the attitude deviation matrix between the visually mapped attitude matrix and the inertial predicted attitude matrix, and convert the attitude deviation matrix into an equivalent Euler angle error to generate a deviation angle vector; The proportional suppression linear correction model is invoked to constrain the inertial prediction attitude matrix by angle gain based on the deviation angle vector to generate a drift correction coefficient vector. The visual mapping attitude matrix and the inertial prediction attitude matrix are then weighted and fused according to the drift correction coefficient vector. The joint attitude calculation result of inertial and visual methods is output by combining the angle standard deviation of attitudes in multiple consecutive frames.

7. The one-click alignment method for heavy-duty equipment in multiple scenarios according to claim 1, characterized in that, The method of driving the hydraulic cylinder and the rotary table to adjust synchronously based on the deviation between the current attitude of the equipment and the target mounting point, and monitoring the trend of attitude angle changes to trigger the suppression feedback adjustment of sensor fusion weights, includes: Extract the reverse vector of the device spindle from the joint attitude calculation results, and calculate the deviation vector and deviation angle between the device spindle and the target mounting point to output the attitude deviation vector; The attitude deviation vector is decomposed into the motion components of each hydraulic cylinder and the rotary table to generate a set of control commands to be executed, which includes multiple linear driving forces and rotational speeds. When executing the set of control commands, the angular velocity response direction of the main body of the device and the direction of execution control are monitored in real time. When the angular velocity response direction and the direction of execution control are opposite, the fusion weights are adaptively adjusted and the joint attitude calculation result of inertia and vision based on the adjusted weight coefficients is output.

8. The one-click alignment method for heavy-duty equipment in multiple scenarios according to claim 7, characterized in that, The device attitude control state based on the adjusted sensor fusion weights calls the micro-motion mechanism to execute fine pose adjustment commands and monitors the attitude error in real time. When the attitude error is within the tolerance range, an attitude lock report is generated, including: The standard deviation of the attitude angle fluctuation of the main body of the device is statistically analyzed within multiple consecutive control cycles. When the standard deviation of the attitude angle fluctuation does not exceed the third threshold, a stable control attitude matrix is ​​output. The attitude deviation vector corresponding to the standard deviation of the attitude angle fluctuation exceeding the third threshold is assigned to the micro actuator to obtain a set of fine pose adjustment instructions. When executing the set of fine pose adjustment instructions, the distance value of the calibration point relative to the device body is updated in real time, and the updated attitude tilt angle and the residual deviation between the current device spindle and the target mounting point are calculated to output the corrected attitude matrix. The standard deviation of the distance value between the updated calibration point and the device body is calculated to generate the device position stability index and the device attitude stability index. When the device position stability index and the device attitude stability index meet the set locking conditions, the attitude lock report is generated and output.

9. A terminal, comprising a processor and a storage medium; characterized in that: The storage medium is used to store instructions; The processor is configured to operate according to the instructions to execute the steps of the one-click alignment method for heavy-duty devices in multiple scenarios according to any one of claims 1-8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the one-click alignment method for heavy-duty devices in multiple scenarios as described in any one of claims 1-8.

Images