CN103955619A - Inverse kinematics calculation method for minimum base disturbance analysis of seven-degree-of-freedom space manipulator - Google Patents

Abstract

The invention discloses an inverse kinematics calculation method for minimum base disturbance analysis of a seven-degree-of-freedom space manipulator. The inverse kinematics calculation method comprises the following steps: S1: carrying out geometric modeling on the seven-degree-of-freedom space manipulator, and adopting arm angles Psi to parameterize other joints except elbow joints; S2: analyzing the influence of joint extremity on the range of the arm angles Psi, analyzing the motion range of all other joints expect the elbow joints on the arm angles Psi, and determining the feasible region of one, two or three arm angles and restricting the search space of an algorithm; S3: analyzing three singularities of the manipulator such as kinematics singularity, algorithm singularity and half singularity, and eliminating the arm angles Psi not according with conditions; S4: establishing the generalized jacobian matrix of kinematical equation of the space manipulator; S5: carrying out optimizing calculation to obtain a manipulator position which can enable carrier satellite attitude change of the space manipulator to be minimum. According to the method, the base disturbance is reduced on the premise that small calculated amount is ensured, and the magnitude and calculated amount of the disturbance are balanced through the optimization algorithm.

Description

Inverse kinematics method is resolved in the minimum pedestal disturbance of a kind of seven freedom space manipulator

Technical field

The invention belongs to mechanical arm control field, relate in particular to the minimum pedestal disturbance of a kind of seven freedom space manipulator and resolve inverse kinematics method.

Background technology

In space probation, Chinese Space Manipulator Technology becomes one of main project of various countries' research.Along with going deep into of research, seven freedom redundant mechanical arm, because the characteristic of its kinematic redundancy has very outstanding effect keeping away aspect barrier, is paid attention to by various countries researcher gradually.The mechanical arm using when solving space manipulator pedestal minimal disturbances motion planning at present domestic and international prior art scheme is six degree of freedom or the mechanical arm below six degree of freedom.

The kinematics control method of general 6DOF space manipulator is that operating speed level Jacobi calculates, but can only obtain so a unique numerical solution, and the precision of velocity stage controller is be not as high as the precision of location class controller, and try to achieve its analytic solution, is unique methods that unique acquisition location class is controlled." the The Cartesian Path Planning of Free-Floating Space Robot using Particle Swarm Optimization " that such scheme is delivered as domestic Harbin Institute of Technology for 2008 etc., and " Engineering Test Satellite VII flight experiments for space robot dynamics and control Theories on laboratory test beds ten years ago, now in orbit " that abroad Tokyo University delivers for 2003 etc.

In this problem aspect redundant space mechanical arm, have more challenge, the attitude of its carrier satellite and position can be subject to the impact of manipulator motion and change, and due to the redundancy properties of mechanical arm itself, we can not obtain definite solution, can only obtain the set of a solution.Fairly perfect method is ground seven freedom mechanical arm control program, " the Analytical Inverse Kinematic Computation for 7-DOF Redundant Manipulators With Joint Limits and Its Application to Redundancy Resolution " delivering for 2008 as Tokyo Univ Japan, " the An Analytical Solution for Inverse Kinematic of 7-DOF Redundant Manipulators with Offset-Wrist " that domestic Harbin Institute of Technology delivers for 2012 etc., but these methods only can be controlled seven freedom mechanical arm on ground, can not control space manipulator.

Summary of the invention

The problem existing according to prior art, the invention discloses the minimum pedestal disturbance of a kind of seven freedom space manipulator and resolve inverse kinematics method: inverse kinematics method is resolved in the minimum pedestal disturbance of a kind of seven freedom space manipulator, comprises the following steps:

S1: seven freedom space manipulator is carried out to Geometric Modeling, adopt arm angle ψ to come parametrization all the other joints except elbow joint;

S2: the impact of the analysis of joint limit on arm angle ψ scope, analyze except elbow joint all the other the impact of articulate range of movement on arm angle ψ, determine the feasible zone at 1,2 or 3 arm angle and limit the search volume of algorithm;

S3: three kinds of analyzing mechanical arm are unusual: kinematics is unusual, algorithm is unusual and half unusual, gets rid of ineligible arm angle ψ;

S4: the generalized Jacobian of setting up the kinematical equation of space manipulator;

S5: calculate and can make the space manipulator carrier attitude of satellite change minimum mechanical arm type position by optimization.

Further, step S5 is specifically in the following way: first we adopt the attitude of hypercomplex number the Representation Equation satellite pedestal, as follows:

$Q=\mathrm{\η}+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}=\mathrm{\η}+q\∈R^4$

Now, whole system: the state equation that comprises satellite, pedestal and mechanical arm is

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\η}}\\ \stackrel{.}{q}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cc}0& -{\mathrm{\ω}}^T\\ \mathrm{\ω}& -\widetilde{\mathrm{\ω}}\end{array}\right]\left[\begin{array}{c}\mathrm{\η}\\ q\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}-q^T\\ \mathrm{\ηE}-\tilde{q}\end{array}\right]\mathrm{\ω}$

