A parameter self-calibration method and device of a rope-driven snake-like manipulator and a storage medium

Abstract

The application discloses a parameter self-calibration method and device of a rope-driven snake-shaped mechanical arm and a storage medium. The method comprises the following steps: a Jacobian matrix mapping method is adopted, a rope is positioned, the joint angle of a rope-driven joint is determined, a D-H coordinate system is established, the rope hole position vector formula of a plurality of rope-driven joints is obtained, a kinematic correction model and a kinematic error model are introduced, a rope length calculation function of a driving rope in the rope-driven joint is obtained, a linear relationship function is obtained according to the rope length calculation function, the linear relationship function is iteratively solved by using the Newton-Raphson method, and the to-be-calibrated parameters are updated. The kinematic correction model, the kinematic error model and the parameter self-calibration method can realize the rapid self-calibration of the kinematic parameters of the rope-driven snake-shaped mechanical arm, improve the motion accuracy of the mechanical arm, and eliminate the influence of errors generated in processing and assembly. The application can be widely applied to the technical field of robot control.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and in particular to a method, apparatus and storage medium for parameter self-calibration of a rope-driven snake-like robotic arm. Background Technology

[0002] Because errors will occur during the processing and assembly of the robotic arm, the actual kinematic parameters of the rope-driven robotic arm will inevitably differ from the design values. Therefore, it is necessary to calibrate the parameters of the rope-driven robotic arm to reduce these errors.

[0003] Traditional calibration methods employ kinematic calibration. Some studies propose kinematic calibration methods based on motion capture systems, using mapping models between geometric structure errors, zero rope length errors, and end effector position errors. Other studies propose kinematic calibration methods for multi-segment flexible robotic arms based on weakened decoupling technology.

[0004] However, most kinematic calibration methods rely on external precision measuring equipment, such as motion capture systems, cameras, laser rangefinders, and inclinometers. These calibration systems are costly to set up, the calibration process is complex, and they are not suitable for cable-driven serpentine robotic arms. Summary of the Invention

[0005] In order to solve at least one of the technical problems existing in the above-mentioned related technologies, this application proposes a parameter self-calibration method, device and storage medium for a rope-driven snake-like robotic arm.

[0006] The first aspect of this application proposes a parameter self-calibration method for a rope-driven serpentine robotic arm, the method comprising:

[0007] The drive ropes of the snake-like robotic arm are classified to obtain positioning ropes and redundant ropes;

[0008] The joint angle of each rope drive joint is determined based on the positioning rope using the Jacobian matrix mapping method.

[0009] A DH coordinate system is established for the multi-segment rope-driven joints of the snake-like robotic arm to obtain the rope hole position vector of each rope-driven joint.

[0010] By introducing a kinematic correction model and a kinematic error model, and based on the rope hole position vector and the joint angle, the rope length calculation function of the driving rope in the rope drive joint is obtained;

[0011] Based on the rope length calculation function, a linear relationship function is obtained; the linear relationship function is used to represent the linear relationship between the length of the redundant rope and the change in the parameter vector to be calibrated.

[0012] The linear relationship function is solved iteratively using the Newton-Raphson method to update the parameters to be calibrated.

[0013] In some embodiments, the step of determining the joint angle of each of the rope drive joints based on the positioning rope using the Jacobian matrix mapping method specifically includes:

[0014] Formula for obtaining the total length of the drive rope;

[0015] Perform a difference operation on the total length formula to obtain the total length-joint angle equation;

[0016] The length of the positioning rope in each segment of the rope drive joint is obtained by a motor encoder. The Newton-Raphson method is used to numerically solve the equation of total length-joint angle to determine the joint angle.

[0017] In some embodiments, the step of introducing a kinematic correction model and a kinematic error model to obtain the rope length calculation function of the drive rope in the rope drive joint based on the rope hole position vector and the joint angle specifically includes:

[0018] Based on the rope hole position vector, the length of the drive rope in the rope drive joint is corrected by the kinematic correction model to obtain the rope length formula;

[0019] Based on the rope length formula and the joint angle, a kinematic error model is introduced to determine the rope length calculation function;

[0020] The rope length calculation function is expressed by the following formula:

[0021]

[0022] Among them, l i，j，k Let be the length of the k-th drive rope in the j-th rope group within the i-th segment of the drive joint. Let q be the function for calculating the rope length of the k-th drive rope in the j-th rope group at the i-th segment of the drive joint. i d represents the joint angle of the i-th chord joint. i r represents the distance between the rope discs at the i-th rope drive joint. i φ represents the radius of the rope at the i-th rope drive joint. i This represents the torsion angle of the i-th chord joint.

[0023] In some embodiments, the step of obtaining the formula for the total length of the drive rope is specifically expressed by the following formula:

[0024]

[0025] Among them, Lj,k Let l be the total length of the k-th drive rope in the j-th rope group. i，j，k Let l0 represent the length of the k-th drive rope in the j-th rope group within the i-th drive joint, and let l0 represent the length of the connecting rod segment. i，j，k (-) is a function for calculating the length of the k-th driving rope in the i-th segment of the j-th rope group, q 2j-1 ,q 2j Let be the joint angle of the j-th rope group in the i-th rope drive joint in the DH coordinate system, where i takes values ​​from 1 to j.

[0026] In some embodiments, the step of correcting the length of the drive rope in the rope drive joint based on the rope hole position vector and using a kinematic correction model to obtain the rope length formula is specifically expressed by the following formula:

[0027]

[0028] Among them, l i，j，k Let be the length of the k-th drive rope in the j-th rope group within the i-th segment of the drive joint. θ represents the angle between the rope hole direction of the k-th drive rope in the j-th rope group and the i-th segment of the drive joint. 2i-1 ,θ 2i Let d be the joint rotation angle of the i-th chord joint. i r represents the distance between the rope discs at the i-th rope drive joint. i φ represents the radius of the rope at the i-th rope drive joint. i This represents the torsion angle of the i-th chord joint.

[0029] In some embodiments, the step of obtaining the linear relationship function based on the rope length calculation function specifically includes:

[0030] Perform total differential processing on the rope length calculation function to obtain the differential function expression;

[0031] Based on the differential function, calculate the first rope length differential function and the second rope length differential function; the first rope length differential function is a function that combines the differential function to obtain the relevant parameters of the positioning rope and the change of the parameter vector to be calibrated; the second rope length differential function is a function that combines the differential function to obtain the relevant parameters of the redundant rope and the change of the parameter vector to be calibrated.

[0032] Based on the first rope length differential function and the second rope length differential function, the Jacobian matrix is ​​introduced to determine the linear relationship function.

[0033] In some embodiments, the step of iteratively solving the linear relationship function using the Newton-Raphson method and updating the parameters to be calibrated specifically includes:

[0034] Collect length error data for multiple sets of redundant ropes;

[0035] Based on the linear relationship function and multiple sets of length error equations, the least squares method is used to fit the error formula of the parameter to be calibrated.

[0036] The error formula of the parameter to be calibrated is solved iteratively using the Newton-Raphson method, and the parameter to be calibrated is updated.

[0037] In some embodiments, the step of fitting the linear relationship function and multiple sets of length error data using the least squares method to obtain the error formula for the parameter to be calibrated is specifically expressed by the following formula:

[0038] ΔC=(J E T J E ) -1 J E T ΔL Err

[0039] Where ΔC is the change in the vector of parameters to be calibrated by the snake-like robotic arm, J E To calibrate the Jacobian matrix of the equation, ΔL Err J is the length difference vector of the redundant ropes. E =[J Err,1 J Err,2 ...J Err,N ] T J Err，N This is the error Jacobian matrix corresponding to the Nth set of length error data collected. The length difference of the redundant rope of the i-th segment of the rope drive joint corresponding to the Nth set of length error data collected.

[0040] A second aspect of this application provides a parameter self-calibration device for a rope-driven snake-like robotic arm, the device comprising:

[0041] The first module is used to classify the drive ropes of the snake-shaped robotic arm to obtain the positioning ropes and redundant ropes;

[0042] The second module is used to determine the joint angle of each of the rope drive joints according to the positioning rope using the Jacobian matrix mapping method.

[0043] The third module is used to establish a DH coordinate system for the multi-segment rope-driven joints of the snake-like robotic arm and obtain the rope hole position vector of each rope-driven joint.

[0044] The fourth module is used to introduce a kinematic correction model and a kinematic error model, and to obtain the rope length calculation function of the driving rope in the rope drive joint based on the rope hole position vector and the joint angle.

[0045] The fifth module is used to obtain a linear relationship function based on the rope length calculation function; the linear relationship function is used to represent the linear relationship between the length of the redundant rope and the change in the parameter vector to be calibrated.

[0046] The sixth module is used to iteratively solve the linear relationship function using the Newton-Raphson method and update the parameters to be calibrated.

[0047] A third aspect of this application provides a computer-readable storage medium comprising a computer program that, when executed by a processor, implements the parameter self-calibration method for the rope-driven snake-like robotic arm described in the first aspect.

[0048] This application provides a parameter self-calibration method, device, and storage medium for a rope-driven serpentine robotic arm. It employs a Jacobi matrix mapping method to determine the joint angles of the rope-driven joints based on the positioning rope, establishes a DH coordinate system, obtains the rope hole position vectors of multiple rope-driven joints, introduces a kinematic correction model and a kinematic error model, obtains the rope length calculation function of the driving rope in the rope-driven joint, and obtains the linear relationship function between the redundant rope and the vector change of the parameter to be calibrated based on the rope length calculation function. The linear relationship function is iteratively solved using the Newton-Raphson method to update the parameter to be calibrated. This application is applicable to the parameter calibration of rope-driven serpentine robotic arms, considering the complex multi-stage transmission problem of rope-driven robotic arms. Through the three parts of the kinematic correction model, kinematic error model, and parameter self-calibration method, it can achieve rapid self-calibration of the kinematic parameters of the rope-driven serpentine robotic arm, improving the motion accuracy of the robotic arm and eliminating the influence of errors generated during processing and assembly. Attached Figure Description

[0049] Figure 1 This is a flowchart of a parameter self-calibration method for a rope-driven serpentine robotic arm provided in an embodiment of this application;

[0050] Figure 2 This is a schematic diagram illustrating the derivation of the forward kinematics from the joint to the distal end using the DH coordinate system in an embodiment of this application.

[0051] Figure 3 This is a schematic diagram of a simplified geometric model of a single cable-driven joint in the robotic arm in an embodiment of this application;

[0052] Figure 4This is a schematic diagram of establishing a DH coordinate system for a single chord drive joint in an embodiment of this application;

