Forward Kinematics Modeling Method of Serial Manipulator Based on Improved D-H Method

Abstract

本发明公开了一种基于改进D‑H方法的串联机械臂正向运动学建模方法，建立串联机械臂连杆坐标系{i}；分析串联机械臂D‑H参数，寻找D‑H参数不能完全描述机械臂连杆坐标系相对位置的情况；插入虚拟D‑H坐标系，其中虚拟D‑H坐标系Z轴的方向任意选择；利用D‑H方法分析含有虚拟D‑H坐标系的串联机械臂连杆参数，并带入机械臂连杆坐标系{i}相对于机械臂连杆坐标系{i‑1}的变换通式中，建立串联机械臂各关节相邻两连杆的坐变换矩阵建立串联机械臂末端连杆坐标系{n}相对于基座的坐标系{0}的描述，即串联机械臂正向运动学方程。该方法可任意增设虚拟D‑H坐标系，简化测绘过程，计算过程重复度高，便于编程运算；适用于任意构型的串联机械臂正向运动学建模。

Description

Technical Field

[0001] The present invention relates to a modeling method, in particular to a forward kinematics modeling method of a serial manipulator based on an improved D-H method. Background Art

[0002] A serial manipulator can be regarded as an open kinematic chain, which is composed of a series of connecting rods connected in series by rotational or translational joints. In order to study the displacement relationship between the connecting rods of the manipulator, a coordinate system can be fixedly connected to each connecting rod, and then the relationship between these coordinate systems can be described. Denavit and Hartenberg proposed a general method (i.e., the D-H method) to describe the spatial relationship between two adjacent connecting rods with a 4×4 homogeneous transformation matrix, so as to derive the equivalent homogeneous transformation matrix of the "end effector coordinate system" relative to the "reference coordinate system" and establish the forward kinematic equation of the manipulator.

[0003] If two adjacent connecting rods i and i-1 are connected by joint i, each connecting rod can be represented by four D-H parameters:

[0004] (1) a i-1 represents the length of connecting rod i-1;

[0005] (2) α i-1 represents the twist angle of connecting rod i-1;

[0006] (3) d i represents the distance between two connecting rods, that is, the intersection point of a i-1 and the axis i to the intersection point of a i and the axis i, measured along the axis i;

[0007] (4) θ i represents the angle between connecting rod a i-1 and a i .

[0008] In order to determine the relative motion relationship between the connecting rods of the robot, a coordinate system is fixedly connected to each connecting rod. The coordinate system fixedly connected to the base is marked as {0}, and the coordinate system fixedly connected to connecting rod i is marked as {i}. For the intermediate connecting rod:

[0009] The Z-axis Z of coordinate system {i-1} i-1 is collinear with joint axis i-1, pointing arbitrarily;

[0010] The X-axis X of coordinate system {i-1} i-1 coincides with the common perpendicular of joint axis i-1, pointing from joint i-1 to joint i. When the length a of connecting rod i-1 i-1 is 0, the X-axis X of coordinate system {i-1} i-1 = ±Z i ×Zi-1 ;

[0011] The Y-axis Y of the coordinate system {i - 1} i-1 is defined according to the right-hand rule, i.e., Y i-1 = Z i-1 × X i-1 .

[0012] For the first and last links:

[0013] The base coordinate system {0} is fixedly connected to the base and is stationary. In principle, it can be arbitrarily specified. The specification of the end-link coordinate system {n} is similar to that of the base coordinate system {0}.

[0014] According to the set link coordinate system, the corresponding link parameters can be defined as follows:

[0015] (1) a i-1 is the distance measured along X i-1 from Z i to Z i-1 ;

[0016] (2) α i-1 is the angle of rotation about X i-1 from Z i to Z i-1 ;

[0017] (3) d i is the distance measured along Z i-1 from X i to X i ;

[0018] (4) θ i is the angle of rotation about Z i-1 from X i to X i ;

[0019] For some relatively complex serial manipulators, directly using the D-H method to describe cannot fully describe the relative position relationship between the origin of the coordinate system {i - 1} and the origin of the coordinate system {i}. For example:

[0020] (1) The Z-axis Z of the coordinate system {i - 1} i-1 is a deflecting joint, i.e., Z i-1 is vertically upward (downward), and the Z-axis Z of the coordinate system {i} i is a pitching joint, i.e., Z i is perpendicular to the paper and pointing into (out of) the paper. The origin of the coordinate system {i} is above the origin of the coordinate system {i - 1}, but not on the Z-axis Z i-1 of the coordinate system {i - 1}. According to the D-H method, the X-axis X i-1 of the coordinate system {i - 1} is in the horizontal direction. At this time, from Z i-1 to Zi Along X i-1 Measured distance a i-1 , from X i-1 to X i Along Z i Measured distance d i is 0, a i-1 and d i cannot fully describe the relative position relationship between the origin of coordinate system {i - 1} and the origin of coordinate system {i}.

[0021] (2) The Z - axis Z of coordinate system {i - 1} i-1 is a deflection joint, that is, Z i-1 is vertically upward (downward), the Z - axis Z of coordinate system {i} i is a horizontal movement joint, that is, Z i is horizontally to the right (left), the origin of coordinate system {i} is on the Z - axis Z of coordinate system {i - 1} i-1 but does not coincide with the origin of coordinate system {i - 1}. According to the D - H method, the X - axis X of coordinate system {i - 1} i-1 is perpendicular to the paper surface. At this time, from Z i-1 to Z i along X i-1 measured distance a i-1 is 0, from X i-1 to X i along Z i measured distance d i , a i-1 and d i cannot fully describe the relative position relationship between the origin of coordinate system {i - 1} and the origin of coordinate system {i}.

