CN101804627A

CN101804627A - Redundant manipulator motion planning method

Info

Publication number: CN101804627A
Application number: CN 201010144515
Authority: CN
Inventors: 张雨浓; 张智军; 李克讷
Original assignee: Sun Yat Sen University
Current assignee: Sun Yat Sen University
Priority date: 2010-04-02
Filing date: 2010-04-02
Publication date: 2010-08-18
Anticipated expiration: 2030-04-02
Also published as: CN101804627B

Abstract

The invention provides a redundant manipulator motion planning method which comprises the following steps that: (1) an upper computer analyzes the inverse kinematics of a manipulator on a velocity layer through quadratic optimization, the designed minimum performance index can be velocity norm, repetitive motion or kinetic energy and is bound by a velocity jacobian equation, an inequation and a joint angular velocity limit, and the angular velocity limit changes with a joint angle; (2) the quadratic optimization of step (1) is optimized into a quadratic programming problem; (3) the quadratic programming problem in step (2) is calculated by a linear variational inequation primal-dual neural network solver or a numerical method; and (4) the calculation result in step (3) is transmitted to a lower computer controller to drive the manipulator to move. The redundant manipulator motion planning method is based on the primal-dual neural network of the linear variational inequation, has global exponential convergence, does not involve matrix inversion and other complicated operations, greatly improves the calculation efficiency, and simultaneously has strong real-time performance, and can adapt to the changes to the joint angular velocity limit.

Description

Redundant manipulator motion planning method

Technical Field

The invention belongs to a redundant manipulator motion planning method, and particularly relates to a redundant manipulator motion planning method with variable-limit joint angular velocity.

Background

The redundant manipulator is a tail end active mechanical device with the degree of freedom greater than the minimum degree of freedom required by a task space, the motion tasks of the redundant manipulator comprise welding, painting, assembling, excavating, drawing and the like, and the redundant manipulator is widely applied to national economic production activities such as equipment manufacturing, product processing, machine operation and the like. The inverse kinematics problem of the redundant manipulator refers to the problem that the terminal pose of the manipulator is known to determine the joint angle of the manipulator. The traditional redundancy analysis method and the industrial mechanical arm control method are mainly based on a pseudo-inverse method: that is, the solution to the problem is converted to a minimum norm solution plus a homogeneous solution. The secondary target can be assigned to a solution of the same kind to control the self-movement of the mechanical arm to avoid obstacles, joint limits, singular points and optimize other objective functions. The method has the defects of difficulty in processing inequality constraints, large calculation amount, poor real-time performance, and generation of an infeasible solution under the condition of singularity, and is greatly restricted in the application of the actual mechanical arm.

Disclosure of Invention

The invention aims to overcome the defects of the prior art and provides a redundant manipulator motion planning method which is small in calculated amount, strong in real-time performance and capable of adapting to the change of the joint angular speed limit.

In order to realize the purpose of the invention, the technical scheme is as follows:

a redundant manipulator motion planning method comprises the following steps:

1) the upper computer analyzes the inverse kinematics of the mechanical arm on a speed layer by quadratic optimization, the designed minimum performance index can be speed norm, repetitive motion or kinetic energy and is constrained by a speed Jacobian equation, an inequality and a joint angular speed limit, and the angular speed limit is changed along with a joint angle;

2) converting the quadratic form optimization of the step 1) into a quadratic programming problem;

3) solving the quadratic programming problem in the step 2) by using a linear variational inequality primal-dual neural network solver or a numerical method;

4) and transmitting the solving result of the step 3) to a lower computer controller to drive the mechanical arm to move.

The primal-dual neural network based on the linear variational inequality has global exponential convergence, does not involve complex operations such as matrix inversion and the like, greatly improves the calculation efficiency, has strong real-time performance and can adapt to the limit change of the angular velocity of the joint.

Drawings

FIG. 1 is a flow chart of the present invention;

FIG. 2 is a front view of a robotic arm structure embodying the present invention;

FIG. 3 is a top view of a robotic arm structure embodying the present invention;

figure 4 is a partial schematic view of a robotic arm.

Detailed Description

The invention is further described below with reference to the accompanying drawings.

