CN103878770B - Robot for space vision time delay error compensating method based on velocity estimation - Google Patents

Abstract

Robot for space vision time delay error compensating method based on velocity estimation, solves Free-floating underchassis space robot vision and measures the compensation problem of time delay error.Including determining the vision time delay of system, set up the mathematical relationship between the vision measurement data of band time delay and the true relative pose of physics；Vision measurement data according to band time delay and the joint instruction of mechanical arm, estimate current robot for space tip speed；Design error controller, reduces the estimation difference of robot for space tip speed；According to correction rear space robot end's Velocity Estimation, the current vision measurement data with time delay are compensated, obtains the vision measurement data through overcompensation.The present invention utilizes historical measurement data to merge joint angle speed command estimation current spatial robot end's speed, and design estimation difference controller is to reduce the error of velocity estimation；Realize the accurate vision time delay of Free-floating underchassis space robot to compensate, be advantageously implemented robot for space and complete accurate operation task accurately.

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 (including carrier spacecraft and mechanical arm) under floating pedestal performs space tasks, faced Individual practical problem is the vision measurement system on its mechanical arm, carries out generally requiring during Vision information processing when consuming longer Between, one reason for this is that the Vision information processing method that robot for space is used is more complicated, separately owing to illumination condition is complicated The processor computing capability that one reason can be used for robot for space Vision information processing is the most limited, owing to vision is believed Contradiction between breath method complexity and Vision information processing equipment computing capability result in vision measurement information often to be had Bigger time delay, shows and will appear as having bigger measurement error on videogrammetry system.Due to Slight measurement errors Exist, make robot for space when tasks such as performance objective captures, its performance accuracy can be affected.In order to enable robot for space More rapid, smoothly achieve fine service role in-orbit, need to consider space machine in Space Robot System design process Device people's vision time delay compensation of error problem.

Some have been had to can be used for the prediction of ground fixed pedestal robot vision time delay error process and estimation side at present Method.Least-squares estimation is the minimum criterion of the quadratic sum with error, according to unknown parameter in observation data estimation linear model A kind of basic parameter method of estimation.Its basic ideas be select estimator make model output with actual measurement output difference square With minimize.This mode seeking error sum of squares can avoid positive negative error to offset, and is easy to Mathematical treatment.Kalman Filtering is a kind of linear minimum-variance estimation, can estimate past and the current state of 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.Wherein estimated Meter signal be the random response caused by white-noise excitation, driving source and response between system equation oneself know, measurement amount and quilt Also oneself knows functional relationship between estimator.Smith predictor is usually applied to ground robot vision time delay and processes.Smith The action principle of prediction device is by introducing time delay rectification link, compensating with the vision data measuring time delay, thus Outside pure time delay in system is partially separated whole closed loop system, thus improve the control performance of whole control system.From The action principle of Smith predictor is it can be seen that the basis of Smith predictor design is must assure that and can predict accurately Go out controlled device dynamic characteristic under given input state, the time delay part in metrical information can be compensated, therefore Smith predictor is highly dependent on sets up accurate mathematical model.Robot for space under floating pedestal is with ground robot not With, the pedestal do not fixed due to robot for space, the motion of mechanical arm is coupled with the motion of carrier spacecraft, any The motion of space manipulator all can change position and the attitude of carrier spacecraft, and the change of carrier Space Vehicle position attitude also can Affecting the location of space manipulator in turn, therefore the dynamics calculation of robot for space is extremely complex.Moreover, along with load The continuous consumption of body spacecraft fuel, the quality of carrier spacecraft, centroid position and inertia matrix are all constantly in change, accurately Mathematical model is difficult to obtain, and therefore classical prediction and method of estimation are difficult to obtain well effect in robot for space is applied Really.

Summary of the invention

The present invention provides a kind of Free-floating underchassis space robot vision time delay error compensation method based on velocity estimation, To reduce the Slight measurement errors that robot for space visual system produces because time delay exists, beneficially robot for space is based on benefit Repay after-vision information and complete fine space tasks.

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