[0022] Aiming at the deficiencies of the D - H method for kinematic modeling of serial manipulators, the present invention proposes an improved D - H method based on virtual D - H coordinate systems to realize kinematic modeling of serial manipulators with arbitrary configurations. Summary of the Invention

[0023] The purpose of the present invention is to provide a forward kinematic modeling method for serial manipulators based on an improved D - H method to solve the problem that the traditional D - H method cannot fully describe the relative position relationship between the origin of coordinate system {i - 1} and the origin of coordinate system {i} when performing forward kinematic modeling of some relatively complex serial manipulators. This method can arbitrarily add virtual D - H coordinate systems as needed to simplify the D - H parameter mapping process; at the same time, the calculation process of this method has a high degree of repetition, which is convenient for programming operations; this method is applicable to forward kinematic modeling of serial manipulators with arbitrary configurations.

[0024] To achieve the above - mentioned purpose, the present invention adopts the following technical solutions:

[0025] A forward kinematics modeling method for a serial manipulator based on an improved D-H method, comprising:

[0026] Step S1, based on the traditional D-H method, establish the link coordinate system of the serial manipulator. Coordinate system {0} is the base, coordinate system {1} is the shoulder yaw joint, coordinate system {2} is the shoulder pitch joint, coordinate system {3} is the shoulder translation joint, coordinate system {4} is the elbow yaw joint, coordinate system {5} is the elbow roll joint, coordinate system {6} is the forearm translation joint, coordinate system {7} is the forearm pitch joint, and coordinate system {8} is the end-effector coordinate system of the manipulator. The Z-axis Z0 of coordinate system {0} coincides with the Z-axis Z1 of coordinate system {1}, which is the rotation axis of the shoulder yaw joint. The Z-axis Z2 of coordinate system {2} is the rotation axis of the shoulder pitch joint. The Z-axis Z3 of coordinate system {3} is the translation direction of the shoulder translation joint, and its origin coincides with the origin of coordinate system {4}. The Z-axis Z4 of coordinate system {4} is the rotation axis of the elbow yaw joint. The Z-axis Z5 of coordinate system {5} is the rotation axis of the elbow roll joint. The Z-axis Z6 of coordinate system {6} is the translation direction of the forearm translation joint, and its origin coincides with the origin of coordinate system {7}. The Z-axis Z7 of coordinate system {7} is the rotation axis of the forearm pitch joint. The origin of coordinate system {8} is located at the end of the actuator. The X-axis X0 of coordinate system {0} points to the end of the actuator, and the direction of the X-axis X8 of coordinate system {8} is the same as the direction of the X-axis X0 of the base 1 coordinate system {0}. The intermediate coordinate systems all satisfy that the X-axis X i-1 The X-axis X of coordinate system {i - 1} i-1 coincides with the common perpendicular of the joint axis i - 1, pointing from joint i - 1 to joint i, where i is an integer from 1 to 8. When the length a of link i - 1 i-1 is 0, the X-axis X of coordinate system {i - 1} i-1 = ±Z i ×Z i-1 .

[0027] Step S2, for the link coordinate system of the manipulator in step 1, analyze the D-H parameters of the serial manipulator to find the situation where the D-H parameters cannot fully describe the relative position of the link coordinate system.

[0028] (1) For coordinate system {2} relative to coordinate system {1}, the distance measured along X1 from Z1 to Z2 is L1, and the distance measured along Z2 from X1 to X2 is 0. At this time, according to the D-H method, the distance d2 between the origin of coordinate system {2} and the origin of coordinate system {1} is not reflected.

[0029] (2) For the coordinate system {5} relative to the coordinate system {4}, the distance measured along X4 from Z4 to Z5 is 0, and the distance measured along Z5 from X4 to X5 is 0. At this time, according to the D-H method, the distance d5 between the origin of the coordinate system {5} and the origin of the coordinate system {4} is not reflected.

[0030] Step S3, according to the analysis result of Step S2, insert a virtual D-H coordinate system to form an improved D-H coordinate system. The purpose of establishing the virtual D-H coordinate system is to supplement the description of the link parameters of the serial manipulator that cannot be fully described by the traditional D-H method. The direction of the Z-axis of the virtual D-H coordinate system can be arbitrarily selected, usually perpendicular or parallel to the axis of the adjacent joints of the serial manipulator. For two adjacent links i and link i-1 connected by joint i, when the Z-axis of the virtual D-H coordinate system is parallel to the Z-axis of link i, the X-axis of the virtual D-H coordinate system is used to supplement the description of the link parameters of the serial manipulator that cannot be fully described by the traditional D-H method; when the Z-axis of the virtual D-H coordinate system is perpendicular to the Z-axis of link i, the Z-axis of the virtual D-H coordinate system is used to supplement the description of the link parameters of the serial manipulator that cannot be fully described by the traditional D-H method. Coordinate system {2 a} is the virtual D-H coordinate system between coordinate system {1} and coordinate system {2}, and coordinate system {5 a} is the virtual D-H coordinate system between coordinate system {4} and coordinate system {5}.

