CN103753530B - Robot of a kind of space rope system surpasses near vision method of servo-controlling - Google Patents

Abstract

The present invention relates to robot of a kind of space rope system and surpass near vision method of servo-controlling, robot of space rope system surpass low coverage approach target time, windsurfing support is full of whole viewing field of camera, position is a little arrested in very difficult measurement, and the straight line that windsurfing bracket edge is formed is easier to detect, the present invention for input, proposes a kind of novel visual servo method with the windsurfing bracket edge straight line detected, robot of effective control space rope system approaches target, and then completes spatial operation task.

Description

Robot of a kind of space rope system surpasses near vision method of servo-controlling

Technical field

The invention belongs to the service field in-orbit of robot of space rope system, be specifically related to robot of a kind of space rope system and surpass near vision method of servo-controlling.

Background technology

Robot of space rope system is a kind of novel Space Robot System, due to features such as it are flexible, safety, cost are low, will be widely used in the task such as inert satellite rescue, space junk cleaning, maintainable technology on-orbit, in-orbit assembling.The general framework of robot of space rope system is " space platform, spatial tether, manipulation robot ", and space platform by spatial tether releasing operation robot, manipulation robot's approximate spatial target, to be arrested; Manipulation robot carries operating robotic arm and end effector hand for performing target acquisition and the task such as service in-orbit.The approximate procedure of robot of space rope system is a kind of typical location-based Visual servoing control pattern.The relative pose of robot/target is measured the main vision that relies on and is realized, after acquisition relative pose metrical information, manipulation robot, under control system effect, completes approaching and catching Action Target, and catch position is typically chosen in the solar energy sailboard support of target satellite.

The operand of robot of space rope system is general comparatively large, and its solar energy sailboard stent length can reach several meters.And manipulation robot is less, it is very short that it measures camera parallax range, and when super close distance approaches, the overlapping region of binocular vision is less, cannot provide suitable pose data, and location-based Visual servoing control pattern cannot use.In addition, target acquistion point is generally in windsurfing mid-stent, and when super low coverage is approached, windsurfing support is full of whole visual field, there is not traditional angle point, brings very large difficulty to the measurement of target.Therefore, robot of space rope system surpass low coverage approach target time, study a kind of novel Visual servoing control method pole and be necessary.

Summary of the invention

The technical problem solved

In order to avoid the deficiencies in the prior art part, the present invention proposes robot of a kind of space rope system and surpasses near vision method of servo-controlling,

Technical scheme

Robot of a kind of space rope system surpasses near vision method of servo-controlling, it is characterized in that space rope system robot vision measuring system is one camera, is installed on manipulation robot front end, windsurfing support be single rod type without thickness thin plate, and be full of viewing field of camera, step is as follows:

Step 1, robot of derivation space rope system surpass the measurement equation that low coverage is approached

$\left\{\begin{array}{c}{k}_1=\frac{x_0\cos \mathrm{\β}\sin \mathrm{\γ}+y_0\sin \mathrm{\β}+L(\sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}\sin \mathrm{\β})}{z_0\sin \mathrm{\β}+x_0\cos \mathrm{\β}\cos \mathrm{\γ}+L(\cos \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}\\ k_2=\frac{x_0\cos \mathrm{\β}\sin \mathrm{\γ}+y_0\sin \mathrm{\β}\βL(\sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}\sin \mathrm{\β})}{z_0\sin \mathrm{\β}+x_0\cos \mathrm{\β}\cos \mathrm{\γL}(\cos \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}\\ b_1=\frac{-r\cos \mathrm{\β}(y_0\cos \mathrm{\γ}+L\cos \mathrm{\α}-z_0\sin \mathrm{\γ})}{x_0\cos \mathrm{\γ}\cos \mathrm{\β}+z_0\sin \mathrm{\β}+L(\sin \mathrm{\α}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}\\ b{2}_{}=\frac{{-}_{}r\cos \mathrm{\β}(y_0\cos \mathrm{\γ}+L\cos \mathrm{\α}\αz_0\sin }{x_0\cos \mathrm{\γ\β}\cos +z_0\sin \mathrm{\β}+L(\sin \mathrm{\α}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}\end{array}\right.$

$\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]=\left[\begin{array}{c}-x\cos \mathrm{\α}\cos \mathrm{\β}-y\cos \mathrm{\β}\sin \mathrm{\α}+z\sin \mathrm{\β}\\ x(\cos \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})-y(\cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})-z\cos \mathrm{\β}\sin \mathrm{\γ}\\ -x(\sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}\sin \mathrm{\β})+y(\cos \mathrm{\α}\sin \mathrm{\γ}-\cos \mathrm{\γ}\sin \mathrm{\α}\sin \mathrm{\β})-z\cos \mathrm{\β}\cos \mathrm{\γ}\end{array}\right]---\left(9\right)$