Step one, determine the vision time delay of visual system according to the vision processing algorithm used and the hardware applied, Set up the mathematical relationship between the vision measurement data of band time delay and the true relative pose of physics；Step 2, basis are with time delay The joint instruction of vision measurement data and mechanical arm, estimates the tip speed (see formula (26)) of current spatial robot；Step 3, Design error controller, reduces the error of robot for space tip speed valuation, obtains correcting rear space robot end's speed Valuation (see formula (30))；Step 4, according to correction rear space robot end's Velocity Estimation, currently vision with time delay is surveyed Amount data compensate, and obtain the vision measurement data (see formula (32)) through overcompensation.

The method have the advantages that the inventive method passes through robot for space mathematics model analysis, give profit Merge joint angle speed command by historical measurement data and estimate current spatial robot end's speed.The present invention utilizes space machine The measurement data of the band time delay that human visual system obtains and the instruction of joint of mechanical arm angular velocity, estimate current spatial robot end Speed, simultaneously design estimation difference controller, reduce the error of robot for space tip speed valuation, according to empty after being corrected Between robot end's Velocity Estimation, target is regarded by the Free-floating underchassis space robot that can effectively compensate for causing due to time delay Feel measurement error, it is ensured that robot for space completes accurate operation task the most accurately.

The present invention is not required to set up the accurate mathematical model of robot for space, and the mechanical arm utilizing mathematics model analysis to obtain is true Real tip speed and the simplification relation of band time delay tip speed, carry out tip speed estimation, and use the method for closed loop control to carry The high precision of robot for space vision time delay error compensation；This method avoid the dynamics calculation of complexity, calculate simple, just Compensate in realizing the accurate vision time delay of Free-floating underchassis space robot, be advantageously implemented robot for space and complete accurately Accurate operation task, can be used for robot for space safeguard in-orbit, space junk cleaning, the space application such as survey of deep space.

Accompanying drawing explanation

Fig. 1 is the vision time delay error control system block diagram of the present invention；Fig. 2 is the flow chart of the present invention；Fig. 3 is the present invention Space Robot System and each coordinate system schematic diagram；Fig. 4 be compensate estimation data and raw measurement data that after-vision measures and True position versus figure relatively, wherein: Fig. 4 a is to compensate estimation data and raw measurement data and the true phase that after-vision is measured To the position comparison diagram in x-axis direction, Fig. 4 b is to compensate estimation data and raw measurement data and the true phase that after-vision is measured To the position comparison diagram in y-axis direction, Fig. 4 c is for compensating estimation data and raw measurement data that after-vision measures and truly The position comparison diagram in z-axis direction relatively；

Fig. 5 is the contrast compensating after-vision measurement data estimation difference with not compensating anterior optic measurement data time delay error Figure, wherein: Fig. 5 a is for compensating after-vision measurement data estimation difference and not compensating anterior optic measurement data time delay error in x-axis side To comparison diagram, Fig. 5 b is vision measurement data estimation error and do not compensate anterior optic measurement data time delay error in y-axis direction Comparison diagram, Fig. 5 c is vision measurement data estimation error and do not compensate anterior optic measurement data time delay error in z-axis direction Comparison diagram.

Detailed description of the invention

Detailed description of the invention one: combining Fig. 1, Fig. 2 and Fig. 3 and present embodiment is described, present embodiment is complete by following steps Become: one, determine the time delay of visual system according to the vision processing algorithm used and the hardware applied, set up regarding of band time delay Mathematical relationship between feel measurement data and the true relative pose of physics (measuring camera and target)；Two, according to regarding with time delay Feel that the joint of measurement data and mechanical arm instructs, estimate the tip speed of current spatial robot；Three, design error controller, Reduce the error of robot for space tip speed valuation, obtain correcting rear space robot end's Velocity Estimation；Four, according to correction The current vision measurement data with time delay are compensated, obtain through overcompensation by rear space robot end's Velocity Estimation Vision measurement data.

Detailed description of the invention two: the present embodiment difference from detailed description of the invention one is: present embodiment is in step The time delay determining visual system according to the vision processing algorithm used and the hardware applied described in one, sets up band time delay Mathematical relationship between vision measurement data and the true relative pose of physics (measuring camera and target) by: according to the vision used Information processing method and the hardware platform applied determine that the time delay that whole vision measurement link causes is m cycle, if system Time a length of t in each cycle_s, then the vision time delay T of Space Robot System_dFor:

T_d=m × t_s (1)

Vision measurement information D in definition space robot k moment_v(k) and actual distance information D_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 any time, and x (k), y (k), z (k) are relative position information, and α (k), β (k), γ (k) are for describing phase Eulerian angles to pose, robot for space vision measurement information D_v(k) and actual distance information D_rK the relation between () is

D_r(k)=D_v(k+m) (4)

K represents that any time, m represent time delay.

If current time is N, vision measurement information is D_v(N), then may know that D_r(N-m) the actual distance information before is all Can be directly obtained by the vision measurement data of last time.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)_rK () is calculated

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

Therefore true velocity information V of robot for space_rAnd V (N-m-1)_r(N-m-1) the true velocity information before also may be used To pass through to be calculated, the most real range information D_k(N) it is

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

Detailed description of the invention three: the present embodiment difference from detailed description of the invention two is: present embodiment is in step Instructing according to the vision measurement data of time delay and the joint of mechanical arm described in two, estimates the end of current spatial robot Speed, its process is:

Utilize the vision measurement data with time delay, available true velocity sequenceValuation sequenceThen can Negotiation speed valuation sequenceTo current vision measurement information D_v(N) compensating, formula (7) can It is rewritten as:

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

Thus obtain the estimated value of current actual distance information

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

Robot for space owing to being under zero gravity state meets the conservation of momentum and the conservation of angular momentum, can set up according to this Mathematical relationship between robot for space joint motions state and end-effector generalized velocity, meets 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_gIt is 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,It is 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{\ω}}_e=\left[\begin{array}{ccc}0& -{\mathrm{sin\α}}_r& {\mathrm{cos\α}}_r{\mathrm{cos\β}}_r\\ 0& {\mathrm{cos\α}}_r& {\mathrm{sin\α}}_r{\mathrm{cos\β}}_r\\ 1& 0& -{\mathrm{sin\β}}_r\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_r\\ {\stackrel{\·}{\mathrm{\β}}}_r\\ {\stackrel{\·}{\mathrm{\γ}}}_r\end{array}\right]---\left(11\right)$

v_eAnd ω_eIt is linear velocity and the angular velocity of robot for space end effector respectively,WithFor space machine The differential of robot end attitude Eulerian angles.

Generalized Jacobian is launched, it is considered to the end generalized velocity of robot for space

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=\underset{i=1}{\overset{n}{\Σ}}{J}_{gi}{\mathrm{\θ}}_i---\left(12\right)$

Wherein J_giIt is the i-th row of broad sense Jacobian matrix.

Because each kinesiology of robot for space, the actual value of kinetic parameter are included in the measurement data of band time delay, fixed It is engraved in the neighborhood of current time N, by J during justice M_gi(M) expand into:

$J_{gi}\left(M\right)=J_{gi}\left(N\right)+\underset{i=1}{\overset{n}{\Σ}}\underset{j=1}{\overset{\mathrm{\∞}}{\Σ}}\frac{1}{j!}\frac{{\∂}^j{J}_{gi}\left(N\right)}{\∂{\mathrm{\θ}}_i^j}{\[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)\]}^j+E_{Ji}^{\′}\left(M\right)---\left(13\right)$

Wherein E'_Ji(M) for the error caused because of carrier position variation.Only retain first order therein, be further deformed into

$\left\{\begin{array}{c}{J}_{gi}\left(M\right)=J_{gi}\left(N\right)+\underset{i=1}{\overset{n}{\Σ}}\frac{\∂J_{gi}\left(N\right)}{\∂{\mathrm{\θ}}_i}\[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)\]+E_{Ji}\left(M\right)\\ E_{Ji}\left(M\right)=\underset{i=1}{\overset{n}{\Σ}}\underset{j=2}{\overset{\mathrm{\∞}}{\Σ}}\frac{1}{j!}\frac{{\∂}^j{J}_{gi}\left(N\right)}{\∂{\mathrm{\θ}}_i^j}{\[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)\]}^j+E_{Ji}^{\′}\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{\ω}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\Σ}}{J}_{gi}\left(M\right){\stackrel{\·}{\mathrm{\θ}}}_i\left(M\right)\\ =J_g\left(N\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)+{\mathrm{\ΔJ}}_N\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)+E_J\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)\end{array}---\left(15\right)$