[0031] In the traditional D-H method, for the coordinate system {2} relative to the coordinate system {1}, the distance measured along X1 from Z1 to Z2 is L1. For the virtual D-H coordinate system {2 a} relative to the coordinate system {1}, the distance measured along X1 from Z1 to Z 2a should remain L1 unchanged and be able to describe the distance d2 between the origin of the coordinate system {2} and the origin of the coordinate system {1}. Therefore, the origin of the virtual D-H coordinate system {2 a} should be located on the X1 axis and directly below the origin of the coordinate system {2}. The direction of the Z-axis of the virtual D-H coordinate system {2 a} is parallel to the direction of the Z-axis of the coordinate system {2}, and the direction of the X-axis of the coordinate system {2 a} satisfies the definition of the D-H method and can include the distance d2 between the origin of the coordinate system {2} and the origin of the coordinate system {1}. Therefore, the direction of the X-axis of the coordinate system {2 a} is vertically upward.

[0032] The origin of the virtual D-H coordinate system {5 a} coincides with the coordinate system {5}, and the direction of its Z-axis is perpendicular to the direction of the Z-axis of the coordinate system {5}. According to the definition of the D-H method, the direction of its X-axis is perpendicular to the paper surface (inward / outward). The Z-axis of the virtual D-H coordinate system {5 a} Supplementary description of the distance d5 between the origin of the base D-H coordinate system {5} and the origin of the coordinate system {4}.

[0033] Step S4, based on Step S3, analyze the link parameters of the serial manipulator with a virtual D-H coordinate system using the D-H method, as shown in Table 1:

[0034] For the coordinate system {1} relative to the coordinate system {0}, the distance measured along X0 from Z0 to Z1 is 0, the angle of rotation around X0 from Z0 to Z1 is 0, the distance measured along Z1 from X0 to X1 is 0, and the angle of rotation around Z1 from X0 to X1 is π + θ1, where θ1 is a control variable;

[0035] Coordinate system {2 a} relative to the coordinate system {1}, the distance measured along X1 from Z1 to Z 2a is L1, the angle of rotation around X1 from Z1 to is the distance measured along from X1 to is 0, the angle of rotation around from X1 to is

[0036] Coordinate system {2} relative to the coordinate system {2 a}, the distance measured from to Z2 along is d2, the angle of rotation around from to Z2 is 0, the distance measured along Z2 from to X2 is 0, the angle of rotation around Z2 from to X2 is θ2, where θ2 is a control variable;

[0037] Coordinate system {3} relative to the coordinate system {2}, the distance measured along X2 from Z2 to Z3 is 0, the angle of rotation around X2 from Z2 to Z3 is the distance measured along Z3 from X2 to X3 is d3, the angle of rotation around Z3 from X2 to X3 is where d3 is a control variable including the initial distance of the origin of the coordinate system {3} relative to the origin of the coordinate system {2};

[0038] Coordinate system {4} relative to the coordinate system {3}, the distance measured along X3 from Z3 to Z4 is 0, the angle of rotation around X3 from Z3 to Z4 is the distance measured along Z4 from X3 to X4 is 0, the angle of rotation around Z4 from X3 to X4 is θ4, where θ4 is a control variable;

[0039] Coordinate system {5a} Relative to the coordinate system {4}, from Z4 to the distance measured along X4 is 0, from Z4 to the angle of rotation about X4 is 0, from X4 to along the measured distance is d5 (d5 < 0), from X4 to about the angle of rotation is 0;

[0040] The coordinate system {5} relative to the coordinate system {5 a}, from to Z5 along the measured distance is 0, from to Z5 about the angle of rotation is from to X5 the distance measured along Z5 is 0, from to X5 the angle of rotation about Z5 is where, θ5 is the control variable;

[0041] The coordinate system {6} relative to the coordinate system {5}, the distance from Z5 to Z6 measured along X5 is L5, the angle of rotation from Z5 to Z1 about X5 is 0, the distance from X5 to X6 measured along Z6 is d6, the angle of rotation from X5 to X6 about Z6 is 0, where d6 is the control variable including the initial distance of the origin of the coordinate system {6} relative to the origin of the coordinate system {5};

[0042] The coordinate system {7} relative to the coordinate system {6}, the distance from Z6 to Z7 measured along X6 is 0, the angle of rotation from Z6 to Z7 about X6 is the distance from X6 to X7 measured along Z7 is 0, the angle of rotation from X6 to X7 about Z7 is where, θ7 is the control variable.

[0043] The coordinate system {8} relative to the coordinate system {7}, the distance from Z7 to Z8 measured along X7 is L7, the angle of rotation from Z7 to Z8 about X7 is the distance from X7 to X8 measured along Z8 is d8, the angle of rotation from X7 to X8 about Z8 is 0.

[0044] Table 1 Link parameters of the serial manipulator based on the improved D-H coordinate system

[0045]

[0046] Step S5, using the improved D-H coordinate system, substitute the link parameters of the serial manipulator based on the improved D-H coordinate system established in step S4 into the general transformation formula T of the link coordinate system {i} relative to {i - 1} ii-1 In it, establish the coordinate transformation matrix of two adjacent connecting rods of each joint of the serial manipulator

[0047] Among them,

[0048] Here, a i-1 represents the length of the connecting rod, α i-1 represents the twist angle of the connecting rod, d i represents the distance between two adjacent connecting rods, θ i represents the included angle between two adjacent connecting rods.

[0049] The transformation matrix of the shoulder deflection joint relative to the base, that is, the coordinate system {1} relative to the coordinate system {0}:

[0050] The transformation matrix of the shoulder pitch joint relative to the shoulder deflection joint, that is, the coordinate system {2} relative to the coordinate system {1}:

[0051] The transformation matrix of the shoulder translation joint relative to the shoulder pitch joint, that is, the coordinate system {3} relative to the coordinate system {2}:

[0052] The transformation matrix of the forearm deflection joint relative to the shoulder translation joint, that is, the coordinate system {4} relative to the coordinate system {3}:

[0053] The transformation matrix of the forearm rotation joint relative to the forearm deflection joint, that is, the coordinate system {5} relative to the coordinate system {4}:

[0054] The transformation matrix of the forearm translation joint relative to the forearm rotation joint, that is, the coordinate system {6} relative to the coordinate system {5}:

[0055] The transformation matrix of the forearm pitch joint relative to the forearm translation joint, that is, the coordinate system {7} relative to the coordinate system {6}:

[0056] The transformation matrix of the end effector of the manipulator relative to the forearm pitch joint, that is, the coordinate system {8} relative to the coordinate system {7}:

[0057] Step S6, based on step S5, establish the description of the coordinate system of the end link of the serial manipulator relative to the base coordinate system {0}, that is, the forward kinematic equation of the serial manipulator.

[0058]

[0059] The improved D-H coordinate system in the present invention refers to the coordinate system obtained by inserting the virtual coordinate system {2 a} and the coordinate system {5 a} on the basis of the original D-H coordinate system. The D-H method is a method for establishing a joint coordinate system, and the coordinate system established by using this method is called the D-H coordinate system. The improved D-H method means that a virtual coordinate system is added on the basis of the original D-H coordinate system, which increases the applicable range of the traditional D-H method.

[0060] The present invention has the following advantages over the prior art:

[0061] (1) The improved D-H method based on the virtual D-H coordinate system proposed by the present invention can be applied to the forward kinematics modeling of serial manipulators with arbitrary configurations;

[0062] (2) The improved D-H method based on the virtual D-H coordinate system proposed by the present invention can arbitrarily add virtual D-H coordinate systems as needed, simplifying the D-H parameter mapping process;

[0063] (3) The improved D-H method based on the virtual D-H coordinate system proposed by the present invention has a high degree of repetition in the calculation process and is convenient for programming operations. Brief Description of the Drawings

[0064] Figure 1 is a schematic diagram of the kinematic diagram of the serial manipulator mechanism of the present invention and the corresponding joint coordinate system; Figure 2 is a schematic diagram of the D-H coordinate system of the serial manipulator of the present invention;

[0065] Figure 3 is a schematic diagram of the improved D-H coordinate system based on the virtual D-H coordinate system of the present invention;

[0066] In the figure, 1. Base, 2. Shoulder yaw joint, 3. Shoulder pitch joint, 4. Shoulder prismatic joint, 5. Elbow yaw joint, 6. Elbow roll joint, 7. Elbow prismatic joint, 8. Wrist pitch joint, 9. End of the manipulator. Detailed Embodiment

[0067] The present invention will be further described below in conjunction with the drawings and embodiments.

[0068] The structures, proportions, sizes, etc. shown in the accompanying drawings of this specification are only used to cooperate with the content disclosed in the specification for those skilled in this technology to understand and read, and are not used to limit the conditions for the implementation of the present invention. Therefore, they do not have substantial technical significance. Any modification of the structure, change in the proportional relationship or adjustment of the size, without affecting the efficacy that the present invention can produce and the purpose that can be achieved, should still fall within the scope covered by the technical content disclosed in the present invention. At the same time, the terms such as "upper", "lower", "left", "right", "middle" and "one" cited in this specification are only for the convenience of clear narration and are not used to limit the scope for the implementation of the present invention. The change or adjustment of their relative relationships, without substantial change in the technical content, should also be regarded as the scope for the implementation of the present invention.

[0069] Step S1, as Figure 1 shown, based on the D-H method, establish the link coordinate system of the serial manipulator. Coordinate system {0} is the base 1, coordinate system {1} is the shoulder yaw joint 2, coordinate system {2} is the shoulder pitch joint 3, coordinate system {3} is the shoulder prismatic joint 4, coordinate system {4} is the elbow yaw joint 5, coordinate system {5} is the elbow roll joint 6, coordinate system {6} is the forearm prismatic joint 7, coordinate system {7} is the forearm pitch joint 8, and coordinate system {8} is the end effector 9 coordinate system of the manipulator. The Z-axis Z0 of coordinate system {0} coincides with the Z-axis Z1 of coordinate system {1}, which is the rotation axis of the shoulder yaw joint 2. The Z-axis Z2 of coordinate system {2} is the rotation axis of the shoulder pitch joint 3. The Z-axis Z3 of coordinate system {3} is the translation direction of the shoulder prismatic joint 4, and its coordinate origin coincides with the coordinate origin of coordinate system {4}. The Z-axis Z4 of coordinate system {4} is the rotation axis of the elbow yaw joint 5. The Z-axis Z5 of coordinate system {5} is the rotation axis of the elbow roll joint 6. The Z-axis Z6 of coordinate system {6} is the translation direction of the forearm prismatic joint 7, and its coordinate origin coincides with the coordinate origin of coordinate system {7}. The Z-axis Z7 of coordinate system {7} is the rotation axis of the forearm pitch joint 8. The coordinate origin of coordinate system {8} is located at the end effector 9 of the actuator (manipulator). The X-axis X0 of coordinate system {0} points to the end effector 9 of the actuator (manipulator), and the direction of the X-axis X8 of coordinate system {8} is the same as the direction of the X-axis X0 of the base 1 coordinate system {0}. The intermediate coordinate systems all satisfy that the X-axis X i-1 of coordinate system {i - 1} i-1 coincides with the common perpendicular of the X-axis X of coordinate system {i - 1} and the joint axis i - 1, and points from joint i - 1 to joint i. When the length a i-1 of link i - 1 is 0, the X-axis X i-1 of coordinate system {i - 1} = ±Z i ×Z i-1 .

