A rope-driven hyper-redundant snake-like manipulator kinematics inverse solution method

Abstract

The application discloses a kinematics inverse solution method of a rope-driven super-redundant snake-shaped mechanical arm. From the perspective of geometry, the method calculates the target pose of the mechanical arm and the angles of all joints of the mechanical arm in a simple and efficient way under the premise that the joint angles are not over-limited according to the motion trajectory of the end joint of the mechanical arm and the current joint angles of the mechanical arm. Then, the method calculates the absolute coordinates of all holes of the mechanical arm by using the homogeneous coordinate system transformation method, and further calculates the change amount of the driving rope of the mechanical arm, thereby obtaining the kinematics inverse solution of the mechanical arm. The application can efficiently solve the kinematics inverse solution of the super-redundant snake-shaped mechanical arm, solves the problems of large calculation amount, poor real-time performance and possible over-limit of joint angles of the traditional calculation method, and meets the motion control requirements of the super-redundant snake-shaped mechanical arm.

Description

Technical Field

[0001] This invention relates to the field of snake-shaped robotic arms, and more particularly to a method for inverse kinematics solution of a rope-driven, super-redundant snake-shaped robotic arm. Background Technology

[0002] The rope-driven super-redundant snake-like robotic arm is a snake-like continuous robotic arm with a number of degrees of freedom far exceeding that of traditional industrial robots. The robotic arm is composed of multiple slender arms connected in series, with each pair of arms linked by a universal joint and driven by three drive ropes, enabling rotation in two directions. Therefore, it can perform agile obstacle avoidance operations in complex and confined spaces. Its rigid structure allows it to bear a certain load and can be equipped with end effectors such as cameras and cutters for various operation scenarios. It also has high control precision. The rope-driven structure allows the drive motors of the robotic arm to be concentrated at the rear end, making it suitable for various hazardous operation scenarios.

[0003] Controlling the pose of a robot requires solving its inverse kinematics. Since the snake-like robot arm has many degrees of freedom, directly using traditional analytical or numerical methods to solve its inverse kinematics is computationally intensive and time-consuming, making it difficult to control the robot arm in real time. Furthermore, due to the physical limitations of its joints, the rotation angles at the joints are limited, and the problem of joint angle exceeding the limit needs to be considered when calculating the inverse kinematics. Therefore, a highly efficient inverse kinematics method that ensures that the joint angles do not exceed the limit while requiring less computation is needed to control the super-redundant snake-like robot arm. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention proposes a kinematics inverse kinematics method for a rope-driven, super-redundant serpentine robotic arm, which can quickly calculate the robotic arm's posture and thus precisely control its movement.

[0005] The objective of this invention is achieved through the following technical solution:

[0006] A method for inverse kinematics solution of a rope-driven, super-redundant serpentine manipulator, comprising the following steps:

[0007] Step 1: Obtain the current joint position of the robotic arm based on the current rotation angle between the joints, thereby obtaining the overall posture of the robotic arm;

[0008] Step 2: Calculate the trajectory of the robotic arm's end effector at the next moment based on the given end effector pose;

[0009] Step 3: Simplify the robotic arm into multiple series of links. The length of each link is the distance between the two universal joints before and after the current joint. This transforms the problem of solving the joint position of the robotic arm end pose into solving the coordinates of each joint point when the coordinates of the end of the link are known.

[0010] Step 4: Based on the coordinates of each joint point obtained in Step 3, obtain the change in the length of the driving rope through coordinate system transformation.

[0011] Furthermore, step three specifically includes the following sub-steps:

[0012] S301: Based on the motion trajectory of the robotic arm's end effector, obtain the coordinates of the robotic arm's end effector at the next moment. Connect the two points of the end effector joint at the next moment and the previous end effector joint at the previous moment to obtain a line segment. Find the point on this line segment that is the distance from the end effector joint at the next moment to the length of the robotic arm. This point is the position of the previous end effector joint at the next moment.

[0013] S302: After obtaining the coordinate position of the previous joint, use the same method to solve for the coordinate position of the remaining joints in the direction of the base. After knowing the coordinates of three joints, solve for the rotation angle of one joint.

