CN103878770A - Space robot visual delay error compensation method based on speed estimation - Google Patents

Abstract

The invention discloses a space robot visual delay error compensation method based on speed estimation. The problem of compensating the visual measurement delay errors of a space robot below a floating base is solved. The method comprises the steps that the visual delay of a system is determined, and the mathematic relation of visual measurement data with the delay and a physical reality relative position is set up; according to the visual measurement data with the delay and the joint instruction of a mechanical arm, the current space robot terminal speed is estimated; an error controller is designed, and estimation errors of the space robot terminal speed are reduced; according to a corrected space robot terminal speed estimation value, the current visual measurement data with the delay are compensated, and the compensated visual measurement data are obtained. According to the method, the current space robot terminal speed is estimated through historical measurement data combined with the joint angular speed instruction, the error controller is designed to reduce the errors of speed estimation, precise visual delay compensation of the space robot below the floating base is achieved, and the space robot can conveniently complete fine operation tasks with high precision.

Description

Robot for space vision time delay error compensating method based on velocity estimation

Technical field

The present invention relates to a kind of robot for space vision measurement time delay error compensation method.

Background technology

When robot for space (comprising carrier spacecraft and mechanical arm) under floating pedestal is carried out space tasks, the practical problem facing is the vision measurement system on its mechanical arm, while carrying out Vision information processing, often need to consume the longer time, one of them reason is due to illumination condition complexity, the Vision information processing method that robot for space adopts is more complicated, Another reason is that to can be used in the processor computing capability of robot for space Vision information processing often very limited, because having caused vision measurement information, the contradiction between visual information method complexity and Vision information processing equipment computing capability often there is larger time delay, show on videogrammetry system and just show as and there is larger measure error.Due to the existence of vision measurement error, make robot for space in the time that performance objective such as catches at the task, can affect its performance accuracy.In order to make robot for space can complete more fast, stably meticulous service role in-orbit, in Space Robot System design process, need to consider robot for space vision time delay compensation of error problem.

At present there are some to can be used for prediction and the method for estimation of ground fixed pedestal robot vision time delay error processing.Least-squares estimation be quadratic sum minimum taking error as criterion, estimate a kind of basic parameter method of estimation of unknown parameter in linear model according to observation data.Its basic ideas are that Selectivity Estimating amount makes model output and the quadratic sum of the difference of actual measurement output reach minimum.This mode of asking error sum of squares can avoid positive negative error to offset, and is convenient to Mathematical treatment.Kalman filtering is that a kind of Linear Minimum Variance is estimated, past and current state that can estimated signal, even can estimate state in the future.Kalman prediction model estimates desired signal by method from the measured value relevant with being extracted signal.Be wherein the random response being caused by white-noise excitation by estimated signal, oneself knows the system equation between driving source and response, measuring amount and also oneself knows by the functional relation between estimator.Smith prediction device is often applied to the processing of ground robot vision time delay.The action principle of Smith prediction device is by introducing time delay rectification link, to compensating with the vision data of measuring time delay, thereby the pure time delay part in system being separated to outside whole closed-loop system, thereby improves the control performance of whole control system.Can find out from the action principle of Smith prediction device, the basis of Smith Predictor Design is must guarantee to dope accurately the dynamic characteristic of controlled device under given input state, can compensate the time delay part in metrical information, therefore Smith prediction device depends on and sets up accurate Mathematical Modeling very much.Robot for space under floating pedestal is different from ground robot, the pedestal of not fixing due to robot for space, together with the sports coupling of the motion of mechanical arm and carrier spacecraft, the motion of any space manipulator all can change position and the attitude of carrier spacecraft, and the change of carrier spacecraft position and attitude also can affect the location of space manipulator conversely, therefore the dynamics calculation of robot for space is very complicated.Moreover, along with the continuous consumption of carrier spacecraft fuel, quality, centroid position and the inertia matrix of carrier spacecraft all constantly changing, and accurate Mathematical Modeling is difficult to obtain, and therefore classical prediction and method of estimation are difficult to obtain good effect in robot for space application.

Summary of the invention

The invention provides a kind of Free-floating underchassis space robot vision time delay error compensation method based on velocity estimation, the vision measurement error producing because time delay exists to reduce robot for space vision system, is conducive to robot for space and completes meticulous space tasks based on compensation after-vision information.

Robot for space vision time delay error compensating method based on velocity estimation is completed by following steps:

Step 1, determine the vision time delay of vision system according to adopted visual processes algorithm and applied hardware, set up the mathematical relationship between vision measurement data with time delay and the true relative pose of physics (measurement camera and target); Step 2, basis, with the vision measurement data of time delay and the joint instruction of mechanical arm, are estimated the tip speed (seeing formula (26)) of current robot for space; Step 3, design error controller, reduce the error of robot for space tip speed valuation, obtains proofreading and correct rear space robot end Velocity Estimation (seeing formula (30)); Step 4, according to proofreading and correct rear space robot end Velocity Estimation, the current vision measurement data with time delay are compensated, obtain the vision measurement data (seeing formula (32)) through overcompensation.

The present invention has following beneficial effect: the inventive method, by robot for space mathematics model analysis, has provided and utilized historical measurement data fusion joint angle speed command to estimate current robot for space tip speed.The measurement data with time delay and the instruction of joint of mechanical arm angular speed that the present invention utilizes robot for space vision system to obtain, estimate current robot for space tip speed, design evaluated error controller simultaneously, reduce the error of robot for space tip speed valuation, according to obtaining proofreading and correct rear space robot end Velocity Estimation, can effectively compensate the vision measurement error of the Free-floating underchassis space robot causing due to time delay to target, can ensure that robot for space completes accurate operation task in-orbit accurately.

