A method and system for three-dimensional continuous shape estimation of a flexible robot

Abstract

The application relates to a three-dimensional continuous shape estimation method and system of a flexible robot, wherein the method combines the arc length of the flexible robot and unknown external loads from the environment with a Cosserat elastic rod model and a Cosserat elastic string model to generate a Lie algebra coupled statics model representing a tendon-driven model, models the external loads as Gaussian process noise to construct a prior probability model, obtains a prior probability distribution of the flexible robot in a Lie algebra space, constructs a measurement noise model in the Lie algebra space according to discrete measurement poses captured by a sensor and measurement noise, obtains a predicted measurement value, fuses the prior probability distribution and the predicted measurement value to obtain a posterior shape estimation of the flexible robot, and calculates a continuous three-dimensional shape estimation of the flexible robot and a first credibility of the continuous three-dimensional shape estimation according to the posterior shape estimation and a Gaussian process interpolation method. Therefore, the application realizes accurate perception of the continuous three-dimensional shape of the flexible robot.

Description

Technical Field

[0001] This invention relates to the field of computer technology, and in particular to a method and system for estimating the three-dimensional continuous shape of a flexible robot. Background Technology

[0002] Flexible robots, with their inherent flexible backbone structure, demonstrate excellent adaptability and operability in confined environments such as minimally invasive surgery. However, to ensure that robots can perform tasks safely and effectively in complex environments, accurate perception of their own three-dimensional shape is a crucial prerequisite.

[0003] Currently, while numerous methods exist for 3D shape perception in flexible continuum robots, they still have shortcomings. For example, sensor-based shape reconstruction methods, while capable of perceiving 3D shapes, require a clear and unobstructed line of sight, which is difficult to guarantee in real-world scenarios. Methods based on fiber Bragg grating sensors, while providing continuous strain measurements, are expensive. Methods based on electromagnetic tracking sensors, although providing pose information within a certain workspace, suffer from uneven measurement accuracy distribution in space. Furthermore, for miniaturization and integration convenience, sensor nodes are arranged discretely and sparsely, making it difficult to directly obtain complete shape information. Among model-based shape prediction methods, geometric models based on the constant curvature assumption have a certain degree of reasonableness when the robot is not subjected to external forces or has a small load. While the shape estimation method can accurately describe the deformation of a continuum, it requires precise external force information to solve a system of differential equations to predict the shape, limiting its application when the external load is unknown. Among probabilistic shape estimation methods, recursive estimation based on Kalman filtering can fuse model and measurement data, but it still requires integral along the robot's arc length to solve a system of differential equations when reconstructing the shape, resulting in low computational efficiency. Although batch estimation based on Gaussian process regression can directly process measurement data in batches to recover the continuous shape, the prior models used in existing methods often employ simplified assumptions, making it difficult to estimate the shape of a continuum robot under complex conditions.

[0004] Therefore, there is an urgent need for a new technical solution that can accurately perceive the continuous three-dimensional shape of a flexible robot under sparse pose measurement and unknown external load conditions. Summary of the Invention

[0005] The technical problem to be solved by the present invention is: the present invention provides a method and system for estimating the three-dimensional continuous shape of a flexible robot, so as to realize the accurate perception of the continuous three-dimensional shape of the flexible robot under sparse pose measurement and unknown external load conditions.

[0006] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows:

[0007] In a first aspect, the present invention provides a method for estimating the three-dimensional continuous shape of a flexible robot, comprising the following steps:

[0008] The arc length of the flexible robot is obtained. Based on the arc length and the unknown external load from the environment, a Cosserat elastic rod model and a Cosserat elastic string model are combined to generate a Lie algebra-coupled static model representing tendon actuation. At the same time, the external load is modeled as Gaussian process noise. Based on the Gaussian process noise, the Lie algebra-coupled static model is transformed into a system of stochastic differential equations. The stochastic differential equations are linearized to construct a prior probability model. The prior probability distribution of the flexible robot in the Lie algebra space is obtained through the prior probability model.

[0009] The flexible robot's discrete measurement pose and measurement noise at all working points are captured by sensors. A measurement noise model is constructed in Lie algebra space based on the discrete measurement pose and the measurement noise. The predicted measurement value of the flexible robot in Lie algebra space is obtained through the measurement noise model.

[0010] The prior probability distribution is fused with the predicted measurement to obtain the posterior shape estimate of the flexible robot. The continuous three-dimensional shape estimate of the flexible robot and the first confidence level of the continuous three-dimensional shape estimate are calculated based on the posterior shape estimate and the Gaussian process interpolation method.

[0011] The beneficial effects of this invention are as follows: By combining the arc length of the flexible robot and the unknown external loads from the environment with the Cosserat elastic rod model and the Cosserat elastic string model, the resulting Lie algebra-coupled static model not only unifies the Cosserat elastic rod model and the Cosserat elastic string model, overcoming the model mismatch problem caused by the oversimplification of tendon actuation in traditional single-point torque models, but also improves the accuracy of the prior probability model constructed based on the Lie algebra-coupled static model. The introduction of Gaussian process noise to handle unknown external loads makes the prior probability model unrestricted by unknown external loads, improving applicability and flexibility. Based on sensor capture... The captured discrete measurement pose and measurement noise are used to construct a measurement noise model in Lie algebra space. That is, the discrete measurement pose and measurement noise captured by the sensor are also transformed into Lie algebra form. The predicted measurement value obtained from the measurement noise model is fused with the prior probability distribution obtained from the prior probability model to improve the accuracy of the posterior shape estimation of the flexible robot. This ensures the accuracy of the continuous three-dimensional shape estimation of the flexible robot calculated by the posterior shape estimation and Gaussian process interpolation algorithm. It enables accurate perception of the continuous three-dimensional shape of the flexible robot under sparse pose measurement and unknown external load conditions, and provides the first confidence level of the continuous three-dimensional shape estimation, thus optimizing the user experience.

[0012] Optionally, the combination of the arc length and the unknown external load from the environment with the Cosserat elastic rod model and the Cosserat elastic string model to generate a Lie algebra-coupled static model representing tendon drive, while modeling the external load as Gaussian process noise, and transforming the Lie algebra-coupled static model into a system of stochastic differential equations based on the Gaussian process noise, includes:

[0013] The shape of the flexible robot is defined by the pose of the three-dimensional special Euclidean group SE(3) using the arc length as a variable, and the motion equations in the Cosserat elastic rod model are converted into generalized motion equations expressed by the Lie algebra se(3) elements of the three-dimensional special Euclidean group SE(3) using a first conversion formula. The first conversion formula is:

[0014] ;

[0015] ;

[0016] ;

[0017] in, Let represent the generalized equation of motion, and s represent the arc length of the flexible robot. The shape of the flexible robot is defined by the pose of the three-dimensional special Euclidean group SE(3). express function, Represents the internal linear strain of a three-dimensional vector. Represents the internal angular strain of a three-dimensional vector. Represents the generalized strain of a six-dimensional vector. Let SO(3) represent the pose of a flexible robot belonging to the three-dimensional orthogonal group SO(3). Represents the position of the flexible robot as a three-dimensional vector;