The redundant manipulator motion planning method shown in fig. 1 mainly comprises a target problem 1, a quadratic programming problem 2, a primal-dual neural network solver or quadratic programming numerical algorithm 3 based on a linear variational inequality, a lower computer controller 4 and a manipulator 5. Solving inverse kinematics on the velocity layer is first designed to minimize

And is constrained to

θ^-≤θ≤θ⁺，

Performance index to be optimized

May be a minimum velocity norm function

Index of repetitive motion

Or minimum kinetic energy function

And converting the various redundancy analysis schemes into a universal quadratic optimization standard form 2, solving by using a primal-dual neural network or quadratic programming numerical method 3 based on a linear variational inequality, and transmitting a solving result to a lower computer controller 4 to drive a mechanical arm 5 to move.

The robot arm shown in fig. 2 and 3 is composed of a robot arm link 1, a push rod 2, a joint 3, a joint and push rod force application point connecting part 4 and a base 5. The existence of the push rod 2 makes the mechanical arm different from the constant angular speed limit mode of the traditional push rod-free series mechanical arm, and is a limit-variable mechanical arm. This angular velocity limit is a function of angle when calculating the inverse solution of the mechanical arm in real time. Thus, by changing constraints of quadratic programming

Thereby realizing the variable limit control.

The robot arm shown in fig. 4 is partially schematic, and in a typical design, mechanical power is considered to be generated by internal motor torque, i.e. power is not assumed to come from a push rod, or 4 is considered to be very small and almost negligible, so that 1 and 2 are coincident. The present invention relates to a robot arm design where the push rod is present, i.e. 4 cannot be ignored. Thus, the original angular velocity limit changes every moment due to the presence of the pushrod, being a function of angle. Let 1 be a in length, 4 b in length, and 2 c in length. The specific derivation process is as follows:

the formula is derived from the time t to obtain

Wherein c is c₀+v*Δt*Δc，c₀The initial c side length is, v is the rotating speed of the stepping motor, and deltac is the corresponding electric push rod elongation of one circle of motor rotation. Further obtain the

Namely, it is

(4)

Therefore, it is not only easy to use

The variable limit of the available joint velocity is therefore

(6)

Wherein v is⁺And v^-Respectively a positive limit and a negative limit of the rotating speed of the relevant joint stepping motor. By considering that the limit of the angular velocity is a function changing along with the angle, corresponding constraint conditions are modified when a control method is designed, and the angular velocity limit condition with variable limit parameters is formed, so that the problem of variable limit is solved.

Based on the foregoing analysis, the inverse kinematics solution for the robotic arm can be designed on the velocity layer as:

θ^-≤θ≤θ⁺， (10)

wherein,

representing the performance index to be optimized; constraint of equality

Expressing the motion trail of the tail end of the mechanical arm; constraint of inequality

May be used for avoidance of environmental obstacles or other performance constraints; theta^-≤θ≤θ⁺、

Respectively, joint angle limit and joint angular velocity limit.

Performance index to be optimized

Optimization criteria for various redundancy resolution schemes can be designed. It may be a minimum velocity norm function, i.e.

Or a repetitive motion indicator, i.e.

Where z is λ (θ - θ (0)), λ > 0 is a positive design parameter used to control the amplitude of joint displacement; it may also be a minimum kinetic energy function

And the like.

As shown in step 1 of fig. 1, the above problem is transformed into a standard quadratic programming problem to be solved and applied to the control of the robot arm. The quadratic programming problem can be written in the following general form:

minx^TWx/2+q^Tx， (12)

s.t.Jx＝d， (13)

Ax≤b， (14)

x^-≤x≤x⁺。 (15)

wherein the decision variable x can be defined as

W，q，J，d，A，b，x^-，x⁺For known corresponding coefficient matrices and vectors, for example, in the minimum velocity norm scheme, W is an identity matrix, q is 0, J is a jacobian matrix,

and A and b can be barrier avoidance parameters or inequality constraints obtained by converting optimization indexes, x^-，x⁺Obtained by transformation from equations (10), (11).

The following describes the processing and transformation process of joint physical limit, i.e. how to convert the formula (10), (11) into the formula (15). When analyzing on the velocity layer, it is necessary to convert equation (10) into the velocity layer