The present invention does not need to set up the accurate Mathematical Modeling of robot for space, the true tip speed of mechanical arm of utilizing mathematics model analysis to obtain and the simplification relation with time delay tip speed, carry out tip speed estimation, and adopt the method for closed-loop control to improve the precision of robot for space vision time delay error compensation; The method has been avoided complicated dynamics calculation, calculate simple, be convenient to realize the accurate vision time delay compensation of Free-floating underchassis space robot, be conducive to implementation space robot and complete accurately accurate operation task, can be used for the space applications such as robot for space is safeguarded in-orbit, space junk cleaning, survey of deep space.

Brief description of the drawings

Fig. 1 is vision time delay error control system block diagram of the present invention; Fig. 2 is flow chart of the present invention; Fig. 3 is Space Robot System of the present invention and each coordinate system schematic diagram; Fig. 4 is data estimator and raw measurement data and the true relative position comparison diagram that compensation after-vision is measured, wherein: Fig. 4 a be compensation after-vision data estimator and the raw measurement data measured and truly relative position at the axial comparison diagram of x, Fig. 4 b be compensation after-vision data estimator and the raw measurement data measured and true relative position at the axial comparison diagram of y, Fig. 4 c is the data estimator measured of compensation after-vision with raw measurement data with true relative position at the axial comparison diagram of z;

Fig. 5 is compensation after-vision measurement data evaluated error and the comparison diagram that does not compensate the time delay error of anterior optic measurement data, wherein: Fig. 5 a is for compensation after-vision measurement data evaluated error and do not compensate the time delay error of anterior optic measurement data at the axial comparison diagram of x, Fig. 5 b is vision measurement data estimation error and do not compensate the time delay error of anterior optic measurement data at the axial comparison diagram of y, and Fig. 5 c is vision measurement data estimation error and do not compensate the time delay error of anterior optic measurement data at the axial comparison diagram of z.

Detailed description of the invention

Detailed description of the invention one: present embodiment is described in conjunction with Fig. 1, Fig. 2 and Fig. 3, present embodiment is completed by following steps: one, determine the time delay of vision system according to adopted visual processes algorithm and applied hardware, set up the mathematical relationship between vision measurement data with time delay and the true relative pose of physics (measurement camera and target); Two, according to the vision measurement data of time delay and the joint instruction of mechanical arm, estimate the tip speed of current robot for space; Three, design error controller, reduces the error of robot for space tip speed valuation, obtains proofreading and correct rear space robot end Velocity Estimation; Four, according to proofreading and correct rear space robot end Velocity Estimation, the current vision measurement data with time delay are compensated, obtain the vision measurement data through overcompensation.

Detailed description of the invention two: the difference of present embodiment and detailed description of the invention one is: present embodiment is being determined the time delay of vision system according to adopted visual processes algorithm and applied hardware described in step 1, mathematical relationship between the vision measurement data of foundation with time delay and the true relative pose of physics (measuring camera and target) for: determine that according to adopted Vision information processing method and applied hardware platform the time delay that whole vision measurement link causes is m cycle, if the duration in each cycle of system is t _s, the vision time delay T of Space Robot System _dfor:

T _d=m×t _s (1)

The vision measurement information D in the k of definition space robot moment _vand actual distance information D (k) _r(k) be

D _v(k)=[x _v(k)y _v(k)z _v(k)α _v(k)β _v(k)γ _v(k)] ^T (2)

D _r(k)=[x _r(k)y _r(k)z _r(k)α _r(k)β _r(k)γ _r(k)] ^T (3)

Wherein k is the general moment, x (k), y (k), z (k) is relative position information, α (k), β (k), γ (k) is for describing the Eulerian angles of relative pose, robot for space vision measurement information D _vand actual distance information D (k) _r(k) pass between is

D _r(k)=D _v(k+m) (4)

K represents any time, and m represents time delay.

If current time is N, vision measurement information is D _v(N), can know D _r(N-m) actual distance information before all can directly be obtained by the vision measurement data in moment in past.Definition true velocity information V _r(k) be

$V_r\left(k\right)={\left[\begin{array}{cccccc}{\stackrel{\·}{x}}_r\left(k\right)& {\stackrel{\·}{y}}_r\left(k\right)& {\stackrel{\·}{z}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\α}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\β}}}_r\left(k\right)& {\stackrel{\·}{\mathrm{\γ}}}_r\left(k\right)\end{array}\right]}^T---\left(5\right)$

Can be by D _rand D (k+1) _r(k) calculate

$V_r\left(k\right)=\frac{D_r(k+1)-D_r\left(k\right)}{t_s}---\left(6\right)$

Therefore the true velocity information V of robot for space _rand V (N-m-1) _r(N-m-1) true velocity information before also can be by calculating, real range information D now _k(N) be

$D_r\left(N\right)=D_v\left(N\right)+t_s\×\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{V}_r(N-i)---\left(7\right)$

Detailed description of the invention three: the difference of present embodiment and detailed description of the invention one or two is: present embodiment described in step 2 according to the joint instruction of the vision measurement data with time delay and mechanical arm, estimate the tip speed of current robot for space, its process is:

Utilize the vision measurement data with time delay, can obtain true velocity sequence

valuation sequence

can Negotiation speed valuation sequence current vision measurement information Dv (N) is compensated, and formula (7) can be rewritten as:

${\tilde{D}}_r\left(N\right)=D_v\left(N\right)+t_s\×\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{\tilde{V}}_r(N-i)---\left(8\right)$

Thereby obtain the estimated value of current actual distance information ;

Below the robot for space vision measurement data with time delay are combined with the joint instruction of mechanical arm, construct the valuation of the robot for space tip speed of current time.

