CN103331756A - Mechanical arm motion control method - Google Patents

Abstract

A kind of mechanical arm motion control method, comprising the following steps: (1) the transformation transfer matrix for establishing complete corresponding n connecting rod calculates the linear velocity and angular speed of each connecting rod; (2) calculate connecting rod mass center obtain inertial tensor, calculate the total kinetic energy and total potential energy of mechanical arm, local derviation sought about mechanical arm generalized coordinates q to the difference of total kinetic energy and total potential energy, obtain driving force vector τ and

Relationship, and founding mathematical models; (3) point at corresponding each moment iterates to calculate setting number using variable-gain iterative control algorithm, obtains the error of each time point and expected pose qd, and subtract each other acquisition initial error with current pose q (0)

It is inputted by system of driving force vector τ, setting number is then iterated to calculate by variable-gain iterative control algorithm, obtaining driving force vector τ to be given needed for each moment point is how many to go to expected pose to control mechanical arm. Present invention control precision is higher, flexibility is good, is suitable for non-linear and time-varying occasion.

Description

A kind of manipulator motion control method

Technical field

The invention belongs to the movement control technology field, be applicable to the manipulator motion control method.

Background technology

At present, domestic mechanical arm industry development is rapid, controller of a great variety.Yet, for the horizontal articulated mechanical arm of multiple degrees of freedom, because there are characteristics such as inertia force, joint coupling, gravity load variation in robot, making its control system is the control system of non-linear a, close coupling, multivariable, relative complex, be difficult to it is set up precise analytic model, this has brought very big difficulty to ROBOT CONTROL.For realization and consideration economically, present most of business machine people's Control System Design method, remain and each joint of robot is used as a simple servo control mechanism handles, though this control method is gone through development, can make the motion of robot reach very high accurate positioning accuracy, but still exist a lot of shortcomings.

Summary of the invention

Lower for the control accuracy that overcomes existing mechanical arm traditional control system, lack flexibility, only be applicable to some linearities, the time deficiency of changing environment not, the invention provides that a kind of control accuracy is higher, flexibility good, be applicable to manipulator motion control method non-linear and that time-varying field is closed.

The technical solution adopted for the present invention to solve the technical problems is as follows:

A kind of manipulator motion control method said method comprising the steps of:

(1) according to the mechanical arm actual parameter, the conversion transfer matrix of setting up n connecting rod of complete correspondence also further calculates linear velocity and the angular speed of each connecting rod thus;

(2) calculate the connecting rod barycenter, draw inertial tensor, and calculate total kinetic energy and total potential energy of mechanical arm thus, total kinetic energy and the difference of total potential energy are asked for local derviation about mechanical arm generalized coordinates q, obtain driving force vector τ with

Relation, described

Correspond to each joint velocity, and set up Mathematical Modeling with this;

(3) corresponding each point constantly uses variable-gain iterative control algorithm iterative computation set point number, obtains each time point and expected pose q _dError, and subtract each other the acquisition initial error with current pose q (0)

Be system input with driving force vector τ, then by variable-gain iterative control algorithm formula $u_{k+1}\left(t\right)=u_k\left(t\right)+K_{d1}(k+1){\stackrel{.}{e}}_{k+1}\left(t\right)+K_{d2}\left(t\right){\stackrel{.}{e}}_k\left(t\right)$ The iterative computation set point number obtains the required driving force vector τ that provides of each moment point and what control mechanical arm for goes to expected pose.

The present invention uses a kind of mechanical arm control system of iterative learning algorithm of variable gain, under priori situation seldom, to expectation work track iteration, eliminate error gradually, and the final tracking that realizes desired trajectory (desired trajectory that comprises position, speed, acceleration).And method computational process is simple, is easy to realize, makes the finishing fast of whole control process.