[0014] S303: Determine whether the angle exceeds the limit. If it does, calculate the position of the next joint point based on the first two joint points and the limit angle. Then, continue to solve for the position of the remaining coordinate points in the direction of the base based on the new position. If it does not exceed the limit, the joint point position remains unchanged. Continue to solve for the position of the remaining joint points in the direction of the base until the joint point closest to the base, i.e., the position of the first joint point, is calculated.

[0015] S304: Find the point on the straight line where the base slide is located, and the distance from the first joint point is the length of the first link. This point is the base position. At this time, the positions of all joints of the robotic arm are obtained.

[0016] Furthermore, the rope-driven super-redundant serpentine robotic arm consists of multiple joints connected in series. Its base is mounted on a slide, and each pair of joints is connected by a universal joint. Each universal joint has two rotational degrees of freedom in the vertical direction, and two angle sensors are installed in each universal joint to detect the relative angle between the two connected joints. Each joint is driven by three steel wire ropes with an included angle of 120 degrees to each other. Each joint has perforated discs at both ends for the steel wire ropes to pass through. For an n-joint serpentine robotic arm, each perforated disc has 3n holes, evenly distributed on its circular surface. The distance from the hole to the center is R, the distance between any two adjacent joints is L, and the distance from the center of the perforated disc to the center of the universal joint is D. Holes numbered j, j+n, and j+2n control the rotation of the j-th joint. <n。

[0017] Furthermore, step four includes the following sub-steps:

[0018] S401: Define the center of the universal joint at the connection between the base and the first joint as the origin O of the coordinate system, the direction of the slide movement as the positive X-axis, the vertically upward direction as the positive Y-axis, and the direction perpendicular to the XOY plane and pointing out of the paper as the positive Z-axis. This coordinate system is the base coordinate system 0.

[0019] Coordinate system 0 is the coordinate system of the end of the first joint near the base. The origin of coordinate system 0 is the center of the first universal joint. The positive direction of the X0 axis is along the feed direction of the base, the positive direction of the Y0 axis is vertically upward, and the positive direction of the Z0 axis is perpendicular to the paper and outward. For the j-th joint, 1≤j≤n, there are a total of 3 coordinate systems connected to it, namely coordinate system 2j-2, coordinate system 2j-1, and coordinate system 2j. Among them, coordinate system 2j-1 is the coordinate system of the end of the j-th joint near the base. The origin of coordinate system 2j-1 coincides with that of coordinate system 2j-2. Coordinate system 2j-1 is the coordinate system of coordinate system 2j-2 around the X-axis. 2j-2 Rotate 90 degrees using the right-hand rule, then rotate around Z. 2j-2 Rotate θ according to the right-hand rule 2j-1 We obtain the coordinate system of coordinate system 2j, which is the coordinate system of the end of the j-th joint furthest from the base. The origin of coordinate system 2j is the center of the universal joint connecting the j-th and (j+1)-th joints. Coordinate system 2j is the coordinate system of coordinate system 2j-1 around Z. 2j-1 Rotate the axis by the right-hand rule θ 2j Then, translate the distance L between the two adjacent universal joints along the axis of the j-th joint away from the base, and then rotate it 90 degrees around the axis of the j-th joint away from the base according to the left-hand rule to obtain the result;

[0020] The perforated plate of the j-th joint that is close to the base is the 2j-1 perforated plate, and the perforated plate of the j-th joint that is far away from the base is the 2j perforated plate.

[0021] S402: j = 1;

[0022] S403: According to θ 2j-1 and θ 2j The homogeneous transformation matrix between coordinate system 2j-2 and coordinate system 2j is calculated.

[0023]

[0024] S404: Multiply the position coordinates of the (2j-1)th orifice in coordinate system 2j by the left multiplier. Obtain the coordinates (x, y) of the i-th hole position of the 2j-1-th hole disk in coordinate system 2j-2. (2j-1)-i y (2j-1)-i , z (2j-1)-i ):

[0025]