Because the robot for space under zero-g state meets the conservation of momentum and the conservation of angular momentum, can set up the mathematical relationship between robot for space joint motions state and end-effector generalized velocity according to this, meet equation:

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=J_g({\mathrm{\Ψ}}_0,\mathrm{\Θ},m_i,I_i)\stackrel{\·}{\mathrm{\Θ}}=\left[\begin{array}{c}{J}_{g\_v}\\ J_{g\_\mathrm{\ω}}\end{array}\right]\stackrel{\·}{\mathrm{\Θ}}---\left(9\right)$

Wherein parameter matrix J _gbeing exactly the broad sense Jacobian matrix of robot for space, is inertia matrix I _i, mass parameter m _iwith joint of robot angle Θ, carrier spacecraft attitude Ψ ₀function,

the joint angle speed of robot.Wherein

$v_e={\left[\begin{array}{ccc}{\stackrel{\·}{x}}_r& {\stackrel{\·}{y}}_r& {\stackrel{\·}{z}}_r\end{array}\right]}^T---\left(10\right)$

${\mathrm{\&omega;}}_e=\left[\begin{array}{ccc}0& -\sin {\mathrm{\&alpha;}}_r& \cos {\mathrm{\&alpha;}}_r\cos {\mathrm{\&beta;}}_{\mathrm{<figure-callout\; id="0"\; label="r"\; filenames="20141013835191000034.png,20141013835191000035.png"\; state="\{\{state\}\}">r</figure-callout>}}\\ 0& \cos {\mathrm{\&alpha;}}_r& \sin {\mathrm{\&alpha;}}_r\cos {\mathrm{\&beta;}}_{\mathrm{<figure-callout\; id="1"\; label="r"\; filenames="HDA0000487995820000051.png"\; state="\{\{state\}\}">r</figure-callout>}}\\ 1& 0& -\sin {\mathrm{\&beta;}}_r\end{array}\right]\left[\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r\\ {\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_r\\ {\stackrel{\&CenterDot;}{\mathrm{\&gamma;}}}_r\end{array}\right]---\left(11\right)$

V _eand ω _erespectively linear velocity and the angular speed of robot for space end effector,

with

for the differential of robot for space terminal angle Eulerian angles.

Generalized Jacobian is launched, consider the end generalized velocity of robot for space

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\&omega;}}_e\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{J}_{\mathrm{gi}}{\mathrm{\&theta;}}_i---\left(12\right)$

Wherein J _githe i row of broad sense Jacobian matrix.

Because the actual value of the each kinematics of robot for space, kinetic parameter is included in the measurement data with time delay, when definition M, be engraved in the neighborhood of current time N, by J _gi(M) expand into:

$J_{\mathrm{gi}}\left(M\right)=J_{\mathrm{gi}}\left(N\right)+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{\&infin;}{\mathrm{\&Sigma;}}}\frac{1}{j!}\frac{{\&PartialD;}^j{J}_{\mathrm{gi}}}{{\&PartialD;\mathrm{\&theta;}}_i^j}{[{\mathrm{\&theta;}}_i\left(M\right)-{\mathrm{\&theta;}}_i\left(N\right)]}^j+E_{\mathrm{Ji}}^{\&prime;}\left(M\right)---\left(13\right)$

Wherein E' _ji(M) for changing because of attitude of carrier the error causing.Only retain once item wherein, be further deformed into

$\left\{\begin{array}{c}{J}_{\mathrm{gi}}\left(M\right)=J_{\mathrm{gi}}\left(N\right)+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{\&PartialD;J_{\mathrm{gi}}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_i}[{\mathrm{\&theta;}}_i\left(M\right)-{\mathrm{\&theta;}}_i\left(N\right)]+E_{\mathrm{Ji}}\left(M\right)\\ E_{\mathrm{Ji}}\left(M\right)=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\underset{j=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}\frac{1}{j!}\frac{{\&PartialD;}^j{J}_{\mathrm{gi}}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_i^j}{[{\mathrm{\&theta;}}_i\left(M\right)-{\mathrm{\&theta;}}_i\left(N\right)]}^j+E_{\mathrm{Ji}}^{\&prime;}\left(M\right)\end{array}\right.---\left(14\right)$

Now the robot for space tip speed in M moment can be expressed as

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\&omega;}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{J}_{\mathrm{gi}}\left(M\right){\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i\left(M\right)\\ =J_g\left(N\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)+{\mathrm{\&Delta;J}}_N\left(M\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)+E_J\left(M\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\end{array}---\left(15\right)$

Wherein

${\mathrm{\&Delta;J}}_N\left(M\right)=\left[\frac{\&PartialD;J_{g1}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_1}\right[{\mathrm{\&theta;}}_1\left(M\right)-{\mathrm{\&theta;}}_1\left(N\right)]\&CenterDot;\&CenterDot;\&CenterDot;\frac{{\&PartialD;J}_{\mathrm{gn}}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_n}[{\mathrm{\&theta;}}_n\left(M\right)-{\mathrm{\&theta;}}_n\left(N\right)\left]\right]---\left(16\right)$

The robot for space joint angle speed in M moment is expressed as

$\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_{\mathrm{\&theta;}}\left(M\right)---\left(17\right)$

Wherein

$E_{\mathrm{\&theta;}}\left(M\right)=\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)-\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)---\left(18\right)$

While moving due to robot for space, joint angle velocity variations is slow, so formula mistake! Do not find Reference source.Section 1 is the major part of M moment robot for space joint angle speed, E _θ(M) be the remainder relevant to acceleration;

Bring formula (17) into formula (15), the robot for space tip speed that obtains the M moment is expressed as:

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\&omega;}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{J}_{\mathrm{gi}}\left(M\right){\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i\left(M\right)\\ =\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}{J}_g\left(N\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+{\mathrm{\&Delta;J}}_N\left(M\right)\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V\left(M\right)\\ =\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\&omega;}}_e\left(N\right)\end{array}\right]+{\mathrm{\&Delta;J}}_N\left(M\right)\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V\left(M\right)\end{array}---\left(19\right)$