Wherein

${\mathrm{\ΔJ}}_N\left(M\right)=\left[\begin{array}{ccc}\frac{\∂J_{g1}\left(N\right)}{\∂{\mathrm{\θ}}_1}\[{\mathrm{\θ}}_1\left(M\right)-{\mathrm{\θ}}_1\left(N\right)\]& \mathrm{...}& \frac{\∂J_{gn}\left(N\right)}{\∂{\mathrm{\θ}}_n}\[{\mathrm{\θ}}_n\left(M\right)-{\mathrm{\θ}}_n\left(N\right)\]\end{array}\right]---\left(16\right)$

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

$\stackrel{\·}{\mathrm{\θ}}\left(M\right)=\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_{\mathrm{\θ}}\left(M\right)---\left(17\right)$

Wherein

$E_{\mathrm{\θ}}\left(M\right)=\stackrel{\·}{\mathrm{\θ}}\left(M\right)-\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)---\left(18\right)$

When moving due to robot for space, joint angle velocity variations is slow, so formula (17) Section 1 is that the M moment is empty Between the major part of joint of robot angular velocity, E_θ(M) it is the remainder relevant to acceleration；

Bring formula (17) into formula (15), obtain the robot for space tip speed in M moment and be expressed as:

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\Σ}}{J}_{gi}\left(M\right){\stackrel{\·}{\mathrm{\θ}}}_i\left(M\right)\\ =\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}{J}_g\left(N\right)\stackrel{\·}{\mathrm{\θ}}\left(N\right)+{\mathrm{\ΔJ}}_N\left(M\right)\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V\left(M\right)\\ =\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]+{\mathrm{\ΔJ}}_N\left(M\right)\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\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{\θ}}\left(M\right)+\mathrm{\Δ}J\left(M\right)E_{\mathrm{\θ}}\left(M\right)+E_J\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)---\left(20\right)$

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

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\ΔJ}}_N(N-P)\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P)\end{array}---\left(21\right)$

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

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\ΔJ}}_N(N-P-1)\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\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_NIt is represented by:

$\begin{array}{c}{\mathrm{\ΔJ}}_N(N-P-1)\\ =\left[\begin{array}{ccc}\frac{\∂J_{g1}\left(N\right)}{\∂{\mathrm{\θ}}_1}\[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)\]& \mathrm{...}& \frac{\∂J_{gn}\left(N\right)}{\∂{\mathrm{\θ}}_n}\[{\mathrm{\θ}}_n(N-P-1)-{\mathrm{\θ}}_n\left(N\right)\]\end{array}\right]\\ =\frac{\[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)\]\·\[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\]}{|\[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\]{|}_2^2}\end{array}---\left(23\right)$

Wherein T_ΔJFor remainder.Can obtain current end speed according to above conclusion is

$\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]+\mathrm{\Δ}E---\left(24\right)$

Wherein

$\left\{\begin{array}{c}\mathrm{\β}=\frac{\[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)\]\·\[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\]}{|\[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\]{|}_2^2}\\ {\mathrm{\α}}_1=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\\ {\mathrm{\α}}_2=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\end{array}\right.---\left(25\right)$

For convenience, α can be claimed₁、α₂For second-order linearity estimation coefficient, β is state difference item.Ignore high-order error delta E, this Time tip speed can be with approximate calculation

