Pose solving method and device for rope-driven continuum robot

Abstract

The application provides a pose solving method and device for a rope-driven continuum robot, the method comprising: obtaining the displacement of the driving ropes of the continuum robot, the continuum robot comprising a plurality of continua and a plurality of driving ropes corresponding to each continuum; determining the joint pose of the continuum robot based on the displacement of the driving ropes and a segmented constant-curvature geometric model; predicting the pose error of the continuum robot based on the displacement of the driving ropes and a pose error compensation model; determining the hybrid pose of the continuum robot based on the joint pose and the pose error; and the pose error compensation model is obtained by training an initial pose error compensation model based on sample driving rope displacement. The method provided by the application combines the joint pose and the pose error to obtain a reliable and accurate hybrid pose, realizes accurate forward kinematics modeling of the rope-driven continuum robot, and improves the accuracy and reliability of the pose solving of the rope-driven continuum robot.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot technology, and in particular to a method and apparatus for solving the pose of a rope-driven continuum robot. Background Technology

[0002] Compared to traditional rigid robots, continuum robots are made of soft materials and can freely change their shape and structure, offering broad application prospects. For example, they can be used for exploration tasks such as space exploration and seabed exploration. Their flexibility and adaptability allow them to move freely in different terrains and environments, performing tasks such as detection, sampling, and repair, providing technical support for human exploration of the unknown.

[0003] However, continuum robots face a series of challenges and difficulties in kinematic modeling. Due to their soft and deformable nature, the shape and structure of continuum robots are highly variable, determined by the combined effects of the internal material properties and the external environment. Therefore, traditional rigid body kinematic models are insufficient to describe the kinematics of continuum robots. Although researchers have proposed assumptions such as constant curvature, piecewise constant curvature, and variable curvature to simplify the modeling problem, the accuracy of related kinematic models is generally low because these assumptions neglect factors such as material stiffness, viscoelasticity, friction, and external forces. Summary of the Invention

[0004] This invention provides a method and apparatus for solving the pose of a rope-driven continuum robot, which addresses the shortcomings of the generally low accuracy of kinematic models of rope-driven continuum robots in the prior art.

[0005] This invention provides a method for solving the pose of a rope-driven continuum robot, comprising:

[0006] The displacement of the drive rope of a continuous robot is obtained. The continuous robot includes multiple continuous bodies and multiple drive ropes corresponding to each continuous body.

[0007] Based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model, the joint pose of the continuum robot is determined.

[0008] Based on the displacement of the driving ropes and the pose error compensation model of the multiple driving ropes corresponding to each continuum, the pose error of the continuum robot is predicted.

[0009] Based on the joint poses of the continuum robot and the pose error, the hybrid pose of the continuum robot is determined.

[0010] The pose error compensation model is obtained by training the initial pose error compensation model based on the sample driving rope displacement.

[0011] According to the present invention, a method for solving the pose of a rope-driven continuum robot, wherein determining the joint pose of the continuum robot based on the displacement of the driving ropes of multiple driving ropes corresponding to each continuum and a segmented constant curvature geometric model includes:

[0012] Based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model, the arc parameters of each continuum are determined.

[0013] Based on the arc parameters of each continuum and the piecewise constant curvature geometric model, the joint pose of the continuum robot is determined;

[0014] The arc parameters of each continuum include the radian angle and the deflection angle of the continuum.

[0015] According to a method for solving the pose of a cable-driven continuum robot provided by the present invention, the method for determining the joint pose of the continuum robot based on the arc parameters of each continuum and the piecewise constant curvature geometric model includes:

[0016] Based on the arc parameters of each continuum, the centerline length of each continuum, and the piecewise constant curvature geometric model, the transformation matrix from the first joint to the last joint of each continuum is determined.

[0017] Based on the transformation matrix from the head joint to the tail joint of each continuum, the joint pose of each continuum is determined to obtain the joint pose of the continuum robot.

[0018] According to the present invention, a method for solving the pose of a rope-driven continuum robot includes the following training steps for the pose error compensation model:

[0019] Obtain the displacement of the sample driving rope and the pose error label corresponding to the displacement of the sample driving rope;

[0020] Based on the sample driving rope displacement and the initial pose error compensation model, the sample pose error is predicted. Based on the sample pose error and the pose error label corresponding to the sample driving rope displacement, the initial pose error compensation model is iterated to obtain the final pose error compensation model.

[0021] The pose error label is obtained based on the sample driving rope displacement and the actual pose corresponding to the sample driving rope displacement.

[0022] According to the method for solving the pose of a rope-driven continuum robot provided by the present invention, the step of obtaining the pose error label corresponding to the sample driving rope displacement includes:

[0023] Based on the sample driving rope displacement and the segmented constant curvature geometric model, the predicted joint pose of the sample driving rope displacement is determined;

[0024] Based on the predicted joint pose and the actual pose corresponding to the sample drive rope displacement, the pose error label corresponding to the sample drive rope displacement is obtained.

[0025] According to the present invention, a method for solving the pose of a rope-driven continuum robot includes the following steps for obtaining the actual pose corresponding to the sample driving rope displacement:

[0026] Obtain the visual joint poses of the first and last joints of each continuum in the visual coordinate system.

[0027] The visual joint pose is transformed to obtain the actual pose in the continuum robot coordinate system.

[0028] The present invention also provides a pose solving device for a rope-driven continuum robot, comprising:

[0029] The acquisition unit acquires the displacement of the drive rope of the continuous robot, which includes multiple continuous bodies and multiple drive ropes corresponding to each continuous body.

[0030] The geometric calculation unit determines the joint pose of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model.

[0031] The pose error compensation unit predicts the pose error of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the pose error compensation model.

[0032] The pose solving unit determines the hybrid pose of the continuum robot based on the joint poses and the pose error.

[0033] The pose error compensation model is obtained by training the initial pose error compensation model based on the sample driving rope displacement.

[0034] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the pose solving method for the rope-driven continuum robot as described above.

[0035] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the pose solving method for a rope-driven continuum robot as described above.

[0036] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the pose solving method for a rope-driven continuum robot as described above.

[0037] The present invention provides a method and apparatus for solving the pose of a rope-driven continuum robot. It calculates the joint pose of the continuum robot under the displacement of the driving rope by using a piecewise constant curvature geometric model, and predicts the pose error of the continuum robot under the displacement of the driving rope by using a pose error compensation model based on machine learning. It combines the joint pose and the pose error to obtain a reliable and accurate hybrid pose, thereby realizing accurate forward kinematic modeling of the rope-driven continuum robot and improving the accuracy and reliability of the pose solution of the rope-driven continuum robot. Attached Figure Description

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

[0039] Figure 1 This is a flowchart illustrating the hybrid positive kinematics modeling method for a continuum robot provided by the present invention.

[0040] Figure 2 This is a schematic diagram of a two-joint rope-driven continuum robot provided by the present invention;

[0041] Figure 3 This is a cross-sectional schematic diagram of each joint provided by the present invention;

[0042] Figure 4 This is a flowchart illustrating the hybrid positive kinematics modeling strategy for a two-joint rope-driven continuum robot provided by the present invention.

[0043] Figure 5 This is a schematic diagram of the pose solving device for a rope-driven continuum robot provided by the present invention.

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

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

[0046] Continuum robots are an emerging robotics technology inspired by the softness and flexibility of living organisms. In the medical field, continuum robots can perform tasks such as endoscopic surgery, drug delivery, and tissue testing, utilizing their softness to perform precise and safe operations inside the human body. In rescue missions, continuum robots can adapt to various complex environments, traverse narrow passages, and perform search, rescue, and reconnaissance tasks, improving rescue efficiency and success rates. Overall, continuum robots, with their soft and deformable characteristics, have demonstrated unique advantages and potential in the fields of medicine, rescue, and exploration.

[0047] However, at the same time, continuum robots face a series of challenges and difficulties in kinematic modeling. Although researchers have proposed assumptions such as constant curvature, piecewise constant curvature, and variable curvature to simplify the modeling problem, the accuracy of related kinematic models is generally low because these assumptions ignore factors such as material stiffness, viscoelasticity, friction, and external forces.

[0048] To address the aforementioned problems, this invention provides a hybrid forward kinematics modeling method for a rope-driven continuum robot, thereby achieving high-precision hybrid forward kinematics modeling for continuum robots. Figure 1 This is a flowchart illustrating the hybrid positive kinematics modeling method for a continuum robot provided by the present invention, as shown below. Figure 1 As shown, the method includes:

[0049] Step 110: Obtain the displacement of the drive rope of the continuous robot, wherein the continuous robot includes multiple continuous bodies and multiple drive ropes corresponding to each continuous body.

[0050] Here, a continuum robot refers to a rope-driven intelligent agent. A continuum robot typically consists of multiple continuums, each driven by multiple driving ropes. For example, Figure 2 This is a schematic diagram of the two-joint rope-driven continuum robot provided by the present invention, as shown below. Figure 2As shown, the continuum robot comprises two continuums (continuum section 1 and continuum section 2). Continuum section 1 includes a base control ring joint and joint 1, while continuum section 2 includes joint 2. Each continuum section contains nine links and eight four-way joints, ensuring high flexibility for the robot. Each continuum section is driven by three drive ropes. Each link is equipped with a control ring to ensure that the drive ropes remain in close contact with the continuum. Furthermore, the drive ropes are fixed to the joint control rings, which ensure the control of the continuum by the drive ropes. Additionally, it is assumed that each continuum section of the continuum robot is a circular arc with uniform curvature, and the radian angles of continuum section 1 and continuum section 2 in the figure are θ1 and θ2, respectively.

