A Transmedia Wired Robot Pose Estimation Method Based on Fiber Optic Shape Sensor

Abstract

This invention discloses a method for pose estimation of a cross-medium wired robot based on an optical fiber shape sensor. The method includes: placing two non-collinear supports within the cross-medium wired robot; a solenoid is mounted on the outer wall of each support; an optical fiber shape sensor extends into the solenoid along its extension direction, with the remaining portion of the optical fiber shape sensor bundled with other cables to extend to the main control device; reconstructing the three-dimensional shape of the optical fiber shape sensor to obtain a measurement point cloud characterizing its shape; calculating the curvature distribution based on the reconstructed shape; segmenting the measurement point cloud according to index values ​​and curvature; extracting the central axis direction vector of the measurement point cloud located within the solenoid as its initial pose; precisely registering this segment of the point cloud to the geometric model of the solenoid; calculating the six-degree-of-freedom precise pose to obtain the robot's pose in the world coordinate system. This invention enables high-precision, highly interference-resistant pose estimation for cross-medium wired controlled robots.

Description

Technical Field

[0001] This invention belongs to the field of cross-medium wired robots, and particularly relates to the field of robot pose estimation technology, specifically to a cross-medium wired robot pose estimation method based on fiber optic shape sensors. Background Technology

[0002] The precise operation of wired robots across media (such as remotely operated vehicles, ROVs) relies on real-time and accurate perception of their position and attitude. However, in complex underwater environments, existing perception methods generally have certain limitations: GPS signals cannot penetrate water and cannot be directly used for underwater positioning. Existing research has attempted to receive GPS signals by mounting surface buoys and using cable geometry constraints to compensate for and estimate the ROV's position (US20090216444A1), but its positioning accuracy is limited because it does not acquire the three-dimensional spatial morphology of the cable in real time and relies on prior geometric modeling; acoustic positioning systems (such as long baselines, LBLs) have poor positioning accuracy in near-surface areas, high deployment and maintenance costs, and are easily affected by changes in the water environment and sound speed, with performance further degrading in the presence of obstructions; while inertial navigation systems (IMUs) can provide continuous pose information, they suffer from cumulative errors, requiring multi-sensor fusion to suppress drift, resulting in high system complexity, and it remains difficult to completely eliminate error accumulation in highly dynamic or long-duration tasks. Existing literature uses long baseline (LBL) fusion with inertial navigation system (IMU) and supplemented by extended Kalman filter (XKF) for joint estimation, but it is still difficult to completely overcome the dependence of acoustic positioning on the environment and the inherent drift problem of inertial sensors.

[0003] Furthermore, the three-dimensional morphology reconstruction and real-time perception of the control cables connecting the robot and the mother ship underwater is another key challenge in cross-medium wired robot control. Real-time monitoring of cable morphology not only helps assess the risk of tangling or knotting, but is also crucial for ensuring the safety of system operation. Summary of the Invention

[0004] To address at least one of the problems existing in the prior art, this invention provides a method for cross-medium wired robot pose estimation based on an optical fiber shape sensor. This invention provides a high-precision, interference-resistant method for cross-medium wired robot pose estimation, thereby solving the problem of interference caused by changes in the medium in existing cross-medium wired robot positioning sensors.

[0005] To achieve the objectives of this invention, a method for estimating the pose of a wired robot across media based on an optical fiber shape sensor is proposed, comprising the following steps:

[0006] Inside the cross-medium wired robot, there are two non-collinear support bodies. The outer walls of the two support bodies are coaxially wound with solenoids. The fiber optic shape sensor is inserted into the solenoid along the extension direction of the solenoid, and the rest of the fiber optic shape sensor is bundled with other cables to form a cable bundle that extends to the main control device.

[0007] The strain distribution data of the fiber optic shape sensor is acquired in real time, the three-dimensional spatial shape of the fiber optic shape sensor is reconstructed, the measurement point cloud of the three-dimensional spatial shape of the fiber optic shape sensor is obtained, and the curvature of the corresponding sampling point is obtained.

[0008] Based on the curvature and index value of the measured point cloud, the measured point cloud is segmented, and the measured point cloud segment located inside the solenoid is identified. The direction vector of the central axis is calculated by principal component analysis, and after orthogonalization, the preliminary pose estimate of the solenoid is obtained.