Wherein

$E_V\left(M\right)=J_g\left(N\right)E_{\mathrm{\&theta;}}\left(M\right)+\mathrm{\&Delta;J}\left(M\right)E_{\mathrm{\&theta;}}+E_J\left(M\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)---\left(20\right)$

Current time is n-hour, and vision measurement time delay is P sampling period, and according to formula (19), the tip speed that obtains the N-P moment is:

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\&omega;}}_e(N-P)\end{array}\right]=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\&omega;}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\&Delta;J}}_N(N-P)\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V(N-P)\end{array}---\left(21\right)$

According to formula (19), the tip speed that obtains the N-P-1 moment is:

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\&omega;}}_e(N-P-1)\end{array}\right]=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P-1)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\&omega;}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\&Delta;J}}_N(N-P-1)\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P-1)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V(N-P-1)\end{array}---\left(22\right)$

And the N-P-1 moment is for the target rough estimate matrix △ J of n-hour _ncan be expressed as:

$\begin{array}{c}{\mathrm{\&Delta;J}}_N(N-P-1)\\ =\left[\frac{{\&PartialD;J}_{g1}\left(N\right)}{\&PartialD;{\mathrm{\&theta;}}_1}\right[{\mathrm{\&theta;}}_1(N-P-1)-{\mathrm{\&theta;}}_1\left(N\right)]\&CenterDot;\&CenterDot;\&CenterDot;\frac{{\&PartialD;J}_{\mathrm{gn}}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_n}[{\mathrm{\&theta;}}_n(N-P-1){\mathrm{\&theta;}}_n\left(N\right)\left]\right]\\ \frac{[{\mathrm{\&theta;}}_1(N-P-1)-{\mathrm{\&theta;}}_1\left(N\right)]\&CenterDot;[{\mathrm{\&theta;}}_1(N-P)-{\mathrm{\&theta;}}_1\left(N\right)]}{{\left|\right[{\mathrm{\&theta;}}_1(N-P)-\left]{\mathrm{\&theta;}}_1\left(N\right)\right]|}_2^2}{\mathrm{\&Delta;J}}_N(N-P)+T_{\mathrm{\&Delta;J}}\end{array}---\left(23\right)$

Wherein T _{△ J}for remainder.Can obtain current tip speed according to above conclusion is

$\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\&omega;}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\&beta;}}{{\mathrm{\&alpha;}}_1(1-\mathrm{\&beta;})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\&omega;}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\&alpha;}}_2(1-\mathrm{\&beta;})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\&omega;}}_e(N-P-1)\end{array}\right]+\mathrm{\&Delta;E}---\left(24\right)$

Wherein

$\left\{\begin{array}{c}\mathrm{\&beta;}=\frac{[{\mathrm{\&theta;}}_1(N-P-1)-{\mathrm{\&theta;}}_1\left(N\right)]\&CenterDot;[{\mathrm{\&theta;}}_1(N-P)-{\mathrm{\&theta;}}_1\left(N\right)]}{{\left|\right[{\mathrm{\&theta;}}_1(N-P)-{\mathrm{\&theta;}}_1\left(N\right)\left]\right|}_2^2}\\ {\mathrm{\&alpha;}}_1=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\right|}_2^2}\\ {\mathrm{\&alpha;}}_2=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P-1)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\end{array}\right.---\left(25\right)$

For convenient, can claim α ₁, α ₂for second-order linearity estimation coefficient, β is state difference item.Ignore high-order error △ E, now tip speed can approximate calculation be

$\left[\begin{array}{c}{\tilde{v}}_e\left(N\right)\\ {\widetilde{\mathrm{\&omega;}}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\&beta;}}{{\mathrm{\&alpha;}}_1(1-\mathrm{\&beta;})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\&omega;}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\&alpha;}}_2(1-\mathrm{\&beta;})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\&omega;}}_e(N-P-1)\end{array}\right]---\left(26\right)$

Evaluated error is now