K ₁for the slope of windsurfing bracket edge line I straight line of image formation, b ₁for windsurfing bracket edge line I straight line of image formation and the intersecting point coordinate of picture plane y-axis, k ₂for the slope of windsurfing bracket edge line II straight line of image formation, b ₂for windsurfing bracket edge line II straight line of image formation and the intersecting point coordinate of picture plane y-axis, r is camera focus, and L is the half of windsurfing support width, (x ₀, y ₀, z ₀) for arresting point coordinates system initial point in the position of camera coordinates system, (γ, β, α) is tied to for arresting point coordinates the anglec of rotation that camera coordinates is 321 posture changings, and (x, y, z) is for manipulation robot is in the three-dimensional position of target track system;

Step 2, analysis operation robot controlling demand, the state equation that super low coverage of deriving is approached:

The demand for control of the manipulation robot of robot of space rope system:

$[{\mathrm{\ω}}_x,{\mathrm{\ω}}_y,{\mathrm{\ω}}_z,\mathrm{\β},\mathrm{\γ}]=\left[\mathrm{0,0,0,0,0}\right],y=0,\stackrel{.}{y}=0$

ω _x, ω _y, ω _zfor the three-dimensional relative angle speed of manipulation robot and target;

Robot of space rope system surpasses the state equation that low coverage is approached:

$\left\{\begin{array}{c}\left[\begin{array}{c}{\stackrel{.}{\mathrm{\ω}}}_x\\ {\stackrel{.}{\mathrm{\ω}}}_y\\ {\stackrel{.}{\mathrm{\ω}}}_z\end{array}\right]=-{\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]}^{-1}\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]+{\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]}^{-1}\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]\\ \stackrel{.}{\mathrm{\β}}={\mathrm{\ω}}_y\cos \mathrm{\γ}-{\mathrm{\ω}}_z\sin \mathrm{\γ}\\ \stackrel{.}{\mathrm{\γ}}={\mathrm{\ω}}_x+({\mathrm{\ω}}_z\cos \mathrm{\γ}+{\mathrm{\ω}}_y\sin \mathrm{\γ})\tan \mathrm{\β}\\ \stackrel{..}{y}+n^{2}y=\frac{F_x\cos \mathrm{\β}\sin \mathrm{\α}+F_y(\cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α})+F_z(-\sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α})}{m}\end{array}\right.$

$\left\{\begin{array}{c}\\ \stackrel{.}{\mathrm{\α}}=({\mathrm{\ω}}_z\cos \mathrm{\γ}+{\mathrm{\ω}}_y\sin \mathrm{\γ})\sec \mathrm{\β}\\ \stackrel{..}{x}-2n\stackrel{.}{z}=\frac{F_x\cos \mathrm{\β}\cos \mathrm{\α}+F_y(-\cos \mathrm{\γ}\sin \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α})+F_z(\sin \mathrm{\γ}\sin \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α})}{m}\\ \stackrel{..}{z}+2n\stackrel{.}{x}-3n^{2}z=\frac{-F_x\sin \mathrm{\β}+F_y\sin \mathrm{\γ}\cos \mathrm{\β}+F_z\cos \mathrm{\γ}\cos \mathrm{\β}}{m}\end{array}\right.$

$\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]$ For manipulation robot's moment of momentum, m is the quality of manipulation robot, and n is target track angular speed, F _x, F _y, F _zand M _x, M _y, M _zbe respectively three-dimensional control and the control moment of manipulation robot;

Step 3: design space Sheng Xi robot surpass low coverage approach controller as shown in the formula, implementation space Sheng Xi robot surpasses near vision method of servo-controlling:

$\left\{\begin{array}{c}{M}_x=-p_1{k}_1-p_2{\mathrm{\ω}}_x\\ M_y=-p_3(k_1-k_2)-p_4{\mathrm{\ω}}_y\\ M_z=-p_5{\mathrm{\ω}}_z\\ F_y=-p_6(b_1+b_2)-p_7({\stackrel{.}{b}}_1+{\stackrel{.}{b}}_2)\end{array}\right.\left\{\begin{array}{ccc}{p}_1>0& p_2>0& p_3>0\\ p_4>0& p_5>0& p_6>0\\ p_7>0& & \end{array}\right.$

P ₁, p ₂, p ₃, p ₄, p ₅, p ₆, p ₇for the controller parameter of design.

Beneficial effect

The robot of a kind of space rope system that the present invention proposes surpasses near vision method of servo-controlling, robot of space rope system surpass low coverage approach target time, windsurfing support is full of whole viewing field of camera, position is a little arrested in very difficult measurement, and the straight line that windsurfing bracket edge is formed is easier to detect, the present invention for input, proposes a kind of novel visual servo method with the windsurfing bracket edge straight line detected, robot of effective control space rope system approaches target, and then completes spatial operation task.

Accompanying drawing explanation

Fig. 1 is that robot of space rope system surpasses low coverage and approaches schematic diagram: 1 is target satellite main body, and 2 is target satellite solar energy sailboard support, and 3 is target satellite solar energy sailboard, and 4 is manipulation robot, and 5 is spatial tether, and 6 is space platform;

Fig. 2 is the principle schematic of this method: 7 is an edge line of windsurfing support, and 8 is another edge line of windsurfing support, and 9 is camera lens

Detailed description of the invention

Now in conjunction with the embodiments, the invention will be further described for accompanying drawing:

The embodiment of the present invention is achieved through the following technical solutions.

When super low coverage is approached, suppose:

1. windsurfing support is single rod type, and support is thin plate, and thickness can be ignored;

2. space rope system robot vision measuring system is one camera, is installed on manipulation robot front end;

3. windsurfing support endless, is full of whole viewing field of camera, and tries to achieve the position of bracket edge straight line by image processing algorithm.

Fig. 1 is that robot of space rope system surpasses low coverage and approaches schematic diagram: manipulation robot 4, spatial tether 5 and space platform 6 constitute robot of space rope system jointly.Approach in visual servo in super low coverage, the coordinate used is target track system (O _tx _ty _tz _t), manipulation robot's body series (O _gx _gy _gz _g), arrest point coordinates system O _zx _zy _zz _z, camera coordinates system O _cx _cy _cz _c, CCD coordinate system O _dy _dz _d.As shown in Figure 1 and Figure 2.In Fig. 2,7 is an edge line of windsurfing support, and 8 is another edge line of windsurfing support, and 9 is camera lens.Arrest the O of point coordinates system _zy _zz _zface is windsurfing chassis plane, and CCD coordinate is the plane coordinate system after imaging.For modeling is convenient, hypothetical target track system O _tx _ty _tz _twith arrest point coordinates system O _zx _zy _zz _zoverlap, camera coordinates system O _cx _cy _cz _cwith manipulation robot's body series (O _gx _gy _gz _g) overlap.

One: the imaging model of derivation space line under camera system

First the camera imaging model of spatial point is set up.Camera model comprises national forest park in Xiaokeng, intrinsic parameter model, outer parameter model three part.National forest park in Xiaokeng is the image-forming principle model of camera, utilizes perspective projection principle, derivation spatial point (x _i, y _i, z _i) national forest park in Xiaokeng be:

${\left[\begin{array}{ccc}{x}_i^{\′}& y_i^{\′}& z_i^{\′}\end{array}\right]}^T={\left[\begin{array}{ccc}-r& -r\frac{y_i}{x_i}& -r\frac{z_i}{x_i}\end{array}\right]}^T---\left(1\right)$

Wherein, r is focal length, [x _i', y _i', z _i'] for the picture of spatial point is at the coordinate of imaging plane, [] ^trepresenting matrix turns order.

The internal reference model of camera characterizes and imaging plane is mapped to the plane of delineation, and ignore lens distortion, camera internal reference model is:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{a}_x& & u_0\\ & a_y& v_0\\ & & 1\end{array}\right]\left[\begin{array}{c}{y}_i^{\′}\\ z_i^{\′}\\ 1\\ \end{array}\right]---\left(2\right)$

A _x, a _yfor imaging plane is to the amplification coefficient of the plane of delineation, (u ₀, v ₀) be the image coordinate of the intersection point of optical axis center line and imaging plane.For deriving conveniently, in this article, suppose a _x=a _y=1, u ₀=v ₀=0.Camera internal reference matrix is unit battle array.

The outer moduli type of camera characterizes the coordinate transform of camera coordinates system and reference frame.If reference coordinate is manipulation robot's body series O _gx _gy _gz _g.If impact point coordinate under camera system is: (x _i, y _i, z _i), then, target coordinate under reference frame is:

[x _Giy _Giz _Gi] ^T＝[x _iy _iz _i] ^T(3)

On the basis setting up spatial point camera imaging model, set up space line imaging model.Ignore camera distortion, the imaging of space line is still straight line.Two characteristic point (x are selected from space line ₁, y ₁, z ₁) and (x ₂, y ₂, z ₂), utilize camera imaging model to calculate subpoint, and then the slope of straight line of image formation can be calculated.

$k=\frac{-y_2{x}_1+y_1{x}_2}{-z_2{x}_1+z_1{x}_2}---\left(4\right)$

If this space line and camera system O _cx _cy _cintersection point is (x ₃, y ₃, 0), then its straight line of image formation and O _dy _dthe intersection point of axle is:

$b=-r\frac{y_3}{x_3}---\left(5\right)$

Two: select virtual feature point on windsurfing bracket edge line, and under being transformed into camera coordinates system

First set and arrest point coordinates system initial point under camera coordinates system as (x ₀, y ₀, z ₀), arrest point coordinates and be tied to camera coordinates system attitude rotation mode and be chosen as 321, the anglec of rotation is (γ, β, α), then arrest the coordinate conversion matrix that point coordinates is tied to camera coordinates system to be:

$T_{\mathrm{TC}}=\left[\begin{array}{cccc}\cos \mathrm{\β}\cos \mathrm{\α}& \cos \mathrm{\β}\sin \mathrm{\α}& -\sin \mathrm{\β}& x_0\\ -\cos \mathrm{\γ}\sin \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}& \cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \sin \mathrm{\γ}\cos \mathrm{\β}& y_0\\ \sin \mathrm{\γ}\sin \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}& -\sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\γ}\cos \mathrm{\β}& z_0\\ 0& 0& 0& 1\end{array}\right]---\left(6\right)$

