A method for solving the motion of a closed-chain spatial mechanism with few degrees of freedom

Abstract

The present application relates to a kind of few degrees of freedom closed-chain space mechanism motion solution method, comprising the following steps: step one: the kinematic analysis of mechanism is carried out, and the main open-chain mechanism model and slave open-chain mechanism model are established, and the position relationship between each component is described;Step two: coupling kinematic analysis is carried out, and the coupling motion relationship between the end pose of main open-chain mechanism and the end pose of slave open-chain mechanism is established;Step three: specify the desired trajectory of main open-chain mechanism, then the forward kinematics of main open-chain mechanism is solved, and the end pose of main open-chain mechanism is obtained;Step four: the end pose of slave open-chain mechanism is obtained, then inverse kinematics is solved to obtain each joint value of slave open-chain mechanism;Step five: using online trajectory generation method carries out trajectory planning, then using closed-loop control method carries out position control to each motor in few degrees of freedom master-slave single closed-chain mechanism.The present application realizes the real-time motion solution of few degrees of freedom master-slave single closed-chain mechanism, and realizes synchronous motion control.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of mechanism motion control, in particular to a motion solving method for a few-degree-of-freedom closed-chain spatial mechanism. BACKGROUND

[0002] At present, mechanical mechanisms applied in many occasions are designed as open-chain structures, and the kinematics and dynamics of such open-chain mechanisms have been mature. However, in practical applications, mechanical devices often only need part of the degrees of freedom, for example, 2-4 degrees of freedom, to meet the use requirements, considering the control performance, structural rigidity, task requirements and other reasons. Such closed-chain mechanism with less than 6 degrees of freedom is called a few-degree-of-freedom closed-chain spatial mechanism. Compared with open-chain mechanical structures, such mechanical structures have greater design diversity, but have fewer controllable degrees of freedom, and thus the kinematics and static force analysis are more complex, and there are problems of difficult solving and accurate control. SUMMARY

[0003] The purpose of the present application is to provide a motion solving method for a few-degree-of-freedom closed-chain spatial mechanism, which can realize real-time motion solving of a few-degree-of-freedom master-slave single closed-chain mechanism, and realize synchronous motion control of the few-degree-of-freedom master-slave single closed-chain mechanism through online trajectory generation and closed-loop control.

[0004] The purpose of the present application is achieved by the following technical solutions:

[0005] A motion solving method for a few-degree-of-freedom closed-chain spatial mechanism, comprising the following steps:

[0006] Step 1: kinematic analysis of the few-degree-of-freedom master-slave single closed-chain mechanism, establishment of master open-chain mechanism model and slave open-chain mechanism model, and description of the positional relationship between each component;

[0007] Step 2: coupled kinematic analysis of the master open-chain mechanism end and the slave open-chain mechanism end, establishment of the coupled motion relationship between the master open-chain mechanism end and the slave open-chain mechanism end pose;

[0008] Step 3: specification of the desired trajectory of the master open-chain mechanism, then forward kinematics solving of the master open-chain mechanism to obtain the master open-chain mechanism end pose;

[0009] Step 4: obtaining of the slave open-chain mechanism end pose according to the coupled motion relationship in Step 2 and the master open-chain mechanism end pose in Step 3, and then inverse kinematics solving to obtain each joint value of the slave open-chain mechanism;

[0010] Step 5: trajectory planning using an online trajectory generation method, and then position control of each motor in the few-degree-of-freedom master-slave single closed-chain mechanism using a closed-loop control method.

[0011] In step one, a fixed coordinate system w is defined as a world coordinate system, then parameters of each component are measured, coordinate system models of each component of the master open chain mechanism and the slave open chain mechanism are established, and Denabit-Hartenberg (DH) method requiring only four parameters is used to describe relative position relationship of each component, so as to obtain a pose relationship matrix between coordinate systems of two adjacent components

[0012]

[0013] In the above formula (7), α i-1 represents an angle of rotation around the x-axis of the coordinate system {i-1}, a i-1 represents a distance of movement along the x-axis of the coordinate system {i-1}, θ i represents an angle of rotation around the z-axis of the coordinate system {i}, d i represents a distance of movement along the z-axis of the coordinate system {i}.

