Kinematics analysis method of new-configuration six-degree-of-freedom force feedback hand controller

Abstract

The invention relates to the technical field of robot man-machine interaction, in particular to a kinematics analysis method for a new-configuration six-degree-of-freedom force feedback hand controller, and the method comprises the steps: constructing a joint coordinate system of each rod piece of the hand controller; obtaining D-H parameters of a joint and a rod piece in the hand controller; performing joint processing on the kinematic model of the hand controller; obtaining a conversion matrix between adjacent joints; and the pose of the end effector of the hand controller in the base coordinate system is obtained. According to the method, kinematics modeling is carried out on the six-degree-of-freedom force feedback hand controller which is subjected to shaft shifting processing and added with the virtual joints by utilizing a D-H method, so that the six-degree-of-freedom force feedback hand controller obtained through modeling is more accurate.

Description

Technical Field

[0001] The present invention relates to the technical field of robot human - computer interaction, and particularly relates to a kinematic analysis method for a new - configuration six - degree - of - freedom force - feedback hand controller. Background Art

[0002] Robots play an increasingly important role in performing extreme tasks such as high temperature, high pressure, strong radiation, zero - gravity / micro - gravity, etc. The control terminal, abbreviated as the hand controller, serves as a human - machine interface in teleoperated robots and is an important medium for people to perceive the environment and control the slave robot. Therefore, hand - controller technology has become one of the key technologies in teleoperation technology.

[0003] To meet the requirements of different teleoperated robots, various hand - controller devices have been developed at home and abroad. The common classic hand - controller configurations are divided into series and parallel types. In a series mechanism, the relationships between components are clear and the motion control algorithm is relatively simple. However, the shapes of each joint and rod tend to be fixed, and due to the self - weight restriction of the manufacturing materials, it is difficult to balance the contradiction between expanding the working space and reducing the joint force. The parallel mechanism can decouple translation and rotation, which is beneficial to improving the motion accuracy, and at the same time has the characteristics of large mechanism stiffness and high load - bearing capacity. Based on this premise, more and more composite hand - controllers have emerged, that is, designs with both series and parallel structures, which can achieve a larger working range while ensuring motion controllability. After completing the mechanical structure design of the composite hand - controller, the subsequent problem is how to describe the geometric relationships between the joints, rods and the end - effector of the composite hand - controller through mathematical modeling, so as to provide a theoretical framework for subsequent motion control, path planning and dynamic analysis.

[0004] To plan the robot's actions, it is necessary to find the spatial descriptions of each joint and the end - effector relative to the fixed coordinate system. For this purpose, the existing technology has proposed the D - H modeling method, a technology for modeling the coordinate system of a robotic arm. This method is widely used in the representation and modeling of robot kinematics and is applicable to various coordinate systems, such as Cartesian, cylindrical, spherical and Euler coordinates. D - H modeling mainly involves the modeling of joints and rods and is applicable to robots with different structures and degrees of freedom. However, this method often cannot accurately describe the motion of each component when dealing with special - shaped rods and series - parallel composite structures, thus affecting the accuracy of the robot.

[0005] Therefore, a kinematic analysis method for a new - configuration six - degree - of - freedom force - feedback hand controller is needed to solve the above problems. Summary of the Invention

[0006] The present invention provides a kinematic analysis method for a new configuration six-degree-of-freedom force feedback hand controller. By performing axis shifting processing and adding virtual joints, it solves the problem that the existing D-H modeling method often cannot accurately describe the motion of each component when dealing with special-shaped rods and series-parallel composite structures, thus affecting the accuracy of the robot.

[0007] The forward kinematic analysis method for a new configuration six-degree-of-freedom force feedback hand controller of the present invention adopts the following technical solutions, including: Taking the axis of each joint on the rod as the Z axis and using the right-hand rule to construct the joint coordinate system of each rod; Based on the joint coordinate system of each rod, obtain the D-H parameters of the joints and rods in the hand controller; Construct a kinematic model of the hand controller. In the kinematic model of the hand controller, perform axis shifting processing on two adjacent joints whose pose relationship cannot be defined by the D-H method to obtain the target joints after axis shifting processing; add at least two virtual joints between the two joints of the parallelogram mechanism in the kinematic model of the hand controller, and add a virtual joint at the end of the end effector as the joint of the end effector; Use the D-H method to perform pose transformation on the target joints, virtual joints, and joints whose pose relationship can be defined by the D-H method to obtain the D-H parameters of each joint, and obtain the transformation matrix between adjacent joints based on the D-H parameters; Based on each transformation matrix, obtain the pose of the end effector of the hand controller in the base coordinate system, that is, complete the forward kinematic analysis of the hand controller.

