CN1570556A - Measuring device and method for spatial pose of rigid body - Google Patents

Abstract

It is the measuring apparatus and method for the space position of rigid body and belongs to the field of observation and control. The invention provides a contact measuring apparatus for space position of a rigid body of low cost and high accuracy, which comprises measuring execution structure, data-collecting apparatus and a computer capable of storing calculation programs. The invention also discloses a method for the measuring. The method is used to get space position information of the rigid body to be measured by the use of iterative equations set according to the initial datum obtained from the said apparatus. The said apparatus is characterized by simple structure and high system real-time capabilities and can realize the closed-loop real-time control of the rigid body to be measured.

Description

Rigid space pose measuring apparatus and measuring method thereof

Technical field

The invention belongs to the observation and control technology field, relate to a kind of method and implement device that is used to measure rigid space six degree of freedom pose.

Background technology

In cartesian coordinate system, rigid body has six-freedom degree.In the Industry Control of lathe, mechanism, robot etc., in order to realize high-precision location, control can be adopted close-loop feedback control usually, and this has just proposed requirement to the kinetic measurement of spatial pose.In the close-loop feedback control that adopts, measure and mainly contain dual mode at present: the displacement (or angular displacement) in measurement topworks joint is fed back or the pose of direct measuring terminals feeds back.

In the scheme that measurement topworks's joint displacements (or angular displacement) is fed back, can be divided into two kinds of methods of semiclosed loop feedback and full cut-off ring feedback again.The displacement (or angular displacement) that the semiclosed loop feedback is only measured motor or leading screw, the kinematic train error after ignoring, the advantage of scheme is that cost is low, is convenient to realize that shortcoming is an error of having ignored machine driven system, precision is low; The displacement (or angular displacement) that full cut-off ring feedback needs to measure the practical set-up joint, instrument commonly used is a grating chi etc., and the advantage of this scheme is the precision height, and shortcoming is the cost height, and environment is had certain requirement.The common issue with that above-mentioned two kinds of schemes exist is, when tested rigid body pose was can not be whole controlled, this measurement scheme can't be finished measurement requirement.

Directly the method for measuring terminals pose normally adopts optical gauge to carry out non-contact measurement, as the positional information that adopts three optical instruments to measure three points on the rigid body can obtain the six degree of freedom posture information of rigid body.But common optical measuring apparatus real-time is poor, and the optical measuring apparatus can requirement of real time the time, cost becomes tens times increase again.Optical gauge has certain requirement to the space of environment and tested rigid body in addition.

The Stewart platform mechanism is the parallel institution that optimum is used for the implementation space six-freedom motion, it comes across nineteen sixties, general Stewart platform by a stationary platform, a motion platform and be connected in stationary platform and motion platform between six actuators form.By driving six actuators, make motion platform implementation space six-freedom motion.Present Stewart platform mechanism all is to be used for ACTIVE CONTROL.

Summary of the invention

The objective of the invention is to overcome the weak point of traditional location and attitude measuring, propose a kind of measurement mechanism and measuring method of low cost, high-precision rigid space position and attitude of contact.The present invention adopts the stay-supported scrambler to replace six actuators in the general Stewart platform mechanism, makes ACTIVE CONTROL become passive measurement, thereby obtains the posture information of tested motion rigid body.

Rigid space pose measuring apparatus disclosed by the invention is characterized in that: this measurement mechanism comprises the computing machine of measuring topworks, data collector and storage computation program; Described measurement topworks comprises a stationary platform and is fixed on 6 ball pivots on the described stationary platform, a motion platform that is connected with motion rigid body and be fixed on 6 ball pivots on the described motion platform, two ends respectively with stationary platform on six stay-supported scramblers linking to each other with 6 ball pivots on the motion platform of 6 ball pivots; Described data collector comprises six counters that link to each other with stay-supported scrambler output terminal respectively, the multiplexer that is connected with described each counter output, and the data transmit circuit that links to each other with described multiplexer; Described data transmit circuit is connected with described computing machine respective input mouth;

Described each ball pivot satisfies at the position coordinates of platform separately measures the Jacobian matrix J of topworks _pConditional number smaller or equal to 10,