[0014] In step one, the pose transformation matrix between coordinate systems of two adjacent components is expressed as:

[0015]

[0016] In the above formula (2), Rot(x, α i-1 ) represents rotation of a i-1 angle around the x-axis of the coordinate system {i-1}, Trans(x, a i-1 ) represents movement of a i-1 distance along the x-axis of the coordinate system {i-1}, Rot(z, θ i ) represents rotation of θ i angle around the z-axis of the coordinate system {i}, Trans(z, d i ) represents movement of d i distance along the z-axis of the coordinate system {i}, and the translation and rotation transformation matrix of the space coordinate system is expressed as follows:

[0017]

[0018]

[0019]

[0020]

[0021] Substituting the above formulas (3)-(6) into formula (2), the following is obtained:

[0022]

[0023] In step two, the end pose of the master open-chain mechanism coincides with the end pose of the slave open-chain mechanism, so:

[0024]

[0025] In the above formula (8), w is the world coordinate system, ai is the i-th coordinate system of the master open-chain mechanism, pi is the i-th coordinate system of the slave open-chain mechanism, and T represents the coordinate system pose transformation relationship of a group of adjacent components, i.e., the above formula (7).

[0026] In step three, the desired trajectory of the master open-chain mechanism is specified, i.e., the joint values θ ai of the master open-chain mechanism are obtained

[0027]

[0028] The end pose of the master open-chain mechanism relative to the world coordinate system is obtained

[0029] In step four, the end pose of the slave open-chain mechanism relative to the world coordinate system is obtained The inverse kinematics of the following formula is solved:

[0030] The joint values θ pi of the slave open-chain mechanism are derived, i.e., i = 1, 2, 3...

[0031] In step four, assuming that the end coordinate is represented by the coordinate vector x and its forward kinematics equation x = f(θ), a nonlinear vector equation from joint coordinates to end coordinates is obtained, where f: R n → R m is differentiable, and x d is the end coordinate system to be solved, i.e., x d is the end pose of the slave open-chain mechanism. According to the Newton-Raphson equation g(θ) = x d -f(θ), the root of the following equation is solved:

[0032] g(θ) = x d -f(θ) = 0 (17);

[0033] The iterative expression for solving the joint angle is obtained:

[0034] θ i+1 = θ i + Δθ i (22); and

[0035] The above formula (22) is repeatedly iterated to generate a series of θ values {θ 0 , θ 1 , θ 2...}, finally converging at the joint solution θ d

[0036] In step four, g(θ) = x d -f(θ) = 0 is solved as follows:

[0037] Given the initial joint values θ 0 , the kinematics equation f(θ) is written in the form of Taylor expansion:

[0038]

[0039] The above equation (18) only takes the first term of the Taylor series, and is equivalent to the Jacobian J(θ 0 ) ∈ R 0 at θ m×n , the above equation (18) can be further simplified as:

[0040] J(θ 0 ) Δθ = X d -f(θ 0 ) (19) ;

[0041] If J(θ 0 ) is a square matrix and invertible, solve Δθ using the following equation:

[0042] Δθ = J -1 (θ 0 ) X d -f(θ 0 ) (20) ;

[0043] Otherwise, use the pseudo-inverse instead of J -1 (θ), where is calculated by:

[0044] J is n > m

[0045] J is n < m;

[0046] After replacing the pseudo-inverse for J -1 (θ), the above equation (20) becomes:

[0047]

[0048] Finally, the iterative expression for solving the joint angle is obtained:

[0049] θ i+1 = θ i + Δθ i (22). ​

[0050] In step five, the trajectory planning is performed using an online trajectory generation method to obtain the position function p(t) of each joint of the master open-chain mechanism and the slave open-chain mechanism from the current value to the target value with respect to time, and then the expected value of each joint of the master open-chain mechanism in step three and the value of each joint of the slave open-chain mechanism obtained in step four are taken as the input value of the trajectory planning, and the output value of the trajectory planning is input into the closed-loop position controller to realize the closed-loop control of the motor.

[0051] The present application has the following advantages and positive effects:

[0052] 1. The present application takes the single closed-chain mechanism with few degrees of freedom as the research object, analyzes the motion form, splits the single closed-chain mechanism with few degrees of freedom into the master open-chain mechanism and the slave open-chain mechanism fixed at the end, obtains the coupling motion relationship between the master open-chain and the slave open-chain, and then performs the forward kinematics calculation on the master open-chain and the inverse kinematics calculation on the slave open-chain according to the coupling motion relationship and the expected position of each joint of the master open-chain, so as to realize the real-time motion calculation of the master-slave single closed-chain mechanism with few degrees of freedom.

[0053] 2. The present application can output the expected position of the mechanism in real time, and realize the synchronous motion control of the master-slave single closed-chain mechanism with few degrees of freedom through online trajectory generation and closed-loop control. BRIEF DESCRIPTION OF DRAWINGS

[0054] Figure 1 The figure is a flowchart of the present application,

[0055] Figure 2 The figure is a schematic diagram of a single closed-chain mechanism with few degrees of freedom,

[0056] Figure 3 The figure is Figure 2 a schematic diagram of the master open-chain mechanism after splitting the mechanism,

[0057] Figure 4 The figure is Figure 2 a schematic diagram of the slave open-chain mechanism after splitting the mechanism,

[0058] Figure 5 The figure is Figure 3 a schematic diagram of the coordinate system model of the master open-chain mechanism,

[0059] Figure 6 The figure is Figure 4 a schematic diagram of the coordinate system model of the slave open-chain mechanism,

[0060] Figure 7 The figure is a schematic diagram of trajectory online planning,

[0061] Figure 8 The figure is a schematic diagram of the control process of the motion control system,

[0062] Figure 9 To verify the x-direction position deviation map obtained from the experiment,

[0063] Figure 10 To verify the position deviation map obtained in the y-direction from the experiment,

[0064] Figure 11 To verify the z-direction position deviation map obtained from the experiment,

[0065] Figure 12 To verify the attitude deviation map obtained in the r-direction from the experiment.

[0066] Figure 13 To verify the attitude deviation map obtained in the p-direction from the experiment,

[0067] Figure 14 To verify the attitude deviation map in the y-direction obtained from the experiment.

[0068] Among them, 1 is the main base, 101 is the first hinge, 2 is the first connecting rod, 3 is the second connecting rod, 301 is the second hinge, 302 is the rotary motor, 4 is the linear motor, 401 is the Hooke hinge, 402 is the third hinge, 5 is the secondary base, and 6 is the base connecting rod. Detailed Implementation

[0069] The invention will now be described in further detail with reference to the accompanying drawings.

[0070] To address the difficulty in calculation and precise control of low-degree-of-freedom master-slave single closed-loop mechanisms composed of active and passive degrees of freedom, the present invention will now be implemented using the following methods... Figure 2 The embodiments shown further illustrate the method of the present invention.

[0071] like Figure 2 As shown, the low-degree-of-freedom master-slave single closed-chain mechanism used in this embodiment includes a main base 1, a first connecting rod 2, a second connecting rod 3, a linear motor 4, a slave base 5, and a base connecting rod 6. The upper end of the main base 1 is hinged to one end of the first connecting rod 2 via a first hinge 101. The other end of the first connecting rod 2 is hinged to one end of the second connecting rod 3 via a second hinge 301. The other end of the second connecting rod 3 is provided with a rotary motor 302. The head end of the linear motor 4 is provided with a Hooke hinge 401 connected to the rotary motor 302. The tail end of the linear motor 4 is hinged to the slave base 5 via a third hinge 402. The main base 1 and the slave base 5 are connected by the base connecting rod 6.

[0072] The method of this invention mainly includes the following steps:

[0073] Step 1: Perform kinematic analysis on the master-slave single closed-loop mechanism with few degrees of freedom, establish the master open-loop mechanism model and the slave open-loop mechanism model, and describe the positional relationship between each component.

[0074] Single-chain mechanisms are typically complex with numerous motion constraints and a relatively small number of degrees of freedom in their configuration space, making direct kinematic analysis challenging. The following Grubler formula can be used to calculate the degrees of freedom in the configuration space of a single-chain mechanism:

[0075]