[0018] Through logarithmic mapping, H The t function and the first formula define the Lie algebra se(3) elements of the pose to obtain the new pose. The first formula is:

[0019] ;

[0020] in, Let represent the new pose, and s represent the arc length of the flexible robot. H represents The inverse function of the t function, The shape of a flexible robot is defined by the pose of the three-dimensional special Euclidean group SE(3);

[0021] Obtain the distributed force function and distributed torque function along the length of the flexible robot. Based on the distributed force function, the distributed torque function, the right Jacobian matrix of the three-dimensional special Euclidean group SE(3), and the left Jacobian matrix of the three-dimensional special Euclidean group SE(3), connect the new pose with the generalized equation of motion in the Lie algebra space to obtain the Lie algebraic static model. The Lie algebraic static model is as follows:

[0022] ;

[0023] ;

[0024] in, Let represent the first-order rate of change of the new pose with respect to s, where s represents the arc length of the flexible robot. Let be the inverse of the right Jacobian matrix of the three-dimensional special Euclidean group SE(3). Represents the generalized strain of a six-dimensional vector. This represents the first-order rate of change of the generalized strain of a six-dimensional vector with respect to s. The matrix representing the diagonal stiffness constants under shear tension. The matrix representing the inverse of the diagonal stiffness constant matrix under shear tension. Represents the internal linear strain of a three-dimensional vector. The reference line strain represents the three-dimensional vector. Let f(x) represent the pose transpose of a flexible robot that belongs to a three-dimensional square matrix of the three-dimensional orthogonal group SO(3). This represents the distributed force function along the length of the flexible robot. The matrix representing the diagonal stiffness constants in bending and torsion. The inverse matrix representing the diagonal stiffness constant matrix of bending and torsion. Represents the internal angular strain of a three-dimensional vector. The reference angular strain represents a three-dimensional vector. This represents the distributed torque function along the length of the flexible robot.

[0025] According to the decomposition formula and the external load, the distributed force function is decomposed into tendon force and external load force. Simultaneously, the distributed moment function is decomposed into tendon moment and external load moment. The tendon force is calculated based on the Cosserat elastic string model and the first formula, and the tendon moment is calculated based on the Cosserat elastic string model and the second formula. The decomposition formula is as follows:

[0026] ;

[0027] ;

[0028] in, Represents the distributed force function. Indicates the force exerted by the tendon. Indicates the external load force. Represents the distributed moment function. Indicates the torque exerted by the tendon. Indicates the torque of the external load;

[0029] The first formula is:

[0030] ;

[0031] ;

[0032] in, The force exerted by the tendon is represented by s, and the arc length of the flexible robot is represented by s. Indicates the total number of tendons. This represents the driving force of the i-th tendon. This represents the first-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s. Let represent the second-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s, and let p(s) represent the position of the flexible robot in three-dimensional vectors. Let represent the pose of a flexible robot belonging to a three-dimensional square matrix of the three-dimensional orthogonal group SO(3). This represents the position coordinates of the i-th tendon in the local coordinate system;

[0033] The second formula is:

[0034] ;

[0035] ;

[0036] in, The torque represents the force exerted by the tendon, and s represents the arc length of the flexible robot. Indicates the total number of tendons. This represents the driving force of the i-th tendon. This represents the first-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s. Let represent the second-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s, and let p(s) represent the position of the flexible robot in three-dimensional vectors. Let represent the pose of a flexible robot belonging to a three-dimensional square matrix of the three-dimensional orthogonal group SO(3). This represents the position coordinates of the i-th tendon in the local coordinate system;

[0037] The tendon force and the tendon torque are substituted into the Lie algebraic static model to obtain the Lie algebraic coupled static model. At the same time, the external load force and the external load torque are modeled as Gaussian process noise. Based on the Gaussian process noise, the Lie algebraic coupled static model is transformed into a system of stochastic differential equations.

[0038] Optionally, simultaneously modeling the external load force and the external load torque as Gaussian process noise, and transforming the Lie algebra-coupled static model into a system of stochastic differential equations based on the Gaussian process noise, includes:

[0039] The external load force is modeled as Gaussian process noise, and the external load torque is modeled as Gaussian process noise, both of which conform to a first vector formula, which is:

[0040] ;

[0041] in, Indicates Gaussian process noise. Indicates external force noise. Let represent the external torque noise, s represent the arc length of the flexible robot, and GP represent the Gaussian process. This represents the diagonal constant matrix of the stationary power spectral density;

[0042] The Lie algebra-coupled static model is transformed into a system of stochastic differential equations based on the Gaussian process noise.

[0043] As described above, the shape of the flexible robot is defined by the pose of the three-dimensional special Euclidean group SE(3), and the Lie algebra se(3) elements of the pose are defined by logarithmic mapping, Hat function and first formula to obtain a new pose. The motion equations in the Cosserat elastic rod model are transformed into generalized motion equations expressed by Lie algebra se(3) elements. The new pose and the generalized motion equations are connected in the Lie algebra space to obtain the Lie algebraic static model, realizing the description of the deformation of the flexible robot in the Lie algebra space. According to the decomposition formula and external load, the distributed force function along the length of the tendon of the flexible robot is determined. The system decomposes the force and external load force into tendon torque and external load torque, and obtains the final Lie algebra-coupled static model based on the tendon force and tendon torque. This model can accurately characterize the influence of tendon actuation on the shape change of the flexible robot. The external load is also refined into external load force and external load torque, and modeled as Gaussian process noise external force noise and external torque noise, respectively. This improves the efficiency of external load processing and also improves the accuracy of converting the Lie algebra-coupled static model into a system of stochastic differential equations based on Gaussian process noise.

[0044] Optionally, obtaining the prior probability distribution of the flexible robot in the Lie algebra space through the prior probability model includes:

[0045] The prior probability model randomly selects K measurement points as working points from all points of interest of the flexible robot, obtains the state vectors of all working points, calculates the mean and covariance matrix of the state vectors, and obtains the prior probability distribution of the flexible robot in the Lie algebra space based on the mean and the covariance matrix.

[0046] As described above, when calculating the prior probability distribution, the selection of the operating point is random, and the prior probability distribution in the Lie algebra space is obtained by using the mean and covariance matrix of the state vectors of all operating points, thereby improving the comprehensiveness of the calculated prior probability distribution.

[0047] Optionally, constructing a measurement noise model in the Lie algebra space based on the discrete measurement pose and the measurement noise includes:

[0048] Obtain the Lie algebra of the measured pose of the flexible robot at all working points, and substitute the Lie algebra of the measured pose into the second transformation formula to obtain the measured value vector. The second transformation formula is:

[0049] ;

[0050] in, Denotes the Lie algebra of the measurement pose at the k-th working point. This represents the vector of measurement values ​​at the k-th operating point;

[0051] Obtain the observation matrix, the noise covariance matrix of the observation noise at each operating point, and the state vector at each operating point. Construct a linear measurement model based on all observation matrices, all noise covariance matrices, all state vectors, and the measured value vector.

[0052] The origin boundary conditions and end-effector boundary conditions of the flexible robot are obtained. The origin boundary conditions and end-effector boundary conditions are used as constraints in the form of virtual measurements. The linear measurement model is updated through the constraints to obtain the measurement noise model.