$J_p=\left[\begin{array}{cc}{{\stackrel{\&RightArrow;}{l}}_1}^T& \frac{{({\mathrm{<figure-callout\; id="1"\; label="Rp"\; filenames="A20041000908300111.png,A20041000908300112.png"\; state="\{\{state\}\}">Rp</figure-callout>}}_1\&times;{\stackrel{\&RightArrow;}{l}}_1)}^T}{\left|{\mathrm{<figure-callout\; id="1"\; label="p"\; filenames="A20041000908300111.png,A20041000908300112.png"\; state="\{\{state\}\}">p</figure-callout>}}_1\right|}\\ .& .\\ .& .\\ .& .\\ {{\stackrel{\&RightArrow;}{l}}_6}^T& \frac{{({\mathrm{Rp}}_6\&times;{\stackrel{\&RightArrow;}{l}}_6)}^T}{\left|p_6\right|}\end{array}\right]\&Element;R^{6\&times;6},$

In the formula

Be respectively along the vector of unit length on each stay-supported scrambler length direction, p ₁～p ₆Be the coordinate of 6 ball pivots in motion platform coordinate system p on the motion platform, R is the direction cosine battle array of motion platform coordinate system p about stationary platform coordinate system P, according to successively around the x of stationary platform coordinate system P, y, z axle anglec of rotation α, the Ka Erdan angle method for expressing of beta, gamma, the formula of embodying is:

$R=\left[\begin{array}{ccc}\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}& -\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&beta;}\\ \sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\cos \mathrm{\&beta;}\\ -\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\end{array}\right].$

Of the present inventionly a kind ofly be improved to described 6 ball pivots that are fixed on the motion platform and directly be fixed on the motion rigid body, save described motion platform.

Stay-supported scrambler of the present invention is digital stay-supported scrambler.

It is of the present invention that a kind of to be improved to described stay-supported scrambler be analog stay-supported scrambler, this rigid space pose measuring apparatus also comprises an A/D capture card, the input end of described A/D capture card links to each other with the output terminal of described analog stay-supported scrambler, and output terminal links to each other with six counters respectively.

The invention also discloses a kind of described measurement mechanism and carry out the method for rigid space pose measurement, it is characterized in that: this measuring method comprises the steps:

1) system initialization is promptly determined the coordinate of each ball pivot in platform coordinate system separately, the initial length of stay-supported scrambler and the initial coordinate of tested rigid body in the stationary platform coordinate system, and import described computing machine;

2) when tested rigid motion, utilize data collector to gather the length variations signal of stay-supported scrambler, and be sent in the computing machine;

3) be the system of equations initial value with the initial coordinate of tested rigid body in the stationary platform coordinate system, the following iterative equation group of the program solution in the computing machine obtains the spatial attitude information and the positional information of tested rigid body:

L _i-|R·p _i+T-P _i|＝0，i＝1，…，6；

Wherein, L ₁～L ₆Be the measurement length that 6 stay-supported scramblers obtain, P _iBe the coordinate of 6 ball pivots in stationary platform coordinate system P on the stationary platform, p _iBe the coordinate of 6 ball pivots in motion platform coordinate system p on the motion platform,

R is the direction cosine battle array of motion platform coordinate system p about stationary platform coordinate system P, has comprised spatial attitude information to be asked, T=[x y z] ^TBe the position of initial point in stationary platform coordinate system P of motion platform coordinate system p, comprised spatial positional information to be asked;

4) with the pose of current motion rigid body initial value, realize continuous coverage as the iterative equation group.

The present invention compares existing technology and has following advantage:

The first, apparatus of the present invention adopt the connected mode of Stewart mechanism, and implementation structure is simple.

The second, this device is directly measured the pose of tested rigid body, and the simultaneity factor real-time is higher, so can realize the full cut-off ring of tested rigid body is controlled in real time.

The 3rd, stay-supported scrambler cost is low, uses reliably, and precision can reach 1 micron, can satisfy the measurement requirement of general precision.

The 4th, by rational ball pivot choice of location, make mechanism relatively more responsive to attitude, measuring accuracy is improved.

The 5th, simple to operate during measurement, quick, not high to the space requirement of environment and tested rigid body, be applicable to multiple occasion.

Description of drawings

Fig. 1 is that the system of rigid space pose measuring apparatus of the present invention constitutes synoptic diagram.

Fig. 2 is the process flow diagram of rigid space pose measuring method.

Fig. 3 a is the structural principle block diagram of one embodiment of the present of invention.

Fig. 3 b is the structural principle block diagram of an alternative embodiment of the invention.

Embodiment

Specifically describe the measuring method and the measurement mechanism of motion rigid body pose of the present invention in conjunction with the accompanying drawings.

