Dual-curvature fusion surface slam navigation method, system, medium and product

Abstract

The application provides a double-curvature fusion curved surface SLAM navigation method, a longitudinal and transverse double-direction curvature parameter is estimated in real time through double-IMU fusion, an extended information filtering framework containing a double-curvature kinematic model, a laser data curved surface projection compensation algorithm and a path double-curvature compensation algorithm are constructed, and the scheme can accurately depict the curved surface geometric constraint where the robot actually locates; the accumulation of the transverse positioning error is inhibited through online estimation and fusion of the transverse curvature parameter; the accurate description and compensation of the robot side slip effect are realized through the establishment of a kinematic model containing a double-curvature coupling term; the data distortion is eliminated and the environment perception accuracy is improved through the projection of the laser scanning data from a plane to the estimated curved surface; finally, the consistency of the control instruction and the curved surface geometry is ensured through the real-time geometric compensation of the planning path based on the double-curvature parameter, and the stability and trajectory tracking accuracy of the control system under the longitudinal and transverse curvature coupling effect are enhanced.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) navigation technology, and more particularly to a dual-curvature fused surface SLAM navigation method and system. Background Technology

[0002] As the wind power industry develops towards larger scale and greater intelligence, the regular inspection and maintenance of wind turbine blades has become a crucial link in ensuring equipment safety and improving power generation efficiency. Traditional manual inspection methods suffer from low efficiency, high risk, and poor consistency. Automated inspection using wall-climbing robots or drones equipped with inspection equipment has become a clear technological trend. However, the inner wall of a wind turbine blade is a working space with complex geometry: it exhibits upward warping (longitudinal curvature) along its length and an elliptical cross-section (lateral curvature) along its width, forming a typical hyperbolic surface environment. This unique geometric feature presents a significant challenge to the robot's high-precision autonomous localization and navigation (SLAM) capabilities.

[0003] Currently, SLAM (Simultaneous Localization and Mapping) technologies for robots applied to curved environments are mostly developed based on the assumption of a single curvature or planar approximation. For example, in environments such as pipes and tanks, compensation is typically only made for a single curvature in the axial or circumferential direction. When such methods are directly applied to the inner wall of a wind turbine blade, the failure to fully characterize its bicurvature geometry leads to cumulative positioning errors in the robot's lateral movement due to the unmodeled lateral curvature. The sideslip effect in the robot's actual motion cannot be accurately described and compensated for due to the incomplete model. This results in the robot facing issues such as accumulated lateral positioning errors, uncompensated sideslip, sensor data distortion, and instability of the control system under multi-curvature coupling. Summary of the Invention

[0004] This invention provides a dual-curvature fusion surface SLAM navigation method and system to solve the technical problems in existing navigation technologies, such as the accumulation of lateral positioning errors, uncompensated motion sideslip, sensor data distortion, and instability of the control system under multi-curvature coupling.

[0005] This invention provides a hyperbolic fusion surface SLAM navigation method, the navigation method comprising:

[0006] S1: Obtain the pitch angle of the robot's direction of travel and the roll angle of the robot's lateral direction;

[0007] S2: Based on the pitch angle and the roll angle, the hyperbola parameter of the working surface is estimated in real time through nonlinear optimization. The hyperbola parameter includes at least the longitudinal radius of curvature and the ellipse parameter describing the lateral curvature.

[0008] S3: Construct an extended information filtering framework that includes the pitch angle and the roll angle as state variables, and establish a robot kinematic model that integrates longitudinal and lateral curvature based on the bicurvature parameters, perform synchronous localization and mapping, and output the robot's state vector.

[0009] S4: Acquire the raw scanning data of the lidar, and based on the currently estimated pitch angle and roll angle, project the raw scanning data from the scanning plane onto the surface defined by the hyperbola parameter to generate compensated observation data for the update step of the extended information filtering;

[0010] S5: Based on the hyperbola parameter and the state vector, perform geometric compensation on the planned path and generate control commands to drive the robot's movement.

[0011] According to the hyperbolic fusion surface SLAM navigation method provided by the present invention, in step S2, the nonlinear optimization adopts the Levenberg-Marquardt algorithm, which solves for the hyperbolic parameters by minimizing the residual function composed of the pitch angle change, the roll angle change and the hyperbolic parameters.

