CN106003057A - Rapid judging method for configuration singularity of mechanical arm with redundant degree of freedom - Google Patents

Abstract

The invention discloses a rapid judging method for configuration singularity of a mechanical arm with the redundant degree of freedom. The method is characterized by including the following steps that the tail end speed and the joint angular speed of the mechanical arm with the redundant degree of freedom meet the mapping relation that x=J*q, wherein x represents the tail end speed, q represents the joint angular speed, and J represents the jacobian matrix; the maximum singular value and the minimum singular value of the jacobian matrix are calculated, the specific value of the maximum singular value and the minimum singular value of the jacobian matrix is used as the conditional number, and when the conditional number is greater than a set number, it is determined that the configuration of the mechanical arm has singularity; and in the process of calculating the conditional number of the jacobian matrix, real-time values of the maximum singular value and the minimum singular value are obtained through intermediate result iteration based on generalized inverse calculation, and the requirement for calculating the conditional number in real time is met. By means of the method, the complex calculating problem of obtaining the conditional number of the jacobian matrix through singular value decomposition is solved, and meanwhile the calculating efficiency is improved.

Description

A kind of redundant degree of freedom quick decision method of mechanical arm configuration singularity

Technical field

The invention belongs to robotics, the configuration singularity relating to a kind of redundant degree of freedom mechanical arm is quick Decision method.

Background technology

When n DOF (degree of freedom) redundance mechanical arm reaches certain particular joint morpheme, i-th joint produces tag wire speed Direction (i=1,2 ..., n) or i-th joint produce terminal angle speed direction (i=l, 2 ..., n) Coplanar, and terminal velocity can not be produced, then in the normal direction vertical direction of straight line (or altogether) of this plane It may be said that relative in the normal direction (or being total to the vertical direction of straight line) of this plane, redundant mechanical arm produces Unusual.

The mechanical arm of redundant degree of freedom is conducive to singularity avoidance morpheme.But not all Singularities is all Can avoid.Therefore the analysis for singular configuration is adjudicated necessary.

At present, the method that many researchs all have employed more complicated Differential Geometry or differential topology, and be Unusual of overall importance of study movement, the research tool of algebraic topology also Zeng Zuowei mechanical arm Singularity Analysis.But Being the particular application for spatial environments etc., the controller resource of mechanical arm is extremely limited, and to speed Having suitable requirement, the existing method of document is all not suitable for.Accordingly, it would be desirable to a kind of reliable and quick decision algorithm, It is most important for the operation safety precautions of space manipulator.

Summary of the invention

In view of this, the invention provides a kind of redundant degree of freedom quick decision method of mechanical arm configuration singularity, Solve the complicated calculations problem being asked for Jacobian matrix conditional number by singular value decomposition, accelerate to calculate simultaneously Efficiency.

In order to solve above-mentioned technical problem, the present invention is achieved in that a kind of redundant degree of freedom mechanical arm structure The unusual quick decision method of type, comprises the steps:

Step one, for redundant degree of freedom mechanical arm, exist such as between its tip speed and each joint angle speed Lower mapping relations:

Wherein,Represent tip speed,Representing joint angle speed, J represents Jacobian matrix；

Step 2, the maximum singular value calculating described Jacobian matrix and minimum singular value, with maximum singular value It is conditional number with the ratio of minimum singular value, when conditional number is more than setting numerical value, then deteminate machine mechanical arm configuration Occur unusual；

During calculating the conditional number of described Jacobian matrix, use and calculate intermediate object program repeatedly based on generalized inverse In generation, asks for the instantaneous value of maximum singular value and minimum singular value, reaches the requirement of real-time design conditions number.

Further, numerical value is set as 1000.

Further, calculate intermediate object program based on generalized inverse and include A and B two, wherein A=J J^T, B=A^-1, I.e. A is 6 × 6 rank Hermite matrixes, and B is the inverse matrix of A；

Then maximum singular value δ₀Instantaneous value δ_0,kFollowing iterative formula is used to solve:

δ_0,k=max (A I_0,k)

I_0,k+1=A I_0,k/δ_0,k

Wherein, I_0,kThe intermediate vector solved for maximum singular value, I_0,0For [1 1111 1]^T, k=0,1 ..., N, N is iterations.

After n times iteration, δ₀=δ_0,N-1。

Minimum singular value δ₅Instantaneous value δ_5,kFollowing iterative formula is used to solve:

δ_5,k=max (B I_5,k)

I_5,k+1=B I_5,k/δ_5,k

Wherein, I_5,kThe intermediate vector solved for minimum singular value, I_5,0For [1 1111 1]^T。

After n times iteration, δ₅=1/ δ_5,N-1。

Then conditional number is Cond=δ₀/δ₅。

Beneficial effect:

The present invention is according to the special nature of single redundancy degree-of-freedom manipulator, by the generalized inverse meter of its Jacobian matrix Calculating formula is divided into two parts to describe；Recycling iterative method asks for maximum singular value and the minimum singular value of matrix. Solve the complicated calculations problem being asked for Jacobian matrix conditional number by singular value decomposition, accelerate meter simultaneously Calculate efficiency, reduce the resource requirement to controller hardware (FPGA etc. process chip), be favorably improved task Complete reliability.

Accompanying drawing explanation

Maximal condition number relative error under Fig. 1 difference iterations；

Maximal condition number relative error and match value under Fig. 2 difference iterations；