[0051] Figure 3 This is a cross-sectional schematic diagram of each joint provided by the present invention, such as... Figure 3 As shown, the coordinate system of the base control loop is O0(X0,Y0), the coordinate system of the joint 1 control loop is O1(X1,Y1), and the coordinate system of the joint 2 control loop is O2(X2,Y2). It is understandable that, since the drive rope of the control continuum section 2 needs to pass through the continuum section 1, the drive rope holes 1 in the base control loop and the joint 1 control loop... ij Contains 6, for drive rope holes l ij Let 'i' represent the i-th segment of the continuum; 'j' represent the j-th driving rope on segment i. Additionally, the centerline of the i-th segment of the continuum lies at x. i-1 -y i-1 Projection of the plane and x i-1 The included angle of the axes represents the deflection angle of the i-th segment of the continuum, i.e. Figure 3 In Specifically, the displacement of each drive rope on each continuous body in the continuous robot can be obtained by displacement sensors installed on each drive rope. Here, the displacement of the drive rope can refer to the amount of displacement change.

[0052] Step 120: Based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model, determine the joint pose of the continuum robot.

[0053] Here, the piecewise constant curvature geometry model refers to a model based on dividing a continuum robot into multiple continuum segments, assuming that each segment is a circular arc with uniform curvature, and that the centerline length of each segment remains constant. The piecewise constant curvature geometry model can be used to describe the shape and pose of each continuum segment based on the displacement of the drive rope.

[0054] Specifically, the displacement of the driving ropes corresponding to each continuum can be input into the piecewise constant curvature geometric model. The piecewise constant curvature geometric model then outputs the joint poses of each joint in each continuum of the continuum robot, thus obtaining the joint poses of the continuum robot. Here, joint pose can refer to the position coordinates of the joints.

[0055] It should be noted that, based on the piecewise constant curvature kinematic geometry model, a preliminary kinematic modeling of a cable-driven continuum robot is achieved. By constructing the relationship between the displacement of the driving cable and the geometric position of each joint, the piecewise constant curvature geometric model can be obtained. This allows for the preliminary calculation of the joint poses of each continuum body in the continuum robot based on the displacement of the driving cable and the piecewise constant curvature geometric model. Therefore, the piecewise constant curvature geometric model obtained based on kinematic modeling ensures the interpretability of the joint poses based on kinematic theory, thereby guaranteeing the reliability of the continuum robot's pose.

[0056] Understandably, because these assumptions neglect factors such as material stiffness, viscoelasticity, friction, and external forces, the accuracy of the relevant kinematic models is generally low. Therefore, step 130 can be performed to obtain the pose error of the continuum robot under the condition of the driving rope displacement. The specific steps are as follows:

[0057] Step 130: Based on the displacement of the driving ropes and the pose error compensation model of the multiple driving ropes corresponding to each continuum, predict the pose error of the continuum robot; the pose error compensation model is obtained by training the initial pose error compensation model based on the sample driving rope displacement.

[0058] Here, the pose error compensation model can be derived from a machine learning model based on a solvable regression problem, including but not limited to multilayer perceptrons, extreme learning machines, higher-order linear models, radial basis functions, and Gaussian mixture models. Therefore, by constructing a training set and a test set using sample driving rope displacements, the initial pose error compensation model is trained using the training set, allowing it to learn the mapping relationship between the driving rope displacements and the pose errors of each joint, thus obtaining the pose error compensation model. Furthermore, the trained pose error compensation model is tested using the test set to obtain the final pose error compensation model.

[0059] Specifically, the pose error of each joint in each continuum robot can be predicted by inputting the displacement of the driving ropes of multiple driving ropes corresponding to each continuum into the pose error compensation model.

[0060] It should be noted that the pose error of each joint is predicted by the pose error compensation model based on machine learning, which makes up for the deficiency of the low accuracy of the predicted joint position caused by the piecewise constant curvature geometric model in step 120, which ignores factors such as material stiffness, viscoelasticity, friction and external force, thereby improving the accuracy of the obtained pose.

[0061] Step 140: Determine the hybrid pose of the continuum robot based on the joint poses and the pose error.

[0062] Specifically, the joint poses of each joint in each continuum of the continuum robot and the corresponding joint errors can be added together to obtain the mixed pose of each joint in each continuum of the continuum robot.

[0063] It is important to emphasize that, due to the poor interpretability of machine learning models, relying solely on them to predict joint poses based on the displacement of the driving ropes makes it even more difficult to guarantee the accuracy of the predicted poses. In contrast, joint poses calculated using a piecewise constant curvature geometric model based on kinematics are more reliable. Therefore, by combining the pose error prediction from a machine learning pose error compensation model with a joint pose with only minor errors, adjustments can be made to the joint poses to obtain a hybrid pose that is both reliable and accurate.

[0064] The method provided in this invention calculates the joint pose of a continuum robot under the displacement of the driving rope using a piecewise constant curvature geometric model, predicts the pose error of the continuum robot under the displacement of the driving rope using a pose error compensation model based on machine learning, and obtains a reliable and accurate hybrid pose by combining the joint pose and the pose error. This achieves accurate forward kinematic modeling of the rope-driven continuum robot and improves the accuracy and reliability of pose solving for the rope-driven continuum robot.

[0065] Based on any of the above embodiments, step 120 includes:

[0066] Based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model, the arc parameters of each continuum are determined.

