FUSION SYSTEM OF MECHANICAL ARM AND DEXTEROUS HAND, AND MOTION CONTROL METHOD

Abstract

Provided are a fusion system of a mechanical arm and a dexterous hand, and a control method therefor. The fusion system comprises a four-degree-of-freedom mechanical arm, a dexterous hand wrist mounted at the tail end of the four-degree-of-freedom mechanical arm, and a dexterous hand mounted on the dexterous hand wrist, wherein the dexterous hand wrist is a single-joint spatial orthogonal two-degree-of-freedom wrist. In a motion control method, the fusion system of the four-degree-of-freedom mechanical arm and the dexterous hand is regarded as a six-axis series mechanical arm, and according to a joint angle, a position and an attitude of the tail end of the dexterous hand wrist can be obtained by means of a forward kinematics method; and according to the position and attitude of the tail end of the dexterous hand wrist, the joint angle can be obtained by means of an inverse kinematics numerical solution method, and then a control instruction is obtained and sent to a servo position controller, so as to drive the tail end of the dexterous hand wrist to reach a preset position and attitude. A six-degree-of-freedom series-parallel mechanism combining the mechanical arm and the dexterous hand wrist has multi-degree-of-freedom redundancy, such that rotation of the five-finger dexterous hand can be achieved without a wide-range motion of the mechanical arm.

Claims

1. A fusion system of a mechanical arm and a dexterous hand, comprising: a four-degree-of-freedom mechanical arm, a dexterous hand wrist mounted at an end of the four-degree-of-freedom mechanical arm, and a dexterous hand mounted on the dexterous hand wrist, wherein the dexterous hand wrist and the four-degree-of-freedom mechanical arm are connected by an electronic circuit.

2. The fusion system of the mechanical arm and the dexterous hand according to claim 1, wherein the dexterous hand wrist is a two-degree-of-freedom parallel mechanism, and comprises two motors, two push rods and two servo position controllers; and the two servo position controllers are an active servo position controller and a slave servo position controller, the two motors comprise a first motor and a second motor, and the two push rods comprise a first push rod and a second push rod; the active servo position controller is configured to drive a motion of the first motor, and the motion of the first motor drives a motion of the first push rod; the slave servo position controller is configured to drive a motion of the second motor, and the motion of the second motor drives a motion of the second push rod; and an end of the first push rod and an end of the second push rod are simultaneously connected to a dexterous hand palm.

3. The fusion system of the mechanical arm and the dexterous hand according to claim 2, wherein the two motors adopt a synchronous differential control mode, and the two motors drive the motion of the two push rods to realize a pitching motion and a yaw rotation of the dexterous hand.

4. The fusion system of the mechanical arm and the dexterous hand according to claim 3, wherein when the two push rods move in a same direction, the dexterous hand performs the pitching motion, and a pitch angle range is 70 degrees to 70 degrees; and when the two push rods move in opposite directions, the dexterous hand performs the yaw rotation, and a yaw angle range is 45 degrees to 45 degrees.

5. A motion control method for a fusion system of a mechanical arm and a dexterous hand, comprising: regarding a fusion system of a four-degree-of-freedom mechanical arm and a dexterous hand as a six-axis serial mechanical arm, taking a center of a base of the four-degree-of-freedom mechanical arm as an origin, establishing a coordinate system of the mechanical arm by a D-H method, and giving D-H parameters of each joint and link of the mechanical arm; according to joint angles .sub.1, .sub.2, .sub.3, .sub.4 of the four-degree-of-freedom mechanical arm and joint angles .sub.5, .sub.6 of the dexterous hand wrist, obtaining a position and attitude matrix $T=\left[\begin{array}{cc}R& P\\ 0& 1\end{array}\right]$ of an end of a dexterous hand wrist through a positive kinematics method of the fusion system of the four-degree-of-freedom mechanical arm and the dexterous hand wrist T, to realize a forward motion control of the fusion system, wherein R represents a rotation matrix at the end of the dexterous hand wrist and P represents a position matrix at the end of the dexterous hand wrist; and if the position and attitude $T=\left[\begin{array}{cc}R& P\\ 0& 1\end{array}\right]$ of the end of the dexterous hand wrist is known, obtaining joint angles .sub.1, .sub.2, .sub.3, .sub.4, .sub.5, .sub.6 of the fusion system through an inverse kinematic numerical solution method of the fusion system of the four-degree-of-freedom mechanical arm and the dexterous hand wrist, obtaining a control instruction according to the joint angles, and sending the control instruction to a servo position controller, to drive the end of the dexterous hand wrist to a predetermined position and attitude.