[0070] Step S2, according to Figure 1The mechanical arm link coordinate system shown is analyzed for the D-H parameters of a serial manipulator to find cases where the D-H parameters cannot fully describe the relative position of the link coordinate system, such as Figure 2 as shown.

[0071] (1) For coordinate system {2} relative to coordinate system {1}, the distance measured along X1 from Z1 to Z2 is L1, and the distance measured along Z2 from X1 to X2 is 0. At this time, according to the D-H method, the distance d2 between the origin of coordinate system {2} and the origin of coordinate system {1} is not reflected.

[0072] (2) For coordinate system {5} relative to coordinate system {4}, the distance measured along X4 from Z4 to Z5 is 0, and the distance measured along Z5 from X4 to X5 is 0. At this time, according to the D-H method, the distance d5 between the origin of coordinate system {5} and the origin of coordinate system {4} is not reflected.

[0073] Step S3, as Figure 3 shown, according to the analysis result of step S2, a virtual D-H coordinate system is inserted. Coordinate system {2 a} is the virtual D-H coordinate system between coordinate system {1} and coordinate system {2}, and coordinate system {5 a} is the virtual D-H coordinate system between coordinate system {4} and coordinate system {5}. The purpose of establishing the virtual D-H coordinate system is to supplement the description of the link parameters of the serial manipulator that cannot be fully described by the traditional D-H method. The direction of the Z-axis of the virtual D-H coordinate system can be arbitrarily selected, usually perpendicular or parallel to the axis of the adjacent joints of the serial manipulator. For two adjacent links i and i-1 connected by joint i, when the Z-axis of the virtual D-H coordinate system is parallel to the Z-axis of link i, the X-axis of the virtual D-H coordinate system is used to supplement the description of the link parameters of the serial manipulator that cannot be fully described by the traditional D-H method; when the Z-axis of the virtual D-H coordinate system is perpendicular to the Z-axis of link i, the Z-axis of the virtual D-H coordinate system is used to supplement the description of the link parameters of the serial manipulator that cannot be fully described by the traditional D-H method.