[0009] Using the initial pose estimation as the starting point for iterative optimization, the measured point cloud is registered to the solenoid model through optimization calculation, and the six-degree-of-freedom pose of the solenoid is solved to obtain the pose of the robot in the world coordinate system.

[0010] Furthermore, the steps for reconstructing the three-dimensional spatial shape of the fiber optic shape sensor include:

[0011] Build by A multi-core fiber optic sensing structure consists of lateral fiber cores symmetrically distributed around a central fiber core. The lateral fiber cores are used to measure axial strain distribution, while the central fiber core is used to monitor temperature changes in real time and compensate for thermal drift errors. The lateral fiber cores are evenly spaced along the axial direction of the multi-core fiber optic sensing structure. One sampling point;

[0012] Optical frequency domain reflection technology is used to perform optical measurements on the central and lateral cores of a multi-core optical fiber. By sweeping laser interferometry and frequency domain demodulation, the Rayleigh scattering spectral characteristics at each sampling point along the length of each core are obtained, and the corresponding sampling points are calculated based on the spectral drift. The Wavelength shift of the fiber core ,in =1, 2, 3… ,forward The root is the lateral core, the first The root is the central core;

[0013] Establish a linear relationship model between the wavelength shift of each fiber core and physical quantities, and calculate the sampling points. The strain value of the root core The temperature change is calculated by using the wavelength offset of the central fiber core, eliminating temperature interference in the lateral fiber core strain measurement, and obtaining the strain value.

[0014] For the Sampling points, based on the same cross section The strain value of the root lateral core, combined with the circumferential radius of the lateral core, and the first... Root lateral core and The positive angle of the axis, Axial direction vector, The axial direction vector is used to calculate curvature and bending direction angle.

[0015] A differential geometric model is established, with the root of the fiber optic shape sensor as the initial point. The position of the next sampling point is iteratively solved based on the curvature, bending direction angle and sampling step size of each sampling point.

[0016] Furthermore, the step of reconstructing the three-dimensional spatial shape of the fiber optic shape sensor also includes:

[0017] Sampling points representing different resolutions are set at equal intervals along the axis of the fiber shape sensor, the sampling step size is adjusted accordingly, and the aforementioned reconstruction steps are repeated to obtain multiple sets of three-dimensional coordinates at different resolutions.

[0018] Unify point clouds at different resolutions to We obtain a set of 3D coordinates with a uniform number of sampling points. Then, we perform weighted fusion of the coordinates of sampling points at different resolutions to obtain the final result. A set of three-dimensional coordinates of each sampling point.

[0019] Furthermore, the solenoids on the two supports are defined as the first solenoid and the second solenoid, respectively. The steps for segmenting the measurement point cloud include:

[0020] Based on the curvature and index value of the measurement point cloud, and according to the curvature amplitude, curvature change trend and index interval, the measurement point cloud is divided into multiple segments. Among the divided segments, the segment where the measurement point cloud corresponding to the cable bundle segment is located is defined as the cable bundle segment, the segment where the measurement point cloud corresponding to the first solenoid is located is defined as the first solenoid segment, and the segment where the measurement point cloud corresponding to the second solenoid is located is defined as the second solenoid segment.

[0021] For the measurement point cloud of the first solenoid segment, the corresponding principal axis direction vector is solved. For the measurement point cloud of the second solenoid segment, the corresponding principal axis direction vector is solved. The third vector is obtained by the cross product of the two principal axis direction vectors. The two principal axis direction vectors and the third vector are orthogonalized to form an orthogonal coordinate system.

[0022] Extract the measurement point cloud between the two solenoid segments, calculate its geometric center as the initial position estimate, and combine it with the orthogonal coordinate system to form the preliminary pose estimate of the solenoid in the robot coordinate system.

[0023] Furthermore, the solenoid model includes OBJ models of the first solenoid segment and the second solenoid segment. The steps of registering the measured point cloud to the solenoid model and optimizing the solution to obtain the six-degree-of-freedom pose of the solenoid include:

[0024] Read the OBJ model of the first solenoid segment and generate the parametric equation of the helical centerline based on the geometric parameters;

[0025] Read the OBJ model of the second solenoid segment and generate the parametric equation of the helical centerline based on the geometric parameters;

