CN-121994211-A - Double-curvature fusion curved surface SLAM navigation method, system, medium and product

CN121994211ACN 121994211 ACN121994211 ACN 121994211ACN-121994211-A

Abstract

The invention provides a double-curvature fusion curved surface SLAM navigation method, which comprises the steps of estimating longitudinal and transverse bidirectional curvature parameters in real time through double IMU fusion, constructing an expansion information filtering frame comprising a double-curvature kinematic model, a laser data curved surface projection compensation algorithm and a scheme of a path double-curvature compensation algorithm, accurately describing the geometric constraint of a curved surface where a robot is actually located, estimating and fusing the transverse curvature parameters on line, inhibiting accumulation of transverse positioning errors, realizing accurate description and compensation of a robot sideslip effect through establishing the kinematic model comprising a double-curvature coupling item, eliminating data distortion through projecting laser scanning data onto an estimated curved surface from a plane, improving environmental perception precision, and finally ensuring the consistency of a control instruction and the geometric of the curved surface based on real-time geometric compensation of a planned path, and enhancing the stability and track tracking precision of a control system under the longitudinal and transverse curvature coupling effect.

Inventors

YIN LI

Assignees

天津卡雷尔机器人技术有限公司

Dates

Publication Date: 20260508
Application Date: 20260410

Claims (10)

1. The double-curvature fusion curved surface SLAM navigation method is characterized by comprising the following steps of: S1, acquiring a pitch angle of a robot in a running direction and a transverse rolling angle of the robot; S2, based on the pitch angle and the roll angle, estimating double curvature parameters of the working curved surface in real time through nonlinear optimization, wherein the double curvature parameters at least comprise a longitudinal curvature radius and an elliptic parameter describing transverse curvature; S3, constructing an expansion information filtering frame which comprises the pitch angle and the roll angle as state variables, establishing a robot kinematic model which fuses longitudinal curvature and transverse curvature based on the double-curvature parameters, executing synchronous positioning and mapping, and outputting a state vector of the robot; s4, acquiring original scanning data of a laser radar, and based on the pitch angle and the roll angle which are currently estimated, performing projection compensation on the original scanning data from a scanning plane to a curved surface defined by the double curvature parameter, and generating compensated observation data for updating the expansion information filtering; And S5, performing geometric compensation on a planned path based on the double curvature parameters and the state vector, and generating a control instruction to drive the robot to move.
2. The dual-curvature fused-surface SLAM navigation method of claim 1, wherein in S2, the nonlinear optimization uses a Levenberg-Marquardt algorithm to solve the dual-curvature parameter by minimizing a residual function composed of the pitch angle variation, the roll angle variation, and the dual-curvature parameter.
3. The dual-curvature fusion curved surface SLAM navigation method of claim 1, wherein in S3, the kinematic model comprises: the relation between the longitudinal speed and the change rate of the pitch angle is satisfied that the change rate of the pitch angle is equal to the longitudinal speed divided by the longitudinal curvature radius; The relation between the transverse speed and the change rate of the rolling angle is satisfied that the change rate of the rolling angle is equal to the transverse speed divided by the transverse curvature radius function value; Wherein the transverse curvature radius function is an elliptic function varying with transverse position.
4. The dual-curvature fusion curved surface SLAM navigation method of claim 1, wherein in S3, in the step of updating the extended information filtering, the observed information is weighted by Huber loss function to reduce the influence of abnormal observed values.
5. The dual-curvature fusion curved surface SLAM navigation method of claim 1, wherein S4 is specifically: and converting the original laser point in the polar coordinate form into a three-dimensional Cartesian coordinate through a preset projection compensation formula, wherein the projection compensation formula comprises the current pitch angle and the current roll angle as compensation parameters.
6. The double-curvature fusion curved surface SLAM navigation method of claim 1, wherein in S5, when compensating for the lateral distance, a compensation formula based on elliptic integral or a binomial approximation formula thereof is adopted to convert the horizontal lateral coordinate of the target point into the lateral coordinate on the curved surface.
7. The dual-curvature fused-surface SLAM navigation method according to any one of claims 1-6, further comprising S6 of monitoring an estimated covariance of an innovation sequence of the extended information filtering framework and the dual-curvature parameters in real time, and triggering a failure recovery mechanism of state rollback or parameter reset when detecting that a positioning divergence or parameter estimation is not reliable.
8. A hyperbolic fusion curved surface SLAM navigation system for implementing the hyperbolic fusion curved surface SLAM navigation method of any of claims 1-7, comprising: The sensor module comprises a main inertial measurement unit for measuring a pitch angle, an auxiliary inertial measurement unit for measuring a roll angle and a laser radar; the data processing and estimating module is configured to execute S2 and estimate double curvature parameters in real time; the fusion positioning and mapping module is configured to execute S3 and S4, realize synchronous positioning and mapping based on the double curvature parameters and the compensated laser data, and output a state vector of the robot; and the path planning and control module is configured to execute S5, and generate a control instruction based on the double curvature parameter and the state vector.
9. A non-transitory computer readable storage medium having stored thereon a computer program, wherein the computer program when executed by a processor implements the steps of the dual curvature fused curved surface SLAM navigation method of any of claims 1-7.
10. A computer program product comprising a computer program which, when executed by a processor, implements the steps of the double curvature fused curved SLAM navigation method of any of claims 1 to 7.