[0053] As described above, the sensor measurements can be used directly without complex nonlinear transformations, simplifying the subsequent optimization process. When constructing the measurement noise model, a linear measurement model is first built based on the observation matrix of each working point, the noise covariance matrix of the observation noise of each working point, the state vector and the measurement value vector of each working point. Then, the origin boundary conditions and end boundary conditions of the flexible robot are used to update the linear measurement model, taking extreme cases into account and improving the accuracy of the obtained measurement noise model.

[0054] Optionally, fusing the prior probability distribution with the predicted measurement to obtain the posterior shape estimate of the flexible robot includes:

[0055] A prior cost term is constructed based on the prior probability distribution, and a measurement cost term is constructed based on the predicted measurement value. The prior cost term is used to penalize the prior probability distribution that deviates from the first threshold, and the measurement cost term is used to penalize the predicted measurement value that deviates from the second threshold.

[0056] A total cost function is constructed based on the prior cost term and the measurement cost term. This total cost function is then substituted into the Gauss-Newton equation to iteratively optimize the prior probability distribution, obtaining an update term for each iteration. The L2 norm of the update term is then determined to be less than an update threshold. If it is, the iterative optimization calculation terminates to obtain the posterior shape estimate of the flexible robot. If not, the iterative optimization calculation continues until the L2 norm of the update term is less than the update threshold. The Gauss-Newton equation is:

[0057] ;

[0058] ;

[0059] W ;

[0060] ;

[0061] Where H represents the augmented Jacobian matrix, Let W be the transpose of the augmented Jacobian matrix, and let W be the weight matrix. This represents the inverse of the weight matrix. Represents the total cost function. Indicates the update item. This represents the lifting state transition matrix. Represents the lifting form of the observation matrix. This indicates an increase in the covariance matrix. This represents the complete measurement covariance matrix in lifting form. Represents the prior cost term. This represents the measurement cost item.

[0062] As described above, by constructing a prior cost term that penalizes the prior probability distribution that deviates from the first threshold and a measurement cost term that penalizes the predicted measurement that deviates from the second threshold, the accuracy of the fused prior probability distribution and the accuracy of the predicted measurement are ensured. The accuracy of the obtained posterior shape estimate is improved by using the Gauss-Newton equation and the total cost function constructed based on the prior cost term and the measurement cost term to perform iterative optimization calculation of the prior probability distribution.

[0063] Optionally, obtaining the posterior shape estimate of the flexible robot includes:

[0064] Calculate the posterior pose covariance matrix of the posterior shape estimate, and use the posterior pose covariance matrix as the second confidence level of the posterior shape estimate.

[0065] As described above, not only is a posterior shape estimate provided, but also a second confidence level for the posterior shape estimate, thus optimizing the user experience.

[0066] Optionally, the step of calculating the continuous three-dimensional shape estimate of the flexible robot and the first confidence level of the continuous three-dimensional shape estimate based on the posterior shape estimate and Gaussian process interpolation method includes:

[0067] The Gaussian process interpolation method is used to calculate the posterior mean of the state at the intermediate point of the posterior shape estimate and the posterior covariance matrix of the intermediate point. Based on the posterior mean of the state at the intermediate point and the posterior covariance matrix of the intermediate point, the continuous three-dimensional shape estimate of the flexible robot and the first confidence level of the continuous three-dimensional shape estimate are obtained.

[0068] As described above, the Gaussian process interpolation method can effectively calculate and express the continuous three-dimensional shape estimation of a flexible robot.

[0069] In a second aspect, the present invention provides a three-dimensional continuous shape estimation system for a flexible robot, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the three-dimensional continuous shape estimation method for a flexible robot described in the first aspect.

[0070] The technical effect of the three-dimensional continuous shape estimation system for a flexible robot provided in the second aspect is the same as that of the three-dimensional continuous shape estimation method for a flexible robot provided in the first aspect. Attached Figure Description

[0071] Figure 1 A flowchart illustrating a three-dimensional continuous shape estimation method for a flexible robot provided in this embodiment;

[0072] Figure 2 This is a schematic diagram of the overall process of a three-dimensional continuous shape estimation method for a flexible robot provided in this embodiment;

[0073] Figure 3 This is a schematic diagram illustrating the process of posterior shape estimation and continuous three-dimensional shape estimation calculation involved in this embodiment;

[0074] Figure 4 This is a schematic diagram of the structure of a three-dimensional continuous shape estimation system for a flexible robot provided in this embodiment.

[0075] Explanation of reference numerals in the attached figures

[0076] 1. A three-dimensional continuous shape estimation system for a flexible robot;

[0077] 2. Processor;

[0078] 3. Memory. Detailed Implementation

[0079] To better understand the above technical solutions, exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that the present invention can be understood more clearly and thoroughly, and that the scope of the present invention can be fully conveyed to those skilled in the art.

[0080] Example 1

[0081] Please refer to Figures 1 to 3 This invention provides a method for estimating the three-dimensional continuous shape of a flexible robot, comprising the following steps:

[0082] S1. Obtain the arc length of the flexible robot. Based on the arc length and the unknown external load from the environment, combine it with the Cosserat elastic rod model and the Cosserat elastic string model to generate a Lie algebra-coupled static model representing tendon actuation. Model the external load as Gaussian process noise. Based on the Gaussian process noise, transform the Lie algebra-coupled static model into a system of stochastic differential equations. Linearize the system of stochastic differential equations to construct a prior probability model. Obtain the prior probability distribution of the flexible robot in the Lie algebra space through the prior probability model.

[0083] At this point, step S1, which combines the arc length and the unknown external load from the environment with the Cosserat elastic rod model and the Cosserat elastic string model to generate a Lie algebra-coupled static model representing tendon drive, simultaneously models the external load as Gaussian process noise, and transforms the Lie algebra-coupled static model into a system of stochastic differential equations based on the Gaussian process noise, includes:

[0084] S11. Define the shape of the flexible robot using the arc length as a variable through the pose of the three-dimensional special Euclidean group SE(3), and convert the motion equations in the Cosserat elastic rod model into generalized motion equations expressed by the Lie algebra se(3) elements of the three-dimensional special Euclidean group SE(3) through a first conversion formula. The first conversion formula is:

[0085] ;

[0086] ;

[0087] ;

[0088] in, Let represent the generalized equation of motion, and s represent the arc length of the flexible robot. The shape of the flexible robot is defined by the pose of the three-dimensional special Euclidean group SE(3). express function, Represents the internal linear strain of a three-dimensional vector. Represents the internal angular strain of a three-dimensional vector. Represents the generalized strain of a six-dimensional vector. Let SO(3) represent the pose of a flexible robot belonging to the three-dimensional orthogonal group SO(3). Represents the position of the flexible robot as a three-dimensional vector;

[0089] In this embodiment, all the multiplication signs “×” appearing in the formula are actually inner products “·”, representing the generalized equation of motion. In reality, it is the first-order rate of change of the shape of the flexible robot with respect to s, defined by the pose of the three-dimensional special Euclidean group SE(3).

[0090] S12, via logarithmic mapping, H The t function and the first formula define the Lie algebra se(3) elements of the pose to obtain the new pose. The first formula is:

[0091] ;