[0026] Based on the corresponding parametric equations, the signed distance function of the inner surface of the first solenoid segment and the signed distance function of the inner surface of the second solenoid segment are constructed respectively.

[0027] A minimum objective function is constructed by combining the attitude of the measured point cloud relative to the solenoid model and the signed distance function;

[0028] Let the initial pose estimate be the initial value for optimization. The pose matrix is ​​updated iteratively, and the six-degree-of-freedom pose of the solenoid is obtained through iteration.

[0029] Furthermore, it also includes the step of: smoothing the pose sequence of consecutive frames using Kalman filtering, and according to the first... The number of iterations in the frame-solved pose matrix during the iterative solution process is used to adaptively adjust the observation noise parameters in the Kalman filter.

[0030] Furthermore, during iteration, the iteration termination condition is set to be that the cumulative point distance error is less than a preset threshold or the maximum number of iterations.

[0031] The present invention also provides a cross-medium wired robot pose estimation system based on an optical fiber shape sensor.

[0032] The present invention also provides a computer device.

[0033] The present invention also provides a computer-readable storage medium.

[0034] Compared with the prior art, the present invention can achieve at least the following beneficial effects:

[0035] (1) The present invention can directly and in real time perceive the three-dimensional spatial shape of the control cable, realize the pose estimation of the cross-medium wired control robot with high precision and strong anti-interference capability, thereby improving its motion control accuracy and overall system adaptability in dynamic working environment.

[0036] (2) This invention arranges the fiber optic shape sensor and the control cable in a synchronous bundle, so that the sensing fiber optic cable undergoes the same deformation as the cable when it is bent, twisted or stressed. The distributed point cloud data output by the fiber optic shape sensor is used to fit the geometric shape of the cable, thereby realizing the identification and shape reconstruction of the cable's spatial attitude. Attached Figure Description

[0037] Figure 1 This is a schematic diagram illustrating the steps of a cross-medium wired robot pose estimation method based on an optical fiber shape sensor provided in an embodiment of the present invention.

[0038] Figure 2 This is a schematic diagram illustrating the cooperation between the cross-medium wired robot, the support structure, and the fiber optic shape sensor in an embodiment of the present invention.

[0039] Figure 3 This is a schematic diagram of the module composition of a cross-medium wired robot pose estimation system based on an optical fiber shape sensor in an embodiment of the present invention. Detailed Implementation

[0040] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0041] Please see Figure 1 The present invention provides a method for estimating the pose of a wired robot across media based on an optical fiber shape sensor, which is implemented according to the following steps:

[0042] S1. A support structure is installed inside the cross-medium wired robot 1. The support structure includes two non-collinear support bodies. A solenoid is coaxially wound on the outer wall of each support body. A fiber optic shape sensor (FOSS) is inserted into the solenoid along the extension direction of the solenoid. The remaining part of the fiber optic shape sensor is bundled with other cables to form a cable bundle extending to the main control device.

[0043] In one embodiment, see Figure 2 The supporting structure is an L-shaped right-angled round bend 2, comprising two mutually perpendicular and non-collinear supporting bodies, defined as the first tube body 2a and the second tube body 2b. The right-angled round bend 2 is installed and fixed inside the transmedium wired robot 1. The outer wall of the right-angled round bend 2 is coaxially wound and fixed with a solenoid. Specifically, step S1 includes the following sub-steps:

[0044] S11. An L-shaped right-angled round bend 2 is provided, and the right-angled round bend 2 is rigidly connected to the load-bearing frame inside the cross-medium wired robot 1; wherein, after installation, the right-angled round bend 2 satisfies the following positional relationship: its first tube body 2a is vertically fixedly installed inside the cross-medium wired robot 1, and the cylindrical axis of its second tube body 2b points to the rear of the cross-medium wired robot; the right-angled round bend 2 constitutes the mounting base of the entire sensing assembly;

[0045] S12. A solenoid 3 is coaxially wound and fixed to the outer wall of the right-angled round bend 2; the interior of the solenoid 3 has a regular and continuous cylindrical cavity channel; the solenoid 3 is wound 4 times in the first tube body 2a and 5 times in the second tube body 2b.