[0008] Preferably, the steps of constructing the joint coordinate system of each rod are: Taking the axis of each joint on the rod as the Z axis, starting from the first rod connected to the base of the hand controller, successively find the X-axis direction and the coordinate origin of each rod, and use the right-hand rule to determine the Y axis, then the joint coordinate system of each rod can be obtained.

[0009] Preferably, the D-H parameters include: rod length, rod twist angle, joint distance, and joint rotation angle; Among them, the i rod length of the rod is: the distance along the X i-1 axis between the Z axes of the joint coordinate systems corresponding to the two joints of the rod; The i rod twist angle of the rod is: the rotation angle along the X i-1 axis between the Z axes of the joint coordinate systems corresponding to two adjacent joints of the rod; The i joint distance of the joint is: the distance along the Z i axis between the X axes of the joint coordinate systems corresponding to two adjacent joints of the rod; Joint i The joint angle of the joint is: the rotation angle along the Z i axis between the X axes of the joint coordinate systems corresponding to two adjacent joints of the rod.

[0010] Preferably, the steps for obtaining the target joint after axis shifting are as follows: Move the origin of the joint coordinate systems of two adjacent joints in the kinematic model of the hand controller that cannot define the pose relationship by the D-H method along their respective joint axes until the origin of the joint coordinate system coincides with the direction of the joint axis of the next joint that can define the pose relationship by the D-H method, and the target joint can be obtained.

[0011] Preferably, the expression of the transformation matrix between two adjacent joints is:

[0012] In the formula, represents the transformation matrix of the joint coordinate system { i} of the i rod relative to the joint coordinate system { i-1}; represents the i axis of the joint coordinate system { i -1} of the rod to the i axis of the joint coordinate system { } along the i axis of the joint coordinate system { -1} represents the i axis of the joint coordinate system { i -1} of the rod to the i axis of the joint coordinate system { } along the i axis of the joint coordinate system { -1} represents the i axis of the joint coordinate system { i -1} of the rod to the i axis of the joint coordinate system { } along the i axis of the joint coordinate system { } represents the i axis of the joint coordinate system { i -1} of the rod to the i axis of the joint coordinate system { } along the i axis of the joint coordinate system { The rotation angle in the axial direction.

[0013] Preferably, the pose expression of the end effector of the manual controller in the base coordinate system is:

[0014]

[0015] In the formula, represents the pose matrix of the joint coordinate system { } of the end effector with respect to the base coordinate system { 0}; represents the transformation matrix of the first joint coordinate system of the manual controller with respect to the base coordinate system { 0}; represents the transformation matrix of the second joint coordinate system of the manual controller with respect to the first joint coordinate system; represents the transformation matrix of the third joint coordinate system of the manual controller with respect to the second joint coordinate system; represents the transformation matrix of the fourth joint coordinate system of the manual controller with respect to the third joint coordinate system; represents the transformation matrix of the fifth joint coordinate system of the manual controller with respect to the fourth joint coordinate system; represents the transformation matrix of the first virtual joint coordinate system { } with respect to the fifth joint coordinate system of the manual controller; represents the transformation matrix of the second virtual joint coordinate system { } with respect to the first virtual joint coordinate system { }; represents the transformation matrix of the sixth joint coordinate system of the manual controller with respect to the second virtual joint coordinate system { }; represents the transformation matrix of the joint coordinate system { } of the end effector with respect to the sixth joint coordinate system; represents the projections of the x-axis of the end effector on the x, y, and z axes of the base coordinate system { 0}; represents the projections of the y-axis of the end effector on the x, y, and z axes of the base coordinate system { 0}; represents the projections of the z-axis of the end effector on the x, y, and z axes of the base coordinate system { 0}; represents the position projections of the coordinate origin of the end effector on the x, y, and z axes of the base coordinate system { 0}.

[0016] A reverse kinematics analysis method for a new configuration six-degree-of-freedom force feedback manual controller adopts the following technical solutions, including: Define the target pose of the end effector of the hand controller; Obtain the pose offset matrix corresponding to the target pose according to the current pose and the target pose; Obtain the Jacobian matrix of the pose offset matrix; Connect the principle of the power system to the Newton iteration method, and obtain the target Newton iteration method according to the Jacobian matrix and the introduced preset matrix parameters; Solve the inverse kinematics of the hand controller using the target Newton iteration method.

[0017] Preferably, the expression of the pose offset matrix is:

[0018] In the formula, represents the pose offset matrix between the target pose and the current pose; represents the element in the first row and first column of the target pose and the corresponding element in the first row and first column of the current pose the difference; represents the element in the first row and second column of the target pose and the corresponding element in the first row and second column of the current pose the difference; represents the element in the first row and third column of the target pose and the corresponding element in the first row and third column of the current pose the difference; represents the element in the first row and fourth column of the target pose and the corresponding element in the first row and fourth column of the current pose the difference; represents the element in the second row and first column of the target pose and the corresponding element in the second row and first column of the current pose the difference; represents the element in the second row and second column of the target pose and the corresponding element in the second row and second column of the current pose the difference; represents the element in the second row and third column of the target pose and the corresponding element in the second row and third column of the current pose the difference; represents the element in the second row and fourth column of the target pose and the corresponding element in the second row and fourth column of the current pose the difference; represents the element in the third row and first column of the target pose and the corresponding element in the third row and first column of the current pose the difference; The element in the second column of the third row representing the target pose And the corresponding element in the second column of the third row of the current pose The difference; The element in the third column of the third row representing the target pose And the corresponding element in the third column of the third row of the current pose The difference; The element in the fourth column of the third row representing the target pose And the corresponding element in the fourth column of the third row of the current pose The difference.

[0019] Preferably, the expression of the Jacobian matrix is:

[0020] In the formula, Represents the Jacobian matrix; Represents the first variable in the joint variables corresponding to the target pose; Represents the second variable in the joint variables corresponding to the target pose; Represents the sixth variable in the joint variables corresponding to the target pose.

[0021] Preferably, the expression of the target Newton iteration method is:

[0022] In the formula, Represents the difference matrix between the target pose and the current pose after the i-th iteration using the Newton iteration method; Represents the Jacobian matrix of the difference matrix between the target pose and the current pose after the i-th iteration using the Newton iteration method; Represents the joint variables corresponding to the current pose after the i-th iteration using the Newton iteration method; Represents a preset matrix parameter.

[0023] The beneficial effects of the present invention are: Based on the D-H method, for two adjacent joints whose pose relationship cannot be defined by the D-H method, axis shifting processing is performed. For a six-degree-of-freedom force feedback hand controller containing a parallelogram mechanism, the parallelogram mechanism changes the degrees of freedom of the original rods of the six-degree-of-freedom force feedback hand controller, that is, it restricts the rotational degree of freedom of the end of the rod of the six-degree-of-freedom force feedback hand controller and only retains the translational degree of freedom. That is, the setting of the parallelogram mechanism makes the six-degree-of-freedom force feedback hand controller contrary to the modeling idea of the D-H method. Therefore, the present invention adds a virtual joint between two joints of the parallelogram mechanism so that the six-degree-of-freedom force feedback hand controller after axis shifting processing and adding the virtual joint conforms to the modeling idea of the D-H method, and thus models the six-degree-of-freedom force feedback hand controller after axis shifting processing and adding the virtual joint on the basis of the D-H method, making the modeled six-degree-of-freedom force feedback hand controller more accurate. Brief Description of the Drawings

[0024] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for description in the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0025] Figure 1 It is a flowchart of the forward kinematics analysis method for a new configuration six-degree-of-freedom force feedback hand controller in an embodiment of the present invention; Figure 2 It is a schematic diagram of the mechanism principle of the six-degree-of-freedom force feedback hand controller in an embodiment of the present invention; Figure 3 It is a coordinate system establishment result diagram of the second and third joints of the six-degree-of-freedom force feedback hand controller in this embodiment based on the MDH method of the prior art; Figure 4 It is a result diagram of axis shifting processing of the second and third joints of the six-degree-of-freedom force feedback hand controller in step S3 of the present invention; Figure 5 It is a coordinate system establishment result diagram of the fourth, fifth, and sixth joints of the six-degree-of-freedom force feedback hand controller in this embodiment based on the MDH method of the prior art; Figure 6 It is a result diagram of adding a virtual joint between the fifth and sixth joints of the six-degree-of-freedom force feedback hand controller in step S3 of the present invention; Figure 7 It is a schematic diagram of the D-H parameters of the joints describing the rods in an embodiment of the present invention; Figure 8 It is a result diagram of modeling the hand controller after axis shifting processing and adding a virtual joint using the D-H method. Detailed implementation manners

[0026] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0027] An embodiment of the forward kinematics analysis method of a new configuration six-degree-of-freedom force feedback hand controller of the present invention. In this embodiment, first, Figure 2 The mechanism analysis of the new configuration six-degree-of-freedom force feedback hand controller shown is carried out. This mechanism belongs to the series hand controller in the broad sense. However, compared with the traditional same-type mechanism, it also adds a special-shaped rigid rod structure and a parallelogram mechanism. Therefore, the traditional D-H method fails when dealing with such structures. In order to accurately perform kinematic analysis on this new configuration force feedback hand controller, it is necessary to improve and optimize the conventional D-H method. The core idea is to perform axis shifting processing on some joints and add appropriate virtual joints. Therefore, as Figure 1 shown, this embodiment specifically includes: S1. Construct the joint coordinate systems of each rod of the hand controller; Specifically, take the axis of each joint on the rod as the Z-axis, and use the right-hand rule to construct the joint coordinate systems of each rod.