[0053] Figure 5 This is a schematic diagram of the parameter self-calibration device of a rope-driven snake-like robotic arm provided in an embodiment of this application. Detailed Implementation

[0054] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0055] It should be noted that although functional modules are divided in the device schematic diagram and a logical order is shown in the flowchart, in some cases, the steps shown or described may be performed in a different order than the module division in the device or the order in the flowchart. The terms "first," "second," etc., in the specification, claims, and the aforementioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence.

[0056] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein is for the purpose of describing embodiments of this application only and is not intended to limit this application.

[0057] Reference Figure 1 , Figure 1 This is an optional flowchart of a parameter self-calibration method for a rope-driven serpentine robotic arm provided in an embodiment of this application. The method may include, but is not limited to, steps S101 to S106:

[0058] Step S101: Classify the drive ropes of the snake-shaped robotic arm to obtain the positioning rope and the redundant rope;

[0059] Step S102: Using the Jacobian matrix mapping method, determine the joint angle of each rope drive joint based on the positioning rope.

[0060] Step S103: Establish a DH coordinate system for the multi-segment cable-driven joints of the snake-like robotic arm and obtain the cable hole position vector of each cable-driven joint.

[0061] Step S104: Introduce the kinematic correction model and the kinematic error model, and obtain the rope length calculation function of the driving rope in the rope drive joint based on the rope hole position vector and the joint angle.

[0062] Step S105: Obtain the linear relationship function based on the rope length calculation function;

[0063] Step S106: The linear relationship function is solved iteratively using the Newton-Raphson method to update the parameters to be calibrated.

[0064] In step S101 of some embodiments, the drive rope is divided into a positioning rope and a redundant rope. Specifically, a single rope drive joint has two degrees of freedom. When the rope is taut, the position of the joint can be determined by obtaining the lengths of any two ropes. These two ropes can be identified as positioning ropes. The positioning ropes are used to calculate the joint angles of the rope drive joint. The third rope is redundant and is identified as a redundant rope. The redundant rope is used to achieve parameter self-calibration.

[0065] In some embodiments, step S102 may include, but is not limited to, steps S201 to S203:

[0066] Step S201: Obtain the formula for the total length of the drive rope;

[0067] Step S202: Perform a difference operation on the total length formula to obtain the total length-joint angle equation;

[0068] Step S203: Obtain the length of the positioning rope in each segment of the rope drive joint by using the motor encoder, and use the Newton-Raphson method to numerically solve the equation of total length-joint angle to determine the joint angle.

[0069] In step S103 of some embodiments, referring to Figure 2 The Denavit-Hartenberg (D–H) method is used to derive the forward kinematics from the joint to the end effector. For a robotic arm with n links and n universal joints, 2n DH coordinate systems can be established at the joints.

[0070] Under the base coordinate {0}, the end can be represented as:

[0071]

[0072] in, This is the pose transformation matrix from the (2n-1)th coordinate system to the 2nth coordinate system.

[0073] For a single cable-driven joint, the mechanical structure is simplified to obtain a simplified geometric model of the single joint segment, such as... Figure 3 As shown, where θ 2i-1 ,θ 2i These are the two joint angles of the i-th segment of the chordal joint.

[0074] Figure 4 This is a schematic diagram illustrating the establishment of a DH coordinate system for a single chord drive joint in an embodiment of this application. Figure 4 The upper and lower ellipses in the image represent the upper and lower guide rope disks, respectively. Figure 3 θ2i-1 ,θ 2i These are the two joint angles of the i-th segment of the rope drive joint, and the rope is represented by line segment A. j,k B j,k It means that A j,k B j,k Let Aj represent the rope segment of the k-th group of ropes in the j-th joint. ,3 ,Bj ,3 Let represent the rope segment of the third group of ropes in the j-th joint. Establish a coordinate system {1} at the center of the lower guide rope disk, where the origin O1 is located at the center of the upper surface of the disk, the Z1 axis is perpendicular to the disk and points upwards, and the X1 axis points towards θ. 2i-1 The rotation axis is perpendicular to the X1 axis. Coordinate system {2} is established at the intersection of the axes of the cross joint, with its origin at O2. The Z2 axis is perpendicular to the disk and points upwards, while the X2 axis remains at θ. 2i-1 The rotation axis direction is θ for the Y2 axis. 2i Rotation axis direction. Coordinate system {3} is established at the center of the upper guide disk surface, with the origin at O3, the Z3 axis perpendicular to the disk (pointing towards the arm axis), and the Y3 axis at θ. 2i The rotation axis direction is that the Y3 axis is perpendicular to the X3 axis.

[0075] Taking the above coordinate systems {1} and {3} as examples, a vector formula for the position of the rope hole is established, specifically expressed by the following formula:

[0076] 1 H j,k = 3 H j,k =[cos(β) j +120(k-1))sin(β j +120(k-1))0 1] T

[0077] in, 1 H j,k Let $\mathbf{k}$ represent the position vectors of the upper and lower guide holes in coordinate system {1} of the $j$ joint of the $k$-th rope group. 3 H j,k β represents the position vector of the upper and lower guide holes of the k-th rope in the coordinate system {3} of the j-th joint. j Let be the angle between the line connecting the center of the guide hole corresponding to the k-th rope at the j-th joint and the center of the disk, and the X1 axis in coordinate system {1}.

[0078] Rope A j,k B j,k The formula for calculating the length is:

[0079]