Beneficial effect of the present invention mainly shows: easy to operate, precision is higher, realized the quick response of mechanical arm control and tracking fully.Native system not only has independent control ability, can carry out motion control and planning to mechanical arm separately by teach box, but also can be by do realizing mechanical arm On-line Control more accurately with the PC communication protocols.

Description of drawings

Fig. 1 is the structure chart of kinetic control system.

Fig. 2 is the realization flow figure of variable-gain iterative control algorithm.

The specific embodiment

Below in conjunction with accompanying drawing the present invention is described further.

See figures.1.and.2, a kind of manipulator motion control method said method comprising the steps of:

(1) according to the mechanical arm actual parameter, the conversion transfer matrix of setting up n connecting rod of complete correspondence also further calculates linear velocity and the angular speed of each connecting rod thus;

(2) calculate the connecting rod barycenter, draw inertial tensor, and this total kinetic energy that calculates mechanical arm and total potential energy are arranged, total kinetic energy and the difference of total potential energy are asked for local derviation about mechanical arm generalized coordinates q, obtain driving force vector τ with

Relation, described

Correspond to each joint velocity, and set up Mathematical Modeling with this;

(3) corresponding each point constantly uses variable-gain iterative control algorithm iterative computation set point number, obtains each time point and expected pose q _dError, and subtract each other the acquisition initial error with current pose q (0)

Be system input with driving force vector τ, then by variable-gain iterative control algorithm formula $u_{k+1}\left(t\right)=u_k\left(t\right)+K_{d1}(k+1){\stackrel{.}{e}}_{k+1}\left(t\right)+K_{d2}\left(t\right){\stackrel{.}{e}}_k\left(t\right)$ The iterative computation set point number obtains the required driving force vector τ that provides of each moment point and what control mechanical arm for goes to expected pose.

In the present embodiment, the hardware platform that the realization of mechanical arm iterative learning is adopted is based on ARM Cortex ^TM-M4 is the motion-control module of the STM32F407 chip of kernel, and cooperates by 232 serial communications with PC and to realize mechanical arm On-line Control more accurately.The servomotor that wherein adopts and supporting driver are respectively ECMA-C20604GS and ASD-B2-0421-B.

(1) generation of mechanical arm track can by the terminal position of mechanical arm change come to, and the change procedure of the terminal position change procedure of connecting rod parameter just.The connecting rod parameter is analyzed, used standard D-H method to obtain the rod member torsional angle α of each connecting rod _i, rod member length a _i, joint rotation angle θ _i, the joint is apart from d _i(wherein have only one to be variable, all the other are constant) can get i+1 connecting rod thus with respect to the transfer matrix of i connecting rod

$T_i^{i+1}={\mathrm{Trans}}_z\left(d_i\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_x\left(a_i\right){\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)$

$=\left[\begin{array}{cccc}{c}_i& -c{\mathrm{\α}}_i{s}_i& s{\mathrm{\α}}_i{s}_i& a_i{c}_i\\ s_i& c{\mathrm{\α}}_i{c}_i& -s{\mathrm{\α}}_i{c}_i& a_i{s}_i\\ 0& s{\mathrm{\α}}_i& c{\mathrm{\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]$

Wherein, $s_i\stackrel{\mathrm{\Δ}}{=}\sin {\mathrm{\θ}}_i,$ $c_i\stackrel{\mathrm{\Δ}}{=}\cos {\mathrm{\θ}}_i,$ $s{\mathrm{\α}}_i\stackrel{\mathrm{\Δ}}{=}\sin {\mathrm{\α}}_i,$ $c{\mathrm{\α}}_i\stackrel{\mathrm{\Δ}}{=}\cos {\mathrm{\α}}_i.$

${\mathrm{Tran}}_z\left(d_i\right)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}I& \left[\begin{array}{c}0\\ 0\\ d\end{array}\right]\\ 0& 1\end{array}\right],{\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}{R}_z\left({\mathrm{\θ}}_i\right)& 0\\ 0& 1\end{array}\right],$