[0076] In equation (1) above, m is the number of degrees of freedom of the rigid body, N is the total number of links, J is the number of joints, and f i It is the number of degrees of freedom corresponding to joint i, in Figure 2 Taking the mechanism shown as an example, its degrees of freedom are:

[0077]

[0078] To simplify kinematic analysis and achieve synchronous motion control of single closed-chain mechanisms with limited freedom, the controllable linkage objects can be selected based on factors such as... Figure 2 The coordinate system at the end of the second link 3 shown divides the single closed chain into a master-slave open chain.

[0079] Then, based on the kinematic analysis, establish as follows: Figure 3 The main open-chain mechanism shown and Figure 4 The open-chain mechanism shown uses the general Denabit-Hartenberg (DH) method, which requires only four parameters, to describe the relative positions of the components. Specifically:

[0080] Define a fixed coordinate system w as the world coordinate system, then measure the parameters of each component and establish a coordinate system as follows: Figure 5 The coordinate system model of each component of the main open-chain mechanism shown and as follows Figure 6 The coordinate system model of each component of the open chain mechanism is shown.

[0081] like Figure 5 and Figure 6 The relative pose relationship between any two coordinate systems in the space shown can be described by translation and rotation. The relative pose relationship between the coordinate systems can be determined by the coordinate values ​​of a specific point in two different coordinate systems.

[0082] Pose transformation matrix between the coordinate systems of two adjacent components Represented as:

[0083]

[0084] In equation (2) above, Rot(x, α) represents the pose transformation relationship between coordinate system {i-1} and coordinate system {i}. i-1 ) represents a rotation a around the x-axis of coordinate system {i-1}. i-1angle, Trans(x, a i-1 ) denotes moving along the x-axis of the coordinate system {i-1} by a i-1 distance, Rot(z, θ i ) denotes rotating around the z-axis of the coordinate system {i} by θ i angle, Trans(z, d i ) denotes moving along the z-axis of the coordinate system {i} by d i distance.

[0085] The translation and rotation transformation matrix expressions of the spatial coordinate system are as follows:

[0086]

[0087]

[0088]

[0089]

[0090] Substituting the above equations (3)-(6) into equation (2), the pose transformation relationship between the coordinate systems of the two adjacent components is obtained as:

[0091]

[0092] In the above equation (7), α i-1 denotes the angle of rotation around the x-axis of the coordinate system {i-1}, a i-1 denotes the distance of movement along the x-axis of the coordinate system {i-1}, θ i denotes the angle of rotation around the z-axis of the coordinate system {i}, and d i denotes the distance of movement along the z-axis of the coordinate system {i}.

[0093] Step two: Perform coupled kinematic analysis on the master open-chain mechanism end and the slave open-chain mechanism end to establish the coupled motion relationship between the master open-chain mechanism end and the slave open-chain mechanism end poses.

[0094] Due to the particularity of the master-slave single closed chain structure, i.e. the master open-chain mechanism end is connected to the slave open-chain mechanism end, the motion between each component is coupled, making the kinematic analysis of the single closed chain challenging. In order to simplify the motion analysis of the master-slave single closed chain mechanism and solve the motion coupling problem, it is necessary to establish the coupled motion relationship between the master open-chain mechanism end and the slave open-chain mechanism end of the master-slave single closed chain. As shown in the pose transformation relationship of each component coordinate system, the master open-chain mechanism and the slave open-chain mechanism have coupled kinematic constraints at the connection position, i.e. the master open-chain mechanism end and the slave open-chain mechanism end poses coincide: Figures 5-6

[0095] ​

[0096] In the above formula (8), w represents the coordinate system (world coordinate system) numbered w, ai represents the i-th coordinate system of the master open chain mechanism, pi represents the i-th coordinate system of the slave open chain mechanism, and T represents the coordinate system pose transformation relationship of a group of adjacent components, that is, the above formula (7).

[0097] For the structure, the above formula (8) is written as: Figures 2-4

[0098]