[0026] in, Here is the general formula for the hole position coordinates of the 2j-1th hole disk in coordinate system 2j;

[0027] S405: Unify the hole positions of the 2j-1th and 2j-2th perforated plates in coordinate system 2j-2, and obtain the distance between the i-th hole positions on the left and right perforated plates of the first universal joint as follows:

[0028]

[0029] The general formula for the coordinates of the i-th hole position of the (2j-2)-th hole disk is:

[0030]

[0031] S406: Based on L from the previous moment ij L calculated at the current time ij The difference is used to obtain the change in the length ΔL of the drive rope between the left and right orifice plates of the j-th universal joint. ij ;

[0032] S407: Does j satisfy j≤n-1? If yes, then j=j+1, return to S403; if no, then execute S408.

[0033] S408: For the j-th joint, the changes in its three drive ropes are:

[0034] Where i = j, j+n, j+2n.

[0035] Furthermore, θ 2j-1 and θ 2j The calculation formula is as follows:

[0036]

[0037]

[0038] Among them, y j z j Let y and y be the coordinates of the gimbal centers connecting the j-th joint and the (j+1)-th joint, respectively. j-1 z j-1 These are the coordinates of the center of the universal joint connecting the (j-1)th joint and the jth joint.

[0039] The beneficial effects of this invention are as follows:

[0040] The inverse kinematics method of this invention has low computational cost and high efficiency, and can quickly solve the inverse kinematics of the robotic arm, thereby improving the control performance of the robotic arm. At the same time, the joint angle limit problem is considered in the inverse solution process to ensure that the joint rotation angle of the robotic arm does not exceed the limit. Brief Description of the Drawings

[0041] Figure 1 It is a simplified three-dimensional model diagram of a cable-driven hyper-redundant snake-like manipulator.

[0042] Figure 2 It is a schematic diagram of the coordinate distribution of the manipulator.

[0043] Figure 3 It is a distribution diagram of the hole positions on the cross-section of the hole disk.

[0044] Figure 4 It is a schematic diagram of the joint rotation posture.

[0045] Figure 5 It is a schematic diagram of solving the joint positions from the end pose. <X

[0046] Figure 6 It is a flow chart of the inverse kinematics solution method of the present invention. Detailed Embodiment

[0047] The present invention will be described in detail below according to the drawings and preferred embodiments. The purpose and effect of the present invention will become clearer. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0048] As Figure 1 shown, the cable-driven hyper-redundant snake-like manipulator of the present invention is composed of multiple joints connected in series. Its base is installed on the sliding table, and each two joints are connected by a universal joint. Each universal joint has two rotational degrees of freedom in perpendicular directions, and two angle sensors are arranged in each universal joint to detect the relative angles of the two connected joints. The change of the driving cable during the movement of the snake-like manipulator is caused by the rotation at the joints, and the control of the driving cable near the end joint is also affected by the joints in the direction close to the base. Each two joints are driven by three steel cables with an included angle of 120 degrees to each other. Both ends of each joint are hole disks for passing the steel cables, and the distribution of the hole disks on the hole disks is as Figure 3 shown. For an n-joint snake-like manipulator, each hole disk has a total of 3n hole disks, and the hole disks are evenly distributed on the circular surface of the hole disk; the distance from the center of the hole disk to the center of the hole disk is R, the distance from the center of the hole disk to the center of the universal joint is D, and the distance between each two adjacent joint points is L. Each hole disk on the hole disk is numbered, and the hole positions numbered j, j + n, j + 2n (j < n) control the rotation of the jth joint.

[0049] The inverse kinematics solution method of the cable-driven hyper-redundant snake-like manipulator in the present invention is divided into the following two steps: solving the joint positions from the end pose and solving the displacement of the driving cable from the joint positions; specifically including:

[0050] Step 1: Obtain the current joint positions of the manipulator according to the rotation angles between the current joints of the manipulator, so as to obtain the overall posture of the manipulator.

[0051] Step 2: Calculate the trajectory of the robotic arm's end effector at the next moment based on the given end effector pose.