[0092] in, Let represent the new pose, and s represent the arc length of the flexible robot. H represents The inverse function of the t function, The shape of a flexible robot is defined by the pose of the three-dimensional special Euclidean group SE(3);

[0093] S13. Obtain the distributed force function and distributed torque function along the length of the flexible robot. Based on the distributed force function, the distributed torque function, the right Jacobian matrix of the three-dimensional special Euclidean group SE(3), and the left Jacobian matrix of the three-dimensional special Euclidean group SE(3), connect the new pose with the generalized equation of motion in the Lie algebra space to obtain the Lie algebraic static model. The Lie algebraic static model is as follows:

[0094] ;

[0095] ;

[0096] in, Let represent the first-order rate of change of the new pose with respect to s, where s represents the arc length of the flexible robot. Let be the inverse of the right Jacobian matrix of the three-dimensional special Euclidean group SE(3). Represents the generalized strain of a six-dimensional vector. This represents the first-order rate of change of the generalized strain of a six-dimensional vector with respect to s. The matrix representing the diagonal stiffness constants under shear tension. The matrix representing the inverse of the diagonal stiffness constant matrix under shear tension. Represents the internal linear strain of a three-dimensional vector. The reference line strain represents the three-dimensional vector. Let f(x) represent the pose transpose of a flexible robot that belongs to a three-dimensional square matrix of the three-dimensional orthogonal group SO(3). This represents the distributed force function along the length of the flexible robot. The matrix representing the diagonal stiffness constants in bending and torsion. The inverse matrix representing the diagonal stiffness constant matrix of bending and torsion. Represents the internal angular strain of a three-dimensional vector. The reference angular strain represents a three-dimensional vector. This represents the distributed moment function along the length;

[0097] In this embodiment, the inverse of the right Jacobian matrix of the three-dimensional special Euclidean group SE(3) in the Lie algebraic static model can be derived from the inverse of the left Jacobian matrix of the three-dimensional special Euclidean group SE(3). The derivation process is as follows:

[0098] Because: the inverse of the left Jacobian matrix of the three-dimensional special Euclidean group SE(3) is:

[0099] ;

[0100] Therefore, the inverse of the right Jacobian matrix of the three-dimensional special Euclidean group SE(3) is:

[0101] ;

[0102] Specifically,

[0103] ;

[0104] ;

[0105] in, The matrix representing the inverse of the left Jacobian matrix of the three-dimensional special Euclidean group SE(3) is... The coupling matrix representing the left Jacobian matrix of the three-dimensional special Euclidean group SE(3) is given. Let represent the rotation components of the three-dimensional vectors in the elements of the Lie algebra se(3) of the three-dimensional special Euclidean group SE(3). Let represent the translation components of the three-dimensional vectors in the elements of the Lie algebra se(3) of the three-dimensional special Euclidean group SE(3). express function;

[0106] S14. Based on the decomposition formula and the external load, the distributed force function is decomposed into tendon force and external load force. Simultaneously, the distributed moment function is decomposed into tendon moment and external load moment. The tendon force is calculated based on the Cosserat elastic string model and the first formula, and the tendon moment is calculated based on the Cosserat elastic string model and the second formula. The decomposition formula is:

[0107] ;

[0108] ;

[0109] in, Represents the distributed force function. Indicates the force exerted by the tendon. Indicates the external load force. Represents the distributed moment function. Indicates the torque exerted by the tendon. Indicates the torque of the external load;

[0110] The first formula is:

[0111] ;

[0112] ;

[0113] in, The force exerted by the tendon is represented by s, and the arc length of the flexible robot is represented by s. Indicates the total number of tendons. This represents the driving force of the i-th tendon. This represents the first-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s. Let represent the second-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s, and let p(s) represent the position of the flexible robot in three-dimensional vectors. Let represent the pose of a flexible robot belonging to a three-dimensional square matrix of the three-dimensional orthogonal group SO(3). This represents the position coordinates of the i-th tendon in the local coordinate system;

[0114] The second formula is:

[0115] ;

[0116] ;

[0117] in, The torque represents the force exerted by the tendon, and s represents the arc length of the flexible robot. Indicates the total number of tendons. This represents the driving force of the i-th tendon. This represents the first-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s. Let represent the second-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s, and let p(s) represent the position of the flexible robot in three-dimensional vectors. Let represent the pose of a flexible robot belonging to a three-dimensional square matrix of the three-dimensional orthogonal group SO(3). This represents the position coordinates of the i-th tendon in the local coordinate system;

[0118] S15. Substitute the tendon force and the tendon torque into the Lie algebraic static model to obtain the Lie algebraic coupled static model. At the same time, model the external load force and the external load torque as Gaussian process noise, and transform the Lie algebraic coupled static model into a system of stochastic differential equations based on the Gaussian process noise.

[0119] In this embodiment, as Figure 2 As shown, the tendon force and tendon torque calculated in step S14 are substituted into the Lie algebraic statics model obtained in step S13 to obtain the Lie algebraic coupled statics model. Simultaneously, the external load force and external load torque are modeled as Gaussian process noise. Based on the Gaussian process noise, the Lie algebraic coupled statics model is transformed into a system of stochastic differential equations. The specific Lie algebraic coupled statics model is as follows:

[0120] ;

[0121] ;

[0122] ;

[0123] ;

[0124] ;

[0125] ;

[0126] ;

[0127] ;

[0128] ;

[0129] ;

[0130] ;

[0131] ;

[0132] in, This represents the additional total linear stiffness matrix. This represents the linear stiffness addition matrix of the i-th tendon. This represents the driving force of the i-th tendon. Indicates the total number of tendons. This represents the position coordinates of the i-th tendon in the local coordinate system. This represents the first-order rate of change of the position coordinates of the i-th tendon in the local coordinate system with respect to s. Represents the internal angular strain of a three-dimensional vector. Represents the internal linear strain of a three-dimensional vector. This represents the total force-angle coupled stiffness matrix. This represents the force-angle coupling stiffness matrix of the i-th tendon. This represents the total stiffness matrix of the moment-linear strain coupling. This represents the additional total matrix of angular stiffness. Represents the total motion coupling force vector. This represents the motion coupling force vector of the i-th tendon. This represents the total motion coupling torque vector. This represents the motion coupling torque vector of the i-th tendon. The matrix representing the diagonal stiffness constants under shear tension. The matrix representing the diagonal stiffness constants during bending and torsion;

[0133] At this point, step S15, which involves simultaneously modeling the external load force and the external load torque as Gaussian process noise and transforming the Lie algebra-coupled static model into a system of stochastic differential equations based on the Gaussian process noise, includes:

[0134] S151. The external load force is modeled as Gaussian process noise, and the external load torque is modeled as Gaussian process noise, and both the external force noise and the external torque noise obey a first vector formula, which is:

[0135] ;

[0136] in, Indicates Gaussian process noise. Indicates external force noise. Let represent the external torque noise, s represent the arc length of the flexible robot, and GP represent the Gaussian process. This represents the diagonal constant matrix of the stationary power spectral density;

[0137] S152. Based on the Gaussian process noise, the Lie algebra-coupled static model is transformed into a system of stochastic differential equations.