As described in Figure 1, rigid space pose measuring apparatus of the present invention comprises the computing machine of measuring topworks, data collector and storage computation program; Described measurement topworks comprises stationary platform 1, is installed in six ball pivots 101～106 on the stationary platform; Motion platform 2 is installed in six ball pivots 201～206 on the motion platform; Corresponding connection ball pivot 101～106 and 201～206 stay-supported scrambler 301～306; The input end of data collector 4 is connected with the output terminal of stay-supported scrambler, and the output terminal of data collector 4 is connected with the respective input mouth of computing machine 5.

Stationary platform 1 is used for installing six ball pivots 101～106, makes the ball pivot stationkeeping, sets up a changeless stationary platform coordinate system on stationary platform, accurately measures the coordinate of ball pivot 101～106 in the stationary platform coordinate system.In specific embodiment, the concrete form of stationary platform 1 can be flat board and also can be framed structure, and according to tested movement of Rigid Body scope and angle, stationary platform can be installed in the top or the below of motion rigid body.

Motion platform 2 is used for installing six ball pivots 201～206, and the motion rigid body of the other end and required measurement is connected simultaneously.On motion platform, set up a motion platform coordinate system, accurately measure the coordinate of ball pivot 201～206 in the motion platform coordinate system.When on the rigid body of required measurement when being easier to ball pivot is installed, can dispense motion platform 2, directly six ball pivots 201～206 are installed on the rigid body of required measurement, and on tested rigid body, set up the motion platform coordinate system.In the present embodiment because the less difficult installation ball pivot of tested rigid body area, so employing motion platform 2.

Each ball pivot is when the position of platform should make tested rigid body in range of movement separately, and the Jacobian matrix conditional number of measuring topworks meets the demands.Jacobian matrix is represented the position and attitude error amount of motion platform and the mapping relations between side chain error in length amount, and its conditional number has reflected designed mechanism ratio of precision on all directions in the space, is an important indicator weighing measuring accuracy.According to discovering, when the conditional number of Jacobian matrix greater than 10 the time, final measuring accuracy will sharply descend, and illustrate that the design of ball pivot coordinate is unreasonable, need redesign ball pivot position.The conditional number of the Jacobian matrix of the measurement topworks in the rigid space pose measuring apparatus of the present invention is smaller or equal to 10, preferably in 4～5.The formula that embodies of Jacobian matrix is:

$J_p=\left[\begin{array}{cc}{{\stackrel{\&RightArrow;}{l}}_1}^T& \frac{{({\mathrm{<figure-callout\; id="1"\; label="Rp"\; filenames="A20041000908300111.png,A20041000908300112.png"\; state="\{\{state\}\}">Rp</figure-callout>}}_1\&times;{\stackrel{\&RightArrow;}{l}}_1)}^T}{\left|{\mathrm{<figure-callout\; id="1"\; label="p"\; filenames="A20041000908300111.png,A20041000908300112.png"\; state="\{\{state\}\}">p</figure-callout>}}_1\right|}\\ .& .\\ .& .\\ .& .\\ {{\stackrel{\&RightArrow;}{l}}_6}^T& \frac{{({\mathrm{Rp}}_6\&times;{\stackrel{\&RightArrow;}{l}}_6)}^T}{\left|p_6\right|}\end{array}\right]\&Element;R^{6\&times;6},$

In the formula

Be respectively along the vector of unit length on each stay-supported scrambler length direction, p ₁～p ₆Be the coordinate of 6 ball pivots in motion platform coordinate system p on the motion platform, R is the direction cosine battle array of motion platform coordinate system p about stationary platform coordinate system P, according to successively around the x of stationary platform coordinate system P, y, z axle anglec of rotation α, the Ka Erdan angle method for expressing of beta, gamma, the formula of embodying is:

$R=\left[\begin{array}{ccc}\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}& -\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&beta;}\\ \sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\cos \mathrm{\&beta;}\\ -\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\end{array}\right].$

Stay-supported scrambler 301～306, two ends link to each other with the motion platform ball pivot with the stationary platform ball pivot respectively, note the situation of length variations between corresponding ball pivot when tested rigid motion by scrambler.The form that data collector 4 becomes computing machine to receive the data-switching of stay-supported scrambler 301～306 is unifiedly resolved data transmission in the computing machine 5 of program to storing pose.