[0099] Definition Figure 3 The joint values of the master open chain mechanism shown in the figure are θ ai , i = 1, 2, Figure 4 The joint values of the slave open chain mechanism shown in the figure are θ pi , i = 1, 2, 3,..., then in the case of setting the desired trajectory of the master open chain, that is, in the case of knowing the joint values of the master open chain mechanism, the motion control problem of the master-slave single closed chain mechanism is converted into solving the joint values θ ai , i = 1, 2 of the slave open chain from the joint values θ pi , i = 1, 2, 3,... and the coupled motion relationship.

[0100] The left side of the equal sign in the above formula (8) is the forward kinematics equation of the master open chain, which can calculate the desired end pose of the master open chain under the given desired control joint values θ ai , i = 1, 2. Figure 2 The embodiment shown in the figure is See Step 3 below for details.

[0101] The right side of the equal sign in the above formula (8) is the forward kinematics equation of the slave open chain, and in the case of knowing the desired end pose of the master open chain , since the master open chain end and the slave open chain end coincide, the pose relationship of the slave open chain mechanism end relative to the world coordinate system is known Figure 2 The embodiment shown in the figure is Then, according to the inverse solution of the forward kinematics equation of the slave open chain, the theoretical joint angle θ pi , i = 1, 2, 3,... of the slave open chain is calculated, see Step 4 below for details.

[0102] Step 3: Specify the desired trajectory of the master open chain mechanism, that is, the joint values of the master open chain mechanism are known, and then perform forward kinematics calculation to derive the pose relationship of the master open chain mechanism end relative to the world coordinate system.

[0103] Forward kinematics is to calculate the homogeneous transformation matrix of the end coordinate system relative to the world coordinate system given the joint angles, which is​Figure 5 The main open-chain mechanism component coordinate system model can be known as follows:

[0104]

[0105] For example, according to the actual measurement of the physical properties of the main open-chain mechanism components, the DH parameters of the main open-chain can be obtained as shown in Table 1: Figure 5 The mechanism is taken as an example, according to the actual measurement of the physical properties of the main open-chain mechanism components, the DH parameters of the main open-chain can be obtained as shown in Table 1:

[0106]

[0107] Table 1 Main open-chain mechanism coordinate parameters

[0108] In Table 1, a i represents the coordinate system of each component of the main open-chain mechanism, a represents the angle of rotation around the x-axis of the coordinate system {i-1}, a represents the distance along the x-axis of the coordinate system {i-1}, a represents the angle of rotation around the z-axis of the coordinate system {i}, and d represents the distance along the z-axis of the coordinate system {i}.

[0109] Using the above formula (7), the homogeneous transformation matrix of each component coordinate system can be calculated:

[0110]

[0111]

[0112]

[0113]

[0114] Substituting formulas (10)-(13) into formula (9), the positive kinematics formula of the main open-chain mechanism is obtained as follows:

[0115]

[0116] In the above formula (14):

[0117] n x = 0.5*cos(θ1+θ2+1.396)+0.5*cos(θ2-θ1+1.396)

[0118] n y = -sin(θ2-0.174)sinθ1

[0119] n z = -cos(θ2-0.174)

[0120] o x = -cos(θ2-0.174)cos(θ1)

[0121] oy = 0.5*cos(θ1+θ2+1.396)-0.5*cos(θ2-θ1+1.396)

