Contouring method for robotic systems using gaussian process regression

Abstract

The application discloses a continuum robot state estimation method based on Gaussian process regression and belongs to the field of robot technology and state estimation. The method comprises the following steps: first, a rod body is divided into multiple continuous intervals; then, a continuous state kinematics model containing the rod body pose and generalized strain is established for each interval, and an a priori error term and a measurement error term are respectively constructed, and a whole optimization objective function containing the pose, the generalized strain and the first and second order derivatives thereof is further constructed; the measured data is used to iteratively optimize the objective function, and the optimal state variables of all discrete nodes are obtained; finally, a Gaussian process interpolation method is used to obtain the optimal state variables and estimation uncertainty of any position in the interval according to the optimal state variables of the nodes at both ends of the interval. The application is suitable for all continuum robots which can be described by the Cosserat rod model and the shape perception, motion control and external force perception scenes of the continuum robots.

Description

Technical Field

[0001] This invention belongs to the field of robotics and state estimation technology, specifically relating to a state estimation method for a continuum robot based on Gaussian process regression. Background Technology

[0002] Continuum robots, with their core being a continuous, compliant structure, are typically constructed by cutting grooves into tubular materials or connecting similar units. This gives them superior flexibility, compliance, and agility compared to traditional serial robots. They possess characteristics such as continuous bending deformation, high compliance, and small, lightweight design, enabling them to perform precise operations in scenarios where traditional rigid robots struggle, such as minimally invasive medical surgery, internal inspection of aero-engines, and exploration of complex, confined spaces. This represents a significant research direction in the field of robotics. In these applications, accurately acquiring continuous state information along the robot's arc length is a prerequisite for achieving precise motion control, safe environmental interaction, and risk avoidance.

[0003] State estimation of a continuum robot requires inverting the core states such as pose and strain along the entire length of the rod by combining discrete sensor measurements with the robot's mechanical model.

[0004] Existing state estimation methods can be mainly divided into three categories: The first category is deterministic optimization methods based on shape fitting. These methods use continuous functions such as polynomials, constant curvature arcs, and Bézier curves to fit discrete measured values ​​and reconstruct the robot's shape. However, these methods do not explicitly consider sensor noise and model uncertainty, cannot quantify the confidence level of the estimation results, and are difficult to solve for higher-order derivatives of strain, resulting in poor robustness. The second category is stochastic estimation methods based on filtering frameworks. These methods often construct mechanical models based on Cosserat / Kirchhoff rod theory and use Kalman filtering to iteratively integrate along the arc length of the rod to achieve state estimation. These methods require point-by-point recursion, with errors accumulating along the arc length, and cannot achieve a globally optimal estimate. Furthermore, the iterative integration calculation mode leads to low solution efficiency and makes it difficult to simultaneously estimate the first and second-order higher-order derivatives of strain, thus failing to provide effective data support for subsequent external force sensing and dynamic control. The third category is state estimation methods based on Gaussian process regression. These methods use Gaussian processes to interpolate and estimate continuous states, can adapt to discrete and noisy measurement data, and simultaneously output the uncertainty of the estimation results. However, existing methods can only estimate robot pose and basic strain, and cannot simultaneously solve for the first and second order higher derivatives of strain. The higher derivatives of strain are key inputs for the estimation of external forces in continuum robots. In addition, the method only uses a single exponential mapping method in pose linearization, which has high computational complexity and is not compatible with the lightweight Cayley mapping scheme. It is difficult to balance computational efficiency and adaptability to multiple scenarios, and cannot meet the real-time state estimation requirements of various continuum robots based on the Cosserat lever model.

[0005] Therefore, there is an urgent need to propose a real-time state estimation method for continuum robots. Summary of the Invention

[0006] To address the problems in existing continuum robot state estimation techniques, such as the lack of consideration for model and measurement uncertainties, the inability to simultaneously solve for higher-order strain derivatives, and the tendency for errors to accumulate along the arc length, this invention provides a continuum robot state estimation method based on Gaussian process regression.

[0007] The specific technical solution is as follows:

[0008] S1 divides the arc length of the continuous robot's shaft into multiple continuous intervals, each interval containing a discrete node as the start point of the interval and a discrete node as the end point of the interval.

[0009] S2, for each interval, based on the Cosserat rod theory, a continuous state kinematic model is constructed that includes the rod pose and generalized strain;

[0010] S3, based on the continuous state kinematic model, combines the generalized strain to construct the expression for the prior error term, and combines the generalized strain or pose to construct the expression for the measurement error term;

[0011] S4. Using the prior error term expression and the measurement error term expression, a global optimization objective function for the discrete node state variables is constructed; the state variables include pose, generalized strain, and the first and second derivatives of the generalized strain with respect to any position on the pole.

[0012] S5, iteratively solve the overall optimization objective function until the stopping condition is met, at which point the optimal state variables of all discrete nodes of the pole are obtained;

[0013] S6, taking intervals as units, calculates the optimal estimates and estimation uncertainties for all positions in the interval based on the optimal state variables of two discrete nodes within the interval through Gaussian process interpolation; and then iterates through all intervals on the pole to obtain the optimal estimates and estimation uncertainties for the state variables at all positions on the pole, which are used as the state estimation results.

[0014] Furthermore, in S2, for any position on the rod, the continuous state kinematic model includes:

[0015] The first derivative of the pose matrix with respect to position is equal to the product of the antisymmetric matrix of the Lie algebra corresponding to the generalized strain and the pose matrix; the pose matrix is ​​a block matrix. , Let be a rotation matrix. For displacement vectors, It is a row vector with one row and three columns, and all elements are 0. The generalized strain is scalar 1; , For linear strain components, These are angular strain components;

[0016] The sum of the first derivative of the internal force with respect to position and the external force acting on the robot is zero;

[0017] The first derivative of the internal torque with respect to position, plus the cross product of the first derivative of the displacement vector with respect to position and the internal force, equals zero.

[0018] Furthermore, in S3, the process of constructing the prior error term expression is as follows:

[0019] Construct a bidirectional mapping relationship between pose in Lie group space and Lie algebra space, thereby mapping pose to Lie algebra space and obtaining transformation Lie algebra;

[0020] Based on the continuous state kinematic model, the transformation Lie algebra, and the third derivative of generalized strain with respect to position, a first-order linear stochastic differential equation is constructed through a zero-mean white noise Gaussian process prior, and further transformations are used to obtain the Gaussian process representation of the local state variables.

[0021] Based on the first-order linear stochastic differential equation, the expressions for the closed-form state transition matrix and the process noise covariance matrix are derived; the expression for the closed-form state transition matrix is ​​specifically as follows:

[0022] ;

[0023] The expression for the process noise covariance matrix is ​​as follows:

[0024] ;

[0025] in, , Let s be any position s within the interval and the endpoint of the interval, respectively. The closed-form state transition matrix and process noise covariance matrix, , All are intermediate variable matrices. Represents the Kronecker product. For the increment of arc length, Increment of arc length squared, The preset process noise matrix; It is the identity matrix;

[0026] Combining the closed-form state transition matrix expression and the Gaussian process representation of the local state variables, an expression for the prior error value distributed along the rod is constructed, specifically:

[0027] ;

[0028] in, For the first shaft The prior error values ​​for each interval. The starting point of the interval and the end point of the interval The closed-form state transition matrix, and These are the local state variables at the end and beginning of the interval, respectively.

[0029] By combining the prior error value expression and the process noise covariance matrix expression, the prior error term expression is obtained.

[0030] Furthermore, the expression for the prior error term is obtained by multiplying the transpose of the prior error value, the inverse of the process noise covariance matrix, and the prior error value.

[0031] Furthermore, in S3, the expression for the measurement error term consists of the transpose of the measurement error value, the inverse matrix of the measurement noise covariance, and the product of the measurement error values;

[0032] The measurement error value is divided into two types: strain measurement and pose measurement. The pose measurement is obtained by multiplying the pose in the state variable at the end of the interval with the inverse matrix of the actual pose measurement value and then mapping it to the Lie algebra space. The strain measurement is obtained by subtracting the generalized strain in the state variable at the end of the interval from the actual generalized strain measurement value.

[0033] Furthermore, in S4, the overall optimization objective function is specifically: under the constraint that the pose matrix of all intervals belongs to the Lie group space, the sum of the prior error terms and the sum of the measurement error terms of all intervals are minimized, with the state variables corresponding to all discrete nodes as optimization variables.

[0034] The specific calculation formula is as follows:

[0035] ;

[0036] in, "With respect to" is an abbreviation for "with respect to" and represents the variable to be optimized. "Such that" is an abbreviation for a constraint condition. For any of the logical operators, Let the state variable be the endpoint of the k-th interval. Let be the pose of the endpoint of the k-th interval. For the generalized strain at the endpoint of the k-th interval, Let be the first derivative of the generalized strain at the endpoint of the k-th interval. Let be the second derivative of the generalized strain at the endpoint of the k-th interval, where K is the total number of intervals in the rod. This represents the measurement error value at the endpoint of the k-th interval; Let be the inverse matrix of the measurement noise covariance at the endpoint of the k-th interval; It is the inverse matrix of the process noise covariance matrix of the k-th interval.

[0037] Furthermore, in S5, the iterative solution process of the overall optimization objective function is as follows:

[0038] S501, Substitute the prior error value expression and the measurement error value expression into the overall optimization objective function and perform a first-order Taylor expansion to obtain the prior error value expression containing the prior Jacobian matrix and the measurement error value expression containing the measurement Jacobian matrix.

[0039] S502, based on the prior error value expression containing the prior Jacobian matrix and the measurement error value expression containing the measurement Jacobian matrix, the update equation for the state variable increment is derived using the Gauss-Newton method.

[0040] S503, initialize the state variables, solve the update equation of the state variable increment by combining the measured values ​​of the pole body, obtain the state variable increment, and further obtain the updated state variables;

[0041] S504: Repeat S503 using the updated state variables until the stopping condition is met. At this point, the optimal state variables for all discrete nodes on the rod are obtained.

[0042] Furthermore, in step S502, the calculation formula for the update equation is as follows:

[0043] ;

[0044] in, Let T represent the prior Jacobian matrix, measurement Jacobian matrix, prior covariance matrix, measurement covariance matrix, prior error value, and measurement error value for all discrete nodes, respectively. The upper right corner T represents the transpose, and the upper right corner -1 represents the inverse matrix. This represents the increment of the state variables for all discrete nodes.

[0045] Furthermore, in step S6, the process of calculating the optimal estimate of the state variables at all locations within the interval is specifically as follows:

[0046] Using the optimal state variables of two discrete nodes within the interval, the local state variables and interpolation weight matrix of each discrete node are calculated. Furthermore, the mean local state value at all locations within the interval is obtained by solving the Gaussian process closed-form interpolation equation. This mean local state value includes the transformation Lie algebra, generalized strain, and the first and second derivatives of the generalized strain with respect to any position on the rod. The specific calculation formula is as follows:

[0047] ;

[0048] in, and These are the local state means at the start and end points of the interval, respectively. , ; The starting node of the interval The interpolation weight matrix at the point, The endpoint of the interval The interpolation weight matrix at the point, This represents the local mean of the states at all locations within the interval.

[0049] Extracting the generalized strain, the first derivative and the second derivative of the generalized strain with respect to any position of the rod from the local state mean yields the optimal estimates of the generalized strain and the first derivative and the second derivative of the generalized strain with respect to any position of the rod.

[0050] The transformation Lie algebra is converted into a pose matrix in the Lie group space through an exponential mapping or a Cayley mapping, which is the optimal estimate of the pose.

[0051] Furthermore, in step S6, the process of calculating the estimated uncertainty for all positions in the interval is specifically as follows:

[0052] Using the optimal state variables of two discrete nodes within the interval, the interpolation weight matrix is ​​calculated. Then, the covariance matrix for all positions within the interval is obtained by solving the Gaussian process covariance interpolation equation. The covariance matrix includes the transformation Lie algebra, generalized strain, and the first and second derivatives of the generalized strain with respect to any position on the rod. The specific calculation formula is as follows:

[0053] ;

[0054] in, Let be the state covariance matrix. For intermediate variables, superscript Represents the transpose of the inverse matrix. To start from the interval The process noise covariance matrix at any position s within the interval. The starting point of the interval To the end of the interval The noise covariance matrix of the entire process;

[0055] Extracting the transformation Lie algebra, generalized strain, first derivative of strain, and second derivative of strain from the covariance matrix yields the corresponding marginal estimate covariance, which is the estimated uncertainty of each parameter in the state variable.

[0056] The beneficial effects of this invention are:

[0057] (1) This invention achieves synchronous estimation of state in all dimensions, constructs a continuous state prior model containing high-order state variables, and combines a closed Gaussian process interpolation equation. Without additional numerical difference calculations, it can synchronously output the optimal estimation results of pose, generalized strain, first derivative and second derivative of strain of the continuum robot at any position along the arc length. This fundamentally avoids the noise amplification and accuracy loss problems caused by numerical difference, and provides the necessary high-order state data for the external force estimation and dynamic compliant control of the continuum robot. It solves the problem that existing state estimation methods can only obtain pose and basic strain and cannot synchronously solve the high-order derivative of strain.

[0058] (2) The present invention constructs a global optimization framework based on batch Gaussian process regression, which replaces the traditional filtering method of solving by iteratively applying the solution along the arc length, thus avoiding the problem of error accumulation along the arc length of the pole.

[0059] (3) The present invention simultaneously quantifies the uncertainty of the full state estimation. Through Gaussian process covariance interpolation and matrix marginalization, while outputting the full-dimensional state estimation results, the marginal estimation covariance of pose, generalized strain, strain first derivative and strain second derivative can be obtained simultaneously, realizing the confidence quantification of the estimation results and improving the reliability of the state estimation results of the continuum robot.

[0060] (4) Based on the general Cosserat rod theory, the present invention constructs a core kinematic model and a priori model, which can be adapted to all continuum robots described by the Cosserat rod model, including line-driven continuum robots, pneumatic soft continuum robots and concentric tube continuum robots. Only the corresponding structural dimensions and boundary conditions need to be adjusted. At the same time, it can be compatible with the separate or fused input of two types of sensor data, discrete pose measurement and discrete strain measurement, and is compatible with mainstream sensing devices such as optical motion capture, electromagnetic tracking, fiber Bragg grating sensors, and strain gauges. Only the corresponding measurement model and noise covariance matrix need to be adjusted. It has engineering practicality and scenario expansion. Attached Figure Description

[0061] Figure 1 This is an overall flowchart of the continuum robot state estimation method based on Gaussian process regression of the present invention.

[0062] Figure 2 This is a schematic diagram showing the distribution of the prior error term along the arc length in the Gaussian process prior model of the present invention.

[0063] Figure 3 This is a schematic diagram showing the distribution of the measurement error term along the arc length of the measurement model of the present invention.

[0064] Figure 4 This illustrates the relationship between the number of iteration convergence steps of the optimization equation and the loss in a specific embodiment of the present invention.

[0065] Figure 5 This is a comparison diagram of the reference value and the estimated result of the three-dimensional shape of the continuum robot in a specific embodiment of the present invention.

[0066] Figure 6 The figures represent the uncertainty, reference value, and optimization result of each component in the position estimation result in a specific embodiment of the present invention, where the red line, green line, and blue line represent the x-axis component, y-axis component, and z-axis component, respectively.

[0067] Figure 7The figures represent the uncertainty, reference value, and optimization result of each component in the strain estimation result in a specific embodiment of the present invention, where the red line, green line, and blue line represent the x-axis component, y-axis component, and z-axis component, respectively.

[0068] Figure 8 The figures represent the uncertainty, reference value, and optimization results of each component of the first derivative of the strain vector in a specific embodiment of the present invention, where the red, green, and blue lines represent the x-axis component, y-axis component, and z-axis component, respectively.

[0069] Figure 9 The figures represent the uncertainty, reference value, and optimization results of each component of the second derivative of the strain vector in a specific embodiment of the present invention, where the red, green, and blue lines represent the x-axis component, y-axis component, and z-axis component, respectively. Detailed Implementation