$\left[\begin{array}{c}{\tilde{v}}_e\left(N\right)\\ {\widetilde{\mathrm{\ω}}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]---\left(26\right)$

Estimation difference now is

$\mathrm{\Δ}E=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}{E}_V(N-P)+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\[T_{\mathrm{\Δ}J}\stackrel{\·}{\mathrm{\θ}}\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{\·}{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\\ =\[\begin{array}{c}{\tilde{v}}_e\left(k\right)\\ {\left[\begin{array}{ccc}0& -{\mathrm{sin\α}}_r& {\mathrm{cos\α}}_r{\mathrm{cos\β}}_r\\ 0& {\mathrm{cos\α}}_r& {\mathrm{sin\α}}_r{\mathrm{cos\β}}_r\\ 1& 0& -{\mathrm{sin\β}}_r\end{array}\right]}^{-1}\end{array}{\widetilde{\mathrm{\ω}}}_e\left(k\right)\]\end{array}---\left(28\right)$

In the case of, robot for space motion less in measurement time delay is relatively stable, error delta E directly carrying out estimating should The least, and estimation difference Δ E is likely to bigger in other cases.For the special circumstances that estimation difference is bigger, design one Error controller, carries out real-time correction to estimated value, thus improves estimated accuracy.

Detailed description of the invention four: the present embodiment difference from detailed description of the invention three is: present embodiment is in step Error controller described in three is designed as: the process of the Velocity Estimation sequence described in step 2 is regarded as surpassing with error Prosomite:

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

WhereinFor the error transfer function of differentiation element, the transmission letter of vision time delay error control system output Number 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{\Δ}}\left(s\right)}{1+G\left(s\right)e^{-{\mathrm{st}}_s}}{V}_r\left(s\right)\end{array}---\left(30\right)$

Wherein V_rS () is the actual value of robot end's speed, Y (s) is robot end's Velocity Estimation, G (s) after correction For error controller.

The estimation difference transmission function of system is:

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

By POLE PLACEMENT USING, design makes error controller G (s) stable for E (s), and estimation difference E (t) is the most gradually received Holding back, after correction, robot end's Velocity Estimation gradually levels off to speed actual value.

Detailed description of the invention five: the present embodiment difference from detailed description of the invention four is: present embodiment is in step Currently vision measurement data with time delay are entered by described according to correction rear space robot end's Velocity Estimation described in four Row compensates, through the vision measurement data of overcompensationIt is calculated as follows:

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

Wherein Y (N-i) is correction rear space robot end's Velocity Estimation that step 4 draws, when so far completing vision Prolong compensation of error.

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

Embodiment

In conjunction with Fig. 1, Fig. 2, Fig. 3, the present embodiment being described, experimental system is made up of robot for space, target satellite, space machine Device people includes sixdegree-of-freedom simulation and carrier spacecraft, and mechanical arm tail end installs camera.Machinery arm straps camera is followed the tracks of close to mesh Mark star, target satellite moves along mechanical arm tail end Z-direction with the speed of 4mm/s.

For setting up analogue system, carrier spacecraft and the kinesiology of sixdegree-of-freedom simulation and kinetic parameter such as table 1 institute Show

Table 1. robot for space parameter

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

Step one, determine according to the Vision information processing method used and the hardware environment applied visual system time Prolong, set up the mathematical relationship between actual distance information and vision measurement information.

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

Step 3, according to formula (30) design vision time delay error controller, via controller export, obtain correct rear space Robot end's Velocity Estimation.

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

Designed robot for space vision time delay error compensation actual effect is as shown in Figure 4, Figure 5.By the compensation backsight of Fig. 4 Estimation data and the raw measurement data and true relative position versus figure that feel is measured, it can be seen that due to depositing of vision time delay , on tri-directions of X, Y and Z, there is bigger error to the measured value of target with actual value in visual system, mends through error Repay, the estimation data closely actual value of vision measurement.By the compensation after-vision measurement data estimation difference of Fig. 5 with do not compensate The comparison diagram of anterior optic measurement data time delay error, it can be seen that compensate anterior optic and measure the maximum error with actual value, at X Direction is reduced to 7mm by 18mm, is reduced to 8mm in Z-direction by 38mm, it can be seen that this delay compensation method of the present invention, can So that Slight measurement errors that Free-floating underchassis space robot due to time delay cause is obviously reduced, be conducive to better ensuring that space Robot completes the requirement of accurate operation task the most accurately, and whole process calculates simple, it is not necessary to set up space The accurate mathematical model of robot, is not required to carry out the dynamics calculation of complexity, can meet engineering actual demand.

Claims (4)

1. a robot for space vision time delay error compensating method based on velocity estimation, it is characterised in that: described method by Following steps complete:

Step one, determine the vision time delay of visual system according to the vision processing algorithm used and the hardware applied, set up Mathematical relationship between vision measurement data with time delay and the true relative pose of physics, process is:

Determine what whole vision measurement link caused according to the Vision information processing method used and the hardware platform applied Time delay is m cycle, if time a length of t of system per cycle_s, then the vision time delay T of Space Robot System_dFor:

T_d=m × t_s (1)

Vision measurement information D in definition space robot k moment_v(k) and actual distance information D_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 any time, and x (k), y (k), z (k) are relative position information, and α (k), β (k), γ (k) are for describing phase para-position The Eulerian angles of appearance, robot for space vision measurement information D_v(k) and actual distance information D_rK the relation between () is:

D_r(k)=D_v(k+m) (4)

K represents that any time, m represent time delay；

If current time is N, vision measurement information is D_v(N), then may know that D_r(N-m) the actual distance information before all can be by The vision measurement data of last time directly obtain, and define 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)$

By D_rAnd D (k+1)_rK () is calculated

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

True velocity information V of robot for space_rAnd V (N-m-1)_r(N-m-1) the true velocity information before is by calculating Arrive: real range information D_k(N) it is

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

Step 2, basis instruct with the vision measurement data of time delay and the joint of mechanical arm, estimate current spatial robot Tip speed；

Step 3, design error controller, reduce the error of robot for space tip speed valuation, obtains correcting rear space machine Robot end Velocity Estimation；

Step 4, according to correction rear space robot end's Velocity Estimation, currently vision measurement data with time delay are carried out Compensate, obtain the vision measurement data through overcompensation.

A kind of robot for space vision time delay error compensating method based on velocity estimation the most according to claim 1, its Being characterised by: in step 2, described instructs according to the vision measurement data of time delay and the joint of mechanical arm, estimates to work as The tip speed of front space robot, its process is:

Utilize the vision measurement data with time delay, obtain true velocity sequenceValuation sequenceThen may be used Negotiation speed valuation sequenceTo current vision measurement information D_v(N) compensating, formula (7) is rewritten as:

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

Thus obtain the estimated value of current actual distance information

By combining with the joint instruction of mechanical arm with the robot for space vision measurement data of time delay, construct current time The valuation of robot for space tip speed；

Robot for space owing to being under zero gravity state meets the conservation of momentum and the conservation of angular momentum, sets up space machine according to this Mathematical relationship between device person joint's kinestate and end-effector generalized velocity, meets 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_gIt is exactly the broad sense Jacobian matrix of robot for space, is inertia matrix I_i, mass parameter m_iAnd machine Person joint angle Θ, carrier spacecraft attitude Ψ₀Function,It is 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{\ω}}_e=\left[\begin{array}{ccc}0& -{\mathrm{sin\α}}_r& {\mathrm{cos\α}}_r{\mathrm{cos\β}}_r\\ 0& {\mathrm{cos\α}}_r& {\mathrm{sin\α}}_r{\mathrm{cos\β}}_r\\ 1& 0& -{\mathrm{sin\β}}_r\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_r\\ {\stackrel{\·}{\mathrm{\β}}}_r\\ {\stackrel{\·}{\mathrm{\γ}}}_r\end{array}\right]---\left(11\right)$

v_eAnd ω_eIt is linear velocity and the angular velocity of robot for space end effector respectively,WithFor robot for space end The differential of attitude Eulerian angles；

Generalized Jacobian is launched, it is considered to the end generalized velocity of robot for space:

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=\underset{i=1}{\overset{n}{\Σ}}{J}_{gi}{\mathrm{\θ}}_i---\left(12\right)$

Wherein J_giIt is the i-th row of broad sense Jacobian matrix, θ_iIt it is i-th joint of robot angle；

Because each kinesiology of robot for space, the actual value of kinetic parameter are included in the measurement data of band time delay, during definition M It is engraved in the neighborhood of current time N, by J_gi(M) expand into:

$J_{gi}\left(M\right)=J_{gi}\left(N\right)+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{\mathrm{\∞}}{\mathrm{\Σ}}}\frac{1}{j!}\frac{{\∂}^j{J}_{gi}\left(N\right)}{\∂{\mathrm{\θ}}_i^j}{\[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)\]}^j+E_{Ji}^{\′}\left(M\right)---\left(13\right)$

Wherein E'_Ji(M) for the error caused because of carrier position variation；Only retain first order therein, be further deformed into