Now deflection error is:

$\left\{\begin{array}{c}\mathrm{\δ\η}={\mathrm{\η}}_{b0}{\mathrm{\η}}_{\mathrm{bf}}+q_{b0}^T{q}_{\mathrm{bf}}\\ \mathrm{\δq}={\mathrm{\η}}_{b0}{q}_{\mathrm{bf}}-{\mathrm{\η}}_{\mathrm{bf}}{q}_{b0}-{\tilde{q}}_{b0}{q}_{\mathrm{bf}}\end{array}\right.$

δ q is designated as the objective function that we will optimize herein, and its value is less, and the attitude change of satellite is just less.

While further, seven freedom space manipulator being carried out to Geometric Modeling, only by parameter of arm angle ψ, come all the other joints of parametrization.

Owing to having adopted technique scheme, inverse kinematics method is resolved in the minimum pedestal disturbance of a kind of seven freedom space manipulator provided by the invention, by space manipulator being carried out to Geometric Modeling, adopting arm angle ψ to come parametrization all the other joints except elbow joint, and by calculating and screening, get rid of ineligible arm angle ψ value, thereby obtain enough making the space manipulator carrier attitude of satellite to change minimum mechanical arm type position.The method has not only improved computational accuracy and saved computational resource, has reached the object of stable spacecraft pedestal simultaneously, guarantees to reduce pedestal disturbance under prerequisite that calculated amount is little as far as possible, and by optimized algorithm, comes size and the calculated amount of balance disturbance.

Accompanying drawing explanation

In order to be illustrated more clearly in the embodiment of the present application or technical scheme of the prior art, to the accompanying drawing of required use in embodiment or description of the Prior Art be briefly described below, apparently, the accompanying drawing the following describes is only some embodiment that record in the application, for those of ordinary skills, do not paying under the prerequisite of creative work, can also obtain according to these accompanying drawings other accompanying drawing.

Fig. 1 is the schematic diagram that closes pitch curve and arm angle feasible zone in the present invention;

Fig. 2 is S-R-S mechanical arm structure diagram in the present invention;

Fig. 3 is mechanical arm designs simplification figure in the present invention;

Fig. 4 (a), Fig. 4 (b), Fig. 4 (c), Fig. 4 (d), Fig. 4 (e), Fig. 4 (f), Fig. 4 (g) and Fig. 4 (h) are joint angle θ in the present invention _iwith arm angle ψ graph of relation;

Fig. 5 be in the present invention 7-DOF space manipulator play up figure.

Embodiment

For making technical scheme of the present invention and advantage clearer, below in conjunction with the accompanying drawing in the embodiment of the present invention, the technical scheme in the embodiment of the present invention is known to complete description:

First we explain the parameter occurring in the present invention:

Wherein R represents rotation matrix, upper left footmark representative is benchmark with this coordinate, such as b represents this rotation matrix, it is the expression under base coordinate system, bottom right footmark represents to participate in calculating the pass joint number of this rotation matrix, such as s represents, forms the synthetic of 1,2,3 three joints of shouldejoint.L represents the vector of a segment distance, and the same upper left footmark represents reference frame, and footmark two letters in bottom right represent respectively the starting point and ending point of this segment distance, such as ^bl _bsrepresentative is the expression of the distance vector from base coordinate system initial point to shoulder joint coordinate origin under base coordinate system under base coordinate system.Whole alphabetical implications is as follows:

b	Pedestal
		s	Shoulder joint
e	Elbow joint
		w	Wrist joint
t	End device

Number designation

The joint of corresponding numeral

S1: seven freedom space manipulator is carried out to Geometric Modeling, adopt arm angle to come parametrization all the other joints except elbow joint, specifically comprise the following steps:

Step 1: by the position of mechanical arm tail end device pose calculating machine arm wrist, and the size in the R joint of definite S-R-S mechanical arm;

Step 2: calculate ^br _s| _ψ=0, and carry out parametrization;

Step 3: identification joint type;

Step 4: the restriction of calculating each Dui Bei angle, joint feasible zone;

Step 5: the arm angle feasible zone that each joint is obtained is got common factor, and remove unusual point;

Step 6: by calculating different solutions for the impact of pedestal attitude, selection can change pedestal to be controlled at the feasible arm angle in minimum radius, and then calculates the Inverse Kinematics Solution of mechanical arm.In computation process, the movement velocity of mechanical arm all meets trapezoidal law of planning.

S2: the impact of the analysis of joint limit on arm angle ψ scope, analyze except elbow joint all the other the impact of articulate range of movement on arm angle ψ, determine the feasible zone at 1,2 or 3 arm angle and limit the search volume of algorithm.

Below we discuss and to adopt this parameter of arm angle to come parametrization all the other joints except elbow joint why:

In order to describe manipulator motion, learn (being the relation between joint angle size and end device pose), we need to define coordinate system.First, Σ ₀for basis coordinates system, Σ _i(i=1 ..., 7) be respectively the coordinate system in each joint.Secondly, Σ _itrue origin on the i+1 of joint, its z axle is this joint turning axle, x axle is the common vertical line of i and i+1 turning axle, y axle is defined by " right-hand rule ".Finally, as joint angle θ ₁=0 o'clock Σ ₀with Σ ₁direction is identical, and due to Σ ₇be positioned on the end device of mechanical arm, when all joint angles are zero, its z direction of principal axis is that z direction of principal axis is identical with basis coordinates, and now all joint arrangement of mechanical arm are in alignment, and end device reaches the maximal margin of work space.Although because mechanical arm D-H parameter can change along with the difference of Coordinate system definition, we still provide parameter, wherein S:sphere by the corresponding S-R-S mechanical arm of above-mentioned definition coordinate system method D-H; R:rotation; S-R-S represents ball-rotation-ball shape structure.Figure 2 shows that S-R-S mechanical arm structure diagram in the present invention.The D-H parameter of 7 degree of freedom space manipulators as shown in Table 1:

Table one

In order to describe the spin of seven freedom mechanical arm, can first joint 3 be made as to zero, allow mechanical arm be equivalent to a 6DOF mechanical arm, the position of the plane that now forearm after mechanical arm tail end device pose determination and upper arm form was zero moment for spinning, being designated as " arm angle " is zero or ψ=0, and this plane is called as " reference planes ".When 7-DOF mechanical arm occurs from supination, the plane being comprised of mechanical arm upper arm and forearm is called as " arm plane ", and now arm angle ψ is for to see to pedestal direction visual angle from end device direction, and the angle that need to be turned over to arm plane by reference planes, as accompanying drawing 3.

When mechanical arm is simplified to said structure, the rotation matrix between each joint of mechanical arm also can be simplified to following form:

^bR _s＝ ⁰R ₁· ¹R ₂· ²R ₃

^sR _e＝ ³R ₄ (1)

^eR _t＝ ⁴R ₅· ⁵R ₆· ⁶R ₇

Length of connecting rod between joint also can represent with vector:

$\begin{array}{cc}l_{\mathrm{bs}}^b={\left[\begin{array}{ccc}0& 0& d_{\mathrm{bs}}\end{array}\right]}^T& l_{\mathrm{se}}^s={\left[\begin{array}{ccc}0& -d_{\mathrm{se}}& 0\end{array}\right]}^T\\ l_{\mathrm{ew}}^e={\left[\begin{array}{ccc}0& 0& d_{\mathrm{ew}}\end{array}\right]}^T& l_{\mathrm{wt}}^w={\left[\begin{array}{ccc}0& 0& d_{\mathrm{wt}}\end{array}\right]}^T\end{array}$

Now the position of end device and attitude can be expressed as:

^bx _t＝ ^bl _bs+ ^bR _s·[ ^sl _se+ ^sR _e·( ^el _ew+ ^eR _w· ^wl _wt)] (2)

^bR _t＝ ^bR _s· ^sR _e· ^eR _t

Because " spin " mentioned in foregoing can be known after the pose determination of end device, the upper arm of mechanical arm and forearm can be connected round shoulder joint become axle rotation with wrist joint, by formula (2) the first row, can release this axle with respect to the expression formula of basis coordinates system to be:

^bx _sw＝ ^bx _t- ^bl _bs- ^bR _t· ^tl _wt (3)

＝ ^bR _s·( ^sl _se+ ^sR _e· ^el _ew)

Because right-hand side the first row in formula (3) is constant, so can prove that carpal position will can not change after end device pose is fixing, position due to shoulder joint can not change yet simultaneously, we can calculate the angle of elbow joint by the triangle being comprised of upper arm, forearm and shoulder joint and carpal line by " cosine law ", we know that elbow joint only has a joint θ certainly ₄:

${\mathrm{\θ}}_e={\mathrm{\θ}}_4=a\cos \frac{{\left|\right|x_{\mathrm{sw}}^b\left|\right|}^2-d_{\mathrm{se}}^2-d_{\mathrm{ew}}^2}{2\·d_{\mathrm{se}}\·d_{\mathrm{ew}}}---\left(4\right)$

By arc cosine computing formula (4), can obtain positive and negative two results, but due to " spin ", in the time of can allowing one of them result and ψ=0 when ψ=π, another one comes to the same thing, so that we only retain a positive result is just passable.

Because the size in the R joint of S-R-S mechanical arm is unique, determine, now only follow the position of mechanical arm wrist ^br _srelevant, again known when end device pose fixedly time wrist location constant, can release ^br _sconstant, and wherein only comprise the θ that forms shoulder joint ₁, θ ₂, θ ₃.In order to carry out parametrization joint angle θ by redundancy ₁, θ ₂, θ ₃, we use

$R_{\mathrm{\ψ}}^b=I_3+\sin \mathrm{\ψ}\·\tilde{u}_{\mathrm{sw}}^b+(1-\cos \mathrm{\ψ})\·\tilde{u}_{\mathrm{sw}}^2{}_b---\left(5\right)$

Express round upper axle and turned over the rotation matrix that ψ produces.And the direction of upper arm can be expressed as:

^bR _s＝ ^bR _ψ· ^bR _s| _ψ＝0 (6)

^br _s| _ψ=0represent to work as the direction of upper arm in the situation that mechanical arm is aforesaid equivalent 6-DOF mechanical arm, now θ ₃=0, we can obtain by the rotation matrix of premultiplication formula (5) the upper arm direction of S-R-S mechanical arm.We can calculate formula (6) substitution formula (3) right-hand side the second row ^br _s| _ψ=0.

In order to calculate the angle in three joints that form shoulder joint, we can obtain formula (5) substitution formula (6) abbreviation:

^bR _s＝A _ssinψ+B _scosψ+C _s (7)

Wherein: $A_s=\tilde{u}_{\mathrm{sw}}^b\·R_s^b{|}_{\mathrm{\ψ}=0},B_s=-\tilde{u}_{\mathrm{sw}}^2{}_b\·R_s^b{|}_{\mathrm{\ψ}=0},C_s=u_{\mathrm{sw}}^b\·u_{\mathrm{sw}}^T{}_b\·R_s^b{|}_{\mathrm{\ψ}=0}$

Attention: if be ^bu _swtilde have in symbol table, rotation matrix formula can be calculated by formula (1) the first row simultaneously:

$R_s^b=\left[\begin{array}{ccc}*& -\cos {\mathrm{\θ}}_1\·\sin {\mathrm{\θ}}_2& *\\ *& -\sin {\mathrm{\θ}}_1\·\sin {\mathrm{\θ}}_2& *\\ -\sin {\mathrm{\θ}}_2\·\cos {\mathrm{\θ}}_3& -\cos {\mathrm{\θ}}_2& \sin {\mathrm{\θ}}_2\·\sin {\mathrm{\θ}}_3\end{array}\right]$

We can be by calculating respectively tan θ ₁= ^br _s(2,2)/ ^br _s(1,2), cos θ ₂=- ^br _s(3,2), tan θ ₃=- ^br _s(3,3)/ ^br _s(3,1) obtain:

θ ₁＝atan2(-a _s22·sinψ-b _s22·cosψ-c _s22,-a _s12·sinψ-b _s12·cosψ-c _s12)

θ ₂＝acos(-a _s32·sinψ-b _s32·cosψ-c _s32) (8)

θ ₃＝atan2(a _s33·sinψ+b _s33·cosψ+c _s33,-a _s31·sinψ-b _s31·cosψ-c _s31)

Wherein, a _sij, b _sij, c _sijbe respectively A _s, B _s, C _s(i, j) individual element.

In like manner we can be by formula (7) and ^sr _ethe second row of substitution formula (2), obtains:

^eR _t＝A _wsinψ+B _wcosψ+C _w

Wherein: $A_w=R_e^T{}_s\·A_s^T\·R_t^b,B_w=R_e^T{}_s\·B_s^T\·R_t^b,C_w=R_e^T{}_s\·C_s^T\·R_t^b$

By formula (1) the third line, can be obtained:

$R_t^e=\left[\begin{array}{ccc}*& *& \cos {\mathrm{\θ}}_5\·\sin {\mathrm{\θ}}_6\\ *& *& \sin {\mathrm{\θ}}_5\·\sin {\mathrm{\θ}}_6\\ -\sin {\mathrm{\θ}}_6\·\cos {\mathrm{\θ}}_7& \sin {\mathrm{\θ}}_6\·\sin {\mathrm{\θ}}_7& \cos {\mathrm{\θ}}_6\end{array}\right]$

Have:

θ ₅＝atan2(a _w23·sinψ+b _w23·cosψ+c _w23,a _w13·sinψ+b _w13·cosψ+c _w13)

θ ₆＝acos(a _w33·sinψ+b _w33·cosψ+c _w33) (9)

θ ₇＝atan2(a _w32·sinψ+b _w32·cosψ+c _w32,-a _w31·sinψ-b _w31·cosψ-c _w31)

So far our θ that only used arm angle this variable parameter of ψ ₁, θ ₂, θ ₃, θ ₅, θ ₆, θ ₇, and θ ₄irrelevant with ψ, only relevant with end device pose.

For some reason, such as each joint of mechanical arm that affects of obstacle of mechanical arm physical construction, electric equipment or wire etc. has a limit, and again due to the spin of S-R-S mechanical arm, still not unique even if end device pose is fixed its inverse kinetics solution.In order to exclude the inverse kinetics solution outside the limit of joint, obtain all inverse kinetics solutions in the limit of joint, below we will discuss unique parameter---the arm angle ψ of these joint limit when how to affect joint parameter.

Except θ ₄be not subject to outside ψ affects, we can find out θ by observation type (8) and (9) _iand the relation between ψ only has two kinds of situations, i.e. tan type and cos type, below we discuss a minute situation.

Tan Type: facilitate us can be θ in order to discuss ₁, θ ₃, θ ₅, θ ₇expression formula write as the form of implicit function:

$\tan {\mathrm{\θ}}_i=\frac{a_n\·\sin \mathrm{\ψ}+b_n\·\cos \mathrm{\ψ}+c_n}{a_d\·\sin \mathrm{\ψ}+b_d\·\cos \mathrm{\ψ}+c_d}---\left(10\right)$

We get micro-ly by implicit function above with gathering the differential equation, obtain:

$\frac{d{\mathrm{\θ}}_i}{\mathrm{d\ψ}}=\frac{a_t\·\sin \mathrm{\ψ}+b_t\·\cos \mathrm{\ψ}+c_t}{f_n^2\left(\mathrm{\ψ}\right)+f_d^2\left(\mathrm{\ψ}\right)}---\left(11\right)$

Wherein: a _t=b _dc _n-b _nc _d, b _t=a _nc _d-a _dc _n, c _t=a _nb _d-a _db _n, and respectively molecule and the denominator of formula (10).

Because formula (11) is the differential expression of function (10), the characteristic that it can representative function (10).Below we discuss θ _ican there be period-luminosity relation and two kinds of situations of monotonic relationshi with ψ.According to Derivative Definition, when we make its molecule be zero, can calculate and can make and θ _ithe ψ value that rest point is corresponding:

If clearly θ _i(ψ) there is no extreme point.Function # in this case _i(ψ) be that monotonic quantity is as accompanying drawing 4 (a).

If θ _i(ψ) will inevitably locate to reach maximum value in one of two results of above formula, and another place reaches minimal value, as accompanying drawing 4 (e).We are respectively by corresponding θ _imaximum value and minimal value ψ value called after ψ ^maxand ψ ^min, what do not obscure is that ψ is in [π, π] interval value, itself there is no maximum value and minimizing concept.

If θ _i(ψ) only have an extreme point, as Fig. 4 (c) and Fig. 4 (d), we can notice θ when ψ is positioned at this extreme point _ican not be well-determined, now for tan type calculate unusual, same following we can carry out special discussion to S-R-S mechanical arm various unusual.

Cos type: with tan type we first by θ ₂, θ ₄write as the form of implicit function:

cosθ _i＝a·sinψ+b·cosψ+c

Next we to implicit function, differentiate obtains:

$\frac{{\mathrm{d\θ}}_i}{\mathrm{d\ψ}}=-\frac{1}{\sin {\mathrm{\θ}}_i}(a\·\cos \mathrm{\ψ}-b\·\sin \mathrm{\ψ})$

As sin θ _i≠ 0 o'clock, another above formula equals zero, and we can solve:

Can prove θ _iin a solution in above formula, reach maximum value respectively, in another one solution, reach minimal value, as Fig. 4 (f).As sin θ _i=0, θ when we cannot determine cos type _i(ψ) gradient, we can be easily by judgement a ²+ b ²-(c-1) ²=0 or a ²+ b ²-(c+1) ²whether=0 be satisfied to judge this kind of situation.And we can find out its rule by Fig. 4 (g) Fig. 4 (h), now unusual for the calculating of cos type, we will be unusual in hereinafter discussing in detail equally.

It should be noted that Fig. 4 (b), as can be seen from the figure, this curve is not around point (0,0) Central Symmetry, and these are slightly different with Fig. 4 (a).In fact this figure is S-R-S mechanical arm the 5th and the 7th the issuable result in joint, certainly no matter be preiodic type or Monotone Type or singular form, it (is not symmetrical with respect to ψ=0 during preiodic type around ψ=0 symmetrical situation of the moment that last 3 joints of mechanical arm all may produce its movement locus with respect to ψ, Monotone Type and singular form are all with respect to (0, Central Symmetry), and front 3 joints are constantly symmetrical around ψ=0 forever 0).In fact these symmetric points are only relevant with the attitude of end device, or with z axle in attitude, be more only that z axle clamp angle is relevant with basis coordinates, the formula that we can be by below easily definite rear three joint angles with respect to the value of the axis of symmetry of the curve of ψ or the ψ at symcenter place:

eul＝dc2eul( ^bR _t)

$\mathrm{axis}=-\frac{\mathrm{eul}\left(2\right)}{\mathrm{abs}\left(\mathrm{eul}\left(2\right)\right)}\·(\mathrm{\π}-\mathrm{eul}\left(2\right))$

We first change into by end device attitude the form representing by Z-Y-Z Eulerian angle, and then calculate the angle turning over around y axle.

By observe Fig. 4 (a), Fig. 4 (b), Fig. 4 (c), Fig. 4 (d), Fig. 4 (e), Fig. 4 (f), Fig. 4 (g) and Fig. 4 (h) we the joint angle θ that affected by arm angle ψ ₁, θ ₂, θ ₃, θ ₅, θ ₆, θ ₇divide substantially for 3 classes with the relation of ψ, i.e. Monotone Type (only comprising tan type), preiodic type (comprising tan and cos type) and singular form (comprising tan and cos type).Fig. 4 (a) is that dull 1 type, Fig. 4 (b) are for dull 2 types, Fig. 4 (c) are for unusual 1 type of tangent, Fig. 4 (d) are unusual 2 types of tangent, for tangent cycle 1 type, Fig. 4 (f), for cosine cycle 1 type, Fig. 4 (g), for unusual 1 type of cosine, Fig. 4 (h), below unusual 2 types of cosine, we discuss respectively the meeting impact on unique parameter ψ in these joints in these three kinds of situations of the joint limit to Fig. 4 (e).

Monotone Type: θ in such cases _iunique corresponding with ψ, so when the joint limit is determined, can be by its upper limit and lower limit in substitution equation (10), can obtain the ψ upper limit ψ of unique correspondence with it respectively ^uwith lower limit ψ ^l, this is the situation of monotone increasing certainly, when function is monotone decreasing corresponding ψ ^l, corresponding ψ ^u.

Preiodic type: although this kind of situation comprised tan type and two kinds of relationship types of cos type, they are as broad as long, and we discuss in the lump.We can see, work as θ from Fig. 4 (e) Fig. 4 (f) _iwhile there is the limit, if or time can make θ _ithe ψ value that reaches extreme point only has ψ ^uand ψ ^l.But work as or time we have two corresponding ψ of ψ value ^uor there are two corresponding ψ of ψ value ^l, this makes to determine that feasible ψ scope becomes difficult, has understood that after this point, we analyze for 5 kinds of different situations:

Case1: or

Now there is not feasible arm angle ψ

Case2: and

ψ now ^maxstill corresponding but corresponding ψ ^minbut not in the scope of feasible arm angle ψ, we need by replace the θ of following formula _limit:

${\mathrm{\ψ}}^{\mathrm{lu}}=2\·a\tan \frac{-b_{\mathrm{\θ}}\±\sqrt{{b_{\mathrm{\θ}}}^2-4\·a_{\mathrm{\θ}}\·c_{\mathrm{\θ}}}}{2\·a_{\mathrm{\θ}}}---\left(13\right)$

Wherein, a _θ=tan (θ _limit) (c _d-b _d)-c _n+ b _n, b _θ=2tan (θ _limit) a _d-a _n, c _θ=tan (θ _limit) (c _d+ b _d)-c _n-b _n.Obtain and two solutions, and

Case3: and

ψ now ^minstill corresponding but corresponding ψ ^maxbut not in the scope of feasible arm angle ψ, we need by θ in alternate form (13) _limit, obtain and two solutions, and $\mathrm{\ψ}\∈[-\mathrm{\π},{\mathrm{\ψ}}_1^u]\∪[{\mathrm{\ψ}}_2^u,\mathrm{\π}].$

Case4: or

Now corresponding ψ ^minand correspondence ψ ^maxnot in the scope of feasible arm angle ψ, we need to be respectively by with θ in alternate form (13) _limit, obtain respectively and two solutions with and the feasible region of ψ is divided into two kinds of situations: (1) when with in separated situation $\mathrm{\ψ}\∈[-\mathrm{\π},{\mathrm{\ψ}}_1^l]\∪[{\mathrm{\ψ}}_2^l,{\mathrm{\ψ}}_1^u]\∪[{\mathrm{\ψ}}_2^u,\mathrm{\π}];$ (2) when $[{\mathrm{\ψ}}_1^l,{\mathrm{\ψ}}_2^l]\∈[{\mathrm{\ψ}}_1^u,{\mathrm{\ψ}}_2^u]$ or $[{\mathrm{\ψ}}_1^u,{\mathrm{\ψ}}_2^u]\∈[{\mathrm{\ψ}}_1^l,{\mathrm{\ψ}}_2^l]$ In situation $\mathrm{\ψ}\∈[{\mathrm{\ψ}}_1^u,{\mathrm{\ψ}}_1^l]\∪[{\mathrm{\ψ}}_2^l,{\mathrm{\ψ}}_2^u]$ or $\mathrm{\ψ}\∈[{\mathrm{\ψ}}_1^l,{\mathrm{\ψ}}_1^u]\∪[{\mathrm{\ψ}}_2^u,{\mathrm{\ψ}}_2^l].$

Case5: and

ψ now ^maxcorresponding ψ ^mincorresponding ψ ∈ [π, π].

S3: three kinds of analyzing mechanical arm are unusual: kinematics is unusual, algorithm is unusual and half unusual

Singular form: we process various unusual problems at unification here.We can be divided into that 3 classes---kinematics is unusual by unusual substantially according to different problems: degree of freedom, algorithm that now mechanical arm can lose in certain direction are unusual: now some formula cannot obtain a result, half unusual: what by the joint limit, caused is unusual.

(1) kinematics is unusual is divided into again that shoulder is unusual, ancon is unusual and wrist is unusual, wherein ancon is unusual and but wrist is unusual can not produce the unusual generation of any impact shoulder time, the position of working as wrist is on the extended line of connecting rod 1, the size in joint 1 cannot uniquely determine, we can be by by θ now ₁directly being set to zero, to solve shoulder unusual.

(2) algorithm is unusual: we can be by judgement a ²+ b ²-(c-1) ²=0 or a ²+ b ²-(c+1) ²=0 judges that cos type reaches unusual, by judge that tan type reaches calculating unusual.

By Fig. 4 (g), Fig. 4 (h) even if we can find out reaches singular point ψ=0 (calculating front 3 joints) or ψ=axis (calculating rear 3 joints) by cos we still can unique θ that determines this point constantly _ivalue, only due to sin θ _i=0 we cannot through type (13) calculate the θ of this point _ivalue, but we can determine θ by calculating its Derivative limit on the left or on the right _ivalue, and cos singular form is still preiodic type function, so its computation rule and preiodic type about arm angle ψ is general.But tan singular form is different, we can observe from Fig. 4 (c), Fig. 4 (d), and as ψ, we can not determine θ during in singular point _ivalue, so tan singular form (is easy to just can prove that tan singular form is dull function except singular point) except being similar to Monotone Type, we also will be from deducting the ψ value of singular point between whole feasible arm angular region.

(3) half is unusual: half unusual be occur when joint of mechanical arm reaches joint extreme position a kind of unusual, this unusual than the unusual solution of kinematics get up difficulty many, we make the final Inverse Kinematics Solution of mechanical arm avoid half singular problem away from each joint limit with a simple monobasic optimized algorithm.

S4: set up the generalized Jacobian of the kinematical equation of space manipulator, this Jacobi matrix equation

For: $\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=[J_m-J_b{I}_b^{-1}{I}_{\mathrm{bm}}]\stackrel{\·}{\mathrm{\Θ}}=J^{*}({\mathrm{\ψ}}_b,\mathrm{\Θ},m_i,I_i)\stackrel{\·}{\mathrm{\Θ}}$

Herein, v _eand ω _ebe respectively linear velocity and the angular velocity of end effector, J _mand J _bbe respectively the Jacobi matrix of mechanical arm and pedestal, I _band I _bmbe respectively the inertial matrix of pedestal and the coupling inertial matrix between pedestal and mechanical arm, for joint of mechanical arm velocity.J ^*for the symbol of generalized Jacobian, Ψ _bfor the attitude of pedestal, m _iand I _ibe respectively quality and the inertial matrix of each connecting rod of mechanical arm.

S5: calculate and can make the space manipulator carrier attitude of satellite change minimum mechanical arm type position by optimization.First we express the attitude of satellite pedestal with hypercomplex number equation:

$Q=\mathrm{\η}+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}=\mathrm{\η}+q\∈R^4$