[0067] Based on the arc parameters of each continuum and the piecewise constant curvature geometric model, the joint pose of the continuum robot is determined;

[0068] The arc parameters of each continuum include the radian angle and the deflection angle of the continuum.

[0069] Here, the arc parameter can refer to the radian angle and deflection angle of the continuum. As mentioned above. Figure 2As shown, θ1 and θ2 represent the radian angles of continuum part 1 and continuum part 2, respectively. (As mentioned above...) Figure 3 As shown, These represent the deflection angles of continuum part 1 and continuum part 2, respectively. It can be understood that the radian angle reflects the degree of curvature of each continuum in a continuum robot, and the deflection angle reflects the degree of deflection of each continuum. Therefore, by considering the curvature and deflection of each continuum, the pose information of the tail joints of the continuum can be calculated to obtain the joint pose of the continuum robot.

[0070] Specifically, firstly, based on the displacement of the multiple drive ropes corresponding to each continuum and the geometric relationship between the drive rope displacement and the arc parameters, the arc parameters of each continuum are calculated using a piecewise constant curvature geometric model. It should be noted that for a multi-continuum robot, the drive rope corresponding to the first continuum segment can include drive ropes controlling both the first and second continuum segments. Therefore, when the second continuum segment deforms, it must also passively adapt to this deformation, ensuring it always maintains preload on the control loop of joint 2. Thus, the displacement of the drive rope in the second continuum segment can include both the spontaneous active displacement of the second continuum segment and the passive displacement adapting to the first continuum segment.

[0071] Finally, based on the arc parameters of each continuum and the geometric relationship between the arc parameters and the pose of each joint, the joint pose of each joint is calculated using a piecewise constant curvature geometric model.

[0072] In one embodiment, the displacement Δl of the driving rope can be derived through the following steps. ij The relationship between the arc parameters (deflection angle and radian angle). Specifically, firstly, to facilitate the representation of the drive rope displacement Δk... ij The relationship between the angle of deflection and the angle of rotation can be expressed as:

[0073]

[0074]

[0075]

[0076] In the formula, This represents the deflection angle of the first driving rope in the i-th segment of the continuum; This indicates that the centerline of the i-th segment of the continuum lies at x. i-1 -y i-1 Projection of the plane and x i-1 The included angle of the axis; This represents the deflection angle of the second driving rope in the i-th segment of the continuum; This represents the deflection angle of the third driving rope in the i-th segment of the continuum.

[0077] Therefore, the Δl of the first segment of the continuum is derived. 1j and The relationship between them is:

[0078]

[0079] In the formula, Δl 1j The displacement of the j driving ropes in the first continuous segment is represented by l1; the centerline length of the first continuous segment is represented by l1; and the radian angle of the first continuous segment is represented by θ1.

[0080] Because the drive ropes controlling the second continuous section must pass through the first continuous section, when the first continuous section deforms, the j drive ropes l in the second continuous section... 2j Similarly, passive changes must occur to adapt to this deformation, ensuring that it always maintains a preload force on the joint 2 control ring of the second continuous segment. Therefore, it can be determined that Δl 2j It consists of two parts; one part is used to actively control the deformation of the second section of the continuum. One part is used to passively adapt to the deformation of the first segment of the continuum. Based on the above description, Δl 2j It can be represented in the following form:

[0081]

[0082]

[0083]

[0084] In the formula, θ2 represents the radian angle of the second segment of the continuum; This represents the deflection angle of the drive rope controlling the second segment of the continuum on the control loop of joint 1, where j = 1, 2, 3. For ease of description, it can be represented by Δl. 2j Default represents Here Specifically, it can be expressed as follows:

[0085]

[0086]

[0087]

[0088] In the formula, These represent the deflection angles of the drive ropes 1, 2, and 3 controlling the second continuous segment on the control ring of joint 1, respectively.

[0089] Further derivation yields the arc parameters. θi respectively with Δl ij The relationship between them is:

[0090]

[0091]

[0092] In the formula, Δl represents the deflection angle of the i-th segment of the continuum. i1 Δl i2 Δl i3 θ represents the displacement of the 1st, 2nd, and 3rd driving ropes of the i-th segment of the continuum; i Δl represents the radian angle of the i-th segment of the continuum. ij Δl represents the displacement of the j-th driving rope of the i-th segment of the continuum, where j = 1, 2; ik Let represent the displacement of the k-th driving rope of the i-th segment of the continuum, where k = j + 1, k = 2, 3.

[0093] The method provided in this invention derives the relationship between the displacement of the driving rope and the arc parameters of each continuum step by step through a segmented constant curvature geometric model constructed based on segmented constant curvature and geometric kinematics. Then, based on the relationship between the arc parameters and the joint pose of the continuum, the joint pose of the continuum is calculated. This realizes the derivation of the relationship between the displacement of the driving rope and the joint pose, ensuring the interpretability of the calculated joint pose, thereby improving the reliability and accuracy of the joint pose.

[0094] Based on any of the above embodiments, determining the joint pose of the continuum robot based on the arc parameters of each continuum and the piecewise constant curvature geometric model includes:

[0095] Based on the arc parameters of each continuum, the centerline length of each continuum, and the piecewise constant curvature geometric model, the transformation matrix from the first joint to the last joint of each continuum is determined.

[0096] Based on the transformation matrix from the head joint to the tail joint of each continuum, the joint pose of each continuum is determined to obtain the joint pose of the continuum robot.

[0097] Here, in Figure 3 The first joint of the continuum 1 can be a base control ring, and the first joint of the continuum 2 can be a joint 1 control ring; the last joint of the continuum 1 can be a joint 1 control ring, and the last joint of the continuum 2 can be a joint 2 control ring.

[0098] Specifically, firstly, the transformation matrix from the first joint to the last joint of each continuum can be derived based on the arc parameters and centerline length of each continuum. The specific derivation formula is as follows:

[0099]

[0100]

[0101] In the formula, T1 represents the transformation matrix from the head joint to the tail joint of the first continuum, for example, it can represent... Figure 3 The transformation matrix from the base control loop to the joint 1 control loop; c and s are abbreviations for cosine and sinine, respectively; T2 represents the transformation matrix from the head joint to the tail joint of the second continuum, for example, it can represent... Figure 3 The transformation matrix from joint 1 control loop to joint 2 control loop.

[0102] Furthermore, by combining the transformation matrices from the first joint to the last joint of each continuum, the joint poses of each continuum can be obtained. Therefore, the joint pose of the end joint of the continuum robot can be used as the joint pose of the continuum robot. For example, the transformation matrix from the first joint to the last joint of a two-joint continuum robot can be directly obtained using the following formula:

[0103] T = T1·T2

[0104] In the formula, T represents the transformation matrix from the first joint to the last joint of a two-joint continuum robot. It can be understood that if there is an i-joint continuum robot, the complete transformation matrix from the first joint to the last joint of the continuum robot can be obtained by multiplying the transformation matrices from the first joint to the last joint of the i continuum robots.

[0105] Based on any of the above embodiments, the training steps of the pose error compensation model include:

[0106] Obtain the displacement of the sample driving rope and the pose error label corresponding to the displacement of the sample driving rope;

[0107] Based on the sample driving rope displacement and the initial pose error compensation model, the sample pose error is predicted. Based on the sample pose error and the pose error label corresponding to the sample driving rope displacement, the initial pose error compensation model is iterated to obtain the final pose error compensation model.

[0108] The pose error label is obtained based on the sample driving rope displacement and the actual pose corresponding to the sample driving rope displacement.

[0109] Here, obtaining the sample drive rope displacement can be used to construct the sample dataset. The pose error label here can be obtained by calculating the predicted joint pose under the sample drive rope displacement, and then based on the difference between the predicted joint pose and the actual measured pose. Thus, the sample label corresponding to each sample data in the dataset can be obtained. Furthermore, the initial pose error compensation model here refers to the original model that has not been trained on the sample dataset.

[0110] Specifically, in the data acquisition phase, the displacement of the sample drive rope can be obtained using a displacement sensor, and the actual pose of each joint under the sample drive rope displacement can be acquired using an image acquisition device. This data acquisition step is repeated to obtain sufficient sample drive rope displacements and corresponding actual poses. Then, the predicted joint pose under the sample drive rope displacements is calculated. Next, the pose error label is obtained by calculating the pose difference between the predicted joint pose under the sample drive rope displacements and the actual pose under the sample drive rope displacements.

[0111] During the training phase, the sample driving rope displacement can be input into the initial pose error compensation model, which then predicts the sample pose error corresponding to the sample driving rope displacement. Next, the prediction loss between the sample pose error and the pose error label corresponding to the sample driving rope displacement is calculated. This prediction loss is then used to iterate the initial pose error compensation model, adjusting its parameters to obtain the final pose error compensation model.

[0112] In one embodiment, a Gaussian mixture model can be selected as the initial pose error compensation model. It should be noted that a Gaussian mixture model is a probability distribution model, assuming the data is composed of several Gaussian distributions. These Gaussian distributions are called components, and each component has its own mean and covariance matrix. The probability density function of the Gaussian mixture model can be expressed as a linear combination of multiple Gaussian distributions, where the weight of each component represents its contribution to the overall distribution. Therefore, we can let the sample driving rope displacement be X, and the sample pose error be Y, to construct a Gaussian mixture distribution model about (X, Y). Assume this distribution is composed of K Gaussian distributions, i.e.:

[0113]

[0114]

[0115] In the formula, P(X,Y) represents a Gaussian distribution with respect to the conditional sample driving rope displacement X and the sample pose error Y; π k μ represents the weight of the k-th Gaussian distribution; kLet ∑ represent the mean vector of the k-th Gaussian distribution with respect to (X,Y); k Let represent the covariance matrix of the k-th Gaussian distribution with respect to (X,Y).

[0116] Next, in order to calculate the joint pose error Y from the joint probability distribution of the Gaussian mixture model, μ can be... k and ∑ k Write it in the following form:

[0117]

[0118]

[0119] In the formula, Let X represent the mean vector of X in the k-th Gaussian distribution; Let Y be the mean vector of Y in the k-th Gaussian distribution. Let X represent the covariance matrix of X in the k-th Gaussian distribution; Let X represent the covariance matrix of X and Y in the k-th Gaussian distribution; yes transpose; Let Y represent the covariance matrix of Y in the k-th Gaussian distribution.

[0120] Therefore, given X, Y can be described by a conditional probability distribution:

[0121]

[0122] In the formula, P(Y|X) represents the probability distribution of the sample pose error Y under the condition of sample driving rope displacement X; β k (X) represents the probability of X occurring in the k-th Gaussian distribution; Let Y be the mean vector of Y given X in the k-th Gaussian distribution. Let Y represent the covariance matrix of Y given X in the k-th Gaussian distribution.

[0123] Where, β k (X) can be expressed by the following formula:

[0124]

[0125] In the formula, π j This represents the weight of the j-th Gaussian distribution; Let X represent the mean vector of the j-th Gaussian distribution with respect to X; Let X represent the mean squared error matrix of the j-th Gaussian distribution with respect to X.

[0126] Finally, after obtaining the probability distribution P(Y|X) of the sample pose error Y under the condition of the sample driving rope displacement X, the expectation of Y under the given condition of X can be further obtained, as shown in the following formula:

[0127]

[0128]

[0129] In the formula, This represents the expectation of Y given X. Let Y represent the expected value of the mean vector of Y in the k-th Gaussian distribution, given X.

[0130] Therefore, the displacement of the sample driving rope can be input into the initial pose error compensation model based on the Gaussian mixture model, and the pose error of the sample can be predicted by the Gaussian mixture model.

[0131] Furthermore, after predicting the sample pose error, the initial pose error compensation model can be iterated using the Expectation-Maximization (EM) algorithm. Specifically, the parameters of the initial pose error compensation model can be optimized through the following two steps: the E-step (Expectation Step) and the M-step (Maximization Step).

[0132] First, the model parameters of the initial pose error compensation model are initialized using the K-means method.