[0138] In this embodiment, as Figure 2 As shown, the external load includes external load force and external load torque. The external load force and external load torque are modeled as Gaussian process noise, respectively, and both external load noise and external load torque noise obey the first vector formula. Therefore, based on the Gaussian process noise, the Lie algebra-coupled static model is transformed into a system of stochastic differential equations. At this time, the system of stochastic differential equations is a first-order nonlinear system of stochastic differential equations, as detailed below:

[0139] ;

[0140] ;

[0141] ;

[0142] in, This represents the additional total linear stiffness matrix. Represents the internal angular strain of a three-dimensional vector. Represents the internal linear strain of a three-dimensional vector. This represents the total force-angle coupled stiffness matrix. This represents the total stiffness matrix of the moment-linear strain coupling. This represents the additional total matrix of angular stiffness. Represents the total motion coupling force vector. This represents the total motion coupling torque vector. The matrix representing the diagonal stiffness constants under shear tension. The matrix representing the diagonal stiffness constants in bending and torsion. Represents the internal linear strain of a three-dimensional vector. The reference line strain represents the three-dimensional vector. Indicates Gaussian process noise. Indicates external force noise. Let represent the external torque noise, s represent the arc length of the flexible robot, and GP represent the Gaussian process. This represents the diagonal constant matrix of the stationary power spectral density;

[0143] When a flexible robot deforms, the shearing and stretching effects are negligible compared to bending and torsion; therefore, let: [0;0;1], further simplifying the above stochastic differential equation system, the simplified stochastic differential equation system is as follows:

[0144] ;

[0145] ;

[0146] ;

[0147] .

[0148] in, Indicates internal force. This represents the first-order rate of change of internal force with respect to s;

[0149] In this implementation, the specific steps for linearizing the system of stochastic differential equations to construct the prior probability model are as follows:

[0150] New position The internal angular strain u(s) and internal forces of the three-dimensional vector Stacked into state variables X(s), i.e., X(s) = [ u(s) ] T Input vector , Indicates all The constant column vector of tension in the root tendon can be represented as: =[ ] T Then the simplified stochastic differential equation can be summarized as X(s) varying along the arc length s. The first-order nonlinear function f of w(s) defines the rate of change of the state variable X(s):

[0151] ;

[0152] During linearization, the state vector x(s) around all points of interest is linearized. Points of interest refer to the shapes of multiple points along the arc length of the flexible robot. Substituting x(s) into X(s) and the zero vector into w(s), the linear function is obtained as follows:

[0153] ;

[0154] A linearized input vector can be defined based on a linear function. System matrix A(s), noise Jacobian matrix L(s):

[0155] ;

[0156] ;

[0157] ;

[0158] Given a system matrix A(S), we can define an initial value problem with respect to the normalized fundamental matrix r(s), where the initial value at the origin s=0 is the 12-dimensional identity matrix I. 12 :

[0159] ;

[0160] r(0)=I 12 ;

[0161] Furthermore, derive the state transition matrix connecting any two working points with different arc lengths:

[0162] ;

[0163] in, Indicates arc length With arc length The state transition matrix between two operating points;

[0164] This completes the linearization of the stochastic differential equation system and constructs a priori probability model.

[0165] At this point, the probability distribution in step S1 includes:

[0166] S16. The prior probability model randomly selects K measurement points as working points from all interest points of the flexible robot, obtains the state vectors of all working points, calculates the mean and covariance matrix of the state vectors, and obtains the prior probability distribution of the flexible robot in the Lie algebra space based on the mean and the covariance matrix.

[0167] In this embodiment, as Figure 2 As shown, the prior probability model randomly selects K measurement points as working points from all points of interest of the flexible robot, and obtains the state vectors of all working points. The mean and covariance matrix of the state vectors represent the prior probability distribution of the flexible robot in the Lie algebra space. The prior probability distribution includes the predicted shape and the confidence level of that shape. The prior probability distribution can be represented as follows:

[0168] ;

[0169] ;

[0170] ;

[0171] ;

[0172] ;

[0173] ;

[0174] in, This represents the lifting state transition matrix. This indicates boosting the input vector. Represents the covariance matrix. This represents the transpose of the lifting state transition matrix. This represents the state vector at the origin. This represents the covariance matrix at the origin. Let K represent the covariance matrix at the Kth operating point. This represents the state vector of all operating points. This represents the mean of the state vectors of all operating points. Let N represent the covariance matrix of the state vectors of all operating points, and let N represent the Gaussian probability distribution.

[0175] S2. The discrete measurement pose and measurement noise of the flexible robot at all working points are captured by the sensor. A measurement noise model is constructed in the Lie algebra space based on the discrete measurement pose and the measurement noise. The predicted measurement value of the flexible robot in the Lie algebra space is obtained through the measurement noise model.

[0176] At this point, the construction of the measurement noise model in the Lie algebra space based on the discrete measurement pose and the measurement noise in step S2 includes:

[0177] S21. Obtain the Lie algebra of the measured pose of the flexible robot at all working points, and substitute the Lie algebra of the measured pose into the second transformation formula to obtain the measured value vector. The second transformation formula is:

[0178]

[0179] in, Denotes the Lie algebra of the measurement pose at the k-th working point. This represents the vector of measurement values ​​at the k-th operating point;

[0180] S22. Obtain the observation matrix, the noise covariance matrix of the observation noise of each operating point, and the state vector of each operating point. Construct a linear measurement model based on all observation matrices, all noise covariance matrices, all state vectors, and the measured value vector.

[0181] S23. Obtain the origin boundary conditions and end boundary conditions of the flexible robot, use the origin boundary conditions and end boundary conditions as virtual measurement values ​​as constraints, update the linear measurement model through the constraints, and obtain the measurement noise model.

[0182] In this embodiment, as Figure 2 As shown, the discrete measurement pose and measurement noise of the flexible robot at all working points are captured by sensors. All working points are K measurement points randomly selected by the prior probability model, i.e., k=1,…,K. Substituting these into the second transformation formula yields the measurement value vector. A linear measurement model is constructed based on all observation matrices, all covariance matrices, all state vectors, and all measurement value vectors. This model represents the pose measurement model at any point, which is disrupted by Gaussian white noise. Specifically, the linear measurement model can be constructed according to the first construction formula, which is:

[0183] ;

[0184] in, This represents the linear measurement model at the k-th operating point. This represents the observation matrix for the k-th operating point. This represents the state vector of the k-th operating point. Let represent the measurement noise at the k-th operating point, and It follows a zero vector with a mean of 6×1, and the measurement covariance matrix is... Gaussian probability distribution, Represents a six-dimensional identity matrix. Represents a six-dimensional zero matrix;

[0185] The origin and end-effector boundary conditions of the flexible robot are obtained, and these conditions are used as constraints in the form of virtual measurements. For the origin, i.e., when k=0, the pose of the origin is initialized to the identity matrix, forming a linear measurement model of the origin. Linear measurement model of the origin Linear measurement model with the k-th operating point Similarly, for the distal end, i.e., when k=K, the angular strain caused by the tendon when driven is proportional to the tension amplitude. This allows the angular strain data of the distal end to be substituted into the measurement vector of the distal end:

[0186] ;