The expression of (A) above:

wherein the coefficient mu > 0 is a feasible region for adjusting the joint angular velocity, and the coefficient mu is selected such that the feasible region after the formula (16) conversion is slightly larger than the original feasible region of the joint angular velocity under normal conditions. Thus, the double-ended constraint equations (10) and (11) can be combined into one unified double-ended constraint: x is the number of^-≤x≤x⁺Wherein x is^-And x⁺Are defined as follows:

the invention uses a double-end inequality to express the avoidance of the joint physical limit, and the selection of the related parameter mu can be based on theoretical analysis or experience.

After the quadratic programming problems (12) - (15) are obtained, the solving method of the invention adopts a primal-dual neural network or a quadratic programming numerical algorithm based on a linear variational inequality to solve the quadratic programming problem in real time.

The following is the construction process of the neural network solver for solving the quadratic programming problem with constraints (12) - (15) based on the primal-dual neural network of the linear variational inequality.

Firstly, converting the equations of quadratic programming problems (12) - (15) into a linear variational inequality, namely solving a primal-dual variable

So that

(y-y^*)^T(My^*+p)≥0， (19)

Wherein, the primal-dual variable y and the upper and lower limits thereof are defined as follows:

y = [\begin{matrix} x \\ u \\ v \end{matrix}],

the dual variables u and v are respectively opposite to an equality constraint (13) and an inequality constraint (14)The preparation method comprises the following steps of; 1_v：＝[1，...，1]^TAre corresponding dimension vectors with elements all being 1;are constants large enough to numerically replace infinity + ∞, and the spreading matrix M, p is defined as follows:

M = [\begin{matrix} W & {- J}^{T} & A^{T} \\ J & 0 & 0 \\ - A & 0 & 0 \end{matrix}],

p = [\begin{matrix} q \\ - d \\ b \end{matrix}];

from this can be summarized as: there is at least one optimal solution x^*The quadratic programming problem (12) - (15) can be transformed into a linear variational inequality problem (19).

Second, the linear variational inequality problem (19) is again equivalent to a linear projection equation, i.e., P_Ω(y- (My + P)) -y ═ 0, where P is_Ω(. is a space R)^{dim(x)+dim(d)+dim(b)}Piecewise linear projection operator, P, to the set Ω_ΩThe i-th calculation unit of (y) is defined as

<math><mrow><mfenced open='{' close=''><mtable><mtr><mtd><msubsup><mi>y</mi><mi>i</mi><mo>-</mo></msubsup></mtd><mtd><mi>if</mi></mtd><mtd><msub><mi>y</mi><mi>i</mi></msub><mo><</mo><msubsup><mi>y</mi><mi>i</mi><mo>-</mo></msubsup></mtd></mtr><mtr><mtd><msub><mi>y</mi><mi>i</mi></msub></mtd><mtd><mi>if</mi></mtd><mtd><msubsup><mi>y</mi><mi>i</mi><mo>-</mo></msubsup><mo>≤</mo><msub><mi>y</mi><mi>i</mi></msub><mo>≤</mo><msubsup><mi>y</mi><mi>i</mi><mo>+</mo></msubsup></mtd></mtr><mtr><mtd><msubsup><mi>y</mi><mi>i</mi><mo>+</mo></msubsup></mtd><mtd><mi>if</mi></mtd><mtd><msub><mi>y</mi><mi>i</mi></msub><mo>></mo><msubsup><mi>y</mi><mi>i</mi><mo>+</mo></msubsup></mtd></mtr></mtable></mfenced><mo>,</mo><mo>&ForAll;</mo><mi>i</mi><mo>&Element;</mo><mo>{</mo><mn>1,2</mn><mo>,</mo><mo>.</mo><mo>.</mo><mo>.</mo><mo>,</mo><mi>dim</mi><mrow><mo>(</mo><mi>x</mi><mo>)</mo></mrow><mo>+</mo><mi>dim</mi><mrow><mo>(</mo><mi>d</mi><mo>)</mo></mrow><mo>+</mo><mi>dim</mi><mrow><mo>(</mo><mi>b</mi><mo>)</mo></mrow><mo>}</mo><mo>.</mo></mrow></math>