[0012] According to the hyperbolic fusion surface SLAM navigation method provided by the present invention, in step S3, the kinematic model includes:

[0013] The relationship between the rate of change of longitudinal velocity and the pitch angle satisfies the following: the rate of change of pitch angle equals the longitudinal velocity divided by the longitudinal radius of curvature;

[0014] The relationship between the rate of change of lateral velocity and the roll angle satisfies the following: the rate of change of roll angle equals the lateral velocity divided by the lateral radius of curvature function value;

[0015] The lateral radius of curvature function is an elliptic function that varies with the lateral position.

[0016] According to the hyperbolic fusion surface SLAM navigation method provided by the present invention, in step S3, in the update step of the extended information filtering, the Huber loss function is used to weight the observation information to reduce the impact of abnormal observations.

[0017] According to the dual-curvature fusion surface SLAM navigation method provided by the present invention, step S4 specifically includes:

[0018] The original laser point in polar coordinates is converted to three-dimensional Cartesian coordinates using a preset projection compensation formula, which includes the current pitch angle and roll angle as compensation parameters.

[0019] According to the hyperbolic fusion surface SLAM navigation method provided by the present invention, in step S5, when compensating for the lateral distance, a compensation formula based on elliptic integral or its binomial approximation formula is used to convert the horizontal lateral coordinates of the target point into lateral coordinates on the surface.

[0020] The hyperbolic fusion surface SLAM navigation method provided by the present invention further includes step S6: real-time monitoring of the estimated covariance between the innovation sequence of the extended information filtering framework and the hyperbolic parameters; when a positioning divergence or unreliable parameter estimation is detected, a fault recovery mechanism of state rollback or parameter reset is triggered.

[0021] The present invention also provides a surface SLAM navigation system with dual curvature fusion, used to implement the dual curvature fusion surface SLAM navigation method described in any of the above technical solutions, comprising:

[0022] The sensor module includes a primary inertial measurement unit for measuring pitch angle, a secondary inertial measurement unit for measuring roll angle, and a lidar.

[0023] The data processing and estimation module is configured to perform step S2 to estimate the hypercurvature parameters in real time;

[0024] The integrated localization and mapping module is configured to execute steps S3 and S4, achieve synchronous localization and mapping based on the hyperbolic parameters and compensated laser data, and output the robot's state vector;

[0025] The path planning and control module is configured to execute step S5, generating control commands based on the hypercurvature parameters and the state vector.

[0026] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of the hyperbolic fusion surface SLAM navigation method described in any of the above technical solutions.

[0027] The present invention also provides a computer program product, including a computer program, which, when executed by a processor, implements the steps of the hyperbolic fusion surface SLAM navigation method described in any of the above technical solutions.

[0028] The dual-curvature fusion surface SLAM navigation method provided by this invention estimates longitudinal and lateral curvature parameters in real time through dual IMU fusion, and constructs a complete scheme including an Extended Information Filter (EIF) framework containing a dual-curvature kinematic model, a laser data surface projection compensation algorithm, and a path dual-curvature compensation algorithm. This method can accurately characterize the geometric constraints of the surface in which the robot actually lies. By estimating and fusing lateral curvature parameters online, the accumulation of lateral positioning errors is significantly suppressed. By establishing a kinematic model containing dual-curvature coupling terms, the robot's sideslip effect is accurately described and compensated. By projecting laser scanning data from a plane onto the estimated surface, data distortion caused by the mismatch between the scanning plane and the real surface is eliminated, improving the accuracy of environmental perception. Finally, real-time geometric compensation of the planned path based on dual-curvature parameters ensures the consistency between control commands and surface geometry, fundamentally enhancing the stability and trajectory tracking accuracy of the control system under longitudinal and lateral curvature coupling. Attached Figure Description

[0029] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0030] Figure 1 This is a flowchart illustrating the dual-curvature fusion surface SLAM navigation method provided by the present invention;

[0031] Figure 2 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0032] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0033] The following is combined with Figure 1 The present invention describes a hyperbolic fusion surface SLAM navigation method, the navigation method comprising:

[0034] S1: Obtain the robot's pitch angle in the direction of travel and the robot's roll angle in the lateral direction. The robot platform is equipped with two independent inertial measurement units (IMUs) and a two-dimensional lidar. The main IMU is installed at the center of the robot body, with its Z-axis aligned with the robot's direction of travel, i.e., aligned with the blade length direction, and is used to accurately measure the robot's pitch angle relative to the horizontal plane. The secondary IMU is installed on one side of the robot body, with its X-axis aligned with the robot's lateral direction, i.e. aligned with the blade width direction, and is used to measure the robot's roll angle around the axis of travel.

[0035] After the system is powered on, the two IMUs are first statically calibrated to eliminate zero bias. During robot movement, the main and auxiliary IMUs synchronously output the raw measured values ​​of pitch and roll angles at a fixed frequency.

[0036] S2: Based on the pitch angle and the roll angle, the hyperbola parameters of the working surface are estimated in real time through nonlinear optimization. The hyperbola parameters include at least the longitudinal radius of curvature and the elliptical parameter describing the lateral curvature.

[0037] Because the inner wall of the wind turbine blade is a complex hypercurvature surface, the system continuously acquires the pitch angle change sequence of the main IMU and the roll angle change sequence of the secondary IMU within a short time window, while recording the longitudinal displacement estimated by the robot. Based on the geometric relationship that the ratio of the angle change to the path length is equal to the curvature, a residual equation for the unknown hypercurvature parameters is constructed. This equation set is solved using a nonlinear optimization method, thereby estimating the surface geometric parameters of the current working area online and adapting to possible changes in the surface parameters.

[0038] The hypercurvature parameters include at least the longitudinal radius of curvature R, which describes the degree of longitudinal warping. y The longitudinal direction is the direction of travel, and the elliptical parameters describe the shape of the transverse elliptical cross section, the major semi-axis a and the minor semi-axis b, and the transverse direction is the width direction.

[0039] S3: Construct an extended information filtering framework that includes the pitch angle and the roll angle as state variables, and establish a robot kinematic model that integrates longitudinal and lateral curvature based on the bicurvature parameters, perform synchronous localization and mapping, and output the robot's state vector.

[0040] First, define the robot's state vector, whose key state variables include the pitch angle and roll angle obtained in step S1, as well as other navigation states, such as two-dimensional plane position, heading angle, and velocity.

[0041] Then, based on the hypercurvature parameter R estimated in real time in step S2 yA, b, establish a kinematic model of the robot on this specific curved surface, incorporating the longitudinal radius of curvature R into the state prediction equation of this model. y and the lateral radius of curvature R that varies with lateral position x The influence of (y) is considered to more accurately describe the robot's true motion on the hyperbolic surface, and this kinematic model is used as the state transition model for Extended Information Filtering (EIF). The EIF framework estimates the robot's complete state vector by fusing multi-sensor information through a recursive process from prediction to update. The state vector includes its three-dimensional pose, velocity, etc., and constructs and continuously updates a globally consistent map of the current inner wall surface of the wind turbine blade.

[0042] S4: Acquire the raw scanning data of the lidar, and based on the currently estimated pitch angle and roll angle, project the raw scanning data from the scanning plane onto the surface defined by the hyperbolic parameters to generate compensated observation data for the update step of the extended information filtering.

[0043] A lidar mounted on the robot scans in a horizontal plane, obtaining a series of two-dimensional polar coordinate point clouds centered on the robot's coordinate system. Since the robot operates on a curved surface with pitch and roll attitudes, this raw point cloud data is relative to the laser scanning plane, and directly using it for localization and mapping based on surface geometry will result in distortion. In this step, using the robot's latest pitch and roll angles estimated by the EIF framework in step S3, combined with the surface geometry information estimated in step S2, each raw laser scanning point is projected backward from the two-dimensional scanning plane where the laser is located onto the three-dimensional curved surface corresponding to the map coordinate system used in step S3. The resulting three-dimensional point cloud data, after compensation, maintains geometric relationships consistent with the surface kinematics model and map representation used in EIF, thus serving as more accurate observation data input into the EIF update step in step S3, improving the accuracy of localization and mapping.

[0044] S5: Based on the hyperbola parameter and the state vector, perform geometric compensation on the planned path and generate control commands to drive the robot's movement.

[0045] Receive the current region hypercurvature parameter output from step S2 and the robot's real-time state vector output from step S3, and first perform path compensation: based on the hypercurvature parameter R... y The coordinates of the target points on the planned path, a and b, are transformed from the original planned coordinate system to the actual hyperbolic surface coordinate system through a geometric mapping relationship. Then, combined with the robot's current state, the motion controller calculates control commands such as the robot's longitudinal velocity, lateral velocity, or steering angular velocity based on the compensated path target. This enables the robot to move more smoothly and accurately along the expected trajectory on the inner wall surface of the fan blade.