[0187] in, This represents the measurement vector at the Kth operating point, i.e., the end point. Let represent the internal angular strain of the three-dimensional vector at the Kth working point. Further, the resulting linear measurement model for the end effector is:

[0188] ;

[0189] in, This represents the measurement vector at the Kth operating point, i.e., the end point. This represents a linear measurement model at the end. This represents the observation matrix for the Kth operating point. This represents the state vector of the Kth operating point. Let represent the measurement noise at the Kth operating point, and It follows a zero vector with a mean of 9×1, and the measurement covariance matrix is... Gaussian probability distribution;

[0190] Furthermore, when only the tip is loaded, the internal angular strain of the three-dimensional vector monotonically decreases from its maximum value to zero from the base to the end point of the flexible robot. Therefore, the internal angular strain of the three-dimensional vector at the tip is constrained to 0. Further constrained to a local force in the XY plane, the third component of the rotation angle at all working points, i.e., the sixth component of the pose Lie algebra, remains zero along the arc length s of the flexible robot. Based on the above constraints and the observation matrix of all working points, the measurement vector and the linear measurement model are updated to obtain the lift-form measurement vector and the measurement noise model. Specifically, the lift-form measurement vector is:

[0191]

[0192] Where y represents the lift-form measurement vector, This represents the transpose of the measurement vector of the first working point. This represents the transpose of the measurement vector at the k-th operating point. This represents the transpose of the measurement vector at the Kth operating point. The measurement noise model is as follows:

[0193] ;

[0194] ;

[0195] ;

[0196] in, Represents the measurement noise model. The observation matrix represents the measurement noise model, where blockdiag represents the block diagonal matrix and diag represents the diagonal matrix. This represents the complete measurement covariance matrix of the measurement noise model. This represents the observation matrix for the Kth operating point. The observation matrix representing the origin, Represents the state vector at the origin. This represents the state vector of the Kth operating point. The complete measurement covariance matrix representing the origin. This represents the complete measurement covariance matrix for the Kth operating point.

[0197] S3. The prior probability distribution is fused with the predicted measurement value to obtain the posterior shape estimate of the flexible robot. The continuous three-dimensional shape estimate of the flexible robot and the first confidence level of the continuous three-dimensional shape estimate are calculated based on the posterior shape estimate and the Gaussian process interpolation method.

[0198] At this point, step S3, which involves fusing the prior probability distribution with the predicted measurement to obtain the posterior shape estimate of the flexible robot, includes:

[0199] S31. Construct a prior cost term based on the prior probability distribution, and simultaneously construct a measurement cost term based on the predicted measurement value. The prior cost term is used to penalize the prior probability distribution that deviates from the first threshold, and the measurement cost term is used to penalize the predicted measurement value that deviates from the second threshold.

[0200] S32. Construct a total cost function based on the prior cost term and the measurement cost term. Substitute the total cost function into the Gauss-Newton equation to perform iterative optimization calculation on the prior probability distribution, obtaining the update term for each iteration. Determine whether the L2 norm of the update term is less than the update threshold. If yes, terminate the iterative optimization calculation to obtain the posterior shape estimate of the flexible robot. If not, continue the iterative optimization calculation until the L2 norm of the update term is less than the update threshold. The Gauss-Newton equation is:

[0201] ;

[0202] ;

[0203] W ;

[0204] ;

[0205] Where H represents the augmented Jacobian matrix, Let W be the transpose of the augmented Jacobian matrix, and let W be the weight matrix. This represents the inverse of the weight matrix. Represents the total cost function. Indicates the update item. This represents the lifting state transition matrix. Represents the lifting form of the observation matrix. This indicates an increase in the covariance matrix. This represents the complete measurement covariance matrix in lifting form. Represents the prior cost term. This represents the measurement cost item.

[0206] In this embodiment, as Figure 3 As shown, a prior cost term is constructed based on the prior probability distribution. This prior cost term is used to penalize prior probability distributions that deviate from a first threshold. The prior cost term can be used... express:

[0207] ;

[0208] in, Represents the prior cost term. This represents the lifting state transition matrix. This indicates boosting the input vector. Represents the state vector of all operating points;

[0209] Simultaneously, a measurement cost term is constructed based on the predicted measurement value. This measurement cost term is used to penalize predicted measurement values ​​that deviate from the second threshold. The measurement cost term can be used... express:

[0210] ;

[0211] in, Let y represent the measurement cost term, and let y represent the lift-form measurement value vector. Represents the measurement noise model;

[0212] Based on prior cost terms and measurement cost item Construct the total cost function , the total cost function Substituting these values ​​into the Gauss-Newton equations allows for iterative optimization of the prior probability distribution, yielding an update term for each iteration. Determine the update item 2-norm If the value is less than the update threshold, the iterative optimization calculation is terminated to obtain the posterior shape estimate of the flexible robot. Specifically, the state vectors of all working points are updated according to the update term to obtain the updated state vectors of all working points.

[0213] ;

[0214] in, This represents the updated state vector of all operating points. This represents the state vector of all operating points. Indicates the updated item;

[0215] The mean of the updated state vectors of all operating points is calculated, and this mean is used as the posterior shape estimate. That is, it is assumed that the updated state vectors of all operating points follow a Gaussian probability distribution of this mean and the new covariance matrix obtained from this mean, which can be expressed by the following formula:

[0216] ;

[0217] The new covariance matrix can be calculated using the Laplace approximation method:

[0218] ;

[0219] in, This represents the updated state vector of all operating points, where N represents a Gaussian probability distribution. This represents the mean of the state vectors of all operating points after the update. H represents the new covariance matrix obtained from the mean of the updated state vectors of all operating points; H represents the augmented Jacobian matrix. Let W be the transpose of the augmented Jacobian matrix, and let W be the weight matrix. This represents the inverse of the weight matrix.

[0220] At this point, obtaining the posterior shape estimate of the flexible robot in step S3 includes:

[0221] S33. Calculate the posterior pose covariance matrix of the posterior shape estimation, and use the posterior pose covariance matrix as the second confidence level of the posterior shape estimation.

[0222] In this embodiment, the posterior pose mean and posterior pose covariance matrix corresponding to the kth working point in the posterior shape estimation are calculated. The posterior pose covariance matrix is ​​used as the second confidence level of the posterior shape estimation. Meanwhile, the posterior pose corresponding to the kth working point follows a Gaussian probability distribution of the posterior pose mean and posterior pose covariance matrix.

[0223] At this point, step S3, which involves calculating the continuous three-dimensional shape estimate of the flexible robot based on the posterior shape estimation and the Gaussian process interpolation method, and the first confidence level of the continuous three-dimensional shape estimate, includes:

[0224] S34. The Gaussian process interpolation method is used to calculate the posterior mean of the state of the intermediate point and the posterior covariance matrix of the intermediate point of the posterior shape estimate. Based on the posterior mean of the state of the intermediate point and the posterior covariance matrix of the intermediate point, the continuous three-dimensional shape estimate of the flexible robot and the first confidence level of the continuous three-dimensional shape estimate are obtained.

[0225] In this embodiment, as Figure 3 As shown, the Gaussian process interpolation method is used to calculate the posterior mean of the state at the intermediate point of the posterior shape estimation and the posterior covariance matrix of the intermediate point. The specific calculation method is as follows:

[0226] ;

[0227] ;

[0228] ;

[0229] ;

[0230] ;

[0231] ;

[0232] ;

[0233] in, This represents the posterior mean of the state at the intermediate point. Indicates the midpoint. This represents the prior state vector of the intermediate point. This represents the backward correction gain matrix. This represents the forward propagation gain matrix. This represents the transpose of the backward correction gain matrix. This represents the transpose of the forward propagation gain matrix. This represents the posterior state vector at the k-th operating point. This represents the posterior state vector at the (k+1)th operating point. This represents the prior state vector for the k-th operating point. This represents the prior state vector for the (k+1)th operating point. This represents the posterior covariance matrix of the intermediate point. This represents the prior covariance matrix of the intermediate point. This represents the posterior covariance matrix of the state at the k-th operating point. Let represent the posterior covariance matrix of the state at the (k+1)th operating point. This represents the posterior forward cross-covariance matrix between the states at the k-th and k+1-th operating points. This represents the posterior backward cross-covariance matrix between the states at the k-th and k+1-th operating points. This represents the prior covariance matrix of the state at the k-th operating point. Let denote the prior covariance matrix of the state at the (k+1)th operating point. This represents the prior forward cross-covariance matrix between the states at the k-th and k+1-th operating points. This represents the prior backward cross-covariance matrix between the states at the k-th and k+1-th operating points. This represents the state transition matrix between the k-th operating point and the intermediate point. This represents the state transition matrix between the (k+1)th working point and the intermediate point. This represents the process noise covariance matrix at the intermediate point. Let represent the inverse matrix of the prior covariance at the (k+1)th operating point. Let L(s) represent the prior covariance matrix of the intermediate point, and L(s) represent the noise Jacobian matrix. Represents all n t A constant column vector composed of the tension of the root tendon. This represents the diagonal constant matrix of the stationary power spectral density.

[0234] The posterior mean of the state at the intermediate point is used as the continuous 3D shape estimate of the flexible robot, and the posterior covariance matrix of the intermediate point is used as the first confidence level of the continuous 3D shape estimate.

[0235] Example 2

[0236] Please refer to Figure 4 The present invention provides a three-dimensional continuous shape estimation system 1 for a flexible robot, including a memory 3, a processor 2, and a computer program stored in the memory 3 and executable on the processor 2. When the processor 2 executes the computer program, it implements the steps in Embodiment 1.

[0237] Since the systems / devices described in the above embodiments of the present invention are systems / devices used to implement the methods of the above embodiments of the present invention, those skilled in the art can understand the specific structure and modifications of the systems / devices based on the methods described in the above embodiments of the present invention, and therefore will not be repeated here. All systems / devices used in the methods of the above embodiments of the present invention fall within the scope of protection of the present invention.

[0238] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0239] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, as well as combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions.

[0240] It should be noted that any reference numerals placed between parentheses in the claims should not be construed as limiting the claims. The word "comprising" does not exclude the presence of components or steps not listed in the claims. The word "a" or "an" preceding a component does not exclude the presence of a plurality of such components. The invention can be implemented by means of hardware comprising several different components and by means of a suitably programmed computer. In claims that enumerate several means, several of these means may be embodied by the same hardware. The use of the terms first, second, third, etc., is merely for convenience of expression and does not indicate any order. These terms can be understood as part of the component names.

[0241] Furthermore, it should be noted that in the description of this specification, the terms "one embodiment," "some embodiments," "embodiment," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Furthermore, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0242] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the claims should be interpreted to include both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0243] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, then this invention should also include these modifications and variations.

Claims

1. A method for estimating the three-dimensional continuous shape of a flexible robot, characterized in that, include: The arc length of the flexible robot is obtained. Based on the arc length and the unknown external load from the environment, a Cosserat elastic rod model and a Cosserat elastic string model are combined to generate a Lie algebra-coupled static model representing tendon actuation. At the same time, the external load is modeled as Gaussian process noise. Based on the Gaussian process noise, the Lie algebra-coupled static model is transformed into a system of stochastic differential equations. The stochastic differential equations are linearized to construct a prior probability model. The prior probability distribution of the flexible robot in the Lie algebra space is obtained through the prior probability model. The flexible robot's discrete measurement pose and measurement noise at all working points are captured by sensors. A measurement noise model is constructed in Lie algebra space based on the discrete measurement pose and the measurement noise. The predicted measurement value of the flexible robot in Lie algebra space is obtained through the measurement noise model. The prior probability distribution is fused with the predicted measurement to obtain the posterior shape estimate of the flexible robot. The continuous three-dimensional shape estimate of the flexible robot and the first confidence level of the continuous three-dimensional shape estimate are calculated based on the posterior shape estimate and the Gaussian process interpolation method.