[0046] S13. Insert the optical fiber shape sensor 4 into the cavity channel inside the solenoid 3, so that it enters from one end of the solenoid 3 and extends to the other end; the shape of the optical fiber shape sensor 4 located in the section inside the solenoid 3 is geometrically constrained by the cavity channel, and a movable gap is maintained between the optical fiber shape sensor 4 and the inner wall of the cavity channel.

[0047] It is understood that in other embodiments, the support structure does not necessarily have to be an L-shaped right-angled bend, and other shapes may also be used.

[0048] S2. The strain distribution data of the fiber optic shape sensor is acquired in real time using optical frequency domain reflection (OFDR) technology. The three-dimensional spatial shape of the fiber optic shape sensor is reconstructed using the Frenet-Serret formula. The curvature value corresponding to each sampling point is calculated and recorded. During the reconstruction process, multi-resolution sampling and smoothing filtering are used to reduce the cumulative error. Finally, the measurement point cloud of the three-dimensional spatial shape of the fiber optic shape sensor is obtained.

[0049] Step S2 includes the following sub-steps:

[0050] S21, constructing by Root lateral core ( ≥3) A multi-core fiber optic sensing structure with symmetrically distributed cores surrounding a central core; wherein the lateral cores are used to measure axial strain distribution, and the central core is used to monitor temperature changes in real time and compensate for thermal drift errors; the cores are evenly spaced along the axial direction of the multi-core fiber optic sensing structure. One sampling point; in one embodiment, =3, 31254, the axial length of the fiber optic shape sensor is 25.0m;

[0051] S22. Optical frequency domain reflectance (OFDR) technology is used to perform optical measurements on the central and lateral cores of the multi-core fiber sensing structure. Rayleigh scattering spectral characteristics at each sampling point along the length of each core are obtained through swept-frequency laser interferometry and frequency domain demodulation, and the corresponding sampling points are calculated based on the spectral drift. The Wavelength shift of the fiber core ,in =1, 2, 3… ,forward The root is the lateral core, the first The root is the central core;

[0052] S23. Establish a linear relationship model between the wavelength offset of each fiber core and physical quantities, and calculate the sampling points according to the following formula. The strain value of the root core Utilizing the wavelength shift of the central fiber core Calculate the temperature change Eliminate temperature interference in lateral fiber core strain measurement to obtain strain values. ;

[0053]

[0054] in , The linear coefficients are experimentally calibrated; in one embodiment, the calibration is... , , It represents microstrain and is an engineering unit of strain.

[0055] S24, Regarding the first Sampling points, based on the same cross section Root lateral fiber core strain value Let the circumferential radius of the lateral fiber core be... , No. Root lateral core and The positive angle between the axes is Establish a cross-sectional coordinate system. The x-axis is the unit direction vector. Calculate the curvature using the unit direction vector of the y-axis. and bending direction angle :

[0056] ,

[0057]

[0058] in, Represents vector curvature. Represents vector curvature of The coordinate values ​​of the axis. Represents vector curvature of The coordinate values ​​of the axis.

[0059] S25. Establish a differential geometric model based on the Frenet-Serret framework, with the root of the fiber optic shape sensor as the initial point (position). tangent vector Along the axial direction), based on each sampling point curvature Bending direction angle and sampling step size (Sampling step size = total length of fiber optic shape sensor / number of sampling points), iteratively solve for the position of the next sampling point:

[0060]

[0061]

[0062]

[0063]

[0064]

[0065] in for The unit tangential vector of the sampling point, The unit principal normal vector, The unit binormal vector, Forming a right-handed system, For torsion, Sampling points Location;

[0066] Given initial values, perform iterative calculations and output. The set of three-dimensional coordinates of each sampling point .

[0067] S26, Equally spaced along the axial direction of the fiber optic shape sensor , Multiple sampling points are used, and the sampling step size is adjusted accordingly. The aforementioned reconstruction process is repeated to obtain multiple sets of 3D coordinates at different resolutions; among them... , , They are of the same order of magnitude; in one embodiment, they are respectively set 15627 62,508 sampling points.

[0068] S27. Use interpolation or downsampling methods to unify point clouds with different numbers of sampling points to a single point cloud. This yields a set of three-dimensional coordinates with a uniform number of sampling points. , Then, the coordinates of the sampling points at different resolutions are weighted and fused according to the following formula to obtain the final result. The set of three-dimensional coordinates of each sampling point ;