[0052] Step 3: Simplify the robotic arm into multiple series of links. The length of each link is the distance between the two universal joints before and after the current joint. This transforms the problem of solving the joint position of the robotic arm end effector into solving the coordinates of each joint point when the coordinates of the end effector of the link are known.

[0053] S301: Based on the motion trajectory of the robotic arm's end effector, obtain the coordinates of the robotic arm's end effector at the next moment. Connect the two points of the end effector joint at the next moment and the previous end effector joint at the previous moment to obtain a line segment. Find the point on this line segment that is the distance from the end effector joint at the next moment to the length of the robotic arm. This point is the position of the previous end effector joint at the next moment.

[0054] S302: After obtaining the coordinate position of the previous joint, use the same method to solve for the coordinate position of the remaining joints in the direction of the base. After knowing the coordinates of three joints, solve for the rotation angle of one joint.

[0055] S303: Determine whether the angle exceeds the limit. If it does, calculate the position of the next joint point based on the first two joint points and the limit angle. Then, continue to solve for the position of the remaining coordinate points in the direction of the base based on the new position. If it does not exceed the limit, the joint point position remains unchanged. Continue to solve for the position of the remaining joint points in the direction of the base until the joint point closest to the base, i.e., the position of the first joint point, is calculated.

[0056] S304: Find the point on the straight line where the base slide is located, and the distance from the first joint point is the length of the first link. This point is the base position. At this time, the positions of all joints of the robotic arm are obtained.

[0057] Step 4: Based on the coordinates of each joint point obtained in Step 3, obtain the change in the length of the driving rope through coordinate system transformation.

[0058] To solve for the displacement of the drive rope, the coordinates of all joint disc holes need to be determined. This invention uses the DH method to establish a model of the robotic arm and solve for all points, where the coordinate system corresponding to the j-th joint is as follows: Figure 2 As shown,

[0059] The general formula for the homogeneous transformation matrix of coordinates in the DH method is:

[0060]

[0061] For a rope-driven, super-redundant serpentine robot with n joints, i represents the i-th coordinate system, i∈[1,2n]; where a i Let Z be the length of the link and Z be the axis of the two joints.i-1 Z i Along Z i-1 Z i Distance between the common perpendiculars; α i The angle between the two axes, i.e., Z i-1 Around X i Switch to Z i The angle between the axes; θ i like Figure 1 As shown, this is the joint angle; d i For bias, i.e. along Z i-1 O of the axis i-1 To Z i-1 With Z i The distance between the perpendiculars.

[0062] For the robotic arm shown in this invention, when i is an odd number, a i =0, α i =90°, d i =0, when i is even, a i =L, where L is the distance between two adjacent universal joints of the robotic arm, α i =-90°, d i =0, θ i like Figure 1 As shown.

[0063] A 3D schematic diagram of the robotic arm joints is shown below. Figure 4 As shown, assuming it is the first joint closest to the base, then θ1 is the pitch angle of joint 1, and θ2 is the yaw angle of joint 1. First, θ1 and θ2 can be solved using the joint coordinates. Then, as shown... Figure 1 In the robotic arm shown, assuming the coordinates of the universal joint center connecting the base and joint one are (x0, y0, z0), and the coordinates of the universal joint center connecting joint one and joint two are (x1, y1, z1), then the following can be solved:

[0064]

[0065]

[0066] Then, new coordinate systems 1 and 2 are defined. Coordinate system 1 is obtained by first rotating coordinate system 0 by 90 degrees around X0 according to the right-hand rule, and then rotating it by θ1 around Z0 according to the right-hand rule. Coordinate system 2 is obtained by rotating coordinate system 1 by θ2 around Z1 according to the right-hand rule, then translating the distance L between two adjacent universal joints of the robotic arm away from the base along the axis of joint 1, and then rotating it by 90 degrees around the axis of joint 1 away from the base according to the left-hand rule. Next, the homogeneous transformation matrix from base coordinate system 0 to coordinate system 1 is solved using the DH method. and the homogeneous transformation matrix from coordinate system 1 to coordinate system 2

[0067]

[0068]