2. The method for estimating the three-dimensional continuous shape of a flexible robot as described in claim 1, characterized in that, The process of combining the arc length and unknown external loads from the environment with the Cosserat elastic rod model and the Cosserat elastic string model to generate a Lie algebraically coupled static model representing tendon drive, while modeling the external loads as Gaussian process noise, and transforming the Lie algebraically coupled static model into a system of stochastic differential equations based on the Gaussian process noise, includes: The shape of the flexible robot is defined by the pose of the three-dimensional special Euclidean group SE(3) using the arc length as a variable, and the motion equations in the Cosserat elastic rod model are converted into generalized motion equations expressed by the Lie algebra se(3) elements of the three-dimensional special Euclidean group SE(3) using a first conversion formula. The first conversion formula is: ; ; ; in, Let s represent the generalized equation of motion, and s represent the arc length of the flexible robot. The shape of the flexible robot is defined by the pose of the three-dimensional special Euclidean group SE(3). express function, Represents the internal linear strain of a three-dimensional vector. Represents the internal angular strain of a three-dimensional vector. Represents the generalized strain of a six-dimensional vector. Let SO(3) represent the pose of a flexible robot belonging to the three-dimensional orthogonal group SO(3). Represents the position of the flexible robot as a three-dimensional vector; Through logarithmic mapping, H The t function and the first formula define the Lie algebra se(3) elements of the pose to obtain the new pose. The first formula is: ; in, Let represent the new pose, and s represent the arc length of the flexible robot. H represents The inverse function of the t function, The shape of a flexible robot is defined by the pose of the three-dimensional special Euclidean group SE(3); Obtain the distributed force function and distributed torque function along the length of the flexible robot. Based on the distributed force function, the distributed torque function, the right Jacobian matrix of the three-dimensional special Euclidean group SE(3), and the left Jacobian matrix of the three-dimensional special Euclidean group SE(3), connect the new pose with the generalized equation of motion in the Lie algebra space to obtain the Lie algebraic static model. The Lie algebraic static model is as follows: ; ； in, Let represent the first-order rate of change of the new pose with respect to s, where s represents the arc length of the flexible robot. Let be the inverse of the right Jacobian matrix of the three-dimensional special Euclidean group SE(3). Represents the generalized strain of a six-dimensional vector. This represents the first-order rate of change of the generalized strain of a six-dimensional vector with respect to s. The matrix representing the diagonal stiffness constants under shear tension. The matrix representing the inverse of the diagonal stiffness constant matrix under shear tension. Represents the internal linear strain of a three-dimensional vector. The reference line strain represents the three-dimensional vector. Let f(x) represent the pose transpose of a flexible robot that belongs to a three-dimensional square matrix of the three-dimensional orthogonal group SO(3). This represents the distributed force function along the length of the flexible robot. The matrix representing the diagonal stiffness constants in bending and torsion. The inverse matrix representing the diagonal stiffness constant matrix of bending and torsion. Represents the internal angular strain of a three-dimensional vector. The reference angular strain represents a three-dimensional vector. This represents the distributed torque function along the length of the flexible robot. According to the decomposition formula and the external load, the distributed force function is decomposed into tendon force and external load force. Simultaneously, the distributed moment function is decomposed into tendon moment and external load moment. The tendon force is calculated based on the Cosserat elastic string model and the first formula, and the tendon moment is calculated based on the Cosserat elastic string model and the second formula. The decomposition formula is as follows: ; ; in, Represents the distributed force function. Indicates the force exerted by the tendon. Indicates the external load force. Represents the distributed moment function. Indicates the torque exerted by the tendon. Indicates the torque of the external load; The first formula is: ; ; in, The force exerted by the tendon is represented by s, and the arc length of the flexible robot is represented by s. Indicates the total number of tendons. This represents the driving force of the i-th tendon. This represents the first-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s. Let represent the second-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s, and let p(s) represent the position of the flexible robot in three-dimensional vectors. Let represent the pose of a flexible robot belonging to a three-dimensional square matrix of the three-dimensional orthogonal group SO(3). This represents the position coordinates of the i-th tendon in the local coordinate system; The second formula is: ; ; in, The torque represents the force exerted by the tendon, and s represents the arc length of the flexible robot. Indicates the total number of tendons. This represents the driving force of the i-th tendon. This represents the first-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s. Let represent the second-order rate of change of the position coordinates of the i-th tendon in global coordinates with respect to s, and let p(s) represent the position of the flexible robot in three-dimensional vectors. Let represent the pose of a flexible robot belonging to a three-dimensional square matrix of the three-dimensional orthogonal group SO(3). This represents the position coordinates of the i-th tendon in the local coordinate system; The tendon force and the tendon torque are substituted into the Lie algebraic static model to obtain the Lie algebraic coupled static model. At the same time, the external load force and the external load torque are modeled as Gaussian process noise. Based on the Gaussian process noise, the Lie algebraic coupled static model is transformed into a system of stochastic differential equations.

3. The method for estimating the three-dimensional continuous shape of a flexible robot as described in claim 2, characterized in that, The step of simultaneously modeling the external load force and the external load torque as Gaussian process noise, and transforming the Lie algebra-coupled static model into a system of stochastic differential equations based on the Gaussian process noise, includes: The external load force is modeled as Gaussian process noise, and the external load torque is modeled as Gaussian process noise, both of which conform to a first vector formula, which is: ； in, Indicates Gaussian process noise. Indicates external force noise. Let represent the external torque noise, s represent the arc length of the flexible robot, and GP represent the Gaussian process. This represents the diagonal constant matrix of the stationary power spectral density; The Lie algebra-coupled static model is transformed into a system of stochastic differential equations based on the Gaussian process noise.

4. The method for estimating the three-dimensional continuous shape of a flexible robot as described in claim 1, characterized in that, The process of obtaining the prior probability distribution of the flexible robot in the Lie algebra space through the prior probability model includes: The prior probability model randomly selects K measurement points as working points from all points of interest of the flexible robot, obtains the state vectors of all working points, calculates the mean and covariance matrix of the state vectors, and obtains the prior probability distribution of the flexible robot in the Lie algebra space based on the mean and the covariance matrix.

5. The three-dimensional continuous shape estimation method for a flexible robot as described in claim 1, characterized in that, The construction of the measurement noise model in the Lie algebra space based on the discrete measurement pose and the measurement noise includes: Obtain the Lie algebra of the measured pose of the flexible robot at all working points, and substitute the Lie algebra of the measured pose into the second transformation formula to obtain the measured value vector. The second transformation formula is: in, Denotes the Lie algebra of the measurement pose at the k-th working point. This represents the vector of measurement values ​​at the k-th operating point; Obtain the observation matrix, the noise covariance matrix of the observation noise at each operating point, and the state vector at each operating point. Construct a linear measurement model based on all observation matrices, all noise covariance matrices, all state vectors, and the measured value vector. The origin boundary conditions and end-effector boundary conditions of the flexible robot are obtained. The origin boundary conditions and end-effector boundary conditions are used as constraints in the form of virtual measurements. The linear measurement model is updated through the constraints to obtain the measurement noise model.

6. The method for estimating the three-dimensional continuous shape of a flexible robot as described in claim 1, characterized in that, The step of fusing the prior probability distribution with the predicted measurement to obtain the posterior shape estimate of the flexible robot includes: A prior cost term is constructed based on the prior probability distribution, and a measurement cost term is constructed based on the predicted measurement value. The prior cost term is used to penalize the prior probability distribution that deviates from the first threshold, and the measurement cost term is used to penalize the predicted measurement value that deviates from the second threshold. A total cost function is constructed based on the prior cost term and the measurement cost term. This total cost function is then substituted into the Gauss-Newton equation to iteratively optimize the prior probability distribution, obtaining an update term for each iteration. The L2 norm of the update term is then determined to be less than an update threshold. If it is, the iterative optimization calculation terminates to obtain the posterior shape estimate of the flexible robot. If not, the iterative optimization calculation continues until the L2 norm of the update term is less than the update threshold. The Gauss-Newton equation is: ； ; W ; ； Where H represents the augmented Jacobian matrix, Let W be the transpose of the augmented Jacobian matrix, and let W be the weight matrix. This represents the inverse of the weight matrix. Represents the total cost function. Indicates the update item. This represents the lifting state transition matrix. Represents the lifting form of the observation matrix. This indicates an increase in the covariance matrix. This represents the complete measurement covariance matrix in lifting form. Represents the prior cost term. This represents the measurement cost item.

7. The three-dimensional continuous shape estimation method for a flexible robot as described in claim 1, characterized in that, The posterior shape estimate of the flexible robot includes: Calculate the posterior pose covariance matrix of the posterior shape estimate, and use the posterior pose covariance matrix as the second confidence level of the posterior shape estimate.

8. The method for estimating the three-dimensional continuous shape of a flexible robot as described in claim 1, characterized in that, The calculation of the continuous three-dimensional shape estimate of the flexible robot and the first confidence level of the continuous three-dimensional shape estimate based on the posterior shape estimation and Gaussian process interpolation method includes: The Gaussian process interpolation method is used to calculate the posterior mean of the state at the intermediate point of the posterior shape estimate and the posterior covariance matrix of the intermediate point. Based on the posterior mean of the state at the intermediate point and the posterior covariance matrix of the intermediate point, the continuous three-dimensional shape estimate of the flexible robot and the first confidence level of the continuous three-dimensional shape estimate are obtained.

9. A three-dimensional continuous shape estimation system for a flexible robot, 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 as described in any one of claims 1 to 8.

Images