[0069]

[0070] All are weighting coefficients. , , Sampling points at different resolutions The location. In one embodiment, .

[0071] S3. Based on the curvature and index value of the measured point cloud, the measured point cloud is segmented, and the measured point cloud segment located inside the solenoid is identified. The central axis direction vector is calculated using the principal component analysis (PCA) method, and after orthogonalization, it is used as the preliminary pose estimate of the solenoid.

[0072] Specifically, step S3 includes the following sub-steps:

[0073] S31: Based on the curvature and index value of the measured point cloud, the measured point cloud is divided into multiple segments according to the curvature amplitude, curvature change trend and index interval.

[0074] In this embodiment, the installation position relationship between the fiber optic shape sensor and the solenoid is stable, and the spatial distribution of each point is highly regular. It can be directly determined based on the curvature amplitude and its changing trend in the index interval [1, The interval is divided into [ ].

[0075] After segmentation, the point cloud in each segment corresponds to a vertically or horizontally arranged solenoid segment, and the point cloud inside can form a spiral curve structure in space that is consistent with the number of turns of the solenoid.

[0076] The number of segmentation points is proportional to the number of tubes in the support structure, and in one embodiment, it is twice the number of supports.

[0077] Let the solenoids on the two supports be defined as the first solenoid and the second solenoid, respectively. Let the radius of the first solenoid be... Then its curvature The radius of the second solenoid is Then its curvature .

[0078] In one embodiment, Based on the curvature amplitude, curvature change trend, and index interval, in the index interval [1, Four segmentation points are set in the [section]. , , , , The results were obtained through experimental determination. The specific segmentation criteria are as follows:

[0079] Cable bundle segment: Index value in The measurement point cloud between these points can be considered as the measurement point cloud located within the cable bundle segment. Let this segment of the measurement point cloud be... ;

[0080] First solenoid segment: Index value in The measured point cloud, and the corresponding curvature value is stable at Within the interval, the curvature change amplitude is less than This can be considered as the measurement point cloud located in the first pipe body 2a of the bend. Let this measurement point cloud be... ;

[0081] Second solenoid segment: Index value in The measured point cloud, and the corresponding curvature value is stable at Within the interval, and the curvature change amplitude is less than This can be considered as the measurement point cloud of the second pipe body 2b located in the bend. Let this measurement point cloud be... ;

[0082] S32. Extract the measurement point cloud of the first solenoid segment. The center of the point cloud is , Given the number of measurement point clouds in the first solenoid segment, solve for the principal axis direction vector. This makes the objective function: Obtain the minimum value;

[0083] S33. When the height of the solenoid is much larger than its diameter, the direction vector of this principal axis... The eigenvector corresponding to the largest eigenvalue of the point cloud covariance matrix can be calculated using principal component analysis (PCA).

[0084] S34. Calculate the principal axis direction vector of the measurement point cloud of the second solenoid segment. ,pass and The cross product operation yields a third vector, and the three vectors (including the principal axis direction vector) are then compared. and The third vector and the third vector are orthogonalized to form an orthogonal coordinate system;

[0085] S35. Extract the measurement point cloud between the two solenoid segments, i.e., the aforementioned location... The measured point cloud of the segment is used to calculate its geometric center as an initial position estimate. Combined with the orthogonal coordinate system obtained in step S34, this forms the initial pose estimate of the solenoid in the robot coordinate system. .

[0086] S4. Using the preliminary pose estimation as the starting point for iterative optimization, the measurement point cloud is registered to the solenoid model using optimization calculation, and the six-degree-of-freedom pose of the solenoid is optimized and solved to obtain the pose of the cross-medium wired robot in the world coordinate system. The measurement point cloud of the cable bundle segment is used to describe the shape of the cable bundle to avoid affecting the normal operation of the robot due to cable knots or entanglements.

[0087] The solenoid model includes the OBJ model of the first solenoid segment (i.e., using the OBJ format) and the OBJ model of the second solenoid segment.

[0088] Specifically, the measured point cloud is precisely registered with the preset geometric model through the following steps:

[0089] S41: Read the OBJ model of the first solenoid segment and base it on the geometric parameters (including the principal radius). Pitch Number of laps Parametric equations for generating the centerline of the spiral :

[0090]

[0091] in, Centerline of the spiral The parameter variables, The parameter on the center line of the spiral is The three-dimensional spatial coordinate vector of the point is used to describe the geometry of the centerline of the first solenoid. The height increment per radian in the axial direction, i.e., the axial step rate of the helix centerline, is determined by the pitch. With angle The proportional relationship is determined to control the upward velocity of the helical centerline in the axial direction; a similar method is used to obtain the parametric equation of the helical centerline of the second solenoid segment: , Centerline of the spiral Parameter variables;

[0092] S42: Construct the signed distance function (SDF) of the inner surface of the first solenoid segment:

[0093]

[0094] in, The inner radius of the first solenoid is... This function represents any point in the measurement point cloud of the first solenoid segment. It calculates the shortest distance from this measurement point to the centerline of the first solenoid, thus characterizing the distance from the measurement point cloud to the inner surface of the first solenoid. Similarly, there is a signed distance function for the inner surface of the second solenoid segment. :

[0095]

[0096] in, The inner radius of the second solenoid. This represents any point in the measurement point cloud of the second solenoid segment;

[0097] S43: Using rigid body transformation matrix This indicates the pose of the measured point cloud relative to the solenoid model (including the OBJ model of the first and second solenoid segments). To represent the pose matrix obtained after iterative solution, a minimum objective function is constructed:

[0098]

[0099]

[0100] in, This represents the number of measurement point clouds in the first solenoid segment. This represents the number of measurement point clouds in the second solenoid segment. , For the corresponding index number, This is the measurement point cloud for the first solenoid segment. This is the measurement point cloud for the second solenoid segment. A robust loss function (preferably the Huber function) is used to suppress the impact of outliers; It is the signed distance function of the first solenoid segment; Using rigid body transformation matrix Multiply by (matrix multiplication) Then substitute The function after that, It is the signed distance function of the second solenoid segment. Using rigid body transformation matrix Multiply by (matrix multiplication) Then substitute The function after that, Using rigid body transformation matrix The objective function with independent variables is used to measure the overall matching error between the measured point cloud and the solenoid model under given pose assumptions.

[0101] S44: Set up initial pose estimation To optimize the initial values, a nonlinear least squares optimization method based on Lie algebra parameterization is used to iteratively update the pose matrix. The iteration termination condition is set as the cumulative point distance error being less than a preset threshold or the maximum number of iterations. In each iteration, the parameters of the nearest point between the measured point cloud and the spiral centerline are updated based on the current pose results. , :

[0102]

[0103]

[0104] in, , The initial parameters are determined by the fiber optic shape sensor index. , , , The width of the local search interval is set empirically; in one embodiment, the maximum number of iterations is 50, and the cumulative point distance error is calculated as follows. The preset threshold for cumulative point distance error:

[0105]

[0106] S45: To suppress inter-frame jitter in the transform matrix, the pose sequence of consecutive frames is smoothed using Kalman filtering, and according to the... pose matrix solved in frames The accumulated point distance error during the iterative solution process is used to adaptively adjust the parameters of the observation noise covariance matrix in the Kalman filter.

[0107] The filter state vector is defined as:

[0108]

[0109] in Represents a frame sequence. Indicates the first Spatial coordinates within the frame, in mm; For the first Euler angles in a frame, measured in mrad, are derived from... It is calculated through the mapping relationship from Euler angles to homogeneous transformation matrix;

[0110] Filter state vector In the The estimated state of the frame after Kalman filtering smoothing is This allows for the real-time acquisition of continuous and smooth six-degree-of-freedom poses of the robot. .

[0111] In this embodiment, the Kalman filter parameters are set as follows:

[0112] The state transition matrix and observation matrix are both set to sixth-order identity matrices, and the initial state is obtained from the state vector solved from the first frame. Initialization, initial error covariance matrix Process noise covariance matrix Observation noise covariance matrix Calibrated by experiment:

[0113] ;

[0114] ;

[0115] ;

[0116] in The adaptive adjustment coefficient is defined as follows: And limited The range of values ​​is .

[0117] In one embodiment, a cross-medium wired robot pose estimation system based on an optical fiber shape sensor is provided to implement the method described in the foregoing embodiments. (See also...) Figure 3 The system includes the following modules:

[0118] The 3D reconstruction module is used to acquire strain distribution data of the fiber optic shape sensor in real time, reconstruct the 3D spatial shape of the fiber optic shape sensor, obtain the measurement point cloud of the 3D spatial shape of the fiber optic shape sensor, and obtain the curvature of the corresponding sampling points.

[0119] The preliminary pose estimation module is used to segment the measurement point cloud based on the curvature and index value, identify the measurement point cloud segment located inside the solenoid, calculate the central axis direction vector using the principal component analysis method, and orthogonalize it as the preliminary pose estimation of the solenoid.

[0120] The pose estimation module is used to take the initial pose estimation as the starting point for iterative optimization, register the measured point cloud to the solenoid model through optimization calculation, optimize and solve the six-degree-of-freedom pose of the solenoid, and thus obtain the pose of the robot in the world coordinate system.

[0121] In one embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the methods described in the foregoing embodiments.

[0122] In one embodiment, a computer-readable storage medium stores a computer program that, when executed by a processor, implements the methods described in the foregoing embodiments.

[0123] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined in this invention may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for cross-medium wired robot pose estimation based on fiber optic shape sensors, characterized in that, Includes the following steps: Inside the cross-medium wired robot, there are two non-collinear support bodies. The outer walls of the two support bodies are coaxially wound with solenoids. The fiber optic shape sensor is inserted into the solenoid along the extension direction of the solenoid, and the rest of the fiber optic shape sensor is bundled with other cables to form a cable bundle that extends to the main control device. The strain distribution data of the fiber optic shape sensor is acquired in real time, the three-dimensional spatial shape of the fiber optic shape sensor is reconstructed, the measurement point cloud of the three-dimensional spatial shape of the fiber optic shape sensor is obtained, and the curvature of the corresponding sampling point is obtained. The steps for reconstructing the three-dimensional spatial shape of an optical fiber shape sensor include: Build by A multi-core fiber optic sensing structure consists of lateral fiber cores symmetrically distributed around a central fiber core. The lateral fiber cores are used to measure axial strain distribution, while the central fiber core is used to monitor temperature changes in real time and compensate for thermal drift errors. The lateral fiber cores are evenly spaced along the axial direction of the multi-core fiber optic sensing structure. One sampling point; Optical frequency domain reflection technology is used to perform optical measurements on the central and lateral cores of a multi-core optical fiber. By sweeping laser interferometry and frequency domain demodulation, the Rayleigh scattering spectral characteristics at each sampling point along the length of each core are obtained, and the corresponding sampling points are calculated based on the spectral drift. The Wavelength shift of the root core ,in =1, 2, 3… ,forward The root is the lateral core, the first The root is the central core; Establish a linear relationship model between the wavelength shift of each fiber core and physical quantities, and calculate the sampling points. The strain value of the root core The temperature change is calculated by using the wavelength offset of the central fiber core, eliminating temperature interference in the lateral fiber core strain measurement, and obtaining the strain value. For the Sampling points, based on the same cross section The strain value of the root lateral core, combined with the circumferential radius of the lateral core, and the first... Root lateral core and The positive angle of the axis, Axial direction vector, The axial direction vector is used to calculate curvature and bending direction angle. A differential geometric model is established, with the root of the fiber optic shape sensor as the initial point. The position of the next sampling point is iteratively solved based on the curvature, bending direction angle, and sampling step size of each sampling point. Based on the curvature and index value of the measured point cloud, the measured point cloud is segmented, and the measured point cloud segment located inside the solenoid is identified. The direction vector of the central axis is calculated by principal component analysis, and after orthogonalization, the preliminary pose estimate of the solenoid is obtained. Using the initial pose estimation as the starting point for iterative optimization, the measured point cloud is registered to the solenoid model through optimization calculation, and the six-degree-of-freedom pose of the solenoid is solved to obtain the pose of the robot in the world coordinate system.

2. The method for cross-medium wired robot pose estimation based on fiber optic shape sensor according to claim 1, characterized in that, The steps for reconstructing the three-dimensional spatial shape of the fiber optic shape sensor also include: Sampling points representing different resolutions are set at equal intervals along the axis of the fiber shape sensor, the sampling step size is adjusted accordingly, and the aforementioned reconstruction steps are repeated to obtain multiple sets of three-dimensional coordinates at different resolutions. Unify point clouds at different resolutions to We obtain a set of 3D coordinates with a uniform number of sampling points. Then, we perform weighted fusion of the coordinates of sampling points at different resolutions to obtain the final result. A set of three-dimensional coordinates of each sampling point.

3. The method for cross-medium wired robot pose estimation based on fiber optic shape sensor according to claim 1, characterized in that, The steps for segmenting the measured point cloud, defining the solenoids on the two supports as the first solenoid and the second solenoid respectively, include: Based on the curvature and index value of the measurement point cloud, and according to the curvature amplitude, curvature change trend and index interval, the measurement point cloud is divided into multiple segments. Among the divided segments, the segment where the measurement point cloud corresponding to the cable bundle segment is located is defined as the cable bundle segment, the segment where the measurement point cloud corresponding to the first solenoid is located is defined as the first solenoid segment, and the segment where the measurement point cloud corresponding to the second solenoid is located is defined as the second solenoid segment. For the measurement point cloud of the first solenoid segment, the corresponding principal axis direction vector is solved. For the measurement point cloud of the second solenoid segment, the corresponding principal axis direction vector is solved. The third vector is obtained by the cross product of the two principal axis direction vectors. The two principal axis direction vectors and the third vector are orthogonalized to form an orthogonal coordinate system. Extract the measurement point cloud between the two solenoid segments, calculate its geometric center as the initial position estimate, and combine it with the orthogonal coordinate system to form the preliminary pose estimate of the solenoid in the robot coordinate system.

4. A method for estimating the pose of a wired robot across media based on an optical fiber shape sensor, as described in any one of claims 1-3, characterized in that, The solenoid model includes OBJ models of the first solenoid segment and the second solenoid segment. The steps of registering the measured point cloud to the solenoid model and optimizing the solution to obtain the six-degree-of-freedom pose of the solenoid include: Read the OBJ model of the first solenoid segment and generate the parametric equation of the helical centerline based on the geometric parameters; Read the OBJ model of the second solenoid segment and generate the parametric equation of the helical centerline based on the geometric parameters; Based on the corresponding parametric equations, the signed distance function of the inner surface of the first solenoid segment and the signed distance function of the inner surface of the second solenoid segment are constructed respectively. A minimum objective function is constructed by combining the attitude of the measured point cloud relative to the solenoid model and the signed distance function; Let the initial pose estimate be the initial value for optimization. The pose matrix is ​​updated iteratively, and the six-degree-of-freedom pose of the solenoid is obtained through iteration.

5. The method for cross-medium wired robot pose estimation based on fiber optic shape sensor according to claim 4, characterized in that, It also includes the following steps: smoothing the pose sequence of consecutive frames using Kalman filtering, and according to the... The number of iterations in the frame-solved pose matrix during the iterative solution process is used to adaptively adjust the observation noise parameters in the Kalman filter.

6. The method for cross-medium wired robot pose estimation based on fiber optic shape sensor according to claim 4, characterized in that, When iterating, the iteration termination condition is set to the cumulative point distance error being less than a preset threshold or the maximum number of iterations.

7. A cross-medium wired robot pose estimation system based on an optical fiber shape sensor, characterized in that, For implementing the method according to any one of claims 1-6, the system comprises the following modules: The 3D reconstruction module is used to acquire strain distribution data of the fiber optic shape sensor in real time, reconstruct the 3D spatial shape of the fiber optic shape sensor, obtain the measurement point cloud of the 3D spatial shape of the fiber optic shape sensor, and obtain the curvature of the corresponding sampling points. The preliminary pose estimation module is used to segment the measurement point cloud based on the curvature and index value, identify the measurement point cloud segment located inside the solenoid, calculate the central axis direction vector using the principal component analysis method, and orthogonalize it as the preliminary pose estimation of the solenoid. The pose estimation module is used to take the initial pose estimation as the starting point for iterative optimization, register the measured point cloud to the solenoid model through optimization calculation, optimize and solve the six-degree-of-freedom pose of the solenoid, and thus obtain the pose of the robot in the world coordinate system.

8. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the method according to any one of claims 1-6.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the method described in any one of claims 1-6.

Images