$\left\{\begin{array}{c}{J}_{gi}\left(M\right)=J_{gi}\left(N\right)+\underset{i=1}{\overset{n}{\Σ}}\frac{\∂J_{gi}\left(N\right)}{\∂{\mathrm{\θ}}_i}\[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)\]+E_{Ji}\left(M\right)\\ E_{Ji}\left(M\right)=\underset{i=1}{\overset{n}{\Σ}}\underset{j=2}{\overset{\mathrm{\∞}}{\Σ}}\frac{1}{j/}\frac{{\∂}^j{J}_{gi}\left(N\right)}{\∂{\mathrm{\θ}}_i^j}{\[{\mathrm{\θ}}_i\left(M\right)-{\mathrm{\θ}}_i\left(N\right)\]}^j+E_{Ji}^{\′}\left(M\right)\end{array}\right.---\left(14\right)$

Now the robot for space tip speed in M moment is represented by:

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{J}_{gi}\left(M\right){\stackrel{\·}{\mathrm{\θ}}}_i\left(M\right)\\ =J_g\left(N\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)+{\mathrm{\ΔJ}}_N\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)+E_J\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)\end{array}---\left(15\right)$

Wherein

${\mathrm{\ΔJ}}_N\left(M\right)=\left[\begin{array}{ccc}\frac{\∂J_{g1}\left(N\right)}{\∂{\mathrm{\θ}}_1}\[{\mathrm{\θ}}_1\left(M\right)-{\mathrm{\θ}}_1\left(N\right)\]& ...& \frac{\∂J_{gn}\left(N\right)}{\∂{\mathrm{\θ}}_n}\[{\mathrm{\θ}}_n\left(M\right)-{\mathrm{\θ}}_n\left(N\right)\]\end{array}\right]---\left(16\right)$

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

$\stackrel{\·}{\mathrm{\θ}}\left(M\right)=\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_{\mathrm{\θ}}\left(M\right)---\left(17\right)$

Wherein

$E_{\mathrm{\θ}}\left(M\right)=\stackrel{\·}{\mathrm{\θ}}\left(M\right)-\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)---\left(18\right)$

When moving due to robot for space, joint angle velocity variations is slow, so formula (17) Section 1 is M moment space machine The major part of device person joint's angular velocity, E_θ(M) it is the remainder relevant to acceleration；

Bring formula (17) into formula (15), obtain the robot for space tip speed in M moment and be expressed as:

$\begin{array}{c}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{J}_{gi}\left(M\right){\stackrel{\·}{\mathrm{\θ}}}_i\left(M\right)\\ =\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}{J}_g\left(N\right)\stackrel{\·}{\mathrm{\θ}}\left(N\right)+{\mathrm{\ΔJ}}_N\left(M\right)\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_v\left(M\right)\\ =\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]+{\mathrm{\ΔJ}}_N\left(M\right)\frac{\stackrel{\·}{\mathrm{\θ}}\left(M\right)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\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{\θ}}\left(M\right)+\mathrm{\Δ}J\left(M\right)E_{\mathrm{\θ}}\left(M\right)+E_J\left(M\right)\stackrel{\·}{\mathrm{\θ}}\left(M\right)---\left(20\right)$

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

$\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\left[\begin{array}{c}{v}_e\left(M\right)\\ {\mathrm{\ω}}_e\left(M\right)\end{array}\right]\\ +{\mathrm{\ΔJ}}_N(N-P)\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P)\end{array}---\left(21\right)$

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

$\begin{array}{c}\begin{array}{c}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]\\ +{\mathrm{\ΔJ}}_N(N-P-1)\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P-1)\end{array}---\left(22\right)\\ \end{array}$

And the N-P-1 moment is for the target rough estimate matrix Δ J of n-hour_NIt is represented by:

$\begin{array}{c}{\mathrm{\ΔJ}}_N(N-P-1)\\ =\left[\begin{array}{ccc}\frac{\∂J_{g1}\left(N\right)}{\∂{\mathrm{\θ}}_1}\[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)\]& ...& \frac{\∂J_{gn}\left(N\right)}{\∂{\mathrm{\θ}}_n}\[{\mathrm{\θ}}_n(N-P-1)-{\mathrm{\θ}}_n\left(N\right)\]\end{array}\right]\\ =\frac{\[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)\]\·\[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\]}{|\[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\]{|}_2^2}{\mathrm{\ΔJ}}_N(N-P)+T_{\mathrm{\Δ}J}\end{array}---\left(23\right)$