[0070] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Many specific details are set forth in the following description to provide a thorough understanding of the present invention. However, the present invention can be practiced in many other ways different from those described herein, and those skilled in the art can make similar modifications without departing from the spirit of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below. Technical features in the various embodiments of the present invention can be combined accordingly without mutual conflict.

[0071] like Figure 1 As shown, the continuum robot state estimation method based on Gaussian process regression proposed in this invention includes five steps: continuous state kinematic model modeling, prior model construction, measurement model construction, objective function optimization, and Gaussian process interpolation. Each step is specifically as follows:

[0072] Step 1: Construct a continuous state kinematic model of the continuum robot based on the Cosserat link theory, and define the robot state variables with the arc length of the link as the independent variable. The state variables include at least the robot's pose and generalized strain distributed along the arc length.

[0073] 1.1) Taking the arc length of the continuous robot's shaft as the independent variable s, the range of values ​​is... L is the total length of the rod, where a value of 0 represents the base end of the robot rod. The value L represents the end point of the robot's shaft.

[0074] The robot's shaft is further divided into... A continuous interpolation interval, For positive integers, each interval can be represented as: .

[0075] For multi-segment continuous robots, the connection points between each segment are used as the dividing point. The continuous state and uncertainty of each segment are calculated using the method proposed in this invention, and then the segments are reassembled to obtain the overall continuous state and uncertainty of the multi-segment continuous robot.

[0076] 1.2) Based on Cosserat rod theory, a continuous-state kinematic equation is constructed to describe the mapping relationship between the first derivative of the rod's pose with respect to arc length and the generalized strain. The specific form of the continuous-state kinematic equation is as follows:

[0077]

[0078]

[0079]

[0080]

[0081]

[0082] in, Represents a broad sense of response. Operators representing the mapping from Lie algebras to matrix form, The derivative of the pose with respect to the arc length s. Represented by arc length Let be the pose matrix of the variables. Represents a rotation matrix. Represents the displacement vector. , representing the linear strain component of generalized strain; Angular strain components representing generalized strain Represents the relative arc length of internal forces The derivative, Represents external force. Represents internal strength. Represents the relative arc length of the displacement vector The derivative, Represents the relative arc length of the internal moment The derivative, It is a 1*3 zero vector, where 1 is a scalar.

[0083] It is important to note that the Cosserat director frame theory is a mechanical model used to describe the large deformation and wide range of motion of slender structures (such as soft robots, DNA, and submarine cables). The core modeling idea is to simplify a three-dimensional continuum into a combination of a one-dimensional baseline curve and a rigid section attached to the baseline (i.e., the Cosserat director frame). In this invention, it is used to construct the continuous state kinematic model of a continuum robot.

[0084] 1.3) Construct boundary condition constraints for the robot base and end effector, which are used to solve for the optimal geometric state of the nodes in step four. The specific form is as follows:

[0085]

[0086]

[0087]

[0088] in, This represents the robot's initial pose at the base, and its value is a 4x4 identity matrix. This indicates that the rotation matrix of the shaft at this point is the identity matrix. The position vector is That is, no translation occurred; , These represent the internal forces and internal moments at the endpoints, respectively. This represents the external forces acting on the shaft.

[0089] Step 2: Based on the above continuous state kinematic model, apply a zero-mean white noise Gaussian process prior to the derivative of the highest-order state variable to construct a robot continuous state prior model based on Gaussian process regression.

[0090] 2.1) The state variables of the pole (including the pole centerline pose, generalized strain, first derivative of strain, and second derivative of strain) are included in the state estimation range, specifically in the form of:

[0091]

[0092] in, The center line of the representative rod is in the arc length The state of being, The center line of the representative rod is in the arc length Position, The center line of the representative rod is in the arc length Generalized strain at the location, The first derivative representing generalized strain, The second derivative representing generalized strain, .

[0093] 2.2) Construct the pose of the location to be queried using exponential mapping or Cayley mapping. A bidirectional mapping relationship between Lie group SE(3) and Lie algebra se(3).

[0094] Gaussian process regression inherently requires that the state variables reside in a vector space (to facilitate the definition of mean and covariance), while pose... It belongs to the Lie group SE(3) and is not a vector space. Therefore, the pose can be represented by an exponential mapping or a Cayley mapping (in this embodiment, a Cayley mapping is used) to represent the local Lie algebra. The mapping is used to transform the global pose, thereby allowing the pose to indirectly participate in the modeling and interpolation of the Gaussian process.

[0095] The specific form of the exponential mapping is as follows:

[0096]

[0097] The specific form of the Cayley mapping is:

[0098]

[0099] in, The centerline of the rod at the starting node of the interval Position, For the mapping operator from Lie algebra to matrix form, For matrix exponentiation operations, For Cayley operations, For relative The transformation Lie algebra. Both guarantee that the pose transformation matrix strictly satisfies the orthogonality and homogeneity constraints of the SE(3) Lie group.

[0100] Both the exponential mapping and the Cayley mapping methods can ensure that the pose transformation matrix strictly satisfies the orthogonality and homogeneity constraints of the SE(3) Lie group, without any singularity problem; among them, the Cayley mapping can reduce the amount of computation and improve the estimation efficiency.

[0101] 2.3) Constructing the motion prior on SE(3), that is, applying a zero-mean white noise Gaussian process prior to the third derivative of strain, and constructing a piecewise defined first-order linear stochastic differential equation, specifically in the form of:

[0102]

[0103] in, The third derivative of generalized strain, Zero-mean Gaussian white noise, These are local state variables.

[0104] It should be noted that zero-mean Gaussian white noise is a priori of higher-order quantities and is widely used in stochastic system modeling. In this invention, it is used to describe the stochastic system modeling of rod deformation along the arc length.

[0105] The solution of the above linear stochastic differential equation It can be expressed in Gaussian process form, specifically as follows:

[0106]

[0107] in, and represent The initial mean and covariance at this point are assumed values ​​in this step. Represents the mean of a Gaussian process. Represents the covariance of a Gaussian process. Represents a Gaussian process. Represents the noise covariance matrix, top right corner Indicates transpose. This represents the state transition matrix.

[0108] 2.4) Based on the first-order linear stochastic differential equation, the closed-form state transition matrix is ​​derived. With process noise covariance matrix .

[0109] Closed state transition matrix The specific form is:

[0110]

[0111] in, For the intermediate variable matrix, Represents the Kronecker product. For the increment of arc length, Increment of arc length The square of .

[0112] Process noise covariance matrix The specific form is:

[0113]

[0114] in, For the intermediate variable matrix, The process noise matrix represents the disturbance intensity of a random jerk disturbance with infinite bandwidth (white noise) that varies with the arc length s and is experienced by the rod. It is usually given by the actual problem.

[0115] 2.5) Combining the closed-form state transition matrix and the process noise covariance matrix, construct the prior error term distributed along the arc length, such as... Figure 2 As shown.

[0116] First, the constructed prior error Specifically:

[0117]

[0118] in, Represents the shaft The prior error of each interval.

[0119] Secondly, the quadratic form of the prior error term is constructed, specifically as follows:

[0120]

[0121] in, Represents the shaft The prior error term at the queried position s within each interval. The process noise covariance matrix The inverse matrix is ​​used as a weight parameter in the expression for the prior error term.

[0122] Step 3: Obtain the noisy pose measurement values ​​and / or strain measurement values ​​of the continuum robot discretely distributed along the arc length, and construct the corresponding measurement model and measurement error term for the measurement type.

[0123] 3.1) The measurement model is used to describe the mathematical relationship between the state variables of the continuum robot and the expected measurement values. The measurement error term is the difference between the expected measurement value and the actual measurement value. In this invention, a measurement error term based on Lie algebra is constructed for pose measurement, and a measurement error term based on vector space is constructed for strain measurement.

[0124] For strain measurement, the constructed measurement model is as follows:

[0125]

[0126] in, Representative at the The measurement error value at the endpoint of each interval. Representative at the Predicted values ​​at discrete nodes Representative at the Measurements at discrete nodes.

[0127] For pose measurement, taking Cayley mapping as an example, the constructed measurement model is as follows:

[0128]

[0129] in, , Representative at the The pose measurement value at the end point of each interval. This represents the mapping operator from matrix form to Lie algebra.

[0130] 3.2) Based on the measurement error value, obtain the measurement error term distributed along the arc length, such as... Figure 3 As shown, the established measurement error term is in quadratic form, specifically:

[0131]

[0132] in, Represents the shaft The end point of each interval Measurement error term at the location, The inverse matrix of the noise covariance is typically given by sensor parameters and used as a weighting parameter in the expression for the measurement error term.

[0133] In this embodiment, a pose sensing configuration scheme is adopted, in which 6-DOF electromagnetic tracking sensors are installed at discrete nodes on both sides of the robot to obtain discrete pose measurements with noise, and a measurement model in Lie algebra space is constructed based on Cayley mapping. Furthermore, the quadratic form of the measurement error term is obtained. .

[0134] Step 4: Combining the prior error term and measurement error term of the Gaussian process prior model, construct the overall optimization objective function, and iteratively optimize and solve to obtain the optimal state of the robot at the discrete nodes of the arc length.

[0135] Prior error term Measurement error term It is a measurement error cost term in the form of weighted least squares. It is a component of the objective function in the Gaussian process regression state estimation framework. It is used to quantify the deviation between the expected measurement value corresponding to the state estimate and the actual measurement value of the sensor. The optimal fusion of measurement data and mechanical model is achieved by minimizing this cost term.

[0136] 4.1) Combining the prior error term of the Gaussian process prior model Measurement error term The overall optimization objective function is constructed, and its specific form is as follows:

[0137]

[0138] in, "With respect to" is an abbreviation for "with respect to," representing the variable to be optimized. It's an abbreviation for "such that," representing constraints. Any of the logical operators.

[0139] 4.2) The prior error value established in step three With measurement error value Substituting into the overall optimization objective function and linearizing it using a first-order Taylor expansion, we obtain... and The specific form is:

[0140]

[0141] in, Represents the prior error in the th Node (i.e., the tail endpoint of the k-th interval) The linearized baseline value at ) The measurement error is represented in the first place. Linearized baseline values ​​at each node Representing the The state vector at each node Represents the prior error in the th The linearized Jacobian matrix at each node Represents the prior error in the th The linearized Jacobian matrix at each node The measurement error is represented in the first place. The linearized Jacobian matrix at each node.

[0142] 4.3) Calculate the prior Jacobian matrix and the measurement Jacobian matrix in blocks, specifically in the following form:

[0143]

[0144]

[0145]

