Method for sensing shape of continuum robot based on fusion of IMU (inertial measurement unit) and PVDF (polyvinylidene fluoride) sensor

Abstract

The invention relates to the field of continuum robot shape reconstruction, in particular to a continuum robot shape sensing method based on IMU and PVDF sensor fusion. The method comprises the following steps: acquiring node attitude information of the continuum robot based on measurement data of an inertial sensor; acquiring node pressure information of the continuum robot through a piezoelectric film sensor, and inputting the node pressure information into a pre-trained convolutional neural network model to obtain node curvature information; fitting a continuous curvature function of the continuum robot based on the node curvature information to obtain continuous curvature information; and the shape information of the continuum robot is obtained by using the node attitude information and the continuous curvature information in combination with the Cosseerat Rod theory. According to the shape sensing method, the PVDF sensor and the inertial sensor work cooperatively, and the machine learning model and the Cosseerat Rod theory are combined, so that high-precision and low-error shape sensing of the continuum robot in a dynamic complex environment is realized.

Description

Technical Field

[0001] The present invention relates to the field of continuum robot shape reconstruction, and particularly to a method for shape perception of a continuum robot based on the fusion of IMU and PVDF sensors. Background Art

[0002] Continuum robots are composed of flexible materials or flexible joints, and have high compliance and adaptability. They can flexibly move through narrow spaces and are applicable to fields such as medical treatment, pipeline inspection, and deep-sea exploration. However, due to their flexible and non-linear characteristics, it is difficult to accurately describe and perceive the shape of continuum robots through traditional modeling methods. And accurately perceiving the shape of a continuum robot is a prerequisite for motion control, spatial pose visualization, collision warning, etc.

[0003] Currently, most methods for shape perception of continuum robots rely on a single type of sensor, such as inertial sensors, vision sensors, etc. Inertial sensors can provide angle and acceleration data, but there is a problem of cumulative error; while PVDF (polyvinylidene fluoride) sensors can provide pressure and strain information, and have high precision in perceiving the surface deformation of robots. However, the use of a single sensor is often limited in the shape reconstruction process and is difficult to provide sufficient accuracy and stability in complex environments. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to overcome the defects of the prior art and provide a method for shape perception of a continuum robot based on the fusion of IMU and PVDF sensors. It realizes high-precision and low-error shape perception of a continuum robot in a dynamic complex environment through the collaborative work of PVDF sensors and inertial sensors, combined with a machine learning model and Cosserat Rod theory.

[0005] To solve the above technical problem, the technical solution of the present invention is: A method for shape perception of a continuum robot based on the fusion of IMU and PVDF sensors, comprising:

[0006] Obtaining the node attitude information of the continuum robot based on the measurement data of the inertial sensor;

[0007] Obtaining the node pressure information of the continuum robot through the piezoelectric film sensor, and inputting the node pressure information into a pre-trained convolutional neural network model to obtain the node curvature information;

[0008] Based on the node curvature information, fitting the continuous curvature function of the continuum robot to obtain the continuous curvature information;

[0009] Using the node attitude information and the continuous curvature information, and combining with Cosserat Rod theory, to obtain the shape information of the continuum robot.

[0010] Further, obtain the attitude information of the continuum robot node based on the measurement data of the inertial sensor; specifically including:

[0011] According to the angular velocity at the k-th moment and the attitude matrix at the k-th moment predict the attitude matrix at the (k + 1)-th moment

[0012]

[0013] Utilize the acceleration at the k-th moment and the error e of the gravity direction [0, 0, -g] i , and fuse the Kalman filter to correct the predicted attitude matrix to obtain the attitude information at the (k + 1)-th moment

[0014] wherein, is the skew-symmetric matrix of the angular velocity , Δt is the time step, and exp(·) is the matrix exponential map; the angular velocity and the acceleration are both measured by the inertial sensor; is the rotation matrix at the k-th moment, representing the attitude of the carrier at the k-th moment.

[0015] Further, utilize the acceleration at the k-th moment and the error e of the gravity direction [0, 0, -g] i to correct the predicted attitude matrix to obtain the attitude information at the (k + 1)-th moment Specifically:

[0016] Calculate the error e between the acceleration and the gravity direction [0, 0, -g] i ; the formula is:

[0017] Solve the Kalman gain K k ; the formula is: Combine the error e i with the gain K k to generate a rotation increment ΔR, and multiply it with the attitude matrix to obtain the corrected attitude information The formula is: ΔR = exp(K k e i ),

[0018] where P k|k-1is the covariance matrix in the prediction stage, H k is the Jacobian matrix of the observation model, R is the noise covariance, and the update formula for the covariance matrix is P k =(I - K k H k )P k∣k-1 , P k is the covariance matrix in the update stage.

[0019] Furthermore, when the continuum robot is in a known curvature state, a large number of node pressure signals P = [P 1 , P 2 ,..., P n and their corresponding true curvature values κ = [κ 1 , κ 2 ,..., κ n are collected to construct a dataset for training a convolutional neural network model;

[0020] During the process of training the convolutional neural network model, the loss function uses the mean square error MSE:

[0021]

[0022] where n is the number of curvature measurement points, is the predicted output of the model, and λ is the model parameter.

[0023] Furthermore, during the process of fitting the continuous curvature function of the continuum robot, the constraint condition is:

[0024] |κ xδ (s j , η) - κ x (s j )| ≤ ε xj

[0025] |κ yδ (s j , η) - κ y (s j )| ≤ ε xj

[0026] In the formula, κ x (s j ) and κ y (s j ) are the curvature information predicted by the convolutional neural network model at the continuum robot rod length s j ; j is the number of each curvature measurement point; κ xδ (s j , η) and κ yδ (s j , η) are the continuum robot rod length s jwhere, the curvature information obtained based on the continuous curvature function; η is the parameter of the continuous curvature function; ε xj and ε yj are the allowable values of the curvature error.

[0027] Furthermore, using the pose information of the nodes and the continuous curvature information, combined with the Cosserat Rod theory to obtain the shape information of the continuum robot; specifically including:

[0028] Discretize the continuum robot along the axis into M position points, and the m-th position point s m = mΔs, m = 0, 1, …, M, and Δs represents the step size;

[0029] Integrate the continuous curvature information between adjacent position points (the fourth-order Runge-Kutta method or the explicit Euler method can be used), that is, integrate segment by segment, calculate the pose matrix at each position point, and use the interpolation method to obtain the preliminary pose prediction function R(s, δ) of the continuum robot along the axis; among them, the initial condition is that R δ (s 0 ) = I, where I is the identity matrix)

[0030] Taking the difference between the pose information of the nodes and the preliminary pose at the nodes calculated based on the preliminary pose prediction function R(s, δ) as a constraint, optimize the preliminary pose prediction function R(s, δ) to obtain the optimized pose prediction function s represents the position of the continuum robot along the axis, δ, represent the control functions before and after optimization;

[0031] Based on Calculate the position vector of the continuum robot along the axis to obtain the shape information of the continuum robot.

[0032] Furthermore, in the process of optimizing the preliminary pose prediction function R(s, δ), the objective function is:

[0033] The objective function is:

[0034]

[0035] The formula for optimizing the objective function is:

[0036]

[0037] In the formula, f is a function describing the difference between R δ (s i , δ) and R(s i ), i is the number of each pose measurement point, N is the total number of pose measurement points; trace(·) represents taking the trace of the matrix, δ is the control function of the predicted pose; R(si ) is the node attitude information, R δ (s i , δ) is the predicted node attitude information.

[0038] After adopting the above technical solution, the present invention uses the multi-sensor fusion technology of piezoelectric film (PVDF) sensors and inertial sensors (IMU). The design of multi-sensor fusion avoids the problems of data loss or insufficient accuracy caused by relying on a single sensor, and at the same time enhances the perception ability of complex deformations, providing more comprehensive and stable data support for the shape reconstruction of continuum robots. The present invention also combines the CNN neural network to establish a non-linear mapping relationship between curvature and pressure, effectively improving the accuracy of shape reconstruction. At the same time, the Cosserat Rod theory is used to constrain the axial deformation of the robot, solving the problem of shape distortion caused by cumulative errors in traditional technologies, ensuring the shape perception accuracy of the robot in complex deformation scenarios, and further improving the shape reconstruction accuracy. Description of the Drawings

[0039] Figure 1 is the flow chart of the shape perception method of the continuum robot based on the fusion of IMU and PVDF sensors of the present invention;

[0040] Figure 2 is the framework diagram of the shape perception of the continuum robot based on the fusion of IMU and PVDF sensors of the present invention. Detailed Embodiments

[0041] In order to make the content of the present invention easier to be clearly understood, the following further details the present invention according to specific embodiments and in conjunction with the drawings.

[0042] As Figure 1 and Figure 2 shown, a shape perception method of a continuum robot based on the fusion of IMU and PVDF sensors includes:

[0043] Step S1, obtaining the node attitude information of the continuum robot based on the measurement data of the inertial sensor;

[0044] Step S2, obtaining the node pressure information of the continuum robot through the piezoelectric film sensor, and inputting the node pressure information into a pre-trained convolutional neural network model to obtain the node curvature information;

[0045] Step S3, fitting the continuous curvature function of the continuum robot based on the node curvature information to obtain the continuous curvature information;

[0046] Step S4, using the node attitude information and the continuous curvature information, and combining the Cosserat Rod theory, to obtain the shape information of the continuum robot.

[0047] In this embodiment, the IMU may include a micro control unit and a sensor. The model of the sensor may be JY60. JY60 includes a three-axis accelerometer and a three-axis gyroscope. JY60 collects the X, Y, and Z axis accelerations a i =[a ix ,a iy ,a iz and the angular velocity ω i =[v ix ,ω iy ,ω iz . These data reflect the dynamic motion state of the node, while the attitude matrix of the node describes the rotation state of the node in space. To extract accurate attitude information from the sensor data, it is necessary to use the Extended Kalman Filter (EKF) method to fuse and process the sensor data. Based on this, in step S1, the attitude information of the continuum robot node is obtained based on the measurement data of the inertial sensor; specifically, it includes:

[0048] According to the angular velocity at the k-th moment and the attitude matrix at the k-th moment predict the attitude matrix at the (k + 1)-th moment

[0049]

[0050] Use the acceleration at the k-th moment and the error e i between the gravity direction [0, 0, -g] to fuse the Kalman filter to correct the predicted attitude matrix to obtain the attitude information at the (k + 1)-th moment

[0051] where, is the skew-symmetric matrix of the angular velocity , Δt is the time step, and exp(·) is the matrix exponential map; the angular velocity and the acceleration are both measured by the inertial sensor; is the rotation matrix at the k-th moment, representing the attitude of the carrier at the k-th moment.

[0052] where, use the acceleration at the k-th moment and the error e i between the gravity direction [0, 0, -g] to correct the predicted attitude matrix to obtain the attitude information at the (k + 1)-th moment Specifically, it is:

[0053] Calculate the acceleration The error e with respect to the gravity direction [0, 0, -g] i ; The formula is as follows:

[0054] Solve for the Kalman gain K k ; The formula is as follows: Combine the error e i with the gain K k to generate a rotational increment ΔR, and multiply it with the attitude matrix to obtain the corrected attitude information The formula is: ΔR = exp(K k e i ),

[0055] where P k|k-1 is the covariance matrix in the prediction stage, H k is the Jacobian matrix of the observation model, R is the noise covariance, and the update formula for the covariance matrix is P k = (I - K k H k )P k∣k-1 , and P k is the covariance matrix in the update stage.

[0056] In this embodiment, piezoelectric film (PVDF) sensors are installed at key positions of each continuum robot node. The PVDF is closely attached to the flexible structure of the robot to detect the stress state of the node in real time. Among them, the PVDF sensor works based on the piezoelectric effect. When the node bends or is stressed, the PVDF outputs a charge signal proportional to the stress. The signal is amplified by a charge amplifier circuit and converted into a processable digital signal by an analog-to-digital converter (ADC). Set the sampling frequency of the PVDF sensor to collect and store the pressure information of the node in real time. The pressure signal P j obtained from the piezoelectric film (PVDF) sensor directly reflects the state of each node of the continuum robot under stress conditions, while the node curvature value κ j is an important parameter describing the local shape of the continuum robot. There is a complex non-linear mapping relationship between the two, and it is difficult to directly solve using traditional analytical methods. Therefore, a data-driven approach is adopted to learn the mapping relationship between pressure and curvature through a neural network. That is, a large number of node pressure signals P = [P 1 , P 2 ,..., P n and their corresponding true curvature values κ = [κ 1 , κ 2 ,..., κ n are collected when the continuum robot is in a known curvature state to construct a dataset and train a convolutional neural network model.

[0057] During the process of training the convolutional neural network model, the mean squared error MSE is adopted as the loss function:

[0058]

[0059] where n is the number of curvature measurement points, is the predicted output of the model, and λ is the model parameter.

[0060] During online operation, the pressure signal P obtained in real time j is directly input into the trained convolutional neural network model, and the curvature value of the node is calculated:

[0061]

[0062] where λ * is the model parameter after training and optimization.

[0063] In this way, the accurate mapping between the node pressure information and the node curvature change is completed.

[0064] In this embodiment, in step S3, during the process of fitting the continuous curvature function of the continuum robot, the constraint condition is:

[0065] |κ xδ (s j ,η)-κ x (s j )|≤ε xj

[0066] |κ yδ (s j ,η)-κ y (s j )|≤ε xj

[0067] In the formula, κ x (s j ) and κ y (s j ) are the curvature information predicted by the convolutional neural network model at the continuum robot rod length s j ; j is the number of each curvature measurement point; κ xδ (s j ,η) and κ yδ (s j ,η) are the curvature information obtained based on the continuous curvature function at the continuum robot rod length s j ; η is the parameter of the continuous curvature function; ε xj and ε yj are the allowable values of the curvature error.

[0068] Specifically, considering the installation error of the PVDF sensor, the constraint on the curvature is appropriately relaxed to avoid overfitting during optimization.

[0069] Among them, the continuous curvature function can be a multi-order polynomial. Curvature data in various bending states can be collected. Try third-order polynomials, fourth-order polynomials,..., and determine the optimal order through the error of the validation set.

[0070] Of course, the continuous curvature function can also be other types of functions.

[0071] In this embodiment, in step S4, using the attitude information and continuous curvature information of the nodes, combined with the Cosserat Rod theory to obtain the shape information of the continuum robot; specifically including:

[0072] Discretize the continuum robot along the axis into M position points, and the m-th position point s m = mΔs, m = 0, 1,... M, where Δs represents the step size;

[0073] Integrate the continuous curvature information between adjacent position points, calculate the attitude matrix at each position point, and use the interpolation method to obtain the preliminary attitude prediction function R(s, δ) of the continuum robot along the axis;

[0074] Taking the difference between the attitude information of the nodes and the preliminary attitude at the nodes calculated based on the preliminary attitude prediction function R(s, δ) as a constraint, optimize the preliminary attitude prediction function R(s, δ) to obtain the optimized attitude prediction function s represents the position of the continuum robot along the axis, and δ, represent the control functions before and after optimization;

[0075] Based on Calculate the position vector of the continuum robot along the axis to obtain the shape information of the continuum robot.

[0076] Specifically, the shape information of the continuum robot is the position vector of each point of the continuum robot along the axis. Based on the Cosserat Rod theory, the derivative of the position vector r(s) is determined by the attitude matrix R(s) and the linear strain e(s): Considering the characteristics of the soft continuum robot such as flexible structure and large aspect ratio, the influence of the linear strain on the shape of the robot is ignored, that is, it can be considered that e(s) = [0 0 1] T , so it is simplified to:

[0077] In this embodiment, in the process of optimizing the preliminary attitude prediction function R(s, δ) in step S4, the objective function is:

[0078]

[0079] The formula for optimizing the objective function is as follows:

[0080]

[0081] In the formula, f is a function describing the difference between R δ (s i , δ) and R(s i ); i is the number of each attitude measurement point, and N is the total number of attitude measurement points; trace(·) represents finding the trace of a matrix, and δ is the control function of the predicted attitude; R(s i ) is the node attitude information, and R δ (s i , δ) is the predicted node attitude information.

[0082] Taking the ideal embodiments of the present invention described above as inspiration, through the above description, relevant staff can completely make various changes and modifications without departing from the technical idea of this invention. The technical scope of this invention is not limited to the content in the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A continuum robot shape perception method based on IMU and PVDF sensor fusion, characterized in that: include: Obtaining node posture information of the continuum robot based on the measurement data of the inertial sensor; The node pressure information of the continuum robot is obtained through a piezoelectric film sensor, and the node pressure information is input into a pre-trained convolutional neural network model to obtain the node curvature information; Based on the node curvature information, the continuous curvature function of the continuum robot is fitted to obtain the continuous curvature information; The shape information of the continuum robot is obtained by using the node posture information and continuous curvature information combined with the Cosserat Rod theory.

2. The shape perception method of a continuum robot based on IMU and PVDF sensor fusion according to claim 1, characterized in that: The posture information of the continuum robot node is obtained based on the measurement data of the inertial sensor; specifically including: According to the angular velocity at the kth moment and the posture matrix at the kth moment Predict the posture matrix at the k+1th moment Using the acceleration a at the kth moment i k The error e between the gravity direction [0,0,-g] i , the attitude matrix predicted by fusion Kalman filter correction Get the posture information at the k+1th moment in, is the angular velocity The antisymmetric matrix of , Δt is the time step, exp(·) is the exponential mapping of the matrix; the angular velocity and acceleration a i k All are measured by inertial sensors; is the rotation matrix at the kth moment, representing the posture of the carrier at the kth moment.

3. The shape perception method of a continuum robot based on IMU and PVDF sensor fusion according to claim 2, characterized in that: Using the acceleration a at the kth moment i k The error e between the gravity direction [0,0,-g] i Correct the predicted pose matrix Get the posture information at the k+1th moment Specifically: Calculate the acceleration a i k The error e between the gravity direction [0,0,-g] i ; The formula is: e i =a i k -R i [0,0,-g] T ; Solving for the Kalman gain K k ; The formula is: The error e i With gain K k Combined to generate the rotation increment ΔR, and combined with the attitude matrix Multiply to get the corrected attitude information The formula is: ΔR = exp(K k e i ), Where P k|k-1 is the covariance matrix of the prediction stage, H k is the Jacobian matrix of the observation model, R is the noise covariance, and the update formula of the covariance matrix is ​​P k =(IK k H k ) k∣k-1 , P k is the covariance matrix of the update phase.

4. The shape perception method of a continuum robot based on IMU and PVDF sensor fusion according to claim 1, characterized in that: The continuum robot is in a known curvature state and collects a large number of node pressure signals P = [P1, P2, ..., P n ] and its corresponding true curvature value κ=[κ1,κ2,...,κ n ], build a data set and train a convolutional neural network model; In the process of training the convolutional neural network model, the loss function uses the mean square error MSE: Where n is the number of curvature measurement points, is the predicted output of the model and λ is the model parameter.

5. The shape perception method of a continuum robot based on the fusion of IMU and PVDF sensors according to claim 1, characterized in that: In the process of fitting the continuous curvature function of the continuum robot, the constraints are: |k xδ (s j ,h)-k x (s j )|≤e xj |k yδ (s j ,h)-k y (s j )|≤e xj In the formula, κ x (s j ) and κ y (s j ) is the length of the continuum robot rod s j , the curvature information predicted by the convolutional neural network model; j is the number of each curvature measurement point; κ xδ (s j ,η) and κ yδ (s j ,η) is the length of the continuum robot rod s j , the curvature information obtained based on the continuous curvature function; η is the parameter of the continuous curvature function; ε xj and ε yj is the allowable value of curvature error.

6. The shape perception method of a continuum robot based on IMU and PVDF sensor fusion according to claim 1, characterized in that: The shape information of the continuum robot is obtained by using the posture information and continuous curvature information of the nodes and combining the Cosserat Rod theory; specifically, it includes: The continuum robot is discretized into M positions along the axis. The mth position s m =mΔs, m = 0, 1, ..., M, Δs represents the step size; The continuous curvature information between adjacent position points is integrated to calculate the posture matrix at each position point, and the preliminary prediction function R(s,δ) of the continuum robot along the axis is obtained by interpolation method; Taking the difference between the posture information of the node and the preliminary posture of the node calculated based on the preliminary posture prediction function R(s,δ) as a constraint, the preliminary posture prediction function R(s,δ) is optimized to obtain the optimized posture prediction function s represents the axial position of the continuum robot, δ, Represents the control function before and after optimization; based on The position vector of the continuum robot in the axial direction is calculated to obtain the shape information of the continuum robot.

7. The shape perception method of a continuum robot based on IMU and PVDF sensor fusion according to claim 6, characterized in that: In the process of optimizing the initial posture prediction function R(s,δ), the objective function is: The formula for optimizing the objective function is: Where f is the value describing R δ (s i ,δ) and R(s i ), i is the number of each attitude measurement point, N is the total number of attitude measurement points; trace(·) means to find the trace of the matrix; R(s i ) is the node posture information, R δ (s i ,δ) is the predicted node posture information.

Images