[0028] Exemplarily, in a specific embodiment, define 1 to n joints and rods. Take the axis of each joint on the rod as the Z-axis. Starting from the joint i = 1 of the first rod, find the X-axis direction and the coordinate origin of each rod in turn, and use the right-hand rule to determine the Y-axis to establish each joint coordinate system {i = 1~n}.

[0029] It should be noted that the joints of the hand controller are driven by motors, and the relative movement between the rods is realized by the joint movement; the rods keep the joint axes at both ends having a fixed geometric relationship, and its characteristics are also defined by these two axes.

[0030] S2. Obtain the D-H parameters of the rods and joints in the hand controller; Specifically, based on the joint coordinate systems of each rod, obtain the D-H parameters of the rods in the hand controller. Among them, the D-H parameters include: rod length, rod twist angle, joint distance, and joint rotation angle.

[0031] Exemplarily, in a specific embodiment, as Figure 7 shown, the rod i The rod length is: the rod i The corresponding joint coordinate system { ito the joint coordinate system { axis to the joint coordinate system { i} of the axis along the joint coordinate system { i to the joint coordinate system { axis; the distance of the rod i twist angle of the rod is: the rod i corresponding joint coordinate system { i to the joint coordinate system { axis to the joint coordinate system { i} of the axis along the joint coordinate system { i to the joint coordinate system { axis; the rotation angle of the joint i distance of the joint is: the rod i corresponding joint coordinate system { i to the joint coordinate system { axis to the joint coordinate system { i} of the axis along the joint coordinate system { i} of the axis direction; the joint i rotation angle of the joint is: the rod i corresponding joint coordinate system { i to the joint coordinate system { axis to the joint coordinate system { i} of the axis along the joint coordinate system { i} of the axis direction.

[0032] S3. Perform joint processing on the kinematic model of the hand controller; Specifically, construct the kinematic model of the hand controller, and perform axis shifting processing on two adjacent joints in the kinematic model of the hand controller that cannot define the pose relationship through the D-H method to obtain the target joints after axis shifting processing; add at least two virtual joints between the two joints of the parallelogram mechanism in the kinematic model of the hand controller, and add a virtual joint at the end of the end effector as the joint of the end effector.

[0033] For the D-H method, is expressed as the pose description of the joint coordinate system {i} relative to the joint coordinate system {i - 1}, that is, the transformation matrix of the joint coordinate system {i} relative to the joint coordinate system {i - 1}, which can be regarded as obtained from the joint coordinate system {i - 1} through the following transformations: rotate axis by angle, move along axis by , rotate axis by Angle, along Axis movement , expressed by the formula as: (1) In the formula, Indicates rotation around Axis by Angle; Indicates movement along Axis by ; Indicates rotation around Axis by Angle; Along Axis movement .

[0034] Then, according to formula (1), the general formula for joint transformation is: (2) In the formula, Indicates the transformation matrix of the joint coordinate system { i} of the i th link relative to the joint coordinate system { i-1}; Indicates the i th link i -1} of the corresponding joint coordinate system { Axis to the joint coordinate system { i} of the Axis along the joint coordinate system { i -1} of the Axis direction distance; Indicates the i th link i -1} of the corresponding joint coordinate system { Axis to the joint coordinate system { i} of the Axis along the joint coordinate system { i -1} of the Axis direction rotation angle; Indicates the i th link i -1} of the corresponding joint coordinate system { Axis to the joint coordinate system { i} of the Axis along the joint coordinate system { i} of the Axis direction distance; Indicates the i th link i -1} of the corresponding joint coordinate system { Axis to the joint coordinate system { i} of the The rotation angle of the axis along the joint coordinate system { i} in the direction of the axis; Then the pose of the end effector of the hand controller in the base coordinate system can be described as: (3) Step 31. The steps of performing axis shifting on two adjacent joints whose pose relationship cannot be defined by the D-H method to obtain the target joint after axis shifting are as follows: As Figure 2 shown, for the six-degree-of-freedom force feedback hand controller of this embodiment, the conversion between some adjacent two joints cannot be achieved through the joint transformation of the traditional D-H method. If the coordinate system is established according to the conventional improved D-H method (MDH method), the result shown in Figure 3 will be obtained (the D-H parameters are shown in Table 1). Since the transformation order of the MDH method is "rotate along the axis by → move along the axis by → rotate around the axis by → move along the axis by ", the position of the fourth joint has a serious deviation based on this transformation.

[0035] Table 1

[0036] Therefore, in this embodiment, two adjacent joints whose pose relationship cannot be defined by the D-H method are subjected to axis shifting to obtain the target joint after axis shifting. Specifically, in this embodiment, the second and third joints are subjected to axis shifting. Among them, the steps of axis shifting are as follows: Combining the analysis of the structural characteristics of the six-degree-of-freedom force feedback hand controller itself, move the origin of the joint coordinate system of two adjacent joints whose pose relationship cannot be defined by the D-H method along their respective joint axes until the origin of the joint coordinate system coincides with the direction of the joint axis of the next joint that can be defined by the D-H method, and the target joint can be obtained, that is, the target joint that can be described by the D-H method can be obtained.

[0037] As Figure 4 shown, the D-H parameters after axis shifting are shown in Table 2. Based on this, the kinematic model well describes the motion relationship between the joints, linkages and the pose of the end effector in this case without changing the basic principle of the D-H method.

[0038] Table 2

[0039] Step 32. The steps of adding at least two virtual joints between two joints of the parallelogram mechanism in the hand controller are as follows: Since the hand controller of this embodiment includes a parallelogram mechanism, this parallelogram mechanism changes the degrees of freedom of the original rod members, that is, it restricts the rotational degree of freedom of the end effector of the rod member and only retains the translational degree of freedom, which is also contrary to the modeling idea of the D-H method. For example, for the coordinate system transformation from the fifth joint to the sixth joint, if the coordinate system is established according to the conventional method, a coordinate system position similar to Figure 5 x6’-z6’ in [reference] will be obtained, which does not conform to the actual situation of the sixth joint and thus cannot be used.

[0040] Exemplarily, in a specific embodiment, in order to adapt to the characteristics of the parallelogram structure while following the principle of minimum change, this embodiment proposes to use the method of adding virtual joints to adapt to the basic theoretical framework of the D-H method, that is, as Figure 5 shown, a first virtual joint and a second virtual joint are added between the fifth joint and the sixth joint to achieve the transition from the fifth joint to the sixth joint, and a third virtual joint is added at the end of the end effector as the joint of the end effector . For the added virtual joints, their D-H parameters are either fixed values or represented by actual joint angles.

[0041] S4. Obtain the transformation matrix between adjacent joints; Specifically, the D-H method is used to perform pose transformation on the target joint, virtual joint, and joints that can define the pose relationship through the D-H method to obtain the D-H parameters of each joint, and the transformation matrix between adjacent joints is obtained based on the D-H parameters.

[0042] S41. Use the D-H method to perform pose transformation on the target joint, virtual joint, and joints that can define the pose relationship through the D-H method to obtain the D-H parameters of each joint, that is, use the D-H method to model the hand controller after axis shifting and adding virtual joints, and the result is as Figure 8 shown.

[0043] S42. Substitute the D-H parameters obtained in step S41 into the transformation general formula (2) to obtain the transformation matrix of each joint of the hand controller.

[0044] (4) (5) (6) (7) (8) (9) (10) (11) (12) In the formula, represents the pose matrix of the joint coordinate system { } of the end effector with respect to the base coordinate system { 0 }; represents the transformation matrix of the first joint coordinate system of the hand controller with respect to the base coordinate system { 0 }; represents the transformation matrix of the second joint coordinate system of the hand controller with respect to the first joint coordinate system; represents the transformation matrix of the third joint coordinate system of the hand controller with respect to the second joint coordinate system; represents the transformation matrix of the fourth joint coordinate system of the hand controller with respect to the third joint coordinate system; represents the transformation matrix of the fifth joint coordinate system of the hand controller with respect to the fourth joint coordinate system; represents the transformation matrix of the first virtual joint coordinate system { } with respect to the fifth joint coordinate system of the hand controller; represents the transformation matrix of the second virtual joint coordinate system { } with respect to the first virtual joint coordinate system { }; represents the transformation matrix of the sixth joint coordinate system of the hand controller with respect to the second virtual joint coordinate system { }; represents the transformation matrix of the joint coordinate system { } of the end effector with respect to the sixth joint coordinate system.

[0045] S5. Obtain the pose of the end effector of the hand controller in the base coordinate system; Specifically, based on each transformation matrix, obtain the pose of the end effector of the hand controller in the base coordinate system, that is, complete the forward kinematic analysis of the hand controller.

[0046] Exemplarily, substitute the transformation matrices between each adjacent joint coordinate system obtained in step 4 into formula (3) to obtain the pose of the end effector of the hand controller with respect to the base. Among them, the pose expression of the end effector of the six-degree-of-freedom force feedback hand controller of this embodiment with respect to the base is: (13) (14) After simplification, it can be obtained: (15) In the formula, Denote the projections of the x-axis of the end effector on the x, y, and z axes of the base coordinate system { 0}; Denote the projections of the y-axis of the end effector on the x, y, and z axes of the base coordinate system { 0}; Denote the projections of the z-axis of the end effector on the x, y, and z axes of the base coordinate system { 0}; Denote the position projections of the origin of coordinates of the end effector on the x, y, and z axes of the base coordinate system { 0}; s i = sinθ i ，c i = cosθ i ， Among them, i= 1, 2, 3, 4, 5, 6; s 45 = sin(θ 4 + θ 5 )，c 45 = cos(θ 4 + θ 5 ),θ i is the joint coordinate system { i} corresponding to the link i -1 axis to the i axis of the joint coordinate system { } along the i -1 axis direction of the joint coordinate system {

[0047] An embodiment of the inverse kinematics analysis method for a new configuration six-degree-of-freedom force feedback hand controller. Since the mechanical structure of the six-degree-of-freedom force feedback hand controller in this embodiment is complex, the method of deriving the analytical solution using the standard D-H inverse kinematics formula is partially invalid, that is, only the joint variables of the second, third, and fourth joints can be obtained, as shown in equations (16) to (17), and the analytical solutions of the other three joint variables cannot be obtained. Therefore, the core idea of this embodiment is to use the "Newton type" iteration to seek the numerical solution. Among them, the method of deriving the analytical solution using the standard D-H inverse kinematics formula is used to solve the joint variables of the second, third, and fourth joints: (16) (17) (18) Therefore, this embodiment specifically includes: Step 1: Define the target pose of the end effector of the manual controller; First, set as the target pose of the end effector of the manual controller, and

[0048] as the current pose of the end effector of the manual controller. Exemplarily, in a specific embodiment, represents the joint variable, represents the pose offset matrix, and construct a non - linear equation system of the pose offset matrix: , that is: (19) In the formula, represents the pose offset matrix between the target pose and the current pose; represents the element in the first row and first column of the target pose and the corresponding element in the first row and first column in the current pose; represents the element in the first row and second column of the target pose and the corresponding element in the first row and second column in the current pose; represents the element in the first row and third column of the target pose and the corresponding element in the first row and third column in the current pose; represents the element in the first row and fourth column of the target pose and the corresponding element in the first row and fourth column in the current pose; represents the element in the second row and first column of the target pose and the corresponding element in the second row and first column in the current pose; represents the element in the second row and second column of the target pose and the corresponding element in the second row and second column in the current pose; represents the element in the second row and third column of the target pose and the corresponding element in the second row and third column in the current pose; represents the element in the second row and fourth column of the target pose and the corresponding element in the second row and fourth column in the current pose; represents the element in the third row and first column of the target pose The difference between the element in the first column of the third row corresponding to the current pose ; The element in the second column of the third row representing the target pose And the element in the second column of the third row corresponding to the current pose ; The element in the third column of the third row representing the target pose And the element in the third column of the third row corresponding to the current pose ; The element in the fourth column of the third row representing the target pose And the element in the fourth column of the third row corresponding to the current pose .

[0049] Then the expression of the Jacobian matrix of the pose offset matrix is: (20) In the formula, Represents the Jacobian matrix; Represents the first variable in the joint variables corresponding to the target pose; Represents the second variable in the joint variables corresponding to the target pose; Represents the sixth variable in the joint variables corresponding to the target pose.

[0050] It should be noted that the virtual joints assumed in the forward kinematics analysis , And The corresponding joint angles are constants or can be represented by known variables, so they are not used as the independent variables for taking partial derivatives in the Jacobian matrix.

[0051] Step 3: Incorporate the principle of the dynamic system into the Newton iteration method, and based on the Jacobian matrix and the introduced preset matrix parameters, obtain the target Newton iteration method; Exemplarily, in a specific embodiment, the Newton iteration formula for solving the system of equations in the Newton iteration method is: (21) In the formula, Represents the difference matrix between the target pose and the current pose after the i-th iteration using the Newton iteration method; Represents the Jacobian matrix of the difference matrix between the target pose and the current pose after the i-th iteration using the Newton iteration method; Represents the joint variables corresponding to the current pose after the i-th iteration using the Newton iteration method.

[0052] Referring to incorporating the principle of the dynamic system into the Newton iteration method, introducing the matrix parameter , is an adjustable factor, and the improved method of the Newton iteration method, that is, the expression of the target Newton iteration method is: (22) Since is a 12×6 matrix and its inverse matrix cannot be obtained, only the pseudo-inverse can be calculated, so equation (22) becomes: (23) Step 4: Use the target Newton iteration method to solve the inverse kinematics of the hand controller, that is, use formula (23) to solve the values of the joint variables of each joint of the end effector of the hand controller corresponding to the target pose.

[0053] The following verifies the kinematic analysis method of the hand controller in combination with specific simulation data: I. Forward kinematics verification: The D-H parameters of the hand controller in this embodiment are shown in Table 3. To verify the correctness of the forward kinematics analysis method, a program is written in MATLAB according to the derivation process Table 3

[0054] The input of the program is the joint parameters of the six-degree-of-freedom force-feedback hand controller , and the output is the pose matrix of the end effector in the space coordinate system. Assuming that the joint parameters of the given device are , the output result, that is, the pose matrix of the end effector of the hand controller, is: (24) The correctness of this result is verified by three-dimensional model measurement, indicating that this method can appropriately describe the geometric relationship between the joints, linkages and end effector of the compound hand controller.

[0055] II. Inverse kinematics verification: To verify the correctness and reliability of the above inverse kinematics solution method, an inverse kinematics solution program and a motion planning program are written in MATLAB to set and approach the target pose, and the target position point and the simulation trajectory are made in three-dimensional space, and the target position and the trajectory end point are compared.

[0056] The input condition of the program is the homogeneous transformation matrix of the target pose , and the output is the values of the joint parameters when reaching the target pose. Regarding parameter settings, , the accuracy requirement is , and the maximum number of iterations is 100 times.

[0057] In this simulation, the target pose input by the program is: (25) When the starting pose is known, the coordinates of the starting point of the end effector trajectory in the space coordinate system are (140.0, -327.6, 290.0), and the expected output result is . After 4 iterations, the actual output result is , and the deviation from the expected result is 0.000448; without introducing the parameter matrix, with the same 4 iterations, the accuracy of the output result is 0.003411. In summary, the effective introduction of the parameter matrix enables the output result to have higher accuracy under the same number of iterations in most cases.

[0058] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A forward kinematics analysis method for a new configuration six-degree-of-freedom force feedback hand controller, characterized in that Including: Taking the axis of each joint on the rod as the Z-axis and using the right-hand rule to construct the joint coordinate system of each rod; Based on the joint coordinate system of each rod, obtaining the D-H parameters of the joints and rods in the hand controller; Constructing the kinematic model of the hand controller, performing axis-shifting processing on two adjacent joints in the hand controller kinematic model that cannot define the pose relationship by the D-H method to obtain the target joints after axis-shifting processing; adding at least two virtual joints between the two joints of the parallelogram mechanism in the hand controller kinematic model, and adding a virtual joint at the end of the end effector as the joint of the end effector; Using the D-H method to perform pose transformation on the target joints, virtual joints, and joints that can define the pose relationship by the D-H method to obtain the D-H parameters of each joint, and obtaining the transformation matrix between adjacent joints based on the D-H parameters; Based on each transformation matrix, obtaining the pose of the end effector of the hand controller in the base coordinate system, that is, completing the forward kinematic analysis of the hand controller.

2. The forward kinematics analysis method of a new configuration six-degree-of-freedom force feedback hand controller according to claim 1, characterized in that The steps to construct the joint coordinate system of each rod are: Taking the axis of each joint on the rod as the Z-axis, starting from the first rod connected to the base of the hand controller, successively finding the X-axis direction and the coordinate origin of each rod, and using the right-hand rule to determine the Y-axis, then the joint coordinate system of each rod can be obtained.

3. The forward kinematics analysis method of a new configuration six-degree-of-freedom force feedback hand controller according to claim 1, characterized in that, The D-H parameters include: rod length, rod twist angle, joint distance, and joint rotation angle; Among them, the rod member i The length of the rod member is: the distance along the X i-1 axis between the Z axes of the joint coordinate systems corresponding to the two joints of the rod member; Bar i The torsional angle of the bar is: the rotation angle along the X-axis direction between the Z-axes of the joint coordinate systems corresponding to two adjacent joints of the bar; i-1 axis direction; Joint i The joint distance of the joint i is: the distance along the Z-axis between the X-axes of the joint coordinate systems corresponding to two adjacent joints of the rod i axis direction; Joint i The joint rotation angle of the joint is: the rotation angle along the Z i axis direction between the X axes of the joint coordinate systems corresponding to two adjacent joints of the rod.

4. The kinematic analysis method of a new configuration six-degree-of-freedom force feedback hand controller according to claim 1, characterized in that, The steps to perform axis-shifting processing to obtain the target joints after axis-shifting processing are: Moving the origin of the joint coordinate system of two adjacent joints in the hand controller kinematic model that cannot define the pose relationship by the D-H method along their respective joint axes until the origin of the joint coordinate system coincides with the direction of the joint axis of the next joint that can define the pose relationship by the D-H method, then the target joints can be obtained.

5. The kinematic analysis method of a new configuration six-degree-of-freedom force feedback hand controller according to claim 1, characterized in that, The expression of the transformation matrix between two adjacent joints is: In the formula, represents the transformation matrix of the joint coordinate system { i} of the i th link relative to the joint coordinate system { i-1}; represents the distance from the i axis of the corresponding joint coordinate system { i -1} of the link to the i axis of the joint coordinate system { } along the direction of the i axis of the joint coordinate system { -1}; represents the rotation angle from the i axis of the corresponding joint coordinate system { i -1} of the link to the i axis of the joint coordinate system { } along the direction of the i axis of the joint coordinate system { -1}; represents the distance from the i axis of the corresponding joint coordinate system { i -1} of the link to the i axis of the joint coordinate system { } along the direction of the i axis of the joint coordinate system { }; represents the rotation angle from the i axis of the corresponding joint coordinate system { i -1} of the link to the i axis of the joint coordinate system { } along the direction of the i axis of the joint coordinate system { }.

6. The forward kinematics analysis method of a new configuration six-degree-of-freedom force feedback hand controller according to claim 1, characterized in that, The pose expression of the end effector of the hand controller in the base coordinate system is: In the formula, represents the pose matrix of the joint coordinate system { } of the end effector with respect to the base coordinate system { 0}; represents the transformation matrix of the first joint coordinate system of the hand controller with respect to the base coordinate system { 0}; represents the transformation matrix of the second joint coordinate system of the hand controller with respect to the first joint coordinate system; represents the transformation matrix of the third joint coordinate system of the hand controller with respect to the second joint coordinate system; represents the transformation matrix of the fourth joint coordinate system of the hand controller with respect to the third joint coordinate system; represents the transformation matrix of the fifth joint coordinate system of the hand controller with respect to the fourth joint coordinate system; represents the transformation matrix of the first virtual joint coordinate system { } with respect to the fifth joint coordinate system of the hand controller; represents the transformation matrix of the second virtual joint coordinate system { } with respect to the first virtual joint coordinate system { }; represents the transformation matrix of the sixth joint coordinate system of the hand controller with respect to the second virtual joint coordinate system { }; represents the transformation matrix of the joint coordinate system { } of the end effector with respect to the sixth joint coordinate system; represents the projections of the x-axis of the end effector on the x, y, and z axes of the base coordinate system; represents the projections of the y-axis of the end effector on the x, y, and z axes of the base coordinate system; represents the projections of the z-axis of the end effector on the x, y, and z axes of the base coordinate system; represents the position projections of the origin of the end effector coordinate system on the x, y, and z axes of the base coordinate system.

7. A method for inverse kinematics analysis of a six-degree-of-freedom force feedback hand controller with a new configuration, characterized in that, Including: Defining the target pose of the end effector of the hand controller; Obtaining the pose offset matrix corresponding to the target pose according to the current pose and the target pose; Obtaining the Jacobian matrix of the pose offset matrix; Connecting the principle of the dynamic system to the Newton iteration method, and obtaining the target Newton iteration method according to the Jacobian matrix and introducing preset matrix parameters; Using the target Newton iteration method to solve the inverse kinematics of the hand controller.

8. A kinematic analysis method for a new configuration six-degree-of-freedom force feedback hand controller according to claim 7, characterized in that The expression of the pose offset matrix is: In the formula, represents the pose offset matrix between the target pose and the current pose; represents the element in the first row and first column of the target pose and the corresponding element in the first row and first column of the current pose The difference; represents the element in the first row and second column of the target pose and the corresponding element in the first row and second column of the current pose The difference; represents the element in the first row and third column of the target pose and the corresponding element in the first row and third column of the current pose The difference; represents the element in the first row and fourth column of the target pose and the corresponding element in the first row and fourth column of the current pose The difference; represents the element in the second row and first column of the target pose and the corresponding element in the second row and first column of the current pose The difference; represents the element in the second row and second column of the target pose and the corresponding element in the second row and second column of the current pose The difference; represents the element in the second row and third column of the target pose and the corresponding element in the second row and third column of the current pose The difference; represents the element in the second row and fourth column of the target pose and the corresponding element in the second row and fourth column of the current pose The difference; represents the element in the third row and first column of the target pose and the corresponding element in the third row and first column of the current pose The difference; represents the element in the third row and second column of the target pose and the corresponding element in the third row and second column of the current pose The difference; represents the element in the third row and third column of the target pose and the corresponding element in the third row and third column of the current pose The difference; represents the element in the third row and fourth column of the target pose and the corresponding element in the third row and fourth column of the current pose The difference.

9. A forward kinematics analysis method for a new configuration six-degree-of-freedom force feedback hand controller according to claim 8, characterized in that, The expression of the Jacobian matrix is: In the formula, represents the Jacobian matrix; represents the first variable among the joint variables corresponding to the target pose; represents the second variable among the joint variables corresponding to the target pose; represents the sixth variable among the joint variables corresponding to the target pose.

10. A forward kinematics analysis method for a new configuration six-degree-of-freedom force feedback hand controller according to claim 7, characterized in that The expression of the target Newton iteration method is: In the formula, represents the difference matrix between the target pose and the current pose after the i-th iteration using the Newton iteration method; represents the Jacobian matrix of the difference matrix between the target pose and the current pose after the i-th iteration using the Newton iteration method; represents the joint variables corresponding to the current pose after the i-th iteration using the Newton iteration method; represents the preset matrix parameter.

Images