CN102509025A - Method for quick solution of six-degree-of-freedom humanoid dexterous arm inverse kinematics - Google Patents

Abstract

The invention discloses a method for quick solution of six-degree-of-freedom humanoid dexterous arm inverse kinematics. The method includes: firstly, setting up a six-degree-of-freedom human arm simulated joint structural model, solving an elbow joint angle and two joint angles of the shoulder joint according to a planar relation formed by the shoulder center, the elbow center, the wrist center and the neck center of the dexterous arm and the tail end position of the dexterous arm, and further solving three joint angles of the wrist joint by the aid of the dexterous arm tail-end posture relation; and calculating position error of a dexterous arm target link to prove effectiveness of the solution of the inverse kinematics by means of a positive kinematic model. Compared with a numerical solving method for robot inverse kinematics, the method for quick solution of six-degree-of-freedom humanoid dexterous arm inverse kinematics is high in precision and quick in speed, and can be used for humanoid dexterous arm operational tasks having high requirements on precision and instantaneity.

Description

The rapid solving method of the dexterous arm inverse kinematics of a kind of six degree of freedom apery

Technical field

The present invention relates to the dexterous arm of a kind of six degree of freedom apery, relate in particular to the parsing rapid solving method of the dexterous arm inverse kinematics of a kind of six degree of freedom apery.

Background technology

Anthropomorphic robot wants to substitute the human various operations of accomplishing, the dexterous arm of apery that must type of having people upper limbs.The connecting rod configuration and the terminal motion conditions of the dexterous arm of apery can be represented with its degree-of-freedom joint angle; The kinematics of dexterous arm can divide positive motion to learn and inverse kinematics; Wherein the former finds the solution the pose motion conditions of dexterous arm target connecting rod according to each degree of freedom joint angle situation of change of dexterous arm, i.e. problem is found the solution in mapping from the joint angle space to cartesian space; Latter's inverse kinematics problem is then opposite, and it is to find the solution problem from cartesian space to the mapping in joint angle space, and it is the basis of realizing dexterous arm motion control, working space analysis and trajectory planning that the dexterous arm inverse kinematics of apery is found the solution problem.

Finding the solution method at present commonly used for the inverse robot kinematics problem has through numerical method as based on Jacobian matrix method, neural net method and genetic algorithm etc.; Wherein need the Jacobi battle array be inverted based on Jacobian matrix such as Newton-Raphson method, plan newton method of conjugate gradient etc.; Have the Jacobian matrix singularity problem, and the value of algorithm original state will influence convergence of algorithm speed and solving precision; Neural net method is to set up robot cartesian space lower link configuration with each joint angles mapping relations through learning network power, threshold parameter; But the needed great amount of samples data of e-learning in reality difficulty obtain; And the e-learning required time is longer, unsuitable control in real time; Genetic algorithm (Genetic Algorithm; GA) finding the solution inverse kinematics is to utilize its overall parallel search characteristic; But precocity and speed of convergence that conventional genetic algorithm has when solving optimization problem wait defective to reduce the optimization Algorithm performance slowly; Influencing inverse kinematics problem solving precision. some hybrid technologies of combining with expert system, fuzzy logic and genetic algorithm of neural network also are applied to during the inverse robot kinematics problem finds the solution in addition, but these methods need high performance calculating configuration and complicated calculation procedure when asking for multi-freedom robot inverse kinematics problem.The inverse kinematics problem that said method is found the solution robot through the optimizing of numerical value iteration has consuming time longer, and solving precision also can not get guaranteeing suitable being applied in the robot manipulating task task of high precision, high real-time.

Except that finding the solution through iterative numerical approach the inverse robot kinematics problem, a kind of outside going back is through analytical method solving inverse robot kinematics problem.Analytical method is found the solution the inverse robot kinematics problem, and it does not need the optimizing of numerical value iterative search; Directly try to achieve each joint angles theoretical value by analytic formula; Therefore it is fast to have speed, advantage of high precision, but analytical method is found the solution the peculiar geometric properties that inverse kinematics depends on the robot particular organization; Be suitable for finding the solution the inverse kinematics of some industrial machinery arm simple in structure, very few application the in the dexterous arm inverse kinematics of baroque apery problem.

Summary of the invention

Deficiency to prior art; The technical matters that the present invention solves provides the rapid solving method of the dexterous arm inverse kinematics of a kind of six degree of freedom apery; It is fast to utilize this method to ask for inverse kinematics problem speed; And precision is higher, can the dexterous arm of apery be applied to high precision, in the job task of high real-time.

For solving the problems of the technologies described above; Technical scheme of the present invention is achieved in that the rapid solving method of the dexterous arm inverse kinematics of a kind of six degree of freedom apery; The dexterous arm of said six degree of freedom apery is by two degree of freedom of shoulder joint, elbow joint one degree of freedom, wrist joint three degree of freedom, neck and be connected in each interarticular connecting rod and form; Especially, the rapid solving method of the dexterous arm inverse kinematics of said six degree of freedom apery comprises the steps:

A, set up its articulation structure model according to the structure of the dexterous arm of six degree of freedom apery, wherein, the corresponding shoulder joint of shoulder heart axle center, the corresponding elbow joint of elbow heart axle center, the corresponding wrist joint of wrist heart axle center, the corresponding neck of the neck heart;

B, confirm the pose (p of the dexterous arm target of said six degree of freedom apery connecting rod ^Ref, R ^Ref);

C, go out the inverse kinematics joint angle vector q of the dexterous arm of six degree of freedom apery through analytic calculation according to said target connecting rod pose,

Concrete realization through analytic calculation inverse kinematics joint angle vector q among the said step c is following:

Definition world coordinate system ∑ _W, said shoulder joint, elbow joint and wrist joint correspond to y respectively around axle under world coordinate system, x, x, z, x, y axle, [r _xr _yr _z] for the shoulder joint being the position coordinates of the wrist heart under the world coordinate system of initial point, elbow joint angle q ₃Triangle relation by the said shoulder heart, the elbow heart and the wrist heart constitute is asked for, shoulder joint angle of pitch q ₁With deflection angle q ₂Plane relation by the said shoulder heart, the elbow heart, the wrist heart and the neck heart constitute is asked for, and adopts following equality respectively:

${\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_3=-\arccos \left(\frac{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2+{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2-{\mathrm{<figure-callout\; id="2"\; label="C"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">C</figure-callout>}}^2}{2\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1\&CenterDot;L2}\right)+\mathrm{\&pi;}$

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_1=\arctan (r_x/r_z)\mathrm{if}{r}_z<0\\ {\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_1=-\mathrm{\&pi;}+\arctan (r_x/r_z)\mathrm{if}{r}_z>0\end{array}\right.$

${\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_2=\mathrm{\&pi;}/2-\arccos \left(\frac{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2+C^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1\&CenterDot;C}\right)-\arccos \left(\frac{{\mathrm{<figure-callout\; id="2"\; label="C"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">C</figure-callout>}}^2+D^2-{\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">F</figure-callout>}}^2}{2\mathrm{CD}}\right)$

Wherein, D is the length between the shoulder heart and the neck heart, and L1 is the length between the shoulder heart and the elbow heart, and L2 is the length between the elbow heart to the wrist heart, and F is the distance between the wrist heart and the neck heart.

Preferably, in the rapid solving method of the dexterous arm inverse kinematics of above-mentioned six degree of freedom apery, asking for elbow joint angle q ₃, shoulder joint angle of pitch q ₁With deflection angle q ₂After, three joint angle q of said wrist joint ₄, q ₅And q ₆Through with the terminal attitude R of the dexterous arm of six degree of freedom apery ^RefRelation is asked for, and adopts following equality:

$R_z\left(q_4\right)R_x\left(q_5\right)R_y\left(q_6\right)=R_x^T\left(q_3\right)R_x^T\left(q_2\right)R_y^T\left(q_1\right)R_0^T{R}_6^{\mathrm{ref}}$

Wherein, R ₀Be the neck attitude matrix,

For rotating q around joint shaft a (unit vector) _jThe time rotation matrix.

Preferably, in the rapid solving method of the dexterous arm inverse kinematics of above-mentioned six degree of freedom apery, also comprise the invert method of the validity of separating joint angle vector q of checking among the said step c, this verification method comprises the steps:

1) positive motion of analyzing the dexterous arm of said six degree of freedom apery is learned model, according to invert separate joint angle vector q by dexterous arm positive motion learn the pose that calculates the target connecting rod (p, R);

2) error e rr (Δ p, the Δ R)=err (p of calculating target connecting rod pose ^Ref-p, R ^TR ^Ref);

3) the checking kinematics of the inverting validity of separating, when position and attitude error err (Δ p, Δ R) is enough for a short time when approaching zero institute asks for contrary separate effective.

Preferably; In the rapid solving method of the dexterous arm inverse kinematics of above-mentioned six degree of freedom apery, said wrist joint is connected with Dextrous Hand, and said Dextrous Hand is provided with the palm of the hand; In the said step 1): the positive motion of analyzing the dexterous arm of six degree of freedom apery is learned model; According to each joint angle vector q of ask by positive motion learn to calculate each connecting rod pose (p, R), adopt following equality:

$\left\{\begin{array}{cc}{\mathrm{<figure-callout\; id="1"\; label="R"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_1=R_0\&CenterDot;R_y\left(q_1\right)& {\mathrm{<figure-callout\; id="2"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_2={\mathrm{<figure-callout\; id="1"\; label="R"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_1\&CenterDot;R_x\left(q_2\right)\\ {\mathrm{<figure-callout\; id="3"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_3=R_2\&CenterDot;R_x\left(q_3\right)& {\mathrm{<figure-callout\; id="4"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_4=R_3\&CenterDot;R_z\left(q_4\right)\\ {\mathrm{<figure-callout\; id="5"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_5=R_4\&CenterDot;R_x\left(q_5\right)& R_6=R_5\&CenterDot;R_y\left(q_6\right)\\ R_7=R_6& \end{array}\right.$

$\left\{\begin{array}{cc}{\mathrm{<figure-callout\; id="1"\; label="p"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_1=p_0+R_0\&CenterDot;b_1& {\mathrm{<figure-callout\; id="1"\; label="b"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">b</figure-callout>}}_1=\left[\begin{array}{ccc}0& -D& 0\end{array}\right]\\ {\mathrm{<figure-callout\; id="2"\; label="p"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_2={\mathrm{<figure-callout\; id="1"\; label="p"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_1+{\mathrm{<figure-callout\; id="1"\; label="R"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_1\&CenterDot;b_2& {\mathrm{<figure-callout\; id="2"\; label="b"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">b</figure-callout>}}_2=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]\\ {\mathrm{<figure-callout\; id="3"\; label="p"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_3={\mathrm{<figure-callout\; id="2"\; label="p"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_2+{\mathrm{<figure-callout\; id="2"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_2\&CenterDot;b_3& {\mathrm{<figure-callout\; id="3"\; label="b"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">b</figure-callout>}}_3=\left[\begin{array}{ccc}0& 0& -L1\end{array}\right]\\ {\mathrm{<figure-callout\; id="4"\; label="p"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_4={\mathrm{<figure-callout\; id="3"\; label="p"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_3+{\mathrm{<figure-callout\; id="3"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_3\&CenterDot;b_4& {\mathrm{<figure-callout\; id="4"\; label="b"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">b</figure-callout>}}_4=\left[\begin{array}{ccc}0& 0& -L2\end{array}\right]\\ {\mathrm{<figure-callout\; id="5"\; label="p"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_5={\mathrm{<figure-callout\; id="4"\; label="p"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_4+{\mathrm{<figure-callout\; id="4"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_4\&CenterDot;b_5& {\mathrm{<figure-callout\; id="5"\; label="b"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">b</figure-callout>}}_5=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]\\ p_6={\mathrm{<figure-callout\; id="5"\; label="p"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_5+{\mathrm{<figure-callout\; id="5"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_5\&CenterDot;b_6& b_6=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]\\ p_7=p_6+R_6\&CenterDot;b_7& b_7=\left[\begin{array}{ccc}0& 0& -\mathrm{<figure-callout\; id="3"\; label="L"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}3\end{array}\right]\end{array}\right.$

P wherein ₀, R ₀Be respectively the position and the attitude of neck, p ₇, R ₇Be respectively the position and the attitude of the palm of the hand, L3 is the distance of the wrist heart to the palm of the hand; Q=[q ₁, q ₂..., q ₆] be from shoulder joint to carpal joint angle vector;

For around axle a _jRotate q _jThe time rotation matrix.

Compared with prior art; The invention has the beneficial effects as follows that direct employing analytic method asks for the inverse kinematics of the dexterous arm of six degree of freedom apery; Not through the combination of numerical value iterative manner search joint angle; It is fast that this method counting yield height is found the solution speed, and find the solution accurately, precision is high, can be used in the high dexterous arm job task of real-time.

Description of drawings

In order to be illustrated more clearly in the technical scheme in the embodiment of the invention; The accompanying drawing of required use is done to introduce simply in will describing embodiment below; Obviously, the accompanying drawing in describing below only is some embodiments of the present invention, for those of ordinary skills; Under the prerequisite of not paying creative work, can also obtain other accompanying drawing according to these accompanying drawings.

Shown in Figure 1 is the dexterous shoulder joint structural model of table tennis operation six degree of freedom apery figure;

Shown in Figure 2 is the illustraton of model of the dexterous arm shoulder joint of table tennis operation six degree of freedom apery angle computing method.

Embodiment

The object of the invention is to provide the parsing method for solving of the dexterous arm inverse kinematics of a kind of six degree of freedom apery, and it is fast to utilize this method to ask for inverse kinematics speed, and precision is higher, can the dexterous arm of apery be applied to high precision, in the job task of high real-time.

To combine the accompanying drawing in the embodiment of the invention below, the technical scheme in the embodiment of the invention is carried out detailed description, obviously, described embodiment only is the present invention's part embodiment, rather than whole embodiment.Based on the embodiment among the present invention, the every other embodiment that those of ordinary skills are obtained under the prerequisite of not making creative work belongs to the scope that the present invention protects.

In the specific embodiment of the invention, be that example is carried out detailed description to technical scheme of the present invention with the operation of the dexterous arm table tennis of six degree of freedom apery.

The operation of the dexterous arm table tennis of apery needs the racket (heart) can be with the attitude R that requires in the batting moment ^RefArrive target location p ^Ref

The length of the ping-pong table of standard is 2740mm.The flying speed of the horizontal direction of table tennis is about 5m/s in the process of playing ball, and the ball flight time is approximately 0.5s.In time, control system will be accomplished the calculating of a series of complicacies such as table tennis visual identity, trajectory predictions, impact point calculating, the positive inverse kinematics of mechanical arm, trajectory planning and mechanical arm servocontrol at this 0.5s.Therefore, ping-pong robot has very high requirement to the real-time and the performance of control system.

The method for solving that The present invention be directed to the positive inverse kinematics of dexterous arm in the said process improves.

The dexterous arm of the six degree of freedom apery of ping-pong robot is by two degree of freedom of shoulder joint, elbow joint one degree of freedom, wrist joint three degree of freedom, neck and be connected in each interarticular connecting rod and form.The end of dexterous arm also is connected with table tennis bat (Dextrous Hand).

Join shown in Figure 1ly, set up its articulation structure model according to the structure of the dexterous arm of the six degree of freedom apery of ping-pong robot.Wherein, the shoulder heart 1 corresponding shoulder joint axle center, the elbow heart 2 corresponding elbow joint axle center, the wrist heart 3 corresponding wrist joint axle center, the neck heart 4 corresponding necks are clapped the position of the corresponding table tennis bat of the heart 5 (palm of the hand) in order to batting.

It is following to find the solution the inverse kinematics method in the positive inverse kinematics software module of the dexterous arm of six degree of freedom apery in the table tennis operation:

A, confirm the dexterous arm target of six degree of freedom apery connecting rod pose (p ^Ref, R ^Ref);

B, go out joint angle vector q through analytic calculation according to object pose;

The positive motion of c, the dexterous arm of analysis is learned, the checking joint angle vector q validity of asking.

Realize following through analytical method to the method for solving of target angle vector q:

∑ _WThe expression world coordinate system, a _i(i=1,2 ..., 6) and expression joint shaft vector, the length between the shoulder heart 1 and the neck heart 4 is D, and the shoulder heart 1 to the elbow heart 2 length are L1, and the elbow heart 2 to the wrist heart 3 length are L2, and the wrist heart 3 is L3 to clapping the heart 5 length.Be respectively p if set the position and the attitude of the neck heart 4 ₀=[0 D 0] ^T, R ₀=E; The pose of the given bat heart does

Then through the table tennis bat pose

The wrist heart 3 positions and the attitude thereof of asking for the dexterous arm connecting rod end of six degree of freedom apery are respectively:

$R_6^{\mathrm{ref}}=R_7^{\mathrm{ref}}---\left(1\right)$

$p_6^{\mathrm{ref}}=p_7^{\mathrm{ref}}-R_6^{\mathrm{ref}}\&CenterDot;{\left[\begin{array}{ccc}0& 0& -L3\end{array}\right]}^T=\left[\begin{array}{ccc}{r}_x& r_y& r_z\end{array}\right]---\left(2\right)$

Can try to achieve the distance between the wrist heart 3 and the shoulder heart 1 thus, be defined as C

$C=\sqrt{r_x^2+{\mathrm{<figure-callout\; id="2"\; label="r\; y"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">r</figure-callout>}}_y^{}+r_z^2}---\left(3\right)$

Distance between the wrist heart 3 and the neck heart 4 is defined as F

$F=\sqrt{r_x^2+{(r_y-D)}^2+r_z^2}---\left(4\right)$

Consider the triangle that the shoulder heart shown in Figure 2-elbow heart-wrist heart constitutes, can be with elbow joint q ₃Angle obtain.Have according to the cosine law

C ²＝L1 ²+L2 ²-2L1·L2·cos(π-q ₃) (5)

Thereby elbow joint q ₃Rotational angle does

${\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_3=-\arccos \left(\frac{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2+{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2-{\mathrm{<figure-callout\; id="2"\; label="C"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">C</figure-callout>}}^2}{2\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1\&CenterDot;L2}\right)+\mathrm{\&pi;}---\left(6\right)$

Analyze by the articulation structure of shoulder joint: the shoulder heart in the computing method illustraton of model of Fig. 2 shoulder joint angle-elbow heart-wrist heart-plane of 4 coexistences of the neck heart, so shoulder joint angle of pitch q ₁Angle does

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_1=\arctan (r_x/r_z)\mathrm{if}{r}_z<0\\ {\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_1=-\mathrm{\&pi;}+\arctan (r_x/r_z)\mathrm{if}{r}_z>0\end{array}\right.---\left(7\right)$

Shoulder joint deflection angle q ₂For

q ₂＝-(α+β-π/2) (8)

Wherein the α angle is constituted the triangle cosine law and is tried to achieve by the shoulder heart-elbow heart-wrist heart

$\mathrm{\&alpha;}=\arccos \left(\frac{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2+C^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1\&CenterDot;C}\right)---\left(9\right)$

The β angle is constituted the triangle cosine law and is tried to achieve by the neck heart-shoulder heart-wrist heart

$\mathrm{\&beta;}=\arccos \left(\frac{{\mathrm{<figure-callout\; id="2"\; label="C"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">C</figure-callout>}}^2+D^2-{\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">F</figure-callout>}}^2}{2\mathrm{CD}}\right)---\left(10\right)$

With (9), (8) get shoulder joint deflection angle q in (10) formula substitution ₂For

${\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_2=-(\mathrm{\&alpha;}+\mathrm{\&beta;}-\mathrm{\&pi;}/2)=\mathrm{\&pi;}/2-\arccos \left(\frac{{\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1}^2+C^2-{\mathrm{<figure-callout\; id="2"\; label="L"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}2}^2}{2\mathrm{<figure-callout\; id="1"\; label="L"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">L</figure-callout>}1\&CenterDot;C}\right)-\arccos \left(\frac{{\mathrm{<figure-callout\; id="2"\; label="C"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">C</figure-callout>}}^2+D^2-{\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">F</figure-callout>}}^2}{2\mathrm{CD}}\right)---\left(11\right)$

Remaining wrist joint swing, lift-over and three joint angles of pitching.Relation according between each connecting rod attitude does

$R_6^{\mathrm{ref}}=R_0{R}_y\left(q_1\right)R_x\left(q_2\right)R_x\left(q_3\right)R_z\left(q_4\right)R_x\left(q_5\right)R_y\left(q_6\right)---\left(12\right)$

For rotating q around joint shaft a (unit vector) _jThe time rotation matrix, rotation matrix can be asked for through following Rodrigues formula

$R_a\left(q_j\right)=e^{\hat{a}{q}_j}=E+\hat{a}\sin q_j+{\hat{a}}^2(1-\cos q_j)$

(12) formula is out of shape and can be got

$R_z\left(q_4\right)R_x\left(q_5\right)R_y\left(q_6\right)=R_x^T\left(q_3\right)R_x^T\left(q_2\right)R_y^T\left(q_1\right)R_0^T{R}_6^{\mathrm{ref}}---\left(14\right)$

The left side is launched, and calculate the value on the right, promptly get

$\left[\begin{array}{ccc}{c}_4{c}_6-s_4{s}_5{s}_6& -s_4{c}_5& c_4{s}_6+s_4{s}_5{c}_6\\ s_4{c}_6+c_4{s}_5{s}_6& c_4{c}_5& s_4{s}_6-c_4{s}_5{c}_6\\ -c_5{s}_6& s_5& c_5{c}_6\end{array}\right]=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]$

Wherein, c ₄≡ cosq ₄, s ₄≡ sinq ₄, the implication of other mark is similar.Scrutiny left side matrix and the right entry of a matrix are plain, can get following result

q ₄＝atan2(-R ₁₂，R ₂₂) (15)

q ₅＝atan2(R ₃₂，-R ₁₂s ₄+R ₂₂c ₄)(16)

q ₆＝atan2(-R ₃₁，R ₃₃) (17)

Go up and be the contrary motion problems of the dexterous arm of analytical method solving six degree of freedom apery.Separating validity for the checking kinematics of inverting need carry out the positive motion credit of dexterous arm and analyse and calculate target connecting rod position and attitude error err (Δ p, Δ R).

Can get 6 joint shaft vectors of the dexterous arm of apery a from Fig. 1 ₁a ₆For

a ₁＝[0，1，0]?a ₂＝[1，0，0]

a ₃＝[1，0，0]?a ₄＝[0，0，1] (18)

a ₅＝[1，0，0]?a ₆＝[0，1，0]

Vector is q=[q from takeing on to the midcarpal joint angle in definition ₁, q ₂, q ₃, q ₄, q ₅, q ₆], each connecting rod pose of dexterous arm

p _j=p _iYear R _ib _j

(19)

$R_j=R_i{R}_{a_j}\left(q_j\right)$

P in the formula _i, R _iShow the absolute pose of female connecting rod under world coordinate system; a _j, b _jBe joint shaft vector in female link rod coordinate system and true origin position;

For around axle a _jRotate q _jThe time rotation matrix; Then each connecting rod attitude matrix and position coordinates under world coordinate system is described as respectively

$\left\{\begin{array}{cc}{\mathrm{<figure-callout\; id="1"\; label="R"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_1=R_0\&CenterDot;R_y\left(q_1\right)& {\mathrm{<figure-callout\; id="2"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_2={\mathrm{<figure-callout\; id="1"\; label="R"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_1\&CenterDot;R_x\left(q_2\right)\\ {\mathrm{<figure-callout\; id="3"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_3={\mathrm{<figure-callout\; id="2"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_2\&CenterDot;R_x\left(q_3\right)& {\mathrm{<figure-callout\; id="4"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_4={\mathrm{<figure-callout\; id="3"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_3\&CenterDot;R_z\left(q_4\right)\\ {\mathrm{<figure-callout\; id="5"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_5={\mathrm{<figure-callout\; id="4"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_4\&CenterDot;R_x\left(q_5\right)& R_6={\mathrm{<figure-callout\; id="5"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_5\&CenterDot;R_y\left(q_6\right)\end{array}\right.---\left(20\right)$

$\left\{\begin{array}{cc}{\mathrm{<figure-callout\; id="1"\; label="p"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_1=p_0+R_0\&CenterDot;b_1& b_1=\left[\begin{array}{ccc}0& -D& 0\end{array}\right]\\ {\mathrm{<figure-callout\; id="2"\; label="p"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_2={\mathrm{<figure-callout\; id="1"\; label="p"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_1+{\mathrm{<figure-callout\; id="1"\; label="R"\; filenames="HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_1\&CenterDot;b_2& b_2=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]\\ {\mathrm{<figure-callout\; id="3"\; label="p"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_3={\mathrm{<figure-callout\; id="2"\; label="p"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_2+{\mathrm{<figure-callout\; id="2"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_2\&CenterDot;b_3& b_3=\left[\begin{array}{ccc}0& 0& -L1\end{array}\right]\\ {\mathrm{<figure-callout\; id="4"\; label="p"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_4={\mathrm{<figure-callout\; id="3"\; label="p"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">p</figure-callout>}}_3+{\mathrm{<figure-callout\; id="3"\; label="R"\; filenames="HDA0000112605050000011.png,HDA0000112605050000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_3\&CenterDot;b_4& b_4=\left[\begin{array}{ccc}0& 0& -L2\end{array}\right]\\ {\mathrm{<figure-callout\; id="5"\; label="p"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_5={\mathrm{<figure-callout\; id="4"\; label="p"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_4+{\mathrm{<figure-callout\; id="4"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_4\&CenterDot;b_5& b_5=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]\\ p_6={\mathrm{<figure-callout\; id="5"\; label="p"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_5+{\mathrm{<figure-callout\; id="5"\; label="R"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">R</figure-callout>}}_5\&CenterDot;b_6& b_6=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]\end{array}\right.---\left(21\right)$

Target connecting rod position and attitude error Function e rr (Δ p, Δ R) defines as follows:

err(Δp，ΔR)＝err(p ^ref-p，R ^TR ^ref)＝||Δp|| ²+||Δω|| ²

Wherein

$\mathrm{\&Delta;R}=\left[\begin{array}{ccc}\mathrm{\&Delta;}{r}_{11}& \mathrm{\&Delta;}{r}_{12}& \mathrm{\&Delta;}{\mathrm{<figure-callout\; id="13"\; label="r"\; filenames="HDA0000112605050000011.png"\; state="\{\{state\}\}">r</figure-callout>}}_{13}\\ \mathrm{\&Delta;}{r}_{21}& \mathrm{\&Delta;}{r}_{22}& \mathrm{\&Delta;}{r}_{23}\\ \mathrm{\&Delta;}{r}_{31}& \mathrm{\&Delta;}{r}_{32}& \mathrm{\&Delta;}{r}_{33}\end{array}\right]$ (23)

$\mathrm{\&theta;}=\arccos \left(\frac{{\mathrm{\&Delta;r}}_{11}+{\mathrm{\&Delta;r}}_{22}+{\mathrm{\&Delta;r}}_{33}-1}{2}\right)$

(p in the formula ^Ref, R ^Ref) be the given target connecting rod pose of dexterous arm, (p R) is dexterous shoulder joint angle combination dexterous arm target connecting rod pose after positive motion is learned calculating.Calculate the validity that the target connecting rod position and attitude error checking kinematics of inverting is separated through following formula.

Experiment effect of the present invention

Consider dexterous arm (right arm) articulation structure of the six degree of freedom apery that designs as shown in Figure 1 model, following table 1 is dexterous each the joint angles scope of arm of this six degree of freedom apery.

The dexterous shoulder joint angular range (degree) of table 1 six degree of freedom apery

The joint	q 1	q 2	q 3	q 4	q 5	q 6
							Lower limit	-120	-130	-20	-130	-90	-60
The upper limit	40	10	120	130	90	60

If the shoulder breadth D=0.14m of the dexterous arm of six degree of freedom apery, the shoulder heart 1 to the elbow heart 2 length L 1=0.26m, the elbow heart 2 to the wrist heart 3 distance L 2=0.25m, the wrist heart 3 is to clapping the heart 5 distance L 3=0.14m; If the shoulder heart 1 is the initial point of world coordinate system, the neck heart 4 poses are respectively p ₀=[00.140] ^TM, R ₀=E; If the dexterous arm target of the six degree of freedom apery connecting rod racket heart 5 positions do

The racket attitude does $R_7^{\mathrm{Ref}}=R_{\mathrm{Rpy}}(\mathrm{\&pi;}/2,-\mathrm{\&pi;}/\mathrm{2,0})=R_z\left(0\right)R_y(-\mathrm{\&pi;}/2)R_x(\mathrm{\&pi;}/2),$ Position and the attitude of asking for dexterous arm connecting rod end through the racket pose are respectively:

$R_6^{\mathrm{ref}}=R_7^{\mathrm{ref}}$

$p_6^{\mathrm{ref}}=p_7^{\mathrm{ref}}-R_6^{\mathrm{ref}}\&CenterDot;{\left[\begin{array}{ccc}0& 0& -L3\end{array}\right]}^T=\left[\begin{array}{ccc}0.18& 0& -0.25\end{array}\right]$

Validity for the dexterous arm analytical method solving of checking six degree of freedom apery of the present invention inverse kinematics problem; Use analytic method of the present invention, Newton-Raphson method, genetic algorithm (GA) and particle swarm optimization algorithm (PSO) to find the solution the dexterous arm inverse kinematics of six degree of freedom apery shown in Figure 1 problem respectively, the present invention sets each method parameter and is provided with as follows: Newton-Raphson method original state q ₀=[0 0 0.01 00 0], angle modification weight coefficient λ=0.50.GA and PSO algorithm population size are m=30, and variable dimension n=6 are according to 6 joint angle q shown in the table 1 _i(i=1,2 ..., 6) and variable range is respectively [120,40] π/180, [130,10] π/180, [20,120] π/180, [130,130] π/180, [90,90] π/180, [60,60] π/180 radians; The non-linear selection operator of GA algorithm use, arithmetic intersect and even mutation operator, and wherein optimum individual is selected probability P _s=0.10, crossover probability P _c=0.90, the variation probability P _m=0.10; The more new-type inertia weight coefficient of PSO algorithm particle rapidity ω=0.50, speedup factor c ₁=c ₂When the convergence precision that=2.0.Newton-Raphson method, genetic algorithm and population method are separated when iterative search reached 1.0E-6, then algorithm was evolved and is stopped; Separate the consuming time long of precision for avoiding GA and the optimizing of PSO algorithm to satisfy search in addition, setting the maximum evolutionary generation of two algorithms is T=15000.Following table 2 is three kinds of methods solve dexterous arm inverse kinematics problem institute's time-consuming of six degree of freedom and solving precision at same computing machine with under the matlab programmed environment result's comparison.

Three kinds of methods of table 2 are found the solution the dexterous arm inverse kinematics of six degree of freedom problem result relatively

	Find the solution the time (s)	Solving precision err (Δ p, Δ R)
			The inventive method (analytical method)	0.000465	Infinitely small
The Newton-Raphson method	0.025430	6.7574E-7
			Genetic algorithm	92.624714	7.1042E-5
Particle swarm optimization algorithm	4.047984	9.9252E-7

From table 2 result, find out: genetic algorithm is owing to there is the operation of complex inheritances such as selection, intersection and variation, when finding the solution dexterous arm inverse kinematics the time consuming time the longest, and solving precision is not high; Find the solution inverse kinematics problem result relatively with Newton-Raphson method, genetic algorithm (GA) and particle swarm optimization algorithm (PSO); Method of the present invention is directly found the solution each degree of freedom joint angle through analysis mode; Position and attitude error err (the Δ p of the shortest and dexterous arm target connecting rod of institute's time-consuming; Δ R) be tending towards infinitely small, solving precision is the highest, and comparative result has been verified the validity of the inventive method.

Adopt analytic method of the present invention to ask for the corresponding joint angle of the dexterous arm target connecting rod pose inverse kinematics of apery to do

Q=[0.62402305-0.89661600 1.84498835 1.04149661 0.34761169-0.52733041] radian.

In sum; The invention has the beneficial effects as follows that direct employing analytic method asks for the inverse kinematics of the dexterous arm of six degree of freedom apery; Not through the combination of numerical value iterative manner search joint angle; It is fast that this method counting yield height is found the solution speed, and find the solution accurately, precision is high, can be used in the high dexterous arm job task of real-time.

To those skilled in the art, obviously the invention is not restricted to the details of above-mentioned example embodiment, and under the situation that does not deviate from spirit of the present invention or essential characteristic, can realize the present invention with other concrete form.Therefore; No matter from which point; All should regard embodiment as exemplary; And be nonrestrictive, scope of the present invention is limited accompanying claims rather than above-mentioned explanation, therefore is intended to the implication of the equivalents that drops on claim and all changes in the scope are included in the present invention.Should any Reference numeral in the claim be regarded as limit related claim.

In addition; Describing according to embodiment though should be appreciated that this instructions, is not that each embodiment only comprises an independently technical scheme; This narrating mode of instructions only is for clarity sake; Those skilled in the art should make instructions as a whole, and the technical scheme among each embodiment also can form other embodiments that it will be appreciated by those skilled in the art that through appropriate combination.

Claims (4)

1. the rapid solving method of the dexterous arm inverse kinematics of a six degree of freedom apery; The dexterous arm of said six degree of freedom apery is by two degree of freedom of shoulder joint, elbow joint one degree of freedom, wrist joint three degree of freedom, neck and be connected in each interarticular connecting rod and form; It is characterized in that the rapid solving method of the dexterous arm inverse kinematics of said six degree of freedom apery comprises the steps:

A, set up its articulation structure model according to the structure of the dexterous arm of six degree of freedom apery, wherein, the corresponding shoulder joint of shoulder heart axle center, the corresponding elbow joint of elbow heart axle center, the corresponding wrist joint of wrist heart axle center, the corresponding neck of the neck heart;

B, confirm the pose (p of the dexterous arm target of said six degree of freedom apery connecting rod ^Ref, R ^Ref);

C, go out the inverse kinematics joint angle vector q of the dexterous arm of six degree of freedom apery through analytic calculation according to said target connecting rod pose,

Concrete realization through analytic calculation inverse kinematics joint angle vector q among the said step c is following:

Definition world coordinate system ∑ _W, said shoulder joint, elbow joint and wrist joint correspond to y respectively around axle under world coordinate system, x, x, z, x, y axle, [r _xr _yr _z] for the shoulder joint being the position coordinates of the wrist heart under the world coordinate system of initial point, elbow joint angle q ₃Triangle relation by the said shoulder heart, the elbow heart and the wrist heart constitute is asked for, shoulder joint angle of pitch q ₁With deflection angle q ₂Plane relation by the said shoulder heart, the elbow heart, the wrist heart and the neck heart constitute is asked for, and adopts following equality respectively:

$q_3=-\arccos \left(\frac{{L1}^2+{L2}^2-C^2}{2L1\&CenterDot;L2}\right)+\mathrm{\&pi;}$

$\left\{\begin{array}{c}{q}_1=\arctan (r_x/r_z)\mathrm{if}{r}_z<0\\ q_1=-\mathrm{\&pi;}+\arctan (r_x/r_z)\mathrm{if}{r}_z>0\end{array}\right.$

$q_2=\mathrm{\&pi;}/2-\arccos \left(\frac{{L1}^2+C^2-{L2}^2}{2L1\&CenterDot;C}\right)-\arccos \left(\frac{C^2+D^2-F^2}{2\mathrm{CD}}\right)$

Wherein, D is the length between the shoulder heart and the neck heart, and L1 is the length between the shoulder heart and the elbow heart, and L2 is the length between the elbow heart to the wrist heart, and F is the distance between the wrist heart and the neck heart.

2. the rapid solving method of the dexterous arm inverse kinematics of six degree of freedom apery according to claim 1 is characterized in that: asking for elbow joint angle q ₃, shoulder joint angle of pitch q ₁With deflection angle q ₂After, three joint angle q of said wrist joint ₄, q ₅And q ₆Through with the terminal attitude R of the dexterous arm of six degree of freedom apery ^RefRelation is asked for, and adopts following equality:

$R_z\left(q_4\right)R_x\left(q_5\right)R_y\left(q_6\right)=R_x^T\left(q_3\right)R_x^T\left(q_2\right)R_y^T\left(q_1\right)R_0^T{R}_6^{\mathrm{ref}}$

Wherein, R ₀Be the neck attitude matrix,

For rotating q around joint shaft a (unit vector) _jThe time rotation matrix.

3. the rapid solving method of the dexterous arm inverse kinematics of six degree of freedom apery according to claim 1 is characterized in that: also comprise the invert method of the validity of separating joint angle vector q of checking among the said step c, this verification method comprises the steps:

1) positive motion of analyzing the dexterous arm of said six degree of freedom apery is learned model, according to invert separate joint angle vector q by dexterous arm positive motion learn the pose that calculates the target connecting rod (p, R);

2) error e rr (Δ p, the Δ R)=err (p of calculating target connecting rod pose ^Ref-p, R ^TR ^Ref);

3) the checking kinematics of the inverting validity of separating, when position and attitude error err (Δ p, Δ R) is enough for a short time when approaching zero institute asks for contrary separate effective.

4. the rapid solving method of the dexterous arm inverse kinematics of six degree of freedom apery according to claim 3; It is characterized in that: said wrist joint is connected with Dextrous Hand; Said Dextrous Hand is provided with the palm of the hand, and in the said step 1): the positive motion of analyzing the dexterous arm of six degree of freedom apery is learned model, is learned the pose (p that calculates each connecting rod by positive motion according to each joint angle vector q of ask; R), adopt following equality:

$\left\{\begin{array}{cc}{R}_1=R_0\&CenterDot;R_y\left(q_1\right)& R_2=R_1\&CenterDot;R_x\left(q_2\right)\\ R_3=R_2\&CenterDot;R_x\left(q_3\right)& R_4=R_3\&CenterDot;R_z\left(q_4\right)\\ R_5=R_4\&CenterDot;R_x\left(q_5\right)& R_6=R_5\&CenterDot;R_y\left(q_6\right)\\ R_7=R_6& \end{array}\right.$

$\left\{\begin{array}{cc}{p}_1=p_0+R_0\&CenterDot;b_1& b_1=\left[\begin{array}{ccc}0& -D& 0\end{array}\right]\\ p_2=p_1+R_1\&CenterDot;b_2& b_2=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]\\ p_3=p_2+R_2\&CenterDot;b_3& b_3=\left[\begin{array}{ccc}0& 0& -L1\end{array}\right]\\ p_4=p_3+R_3\&CenterDot;b_4& b_4=\left[\begin{array}{ccc}0& 0& -L2\end{array}\right]\\ p_5=p_4+R_4\&CenterDot;b_5& b_5=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]\\ p_6=p_5+R_5\&CenterDot;b_6& b_6=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]\\ p_7=p_6+R_6\&CenterDot;b_7& b_7=\left[\begin{array}{ccc}0& 0& -L3\end{array}\right]\end{array}\right.$

P wherein ₀, R ₀Be respectively the position and the attitude of neck, p ₇, R ₇Be respectively the position and the attitude of the palm of the hand, L3 is the distance of the wrist heart to the palm of the hand; Q=[q ₁, q ₂..., q ₆] be from shoulder joint to carpal joint angle vector;

For around axle a _jRotate q _jThe time rotation matrix.

Images