[0133] Secondly, in step E, ε is calculated for each sample. i =(c i ,y i The posterior probability γ of belonging to each component ik That is, the k-th component generates sample ε i The probability of this. This can be obtained using Bayes' theorem:

[0134]

[0135] Then, in the M-step, the model parameters are updated using the posterior probabilities calculated in the E-step. Specifically, the parameter π can be updated based on the weighted maximum likelihood estimation method. k μ k , ∑ k :

[0136]

[0137]

[0138]

[0139] It should be noted that when calculating ∑ k Use the updated μ k Finally, repeat steps E and M until the initial pose error compensation model converges, thus obtaining the final pose error compensation model. Furthermore, the trained pose error compensation model can be tested on a test set to check its performance.

[0140] The method provided in this invention calculates the predicted joint pose corresponding to the sample driving rope displacement, and constructs a pose error label corresponding to the sample driving rope displacement based on the predicted joint pose and the actual pose corresponding to the actual measured sample driving rope displacement. This label is then used for iterative training of the initial pose error compensation model to obtain the final trained pose error compensation model. This ensures the accuracy of the prediction results of the pose error compensation model, thereby improving the accuracy of the predicted pose error in the application stage.

[0141] Based on any of the above embodiments, the steps for obtaining the pose error label corresponding to the displacement of the sample driving rope include:

[0142] Based on the sample driving rope displacement and the segmented constant curvature geometric model, the predicted joint pose of the sample driving rope displacement is determined;

[0143] Based on the predicted joint pose and the actual pose corresponding to the sample drive rope displacement, the pose error label corresponding to the sample drive rope displacement is obtained.

[0144] Specifically, the sample drive rope displacement can be input into a piecewise constant curvature geometric model, and the predicted joint pose under the sample drive rope displacement can be calculated using the piecewise constant curvature geometric model. Then, the pose error label can be obtained by calculating the pose difference between the predicted joint pose under the sample drive rope displacement and the actual pose under the sample drive rope displacement.

[0145] The method provided in this invention predicts the joint pose corresponding to the sample driving rope displacement by using a piecewise constant curvature geometric model. Based on the predicted joint pose and the actual pose corresponding to the actual measured sample driving rope displacement, a pose error label corresponding to the sample driving rope displacement is constructed for iterative training of the initial pose error compensation model. The method cleverly uses a piecewise constant curvature geometric model to obtain the pose error label, thereby improving the efficiency of obtaining the pose error label.

[0146] Based on any of the above embodiments, the step of obtaining the actual pose corresponding to the sample driving rope displacement includes:

[0147] Obtain the visual joint poses of the first and last joints of each continuum in the visual coordinate system.

[0148] The visual joint pose is transformed to obtain the actual pose in the continuum robot coordinate system.

[0149] Specifically, to facilitate the measurement of the pose of each joint of the continuum robot, an ArUco calibration plate can be mounted on each joint control ring. Furthermore, a binocular vision camera is fixed diagonally above the continuum robot to measure the pose of the ArUco calibration plate. Thus, the visual joint pose of each joint can be calculated based on the positional transformation relationship between the ArUco calibration plate and the joint control ring. Next, the continuum robot and the binocular vision camera can be calibrated using hand-eye calibration to transform the visual joint pose from the camera coordinate system to the robot's base coordinate system, obtaining the actual pose in the continuum robot's coordinate system.

[0150] Based on any of the above embodiments Figure 4 This is a flowchart illustrating the hybrid positive kinematics modeling strategy for a two-joint rope-driven continuum robot provided by the present invention, as shown below. Figure 4 As shown, the strategy includes:

[0151] First, the acquired cable-driven displacement is input into the true kinematics model, and the joint pose is output through the positive kinematics model. Specifically, the positive kinematics model is constructed jointly by a piecewise constant curvature geometry model and a joint pose error compensation model. That is, by inputting the cable-driven displacement into the piecewise constant curvature geometry model and the joint pose error compensation model respectively, the pose to be compensated output by the piecewise constant curvature geometry model and the pose error output by the joint pose error compensation model are obtained. The pose to be compensated and the pose error are then added together to obtain the final, highly accurate, and reliable joint pose.

[0152] Based on any of the above embodiments Figure 5 This is a schematic diagram of the pose solving device for a rope-driven continuum robot provided by the present invention, as shown below. Figure 5 As shown, the device includes:

[0153] The acquisition unit 510 acquires the displacement of the drive rope of the continuous robot, wherein the continuous robot includes multiple continuous bodies and multiple drive ropes corresponding to each continuous body.

[0154] The geometric calculation unit 520 determines the joint pose of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model.

[0155] The pose error compensation unit 530 predicts the pose error of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the pose error compensation model.

[0156] The pose solving unit 540 determines the hybrid pose of the continuum robot based on the joint poses and the pose error.

[0157] The pose error compensation model is obtained by training the initial pose error compensation model based on the sample driving rope displacement.

[0158] The apparatus provided in this invention calculates the joint pose of a continuum robot under the displacement of the driving rope using a piecewise constant curvature geometric model, predicts the pose error of the continuum robot under the displacement of the driving rope using a pose error compensation model based on machine learning, and obtains a reliable and accurate hybrid pose by combining the joint pose and the pose error. This achieves accurate forward kinematic modeling of the rope-driven continuum robot and improves the accuracy and reliability of pose solving for the rope-driven continuum robot.

[0159] Based on any of the above embodiments, the geometric calculation unit is specifically used for:

[0160] Based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model, the arc parameters of each continuum are determined.

[0161] Based on the arc parameters of each continuum and the piecewise constant curvature geometric model, the joint pose of the continuum robot is determined;

[0162] The arc parameters of each continuum include the radian angle and the deflection angle of the continuum.

[0163] Based on any of the above embodiments, the geometric calculation unit is further specifically used for:

[0164] Based on the arc parameters of each continuum, the centerline length of each continuum, and the piecewise constant curvature geometric model, the transformation matrix from the first joint to the last joint of each continuum is determined.

[0165] Based on the transformation matrix from the head joint to the tail joint of each continuum, the joint pose of each continuum is determined to obtain the joint pose of the continuum robot.

[0166] Based on any of the above embodiments, the pose solving device for the rope-driven continuum robot further includes a training unit, which is specifically used for:

[0167] Obtain the displacement of the sample driving rope and the pose error label corresponding to the displacement of the sample driving rope;

[0168] Based on the sample driving rope displacement and the initial pose error compensation model, the sample pose error is predicted. Based on the sample pose error and the pose error label corresponding to the sample driving rope displacement, the initial pose error compensation model is iterated to obtain the final pose error compensation model.

[0169] The pose error label is obtained based on the sample driving rope displacement and the actual pose corresponding to the sample driving rope displacement.

[0170] Based on any of the above embodiments, the training unit includes a training data acquisition unit, which is specifically used for:

[0171] Based on the sample driving rope displacement and the segmented constant curvature geometric model, the predicted joint pose of the sample driving rope displacement is determined;

[0172] Based on the predicted joint pose and the actual pose corresponding to the sample drive rope displacement, the pose error label corresponding to the sample drive rope displacement is obtained.

[0173] Based on any of the above embodiments, the training unit further includes a training data acquisition unit, which is specifically used for:

[0174] Obtain the visual joint poses of the first and last joints of each continuum in the visual coordinate system.

[0175] The visual joint pose is transformed to obtain the actual pose in the continuum robot coordinate system.

[0176] Figure 6 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 6As shown, the electronic device may include: a processor 610, a communication interface 620, a memory 630, and a communication bus 640, wherein the processor 610, the communication interface 620, and the memory 630 communicate with each other through the communication bus 640. The processor 610 can call logic instructions in the memory 630 to execute a pose solving method for a rope-driven continuum robot. The method includes: obtaining the displacement of the driving ropes of the continuum robot, wherein the continuum robot includes multiple continuums and multiple driving ropes corresponding to each continuum; determining the joint pose of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and a segmented constant curvature geometric model; predicting the pose error of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and a pose error compensation model; and determining the hybrid pose of the continuum robot based on the joint pose and the pose error. The pose error compensation model is obtained by training an initial pose error compensation model based on sample driving rope displacements.

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

[0178] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the pose solving method for a rope-driven continuum robot provided by the above methods. The method includes: obtaining the displacement of the driving rope of the continuum robot, wherein the continuum robot includes multiple continuums and multiple driving ropes corresponding to each continuum; determining the joint pose of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and a piecewise constant curvature geometric model; predicting the pose error of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and a pose error compensation model; and determining the hybrid pose of the continuum robot based on the joint pose and the pose error. The pose error compensation model is obtained by training an initial pose error compensation model based on sample driving rope displacements.

[0179] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements a method for solving the pose of a rope-driven continuum robot provided by the methods described above. This method includes: obtaining the displacement of the driving ropes of the continuum robot, the continuum robot comprising multiple continuums and multiple driving ropes corresponding to each continuum; determining the joint pose of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and a piecewise constant curvature geometric model; predicting the pose error of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and a pose error compensation model; and determining the hybrid pose of the continuum robot based on the joint pose and the pose error; wherein the pose error compensation model is obtained by training an initial pose error compensation model based on sample driving rope displacements.

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

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

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

Claims

1. A method for solving the pose of a rope-driven continuum robot, characterized in that, include: The displacement of the drive rope of a continuous robot is obtained. The continuous robot includes multiple continuous bodies and multiple drive ropes corresponding to each continuous body. Based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model, the joint pose of the continuum robot is determined. Based on the displacement of the driving ropes and the pose error compensation model of the multiple driving ropes corresponding to each continuum, the pose error of the continuum robot is predicted. Based on the joint poses of the continuum robot and the pose error, the hybrid pose of the continuum robot is determined. The pose error compensation model is obtained by training the initial pose error compensation model based on the sample driving rope displacement. The determination of the joint pose of the continuum robot based on the displacement of the multiple drive ropes corresponding to each continuum and the segmented constant curvature geometric model includes: Based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model, the arc parameters of each continuum are determined. Based on the arc parameters of each continuum and the piecewise constant curvature geometric model, the joint pose of the continuum robot is determined; The arc parameters of each continuum include the radian angle and the deflection angle of the continuum; The determination of the joint pose of the continuum robot based on the arc parameters of each continuum and the piecewise constant curvature geometric model includes: Based on the arc parameters of each continuum, the centerline length of each continuum, and the piecewise constant curvature geometric model, the transformation matrix from the first joint to the last joint of each continuum is determined. Based on the transformation matrix from the first joint to the last joint of each continuum, the joint pose of each continuum is determined to obtain the joint pose of the continuum robot. The training steps of the pose error compensation model include: Obtain the displacement of the sample driving rope and the pose error label corresponding to the displacement of the sample driving rope; Based on the sample driving rope displacement and the initial pose error compensation model, the sample pose error is predicted. Based on the sample pose error and the pose error label corresponding to the sample driving rope displacement, the initial pose error compensation model is iterated to obtain the final pose error compensation model. The pose error label is obtained based on the sample driving rope displacement and the actual pose corresponding to the sample driving rope displacement.

2. The pose calculation method for a rope-driven continuum robot according to claim 1, characterized in that, The steps for obtaining the pose error label corresponding to the displacement of the sample driving rope include: Based on the sample driving rope displacement and the segmented constant curvature geometric model, the predicted joint pose of the sample driving rope displacement is determined; Based on the predicted joint pose and the actual pose corresponding to the sample drive rope displacement, the pose error label corresponding to the sample drive rope displacement is obtained.

3. The pose calculation method for a rope-driven continuum robot according to claim 1, characterized in that, The steps for obtaining the actual pose corresponding to the displacement of the sample driving rope include: Obtain the visual joint poses of the first and last joints of each continuum in the visual coordinate system. The visual joint pose is transformed to obtain the actual pose in the continuum robot coordinate system.

4. A pose-solving device for a rope-driven continuum robot based on the pose-solving method of any one of claims 1 to 3, characterized in that, include: The acquisition unit acquires the displacement of the drive rope of the continuous robot, which includes multiple continuous bodies and multiple drive ropes corresponding to each continuous body. The geometric calculation unit determines the joint pose of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the segmented constant curvature geometric model. The pose error compensation unit predicts the pose error of the continuum robot based on the displacement of the driving ropes of the multiple driving ropes corresponding to each continuum and the pose error compensation model. The pose solving unit determines the hybrid pose of the continuum robot based on the joint poses and the pose error. The pose error compensation model is obtained by training the initial pose error compensation model based on the sample driving rope displacement.

5. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the pose solving method for the rope-driven continuum robot as described in any one of claims 1 to 3.

6. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the pose solving method for the rope-driven continuum robot as described in any one of claims 1 to 3.

7. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the pose solving method for the rope-driven continuum robot as described in any one of claims 1 to 3.

Images