[0146] in, Representative from the first The node to the first The left Jacobian matrix in the Lie algebra space of the linearized reference values ​​of the relative poses between nodes is used to describe the linearization relationship of pose perturbations under the exponential mapping. Representing the The Jacobian matrix corresponding to the linearized reference value of the relative pose of each node. Representing the The node to the first The adjoint matrix of the transformation matrix between nodes , representing the increment of arc length; It represents a 6x6 identity matrix.

[0147] Left Jacobian matrix and the adjoint matrix This can be further expressed as:

[0148]

[0149]

[0150]

[0151] in, For the adjoint matrix, Representing the The adjoint matrix corresponding to each interval pose matrix Representing the Linearized baseline values ​​of the pose matrix for each interval This represents the left Jacobian matrix.

[0152] Adjoint matrix The specific form is:

[0153]

[0154] It is important to note that the Jacobian matrix is ​​used to linearize the nonlinear kinematic relationships to a first order, enabling numerical solutions. This invention, by constructing a priori and measurement Jacobian matrices, unifies the kinematic constraints and sensor observation models of a continuum robot along the arc length direction into a sparse linear system.

[0155] 4.4) Combining boundary condition constraints, sensor measurements (pose measurements or strain measurements), noise parameters (measurement noise covariance matrix), measured values ​​such as arc length increments between each discrete node, and initial state estimates at discrete nodes, the Gauss-Newton method is used to iteratively solve the overall optimization objective function to obtain the optimal geometric state of all discrete nodes (i.e., endpoints) of the pole.

[0156] In the first iteration, the state variables are obtained through initialization, and the updated state variables are obtained based on the measured values; the next iteration is carried out based on the updated state variables of the previous round.

[0157] During the solution process, due to the first-order Taylor expansion, the optimization variables of the overall objective function are changed from... Become an increment Including the increments of the optimal geometric state variables for all discrete nodes. The update equation is constructed as follows:

[0158]

[0159] in, The prior Jacobian matrix, measurement Jacobian matrix, prior covariance matrix, measurement covariance matrix, prior error value, and measurement error value assembled from the measurement error values ​​of all discrete nodes are respectively obtained by combining all corresponding matrices or error values ​​on the rod.

[0160] by For example, suppose the interval K is 4. The expression is:

[0161]

[0162] in, and These are the two prior Jacobian matrices for the first interval. During the solution process, the measured values ​​are substituted into the corresponding expressions above to obtain the results.

[0163] The increment of the optimal geometric state variables for all discrete nodes, as calculated, is in the following form:

[0164]

[0165] in, Represents the pose increment. Represents the generalized strain increment. Represents the increment of the first derivative of generalized strain. It represents the increment of the second derivative of the generalized strain.

[0166] The coefficient matrix on the left side of the update equation has a block tridiagonal sparse structure, which is achieved using sparse Cholesky decomposition. A fast solution with minimal time complexity yields the optimal state variable increment. Based on the increments of the solved state variables, the state of the current discrete node is iteratively updated until convergence, specifically in the form of:

[0167]

[0168] in, Representing the The linearization point of the pose matrix. Representing the The linearization point of the generalized strain. Representing the The linearization point of the first derivative of the generalized strain section. Representing the The linearization point of the second derivative of the generalized strain section. The representative corresponds to the first The pose increment of a node is represented by a Lie algebra. The representative corresponds to the first The generalized strain increment of the section, The representative corresponds to the first The increment of the first derivative of the generalized strain of the section, The representative corresponds to the first The increment of the second derivative of the generalized strain of the section.

[0169] When the optimal state variable increment When the norm is less than 0.1% of the norm calculated in the previous iteration, the calculation is considered to have converged, and the iteration is stopped (in this specific embodiment, the L2 norm is used), thus obtaining the optimal state and corresponding covariance matrix at all discrete nodes. .

[0170] Step 5: Based on the optimal state of the discrete nodes, the optimal estimates of the robot's pose, generalized strain, first derivative of strain, and second derivative of strain at any position along the arc length are obtained through Gaussian process interpolation. Simultaneously, the estimated uncertainties corresponding to each state variable are extracted to obtain the continuous state and uncertainty of the entire rod.

[0171] The Gaussian process prior constructed in this invention is based on linear time-invariant stochastic differential equations and possesses Markov properties. The posterior distribution of the state at any arc length position within the interval is uniquely determined only by the optimal state and covariance of the discrete nodes at both ends of the interval. The time complexity can be achieved through closed-form interpolation equations. It provides a fast solution without traversing all discrete nodes of the pole.

[0172] 5.1) For the interpolation interval Based on the optimal state obtained in step four at both ends of the interval, the optimal local state is calculated. , The location to be queried is obtained by solving the closed-form interpolation equation of the Gaussian process. The local state mean includes the transformation Lie algebra, generalized strain, and the first and second derivatives of the generalized strain with respect to any position on the rod. :

[0173]

[0174] in, and These are the local state means at the start and end points of the interval, respectively. , ; The starting node of the interval The interpolation weight matrix at the point, The endpoint of the interval The interpolation weight matrix at each point has a closed analytical expression, specifically in the form:

[0175]

[0176] in, The process noise covariance matrix defined in step 2, Let be the closed-form state transition matrix from the start node of the interval to the node to be queried. This is the closed-form state transition matrix from the position to be queried to the end node of the interval. It is a closed-form state transition matrix from the start node to the end node of the interval.

[0177] Extracting local state mean The transformations Lie algebra, generalized strain, strain first derivative, and strain second derivative are used to obtain the optimal estimate at any position within the interval.