$\mathrm{\&Delta;E}=\frac{-\mathrm{\&beta;}}{{\mathrm{\&alpha;}}_1(1-\mathrm{\&beta;})}{E}_V(N-P)+\frac{1}{{\mathrm{\&alpha;}}_2(1-\mathrm{\&beta;})}[T_{\mathrm{\&Delta;J}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V(N-P-1)]---\left(27\right)$

Estimating speed now may be calculated

$\begin{array}{c}{{\tilde{V}}_r\left(k\right)=\left[\begin{array}{cccccc}{\stackrel{\&CenterDot;}{x}}_r\left(k\right)& {\stackrel{\&CenterDot;}{y}}_r\left(k\right)& {\stackrel{\&CenterDot;}{z}}_r\left(k\right)& {\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r\left(k\right)& {\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_r\left(k\right)& {\stackrel{\&CenterDot;}{\mathrm{\&gamma;}}}_r\left(k\right)\end{array}\right]}^T\\ \left[\begin{array}{c}{v}_e\\ ={\left[\begin{array}{ccc}0& -\sin {\mathrm{\&alpha;}}_r& \cos {\mathrm{\&alpha;}}_r\cos {\mathrm{\&beta;}}_{\mathrm{<figure-callout\; id="0"\; label="r"\; filenames="20141013835191000034.png,20141013835191000035.png"\; state="\{\{state\}\}">r</figure-callout>}}\\ 0& \cos {\mathrm{\&alpha;}}_r& \sin {\mathrm{\&alpha;}}_r{\cos \mathrm{\&beta;}}_{\mathrm{<figure-callout\; id="1"\; label="r"\; filenames="HDA0000487995820000051.png"\; state="\{\{state\}\}">r</figure-callout>}}\\ 1& 0& -\sin {\mathrm{\&beta;}}_r\end{array}\right]}^{-1}{\mathrm{\&omega;}}_e\end{array}\right]\end{array}.---\left(28\right)$

Measuring the motion of less, the robot for space of time delay more stably in situation, the error △ E directly estimating should be very little, and evaluated error △ E also may be larger in other cases.For the larger special circumstances of evaluated error, design an error controller, estimated value is carried out to real-time correction, thereby improve estimated accuracy.

Detailed description of the invention four: the difference of present embodiment and detailed description of the invention three is: present embodiment is designed at the error controller described in step 3: regard the process of the estimating speed sequence described in step 2 as a differentiation element with error:

$P\left(s\right)={\tilde{e}}^{{\mathrm{st}}_s}=e^{{\mathrm{st}}_s}[1+H_{\mathrm{\&Delta;}}\left(s\right)]---\left(29\right)$

Wherein

for the error transfer function of differentiation element, the transfer function of vision time delay error control system output is:

$\begin{array}{c}Y\left(s\right)=\frac{e^{{-\mathrm{st}}_s}{\tilde{e}}^{{\mathrm{st}}_s}+G\left(s\right)e^{{-\mathrm{<figure-callout\; id="1"\; label="st\; s"\; filenames="HDA0000487995820000051.png"\; state="\{\{state\}\}">st</figure-callout>}}_s}}{1+G\left(s\right)e^{{\mathrm{st}}_s}}{V}_r\left(s\right)\\ =V_r\left(s\right)+\frac{H_{\mathrm{\&Delta;}}\left(s\right)}{1+G\left(s\right)e^{-{\mathrm{st}}_s}}{V}_r\left(s\right)\end{array}---\left(30\right)$

Wherein V _r(s) be the actual value of robot end's speed, Y (s) is robot end's Velocity Estimation after proofreading and correct, and G (s) is error controller.

The evaluated error transfer function of system is:

$E\left(s\right)\frac{-H_{\mathrm{\&Delta;}}\left(s\right)e^{{-\mathrm{st}}_s}-G\left(s\right)e^{{-2\mathrm{<figure-callout\; id="1"\; label="st\; s"\; filenames="HDA0000487995820000051.png"\; state="\{\{state\}\}">st</figure-callout>}}_s}}{1+G\left(s\right)e^{-s{t}_s}}{V}_r\left(s\right)---\left(31\right)$

By POLE PLACEMENT USING, design makes the error controller G (s) that E (s) is stable, and evaluated error E (t) is convergence gradually in time, and after proofreading and correct, robot end's Velocity Estimation levels off to speed actual value gradually.

Detailed description of the invention five: the difference of present embodiment and detailed description of the invention four is: described according to proofreading and correct rear space robot end Velocity Estimation described in step 4 of present embodiment, the current vision measurement data with time delay are compensated, through the vision measurement data of overcompensation be calculated as follows:

${\tilde{D}}_r\left(N\right)=D_v\left(N\right)+t_s\&times;\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}Y(N-i)---\left(32\right)$

Wherein Y (N-i) is the correction rear space robot end Velocity Estimation that step 4 draws, has so far completed vision time delay compensation of error.

The content not being described in detail in description of the present invention belongs to the known prior art of professional and technical personnel in the field.

Embodiment

In conjunction with Fig. 1, Fig. 2, Fig. 3, the present embodiment is described, experimental system is made up of robot for space, target satellite, and robot for space comprises six degree of freedom mechanical arm and carrier spacecraft, and mechanical arm tail end is installed camera.Mechanical arm band camera is followed the tracks of and is approached target star, and target satellite moves along mechanical arm tail end Z-direction with the speed of 4mm/s.

For setting up analogue system, kinematics and the kinetic parameter of carrier spacecraft and six degree of freedom mechanical arm are as shown in table 1

Table 1. robot for space parameter

The vision time delay error compensation step of robot for space is:

Step 1, determine the time delay of vision system according to adopted Vision information processing method and applied hardware environment, according to formula mistake! Do not find Reference source.Set up the mathematical relationship between actual distance information and vision measurement information.

Step 2, utilize formula (26) to calculate current time robot for space tip speed to estimate.

Step 3, according to formula (30) design vision time delay error controller, via controller output, obtains proofreading and correct rear space robot end Velocity Estimation.

Step 4, utilize formula (32) calculate compensation after vision measurement data.

Designed robot for space vision time delay error compensation actual effect as shown in Figure 4, Figure 5.The data estimator of being measured by the compensation after-vision of Fig. 4 and raw measurement data and true relative position comparison diagram, can find out, due to the existence of vision time delay, in X, Y and tri-directions of Z, there is larger error in measured value and the actual value of vision system to target, through error compensation, the data estimator of vision measurement approaches actual value very much.By the compensation after-vision measurement data evaluated error and the comparison diagram that does not compensate the time delay error of anterior optic measurement data of Fig. 5, can find out, compensation anterior optic is measured the worst error with actual value, be reduced to 7mm at directions X by 18mm, be reduced to 8mm in Z-direction by 38mm, can find out this delay compensation method of the present invention, can obviously reduce the vision measurement error that Free-floating underchassis space robot causes due to time delay, be conducive to ensure better that robot for space completes the requirement of accurate operation task in-orbit accurately, and whole process computation is simple, do not need to set up the accurate Mathematical Modeling of robot for space, do not need to carry out complicated dynamics calculation, can meet engineering actual demand.

Claims (5)

1. the robot for space vision time delay error compensating method based on velocity estimation, is characterized in that: described method is completed by following steps:

Step 1, determine the vision time delay of vision system according to adopted visual processes algorithm and applied hardware, set up the mathematical relationship between vision measurement data and the true relative pose of physics with time delay;

Step 2, basis, with the vision measurement data of time delay and the joint instruction of mechanical arm, are estimated the tip speed of current robot for space;

Step 3, design error controller, reduce the error of robot for space tip speed valuation, obtains proofreading and correct rear space robot end Velocity Estimation;

Step 4, according to proofreading and correct rear space robot end Velocity Estimation, the current vision measurement data with time delay are compensated, obtain the vision measurement data through overcompensation.

2. a kind of robot for space vision time delay error compensating method based on velocity estimation according to claim 1, is characterized in that:

In step 1, determine the time delay of vision system according to adopted visual processes algorithm and applied hardware, set up the mathematical relationship between vision measurement data with time delay and the true relative pose of physics (measurement camera and target), its process is:

Determine that according to adopted Vision information processing method and applied hardware platform the time delay that whole vision measurement link causes is m cycle, if the duration in each cycle of system is t _s, the vision time delay T of Space Robot System _dfor:

T _d=m×t _s (1)

The vision measurement information D in the k of definition space robot moment _vand actual distance information D (k) _r(k) be:

D _v(k)=[x _v(k)y _v(k)z _v(k)α _v(k)β _v(k)γ _v(k)] ^T (2)

D _r(k)=[x _r(k)y _r(k)z _r(k)α _r(k)β _r(k)γ _r(k)] ^T (3)

Wherein k is the general moment, x (k), _y(k), z (k) is relative position information, α (k), and β (k), γ (k) is for describing the Eulerian angles of relative pose, robot for space vision measurement information D _vand actual distance information D (k) _r(k) pass between is:

D _r(k)=D _v(k+m) (4)

K represents any time, and m represents time delay;

If current time is N, vision measurement information is D _v(N), can know D _r(N-m) actual distance information before all can directly be obtained by the vision measurement data in moment in past.Definition true velocity information V _r(k) be:

$V_r\left(k\right)={\left[\begin{array}{cccccc}{\stackrel{\&CenterDot;}{x}}_r\left(k\right)& {\stackrel{\&CenterDot;}{y}}_r\left(k\right)& {\stackrel{\&CenterDot;}{z}}_r\left(k\right)& {\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r\left(k\right)& {\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_r\left(k\right)& {\stackrel{\&CenterDot;}{\mathrm{\&gamma;}}}_r\left(k\right)\end{array}\right]}^T---\left(5\right)$

By D _rand D (k+1) _r(k) calculate

$V_r\left(k\right)=\frac{D_r(k+1)-D_r\left(k\right)}{t_s}---\left(6\right)$

The true velocity information V of robot for space _rand V (N-m-1) _r(N-m-1) true velocity information exchange is before crossed and is calculated: real range information D _k(N) be

$D_r\left(N\right)=D_v\left(N\right)+t_s\&times;\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{V}_r(N-i)---\left(7\right)$

3. a kind of robot for space vision time delay error compensating method based on velocity estimation according to claim 1 and 2, it is characterized in that: in step 2, described according to the joint instruction of the vision measurement data with time delay and mechanical arm, estimate the tip speed of current robot for space, its process is:

Utilize the vision measurement data with time delay, obtain true velocity sequence valuation sequence

can Negotiation speed valuation sequence

to current vision measurement information D _v(N) compensate, formula (7) is rewritten as:

${\tilde{D}}_r\left(N\right)=D_v\left(N\right)+t_s\&times;\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{\tilde{V}}_r(N-i)---\left(8\right)$

Thereby obtain the estimated value of current actual distance information

Robot for space vision measurement data with time delay are combined with the joint instruction of mechanical arm, construct the valuation of the robot for space tip speed of current time;

Because the robot for space under zero-g state meets the conservation of momentum and the conservation of angular momentum, set up the mathematical relationship between robot for space joint motions state and end-effector generalized velocity according to this, meet equation:

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\&omega;}}_e\end{array}\right]=J_g({\mathrm{\&Psi;}}_0,\mathrm{\&Theta;},m_i,I_i)\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}=\left[\begin{array}{c}{J}_{g\_v}\\ J_{g\_\mathrm{\&omega;}}\end{array}\right]\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}---\left(9\right)$