[0080] in, Let be the pose transformation matrix from coordinate system {1} to coordinate system {3}. 1 H j,k Let $\mathbf{k}$ represent the position vectors of the upper and lower guide holes in coordinate system {1} of the $j$ joint of the $k$-th rope group. 3 H j,k Let |A| represent the position vectors of the upper and lower guide holes of the k-th rope in the coordinate system {3} of the j-th joint. j,k B j,k || indicates the length of the rope.

[0081] The rope-driven serpentine robotic arm has excessively redundant degrees of freedom, making it difficult to directly obtain analytical solutions for its inverse kinematics. Therefore, the Jacobian matrix mapping method can be used to solve its inverse kinematics.

[0082] When a joint moves, the rope length only changes in the joint segments between adjacent links that produce relative motion. The rope length within a link remains consistent with the arm length. Therefore, when performing inverse kinematics calculations in the rope-joint space, only the rope length changes in the joint segments need to be analyzed.

[0083] When multiple cable-driven joints in a robotic arm move in tandem, the movement of the cable-driven joints near the root will couple the movement of the drive cables of all the cable-driven joints at the rear.

[0084] The length of the driving rope of the i-th segment of the chord joint is not only determined by the joint angle of the i-th segment of the chord joint, but also affected by the joint angle of the preceding i-1 segments of the chord joint.

[0085] In step S201 of some embodiments, the total length L of the k-th drive rope of the j-th rope group is... j,k The total length of the drive rope, calculated as the sum of the lengths of all joint segments it passes through and the length of the boom segment, is expressed by the following formula:

[0086] Formula 1:

[0087] Among them, L j,k Let l be the total length of the k-th drive rope in the j-th rope group. i，j，k Let l0 represent the length of the k-th drive rope in the j-th rope group within the i-th drive joint, and let l0 represent the length of the connecting rod segment. i，j，k (-) is a function for calculating the length of the k-th driving rope in the i-th segment of the j-th rope group, q 2j-1 ,q 2j Let be the joint angle of the j-th rope group in the i-th rope drive joint in the DH coordinate system, where i takes values ​​from 1 to j.

[0088] The j-th rope group consists of all the drive ropes in the i-th rope drive joint, where i ranges from 1 to j. The j in the j-th rope group is the same as the i value in the corresponding i-th rope drive joint. That is, the 3rd rope group corresponds to the 3rd rope drive joint, and the 6th rope group corresponds to the 6th rope drive joint.

[0089] In step S202 of some embodiments, a difference operation is performed on Equation 1 above to obtain the total length-joint angle equation, specifically expressed by the following formula:

[0090] Equation 2: ΔQ=R + ΔL

[0091] Where ΔQ is the joint angle difference vector, ΔL is the total rope length difference vector, and R... + The formula for the total length of Equation 1 above uses multiple f. j,k (g) represents the pseudo-inverse matrix of the Jacobian matrix R of the function.

[0092] In step S203 of some embodiments, the length of the positioning rope in each segment of the rope drive joint is obtained by the motor encoder, substituted into Equation 2 above, and then the Newton-Raphson method is used to numerically solve Equation 2. The estimated joint angle is then:

[0093] Equation 3: Q m+1 =Q m +ΔQ

[0094] Where m represents the iteration number, Q m+1 Q m These are the estimated values ​​of the joint angles in the (m+1)th and mth iterations, respectively.

[0095] Repeat Equations 2 and 3 above to update the estimated joint angles. A certain allowable error value can be set. When the rope length error is less than the error value, the iteration will stop and the numerical solution of the joint angle will be obtained.

[0096] In step S104 of some embodiments, the upper and lower discs in the rope drive joint are typically installed with the rope drive joint using a shaft-hole fit. Due to positioning errors during installation, some errors inevitably exist in the assembly of the boom in the torsional direction. If adjacent booms have a certain torsion angle, the guide hole will also experience a torsional shift in position, leading to a corresponding shift in the rope. When there is a torsional error between adjacent booms, the rope shifts, and the rope length exceeds the length under the ideal model. To more accurately describe the kinematic relationship of the rope joint, a kinematic correction model needs to be introduced to correct the calculation of the rope length.

[0097] In some embodiments, step S104 may include, but is not limited to, steps S301 to S302:

[0098] Step S301: Based on the rope hole position vector formula, the length of the driving rope in the rope drive joint is corrected through the kinematic correction model to obtain the rope length formula.

[0099] Step S302: Based on the rope length formula and joint angles, a kinematic error model is introduced to determine the rope length calculation function.

[0100] In step S301 of some embodiments, considering the torsion of the disk, the pose transformation matrix of the coordinate system of the upper and lower disk centers can be expressed as:

[0101] Formula 4:

[0102] in, Let θ be the pose transformation matrix from coordinate system {1} to coordinate system {3} of the center of the disk in the i-th chord drive joint. 2i-1 ,θ 2i φ represents the two joint rotation angles of the i-th roping joint, d represents the distance between the roping discs of the i-th roping joint, and φ represents the torsion angle of the roping discs of the i-th roping joint.

[0103] Combining Equation 4 above and the above rope hole position vector formula, the corrected rope length formula is obtained, specifically expressed as follows:

[0104]