Next, the linear variational inequality problem and the quadratic programming problem are solved by the following dynamical system (as a dynamical description form of the primal-dual neural network based on the linear variational inequality, as in step 3 of fig. 1):

wherein, the design parameter gamma is more than 0 to adjust the convergence of the network, and the larger gamma is, the faster the network converges. In addition, when (12) - (15) at least one optimal solution x exists^*From any initial state, linear variational inequality primal-dual neural network(20) Will converge to some equilibrium point y^*The first dim (x) elements of which constitute the optimal solution x of the quadratic programming problem (12) - (15)^*. If there is a constant ρ > 0, let | | | y-P_Ω(y-(My+p))||²≥ρ||y-y^*||²If true, the global index of the neural network (20) converges to the equilibrium point y^*And the problem optimal solution x^*(the convergence rate is proportional to γ ρ). And transmitting the calculated angular velocity to a lower computer controller so as to control the motion of the mechanical arm, thereby realizing the method of the invention.

Claims

1. A redundant manipulator motion planning method is characterized by comprising the following steps:

3) solving the quadratic programming problem in the step 2) by a primal-dual neural network solver or a quadratic programming numerical method based on a linear variational inequality;

2. The method for redundant manipulator motion planning according to claim 1, wherein the quadratic optimization redundancy resolution scheme of step 1) designs the inverse kinematics solution of the manipulator on the velocity level as: minimization

Is constrained to

θ^-≤θ≤θ⁺，

Wherein

Representing performance indicators to be optimized, equality constraints

Expressing the motion trail of the tail end of the mechanical arm and inequality constraint

Representing avoidance Performance constraints for environmental obstacles, θ^-≤θ≤θ⁺、Respectively representing joint angle limit and joint angular velocity limit;

said property to be optimizedEnergy index

For optimization criteria of various redundancy resolution schemes, at the speed level

Using a minimum velocity norm function, i.e.

Or repetitive motion indicators, i.e.

Where z is λ (θ - θ (0)), λ > 0 is a positive design parameter, or minimum kinetic energy function, used to control the amplitude of joint displacement

Wherein H is the arm inertia matrix.

3. The method of claim 2, wherein the quadratic programming problem of step 2) is transformed into a linear variational inequality, i.e. a primal-dual variational solutionSo that

(y-y^*)^T(My^*+ p) is greater than or equal to 0, wherein the primal-dual variable y and the upper and lower limits thereof are defined as follows:

y = [\begin{matrix} x \\ u \\ v \end{matrix}],

the dual variables u and v correspond to an equality constraint (7) and an inequality constraint (8), respectively; 1_v：＝[1，...，1]^TAre corresponding dimension vectors with elements all being 1;are constants large enough to numerically replace infinity + ∞, and the spreading matrix M, p is defined as follows:

M = [\begin{matrix} W & - J^{T} & A^{T} \\ J & 0 & 0 \\ - A & 0 & 0 \end{matrix}],

p = [\begin{matrix} q \\ - d \\ b \end{matrix}] .

4. the linear variational inequality according to claim 3 characterized in that it is equivalent to the linear projection equation P_Ω(y- (My + P)) -y ═ 0, where P is_Ω(. is a space R)^{dim(x)+dim(d)+dim(b)}Piecewise linear projection operator, P, to the set Ω_ΩThe i-th calculation unit of (y) is defined as

<math><mrow><mo>&ForAll;</mo><mi>i</mi><mo>&Element;</mo><mo>{</mo><mn>1,2,3</mn><mo>,</mo><mo>.</mo><mo>.</mo><mo>.</mo><mo>,</mo><mi>dim</mi><mrow><mo>(</mo><mi>x</mi><mo>)</mo></mrow><mo>+</mo><mi>dim</mi><mrow><mo>(</mo><mi>d</mi><mo>)</mo></mrow><mo>+</mo><mi>dim</mi><mrow><mo>(</mo><mi>b</mi><mo>)</mo></mrow><mo>}</mo><mo>.</mo></mrow></math>

Then, using a kinetic system

Solving the linear variational inequality problem and the quadratic programming problem, wherein the design parameter gamma is more than 0 to adjust the convergence of the network, and the larger the gamma is, the faster the network converges.