Then, the characteristic point that each selection two is virtual from windsurfing support two edge lines, and under being transformed into camera coordinates system.If windsurfing support width is 2L, a is non-vanishing arbitrary value.Selected characteristic point and the coordinate under camera system as shown in the table.

Edge line I and camera system O _cx _cy _cplane point of intersection is: $\left[\begin{array}{c}{x}_0+\frac{z_0\sin \mathrm{\β}+L(\sin \mathrm{\α}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}{\cos \mathrm{\β}\cos \mathrm{\γ}}\\ y_0+\frac{L\cos \mathrm{\α}-z_0\sin \mathrm{\γ}}{\cos \mathrm{\γ}}\\ 0\\ \end{array}\right].$

Edge line II and camera system O _cx _cy _cplane point of intersection is: $\left[\begin{array}{c}{x}_0+\frac{z_0\sin \mathrm{\β}+L(\sin \mathrm{\α}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}{\cos \mathrm{\β}\cos \mathrm{\γ}}\\ y_0+\frac{L\cos \mathrm{\α}+z_0\sin \mathrm{\γ}}{\cos \mathrm{\γ}}\\ 0\\ \end{array}\right].$

Three: derivation windsurfing bracket edge line imaging formula, set up the observation model that super close distance approaches

Utilize the imaging model of space line imaging model derivation edge line I:

$\left\{\begin{array}{c}{k}_1=\frac{x_0\cos \mathrm{\β}\sin \mathrm{\γ}+y_0\sin \mathrm{\β}+L(\sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}\sin \mathrm{\β})}{z_0\sin \mathrm{\β}+x_0\cos \mathrm{\β}\cos \mathrm{\γ}+L(\cos \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}\\ b_1=-r\frac{y_0+\frac{L\cos \mathrm{\α}-z_0\sin \mathrm{\γ}}{\cos \mathrm{\γ}}}{x_0+\frac{z_0\sin \mathrm{\β}+L(\sin \mathrm{\α}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}{\cos \mathrm{\β}\cos \mathrm{\γ}}}\end{array}\right.---\left(7\right)$

The imaging model of edge line II:

$\left\{\begin{array}{c}{k}_2=\frac{x_0\cos \mathrm{\β}\sin \mathrm{\γ}+y_0\sin \mathrm{\β}-L(\sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}\sin \mathrm{\β})}{z_0\sin \mathrm{\β}+x_0\cos \mathrm{\β}\cos \mathrm{\γ}-L(\cos \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}\\ b_2=-r\frac{y_0-\frac{L\cos \mathrm{\α}+z_0\sin \mathrm{\γ}}{\cos \mathrm{\γ}}}{x_0+\frac{z_0\sin \mathrm{\β}-L(\sin \mathrm{\α}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})}{\cos \mathrm{\β}\cos \mathrm{\γ}}}\end{array}\right.---\left(8\right)$

The imaging model of edge line set up is the observational equation that robot of space rope system approaches based on the super low coverage of line tracking.

Under target track system, if the relative position of manipulation robot and target is [xyz], then,

$\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]=\left[\begin{array}{c}-x\cos \mathrm{\α}\cos \mathrm{\β}-y\cos \mathrm{\β}\sin \mathrm{\α}+z\sin \mathrm{\β}\\ x(\cos \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})-y(\cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ})-z\cos \mathrm{\β}\sin \mathrm{\γ}\\ -x(\sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}\sin \mathrm{\β})+y(\cos \mathrm{\α}\sin \mathrm{\γ}-\cos \mathrm{\γ}\sin \mathrm{\α}\sin \mathrm{\β})-z\cos \mathrm{\β}\cos \mathrm{\γ}\end{array}\right]---\left(9\right)$

Four, the demand for control of the manipulation robot of analysis space Sheng Xi robot, sets up robot and surpasses the state equation that low coverage approaches

For deriving conveniently, if manipulation robot's moment of momentum is I=diag (I _x, I _y, I _z).Hypothetical target does not carry out attitude maneuver, and ignore the impact of spatial tether and other interference, the manipulation robot of robot of space rope system and the relative attitude kinetics equation of target are:

$\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]\left[\begin{array}{c}{\stackrel{.}{\mathrm{\ω}}}_x\\ {\stackrel{.}{\mathrm{\ω}}}_y\\ {\stackrel{.}{\mathrm{\ω}}}_z\end{array}\right]+\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]=\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]---\left(10\right)$

Wherein, [ω _xω _yω _z] be the relative angle speed of manipulation robot and target, M _x, M _y, M _zfor control moment.

The manipulation robot of robot of space rope system and the relative attitude kinematical equation of target are (321 rotate):