[0178] 5.2) The interpolated transformation Lie algebra is converted into a global pose transformation matrix on the SE(3) Lie group through an exponential mapping or Cayley mapping consistent with step 2, to obtain the optimal estimate of the global pose at the query position s. .

[0179] 5.3) For the location to be queried The estimation uncertainty at the given point is determined by the Gaussian process covariance interpolation equation, combined with the interpolation weight matrix. and Solve for the covariance matrix of the local states. That is, the estimation uncertainty.

[0180] The calculation formula is:

[0181]

[0182]

[0183] in, The state covariance matrix is ​​obtained after optimization and convergence of the discrete nodes at the initial end of the interval, and the optimal state variable update equation corresponding to the iteration convergence is the first... The inverse of the coefficient matrix of the section, For intermediate variables in the calculation, superscript Represents the transpose of the inverse matrix. To start from the interval The process noise covariance matrix to s, for arrive The noise covariance matrix of the entire process.

[0184] The covariance matrix obtained by solving Lie algebra of transformations containing the position to be queried Generalized strain First derivative of strain strain second derivative .

[0185] Extract the transformation Lie algebra, generalized strain, first derivative of strain, and second derivative of strain from the covariance matrix respectively. The corresponding matrix block is used to obtain the corresponding marginal estimated covariance, which is the estimated uncertainty of each parameter in the state variable.

[0186] By traversing all interpolation intervals along the continuous robot rod and repeating the above interpolation process for any position of the arc length to be queried, the optimal estimates of the continuous pose, generalized strain, first derivative of strain, and second derivative of strain of the robot at any position along the entire arc length of the rod, as well as the estimation uncertainties of each corresponding state quantity, can be obtained, thus completing the full estimation of the continuous state of the continuous robot in all dimensions.

[0187] To verify the effectiveness of the method proposed in this invention, a single-segment continuous robot whose core compliant structure can be modeled using Cosserat rod theory was selected as the implementation object for simulation experiments.

[0188] In this embodiment, the total length of the continuous robot's shaft is L = 0.1 m; the robot's centerline is made of a superelastic nickel-titanium alloy material with a Young's modulus E = 82 GPa and a diameter d = 1 mm; the robot's base is fixed at one end, while the end can be freely bent. Calculations were performed sequentially using steps one through five, and the experimental results are as follows: Figures 4 to 9 As shown.

[0189] in, Figure 4 This represents the relationship between the number of iterations to convergence and the loss in the optimization equation. Figure 5 This represents a comparison between the reference value and the optimization result of the shape in three-dimensional space. Figure 6 This represents the uncertainty, reference value, and optimization result of each component of the position vector. Red, green, and blue represent the x, y, and z axis components, respectively. Figure 7 The values ​​represent the uncertainty, reference value, optimization result, and measured value of each component of the strain vector, with red, green, and blue representing the x, y, and z axis components, respectively.

[0190] in, Figure 8 This represents the uncertainty, reference value, and optimization result of each component of the first derivative of the strain vector. Red, green, and blue represent the x, y, and z axis components, respectively. Figure 9 The variables represent the uncertainty, reference value, and optimization result of each component of the second derivative of the strain vector. Red, green, and blue represent the x, y, and z axis components, respectively.

[0191] It can be seen that this embodiment can realize the estimation of continuous pose, strain and higher-order derivatives of a continuum robot. It can usually converge in 3-5 iterations with a time of less than 1 ms, which can meet the real-time calculation requirements.

[0192] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the invention. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the invention. Therefore, all technical solutions obtained through equivalent substitution or transformation fall within the protection scope of the present invention.

Claims

1. A continuum robot state estimation method based on Gaussian process regression, characterized in that, include: S1 divides the arc length of the continuous robot's shaft into multiple continuous intervals, each interval containing a discrete node as the start point of the interval and a discrete node as the end point of the interval. S2, for each interval, based on the Cosserat rod theory, a continuous state kinematic model is constructed that includes the rod pose and generalized strain; S3, based on the continuous state kinematic model, combines the generalized strain to construct the expression for the prior error term, and combines the generalized strain or pose to construct the expression for the measurement error term; S4. Using the prior error term expression and the measurement error term expression, we construct the overall optimization objective function for the discrete node state variables. The state variables include pose, generalized strain, and the first and second derivatives of the generalized strain with respect to any position on the shaft. S5, iteratively solve the overall optimization objective function until the stopping condition is met, at which point the optimal state variables of all discrete nodes of the pole are obtained; S6, taking intervals as units, calculates the optimal estimates and estimation uncertainties for all positions in the interval based on the optimal state variables of two discrete nodes within the interval through Gaussian process interpolation; and then iterates through all intervals on the pole to obtain the optimal estimates and estimation uncertainties for the state variables at all positions on the pole, which are used as the state estimation results.

2. The continuum robot state estimation method based on Gaussian process regression according to claim 1, characterized in that, In S2, for any position on the rod, the continuous state kinematic model includes: The first derivative of the pose matrix with respect to position is equal to the product of the antisymmetric matrix of the Lie algebra corresponding to the generalized strain and the pose matrix; the pose matrix is ​​a block matrix. , Let be a rotation matrix. It is a displacement vector. It is a row vector with one row and three columns, and all elements are 0. The generalized strain is scalar 1; , For linear strain components, These are angular strain components; The sum of the first derivative of the internal force with respect to position and the external force acting on the robot is zero; The first derivative of the internal torque with respect to position, plus the cross product of the first derivative of the displacement vector with respect to position and the internal force, equals zero.