[0069] Therefore, the homogeneous transformation matrix from base coordinate system 0 to coordinate system 2

[0070] For an n-joint cable-driven super-redundant serpentine robotic arm, the cable holes driving the first universal joint rotation are numbered (1, 1+n, 1+2n). The coordinate formula of the i-th hole position on the base in the base coordinate system 0 is given by:

[0071]

[0072] And the general formula for the hole position coordinates of the hole disk 1 in coordinate system 2 is:

[0073]

[0074] The hole position coordinates of orifice 1 in the 2-coordinate system are expressed in matrix form and multiplied by the homogeneous transformation matrix on the left. This yields the coordinates of hole position 1 in the base coordinate system 0:

[0075]

[0076] By unifying the hole positions of the orifice plate 1 and the base hole positions in a single coordinate system, the distance between the i-th hole positions of the first universal joint can be obtained as follows:

[0077]

[0078] The change in the length of the driving rope is then calculated.

[0079] The calculation of the rope length between the joints is the same, such as... Figure 2 As shown, therefore, step four can be written as a unified process as follows:

[0080] S401: Define the center of the universal joint at the connection between the base and the first joint as the origin 0 of the coordinate system. The direction of the slide movement is the positive X-axis, the vertically upward direction is the positive Y-axis, and the direction perpendicular to the XOY plane and pointing out of the paper is the positive Z-axis. This coordinate system is the base coordinate system 0.

[0081] Coordinate system 0 is the coordinate system of the end of the first joint near the base. The origin of coordinate system 0 is the center of the first universal joint. The positive direction of the X0 axis is along the feed direction of the base, the positive direction of the Y0 axis is vertically upward, and the positive direction of the Z0 axis is perpendicular to the paper and outward. For the j-th joint, 1≤j≤n, there are a total of 3 coordinate systems connected to it, namely coordinate system 2j-2, coordinate system 2j-1, and coordinate system 2j. Among them, coordinate system 2j-1 is the coordinate system of the end of the j-th joint near the base. The origin of coordinate system 2j-1 coincides with that of coordinate system 2j-2. Coordinate system 2j-1 is the coordinate system of coordinate system 2j-2 around the X-axis. 2j-2 Rotate 90 degrees using the right-hand rule, then rotate around Z. 2j-2 Rotate θ according to the right-hand rule 2j-1 We obtain the coordinate system of coordinate system 2j, which is the coordinate system of the end of the j-th joint furthest from the base. The origin of coordinate system 2j is the center of the universal joint connecting the j-th and (j+1)-th joints. Coordinate system 2j is the coordinate system of coordinate system 2j-1 around Z. 2j-1 Rotate the axis by the right-hand rule θ 2j Then, translate the distance L between the two adjacent universal joints along the axis of the j-th joint away from the base, and then rotate it 90 degrees around the axis of the j-th joint away from the base according to the left-hand rule to obtain the result;

[0082] The perforated plate of the j-th joint that is close to the base is the 2j-1 perforated plate, and the perforated plate of the j-th joint that is far away from the base is the 2j perforated plate.

[0083] S402: j = 1; Calculate θ 2j-1 and θ 2j The calculation formula is as follows:

[0084]

[0085]

[0086] Among them, y j z j Let y and y be the coordinates of the gimbal centers connecting the j-th joint and the (j+1)-th joint, respectively. j-1 z j-1 These are the coordinates of the center of the universal joint connecting the (j-1)th joint and the jth joint.

[0087] S403: According to θ 2j-1 and θ 2j The homogeneous transformation matrix between coordinate system 2j-2 and coordinate system 2j is calculated.

[0088]

[0089] S404: Multiply the position coordinates of the (2j-1)th orifice in coordinate system 2j by the left multiplier. Obtain the coordinates (x, y) of the i-th hole position of the 2j-1-th hole disk in coordinate system 2j-2. (2j-1)-i y (2j-1)-i , z (2j-1)-i ):

[0090]

[0091] in, Here is the general formula for the hole position coordinates of the 2j-1th hole disk in coordinate system 2j;