$\left\{\begin{array}{c}\stackrel{.}{\mathrm{\α}}=({\mathrm{\ω}}_z\cos \mathrm{\γ}+{\mathrm{\ω}}_y\sin \mathrm{\γ})\sec \mathrm{\β}\\ \stackrel{.}{\mathrm{\β}}={\mathrm{\ω}}_y\cos \mathrm{\γ}-{\mathrm{\ω}}_z\sin \mathrm{\γ}\\ \stackrel{.}{\mathrm{\γ}}={\mathrm{\ω}}_x+({\mathrm{\ω}}_z\cos \mathrm{\γ}+{\mathrm{\ω}}_y\sin \mathrm{\γ})\tan \mathrm{\β}\end{array}\right.---\left(11\right)$

Simple for deriving, hypothetical target star (track) coordinate system with arrest point coordinates system and overlap, hypothetical target circle of position track, only by centrifugal force, manipulation robot and target range are relatively little, ignore tether impact, under target track system, the relative orbit control kinetic model of manipulation robot and target is:

$\left\{\begin{array}{c}\stackrel{..}{x}-2n\stackrel{.}{z}=a_x\\ \stackrel{..}{y}+n^{2}y=a_y\\ \stackrel{..}{z}+2n\stackrel{.}{x}-3n^{2}z=a_z\end{array}\right.---\left(12\right)$

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[A_{\mathrm{GT}}\right]\left[\begin{array}{c}{F}_x/m\\ F_y/m\\ F_z/m\end{array}\right]---\left(13\right)$

[a _xa _ya _z] ^tfor the component of acceleration under target track system that manipulation robot's control produces, [F _xf _yf _z] ^tfor the control of manipulation robot under body series, n is target track angular speed.A _gTfor manipulation robot's body is tied to the attitude spin matrix of target track system.

$A_{\mathrm{GT}}=A_{\mathrm{CT}}=A_{\mathrm{TC}}^T---\left(14\right)$

First relative attitude demand is analyzed below: manipulation robot's trapping manipulator of robot of space rope system generally possesses the ability of encircling, and windsurfing support firmly can be embraced.Suppose that catching hand is positioned at manipulation robot front end, operation coordinate system overlaps with camera coordinates system, and catching hand operating surface is X _co _cy _cplane, the attitude demand that operating side and target arrest point coordinates system is:

[ω _x,ω _y,ω _z,β,γ]＝[0,0,0,0,0] (15)

Then relative position demand is analyzed.Suppose that x is that direction is approached in operating side, when super low coverage is approached, approach direction and carry out programme-control; Due to windsurfing endless, z is to not controlling; Y to demand for control is:

$\left\{\begin{array}{c}y=0\\ \stackrel{.}{y}=0\end{array}\right.---\left(16\right)$

According to demand for control, the control channel of the super low coverage approximate model of deriving is controlled item and is separated with program control item, nothing, simultaneous Attitude control model and position control model:

$\left\{\begin{array}{c}\left[\begin{array}{c}{\stackrel{.}{\mathrm{\ω}}}_x\\ {\stackrel{.}{\mathrm{\ω}}}_y\\ {\stackrel{.}{\mathrm{\ω}}}_z\end{array}\right]=-{\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]}^{-1}\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]+{\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]}^{-1}\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]\\ \stackrel{.}{\mathrm{\β}}={\mathrm{\ω}}_y\cos \mathrm{\γ}-{\mathrm{\ω}}_z\sin \mathrm{\γ}\\ \stackrel{.}{\mathrm{\γ}}={\mathrm{\ω}}_x+({\mathrm{\ω}}_z\cos \mathrm{\γ}+{\mathrm{\ω}}_y\sin \mathrm{\γ})\tan \mathrm{\β}\\ \stackrel{..}{y}+n^{2}y=\frac{F_x\cos \mathrm{\β}\sin \mathrm{\α}+F_y(\cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α})+F_z(-\sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α})}{m}\end{array}\right.---\left(17\right)$

$\left\{\begin{array}{c}\\ \stackrel{.}{\mathrm{\α}}=({\mathrm{\ω}}_z\cos \mathrm{\γ}+{\mathrm{\ω}}_y\sin \mathrm{\γ})\sec \mathrm{\β}\\ \stackrel{..}{x}-2n\stackrel{.}{z}=\frac{F_x\cos \mathrm{\β}\cos \mathrm{\α}+F_y(-\cos \mathrm{\γ}\sin \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α})+F_z(\sin \mathrm{\γ}\sin \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α})}{m}\\ \stackrel{..}{z}+2n\stackrel{.}{x}-3n^{2}z=\frac{-F_x\sin \mathrm{\β}+F_y\sin \mathrm{\γ}\cos \mathrm{\β}+F_z\cos \mathrm{\γ}\cos \mathrm{\β}}{m}\end{array}\right.---\left(18\right)$

Five: robot of space rope system surpasses low coverage and approaches Controller gain variations

According to Controlling model above and control objectives, Controller gain variations is:

$\left\{\begin{array}{c}{M}_x=-p_1{k}_1-p_2{\mathrm{\ω}}_x\\ M_y=-p_3(k_1-k_2)-p_4{\mathrm{\ω}}_y\\ M_z=-p_5{\mathrm{\ω}}_z\\ F_y=-p_6(b_1+b_2)-p_7({\stackrel{.}{b}}_1+{\stackrel{.}{b}}_2)\end{array}\right.---\left(19\right)$

Wherein k ₁, k ₁, b ₁, b ₂measure by camera and obtain, respectively by b ₁, b ₂difference approximation obtains.Hypothetical target attitude without motor-driven, ω _x, ω _y, ω _zcan be obtained by operating side inertia assembly.P ₁p ₇for controller parameter.

Below from the selection of the angle analysis controling parameters of stability.

Bring controller into model, abbreviation, and write as traditional non-linear form:

X＝f(X,t) (20)

Wherein $X=\left[\begin{array}{ccccccc}{\mathrm{\ω}}_x& {\mathrm{\ω}}_y& {\mathrm{\ω}}_z& \mathrm{\β}& \mathrm{\γ}& y& \stackrel{.}{y}\end{array}\right],$ Utilize Liapunov first method to carry out stability to prove.By model at equalization point X _elaunch:

$\stackrel{.}{X}=\frac{\∂f}{\∂X}(X-X_e)+R\left(X\right)---\left(21\right)$

Surpass in low coverage approximate procedure in robot of space rope system, equalization point is X _e=[0 00000 0].

$\begin{array}{c}\frac{\∂f}{\∂X}{|}_{x=x_e}=\\ \left[\begin{array}{ccccccc}-\frac{p_2}{I_x}& 0& 0& -\frac{p_1\left(L\cos \mathrm{\α}\right)}{I_x(L\sin \mathrm{\α}-x\cos \mathrm{\α})}& -\frac{p_1}{I_x}& 0& 0\\ 0& -\frac{p_4}{I_y}& 0& X_{24}& 0& 0& 0\\ 0& 0& -\frac{p_5}{I_z}& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& \frac{F_{z\_G}\sin \mathrm{\α}}{m}& \frac{-F_{z\_G}\cos \mathrm{\α}}{m}& X_{76}& X_{77}\end{array}\right]\end{array}---\left(22\right)$

Wherein,

$X_{24}=\frac{2L{p}_{3}x}{I_y(x^2{\cos }^2\mathrm{\α}-L^2{\sin }^2\mathrm{\α})}$

$X_{76}=-n^2-2r\cos \mathrm{\α}(\frac{-p_{6}x(L^2{\sin }^2\mathrm{\α}+x^2{\cos }^2\mathrm{\α})}{m{(L^2{\sin }^2\mathrm{\α}-x^2{\cos }^2\mathrm{\α})}^2}+\frac{-p_7\stackrel{.}{x(L^4{\sin }^4\mathrm{\α}+6L^2{x}^2{\cos }^2\mathrm{\α}{\sin }^2\mathrm{\α}+x^4{\cos }^4\mathrm{\α})}}{{(L^2{\sin }^2\mathrm{\α}-x^2{\cos }^2\mathrm{\α})}^3})---\left(23\right)$

$X_{77}=\frac{2p_7\mathrm{rx}\cos \mathrm{\α}(L^2{\sin }^2\mathrm{\α}+x^2{\cos }^2\mathrm{\α})}{{m(x^2{\cos }^2\mathrm{\α}-L^2{\sin }^2\mathrm{\α})}^2}$

Obviously, X ₂₄, X ₇₆, X ₇₇for independent variable.The characteristic value solving Jacobin matrix is:

$\left[\begin{array}{cccc}\frac{-p_2\±\sqrt{p_2^2-4I_x{p}_1}}{2I_x}& \frac{-p_4\±\sqrt{p_4^2+4X_{24}{I}_y^2}}{2I_y}& -\frac{p_5}{I_z}& \frac{X_{77}\±\sqrt{X_{77}^2+4X_{76}}}{2}\end{array}\right]---\left(24\right)$

If each characteristic value all has negative real part, then system is at poised state X _ebe asymptotically stable, and stability and R (X) have nothing to do.Therefore, can discuss as follows:

(1) if $\left\{\begin{array}{c}{p}_2>0\\ p_4>0\\ p_5>0\end{array}\right.,$ Characteristic value there is negative real part.

(2) if $\left\{\begin{array}{c}{p}_1>0\\ p_2>0\end{array}\right.$ Time, characteristic value there is negative real part

(3) if $\left\{\begin{array}{c}{p}_4>0\\ X_{24}<0\end{array}\right.,$ Characteristic value there is negative real part.

(4) if X ₇₇< 0, characteristic value there is negative real part.

(5) if $\left\{\begin{array}{c}{X}_{77}<0\\ X_{76}<0\end{array}\right.,$ Characteristic value there is negative real part.

In sum, if meet the following conditions, each characteristic value of system all has negative real part, then system is at poised state X _ebe asymptotically stable, and stability and R (X) have nothing to do.

$\left\{\begin{array}{c}{p}_1>0\\ p_2>0\\ p_4>0\\ p_5>0\end{array}\right.---\left(25\right)$

$\left\{\begin{array}{c}{X}_{24}<0\\ X_{76}<0\\ X_{77}<0\end{array}\right.---\left(26\right)$

The condition meeting formula (26) is discussed below.For deriving conveniently, in robot of space rope system approximate procedure, suppose as follows.

(1) manipulation robot approaches target from rear, that is: x < 0,

(2) when manipulation robot has approached, manipulation robot's front panel and windsurfing support collisionless.That is: | x| > L

(3) for guarantee two edge line imagings are separated, attitude angle α is less, supposes

Analyze one by one below

(1) if p ₇> 0, then $X_{77}=\frac{2p_7\mathrm{rx}\cos \mathrm{\&alpha;}(L^2{\sin }^2\mathrm{\&alpha;}+x^2{\cos }^2\mathrm{\&alpha;})}{{m(x^2{\cos }^2\mathrm{\&alpha;}-L^2{\sin }^2\mathrm{\&alpha;})}^2}<0$

(2) if $\frac{p_3}{x^2{\cos }^2\mathrm{\&alpha;}-L^2{\sin }^2\mathrm{\&alpha;}}>0,$ Then $X_{24}=\frac{2L{p}_{3}x}{I_y(x^2{\cos }^2\mathrm{\&alpha;}-L^2{\sin }^2\mathrm{\&alpha;})}<0$

(3) if p ₆> 0, then if $\frac{{-n}^2}{2r\cos \mathrm{\&alpha;}}+\frac{p_7\stackrel{.}{x}(L^4{\sin }^4\mathrm{\&alpha;}+6L^2{x}^2{\cos }^2\mathrm{\&alpha;}{\sin }^2\mathrm{\&alpha;}+x^4{\cos }^4\mathrm{\&alpha;})}{{(L^2{\sin }^2\mathrm{\&alpha;}-x^2{\cos }^2\mathrm{\&alpha;})}^3}<0;$ Then

$X_{76}=-n^2+2r\cos \mathrm{\&alpha;}(\frac{p_{6}x(L^2{\sin }^2\mathrm{\&alpha;}+x^2{\cos }^2\mathrm{\&alpha;})}{m{(L^2{\sin }^2\mathrm{\&alpha;}-x^2{\cos }^2\mathrm{\&alpha;})}^2}+\frac{p_7\stackrel{.}{x(L^4{\sin }^4\mathrm{\&alpha;}+6L^2{x}^2{\cos }^2\mathrm{\&alpha;}{\sin }^2\mathrm{\&alpha;}+x^4{\cos }^4\mathrm{\&alpha;})}}{{(L^2{\sin }^2\mathrm{\&alpha;}-x^2{\cos }^2\mathrm{\&alpha;})}^3})<0$

In sum, if meet the following conditions, then, under the controller action of design, system is at poised state X _easymptotically stable.

$\left\{\begin{array}{c}{p}_1>0\\ p_2>0\\ p_3>0\\ p_4>0\\ p_5>0\\ p_6>0\\ p_7>0\end{array}\right.---\left(27\right)$

Method of the present invention is found out by embodiment, robot of space rope system surpass low coverage approach target time, windsurfing support is full of whole viewing field of camera, position is a little arrested in very difficult measurement, and the straight line that windsurfing bracket edge is formed is easier to detect, the present invention for input, proposes a kind of novel visual servo method with the windsurfing bracket edge straight line detected, robot of effective control space rope system approaches target, and then completes spatial operation task.

Claims (1)

1. a robot of space rope system surpasses near vision method of servo-controlling, it is characterized in that space rope system robot vision measuring system is one camera, be installed on manipulation robot front end, windsurfing support is that single rod type is without thickness thin plate, and be full of viewing field of camera, step is as follows:

Step 1, robot of derivation space rope system surpass the measurement equation that low coverage is approached

$\left\{\begin{array}{c}{k}_1=\frac{x_0\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}+y_0\sin \mathrm{\&beta;}+L(\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}\sin \mathrm{\&beta;})}{z_0\sin \mathrm{\&beta;}+x_0\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}+L(\cos \mathrm{\&gamma;}\sin \mathrm{\&alpha;}-\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;})}\\ k_2=\frac{x_0\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}+y_0\sin \mathrm{\&beta;}-L(\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}\sin \mathrm{\&beta;})}{z_0\sin \mathrm{\&beta;}+x_0\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}-L(\cos \mathrm{\&gamma;}\sin \mathrm{\&alpha;}-\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;})}\\ b_1=\frac{-r\cos \mathrm{\&beta;}(y_0\cos \mathrm{\&gamma;}+L\cos \mathrm{\&alpha;}-z_0\sin \mathrm{\&gamma;})}{x_0\cos \mathrm{\&gamma;}\cos \mathrm{\&beta;}+z_0\sin \mathrm{\&beta;}+L(\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}-\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;})}\\ b_2=\frac{-r\cos \mathrm{\&beta;}(y_0\cos \mathrm{\&gamma;}-L\cos \mathrm{\&alpha;}-z_0\sin \mathrm{\&gamma;})}{x_0\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}+z_0\sin \mathrm{\&beta;}+L(\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}-\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;})}\end{array}\right.$