Now the state equation of whole system (comprising satellite, pedestal and mechanical arm) is:

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\η}}\\ \stackrel{.}{q}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cc}0& -{\mathrm{\ω}}^T\\ \mathrm{\ω}& -\widetilde{\mathrm{\ω}}\end{array}\right]\left[\begin{array}{c}\mathrm{\η}\\ q\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}-q^T\\ \mathrm{\ηE}-\tilde{q}\end{array}\right]\mathrm{\ω}$

Now deflection error is:

$\left\{\begin{array}{c}\mathrm{\δ\η}={\mathrm{\η}}_{b0}{\mathrm{\η}}_{\mathrm{bf}}+q_{b0}^T{q}_{\mathrm{bf}}\\ \mathrm{\δq}={\mathrm{\η}}_{b0}{q}_{\mathrm{bf}}-{\mathrm{\η}}_{\mathrm{bf}}{q}_{b0}-{\tilde{q}}_{b0}{q}_{\mathrm{bf}}\end{array}\right.$

δ q is designated as the objective function that we will optimize herein, and its value is approximately little, and the attitude change of satellite is just less.

Embodiment:

The link parameters of space manipulator can represent as follows:

d _bs＝0.13md _se＝1.62md _ew＝1.46md _wt＝0.08m,

Table two is the constraint condition in a space manipulator joint:

Table two

Table three is the mass property of each connecting rod of space manipulator:

Body comprises that mechanical arm pedestal gross mass is 1146.347kg, pedestal with respect to the position at satellite geometry center be [0 0.850 0.476] (m), total inertial matrix of satellite and pedestal is:

$I_0=\left[\begin{array}{ccc}291.1& 10.16& -25.15\\ 10.16& 536.3& -4.100\\ -25.15& -5.000& 669.6\end{array}\right]$

Suppose mechanical arm 20 seconds working times altogether, accelerate and time of slowing down is 3 seconds, end device with respect to the expected pose at satellite geometry center is:

$T_d=\left[\begin{array}{cccc}0.105& 0.824& 0.557& 1.500\\ 0.653& 0.365& -0.663& 1.200\\ -0.750& 0.433& -0.500& 0.700\\ 0& 0& 0& 1\end{array}\right]$

We can obtain the scope at its counterpart arm angle from the restriction in each joint:

ψ ₁∈[-180 180]deg

ψ ₂∈[-180 180]deg

ψ ₃∈[-159.454 159.454]deg

ψ ₄∈[-180 180]deg

ψ ₅∈[-180 116.769]deg

ψ ₆∈[-180 180]deg

ψ ₇∈[-180 -38.236]∪[32.140 180]deg.

Above-mentioned feasible zone is got to common factor, and we obtain

[-159.454 -38.216]∪[32.143 116.769]deg

Its visual performance can see shown in Fig. 1, and now optimum arm angle is

ψ ^opt＝-96.452.

The size in each joint that this arm angle is corresponding is

[70.389 92.328 -97.949 121.281 89.096 38.009 28.5831]deg.

When ψ=0, we have the value of other one group of joint position

[15.479 29.437 0 121.282 -95.001 52.2726 175.764]deg.

We can notice that result joint seven is not above in its joint constraints, and also make the pedestal of space manipulator keep as far as possible stable through the result of optimized algorithm, and now the attitude of space manipulator pedestal changes and is expressed as by Eulerian angle:

ω _e＝[17.029 5.658 1.632] ^Tdeg.

Clearly, except x direction of principal axis changes greatly, y axle and z direction of principal axis change all less, and this is just meeting the thought of our method.

The above; it is only preferably embodiment of the present invention; but protection scope of the present invention is not limited to this; anyly be familiar with those skilled in the art in the technical scope that the present invention discloses; according to technical scheme of the present invention and inventive concept thereof, be equal to replacement or changed, within all should being encompassed in protection scope of the present invention.

Claims (3)

1. an inverse kinematics method is resolved in the minimum pedestal disturbance of seven freedom space manipulator, it is characterized in that: the method comprises the following steps:

S1: seven freedom space manipulator is carried out to Geometric Modeling, adopt arm angle ψ to come parametrization all the other joints except elbow joint;

S2: the impact of the analysis of joint limit on arm angle ψ scope, analyze except elbow joint all the other the impact of articulate range of movement on arm angle ψ, determine the feasible zone at 1,2 or 3 arm angle and limit the search volume of algorithm;

S3: three kinds of analyzing mechanical arm are unusual: kinematics is unusual, algorithm is unusual and half unusual, gets rid of ineligible arm angle ψ;

S4: the generalized Jacobian of setting up the kinematical equation of space manipulator;

S5: calculate and can make the space manipulator carrier attitude of satellite change minimum mechanical arm type position by optimization.

2. inverse kinematics method is resolved in the minimum pedestal disturbance of a kind of seven freedom space manipulator according to claim 1, be further characterized in that: step S5 specifically in the following way: first we adopt the attitude of hypercomplex number the Representation Equation satellite pedestal, as follows:

$Q=\mathrm{\η}+q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}=\mathrm{\η}+q\∈R^4$