${\mathrm{Tran}}_z\left(a_i\right)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}I& \left[\begin{array}{c}{a}_i\\ 0\\ 0\end{array}\right]\\ 0& 1\end{array}\right],{\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}{R}_x\left({\mathrm{\α}}_i\right)& 0\\ 0& 1\end{array}\right]$

(2) by the transfer matrix in the step (1)

Try to achieve the rotation transformation matrix between each joint

With the translation transformation matrix

The generalized coordinates of mechanical arm is (being joint angle or the connecting rod offset distance vector in each joint of mechanical arm) q.We can get linear velocity and the angular speed in each joint thus.

Because the pedestal of mechanical arm is generally fixed, and obtains: ⁰W _o=0, ⁰v _o=0

When being rotary joint for i+1 joint, have angular speed and linear velocity formula as follows:

$w_{i+1}^{i+1}=R_i^i{}_{i+1}{w}_i+{\stackrel{.}{\mathrm{\θ}}}_{i+1}\widehat{Z_{i+1}^{i+1}}$

$v_{i+1}^{i+1}=R_i^{i+1}(v_i^i+w_i^i\×P_{i+1}^i)$

When being linear joint for i+1 joint, corresponding relation becomes:

$w_{i+1}^{i+1}=R_i^i{}_{i+1}{w}_i$

$v_{i+1}^{i+1}=R_i^{i+1}(v_i^i+w_i^i\×P_{i+1}^i)+{\stackrel{.}{d}}_{i+1}\widehat{Z_{i+1}^{i+1}}$

Wherein, ^I+1w _I+1The expression link rod coordinate system angular speed of i+1}, ^I+1v _I+1The expression link rod coordinate system linear velocity of i+1},

Expression z axle rotation trace.

(3) according to the actual physics parameter designing of mechanical arm, determine the quality m of each connecting rod _i, the length of each connecting rod is respectively l _i, acceleration of gravity is g _oAnd calculate the inertial tensor of each connecting rod thus

${}^{C_i}{I}_i=\left[\begin{array}{ccc}{I}_{\mathrm{xx}}& -I_{\mathrm{xy}}& -I_{\mathrm{xz}}\\ -I_{\mathrm{xy}}& I_{\mathrm{yy}}& -I_{\mathrm{yz}}\\ -I_{\mathrm{xz}}& -I_{\mathrm{yz}}& I_{\mathrm{zz}}\end{array}\right]$

Suppose C _iBe the barycenter v of connecting rod i _CiThe systemic velocity of expression connecting rod.

Because the motion in connecting rod mass center line speed and joint is irrelevant, no matter be rotary joint or linear joint therefore, for connecting rod i+1, its mass center line speed can be expressed as:

$v_{C_{i+1}}=v_{i+1}^{i+1}+w_{i+1}^{i+1}\×{}^{i+1}{P}_{C_{i+1}},$

Wherein,

Expression barycenter C _I+1At the link rod coordinate system { position vector among the i+1}.

So kinetic energy k of i connecting rod _iCan be expressed as:

$k_i=\frac{1}{2}{m}_i{v}_{C_i}^T{v}_{C_i}+\frac{1}{1}w_i^T{}_i{}^{C_i}{I}_{i}w_i^i$

In the formula, first is the kinetic energy that expression is produced by connecting rod mass center line speed, and the kinetic energy that second expression produced by the angular speed of connecting rod.Wherein With ⁱw _iBe about joint position q and joint velocity The function vector, so the kinetic energy of whole mechanical arm is the kinetic energy sum of each connecting rod be:

$k(q,\stackrel{.}{q})=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{k}_i$

Note ⁰G is the acceleration of gravity vector in the reference frame 0, Be connecting rod barycenter C _iPosition vector in reference frame 0.The potential energy that then can get i connecting rod is:

$u_i=-{m_i}^0{g}^T{}^0{P}_{C_i}$

So total potential energy of mechanical arm can be expressed as:

$u\left(q\right)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{u}_i$