$\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]\left[\begin{array}{c}-x\cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}-y\cos \mathrm{\&beta;}\sin \mathrm{\&alpha;}+z\sin \mathrm{\&beta;}\\ x(\cos \mathrm{\&gamma;}\sin \mathrm{\&alpha;}-\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;})-y(\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;})-z\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}\\ -x(\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}\sin \mathrm{\&beta;})+y(\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;})-z\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}\end{array}\right]$

K ₁for the slope of windsurfing bracket edge line I straight line of image formation, b ₁for windsurfing bracket edge line I straight line of image formation and the intersecting point coordinate of picture plane y-axis, k ₂for the slope of windsurfing bracket edge line II straight line of image formation, b ₂for windsurfing bracket edge line II straight line of image formation and the intersecting point coordinate of picture plane y-axis, r is camera focus, and L is the half of windsurfing support width, (x ₀, y ₀, z ₀) for arresting point coordinates system initial point in the position of camera coordinates system, (γ, β, α) is tied to for arresting point coordinates the anglec of rotation that camera coordinates is 321 posture changings, and (x, y, z) is for manipulation robot is in the three-dimensional position of target track system;

Step 2, analysis operation robot controlling demand, the state equation that super low coverage of deriving is approached:

The demand for control of the manipulation robot of robot of space rope system:

[ω _x,ω _y,ω _z,β,γ]＝[0,0,0,0,0]，y＝0，

ω _x, ω _y, ω _zfor the three-dimensional relative angle speed of manipulation robot and target;

Robot of space rope system surpasses the state equation that low coverage is approached:

$\left\{\begin{array}{c}\left[\begin{array}{c}{\stackrel{.}{\mathrm{\&omega;}}}_x\\ {\stackrel{.}{\mathrm{\&omega;}}}_y\\ {\stackrel{.}{\mathrm{\&omega;}}}_z\end{array}\right]=-{\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]}^{-1}\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z& 0& -{\mathrm{\&omega;}}_x\\ -{\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_x& 0\end{array}\right]\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]\left[\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right]+{\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]}^{-1}\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]\\ =\stackrel{.}{\mathrm{\&beta;}}={\mathrm{\&omega;}}_y\cos \mathrm{\&gamma;}-{\mathrm{\&omega;}}_z\sin \mathrm{\&gamma;}\\ \stackrel{.}{\mathrm{\&gamma;}}={\mathrm{\&omega;}}_x+({\mathrm{\&omega;}}_z\cos \mathrm{\&gamma;}+{\mathrm{\&omega;}}_y\sin \mathrm{\&gamma;})\tan \mathrm{\&beta;}\\ \stackrel{..}{y}+n^{2}y=\frac{F_x\cos \mathrm{\&beta;}\sin \mathrm{\&alpha;}+F_y(\cos \mathrm{\&gamma;}\cos \mathrm{\&alpha;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&beta;}\sin \mathrm{\&alpha;})+F_z(-\sin \mathrm{\&gamma;}\cos \mathrm{\&alpha;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&beta;}\sin \mathrm{\&alpha;})}{m}\end{array}\right.$

$\left\{\begin{array}{c}\stackrel{.}{\mathrm{\&alpha;}}=({\mathrm{\&omega;}}_z\cos \mathrm{\&gamma;}+{\mathrm{\&omega;}}_y\sin \mathrm{\&gamma;})\sec \mathrm{\&beta;}\\ \stackrel{..}{x}-2n\stackrel{.}{z}=\frac{F_x\cos \mathrm{\&beta;}\cos \mathrm{\&alpha;}+F_y(-\cos \mathrm{\&gamma;}\sin \mathrm{\&alpha;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&beta;}\cos \mathrm{\&alpha;})+F_z(\sin \mathrm{\&gamma;}\sin \mathrm{\&alpha;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&beta;}\cos \mathrm{\&alpha;})}{m}\\ \stackrel{..}{z}+2n\stackrel{.}{x}-3n^{2}z=\frac{-F_x\sin \mathrm{\&beta;}+F_y\sin \mathrm{\&gamma;}\cos \mathrm{\&beta;}+F_z\cos \mathrm{\&gamma;}\cos \mathrm{\&beta;}}{m}\end{array}\right.$

$\left[\begin{array}{ccc}{I}_x& & \\ & I_y& \\ & & I_z\end{array}\right]$ For manipulation robot's moment of momentum, m is the quality of manipulation robot, and n is target track angular speed, F _x, F _y, F _zand M _x, M _y, M _zbe respectively three-dimensional control and the control moment of manipulation robot;

Step 3: design space Sheng Xi robot surpass low coverage approach controller as shown in the formula, implementation space Sheng Xi robot surpasses near vision method of servo-controlling:

$\begin{array}{cc}\left\{\begin{array}{c}{M}_x=-p_1{k}_1-p_2{\mathrm{\&omega;}}_x\\ M_y=-p_3(k_1-k_2)-p_4{\mathrm{\&omega;}}_y\\ M_z=-p_5{\mathrm{\&omega;}}_z\\ F_y=-p_6(b_1+b_2)-p_7({\stackrel{.}{b}}_1+{\stackrel{.}{b}}_2)\end{array}\right.& \left\{\begin{array}{ccc}{p}_1>0& p_2>0& p_3>0\\ p_4>0& p_5>0& p_6>0\\ p_7>0& & \end{array}\right.\end{array}$

P ₁, p ₂, p ₃, p ₄, p ₅, p ₆, p ₇for the controller parameter of design.