6. The motion control method for the fusion system of the mechanical arm and the dexterous hand according to claim 5, wherein when the forward motion of the fusion system is controlled, the position and attitude of the end of the dexterous hand wrist is T=.sub.6.sup.0T=.sub.1.sup.0T*.sub.2.sup.1T*.sub.3.sup.2T*.sub.4.sup.3T*.sub.5.sup.4T*.sub.6.sup.5T, wherein .sub.1.sup.0T is a coordinate transformation matrix from a base coordinate system of the mechanical arm to a first joint of the mechanical arm in the fusion system; .sub.2.sup.1T is a coordinate transformation matrix from the first joint of the mechanical arm to a second joint of the mechanical arm in the fusion system; .sub.3.sup.2T is a coordinate transformation matrix from the second joint of the mechanical arm to a third joint of the mechanical arm in the fusion system; .sub.4.sup.3T is a coordinate transformation matrix from the third joint of the mechanical arm to a fourth joint of the mechanical arm in the fusion system; .sub.5.sup.4T is a coordinate transformation matrix from the fourth joint of the mechanical arm to a first joint of the dexterous hand wrist in the fusion system; .sub.6.sup.5T is a coordinate transformation matrix from the first joint of the dexterous hand wrist to a second joint of the dexterous hand wrist in the fusion system; and .sub.6.sup.0T is a coordinate transformation matrix from the base coordinate system of the mechanical arm to the end of the dexterous hand wrist in the fusion system.

7. The motion control method for the fusion system of the mechanical arm and the dexterous hand according to claim 6, wherein: $\begin{array}{ccc}{}_1^{0}T=\left[\begin{array}{cccc}c1& -s1& 0& 0\\ s1& c1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]& {}_2^{1}T=\left[\begin{array}{cccc}c2& -s2& 0& 0\\ 0& 0& 1& d_2\\ -s2& -c2& 0& 0\\ 0& 0& 0& 1\end{array}\right]& {}_3^{2}T=\left[\begin{array}{cccc}c3& -s3& 0& a_2\\ s3& c3& 0& 0\\ 0& 0& 1& d_3\\ 0& 0& 0& 1\end{array}\right]\end{array}\begin{array}{cc}{}_4^{3}T=\left[\begin{array}{cccc}c4& -s4& 0& 0\\ 0& 0& -1& d_4\\ s4& c4& 0& 0\\ 0& 0& 0& 1\end{array}\right]& {}_5^{4}T=\left[\begin{array}{cccc}c5& -s5& 0& 0\\ 0& 0& -1& d_5\\ s5& c5& 0& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}{}_6^{5}T=\left[\begin{array}{cccc}c6& -s6& 0& 0\\ 0& 0& 1& 0\\ -s6& -c6& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ wherein, dj is a link distance between a j.sup.th joint and a (j1).sup.th joint, j=2, 3, 4, 5; a.sub.p is a link length of a p.sup.th joint, p=2; and si is an i.sup.th joint angle sine, ci is an i.sup.th joint angle cosine, and i=1, 2, 3, 4, 5, 6.

8. The motion control method for the fusion system of the mechanical arm and the dexterous hand according to claim 7, wherein si=sin .sub.i, ci=cos .sub.i.

9. The motion control method for the fusion system of the mechanical arm and the dexterous hand according to claim 5, wherein the joint angle .sub.6 is numerically solved in an iterative mode.

10. The motion control method for the fusion system of the mechanical arm and the dexterous hand according to claim 9, wherein a formula for a (k+1).sup.th iteration of .sub.6 is as follows: ${}_{6\left(k+1\right)}={}_{6\left(k\right)}-\frac{f\left({}_{6\left(k\right)}\right)\left({}_{6\left(k\right)}-{}_{6\left(k-1\right)}\right)}{f\left({}_{6\left(k\right)}\right)-f\left({}_{6\left(k-1\right)}\right)}$ wherein .sub.6(k+1) is a result of the (k+1).sup.th iteration of .sub.6, .sub.6(k) is a result of a k.sup.th iteration of .sub.6, .sub.6(k1) is a result of a (k1).sup.th iteration of .sub.6, f(.sub.6(k+1)) is an inverse kinematics formula at .sub.6(k+1), f(.sub.6(k)) is an inverse kinematics formula at .sub.6(k), and f(.sub.6(k1)) is an inverse kinematics formula at .sub.6(k1).

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0037] FIG. 1 is a schematic diagram of a partial control of a dexterous hand wrist;