[0092] S405: Unify the hole positions of the 2j-1th and 2j-2th perforated plates in coordinate system 2j-2, and obtain the distance between the i-th hole positions on the left and right perforated plates of the first universal joint as follows:

[0093]

[0094] The general formula for the coordinates of the i-th hole position of the (2j-2)-th hole disk is:

[0095]

[0096] S406: Based on L from the previous moment ij L calculated at the current time ij The difference is used to obtain the change in the length ΔL of the drive rope between the left and right orifice plates of the j-th universal joint. ij ;

[0097] S407: Does j satisfy j≤n-1? If yes, then j=j+1, return to S403; if no, then execute S408.

[0098] S408: For the j-th joint, the changes in its three drive ropes are:

[0099] Where i = j, j+n, j+2n.

[0100] The following specific embodiment further explains the inverse kinematics solution method of the present invention.

[0101] Figure 1 The robotic arm shown has a total of 6 joints, or 12 revolute joints, plus the prismatic joints of the base, for a total of 13 degrees of freedom. Driving these joints requires 19 motors, of which 18 motors drive the drive ropes and 1 motor drives the slide table. To accurately control the robotic arm's motion, the displacement of the drive ropes needs to be calculated from the end effector's pose. The limiting rotation angle at the joint is τ, and the inverse kinematics solution for the robotic arm is as follows:

[0102] First, the joint positions are determined from the end-effector pose: given the robot arm's previous pose as follows... Figure 5 As shown by the solid line a0-a6 on the left, to move the end a6 to point a6', draw point a6' in space, connect a6' and a5, and find point a5' on this line segment that is a distance L from a6' (L is the distance between the two joints of the robotic arm). Then connect a5' and a4 and find point a4' on the line segment that is a distance L from a5'. Then connect a4' and a3 and find point a3' on the line segment that is a distance L from a4'. Then connect a3' and a2 and find point a2' on the line segment that is a distance L from a3'. Then connect a2' and a1 and find point a1' on the line segment that is a distance L from a2'. Finally, draw a sphere with a1' as the center and intersect the horizontal base's forward direction X0 at point a0' (take the intersection point closer to a0).

[0103] After determining the positions of all the robot arm joints, calculate the angles between each joint and its adjacent joints. This can be done using the vector angle formula. Taking a5'a6' and a5'a4' as an example, their angle...

[0104]

[0105] If the calculated included angle ζ > τ, it indicates that the calculated angle exceeds the limit and compensation is required. The compensation strategy is as follows:

[0106] like Figure 5 As shown on the right, the dashed line a0'-a6' represents the original joint position before compensation, and the dotted line a0”-a6” represents the new joint position after compensation. Keeping a6' and a5' unchanged, find a point a4” on the original a6'a5'a4' plane such that the vector and The included angle is the limiting rotation angle τ, and Connect a4” and a3’ and find the point a3 on the line segment that is a distance L from a4”. Then connect a3” and a2’ and find the point a2 on the line segment that is a distance L from a3”. Then connect a2” and a1’ and find the point a1 on the line segment that is a distance L from a2”. Then draw sphere 4 with a1” as the center and intersect the horizontal base direction X0 at point a0 (take the intersection point closer to a0’).

[0107] Then, it is determined whether the remaining joint angles exceed the limit angle. If so, the compensation is continued according to the above method until all joint angles are less than the limit angle. The calculation ends, and the positions of all joints of the robot arm and the feed distance of the robot arm base are obtained.

[0108] The displacement of the driving rope is then calculated from the joint position. The key to solving this step is to find the position coordinates of the hole on the disc. The hole position is solved using the first joint as an example.

[0109]

[0110]

[0111] Then, the homogeneous transformation matrix from coordinate system 0 to coordinate system 1 is calculated. and the homogeneous transformation matrix from coordinate system 1 to coordinate system 2

[0112]

[0113]

[0114] The homogeneous transformation matrix from coordinate system 0 to 2 is:

[0115]

[0116] The coordinates of all holes on hole plate 1 in the 2-coordinate system are (where i represents the i-th drive rope):

[0117]