[0046] By fusing dual IMUs to estimate longitudinal and lateral curvature parameters in real time, and constructing a complete scheme including an Extended Information Filter (EIF) framework with a bi-curvature kinematic model, a laser data surface projection compensation algorithm, and a path bi-curvature compensation algorithm, a precise characterization of the surface geometric constraints actually in which the robot is located is achieved. Online estimation and fusion of lateral curvature parameters significantly suppresses the accumulation of lateral positioning errors. By establishing a kinematic model including bi-curvature coupling terms, accurate description and compensation of the robot's sideslip effect are achieved. Projecting laser scanning data from a plane onto the estimated surface eliminates data distortion caused by the mismatch between the scanning plane and the real surface, improving environmental perception accuracy. Finally, real-time geometric compensation of the planned path based on bi-curvature parameters ensures the consistency between control commands and surface geometry, fundamentally enhancing the stability and trajectory tracking accuracy of the control system under longitudinal and lateral curvature coupling.

[0047] In a preferred embodiment of the present invention, in step S2, the nonlinear optimization employs the Levenberg-Marquardt algorithm, which solves for the hypercurvature parameter by minimizing the residual function composed of the pitch angle change, the roll angle change, and the hypercurvature parameter.

[0048] The specific process of step S2 is as follows:

[0049] S2.1: Data Acquisition and Preprocessing;

[0050] The system synchronously acquires the pitch angle from the main IMU at a fixed frequency. and roll angle from the sub-IMU It also records the robot's longitudinal displacement estimated by a wheel encoder or visual odometry within the corresponding time period. With lateral displacement And the original angle is filtered by moving average.

[0051] S2.2: Construct the residual function;

[0052] According to the principles of hypercurvature geometry, the change in pitch angle is mainly related to longitudinal motion and the longitudinal radius of curvature. R y The change in roll angle is related to lateral motion and the lateral radius of curvature that varies with position. Related. Define the parameter vector to be estimated as... Construct residual vector as follows: ,

[0053] in, y The coordinates are along the blade width direction. The radius of curvature is the transverse radius. It is based on the currently estimated major axis parameters of the ellipse. a minor axis parameters b and blade width direction coordinates y Calculated lateral radius of curvature It represents the change in the robot's pitch angle as measured by the main IMU within a specific time period or displacement; This represents the change in the robot's roll angle as measured by the sub-IMU within the same time period or corresponding displacement. This represents the arc length (distance along the curved path) that the robot actually travels. The longitudinal radius of curvature, The physical meaning of is the central angle corresponding to a segment of a circle, and its calculation result is also in radians; This indicates the amount of displacement of the robot in the lateral direction (i.e., along the length of the blade).

[0054] S2.3: Iterative optimization is performed using the Levenberg-Marquardt algorithm;

[0055] Set initial values ​​for parameters Initial damping factor A small positive number is selected, and the iteration stops when the residual change is less than the threshold or the maximum number of iterations is reached.

[0056] In the k-th iteration:

[0057] ,

[0058] in For Jacobian matrices, This represents the parameter update increment in the k-th iteration; Let be the damping factor for the k-th iteration; It is the product of the transpose of the Jacobian matrix and itself; For the reason A diagonal matrix composed of the diagonal elements; For residual vectors Regarding parameters The Jacobian matrix, in Evaluate at the specified location; Estimate the current parameters The residual vector below; The parameter vector estimate at the k-th iteration.

[0059] S2.4: Output the estimation results:

[0060] Once the iteration converges, the final parameter vector is used as the optimal estimate of the hypercurvature parameters of the working surface at the current moment, and output to the subsequent SLAM framework and path compensation module.

[0061] Through the Levenberg-Marquardt optimization process, the system can robustly and efficiently estimate the key hypercurvature parameters describing the geometry of complex surfaces online based on the angle changes and displacement information measured by the IMU, providing accurate geometric prior knowledge for the entire navigation system.

[0062] In a preferred embodiment of the present invention, the kinematic model in step S3 is specifically described, and the kinematic model includes:

[0063] The relationship between the rate of change of longitudinal velocity and the pitch angle satisfies the following: the rate of change of pitch angle equals the longitudinal velocity divided by the longitudinal radius of curvature;

[0064] The relationship between the rate of change of lateral velocity and the roll angle satisfies the following: the rate of change of roll angle equals the lateral velocity divided by the lateral radius of curvature function value;

[0065] The lateral radius of curvature function is an elliptic function that varies with the lateral position.

[0066] In a hyperbolic surface environment, the robot's state needs to include its position, orientation, and coupled motion information. The state vector X defined in this embodiment is as follows:

[0067] ,

[0068] Where x, y, z: represent the robot's three-dimensional position in the world coordinate system.

[0069] : Indicates the robot's heading angle.

[0070] The pitch angle, measured by the main IMU, describes the robot's rotation about its lateral axis, i.e., its longitudinal tilt relative to the horizontal plane.

[0071] : Represents the roll angle measured by the sub-IMU, describing the robot's rotation about its longitudinal axis, i.e., its lateral tilt relative to the horizontal plane.

[0072] : These represent the linear velocities of the robot along its own coordinate system in the longitudinal and lateral directions, respectively.

[0073] : Indicates the robot's heading angular velocity.

[0074] T represents the transformation of a row vector into a column vector; R 9 It represents a 9-dimensional real number space.

[0075] The robot's motion along the curved surface is decomposed into two dimensions: longitudinal (along the blade length direction) and lateral (along the blade width direction), and then compared with the longitudinal radius of curvature R estimated in step S2. y and transverse radius of curvature function The main differential relationships are related to:

[0076] .

[0077] The resultant velocity of the robot's longitudinal and lateral velocities; These are the velocity components along the three axes in the world coordinate system. This indicates that the rate of change of the heading angle is equal to the heading angular velocity.

[0078] This indicates that the rate of change of the robot's pitch angle is equal to its longitudinal velocity. v x Divide by longitudinal radius of curvature R y When a robot travels on an arc surface with a fixed longitudinal curvature, its pitch angle changes at a rate proportional to the curvature and the speed.

[0079] This indicates that the rate of change of the robot's roll angle is equal to its lateral velocity. v y Divide by the lateral radius of curvature function value at the current position .

[0080] Among them, state prediction: based on the state estimate of the previous time step. With control inputs (such as speed commands), the differential equations are discretized and integrated using numerical integration methods, such as the fourth-order Runge-Kutta method, to predict the state at the next moment.

[0081] ,

[0082] in, , Let k be the state vector at time k. Let k be the control input vector at time k. Let k1 be the discrete time step, k2 be the slope of the first stage, k3 be the slope of the third stage, and k4 be the slope of the fourth stage. Let k+1 be the state vector. It is a continuous-time state transition function.

[0083] Furthermore, calculate the state transition Jacobian matrix:

[0084] ,

[0085] Includes hypercurvature coupling terms, such as: ,

[0086] in, The resultant velocity of the robot's longitudinal and lateral velocities is expressed in m / s. The roll angle is measured by the sub-IMU, in rad. The tangent of the roll angle is dimensionless. Indicates the rate of change of height The partial derivative with respect to the lateral position y reflects the coupling effect in hyperbolic geometry; express For horizontal position The derivative of . The reciprocal of the transverse curvature is denoted as . , which represents the degree of curvature of the elliptical cross section at position y; Represents the transverse radius of curvature function cubed.

[0087] Preferably, step S3 further includes a robust Kalman filter compensation step: in the extended information filtering update step, the Huber loss function is used to weight the observation information to reduce the impact of outlier observations.

[0088] Huber Robust Loss Function ,

[0089] exist At that time, small errors are smoothed and weighted; At the same time, it limits the excessive impact of large errors.

[0090] Its influence function: ,

[0091] in, Here, 'e' represents the preset adjustment threshold, and 'e' represents the difference between the actual observed value and the predicted observed value. When, it is consistent with the least squares method; in At that time, the update weight of outliers is reduced.

[0092] Preferably, step S4 specifically comprises:

[0093] The original laser point in polar coordinates is converted to three-dimensional Cartesian coordinates using a preset projection compensation formula, which includes the current pitch angle and roll angle as compensation parameters.

[0094] The specific process is as follows:

[0095] The lidar scans at a fixed frequency and outputs a series of raw scan points in its own sensor coordinate system. The data for each point is represented in polar coordinates, denoted as . ,in, To measure the straight-line distance from the laser radar to the obstacle, The horizontal deflection angle of the original scan point relative to the forward centerline of the lidar.

[0096] Simultaneously, the robot attitude angle estimated at the laser scanning moment is obtained from the Extended Information Filtering (EIF) framework in step S3, namely: pitch angle. Roll angle .

[0097] Using the projection compensation formula, the original laser point is transformed from two-dimensional polar coordinates to three-dimensional Cartesian coordinates aligned with the world coordinate system: ,

[0098] x c The coordinate components in the direction of travel (longitudinal) after compensation; y c To compensate for the horizontal coordinate components; z c The coordinate components in the vertical direction after compensation; The raw distance (in meters) measured by the lidar is the straight-line distance from the lidar to the obstacle. The horizontal deflection angle of the original laser point.

[0099] In a preferred embodiment of the present invention, in step S5, when compensating for the lateral distance, a compensation formula based on elliptic integrals or its binomial approximation formula is used to convert the horizontal lateral coordinates of the target point into lateral coordinates on the curved surface.

[0100] According to the principle of calculating the arc length of an ellipse, from the center of the ellipse y=0 to the horizontal coordinate y h The corresponding target point corresponds to the elliptical arc length y on the true hyperbolic surface. s The exact solution is obtained from the elliptic integral of the second kind:

[0101] ,

[0102] in, This is an elliptic integral of the second kind. Let be the eccentricity of the ellipse.

[0103] when An approximate solution is adopted: ,in, It represents a higher-order infinitesimal term.

[0104] By using the lateral distance compensation based on elliptic integrals or their approximate formulas, the target position command received by the robot control system is ensured, enabling the robot to accurately move to the target point along the curved surface. This fundamentally avoids lateral positioning deviations and trajectory tracking errors caused by misapplying planar distances to curved surfaces, significantly improving navigation accuracy and control stability on complex hyperbolic curved surfaces.

[0105] Furthermore, it also includes step S6: real-time monitoring of the estimated covariance between the information sequence of the extended information filtering framework and the hypercurvature parameter; when a positioning divergence or unreliable parameter estimation is detected, a fault recovery mechanism of state rollback or parameter reset is triggered.

[0106] The threshold parameter of the Huber loss function is adaptively calculated based on the current innovation covariance. , .

[0107] in Let be the new information covariance matrix.

[0108] For observation model The Jacobian matrix at the current predicted state.

[0109] To observe the transpose of the Jacobian matrix.

[0110] This is the state prediction covariance matrix obtained from the EIF prediction step.

[0111] To observe the noise covariance matrix.

[0112] The average standard deviation of each component of the new information reflects the overall unreliability of the current observations.

[0113] The magnitude of the value directly indicates the degree of unreliability of the current overall observations.

[0114] The new covariance matrix The traces.

[0115] m is the length of the current observation vector.

[0116] This is a preset scaling factor.

[0117] Make the core parameters of the robust filter layer The method transforms static values ​​into dynamic functions of system uncertainty, and can automatically adjust to the optimal filtering state when faced with various working conditions ranging from stable to severe, thereby significantly improving the overall performance and reliability of the method in real-world complex and unstructured curved surface environments.

[0118] The present invention also provides a dual-curvature fused surface SLAM navigation system for implementing the navigation method, the navigation system comprising:

[0119] The sensor module includes a primary inertial measurement unit for measuring pitch angle, a secondary inertial measurement unit for measuring roll angle, and a lidar.

[0120] The data processing and estimation module is configured to perform step S2 to estimate the hypercurvature parameters in real time;

[0121] The integrated localization and mapping module is configured to execute steps S3 and S4, achieve synchronous localization and mapping based on the hyperbolic parameters and compensated laser data, and output the robot's state vector;

[0122] The path planning and control module is configured to execute step S5, generating control commands based on the hypercurvature parameters and the state vector;

[0123] In addition, the system monitoring and recovery module is configured to perform step S6 to ensure robust system operation.

[0124] The secondary IMU is mounted on the side of the robot body to sense lateral vibrations and rolling motions, and is fused with data from the primary IMU to compensate for lever arm effects.