[0038] FIG. 2 shows a single-joint spatial orthogonal two-degree-of-freedom integrated wrist; and

[0039] FIG. 3 shows a configuration of a four-degree-of-freedom mechanical arm combined with a dexterous hand dual-degree-of-freedom wrist.

DETAILED DESCRIPTION OF THE DISCLOSURE

[0040] The present disclosure is further elaborated below in conjunction with the accompanying drawings.

[0041] Under the premise of not modifying a dexterous hand, the present disclosure designs a both beautiful and practical fusion system of a mechanical arm and a dexterous hand, saves research and development costs, shortens a research and development cycle; and at the same time, proposes an inverse kinematics numerical solution solving method to realize the motion control of a fusion system of a mechanical arm and a dexterous hand.

[0042] A fusion system of a mechanical arm and a dexterous hand is shown in FIG. 3, which includes a four-degree-of-freedom mechanical arm, a dexterous hand wrist (a single-joint spatial orthogonal two-degree-of-freedom integrated wrist), and a dexterous hand. The dexterous hand two-degree-of-freedom wrist is mounted at an end of the four-degree-of-freedom mechanical arm, and the two are connected by an electronic circuit for communication and power supply. The dexterous hand palm is mounted on the dexterous hand wrist. A motion control method for the fusion system of a four-degree-of-freedom mechanical arm and a dexterous hand wrist includes a control method for a single-joint spatial orthogonal two-degree-of-freedom integrated wrist, a positive kinematics method for the fusion system of the mechanical arm and the dexterous hand, and an inverse kinematics method for the fusion system of the mechanical arm and the dexterous hand.

[0043] As shown in FIG. 2, the single-joint spatial orthogonal two-degree-of-freedom integrated wrist adopts dual motors M1, M2 for synchronous differential control. A pitching motion and a yaw rotation of the dexterous hand are realized by using the dual motors to push the double push rods L1 and L2. When the double push rods L1 and L2 move in a same direction, the dexterous hand performs the pitching motion, the wrist can be regarded as a joint J1, and a pitch angle range of the joint J1 is 70 degrees to 70 degrees. When the double push rods L1 and L2 move in opposite directions, the dexterous hand performs the yaw rotation, the wrist can be regarded as a joint J2, and a yaw angle of the joint J2 is 45 degrees to 45 degrees. One motor m1 of the dual motors is driven by a servo position controller A (active), and the other motor m2 is driven by a servo position controller B (slave).

[0044] The control method for the dexterous hand two-degree-of-freedom wrist is shown in FIG. 1, a target position of the dexterous hand is input; the fusion system calculates a control instruction through a control law, and sends the control instruction to the servo position controller A (active) and the servo position controller B (slave); and the servo controller then combines the servo control instruction with a load transmission ratio KA or KB to obtain a drive instruction of the corresponding motor. An actual position of the joint is fed back to the fusion system through a position sensor and then through an integrator operation, to achieve a large closed-loop control.