(4) Lagrangian of mechanical arm (kinetic energy of a mechanical system and the difference of potential energy) can be expressed as:

$L(q,\stackrel{.}{q})=k(q,\stackrel{.}{q})-u\left(q\right)$

Then the kinetics equation of describing with Lagrange's equation is:

$\frac{d}{\mathrm{dt}}\frac{\∂L}{\∂\stackrel{.}{q}}-\frac{\∂L}{\∂q}=\mathrm{\τ}$

Wherein, τ is the driving moment vector of n * 1.For mechanical arm, equation can be changed into:

$\frac{d}{\mathrm{dt}}\frac{\∂k}{\∂\stackrel{.}{q}}-\frac{\∂k}{\∂q}+\frac{\∂u}{\∂q}=\mathrm{\τ}$

The kinetic energy of having been tried to achieve by step (3)

And potential energy u (q).Bring formula into back and put in order and can get following matrix form:

$\mathrm{\τ}=M\left(q\right)\stackrel{..}{q}+(q,\stackrel{.}{q})+G\left(q\right)$

Can obtain the relation between power vector matrix τ and the generalized coordinates q of robot thus.

(5) the iterative learning mode of use variable gain serves as input u (t) with power vector matrix τ, and the generalized coordinates q of robot is output y (t).Use variable-gain feedback D type iterative learning control law

$u_{k+1}\left(t\right)=u_k\left(t\right)+K_{d1}(k+1){\stackrel{.}{e}}_{k+1}\left(t\right)+K_{d2}\left(t\right){\stackrel{.}{e}}_k\left(t\right)$

Wherein, feedback oscillator K _D1(k)=s (k) K _D1(0), K _Di∈ R ^{M * r}Be the study gain matrix, s (k)＞the 1st is about the increasing function of iterations k.Be the current input quantity that provides ^K+1τ is by the input quantity of last time ^kτ adds last error variable quantity

Multiply by feedback matrix K _Dl(k+1) and current error variable quantity Multiply by study gain matrix K _D2(t) obtain, thereby accelerated the convergence rate of iteration control, and agitating of producing can avoid driving the operation of executing agency the time.And in order to obtain data relatively fast, and keep precision preferably, the iterations of this programme is 15 times, and the study gain of choosing is:

$K_{d1}\left(0\right)=\left[\begin{array}{ccc}450& 0& 0\\ 0& 450& 0\\ 0& 0& 400\end{array}\right],$ $K_{d2}=\left[\begin{array}{ccc}750& 0& 0\\ 0& 750& 0\\ 0& 0& 700\end{array}\right],$ s(k)＝k。

Final by this kind iterative algorithm, calculate required driving force vector τ of each step, can control mechanical arm and reach the expectation attitude.

Claims (6)

1. manipulator motion control method is characterized in that: said method comprising the steps of:

(1) according to the mechanical arm actual parameter, the conversion transfer matrix of setting up n connecting rod of complete correspondence also further calculates linear velocity and the angular speed of each connecting rod thus;

(2) calculate the connecting rod barycenter, draw inertial tensor, and calculate total kinetic energy and total potential energy of mechanical arm thus, total kinetic energy and the difference of total potential energy are asked for local derviation about mechanical arm generalized coordinates q, obtain driving force vector τ with

Relation, described

Correspond to each joint velocity, and set up Mathematical Modeling with this;

(3) corresponding each point constantly uses variable-gain iterative control algorithm iterative computation set point number, obtains each time point and expected pose q _dError, and subtract each other the acquisition initial error with current pose q (0)

Be system input with driving force vector τ, then by variable-gain iterative control algorithm formula $u_{k+1}\left(t\right)=u_k\left(t\right)+K_{d1}(k+1){\stackrel{.}{e}}_{k+1}\left(t\right)+K_{d2}\left(t\right){\stackrel{.}{e}}_k\left(t\right)$ The iterative computation set point number obtains the required driving force vector τ that provides of each moment point and what control mechanical arm for goes to expected pose.