The stay-supported scrambler can be digital or analog on the output data mode.When adopting digital stay-supported scrambler, see Fig. 3 a, data collector 4 comprises the counter of six record coding device variable quantities, with six road signals merge into the multiplexer of one road signal and link to each other with multiplexer will converge after signal send to the data transmit circuit of computing machine.Digital stay-supported scrambler output terminal links to each other with six counters, and each counter output is connected with multiplexer, and data transmit circuit is connected with described computing machine respective input mouth.When adopting analog stay-supported scrambler, see Fig. 3 b, this rigid space pose measuring apparatus comprises that also one is the A/D capture card of digital signal with analog signal conversion, the input end of described A/D capture card links to each other with the output terminal of described analog stay-supported scrambler, and output terminal links to each other with six counters in the data collector 4 respectively.

The stay-supported scrambler is except that the way of output, aspect counting, specifically can adopt increment type or absolute type, on the circuit characteristic, can select voltage-type, current mode etc., in specific embodiment, can select encapsulation complete finished product stay-supported scrambler and multiplexer for use, as the digital increments formula voltage-type stay-supported scrambler HLS-M-1005 of HONTKO company, 74151 multiplexers etc.The data transmit circuit of data collector can be selected general serial ports transtation mission circuit, the receivable modes of computing machine such as parallel port transtation mission circuit for use.

Stored the computing machine 5 that pose resolves algorithm, the data and coordinate and 201～206 ball pivots the coordinate in motion platform coordinate system of ball pivot 101～106 in the stationary platform coordinate system of stay-supported scrambler are combined, by finding the solution position and the attitude that the motion of mechanism equation obtains motion platform relative fixed platform.

As shown in Figure 2.The flow process of rigid space pose measuring method of the present invention is as follows:

(1) system initialization: this part is mainly determined the parameter that is applied in the follow-up measurements and calculations, comprising ball pivot 101～106 and 201～206 coordinate in stationary platform coordinate system and motion platform coordinate system respectively, the initial length of stay-supported scrambler and be used under the tested rigid body initial situation of iterative equation coordinate at the stationary platform coordinate system, and with described coordinate input computing machine;

(2) data that the stay-supported scrambler is measured are transferred in the computing machine by data collector.If the stay-supported scrambler is increment type stay-supported scrambler, then need distance between ball pivot is respectively organized in the initial length addition of length variations amount in the stay-supported scrambler and stay-supported scrambler.

(3) find the solution locus and the spatial attitude that following iterative equation group obtains motion rigid body.

L _i-|R·p _i+T-P _i|＝0，i＝1，…，6

T=[x y z in the formula] ^TBe the position of initial point in coordinate system P of coordinate system p, can obtain the spatial positional information of motion rigid body; R is the direction cosine battle array of motion platform coordinate system p about stationary platform coordinate system P, according to successively around the x of stationary platform coordinate system P, y, z axle limited angle rotating α, the Ka Erdan angle method for expressing of beta, gamma, the formula of embodying is:

$R=\left[\begin{array}{ccc}\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}& -\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&beta;}\\ \sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\cos \mathrm{\&beta;}\\ -\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\end{array}\right]$

L ₁～L ₆Be the measurement length that 6 stay-supported scramblers obtain.P _iBe the coordinate of six ball pivots in stationary platform coordinate system P on the stationary platform.p _iBe the coordinate of six ball pivots in motion platform coordinate system p on the motion platform.

In concrete enforcement, above-mentioned iterative equation group can adopt newton-La Feixunfa to find the solution.Below in order to express easily, this iterative algorithmic formula is described below:

Adopt newton-La Feixunfa to find the solution the method for Stewart kinematics of mechanism normal solution.

Motion of mechanism is just being solved an equation and can be expressed as:

f _i(x，y，z，α，β，γ)＝L _i-|R·p _i+T-P _i|＝0 i＝1，2，…6

This system of equations is 6 nonlinear equations that contain 6 unknown numbers.Make X=(x, y, z, α, beta, gamma), F (X)=[f ₁(X), f ₂(X), f ₃(X), f ₄(X), f ₅(X), f ₆(X)] ^TX ^*Be solution of equations.Newton-La Feixunfa is an iterative algorithm.Equation Taylor is launched, obtains:

F(X+ΔX)＝F(X)+JδX+O(δX ²)

J is the Jacobian matrix of equation in the formula, $J_{\mathrm{ij}}=\frac{{\&PartialD;f}_i}{{\&PartialD;X}_j}.$ Ignore the second order amount of equation, and make F that (X+ Δ X)=0 has:

JδX＝-F(X)

This is a system of linear equations, separates the X=-J into δ ^-1F (X).Therefore its iterative process is: X _New=X _Old+ δ X.

(4) pose of current motion rigid body is substituted the initial value of iterative equation, prepare, realize continuous coverage for measure next time.

Claims (5)

1. rigid space pose measuring apparatus, it is characterized in that: this measurement mechanism comprises the computing machine of measuring topworks, data collector and storage computation program; Described measurement topworks comprises a stationary platform and is fixed on 6 ball pivots on the described stationary platform, a motion platform that is connected with motion rigid body and be fixed on 6 ball pivots on the described motion platform, two ends respectively with stationary platform on six stay-supported scramblers linking to each other with 6 ball pivots on the motion platform of 6 ball pivots; Described data collector comprises six counters that link to each other with stay-supported scrambler output terminal respectively, the multiplexer that is connected with described each counter output, and the data transmit circuit that links to each other with described multiplexer; Described data transmit circuit is connected with described computing machine respective input mouth;

Described each ball pivot the position coordinates of platform separately satisfy measure the Jacobian matrix Jp of topworks conditional number smaller or equal to 10,

$J_P=\left[\begin{array}{cc}{{\stackrel{\&RightArrow;}{l}}_1}^T& \frac{{({\mathrm{Rp}}_1\&times;{\stackrel{\&RightArrow;}{l}}_1)}^T}{\left|p_1\right|}\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ {{\stackrel{\&RightArrow;}{l}}_6}^T& \frac{{({\mathrm{Rp}}_6\&times;{\stackrel{\&RightArrow;}{l}}_6)}^T}{\left|p_6\right|}\end{array}\right]\&Element;R^{6\&times;6},$

In the formula Be respectively along the vector of unit length on each stay-supported scrambler length direction, p ₁～p ₆Be the coordinate of 6 ball pivots in motion platform coordinate system P on the motion platform, R is the direction cosine battle array of motion platform coordinate system P about stationary platform coordinate system P, according to successively around the x of stationary platform coordinate system P, y, z axle anglec of rotation α, the Ka Erdan angle method for expressing of beta, gamma, the formula of embodying is:

$R=\left[\begin{array}{ccc}\cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}& -\cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&beta;}\\ \sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\cos \mathrm{\&beta;}\\ -\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\end{array}\right].$

2. rigid space pose measuring apparatus according to claim 1 is characterized in that: described 6 ball pivots that are fixed on the motion platform directly are fixed on the motion rigid body, save described motion platform.

3. rigid space pose measuring apparatus according to claim 1 and 2 is characterized in that: described stay-supported scrambler is digital stay-supported scrambler.

4. rigid space pose measuring apparatus according to claim 1 and 2, it is characterized in that: described stay-supported scrambler is analog stay-supported scrambler, this rigid space pose measuring apparatus also comprises an A/D capture card, the input end of described A/D capture card links to each other with the output terminal of described analog stay-supported scrambler, and output terminal links to each other with six counters respectively.

5. utilize the described measurement mechanism of claim 1 to carry out the method for rigid space pose measurement, it is characterized in that: this measuring method comprises the steps:

1) system initialization is promptly determined the coordinate of each ball pivot in platform coordinate system separately, the initial length of stay-supported scrambler and the initial coordinate of tested rigid body in the stationary platform coordinate system, and import described computing machine;

2) when tested rigid motion, utilize data collector to gather the length variations signal of stay-supported scrambler, and be sent in the computing machine;

3) be the system of equations initial value with the initial coordinate of tested rigid body in the stationary platform coordinate system, the following iterative equation group of the program solution in the computing machine obtains the spatial attitude information and the positional information of tested rigid body:

L _i-|R·p _i+T-P _i|＝0，i＝1，…，6；

Wherein, L ₁～L ₆Be the measurement length that 6 stay-supported scramblers obtain, P _iBe the coordinate of 6 ball pivots in stationary platform coordinate system P on the stationary platform, p _iBe the coordinate of 6 ball pivots in motion platform coordinate system p on the motion platform,

R is the direction cosine battle array of motion platform coordinate system p about stationary platform coordinate system P, has comprised spatial attitude information to be asked, T=[x y z] ^TBe the position of initial point in stationary platform coordinate system P of motion platform coordinate system p, comprised spatial positional information to be asked;

4) with the pose of current motion rigid body initial value, realize continuous coverage as the iterative equation group.

Images