[0105] Among them, l i，j，k Let be the length of the k-th drive rope in the j-th rope group within the i-th segment of the drive joint. θ represents the angle between the rope hole direction of the k-th drive rope in the j-th rope group and the i-th segment of the drive joint. 2i-1 ,θ 2i Let d be the joint rotation angle of the i-th chord joint. i r represents the distance between the rope discs at the i-th rope drive joint. i φ represents the radius of the rope at the i-th rope drive joint. i This represents the torsion angle of the i-th chord joint.

[0106] In step S302 of some embodiments, due to machining and installation errors of the robotic arm parts and some ideal assumptions made when establishing the kinematic model of the robotic arm, there is a certain error between the nominal and actual values ​​of these parameters. Furthermore, the error between the design and actual values ​​of the parameters of the robotic arm drive rope leads to inaccurate kinematic calculations. From the rope length formula obtained through the kinematic correction model, it can be seen that the length of the drive rope is determined by the rope disc spacing d, the rope radius r, the joint angle q, and the rope hole orientation angle. The kinematic error is determined by five parameters, including the torsion angle φ. Therefore, before calibrating the parameters, it is necessary to establish the above kinematic error model and analyze the relationship between the kinematic error and the errors of each parameter.

[0107] The twist angle φ has already been accounted for in the above rope length formula obtained through the kinematic correction model. The following analysis examines the main sources of error for the remaining four kinematic parameters:

[0108] Rope spool spacing d: Considering the dimensional and installation errors in the parts' machining and installation, the actual rope spool spacing after assembly with the universal joint will deviate from the nominal value. Furthermore, the ideal model ignores the rope spool thickness, ideally assuming that rope length changes only occur between the spools. In reality, the rope begins to bend and deform at the contact point between the spool's circular hole and the rope. Therefore, the actual parameter d should not be the distance d between the upper and lower spool surfaces as given in the nominal value. N It is not the distance d between the bending points of the rope in the upper and lower discs. R This distance cannot be obtained through direct measurement, so the nominal value inevitably differs from the actual value.

[0109] Rope radius r: When designing the robotic arm, to reduce friction between the rope and the guide hole and ensure smooth rope passage, the diameter of the rope hole on the disk is designed to be slightly larger than the rope diameter. Therefore, the rope axis may deviate from the center of the guide hole during robotic arm movement, resulting in the actual rope radius not being equal to the radius of the guide hole center on the disk as defined in the nominal model.

[0110] Joint rotation angle q: Affected by the dimensions of the part and the machining accuracy of perpendicularity, parallelism, etc., the actual joint axis coordinate system differs from that defined in the nominal model (e.g., errors caused by the two universal joint axes not being perpendicular to each other, or the joint axis not being parallel to the disk plane). Errors in the joint axis coordinate system cause errors between the nominal joint angle and the actual measured joint angle.

[0111] Rope hole direction angle The error in the direction angle of the rope hole is mainly due to the influence of the machining accuracy of the parts.

[0112] Considering that the parts of the robotic arm are all machined using high-precision CNC machining, the dimensional and positional errors of the parts are small. Compared with the errors caused by installation and ideal assumptions, the machining errors are negligible. Therefore, based on the above analysis of error sources, the parameters that are prone to large errors in the kinematic error model calibration, namely the rope disc spacing d, the rope radius r, and the torsion angle φ, can be specifically expressed by the following formula for the rope length calculation function:

[0113]

[0114] Among them, l j，j，k Let be the length of the k-th drive rope in the j-th rope group within the i-th segment of the drive joint. Let q be the function for calculating the rope length of the k-th drive rope in the j-th rope group at the i-th segment of the drive joint.i d represents the joint angle of the i-th chord joint. i r represents the distance between the rope discs at the i-th rope drive joint. i φ represents the radius of the rope at the i-th rope drive joint. i This represents the torsion angle of the i-th chord joint.

[0115] In step S105 of some embodiments, the above-mentioned linear relationship function is used to represent the linear relationship between the length of the redundant rope and the change in the parameter vector to be calibrated.

[0116] In some embodiments, step S105 may include, but is not limited to, steps S401 to S403:

[0117] Step S401: Perform total differential processing on the rope length calculation function to obtain the differential function expression;

[0118] Step S402: Calculate the differential function expressions for the first rope length and the second rope length based on the differential function expressions;

[0119] Step S403: Based on the differential function expressions of the first and second rope lengths, introduce the Jacobian matrix to determine the linear relationship function.

[0120] In step S401 of some embodiments, the total differential of the above rope length calculation function is performed to obtain a differential function, which is specifically expressed by the following formula:

[0121]

[0122] Among them, l j，j，k Let be the length of the k-th drive rope in the j-th rope group within the i-th segment of the drive joint. Let q be the function for calculating the rope length of the k-th drive rope in the j-th rope group at the i-th segment of the drive joint. i d represents the joint angle of the i-th chord joint. i r represents the distance between the rope discs at the i-th rope drive joint. i φ represents the radius of the rope at the i-th rope drive joint. i This represents the torsion angle of the i-th chord joint.

[0123] In step S402 of some embodiments, the first rope length differential function is a function that combines differential functions to obtain the changes in the relevant parameters of the positioning rope and the vector of parameters to be calibrated, and the second rope length differential function is a function that combines differential functions to obtain the changes in the relevant parameters of the redundant rope and the vector of parameters to be calibrated.

[0124] Combining the above differential function expressions, we obtain the differential function expressions for the first rope length and the second rope length, specifically expressed by the following formula:

[0125] The differential function of the first rope length: dl pi =A 1,i dC i +B 1,i dq i