2. manipulator motion control method as claimed in claim 1 is characterized in that: in the described step (1), use standard D-H method to obtain the rod member torsional angle α of each connecting rod _i, rod member length a _i, joint rotation angle θ _i, the joint is apart from d _i, can get i+1 connecting rod thus with respect to the transfer matrix of i connecting rod

$T_i^{i+1}={\mathrm{Trans}}_z\left(d_i\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_x\left(a_i\right){\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)$ $=\left[\begin{array}{cccc}{c}_i& -c{\mathrm{\α}}_i{s}_i& s{\mathrm{\α}}_i{s}_i& a_i{c}_i\\ s_i& c{\mathrm{\α}}_i{c}_i& -s{\mathrm{\α}}_i{c}_i& a_i{s}_i\\ 0& s{\mathrm{\α}}_i& c{\mathrm{\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]$

Wherein, $s_i\stackrel{\mathrm{\Δ}}{=}\sin {\mathrm{\θ}}_i,$ $c_i\stackrel{\mathrm{\Δ}}{=}\cos {\mathrm{\θ}}_i,$ $s{\mathrm{\α}}_i\stackrel{\mathrm{\Δ}}{=}\sin {\mathrm{\α}}_i,$ $c{\mathrm{\α}}_i\stackrel{\mathrm{\Δ}}{=}\cos {\mathrm{\α}}_i;$

${\mathrm{Tran}}_z\left(d_i\right)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}I& \left[\begin{array}{c}0\\ 0\\ d\end{array}\right]\\ 0& 1\end{array}\right],{\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}{R}_z\left({\mathrm{\θ}}_i\right)& 0\\ 0& 1\end{array}\right],$

${\mathrm{Tran}}_z\left(a_i\right)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}I& \left[\begin{array}{c}{a}_i\\ 0\\ 0\end{array}\right]\\ 0& 1\end{array}\right],{\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)\stackrel{\mathrm{\Δ}}{=}\left[\begin{array}{cc}{R}_x\left({\mathrm{\α}}_i\right)& 0\\ 0& 1\end{array}\right].$

3. manipulator motion control method as claimed in claim 1 or 2 is characterized in that: in the described step (1), according to described transfer matrix

Try to achieve the rotation transformation matrix between each joint

With the translation transformation matrix The generalized coordinates of mechanical arm is q, obtains linear velocity and the angular speed in each joint thus,

Set the pedestal of mechanical arm and fix, obtain: ⁰w _o=0, ⁰v _o=0;

When being rotary joint for i+1 joint, have angular speed and linear velocity formula as follows:

$w_{i+1}^{i+1}=R_i^i{}_{i+1}{w}_i+{\stackrel{.}{\mathrm{\θ}}}_{i+1}\widehat{Z_{i+1}^{i+1}}$

$v_{i+1}^{i+1}=R_i^{i+1}(v_i^i+w_i^i\×P_{i+1}^i)$

When being linear joint for i+1 joint, corresponding relation becomes:

$w_{i+1}^{i+1}=R_i^i{}_{i+1}{w}_i$

$v_{i+1}^{i+1}=R_i^{i+1}(v_i^i+w_i^i\×P_{i+1}^i)+{\stackrel{.}{d}}_{i+1}\widehat{Z_{i+1}^{i+1}}$

Wherein, ^I+1w _I+1The expression link rod coordinate system angular speed of i+1}, ^I+1v _I+1The expression link rod coordinate system linear velocity of i+1},

Expression z axle rotation trace.

4. manipulator motion control method as claimed in claim 1 or 2 is characterized in that: in the described step (2), and the quality m of each connecting rod _i, the length of each connecting rod is respectively l _i, acceleration of gravity is g, and calculates the inertial tensor of each connecting rod thus