[0074] In the traditional D-H method, for coordinate system {2} relative to coordinate system {1}, the distance measured along X1 from Z1 to Z2 is L1. For the virtual D-H coordinate system {2 a} relative to coordinate system {1}, the distance measured along X1 from Z1 to should remain L1 unchanged and be able to describe the distance d2 between the origin of coordinate system {2} and the origin of coordinate system {1}. Therefore, the origin of the virtual D-H coordinate system {2 a} should be located on the X1 axis and directly below the origin of coordinate system {2}. The direction of the Z-axis of the virtual D-H coordinate system {2 a} is parallel to the direction of the Z-axis of coordinate system {2}, and coordinate system {2 aThe direction of the X-axis satisfies the D-H method definition and can include the distance d2 between the origin of coordinate system {2} and the origin of coordinate system {1}. Therefore, coordinate system {2 a} has its X-axis direction vertically upward.

[0075] The coordinate origin of the virtual D-H coordinate system {5 a} coincides with that of coordinate system {5}. The direction of its Z-axis is perpendicular to the Z-axis direction of coordinate system {5}. According to the D-H method definition, the direction of its X-axis is perpendicular to the paper surface (inward / outward). The Z-axis of the virtual D-H coordinate system {5 a} is Z 5a which is a supplementary description of the distance d5 between the origin of the basic D-H coordinate system {5} and the origin of coordinate system {4}.

[0076] Step S4, as Figure 3 shown, based on step S3, use the D-H method to analyze the link parameters of the serial manipulator with the virtual D-H coordinate system, as shown in Table 1:

[0077] For coordinate system {1} relative to coordinate system {0}, the distance measured along X0 from Z0 to Z1 is 0, the angle of rotation around X0 from Z0 to Z1 is 0, the distance measured along Z1 from X0 to X1 is 0, and the angle of rotation around Z1 from X0 to X1 is π + θ1, where θ1 is a control variable;

[0078] For coordinate system {2 a} relative to coordinate system {1}, the distance measured along X1 from Z1 to is L1, the angle of rotation around X1 from Z1 to is The distance measured along from X1 to is 0, and the angle of rotation around from X1 to is

[0079] For coordinate system {2} relative to coordinate system {2 a}, the distance measured along from to Z2 is d2, the angle of rotation around from to Z2 is 0, the distance measured along Z2 from to X2 is 0, and the angle of rotation around Z2 from to X2 is θ2, where θ2 is a control variable;

[0080] For coordinate system {3} relative to coordinate system {2}, the distance measured along X2 from Z2 to Z3 is 0, and the angle of rotation around X2 from Z2 to Z3 is The distance measured along Z3 from X2 to X3 is d3, and the angle of rotation around Z3 from X2 to X3 is where d3 is a control variable that includes the initial distance of the origin of coordinate system {3} relative to the origin of coordinate system {2};

[0081] For coordinate system {4} relative to coordinate system {3}, the distance measured along X3 from Z3 to Z4 is 0, and the angle of rotation around X3 from Z3 to Z4 is The distance measured along Z4 from X3 to X4 is 0, and the angle of rotation around Z4 from X3 to X4 is θ4, where θ4 is a control variable;

[0082] Coordinate system {5 a} relative to coordinate system {4}, from Z4 to The distance measured along X4 is 0, from Z4 to The angle of rotation around X4 is 0, from X4 to The distance measured along is d5 (d5 < 0), from X4 to The angle of rotation around is 0;

[0083] Coordinate system {5} relative to coordinate system {5 a}, from to Z5 along The distance measured is 0, from to Z5 around The angle of rotation is From to X5 along Z5 the distance measured is 0, from to X5 around Z5 the angle of rotation is where θ5 is a control variable;

[0084] For coordinate system {6} relative to coordinate system {5}, the distance measured along X5 from Z5 to Z6 is L5, the angle of rotation around X5 from Z5 to Z1 is 0, the distance measured along Z6 from X5 to X6 is d6, and the angle of rotation around Z6 from X5 to X6 is 0, where d6 is a control variable that includes the initial distance of the origin of coordinate system {6} relative to the origin of coordinate system {5};

[0085] For coordinate system {7} relative to coordinate system {6}, the distance measured along X6 from Z6 to Z7 is 0, and the angle of rotation around X6 from Z6 to Z7 is The distance measured along Z7 from X6 to X7 is 0, and the angle of rotation around Z7 from X6 to X7 is where θ7 is a control variable.

[0086] For coordinate system {8} relative to coordinate system {7}, the distance measured along X7 from Z7 to Z8 is L7, and the angle of rotation around X7 from Z7 to Z8 is The distance measured along Z8 from X7 to X8 is d8, and the angle of rotation around Z8 from X7 to X8 is 0.

[0087] Table 1 Link parameters of the serial manipulator based on the improved D-H coordinate system

[0088]

[0089] Step S5: Using the improved D-H coordinate system, substitute the link parameters of the serial manipulator based on the improved D-H coordinate system established in step S4 into the general transformation formula T of the link coordinate system {i} relative to {i-1} i i-1 to establish the coordinate transformation matrix between adjacent two links of each joint of the serial manipulator

[0090] Among them, where a i-1 represents the link length, α i-1 represents the link twist angle, d i represents the distance between adjacent two links, and θ i represents the angle between adjacent two links.

[0091] As Figure 1 shown, the transformation matrix of the boom deflection joint 2 relative to the base 1, that is, the coordinate system {1} relative to the coordinate system {0}

[0092] The transformation matrix of the boom pitch joint 3 relative to the boom deflection joint 2, that is, the coordinate system {2} relative to the coordinate system {1}

[0093] The transformation matrix of the boom translation joint 4 relative to the boom pitch joint 3, that is, the coordinate system {3} relative to the coordinate system {2}

[0094] The transformation matrix of the forearm deflection joint 5 relative to the boom translation joint 4, that is, the coordinate system {4} relative to the coordinate system {3}

[0095] The transformation matrix of the forearm rotation joint 6 relative to the forearm deflection joint 5, that is, the coordinate system {5} relative to the coordinate system {4}

[0096] The transformation matrix of the forearm translation joint 7 relative to the forearm rotation joint 6, that is, the coordinate system {6} relative to the coordinate system {5}

[0097] The transformation matrix of the forearm pitch joint 8 relative to the forearm translation joint 7, that is, the coordinate system {7} relative to the coordinate system {6}

[0098] The transformation matrix of the end 9 of the robotic arm relative to the pitch joint 8 of the forearm, i.e., the coordinate system {8} relative to the coordinate system {7}

[0099] Step S6, based on step S5, establish the description of the coordinate system of the end 9 link of the serial robotic arm relative to the coordinate system {0} of the base 1, i.e., the forward kinematics equation of the serial robotic arm.

[0100]

[0101] Example verification

[0102] Using the rotation operators Rot(X,γ), Rot(Y,β), Rot(Z,α) and the translation operator Trans(P x ,P y ,P z ), construct the homogeneous coordinate transformation matrix of the coordinate system {i} relative to the coordinate system {i - 1} Thus, establish the forward kinematics model of the serial robotic arm as shown in Figure 1 . Among them, Rot(X,γ) represents that the coordinate system {i} rotates around the X-axis of the coordinate system {i - 1} by an angle of γ, Rot(Y,β) represents that the coordinate system {i} rotates around the Y-axis of the coordinate system {i - 1} by an angle of β, Rot(Z,α) represents that the coordinate system {i} rotates around the Z-axis of the coordinate system {i - 1} by an angle of α, and Trans(P x ,P y ,P z ) represents that the translation amounts of the origin of the coordinate system {i} relative to the X-axis, Y-axis, and Z-axis of the coordinate system {i - 1} are P x ,P y ,P z .

[0103] Among them, the translation operator

[0104] The rotation operator

[0105] The rotation operator

[0106] The rotation operator

[0107] The homogeneous coordinate transformation matrix of the coordinate system {i} relative to the coordinate system {i - 1}

[0108]

[0109] As shown in Figure 1As shown in the figure, the homogeneous transformation matrix of the boom deflection joint 2 relative to the base 1, that is, the coordinate system {1} relative to the coordinate system {0}:

[0110] The transformation matrix of the boom pitch joint 3 relative to the boom deflection joint 2, that is, the coordinate system {2} relative to the coordinate system {1}:

[0111] The transformation matrix of the boom translation joint 4 relative to the boom pitch joint 3, that is, the coordinate system {3} relative to the coordinate system {2}:

[0112] The transformation matrix of the forearm deflection joint 5 relative to the boom translation joint 4, that is, the coordinate system {4} relative to the coordinate system {3}:

[0113] The transformation matrix of the forearm rotary joint 6 relative to the forearm deflection joint 5, that is, the coordinate system {5} relative to the coordinate system {4}:

[0114] The transformation matrix of the forearm translation joint 7 relative to the forearm rotary joint 6, that is, the coordinate system {6} relative to the coordinate system {5}:

[0115] The transformation matrix of the forearm pitch joint 8 relative to the forearm translation joint 7, that is, the coordinate system {7} relative to the coordinate system {6}

[0116] The transformation matrix of the end effector 9 of the robotic arm relative to the forearm pitch joint 8, that is, the coordinate system {8} relative to the coordinate system {7}

[0117] In summary, for a serial robotic arm with a complex configuration, the pose description of the coordinate system {i} relative to the coordinate system {i - 1} obtained by using the improved D - H method based on the virtual D - H coordinate system proposed in the present invention is consistent with the result obtained by using the homogeneous coordinate transformation matrix This proves the correctness of the method proposed in the present invention.

[0118] Although the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, it is not a limitation on the protection scope of the present invention. Those skilled in the art should understand that, based on the technical solutions of the present invention, various modifications or deformations that can be made by those skilled in the art without creative efforts are still within the protection scope of the present invention.

Claims

1. A forward kinematics modeling method for a serial manipulator based on an improved D-H method, characterized in that, It includes the following steps: Step S1: Based on the traditional D-H method, establish the link coordinate system {i} of the serial manipulator, where i is an integer from 0 to 8; Step S2: According to the link coordinate system of the manipulator in Step S1, analyze the D-H parameters of the serial manipulator to find the situations where the D-H parameters cannot fully describe the relative position of the link coordinate system of the manipulator; Step S3: According to the analysis result in Step S2, insert a virtual D-H coordinate system to form an improved D-H coordinate system, where the direction of the Z-axis of the virtual D-H coordinate system is arbitrarily selected, perpendicular or parallel to the adjacent joint axis of the serial manipulator; For two adjacent links i and i - 1 connected by joint i, when the Z-axis of the virtual D-H coordinate system is parallel to the Z-axis of link i, the X-axis of the virtual D-H coordinate system is used to supplement the description of the link parameters of the serial manipulator that cannot be fully described by the traditional D-H method; when the Z-axis of the virtual D-H coordinate system is perpendicular to the Z-axis of link i, the Z-axis of the virtual D-H coordinate system is used to supplement the description of the link parameters of the serial manipulator that cannot be fully described by the traditional D-H method; Coordinate system {2 a} is the virtual D-H coordinate system between coordinate system {1} and coordinate system {2}, and coordinate system {5 a} is the virtual D-H coordinate system between coordinate system {4} and coordinate system {5}; In the traditional D-H method, the distance measured along X1 from Z1 to Z2 for coordinate system {2} relative to coordinate system {1} is L1. The virtual D-H coordinate system {2 a} relative to coordinate system {1}, the distance measured along X1 from Z1 to should remain L1 unchanged and be able to describe the distance d2 between the origin of coordinate system {2} and the origin of coordinate system {1}; therefore, the origin of the virtual D-H coordinate system {2 a} should be located on the X1 axis and directly below the origin of coordinate system {2}; the direction of the Z axis of the virtual D-H coordinate system {2 a} is parallel to the direction of the Z axis of coordinate system {2}, and the direction of the X axis of coordinate system {2 a} satisfies the definition of the D-H method and can include the distance d2 between the origin of coordinate system {2} and the origin of coordinate system {1}. Therefore, the direction of the X axis of coordinate system {2 a} is vertically upward; Virtual D-H coordinate system {5 a} has its origin coinciding with that of coordinate system {5}, and the direction of its Z-axis is perpendicular to the Z-axis of coordinate system {5}. According to the D-H method definition, the direction of its X-axis is perpendicular to the paper, either inward or outward. The Z-axis of the virtual D-H coordinate system {5 a} is a supplementary description of the distance d5 between the origin of the basic D-H coordinate system {5} and the origin of coordinate system {4}; Step S4: On the basis of Step S3, use the D-H method to analyze the link parameters of the serial manipulator containing the virtual D-H coordinate system; Step S5: Using the improved D-H coordinate system, substitute the link parameters of the serial manipulator established in Step S4 based on the improved D-H coordinate system into the general transformation formula T of the manipulator link coordinate system {i} relative to the manipulator link coordinate system {i-1} i i-1 to establish the coordinate transformation matrix between two adjacent links of each joint of the serial manipulator Step S6: On the basis of Step S5, establish the description of the coordinate system {n} of the end link of the serial manipulator relative to the coordinate system {0} of the base, that is, the forward kinematics equation of the serial manipulator.

2. The forward kinematics modeling method of the serial manipulator based on the improved D-H method according to claim 1, characterized in that, In the said step S1, the X-axis X0 of the coordinate system {0} points to the end of the actuator, and the direction of the X-axis X8 of the coordinate system {8} is the same as that of the X-axis X0 of the coordinate system {0}; the remaining coordinate systems all satisfy that the X-axis X i-1 of the coordinate system {i - 1} i-1 coincides with the common perpendicular of the joint axis i - 1 and points from joint i - 1 to joint i. When the length a i-1 of the link i - 1 is 0, the X-axis X i-1 of the coordinate system {i - 1} is equal to ±Z i ×Z i-1 .

3. The forward kinematics modeling method of the serial manipulator based on the improved D-H method according to claim 2, characterized in that In the said Step S2, relative to the coordinate system {1}, the distance measured along X1 from Z1 to Z2 is L1, and the distance measured along Z2 from X1 to X2 is 0; at this time, according to the D-H method, the distance d2 between the origin of the coordinate system {2} and the origin of the coordinate system {1} is not reflected.

4. The forward kinematics modeling method of the serial manipulator based on the improved D-H method according to claim 2, characterized in that, In the said Step S2, relative to the coordinate system {4}, the distance measured along X4 from Z4 to Z5 is 0, and the distance measured along Z5 from X4 to X5 is 0; at this time, according to the D-H method, the distance d5 between the origin of the coordinate system {5} and the origin of the coordinate system {4} is not reflected.

5. The forward kinematics modeling method of the serial manipulator based on the improved D-H method according to claim 1, characterized in that, In the said Step S4, relative to the coordinate system {0}, the distance measured along X0 from Z0 to Z1 is 0, the angle of rotation around X0 from Z0 to Z1 is 0, the distance measured along Z1 from X0 to X1 is 0, and the angle of rotation around Z1 from X0 to X1 is π + θ1, where θ1 is a control variable; Coordinate system {2 a} is relative to coordinate system {1}, from Z1 to The distance measured along X1 is L1, from Z1 to The angle of rotation about X1 is From X1 to Along The measured distance is 0, from X1 to Around The angle of rotation is The coordinate system {2} is relative to the coordinate system {2 a}, from to Z2 along the measured distance is d2, from to Z2 around the rotation angle is 0, from to X2 along Z2 the measured distance is 0, from to X2 around Z2 the rotation angle is θ2, where θ2 is a control variable; The distance measured along X2 from Z2 to Z3 of coordinate system {3} relative to coordinate system {2} is 0, and the angle of rotation about X2 from Z2 to Z3 is The distance measured along Z3 from X2 to X3 is d3, and the angle of rotation about Z3 from X2 to X3 is where d3 is a control variable that includes the initial distance of the origin of coordinate system {3} relative to the origin of coordinate system {2}; The distance measured along X3 from Z3 to Z4 is 0, and the angle of rotation about X3 from Z3 to Z4 is 0 for coordinate system {4} with respect to coordinate system {3}. The distance measured along Z4 from X3 to X4 is 0, and the angle of rotation about Z4 from X3 to X4 is θ4, where θ4 is a control variable. Coordinate system {5 a} is relative to coordinate system {4}, the distance measured along X4 from Z4 to is 0, the angle of rotation about X4 from Z4 to is 0, the distance measured along from X4 to is d5, where d5 < 0, the angle of rotation about from X4 to is 0; The coordinate system {5} is relative to the coordinate system {5 a}, from to Z5 along the measured distance is 0, from to Z5 rotating around the angle of rotation is from to X5 along Z5 the measured distance is 0, from to X5 rotating around Z5 the angle of rotation is where θ5 is a control variable; Relative to the coordinate system {5}, the distance measured along X5 from Z5 to Z6 is L5, the angle of rotation around X5 from Z5 to Z1 is 0, the distance measured along Z6 from X5 to X6 is d6, and the angle of rotation around Z6 from X5 to X6 is 0, where d6 is a control variable including the initial distance of the origin of the coordinate system {6} relative to the origin of the coordinate system {5}; The distance measured along X6 from Z6 to Z7 of coordinate system {7} relative to coordinate system {6} is 0, and the angle of rotation about X6 from Z6 to Z7 is The distance measured along Z7 from X6 to X7 is 0, and the angle of rotation about Z7 from X6 to X7 is where θ7 is a control variable; The distance measured along X7 from Z7 to Z8 for coordinate system {8} relative to coordinate system {7} is L7, and the angle of rotation about X7 from Z7 to Z8 is The distance measured along Z8 from X7 to X8 is d8, and the angle of rotation about Z8 from X7 to X8 is 0.

6. The forward kinematics modeling method of the serial manipulator based on the improved D-H method according to claim 1, characterized in that In the said step S5, the transformation general formula T i i-1 is as follows: Among them, a i-1 represents the connecting rod length, α i-1 represents the connecting rod torsion angle, d i represents the distance between two adjacent connecting rods, θ i represents the included angle between two adjacent connecting rods.

7. The forward kinematics modeling method of the serial manipulator based on the improved D-H method according to claim 6, characterized in that In the step S5, the coordinate transformation matrix between two adjacent linkages of each joint of the serial manipulator is specifically as follows: the transformation matrix of the shoulder joint with respect to the base, that is, the coordinate system {1} with respect to the coordinate system {0}: The transformation matrix of the shoulder pitch joint relative to the shoulder yaw joint, that is, the coordinate system {2} relative to the coordinate system {1}: The transformation matrix of the shoulder roll joint relative to the shoulder pitch joint, that is, the coordinate system {3} relative to the coordinate system {2}: The transformation matrix of the elbow yaw joint relative to the shoulder roll joint, that is, the coordinate system {4} relative to the coordinate system {3}: The transformation matrix of the forearm rotary joint relative to the forearm deflection joint, i.e., the coordinate system {5} relative to the coordinate system {4}: The transformation matrix of the forearm translation joint relative to the forearm rotary joint, i.e., the coordinate system {6} relative to the coordinate system {5}: The transformation matrix of the forearm pitch joint relative to the forearm translation joint, i.e., the coordinate system {7} relative to the coordinate system {6}: The transformation matrix of the end of the robotic arm relative to the forearm pitch joint, i.e., the coordinate system {8} relative to the coordinate system {7}:

8. The forward kinematics modeling method of the serial manipulator based on the improved D-H method according to claim 7, characterized in that, In the step S5, the forward kinematic equation of the serial robotic arm is:

Images