Wherein parameter matrix J _gbeing exactly the broad sense Jacobian matrix of robot for space, is inertia matrix I _i, mass parameter m _iwith joint of robot angle Θ, carrier spacecraft attitude Ψ ₀function,

the joint angle speed of robot; Wherein

$v_e={\left[\begin{array}{ccc}{\stackrel{\&CenterDot;}{x}}_r& {\stackrel{\&CenterDot;}{y}}_r& {\stackrel{\&CenterDot;}{z}}_r\end{array}\right]}^T---\left(10\right)$

${\mathrm{\&omega;}}_e=\left[\begin{array}{ccc}0& -\sin {\mathrm{\&alpha;}}_r& \cos {\mathrm{\&alpha;}}_r\cos {\mathrm{\&beta;}}_r\\ 0& \cos {\mathrm{\&alpha;}}_r& \sin {\mathrm{\&alpha;}}_r\cos {\mathrm{\&beta;}}_r\\ 1& 0& -\sin {\mathrm{\&beta;}}_r\end{array}\right]\left[\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r\\ {\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_r\\ {\stackrel{\&CenterDot;}{\mathrm{\&gamma;}}}_r\end{array}\right]---\left(11\right)$

V _eand ω _erespectively linear velocity and the angular speed of robot for space end effector,

with

for the differential of robot for space terminal angle Eulerian angles;

Generalized Jacobian is launched, considers the end generalized velocity of robot for space:

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\&omega;}}_e\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{J}_{\mathrm{gi}}{\mathrm{\&theta;}}_i---\left(12\right)$

Wherein J _githe i row of broad sense Jacobian matrix, θ _ii joint of robot angle;

Because the actual value of the each kinematics of robot for space, kinetic parameter is included in the measurement data with time delay, when definition M, be engraved in the neighborhood of current time N, by J _gi(M) expand into:

$J_{\mathrm{gi}}\left(M\right)=J_{\mathrm{gi}}\left(N\right)+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{\&infin;}{\mathrm{\&Sigma;}}}\frac{1}{j!}\frac{{\&PartialD;}^j{J}_{\mathrm{gi}}}{{\&PartialD;\mathrm{\&theta;}}_i^j}{[{\mathrm{\&theta;}}_i\left(M\right)-{\mathrm{\&theta;}}_i\left(N\right)]}^j+E_{\mathrm{Ji}}^{\&prime;}\left(M\right)---\left(13\right)$

Wherein E ' _ji(M) for changing because of attitude of carrier the error causing; Only retain once item wherein, be further deformed into

$\left\{\begin{array}{c}{J}_{\mathrm{gi}}\left(M\right)=J_{\mathrm{gi}}\left(N\right)+\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{\&PartialD;J_{\mathrm{gi}}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_i}[{\mathrm{\&theta;}}_i\left(M\right)-{\mathrm{\&theta;}}_i\left(N\right)]+E_{\mathrm{Ji}}\left(M\right)\\ E_{\mathrm{Ji}}\left(M\right)=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\underset{j=2}{\overset{\&infin;}{\mathrm{\&Sigma;}}}\frac{1}{j!}\frac{{\&PartialD;}^j{J}_{\mathrm{gi}}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_i^j}{[{\mathrm{\&theta;}}_i\left(M\right)-{\mathrm{\&theta;}}_i\left(N\right)]}^j+E_{\mathrm{Ji}}^{\&prime;}\left(M\right)\end{array}\right.---\left(14\right)$

Now the robot for space tip speed in M moment can be expressed as

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\&omega;}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{J}_{\mathrm{gi}}\left(M\right){\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i\left(M\right)\\ =J_g\left(N\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)+{\mathrm{\&Delta;J}}_N\left(M\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)+E_J\left(M\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\end{array}---\left(15\right)$

Wherein

${\mathrm{\&Delta;J}}_N\left(M\right)=\left[\frac{\&PartialD;J_{g1}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_1}\right[{\mathrm{\&theta;}}_1\left(M\right)-{\mathrm{\&theta;}}_1\left(N\right)]\&CenterDot;\&CenterDot;\&CenterDot;\frac{{\&PartialD;J}_{\mathrm{gn}}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_n}[{\mathrm{\&theta;}}_n\left(M\right)-{\mathrm{\&theta;}}_n\left(N\right)\left]\right]---\left(16\right)$

The robot for space joint angle speed in M moment is expressed as:

$\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_{\mathrm{\&theta;}}\left(M\right)---\left(17\right)$

Wherein

$E_{\mathrm{\&theta;}}\left(M\right)=\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)-\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)---\left(18\right)$

While moving due to robot for space, joint angle velocity variations is slow, so formula (17) Section 1 is the major part of M moment robot for space joint angle speed, and E _θ(M) be the remainder relevant to acceleration;

Bring formula (17) into formula (15), the robot for space tip speed that obtains the M moment is expressed as:

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\&omega;}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{J}_{\mathrm{gi}}\left(M\right){\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i\left(M\right)\\ =\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}{J}_g\left(N\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+{\mathrm{\&Delta;J}}_N\left(M\right)\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V\left(M\right)\\ =\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\&omega;}}_e\left(N\right)\end{array}\right]+{\mathrm{\&Delta;J}}_N\left(M\right)\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V\left(M\right)\end{array}---\left(19\right)$

Wherein

$E_V\left(M\right)=J_g\left(N\right)E_{\mathrm{\&theta;}}\left(M\right)+\mathrm{\&Delta;J}\left(M\right)E_{\mathrm{\&theta;}}+E_J\left(M\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(M\right)---\left(20\right)$

Current time is n-hour, and vision measurement time delay is P sampling period, and according to formula (19), the tip speed that obtains the N-P moment is:

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\&omega;}}_e(N-P)\end{array}\right]=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\&omega;}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\&Delta;J}}_N(N-P)\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V(N-P)\end{array}---\left(21\right)$

According to formula (19), the tip speed that obtains the N-P-1 moment is:

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\&omega;}}_e(N-P-1)\end{array}\right]=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P-1)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\&omega;}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\&Delta;J}}_N(N-P-1)\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P-1)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V(N-P-1)\end{array}---\left(22\right)$

And the N-P-1 moment is for the target rough estimate matrix △ J of n-hour _ncan be expressed as:

$\begin{array}{c}{\mathrm{\&Delta;J}}_N(N-P-1)\\ =\left[\frac{{\&PartialD;J}_{g1}\left(N\right)}{\&PartialD;{\mathrm{\&theta;}}_1}\right[{\mathrm{\&theta;}}_1(N-P-1)-{\mathrm{\&theta;}}_1\left(N\right)]\&CenterDot;\&CenterDot;\&CenterDot;\frac{{\&PartialD;J}_{\mathrm{gn}}\left(N\right)}{{\&PartialD;\mathrm{\&theta;}}_n}[{\mathrm{\&theta;}}_n(N-P-1){\mathrm{\&theta;}}_n\left(N\right)\left]\right]\\ \frac{[{\mathrm{\&theta;}}_1(N-P-1)-{\mathrm{\&theta;}}_1\left(N\right)]\&CenterDot;[{\mathrm{\&theta;}}_1(N-P)-{\mathrm{\&theta;}}_1\left(N\right)]}{{\left|\right[{\mathrm{\&theta;}}_1(N-P)-\left]{\mathrm{\&theta;}}_1\left(N\right)\right]|}_2^2}{\mathrm{\&Delta;J}}_N(N-P)+T_{\mathrm{\&Delta;J}}\end{array}---\left(23\right)$

Wherein T _{△ J}for remainder, according to formula (19), formula (21) and formula (22), the tip speed that can obtain current n-hour is:

$\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\&omega;}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\&beta;}}{{\mathrm{\&alpha;}}_1(1-\mathrm{\&beta;})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\&omega;}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\&alpha;}}_2(1-\mathrm{\&beta;})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\&omega;}}_e(N-P-1)\end{array}\right]+\mathrm{\&Delta;E}---\left(24\right)$

Wherein

$\left\{\begin{array}{c}\mathrm{\&beta;}=\frac{[{\mathrm{\&theta;}}_1(N-P-1)-{\mathrm{\&theta;}}_1\left(N\right)]\&CenterDot;[{\mathrm{\&theta;}}_1(N-P)-{\mathrm{\&theta;}}_1\left(N\right)]}{{\left|\right[{\mathrm{\&theta;}}_1(N-P)-{\mathrm{\&theta;}}_1\left(N\right)\left]\right|}_2^2}\\ {\mathrm{\&alpha;}}_1=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\right|}_2^2}\\ {\mathrm{\&alpha;}}_2=\frac{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}(N-P-1)\&CenterDot;\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)}{{\left|\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)\right|}_2^2}\end{array}\right.---\left(25\right)$

α ₁, α ₂for second-order linearity estimation coefficient, β is state difference item; Ignore high-order error △ E, now tip speed can approximate calculation be

$\left[\begin{array}{c}{\tilde{v}}_e\left(N\right)\\ {\widetilde{\mathrm{\&omega;}}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\&beta;}}{{\mathrm{\&alpha;}}_1(1-\mathrm{\&beta;})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\&omega;}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\&alpha;}}_2(1-\mathrm{\&beta;})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\&omega;}}_e(N-P-1)\end{array}\right]---\left(26\right)$

Evaluated error is:

$\mathrm{\&Delta;E}=\frac{-\mathrm{\&beta;}}{{\mathrm{\&alpha;}}_1(1-\mathrm{\&beta;})}{E}_V(N-P)+\frac{1}{{\mathrm{\&alpha;}}_2(1-\mathrm{\&beta;})}[T_{\mathrm{\&Delta;J}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(N\right)+E_V(N-P-1)]---\left(27\right)$

According to formula (10) and formula (11), estimating speed for:

$\begin{array}{c}{{\tilde{V}}_r\left(k\right)=\left[\begin{array}{cccccc}{\stackrel{\&CenterDot;}{x}}_r\left(k\right)& {\stackrel{\&CenterDot;}{y}}_r\left(k\right)& {\stackrel{\&CenterDot;}{z}}_r\left(k\right)& {\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_r\left(k\right)& {\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_r\left(k\right)& {\stackrel{\&CenterDot;}{\mathrm{\&gamma;}}}_r\left(k\right)\end{array}\right]}^T\\ \left[\begin{array}{c}{v}_e\\ ={\left[\begin{array}{ccc}0& -\sin {\mathrm{\&alpha;}}_r& \cos {\mathrm{\&alpha;}}_r\cos {\mathrm{\&beta;}}_r\\ 0& \cos {\mathrm{\&alpha;}}_r& \sin {\mathrm{\&alpha;}}_r{\cos \mathrm{\&beta;}}_r\\ 1& 0& -\sin {\mathrm{\&beta;}}_r\end{array}\right]}^{-1}{\mathrm{\&omega;}}_e\end{array}\right]\end{array}.---\left(28\right)$

4. a kind of robot for space vision time delay error compensating method based on velocity estimation according to claim 3, is characterized in that: in step 3, described error controller is designed to:

Regard the process of the estimating speed sequence described in step 2 as a differentiation element with error:

$P\left(s\right)={\tilde{e}}^{{\mathrm{st}}_s}=e^{{\mathrm{st}}_s}[1+H_{\mathrm{\&Delta;}}\left(s\right)]---\left(29\right)$

Wherein

for the error transfer function of differentiation element, the transfer function of vision time delay error control system output is:

$\begin{array}{c}Y\left(s\right)=\frac{e^{{-\mathrm{st}}_s}{\tilde{e}}^{{\mathrm{st}}_s}+G\left(s\right)e^{{-\mathrm{st}}_s}}{1+G\left(s\right)e^{{\mathrm{st}}_s}}{V}_r\left(s\right)\\ =V_r\left(s\right)+\frac{H_{\mathrm{\&Delta;}}\left(s\right)}{1+G\left(s\right)e^{-{\mathrm{st}}_s}}{V}_r\left(s\right)\end{array}---\left(30\right)$

Wherein V _r(s) be the actual value of robot end's speed, Y (s) is robot end's Velocity Estimation after proofreading and correct, and G (s) is error controller;

The evaluated error transfer function of system is:

$E\left(s\right)\frac{-H_{\mathrm{\&Delta;}}\left(s\right)e^{{-\mathrm{st}}_s}-G\left(s\right)e^{{-2\mathrm{st}}_s}}{1+G\left(s\right)e^{-s{t}_s}}{V}_r\left(s\right)---\left(31\right)$

By POLE PLACEMENT USING, design makes the error controller G (s) that E (s) is stable, and evaluated error E (t) is convergence gradually in time, and after proofreading and correct, robot end's Velocity Estimation levels off to speed actual value gradually.

5. a kind of robot for space vision time delay error compensating method based on velocity estimation according to claim 4, it is characterized in that: in step 4, described according to proofreading and correct rear space robot end Velocity Estimation, the current vision measurement data with time delay are compensated, through the vision measurement data of overcompensation

(N) be calculated as follows:

${\tilde{D}}_r\left(N\right)=D_v\left(N\right)+t_s\&times;\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}Y(N-i)---\left(32\right)$

Wherein Y (N-i) is the correction rear space robot end Velocity Estimation that step 4 draws, has so far completed vision time delay compensation of error.

Images