${}^{C_i}{I}_i=\left[\begin{array}{ccc}{I}_{\mathrm{xx}}& -I_{\mathrm{xy}}& -I_{\mathrm{xz}}\\ -I_{\mathrm{xy}}& I_{\mathrm{yy}}& -I_{\mathrm{yz}}\\ -I_{\mathrm{xz}}& -I_{\mathrm{yz}}& I_{\mathrm{zz}}\end{array}\right]$

Suppose C _iBe the barycenter v of connecting rod i _CiThe systemic velocity of expression connecting rod;

For connecting rod i+1, its mass center line speedometer is shown:

$v_{C_{i+1}}=v_{i+1}^{i+1}+w_{i+1}^{i+1}\×{}^{i+1}{P}_{C_{i+1}},$

Wherein,

Expression barycenter C _I+1Link rod coordinate system the position vector among the i+1},

So kinetic energy k of i connecting rod _iBe expressed as:

$k_i=\frac{1}{2}{m}_i{v}_{C_i}^T{v}_{C_i}+\frac{1}{1}w_i^T{}_i{{}^{C_i}I}_{i}w_i^i$

In the formula, first is the kinetic energy that expression is produced by connecting rod mass center line speed, and the kinetic energy that second expression produced by the angular speed of connecting rod, wherein

With ⁱw _iBe about joint position q and joint velocity The function vector, so the kinetic energy of whole mechanical arm is the kinetic energy sum of each connecting rod be:

$k(q,\stackrel{.}{q})=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{k}_i$

Note ⁰G is the acceleration of gravity vector in the reference frame 0,

Be connecting rod barycenter C _iPosition vector in reference frame 0, the potential energy that then gets i connecting rod is:

$u_i=-{m_i}^0{g}^T{}^0{P}_{C_i}$

So total potential energy of mechanical arm is expressed as:

$u\left(q\right)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{u}_i.$

5. manipulator motion control method as claimed in claim 4, it is characterized in that: in the described step (2), the Lagrangian of mechanical arm is expressed as:

$L(q,\stackrel{.}{q})=k(q,\stackrel{.}{q})-u\left(q\right)$

Then the kinetics equation of describing with Lagrange's equation is:

$\frac{d}{\mathrm{dt}}\frac{\∂L}{\∂\stackrel{.}{q}}-\frac{\∂L}{\∂q}=\mathrm{\τ}$

Wherein, τ is the driving moment vector of n * 1, and for mechanical arm, equation can be changed into:

$\frac{d}{\mathrm{dt}}\frac{\∂k}{\∂\stackrel{.}{q}}-\frac{\∂k}{\∂q}+\frac{\∂u}{\∂q}=\mathrm{\τ}$

The kinetic energy of having been tried to achieve by step (3)

And potential energy u (q), bring formula into back and be organized into and be following matrix form:

$\mathrm{\τ}=M\left(q\right)\stackrel{..}{q}+(q,\stackrel{.}{q})+G\left(q\right)$

Namely obtain the relation between power vector matrix τ and the generalized coordinates q of robot thus.

6. manipulator motion control method as claimed in claim 4, it is characterized in that: in the described step (3), using the iterative learning mode of variable gain, serves as input u (t) with power vector matrix τ, the generalized coordinates q of robot is output y (t), uses variable-gain feedback D type iterative learning control law

$u_{k+1}\left(t\right)=u_k\left(t\right)+K_{d1}(k+1){\stackrel{.}{e}}_{k+1}\left(t\right)+K_{d2}\left(t\right){\stackrel{.}{e}}_k\left(t\right)$

Wherein, feedback oscillator K _D1(k)=s (k) K _D1(0), K _Di∈ R ^{M * r}Be the study gain matrix, s (k)＞the 1st is about the increasing function of iterations k; Be the current input quantity that provides ^K+1τ is by the input quantity of last time ^kτ adds last error variable quantity

Multiply by feedback matrix K _D1(k+l) and current error variable quantity

Multiply by study gain matrix K _D2(t) obtain.

Images