Description

Double-curvature fusion curved surface SLAM navigation method, system, medium and product Technical Field The invention relates to the technical field of unmanned aerial vehicle navigation, in particular to a double-curvature fusion curved surface SLAM navigation method and system. Background With the development of the wind power industry in the large-scale and intelligent directions, the regular detection and maintenance of the wind power generator blades become a key link for guaranteeing the safety of equipment and improving the power generation efficiency. The traditional manual detection mode has the problems of low efficiency, high risk, poor consistency and the like, and the adoption of the wall climbing robot or unmanned aerial vehicle to carry detection equipment for automatic inspection has become a clear technical trend. However, the inner wall of the fan blade is a working space with complex geometric characteristics, namely, the fan blade has upward warping (longitudinal curvature) along the length direction of the blade and simultaneously presents an elliptic section (transverse curvature) along the width direction, so that a typical double-curvature curved surface environment is formed, and the special geometric characteristics bring serious challenges to high-precision autonomous positioning and navigation SLAM (Simultaneous Localization AND MAPPING and positioning and mapping) of a robot. Currently, the synchronous positioning and mapping (SLAM) technology of robots applied to curved surface environments is mostly developed based on a single curvature assumption or plane approximation. For example, in the context of pipes, tanks, etc., only a single curvature, either axial or circumferential, is typically compensated. When the method is directly applied to the inner wall of the fan blade, the unmodeled transverse curvature can cause cumulative positioning errors when the robot moves laterally due to the fact that the double curvature geometric features of the method cannot be completely described, and the sideslip effect in the actual movement of the robot cannot be accurately described and compensated due to incomplete model. Causing the robot to face lateral positioning error accumulation, motion sideslip uncompensation, sensor data distortion, and control system instability under multi-curvature coupling. Disclosure of Invention The invention provides a double-curvature fusion curved surface SLAM navigation method and a double-curvature fusion curved surface SLAM navigation system, which are used for solving the technical problems that a robot faces transverse positioning error accumulation, uncompensated motion sideslip, sensor data distortion and unstable control system under multi-curvature coupling caused by navigation technology in the prior art. The invention provides a double-curvature fusion curved surface SLAM navigation method, which comprises the following steps: S1, acquiring a pitch angle of a robot in a running direction and a transverse rolling angle of the robot; S2, based on the pitch angle and the roll angle, estimating double curvature parameters of the working curved surface in real time through nonlinear optimization, wherein the double curvature parameters at least comprise a longitudinal curvature radius and an elliptic parameter describing transverse curvature; S3, constructing an expansion information filtering frame which comprises the pitch angle and the roll angle as state variables, establishing a robot kinematic model which fuses longitudinal curvature and transverse curvature based on the double-curvature parameters, executing synchronous positioning and mapping, and outputting a state vector of the robot; s4, acquiring original scanning data of a laser radar, and based on the pitch angle and the roll angle which are currently estimated, performing projection compensation on the original scanning data from a scanning plane to a curved surface defined by the double curvature parameter, and generating compensated observation data for updating the expansion information filtering; And S5, performing geometric compensation on a planned path based on the double curvature parameters and the state vector, and generating a control instruction to drive the robot to move. According to the double-curvature fusion curved surface SLAM navigation method provided by the invention, in the step S2, the nonlinear optimization adopts a Levenberg-Marquardt algorithm, and the double-curvature parameter is solved by minimizing a residual function formed by the pitching angle variation, the rolling angle variation and the double-curvature parameter. According to the dual-curvature fusion curved surface SLAM navigation method provided by the invention, in the step S3, the kinematic model comprises the following steps: the relation between the longitudinal speed and the change rate of the pitch angle is satisfied that the change rate of the pitch angle is equal to the longitudinal