Now, whole system: the state equation that comprises satellite, pedestal and mechanical arm is

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\η}}\\ \stackrel{.}{q}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cc}0& -{\mathrm{\ω}}^T\\ \mathrm{\ω}& -\widetilde{\mathrm{\ω}}\end{array}\right]\left[\begin{array}{c}\mathrm{\η}\\ q\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}-q^T\\ \mathrm{\ηE}-\tilde{q}\end{array}\right]\mathrm{\ω}$

Now deflection error is:

$\left\{\begin{array}{c}\mathrm{\δ\η}={\mathrm{\η}}_{b0}{\mathrm{\η}}_{\mathrm{bf}}+q_{b0}^T{q}_{\mathrm{bf}}\\ \mathrm{\δq}={\mathrm{\η}}_{b0}{q}_{\mathrm{bf}}-{\mathrm{\η}}_{\mathrm{bf}}{q}_{b0}-{\tilde{q}}_{b0}{q}_{\mathrm{bf}}\end{array}\right.$

δ q is designated as the objective function that we will optimize herein, and its value is less, and the attitude change of satellite is just less.

3. inverse kinematics method is resolved in the minimum pedestal disturbance of a kind of seven freedom space manipulator according to claim 1, is further characterized in that: when seven freedom space manipulator is carried out to Geometric Modeling, only by parameter of arm angle ψ, come all the other joints of parametrization.