[0045] A motion control method for a fusion system of a four-degree-of-freedom mechanical arm and a dexterous hand dual-degree-of-freedom wrist includes the steps as follows. [0046] (1) The fusion system of the four-degree-of-freedom mechanical arm and the dexterous hand wrist fusion system is regarded as a six-axis serial mechanical arm. A center of a base of the mechanical arm is taken as the origin, a coordinate system is established by using a D-H method, and D-H parameters of each link and joint are given, in which the D-H parameters of the joint included joint angles .sub.1, .sub.2, .sub.3, .sub.4 of the four-degree-of-freedom mechanical arm, and joint angles .sub.5, .sub.6 of the dexterous hand wrist. [0047] (2) According to the known joint angles .sub.1, .sub.2, .sub.3, .sub.4 of the four-degree-of-freedom mechanical arm, and the joint angles .sub.5, .sub.6 of the wrist, a position and attitude

[00005]$T=\left[\begin{array}{cc}R& P\\ 0& 1\end{array}\right]$

of the end of the dexterous hand wrist is obtained through a positive kinematics method of the fusion system of the four-degree-of-freedom mechanical arm and the dexterous hand wrist, to realize a forward motion control of the system. R represents a rotation matrix at the end of the dexterous hand wrist and P represents a position matrix at the end of the dexterous hand wrist.

[0048] The transformation matrices between coordinate systems of the fusion system of the four-degree-of-freedom mechanical arm and the dexterous hand wrist are as follows:

[00006]$\begin{array}{ccc}{}_1^{0}T=\left[\begin{array}{cccc}c1& -s1& 0& 0\\ s1& c1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]& {}_2^{1}T=\left[\begin{array}{cccc}c2& -s2& 0& 0\\ 0& 0& 1& d_2\\ -s2& -c2& 0& 0\\ 0& 0& 0& 1\end{array}\right]& {}_3^{2}T=\left[\begin{array}{cccc}c3& -s3& 0& a_2\\ s3& c3& 0& 0\\ 0& 0& 1& d_3\\ 0& 0& 0& 1\end{array}\right]\end{array}\begin{array}{cc}{}_4^{3}T=\left[\begin{array}{cccc}c4& -s4& 0& 0\\ 0& 0& -1& d_4\\ s4& c4& 0& 0\\ 0& 0& 0& 1\end{array}\right]& {}_5^{4}T=\left[\begin{array}{cccc}c5& -s5& 0& 0\\ 0& 0& -1& d_5\\ s5& c5& 0& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}{}_6^{5}T=\left[\begin{array}{cccc}c6& -s6& 0& 0\\ 0& 0& 1& 0\\ -s6& -c6& 0& 0\\ 0& 0& 0& 1\end{array}\right]{}_6^{0}T={}_1^{0}T*{}_2^{1}T*{}_3^{2}T*{}_4^{3}T*{}_5^{4}T*{}_6^{5}T,$

[0049] in which .sub.0.sup.1T is a coordinate transformation matrix from a base coordinate system of the mechanical arm to a first joint of the mechanical arm in the fusion system;

[0050] .sub.2.sup.1T is a coordinate transformation matrix from the first joint of the mechanical arm to a second joint of the mechanical arm in the fusion system;

[0051] .sub.3.sup.2T is a coordinate transformation matrix from the second joint of the mechanical arm to a third joint of the mechanical arm in the fusion system;

[0052] .sub.4.sup.3T is a coordinate transformation matrix from the third joint of the mechanical arm to a fourth joint of the mechanical arm in the fusion system;

[0053] .sub.5.sup.4T is a coordinate transformation matrix from the fourth joint of the mechanical arm to a first joint of the dexterous hand wrist in the fusion system;

[0054] .sub.6.sup.5 T is a coordinate transformation matrix from the first joint of the dexterous hand wrist to a second joint of the dexterous hand wrist in the fusion system; and

[0055] .sub.6.sup.0 T is a coordinate transformation matrix from the base coordinate system of the mechanical arm to the end of the dexterous hand wrist in the fusion system.

[0056] In which, dj is a link distance between a j.sup.th joint and a (j1).sup.th joint, j=2, 3, 4, 5; a.sub.p is a link length of a p.sup.th joint, p=2; and