[0125] Figure 2 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 2 As shown, the electronic device may include a processor 810, a communication interface 820, a memory 830, and a communication bus 840, wherein the processor 810, the communication interface 820, and the memory 830 communicate with each other via the communication bus 840. The processor 810 can call logical instructions in the memory 830 to execute a navigation method.

[0126] Furthermore, the logical instructions in the aforementioned memory 830 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0127] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer is able to execute the navigation methods provided by the above methods.

[0128] In another aspect, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to perform the navigation methods provided by the methods described above.

[0129] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0130] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0131] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A hypercurvature fused surface SLAM navigation method, characterized in that, The navigation method includes: S1: Obtain the pitch angle of the robot's direction of travel and the roll angle of the robot's lateral direction; S2: Based on the pitch angle and the roll angle, the hyperbola parameter of the working surface is estimated in real time through nonlinear optimization. The hyperbola parameter includes at least the longitudinal radius of curvature and the ellipse parameter describing the lateral curvature. S3: Construct an extended information filtering framework that includes the pitch angle and the roll angle as state variables, and establish a robot kinematic model that integrates longitudinal and lateral curvature based on the bicurvature parameters, perform synchronous localization and mapping, and output the robot's state vector. S4: Acquire the raw scanning data of the lidar, and based on the currently estimated pitch angle and roll angle, project the raw scanning data from the scanning plane onto the surface defined by the hyperbola parameter to generate compensated observation data for the update step of the extended information filtering; S5: Based on the hyperbola parameter and the state vector, perform geometric compensation on the planned path and generate control commands to drive the robot's movement.

2. The hypercurvature fusion surface SLAM navigation method according to claim 1, characterized in that, In S2, the nonlinear optimization employs the Levenberg-Marquardt algorithm, which solves for the hypercurvature parameter by minimizing the residual function composed of the pitch angle change, the roll angle change, and the hypercurvature parameter.

3. The hypercurvature fusion surface SLAM navigation method according to claim 1, characterized in that, In S3, the kinematic model includes: The relationship between the rate of change of longitudinal velocity and the pitch angle satisfies the following: the rate of change of pitch angle equals the longitudinal velocity divided by the longitudinal radius of curvature; The relationship between the rate of change of lateral velocity and the roll angle satisfies the following: the rate of change of roll angle equals the lateral velocity divided by the lateral radius of curvature function value; The lateral radius of curvature function is an elliptic function that varies with the lateral position.

4. The hypercurvature fusion surface SLAM navigation method according to claim 1, characterized in that, In S3, during the update step of the extended information filtering, the Huber loss function is used to weight the observation information in order to reduce the impact of outlier observations.

5. The hypercurvature fusion surface SLAM navigation method according to claim 1, characterized in that, S4 specifically refers to: The original laser point in polar coordinates is converted to three-dimensional Cartesian coordinates using a preset projection compensation formula, which includes the current pitch angle and roll angle as compensation parameters.

6. The hypercurvature fusion surface SLAM navigation method according to claim 1, characterized in that, In S5, when compensating for the lateral distance, a compensation formula based on elliptic integrals or its binomial approximation formula is used to convert the horizontal lateral coordinates of the target point into lateral coordinates on the curved surface.

7. The hypercurvature fusion surface SLAM navigation method according to any one of claims 1 to 6, characterized in that, It also includes S6: real-time monitoring of the estimated covariance between the information sequence of the extended information filtering framework and the hypercurvature parameter, and triggering a fault recovery mechanism of state rollback or parameter reset when a positioning divergence or unreliable parameter estimation is detected.

8. A hyperbolic surface SLAM navigation system, used to implement the hyperbolic surface SLAM navigation method according to any one of claims 1 to 7, characterized in that, include: The sensor module includes a primary inertial measurement unit for measuring pitch angle, a secondary inertial measurement unit for measuring roll angle, and a lidar. The data processing and estimation module is configured to execute S2 to estimate the hypercurvature parameters in real time. The integrated localization and mapping module is configured to execute S3 and S4 to achieve synchronous localization and mapping based on the hyperbolic parameters and compensated laser data, and output the robot's state vector. The path planning and control module is configured to execute S5, which generates control commands based on the hypercurvature parameters and the state vector.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the hypercurvature fused surface SLAM navigation method as described in any one of claims 1 to 7.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the hypercurvature fused surface SLAM navigation method as described in any one of claims 1 to 7.

Images