[0122] o z = -cos(θ2+1.396

[0123] k x = -sin(θ1)

[0124] k y = cos(θ1)

[0125] p x = 0.106*cos(θ1)cos(θ2)-0.3*sin(θ1)+0.5

[0126] p y = 0.3*cos(θ1)+0.106*cos(θ2)*sin(θ1)-0.3

[0127] p z = 0.079-0.106*sin(θ2);

[0128] Thus, given the joint variables θ1, θ2 of the master open-chain, substituting them into the above equation (14), the pose of the end of the master open-chain relative to the world coordinate system can be obtained

[0129] Step four: according to the coupling motion relationship of step two and the pose relationship of the end of the master open-chain relative to the world coordinate system obtained in step three that is, the pose relationship of the end of the slave open-chain relative to the world coordinate system is obtained Then, the inverse kinematics of the following equation is solved:

[0130] to derive the joint values θ pi , i = 1, 2, 3,... of the slave open-chain.

[0131] For a general n-degree-of-freedom open-chain mechanism, if its forward kinematics is written as T(θ), θ ∈ R n , then its inverse kinematics problem can be described as: given a homogeneous transformation matrix X ∈ SE(3), find the joint angle θ that satisfies T(θ) = X.

[0132] For open-chain mechanisms, the inverse kinematics may have multiple solutions, unlike the forward kinematics, where a given set of joint angles always corresponds to a unique end-effector pose. For more general open-chain spatial mechanisms, the Newton-Raphson method can be used to solve the inverse kinematics problem. This process is essentially a numerical iterative process, and if the initial values ​​of the selected joint angles are close to the true values, the calculation results can easily converge.

[0133] for Figure 4 The joint solutions for specific poses are obtained from the open-chain mechanism using the Newton-Raphson method. The specific process is as follows:

[0134] 1. Given the differential equation g(θ): R→R, find the numerical solution to the equation g(θ)=0, assuming θ 0 Given an initial value, the Taylor series of g(θ) in θ 0 Expand the list and retrieve only the first item:

[0135] g(θ)=g(θ 0 )+g′(θ 0 )(θ-θ 0 ) = 0

[0136] θ=θ 0 -(g′(θ 0 )) -1 g(θ 0 (15);

[0137] 2. Using the value obtained from equation (15) as the initial value, substitute it back into the equation and solve it again to obtain the following equation:

[0138] θ k+1 =θ k -(g′(θ k )) -1 g(θ k (16);

[0139] The same formula can be extended to the numerical algorithm of inverse kinematics for open-chain spatial mechanisms. Assuming that the end-effector coordinates are represented by the coordinate vector x and its forward kinematic equation x = f(θ), a nonlinear vector equation from joint coordinates to end-effector coordinates will be obtained, where f: R n →R m Differentiable, let x d The coordinate system of the endpoint to be solved, i.e., x d To obtain the desired end pose from the open chain mechanism According to the Newton-Raphson method equation g(θ) = x d -f(θ), the objective of the method of this invention is to find the root of the following equation:

[0140] g(θ) = xd -f(θ) = 0 (17);

[0141] Similar to the above equation (15), the initial joint value θ 0 is known, the kinematics equation f(θ) can be written in the form of Taylor expansion:

[0142]

[0143] The above equation (18) only takes the first term of Taylor series, and is equivalent to the Jacobian J(θ 0 ) at θ 0 ∈R m×n , according to the above equation (17), the above equation (18) can be further simplified as:

[0144] J(θ 0 )Δθ = X d -f(θ 0 ) (19);

[0145] If J(θ 0 ) is a square matrix and invertible, then Δθ can be solved by the following equation:

[0146] Δθ = J -1 (θ 0 )X d -f(θ 0 ) (20);

[0147] Otherwise, the pseudo-inverse is used instead of J -1 (θ), where can be calculated by the following equation:

[0148] J is n > m

[0149] J is n < m;

[0150] After replacing J -1 (θ) with the pseudo-inverse , the above equation (20) becomes:

[0151]

[0152] Finally, the iterative expression for solving the joint angle is obtained:

[0153] θ i+1 = θ i + Δθ i (22);

[0154] Continuously repeating the above equation (22) generates a series of θ values {θ 0θ 1 θ 2 ...}, finally in the joint solution θ d The point converges.

[0155] Step 5: To achieve synchronous motion control of a low-degree-of-freedom master-slave single closed-chain mechanism, this invention uses an online trajectory generation method for trajectory planning based on the above motion calculation. This yields a sufficiently smooth time-varying position function p(t) for each joint of the master and slave open-chain mechanisms, interpolated from the current value to the target value, while adhering to constraints related to joint velocity and acceleration. This process can be implemented using an online trajectory planner, which is a well-known technique in the field. A schematic diagram of the planned trajectory effect in this embodiment is shown below. Figure 7 As shown, a closed-loop control method is then used, utilizing a closed-loop position controller to... Figure 2 The motor drivers on each motor in the mechanism shown implement position control, enabling the spatial mechanism to track a given position command.

[0156] The specific motion control process of the mechanism of this invention is as follows: Figure 8 As shown: The user provides the desired trajectory of the main open-chain mechanism, that is, the joint values ​​θ of the main open-chain mechanism. a1 θ a2 Substituting this into equation (9) above, we perform the main open-chain forward kinematics calculation to obtain the pose of the end effector of the main open-chain mechanism relative to the world coordinate system. Then As the desired end pose from the open chain mechanism Substituting into equation (17) above, we perform iterative solution from the open-chain inverse kinematics to obtain the joint values ​​θ of each joint under the specific pose of the open-chain mechanism. pi Let i = 1, 2, 3..., and then use the obtained joint values ​​as input values ​​θ. r The online trajectory planner performs the planning and outputs the planned value θ. c The input closed-loop position controller enables closed-loop control of each motor. Both the online trajectory planner and the closed-loop position controller are technologies known in the art.

[0157] This invention verifies the feasibility of the above method through simulation, specifically as follows:

[0158] Establish using Matlab simulation software Figure 2 The diagram shows a low-degree-of-freedom master-slave single closed-loop mechanism, where the parameters of the master open-loop mechanism are shown in Table 1 above. The joint values ​​of the master open-loop mechanism are set. i = 1, 2 are traversed from -π / 6 to π / 6 in increments of 0.05, and the corresponding end poses of the main open-chain mechanism are calculated. According to the coupling motion relation (8), the end pose of the main open-chain mechanism is used as the end pose of the slave open-chain mechanism. The corresponding joint angle θ of the slave open-chain mechanism is calculated by solving the inverse kinematic equation of the slave open-chain mechanism.pi , i = 1, 2, 3..., the master-slave single closed chain model constructed by using the robot toolbox under Matlab is used to draw all the waypoints, and the end pose of the slave open chain under the calculated joint angle condition is obtained and compared with the end pose of the master open chain, wherein θ ai When i = 1, 2 ∈ [-10°, 10°], the maximum displacement error is 41 mm, and the maximum attitude error is 0.114 rad, and in other cases, because the joint value movement angle of the master-slave open chain is large, the error is relatively large, the maximum displacement error is 120 mm, and the maximum attitude error is 0.302 rad, but it can still meet the control requirements, and the specific results are shown in Table 1 Figures 9-14 It can be seen from Table 1 that the end pose of the slave open chain can follow the end pose of the master open chain, meet the control requirements of the master-slave single closed chain mechanism with few degrees of freedom, and verify the correctness and feasibility of the method.

Claims

1. A method for solving the motion of a closed-loop spatial mechanism with few degrees of freedom, characterized in that: Includes the following steps: Step 1: Perform kinematic analysis on a low-degree-of-freedom master-slave single closed-loop mechanism, establish the master open-loop mechanism model and the slave open-loop mechanism model, and describe the positional relationships between the various components, specifically: The configuration space degrees of freedom of a single closed-loop mechanism are calculated using the following Grubler formula: （1）； In the above formula (1), m It is the degree of freedom of a rigid body. N It is the total number of links, J It is the number of joints, It is a joint i The corresponding degrees of freedom; Based on the coordinate system of the end-point of the linkage object to be controlled, a master open-chain mechanism and a slave open-chain mechanism are established. The Denabit-Hartenberg method, which requires only four parameters, is used to describe the relative positional relationships of each component, specifically: Define a fixed coordinate system w as the world coordinate system, then measure the parameters of each component, and establish coordinate system models of each component of the main open chain mechanism and each component of the slave open chain mechanism. In the space shown by the main open chain mechanism and the slave open chain mechanism, the relative pose relationship between any two coordinate systems is described by translation and rotation. The relative pose relationship between the coordinate systems can be determined by the coordinate values ​​of a specific point in two different coordinate systems. Pose transformation matrix between the coordinate systems of two adjacent components Represented as: （7）； In the above formula (7), Indicates the coordinate system The angle of rotation along the x-axis, Indicates along the coordinate system The distance moved along the x-axis, Indicates the coordinate system The angle of rotation along the z-axis, Indicates along the coordinate system The distance moved along the z-axis; Step 2: Perform coupled kinematic analysis on the end effectors of the main open-chain mechanism and the slave open-chain mechanism to establish the coupled kinematic relationship between their poses: Since the positions of the ends of the main open chain mechanism and the slave open chain mechanism coincide, therefore: （8）； In the above equation (8), w is the world coordinate system, ai is the i-th coordinate system of the main open chain mechanism, pi is the i-th coordinate system of the slave open chain mechanism, and T represents the coordinate system pose transformation relationship of a set of adjacent components, which is the above equation (7). Step 3: Specify the desired trajectory of the main open chain mechanism, and then perform forward kinematics calculation on the main open chain mechanism to obtain the end pose of the main open chain mechanism; Step 4: Based on the coupled motion relationship in Step 2 and the end pose of the main open chain mechanism in Step 3, obtain the end pose of the secondary open chain mechanism, and then perform inverse kinematics calculation to obtain the joint values ​​of the secondary open chain mechanism. Step 5: Use an online trajectory generation method for trajectory planning, and then use a closed-loop control method to implement position control on each motor in the master-slave single closed-loop mechanism with few degrees of freedom.

2. The motion calculation method for a closed-loop spatial mechanism with few degrees of freedom according to claim 1, characterized in that: In step one, the pose transformation matrix between the coordinate systems of two adjacent components Represented as: （2）； In the above formula (2), Indicates the coordinate system x-axis rotation angle, Indicates along the coordinate system x-axis movement distance, Indicates the coordinate system z-axis rotation angle, Indicates along the coordinate system z-axis movement The distance, and the expressions for the translation and rotation transformation matrices of the spatial coordinate system are as follows: （3）； （4）； （5）； （6）； Substituting equations (3)-(6) into equation (2), we obtain: （7）。 3. The motion calculation method for a closed-loop spatial mechanism with few degrees of freedom according to claim 1, characterized in that: In step three, the desired trajectory of the main open-chain mechanism is specified, which yields the joint values ​​of the main open-chain mechanism. Substitute the following equation to perform the forward kinematics solution: ； Obtain the pose of the end effector of the main open-chain mechanism relative to the world coordinate system. .

4. The motion calculation method for a closed-loop spatial mechanism with few degrees of freedom according to claim 3, characterized in that: In step four, the pose relationship between the end of the open chain mechanism and the world coordinate system is determined. = For the following formula: Inverse kinematics calculations were performed to derive the joint values ​​of the open-chain mechanism. .

5. The motion calculation method for a closed-loop spatial mechanism with few degrees of freedom according to claim 1 or 4, characterized in that: In step four, we assume that the coordinate vector x and its forward kinematic equations are used. Representing the end-effector coordinates, we obtain a nonlinear vector equation from the joint coordinates to the end-effector coordinates, where Minor, let The coordinate system of the endpoint to be solved is, i.e. To determine the pose from the end of the open-chain mechanism, according to the Newton-Raphson equations... The objective is to find the roots of the following equation: （17）； Obtain the iterative expression for solving the joint angles: （22）； By repeatedly iterating over equation (22), a series of... value Finally, in the joint dissection The point converges.

6. The motion calculation method for a closed-loop spatial mechanism with few degrees of freedom according to claim 5, characterized in that: In step four, The solution process is as follows: Given initial joint values kinematic equations Written in Taylor expansion form: （18）； Equation (18) above only extracts the first term of the Taylor series, and Equivalent to Jacobi at the place Then the above equation (18) can be further simplified to: （19）； if Given a square matrix that is invertible, the following formula can be used to solve it. : （20）； Otherwise, use pseudo-inverse. replace ,in Calculated using the following formula: ； pseudo-reversal replace Then, the above equation (20) becomes: （21）； Finally, the iterative expression for solving the joint angles is obtained: （22）。 7. The motion calculation method for a closed-loop spatial mechanism with few degrees of freedom according to claim 1, characterized in that: In step five, an online trajectory generation method is used for trajectory planning to obtain the position function of each joint of the main open-chain mechanism and the secondary open-chain mechanism with respect to time, from the current value to the target value. Then, the expected joint values ​​of the main open chain mechanism in step three and the joint values ​​of the secondary open chain mechanism obtained in step four are used as the trajectory planning input values, and the trajectory planning output values ​​are input to the closed-loop position controller to realize the closed-loop control of the motor.

Images