[0057] si is an i.sup.th joint angle sine, ci is an i.sup.th joint angle cosine, and i=1, 2, 3, 4, 5, 6. si=sin .sub.i, ci=cos .sub.i, and .sub.i is an i.sup.th joint angle.

[0058] A first joint angle range is 90<.sub.1<90. A second joint angle range is 0<.sub.2<90. A third joint angle range is 120<.sub.3<0. A fourth joint angle range is 0<.sub.4<180. A fifth joint angle range is 70<.sub.5<70. A sixth joint angle range is 45<.sub.6<45. [0059] (3) According to the known position and attitude

[00007]$\left[\begin{array}{cc}R& P\\ 0& 1\end{array}\right]$

at the end of the system, because the three axes at the end of the system are not orthogonal and there is no analytical solution, the joint angles .sub.1, .sub.2, .sub.3, .sub.4, .sub.5, .sub.6 of the system is obtained through an inverse kinematic numerical solution method of the fusion system of the four-degree-of-freedom mechanical arm and the dexterous hand wrist, to realize a reverse motion control of the system.

[0060] .sub.1, .sub.2, .sub.3, .sub.4, and .sub.5 are obtained by solving the robot inverse kinematics, but .sub.6 cannot be solved analytically, so only the numerical solution can be obtained.

[0061] Substantially, the numerical solution is guessing and iterating until the error is small enough, or until it is thought to have given up. The common Newton-Raphson algorithm is used in this method because it is conceptually simple and has a quadratic convergence rate if the initial guess is close enough to the solution. The Newton iterative method needs to calculate a first order derivative of f(x) at x.sub.k, and for complex functions, especially multivariate implicit functions, finding the derivative or partial derivative is relatively cumbersome and complex, an approximate cut line method is used with the following equation,

[00008]$x_{k+1}=x_k-\frac{f\left(x_k\right)\left(x_k-x_{k-1}\right)}{f\left(x_k\right)-f\left(x_{k-1}\right)}.$

[0062] However, there is no guarantee that the algorithm will converge or meet the application requirements fast enough and return only one solution. In order to produce a solution for a wide range of possible postures, different initial conditions must be used.

[0063] Therefore, in the present disclosure, a solution formula of the (k+1).sup.th joint angle .sub.6 is as follows:

[00009]${}_{6\left(k+1\right)}={}_{6\left(k\right)}-\frac{f\left({}_{6\left(k\right)}\right)\left({}_{6\left(k\right)}-{}_{6\left(k-1\right)}\right)}{f\left({}_{6\left(k\right)}\right)-f\left({}_{6\left(k-1\right)}\right)}$

[0064] In which, .sub.6(k+1) is a result of the (k+1).sup.th iteration of .sub.6, .sub.6(k) is a result of the k.sup.th iteration of .sub.6, .sub.6(k1) is a result of the (k1).sup.th iteration of .sub.6, f(.sub.6(k+1)) is an inverse kinematics formula at .sub.6(k+1), f(.sub.6(k)) is an inverse kinematics formula at .sub.6(k), and f(.sub.6(k1)) is an inverse kinematics formula at .sub.6(k1).

[0065] The present disclosure adopts the six-degree-of-freedom series-parallel mechanism that combines the four-degree-of-freedom mechanical arm and the dexterous hand two-degree-of-freedom wrist to carry out the fusion control of the mechanical arm and the dexterous hand, the six-degree-of-freedom arm composed of the four-degree-of-freedom mechanical arm and the two joints of the dexterous hand wrist does not satisfy the end three-axes intersecting at a point, and there is no inverse kinematic analytical solution, only a numerical solution, so a kind of inverse kinematic numerical solution method based on this arm type is designed, which is suitable for the numerical solution of all serial mechanical arms.

[0066] The six-degree-of-freedom series-parallel mechanism that combines the four-degree-of-freedom mechanical arm with the dexterous hand two-degree-of-freedom wrist in the present disclosure has multi-degree-of-freedom redundancy, so that the mechanical arm can be made to realize the rotation of the five-finger dexterous hand without wide range motion.

[0067] The contents not described in detail in the description of the present disclosure belong to the well-known technology of professional and technical personnel in the art.