Fig. 3 conditional number theoretical value compares (iterations 50) with value of calculation；

Fig. 4 conditional number relative error；

Fig. 5 this method flow chart.

Detailed description of the invention

Develop simultaneously embodiment below in conjunction with the accompanying drawings, describes the present invention.

During Mechanical transmission test calculates, exist certain between its tip speed and each joint angle speed Mapping relations,

$\stackrel{\·}{x}=J\·\stackrel{\·}{q}$

In formula,Represent tip speed,Representing joint angle speed, J represents Jacobian matrix.

When judging mechanical arm configuration singularity, generally calculate the conditional number of Jacobian matrix, when conditional number tends to Time infinitely great, represent that mechanical arm configuration occurs unusual.The present embodiment is specifically configured to conditional number more than 100 Time be infinity.

Therefore, it is judged that the conditional number that it is critical only that calculating Jacobian matrix of configuration singularity.

During calculating, use and ask for minimum and maximum singular value based on generalized inverse calculating intermediate object program iteration, Reach the requirement of real-time design conditions number.

Jacobian matrix be 6 × 7 matrixes be J, its generalized inverse matrix is J⁺.During generalized inverse calculates, There are two intermediate variable (1) A=J J^T；(2) B=A^-1.This algorithm utilize the two intermediate variable carry out The calculating of conditional number.

Wherein, A is 6 × 6 rank Hermite matrixes, and B is the inverse of A.

Maximum singular value δ₀Solve iterative formula:

V_k=A I_0,k

δ_0,k=max (V_k)

I_0,k+1=V_k/δ_0,k

Wherein, I_0,0For initial vector [1 1111 1]^T, k=0,1 ..., N.N is iterations.

After n times iteration, δ₀=δ_0,N-1。

Minimum singular value δ₅Solve iterative formula:

V_k=B I_5,k

δ_5,k=max (V_k)

I_5,k+1=V_k/δ_5,k

Wherein, I_5,0For initial vector [1 1111 1]^T, k=0,1 ..., N.N is iterations.

After n times iteration, δ₅=1/ δ_5,N-1。

According to definition, conditional number is Cond=δ₀/δ₅。

Below in conjunction with the accompanying drawings the computation complexity of the present invention is described further.

During unusual judgement calculates in real time, single iteration Floating-point Computation number of times is 78 times (thinks floating-point division It is multiply-add 2 times), then when iterations is N, the calculating time is 40N microsecond.When iterations is 50 Time, the calculating time is 2 milliseconds.

By Fig. 1 it will be seen that the most maximum relative errors of iterations are the least.During iterations 22 times, by mistake Differ from a magnitude；During iterations 50 times, two magnitudes of error；During iterations 100 times, error four Magnitude.

In actual use, during whole arm configuration singularity, conditional number decision threshold should test feelings according to reality Condition is set, and 100 iteration can be used in worst case to calculate, whole calculating 4 milliseconds. Thereby ensure that conditional number calculates the real-time of process, effectiveness.

Fig. 5 is this method flow chart.

In sum, these are only presently preferred embodiments of the present invention, be not intended to limit the guarantor of the present invention Protect scope.All within the spirit and principles in the present invention, any modification, equivalent substitution and improvement etc. made, Should be included within the scope of the present invention.

Claims (3)

1. the redundant degree of freedom quick decision method of mechanical arm configuration singularity, it is characterised in that include as follows Step:

Step one, for redundant degree of freedom mechanical arm, exist such as between its tip speed and each joint angle speed Lower mapping relations:

Wherein,Represent tip speed,Representing joint angle speed, J represents Jacobian matrix；

Step 2, the maximum singular value calculating described Jacobian matrix and minimum singular value, with maximum singular value It is conditional number with the ratio of minimum singular value, when conditional number is more than setting numerical value, then deteminate machine mechanical arm configuration Occur unusual；

During calculating the conditional number of described Jacobian matrix, use and calculate intermediate object program repeatedly based on generalized inverse In generation, asks for the instantaneous value of maximum singular value and minimum singular value, reaches the requirement of real-time design conditions number.

2. a kind of redundant degree of freedom quick decision method of mechanical arm configuration singularity as claimed in claim 1, its Being characterised by, the described numerical value that sets is as 1000.

3. a kind of redundant degree of freedom quick decision method of mechanical arm configuration singularity as claimed in claim 1, its Be characterised by, described based on generalized inverse calculate intermediate object program include A and B two, wherein A=J J^T, B=A^-1, I.e. A is 6 × 6 rank Hermite matrixes, and B is the inverse matrix of A；

Then maximum singular value δ₀Instantaneous value δ_0,kFollowing iterative formula is used to solve:

δ_0,k=max (A I_0,k)

I_0,k+1=A I_0,k/δ_0,k

Wherein, I_0,kThe intermediate vector solved for maximum singular value, I_0,0For [1 1111 1]^T, k=0,1 ..., N, N is iterations；

After n times iteration, δ₀=δ_0,N-1；

Minimum singular value δ₅Instantaneous value δ_5,kFollowing iterative formula is used to solve:

δ_5,k=max (B I_5,k)

I_5,k+1=B I_5,k/δ_5,k

Wherein, I_5,kThe intermediate vector solved for minimum singular value, I_5,0For [1 1111 1]^T；

After n times iteration, δ₅=1/ δ_5,N-1；

Then conditional number is Cond=δ₀/δ₅。