[0126] The differential function of the second rope length: dl ri =A 2,i dC i +B 2,i dq i

[0127] in, Let be the length vector of the positioning rope in the i-th rope drive joint. Let A be the length vector of the redundant rope in the i-th rope-driven joint. 1,i Let A be the partial derivative matrix of the rope length function of the positioning rope in the i-th rope drive joint with respect to the parameters to be calibrated. 2,i Let B be the partial derivative matrix of the rope length function of the redundant rope in the i-th rope drive joint with respect to the parameters to be calibrated. 1,i Let B be the partial derivative matrix of the rope length function of the positioning rope in the i-th rope drive joint with respect to the joint angle. 2,i Let C be the partial derivative matrix of the rope length function of the redundant rope in the i-th rope-driven joint with respect to the joint angle. i Let C be the parameter vector to be calibrated. i =[d i r i φ i ] T d i r represents the distance between the rope discs at the i-th rope drive joint. i φ represents the radius of the rope at the i-th rope drive joint. i This represents the torsion angle of the i-th chord joint.

[0128] Combining the differential functions of the first and second rope lengths above, we obtain the following equation:

[0129] Equation 5: dl ri =J err dC i +B 2,i B 1,i -1 dl pi

[0130] Formula 6: J err =(-B 2,i B 1,i -1 A 1,i +A 2,i )

[0131] Among them, J err The error Jacobian matrix is ​​defined by the letters in Equations 5 and 6, which have the same meanings as those in the first rope length differential function and the second rope length differential function.

[0132] Using the redundant rope length difference as an approximation of the rope length differential, Equation 5 is transformed into Equation 7, as follows:

[0133] Formula 7:

[0134] Formula 8:

[0135] in, The difference between the calibrated rope length and the actual measured rope length of the redundant rope. ΔC is the difference between the calibrated rope length and the actual measured rope length of the positioning rope. i Let ΔC be the change in the parameter to be calibrated. i This represents the parameter difference between the model calibration result and the actual calibration result for the parameters to be calibrated. The calibration results are for the model with parameters to be calibrated. This represents the actual calibration result of the parameter to be calibrated.

[0136] In step S403 of some embodiments, since the length of the positioning rope is measured by a high-resolution encoder on the motor, it can be assumed that there is no measurement error. Therefore, Equation 7 can be transformed into Equation 9, which is the above-mentioned linear relationship function, specifically expressed as follows:

[0137] Formula 9:

[0138] in, ΔC is the difference between the calibrated rope length and the actual measured rope length of the redundant rope. i Let ΔC be the change in the parameter to be calibrated. i J represents the parameter difference between the model calibration result and the actual calibration result for the parameters to be calibrated. err The error Jacobian matrix is ​​J in Equation 6 above. err same.

[0139] In some embodiments, step S106 may include, but is not limited to, steps S501 to S503:

[0140] Step S501: Collect multiple sets of length error data for redundant ropes;

[0141] Step S502: Based on the linear relationship function and multiple sets of length error equations, the least squares method is used to fit the equations to obtain the error formula of the parameter to be calibrated.

[0142] Step S503: The error formula of the parameter to be calibrated is iteratively solved using the Newton-Raphson method to update the parameter to be calibrated.

[0143] In some embodiments, considering the overall cable-driven robotic arm, Equation 9 above is transformed into Equation 10, which is specifically expressed by the following formula:

[0144] Equation 10: ΔL r =J Err ΔC

[0145] Where, ΔL r For the difference of the rope length vector of the redundant ropes of the entire rope-driven robotic arm, J Err Let be the error Jacobian matrix of the entire robotic arm, and ΔC be the change in the parameters to be calibrated for the entire robotic arm.

[0146] In step S501 of some embodiments, the error Jacobian matrix J of the entire robotic arm is... Err It is calculated using the given nominal joint parameters. Furthermore, for a given joint angle of the chord-driven joint, a redundant chord length difference can be obtained by measurement, and a set of equations can be derived from the above formula.

[0147] Since there are 3 parameters to be calibrated, at least 3 sets of independent equations are required to solve them. In order to minimize the measurement error and the error caused by uncertain interference during the measurement process, it is usually necessary to collect more than 3 sets of data, and collect N sets of redundant rope length error Δl3 (redundant rope length error data).

[0148] In step S502 of some embodiments, the error result of the parameter to be calibrated is fitted by the least squares method to obtain the error formula of the parameter to be calibrated, which is specifically expressed by the following formula:

[0149] ΔC=(J E T J E ) -1 J E T ΔL Err

[0150] Where ΔC is the change in the vector of parameters to be calibrated by the snake-like robotic arm, J E To calibrate the Jacobian matrix of the equation, ΔL Err J is the length difference vector of the redundant ropes. E =[J Err,1 J Err,2 ...J Err,N ] T J Err，N This is the error Jacobian matrix corresponding to the Nth set of length error data collected. The length difference of the redundant rope of the i-th segment of the rope drive joint corresponding to the Nth set of length error data collected.

[0151] In step S503 of some embodiments, the error formula of the parameter to be calibrated is solved iteratively by the Newton-Raphson method, and the parameter to be calibrated is updated to realize the self-calibration of the parameters of the rope-driven snake robot arm.

[0152] Reference Figure 5 , Figure 5 This is an optional structural diagram of a parameter self-calibration device for a rope-driven snake-like robotic arm provided in an embodiment of this application. The device includes:

[0153] The first module is used to classify the drive ropes of the snake-shaped robotic arm to obtain the positioning ropes and redundant ropes;

[0154] The second module is used to determine the joint angle of each rope drive joint based on the positioning rope using the Jacobian matrix mapping method.

[0155] The third module is used to establish a DH coordinate system for the multi-segment rope-driven joints of the snake-like robotic arm and obtain the rope hole position vector of each rope-driven joint.

[0156] The fourth module is used to introduce the kinematic correction model and the kinematic error model, and obtain the rope length calculation function of the driving rope in the rope drive joint based on the rope hole position vector and the joint angle.

[0157] The fifth module is used to calculate the linear relationship function based on the rope length calculation function; the linear relationship function is used to represent the linear relationship between the length of the redundant rope and the change in the vector of the parameter to be calibrated.

[0158] The sixth module is used to iteratively solve the linear relationship function using the Newton-Raphson method and update the parameters to be calibrated.

[0159] The specific implementation of the parameter self-calibration device of the rope-driven snake-like robotic arm is basically the same as the specific embodiment of the parameter self-calibration method of the rope-driven snake-like robotic arm described above, and will not be repeated here.

[0160] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described parameter self-calibration method for a rope-driven snake-like robotic arm.

[0161] Memory, as a non-transitory computer-readable storage medium, can be used to store non-transitory software programs and non-transitory computer-executable programs. Furthermore, memory may include high-speed random access memory, and may also include non-transitory memory, such as at least one disk storage device, flash memory device, or other non-transitory solid-state storage device. In some embodiments, memory may optionally include memory remotely located relative to the processor, and these remote memories can be connected to the processor via a network. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof.

[0162] This application provides a parameter self-calibration method, device, and storage medium for a rope-driven serpentine robotic arm. It employs a Jacobi matrix mapping method to determine the joint angles of the rope-driven joints based on the positioning rope, establishes a DH coordinate system, obtains the rope hole position vectors of multiple rope-driven joints, introduces a kinematic correction model and a kinematic error model, obtains the rope length calculation function of the driving rope in the rope-driven joint, and obtains the linear relationship function between the redundant rope and the vector change of the parameter to be calibrated based on the rope length calculation function. The linear relationship function is iteratively solved using the Newton-Raphson method to update the parameter to be calibrated. This application is applicable to the parameter calibration of rope-driven serpentine robotic arms, considering the complex multi-stage transmission problem of rope-driven robotic arms. Through the three parts of the kinematic correction model, kinematic error model, and parameter self-calibration method, it can achieve rapid self-calibration of the kinematic parameters of the rope-driven serpentine robotic arm, improving the motion accuracy of the robotic arm and eliminating the influence of errors generated during processing and assembly.

[0163] The embodiments described in this application are for the purpose of more clearly illustrating the technical solutions of the embodiments of this application, and do not constitute a limitation on the technical solutions provided by the embodiments of this application. As those skilled in the art will know, with the evolution of technology and the emergence of new application scenarios, the technical solutions provided by the embodiments of this application are also applicable to similar technical problems.

[0164] Those skilled in the art will understand that the technical solutions shown in the figures do not constitute a limitation on the embodiments of this application, and may include more or fewer steps than shown, or combine certain steps, or different steps.

[0165] The preferred embodiments of the present application have been described above with reference to the accompanying drawings, but this does not limit the scope of the claims of the present application. Any modifications, equivalent substitutions, and improvements made by those skilled in the art without departing from the scope and substance of the embodiments of the present application shall be within the scope of the claims of the present application.

Claims

1. A method for parameter self-calibration of a rope-driven serpentine robotic arm, characterized in that, include: The drive ropes of the snake-like robotic arm are classified to obtain positioning ropes and redundant ropes; The joint angle of each rope drive joint is determined using the Jacobian matrix mapping method based on the positioning rope. A DH coordinate system is established for the multiple cable-driven joints of the snake-like robotic arm to obtain the cable hole position vector of each cable-driven joint; By introducing a kinematic correction model and a kinematic error model, and based on the rope hole position vector and the joint angle, the rope length calculation function of the driving rope in the rope drive joint is obtained; Based on the rope length calculation function, a linear relationship function is obtained; The linear relationship function is used to represent the linear relationship between the length of the redundant rope and the change in the parameter vector to be calibrated. The linear relationship function is solved iteratively using the Newton-Raphson method to update the parameters to be calibrated. The step of introducing a kinematic correction model and a kinematic error model, and obtaining the rope length calculation function of the driving rope in the rope drive joint based on the rope hole position vector and the joint angle, specifically includes: Based on the rope hole position vector, the length of the drive rope in the rope drive joint is corrected by the kinematic correction model to obtain the rope length formula; Based on the rope length formula and the joint angle, a kinematic error model is introduced to determine the rope length calculation function; The rope length calculation function is expressed by the following formula: in, For the j-th rope group k The function for calculating the rope length of the driving rope at the i-th segment of the driving joint. q i Indicates the first i The joint angle of the segmented rope drive joint. d i Indicates the first i The distance between the rope discs at the segment drive joint. r i Indicates the first i The radius of the cloth rope in the segmented rope drive joint. i Indicates the first i The torsional angle of the joint of the rope segment.