Wherein T_ΔJFor remainder, according to formula (19), formula (21) and formula (22), can obtain the tip speed of current n-hour is:

$\left[\begin{array}{c}{v}_e\left(N\right)\\ {\mathrm{\ω}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]+\mathrm{\Δ}E---\left(24\right)$

Wherein

$\left\{\begin{array}{c}\mathrm{\β}=\frac{\[{\mathrm{\θ}}_1(N-P-1)-{\mathrm{\θ}}_1\left(N\right)\]\·\[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\]}{|\[{\mathrm{\θ}}_1(N-P)-{\mathrm{\θ}}_1\left(N\right)\]{|}_2^2}\\ {\mathrm{\α}}_1=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\\ {\mathrm{\α}}_2=\frac{\stackrel{\·}{\mathrm{\θ}}(N-P-1)\·\stackrel{\·}{\mathrm{\θ}}\left(N\right)}{|\stackrel{\·}{\mathrm{\θ}}\left(N\right){|}_2^2}\end{array}\right.---\left(25\right)$

α₁、α₂For second-order linearity estimation coefficient, β is state difference item；Ignoring high-order error delta E, now tip speed can approximate meter It is:

$\left[\begin{array}{c}{\tilde{v}}_e\left(N\right)\\ {\widetilde{\mathrm{\ω}}}_e\left(N\right)\end{array}\right]=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P)\\ {\mathrm{\ω}}_e(N-P)\end{array}\right]+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\left[\begin{array}{c}{v}_e(N-P-1)\\ {\mathrm{\ω}}_e(N-P-1)\end{array}\right]---\left(26\right)$

Estimation difference is:

$\mathrm{\Δ}E=\frac{-\mathrm{\β}}{{\mathrm{\α}}_1(1-\mathrm{\β})}{E}_V(N-P)+\frac{1}{{\mathrm{\α}}_2(1-\mathrm{\β})}\[T_{\mathrm{\Δ}J}\stackrel{\·}{\mathrm{\θ}}\left(N\right)+E_V(N-P-1)\]---\left(27\right)$

According to formula (10) and formula (11), estimating speedFor:

$\begin{array}{c}{\tilde{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[\begin{array}{c}{v}_e\\ {\left[\begin{array}{ccc}0& -{\mathrm{sin\α}}_r& {\mathrm{cos\α}}_r{\mathrm{cos\β}}_r\\ 0& {\mathrm{cos\α}}_r& {\mathrm{sin\α}}_r{\mathrm{cos\β}}_r\\ 1& 0& -{\mathrm{sin\β}}_r\end{array}\right]}^{-1}{\mathrm{\ω}}_e\end{array}\right]\end{array}---\left(28\right).$

A kind of robot for space vision time delay error compensating method based on velocity estimation the most according to claim 2, its Being characterised by: in step 3, described error controller is designed as:

The process of the Velocity Estimation sequence described in step 2 is regarded as a differentiation element with error:

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

WhereinFor the error transfer function of differentiation element, the transmission 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{\Δ}}\left(s\right)}{1+G\left(s\right)e^{-{\mathrm{st}}_s}}{V}_r\left(s\right)\end{array}---\left(30\right)$

Wherein V_rS () is the actual value of robot end's speed, Y (s) is robot end's Velocity Estimation after correction, and G (s) is for by mistake Difference controller；

The estimation difference transmission function of system is:

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

By POLE PLACEMENT USING, design makes error controller G (s) stable for E (s), and estimation difference E (t) the most gradually restrains, school After just, robot end's Velocity Estimation gradually levels off to speed actual value.

A kind of robot for space vision time delay error compensating method based on velocity estimation the most according to claim 3, its It is characterised by: in step 4, described according to correction rear space robot end's Velocity Estimation, to currently regarding with time delay Feel that measurement data compensates, through the vision measurement data of overcompensationIt is calculated as follows:

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

Wherein Y (N-i) is correction rear space robot end's Velocity Estimation that step 4 draws, so far completes vision time delay by mistake The compensation of difference.