[0118] Expressing it in matrix form, we obtain the following relationship:

[0119]

[0120] Where (x0, y0, z0) are the coordinates of all holes on the orifice plate 1 in the 0 coordinate system.

[0121] The coordinates of the i-th hole on the base are (-D, R*cos((i-1)*π), R*sin((i-1)*π);

[0122] After obtaining the absolute coordinates of the holes on the two orifice plates, the distance between the two points can be calculated, and then the displacement of the drive rope of the first joint can be obtained.

[0123] To determine the length of the driving rope for joint 2, we need to find the coordinates of joints 2 and 3 in the 2-coordinate system. Given the coordinates of joints 2 and 3 in the 0-coordinate system and the coordinate transformation matrix from the 0-coordinate system to the 2-coordinate system, we only need to express the coordinates of joints 2 and 3 in the 0-coordinate system as a matrix and then multiply them by the inverse of the homogeneous coordinate transformation matrix. The remaining steps are the same as those for solving joint one above. First, solve for the coordinates of all hole positions, then solve for the displacement of the driving rope, and add the displacement of the driving rope caused by the change in the previous joint to obtain the total displacement of the driving rope. Continue to solve for the remaining joints using this method, and finally obtain the displacement of all driving ropes, completing the inverse kinematics solution.

[0124] The flowchart of the algorithm for solving the inverse kinematics is as follows: Figure 6 As shown.

[0125] It will be understood by those skilled in the art that the above descriptions are merely preferred examples of the invention and are not intended to limit the invention. Although the invention has been described in detail with reference to the foregoing examples, those skilled in the art can still modify the technical solutions described in the foregoing examples or make equivalent substitutions for some of the technical features. All modifications and equivalent substitutions made within the spirit and principles of the invention should be included within the scope of protection of the invention.

Claims

1. A method for inverse kinematics solution of a rope-driven, super-redundant serpentine robotic arm, characterized in that, The method includes the following steps: Step 1: Obtain the current joint position of the robotic arm based on the current rotation angle between the joints, thereby obtaining the overall posture of the robotic arm; Step 2: Calculate the trajectory of the robotic arm's end effector at the next moment based on the given end effector pose; Step 3: Simplify the robotic arm into multiple series of links. The length of each link is the distance between the two universal joints before and after the current joint. This transforms the problem of solving the joint position of the robotic arm end pose into solving the coordinates of each joint point when the coordinates of the end of the link are known. Step 4: Based on the coordinates of each joint point obtained in Step 3, obtain the change in the length of the driving rope through coordinate system transformation; The rope-driven super-redundant serpentine robotic arm consists of multiple joints connected in series. Its base is mounted on a slide table, and every two joints are connected by a universal joint. Each universal joint has two rotational degrees of freedom in the vertical direction, and two angle sensors are set in each universal joint to detect the relative angle between the two connected joints. Each joint is driven by three steel wire ropes with an included angle of 120 degrees to each other. Each joint has perforated discs at both ends for threading steel wire ropes. For an n-joint serpentine robotic arm, each disc has 3n perforations, evenly distributed on its circular surface. The distance from each perforation to the center of the disc is R, the distance between any two adjacent joints is L, and the distance from the center of the disc to the center of the universal joint is D. The perforations numbered j, j+n, and j+2n control the rotation of the j-th joint. <n; Step four includes the following sub-steps: S401: Define the center of the universal joint at the connection between the base and the first joint as the origin O of the coordinate system, the direction of the slide movement as the positive X-axis, the vertically upward direction as the positive Y-axis, and the direction perpendicular to the XOY plane and pointing out of the paper as the positive Z-axis. This coordinate system is the base coordinate system 0. Coordinate system 0 is the coordinate system of the end of the first joint near the base. The origin of coordinate system 0 is the center of the first universal joint. The positive direction of the X0 axis is along the feed direction of the base, the positive direction of the Y0 axis is vertically upward, and the positive direction of the Z0 axis is perpendicular to the paper and outward. For the j-th joint, 1≤j≤n, there are a total of 3 coordinate systems connected to it, namely coordinate system 2j-2, coordinate system 2j-1, and coordinate system 2j. Among them, coordinate system 2j-1 is the coordinate system of the end of the j-th joint near the base. The origin of coordinate system 2j-1 coincides with that of coordinate system 2j-2. Coordinate system 2j-1 is the coordinate system of coordinate system 2j-2 around the X-axis. 2j-2 Rotate 90 degrees using the right-hand rule, then rotate around Z. 2j-2 Rotate θ according to the right-hand rule 2j-1 We obtain the coordinate system of coordinate system 2j, which is the coordinate system of the end of the j-th joint furthest from the base. The origin of coordinate system 2j is the center of the universal joint connecting the j-th and (j+1)-th joints. Coordinate system 2j is the coordinate system of coordinate system 2j-1 around Z. 2j-1 Rotate the axis by the right-hand rule θ 2j Then, translate the distance L between the two adjacent universal joints along the axis of the j-th joint away from the base, and then rotate it 90 degrees around the axis of the j-th joint away from the base according to the left-hand rule to obtain the result; The perforated plate of the j-th joint that is close to the base is the 2j-1 perforated plate, and the perforated plate of the j-th joint that is far away from the base is the 2j perforated plate. S402: j=1; S403: According to θ 2j-1 and θ 2j The homogeneous transformation matrix between coordinate system 2j-2 and coordinate system 2j is calculated. : = ； S404: Multiply the position coordinates of the (2j-1)th orifice in coordinate system 2j by the left multiplier. The coordinates of the i-th hole position of the 2j-1-th hole disk in the coordinate system 2j-2 are obtained. : ； in, Here is the general formula for the hole position coordinates of the 2j-1th hole disk in coordinate system 2j; S405: Unify the hole positions of the 2j-1th and 2j-2th perforated plates in coordinate system 2j-2, and obtain the distance between the i-th hole positions on the left and right perforated plates of the first universal joint as follows: ； The general formula for the coordinates of the i-th hole position of the (2j-2)-th hole disk is: ； S406: Based on the previous moment Calculated with the current time The difference is used to obtain the change in the length Δ of the drive rope between the left and right orifice plates of the j-th universal joint. ; S407: Does j satisfy j≤n-1? If yes, then j=j+1 and return to S403; if no, then execute S408. S408: For the j-th joint, the changes in its three drive ropes are as follows: , where i = j, j + n, j + 2n.

2. The inverse kinematics solution method for a rope-driven, super-redundant serpentine robotic arm according to claim 1, characterized in that, Step three specifically includes the following sub-steps: S301: Based on the motion trajectory of the robotic arm's end effector, obtain the coordinates of the robotic arm's end effector at the next moment. Connect the two points of the end effector joint at the next moment and the previous end effector joint at the previous moment to obtain a line segment. Find the point on this line segment that is the distance from the end effector joint at the next moment to the length of the robotic arm. This point is the position of the previous end effector joint at the next moment. S302: After obtaining the coordinate position of the previous joint, use the same method to solve for the coordinate position of the remaining joints in the direction of the base. After knowing the coordinates of three joints, solve for the rotation angle of one joint. S303: Determine whether the angle exceeds the limit. If it does, calculate the position of the next joint point based on the first two joint points and the limit angle. Then, continue to solve for the position of the remaining coordinate points in the direction of the base based on the new position. If it does not exceed the limit, the joint point position remains unchanged. Continue to solve for the position of the remaining joint points in the direction of the base until the joint point closest to the base, i.e., the position of the first joint point, is calculated. S304: Find the point on the straight line where the base slide is located, and the distance from the first joint point is the length of the first link. This point is the base position. At this time, the positions of all joints of the robotic arm are obtained.

3. The inverse kinematics solution method for a rope-driven, super-redundant serpentine robotic arm according to claim 1, characterized in that, θ 2j-1 and θ 2j The calculation formula is as follows: )； ； in, , These are the coordinates of the gimbal center connecting the j-th joint and the (j+1)-th joint. , These are the coordinates of the center of the universal joint connecting the (j-1)th joint and the jth joint.

Images