3. The continuum robot state estimation method based on Gaussian process regression according to claim 1, characterized in that, In S3, the specific process of constructing the prior error term expression is as follows: Construct a bidirectional mapping relationship between pose in Lie group space and Lie algebra space, thereby mapping pose to Lie algebra space and obtaining transformation Lie algebra; Based on the continuous state kinematic model, the transformation Lie algebra, and the third derivative of generalized strain with respect to position, a first-order linear stochastic differential equation is constructed through a zero-mean white noise Gaussian process prior, and further transformations are used to obtain the Gaussian process representation of the local state variables. Based on the first-order linear stochastic differential equation, the expressions for the closed-form state transition matrix and the process noise covariance matrix are derived; the expression for the closed-form state transition matrix is ​​specifically as follows: ； The expression for the process noise covariance matrix is ​​as follows: ； in, , Let s be any position s within the interval and the endpoint of the interval, respectively. The closed-form state transition matrix and process noise covariance matrix, , All are intermediate variable matrices. Represents the Kronecker product. For the increment of arc length, Increment of arc length squared, The preset process noise matrix; It is the identity matrix; By combining the closed-form state transition matrix expression and the Gaussian process representation of local state variables, an expression for the prior error value distributed along the pole is constructed. By combining the prior error value expression and the process noise covariance matrix expression, the prior error term expression is obtained.

4. The continuum robot state estimation method based on Gaussian process regression according to claim 3, characterized in that, The expression for the prior error term is obtained by multiplying the transpose of the prior error value, the inverse of the process noise covariance matrix, and the prior error value.

5. The continuum robot state estimation method based on Gaussian process regression according to claim 1, characterized in that, In S3, the expression for the measurement error term consists of the transpose of the measurement error value, the inverse matrix of the measurement noise covariance, and the product of the measurement error values. The measurement error value is divided into two types: strain measurement and pose measurement. The pose measurement is obtained by multiplying the pose in the state variable at the end of the interval with the inverse matrix of the actual pose measurement value and then mapping it to the Lie algebra space. The strain measurement is obtained by subtracting the generalized strain in the state variable at the end of the interval from the actual generalized strain measurement value.

6. The continuum robot state estimation method based on Gaussian process regression according to claim 1, characterized in that, In S4, the overall optimization objective function is as follows: under the constraint that the pose matrix of all intervals belongs to the Lie group space, the state variables corresponding to all discrete nodes are used as optimization variables to minimize the sum of the prior error terms and the sum of the measurement error terms of all intervals.

7. The continuum robot state estimation method based on Gaussian process regression according to claim 1, characterized in that, In S5, the iterative solution process of the overall optimization objective function is as follows: S501, Substitute the prior error value expression and the measurement error value expression into the overall optimization objective function and perform a first-order Taylor expansion to obtain the prior error value expression containing the prior Jacobian matrix and the measurement error value expression containing the measurement Jacobian matrix. S502, based on the prior error value expression containing the prior Jacobian matrix and the measurement error value expression containing the measurement Jacobian matrix, the update equation for the state variable increment is derived using the Gauss-Newton method. S503, initialize the state variables, solve the update equation of the state variable increment by combining the measured values ​​of the pole body, obtain the state variable increment, and further obtain the updated state variables; S504: Repeat S503 using the updated state variables until the stopping condition is met. At this point, the optimal state variables for all discrete nodes on the pole are obtained.

8. The continuum robot state estimation method based on Gaussian process regression according to claim 7, characterized in that, In step S502, the calculation formula for the update equation is as follows: ； in, Let T represent the prior Jacobian matrix, measurement Jacobian matrix, prior covariance matrix, measurement covariance matrix, prior error value, and measurement error value for all discrete nodes, respectively. The upper right corner T represents the transpose, and the upper right corner -1 represents the inverse matrix. This represents the increment of the state variables for all discrete nodes.

9. The continuum robot state estimation method based on Gaussian process regression according to claim 1, characterized in that, In step S6, the process of calculating the optimal estimate of the state variables at all locations within the interval is as follows: Using the optimal state variables of two discrete nodes within the interval, the local state variables and interpolation weight matrix of each discrete node are calculated. Then, the mean local state value at all positions within the interval is obtained by solving the Gaussian process closed interpolation equation. The mean local state value includes the transformation Lie algebra, generalized strain, and the first and second derivatives of the generalized strain with respect to any position of the rod. Extracting the generalized strain, the first derivative and the second derivative of the generalized strain with respect to any position of the rod from the local state mean yields the optimal estimates of the generalized strain and the first derivative and the second derivative of the generalized strain with respect to any position of the rod. The transformation Lie algebra is converted into a pose matrix in the Lie group space through an exponential mapping or a Cayley mapping, which is the optimal estimate of the pose.

10. The continuum robot state estimation method based on Gaussian process regression according to claim 1, characterized in that, In step S6, the process of calculating the estimated uncertainty of all positions in the interval is as follows: Using the optimal state variables of two discrete nodes within the interval, the interpolation weight matrix is ​​calculated. Then, the covariance matrix of all positions within the interval is obtained by solving the Gaussian process covariance interpolation equation. The covariance matrix includes the transformation Lie algebra, generalized strain, and the first and second derivatives of the generalized strain with respect to any position of the rod. Extracting the transformation Lie algebra, generalized strain, first derivative of strain, and second derivative of strain from the covariance matrix yields the corresponding marginal estimate covariance, which is the estimated uncertainty of each parameter in the state variable.

Images