2. The parameter self-calibration method for the rope-driven serpentine robotic arm according to claim 1, characterized in that, The step of determining the joint angle of each rope drive joint using the Jacobian matrix mapping method, based on the positioning rope, specifically includes: Formula for obtaining the total length of the drive rope; Perform a difference operation on the total length formula to obtain the total length-joint angle equation; The length of the positioning rope in each segment of the rope drive joint is obtained by a motor encoder. The Newton-Raphson method is used to numerically solve the equation of total length-joint angle to determine the joint angle.

3. The parameter self-calibration method for the rope-driven serpentine robotic arm according to claim 2, characterized in that, The step of obtaining the formula for the total length of the drive rope is specifically expressed by the following formula: in, For the first j The first rope group k The total length of the drive rope, l i，j，k For the first j The first rope group k The length of the drive rope in the i-th segment of the drive joint. Indicates the length of the connecting rod segment. f i，j，k (-) is the first j The first rope group k The drive rope in the first i Function for calculating rope length in a segmented rope drive joint In the DH coordinate system, the first j The rope group in the i The joint angle in the segmental rope drive joint, i takes values ​​from 1 to j.

4. The parameter self-calibration method for the rope-driven serpentine robotic arm according to claim 1, characterized in that, The step of correcting the length of the drive rope in the rope drive joint based on the rope hole position vector and using a kinematic correction model to obtain the rope length formula is specifically expressed by the following formula: in, l i，j，k For the first j The first rope group k The length of the drive rope in the i-th segment of the drive joint. Indicates the first j The first rope group k The included angle of the drive rope in the direction of the rope hole at the i-th segment of the drive joint. For the first i The joint angle of the segmental rope drive joint. d i Indicates the first i The distance between the rope discs at the segment drive joint. r i Indicates the first i The radius of the cloth rope in the segmented rope drive joint. i Indicates the first i The torsional angle of the joint of the rope segment.

5. The parameter self-calibration method for the rope-driven serpentine robotic arm according to claim 1, characterized in that, The step of obtaining the linear relationship function based on the rope length calculation function specifically includes: Perform total differential processing on the rope length calculation function to obtain the differential function expression; Based on the differential function, calculate the first rope length differential function and the second rope length differential function; the first rope length differential function is a function that combines the differential function to obtain the relevant parameters of the positioning rope and the change of the parameter vector to be calibrated; the second rope length differential function is a function that combines the differential function to obtain the relevant parameters of the redundant rope and the change of the parameter vector to be calibrated. Based on the first rope length differential function and the second rope length differential function, the Jacobian matrix is ​​introduced to determine the linear relationship function.

6. The parameter self-calibration method for the rope-driven serpentine robotic arm according to claim 1, characterized in that, The step of iteratively solving the linear relationship function using the Newton-Raphson method and updating the parameters to be calibrated specifically includes: Collect length error data for multiple sets of redundant ropes; Based on the linear relationship function and multiple sets of length error data, the least squares method is used to fit the error formula of the parameter to be calibrated. The error formula of the parameter to be calibrated is solved iteratively using the Newton-Raphson method, and the parameter to be calibrated is updated.

7. The parameter self-calibration method for the rope-driven serpentine robotic arm according to claim 6, characterized in that, The step of fitting the linear relationship function and multiple sets of length error data using the least squares method to obtain the error formula for the parameter to be calibrated is specifically expressed by the following formula: in, The change in the vector of parameters to be calibrated for the snake-like robotic arm. To calibrate the Jacobian matrix of the equation, Let the length difference vector of the redundant ropes be denoted as . , J Err，N For the first time collected N The error Jacobian matrix corresponding to the group length error data. , For the first time collected N The first group of length error data corresponding to the first group i The length difference of the redundant rope in the segmented rope drive joint.

8. A parameter self-calibration device for a rope-driven serpentine robotic arm, characterized in that, include: The first module is used to classify the drive ropes of the snake-shaped robotic arm to obtain the positioning ropes and redundant ropes; The second module is used to determine the joint angle of each rope drive joint based on the positioning rope using the Jacobian matrix mapping method. The third module is used to establish a DH coordinate system for the multiple rope-driven joints of the snake-like robotic arm and obtain the rope hole position vector of each rope-driven joint. The fourth module is used to introduce a kinematic correction model and a kinematic error model, and to obtain the rope length calculation function of the driving rope in the rope drive joint based on the rope hole position vector and the joint angle. The fifth module is used to obtain a linear relationship function based on the rope length calculation function; The linear relationship function is used to represent the linear relationship between the length of the redundant rope and the change in the parameter vector to be calibrated. The sixth module is used to iteratively solve the linear relationship function using the Newton-Raphson method and update the parameters to be calibrated. The fourth module is specifically used for: Based on the rope hole position vector, the length of the drive rope in the rope drive joint is corrected by the kinematic correction model to obtain the rope length formula; Based on the rope length formula and the joint angle, a kinematic error model is introduced to determine the rope length calculation function; The rope length calculation function is expressed by the following formula: in, For the j-th rope group k The function for calculating the rope length of the driving rope at the i-th segment of the driving joint. q i Indicates the first i The joint angle of the segmented rope drive joint. d i Indicates the first i The distance between the rope discs at the segment drive joint. r i Indicates the first i The radius of the cloth rope in the segmented rope drive joint. i Indicates the first i The torsional angle of the joint of the rope segment.

9. A computer-readable storage medium comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the parameter self-calibration method of the rope-driven snake-like robotic arm according to any one of claims 1 to 7.

Images