CN103447877A - Normal vector detection and posture-adjusting movement method of autonomous moving mechanism - Google Patents

Abstract

Disclosed is a normal vector detection and posture-adjusting movement method of an autonomous moving mechanism. The method is characterized by including the following steps: firstly, mounting three or four laser ranging sensors on a tail end executor of the autonomous moving mechanism; secondly, acquiring a normal vector n of a measured drilling plane by utilizing values measured by the laser ranging sensors; thirdly, according to the normal vector n, of the drilling plane, acquired by measuring and calculating, and through a posture-adjusting movement inverse solution algorithm, acquiring driving quantity of each leg when the autonomous moving mechanism reaches a drilling normal vector, wherein the driving quantity is used for controlling a system to realize posture-adjusting movement; finally, according to the normal vector n, of the drilling plane, acquired by measuring and calculating, and through the posture-adjusting movement inverse solution algorithm, acquiring deviation quantity of a tool when the autonomous moving mechanism reaches the drilling normal vector, wherein the deviation quantity is used for controlling the system to realize tool deviation correction. The normal vector detection and posture-adjusting movement method is simple, easy to control, high in controlling accuracy and high in efficiency and speed.

Description

The autonomous institutionalization is vowed and is detected and the posture adjustment movement technique

Technical field

The present invention relates to a kind of autonomous mechanism of aircraft assembling, in especially a kind of aircraft assembling process, the method for autonomous mechanism is vowed detection, method arrow posture adjustment and Tool Compensation, and specifically a kind of autonomous institutionalization is vowed and detected and the posture adjustment movement technique.

Background technology

At present, the main method of attachment that aircaft configuration adopts is mechanical connection, and on a large aircraft, nearly 1,500,000～2,000,000 rivets and bolts, in order to meet the requirement of present generation aircraft high life, at first will guarantee the safety and reliability of mechanical connection.In recent years, for guaranteeing the aircraft assembly quality, the fatigue life of improving body, low cost and high efficiency in realizing producing in enormous quantities, with B787, A380, adopted the automation mounting technology in the Modern New large aircraft assembling process for representative such as C-17 in a large number.The development of aircraft automatic assembly system is mainly both direction.A kind of is the large-scale special for automatic assembly system that cost is higher, be applicable to the larger product of batch, as MPAC, VPAC etc.Another kind be lower-cost, be applicable in batches than the light-duty automatic assembly system of miscellaneous goods, the developing direction of this kind of system is lightness, flexibility, modularization, and its Typical Representative has based on industrial machine arm automatic setup system, flexible rail automatic assembly system at present.

Based on industrial machine arm automatic setup system, be that to utilize industrial machine arm general on market be apparatus body, the automatic assembly system that coordinates corresponding end effector to form.It has the housing construction maturation, and integration realization is comparatively easy, the advantage such as work flexible, but exist, scope of activities is less, structure is large and the poor shortcoming of mobility.Automatic assembly system based on flexible rail, to take flexible rail as platform, absorption from the teeth outwards, mix corresponding end effector, complete the work such as automation drilling, as the disclosed flexible rail multiaxis machine tool of Chinese invention patent CN200580025525.X and the method for Boeing's application.But also there is the shortcoming of the large and poor mobility of structure in this automatic assembly system based on flexible rail, especially, this system needs other attachment rail, to frock require morely, the work early-stage preparations time such as grows at the shortcoming.

Autonomous mechanism (as shown in Figure 1 can referring to Chinese patent CN201310030879.X), belong to eight sufficient parallel institutions, mechanism has down the characteristics of the high rigidity of parallel institution, high accuracy, high-bearing capacity in working order, can walk and position in the aircraft product surface, and possess the ability that realizes the normal direction posture adjustment, but due to complex structure, positive resolving Algorithm is difficult to apply in process that its motion solves, and the particularity of its structure, can not apply the algorithm of pervasive parallel institution.In view of it is applied to the curved surface covering surface more, determined the complexity of its motion, determined the algorithm complexity, control difficulty large, this be there is no at present to desirable solution.

Summary of the invention

The objective of the invention is, for existing eight sufficient autonomous mechanism location and the large problems of mobile difficulty, by means of airborne laser range finder, to invent a kind of autonomous institutionalization and vow detection and posture adjustment movement technique.

Technical scheme of the present invention is:

A kind of autonomous institutionalization is vowed and is detected and the posture adjustment movement technique, it is characterized in that it comprises the following steps:

At first, three or four laser range sensors are installed on autonomous mechanism end effector;

Secondly, utilize the value that three or four laser range sensors are surveyed to try to achieve surveyed drilling planar process arrow n;

The 3rd, calculate the drilling planar process of institute according to above-mentioned measurement and vow that n, by the anti-resolving Algorithm of posture adjustment campaign, obtains each leg driving amount that autonomous mechanism arrives drilling method arrow, realize the posture adjustment campaign for control system;

Finally, calculate the drilling planar process of institute according to above-mentioned measurement and vow that n is by the anti-resolving Algorithm of posture adjustment campaign, when obtaining autonomous mechanism and arriving the drilling method and vow, the side-play amount of cutter, realize the tool offset correction for control system.

Described planar process vows that definite method of n comprises the planar process arrow n that utilizes three laser range sensors and the value of utilizing four laser range sensors to survey to determine the drilling position;

The planar process that the described value of utilizing three laser range sensors to survey is tried to achieve is vowed n:

n=(l,m,n) ^T=C ₁B ₁×B ₁A ₁ (1)

Wherein by vector correlation, can obtain vector C ₁b ₁as shown in Equation (2), note the direction of vector,

C ₁B ₁=C ₁C+CB+BB ₁

=(|C ₁C|-|BB ₁|)·(0,0,1) ^T-|CB|·(0,1,0) ^T (2)

Can obtain vector B with the reason vector correlation ₁a ₁as shown in Equation (3), note the direction of vector

B ₁A ₁=B ₁B+BA+AA ₁

=(|B ₁B|-|AA ₁|)·(0,0,1) ^T+|BA|·(1,0,0) ^T (3)

The measured value that in formula (2), (3), parameter is each laser sensor, can try to achieve surveyed planar process by formula (2) (3) substitution formula (1) and vow n;

The planar process that the described value of utilizing four laser range sensors to survey is tried to achieve vows that n can solve cutter by formula (4) again

The normal vector of tool point, vow as surveyed planar process with the results averaged of formula (1), improved certainty of measurement.

(l,m,n) ^T=D ₁C ₁×D ₁A ₁

whereD ₁C ₁=(|D ₁D|-|C ₁C|)·(0,0,1) ^T-|CD|·(1,0,0) ^T (4)

D ₁A ₁=(|D ₁D|-|A ₁A|)·(0,0,1) ^T-|AD|·(0,1,0) ^T

If there is a deviation very large in A, B, C, D, illustrate that this place breaks away from measurement category (as measured on edge or there is cavity in somewhere), the data computing method that adopts its excess-three point to:

1. when the super scope of A point, with B, C, D, calculate, as formula (5)

(l,m,n) ^T=C ₁B ₁×C ₁D ₁ (5)

2. when the super scope of B point, with A, C, D, calculate, as formula (4)

3. when the super scope of C point, with A, B, D, calculate, as formula (6)

(l,m,n) ^T=A ₁D ₁×A ₁B ₁ (6)

4. when the super scope of D point, with A, B, C, calculate, as shown in Equation (1);

In above formula, the normal vector of establishing autonomous mechanism moving platform is parallel all the time with the drilling main shaft, note n ₀=(0,0,1) ^t, laser range sensor A, B, C, D, the vector direction of range finding slotted line is parallel with the drilling main shaft, is also n ₀=(0,0,1) ^t, distance measuring sensor ABCD plane parallel is in autonomous mechanism frame body, and BA is parallel with outside framework, and under the note original state, the BA direction is (1,0,0) ^t, BC is vertical with outside framework, and under the note original state, the BC direction is (0,1,0) ^t, | A ₁a|, | B ₁b|, | C ₁c|, | D ₁d| is the respective sensor measuring distance; Detect curved surface A ₁b ₁c ₁d ₁an approximate plane (the general A of actual conditions that regards as in zone ₁b ₁c ₁and A ₁b ₁d ₁two planes), method is vowed to detect and is surveyed curved surface A ₁b ₁c ₁d ₁normal vector, i.e. plane A ₁b ₁c ₁(with plane A ₁b ₁d ₁) normal vector; The drilling method of surveying vow, also referred to as the posture adjustment goal method, vow, be designated as n=(l, m, n) ^t.

The anti-resolving Algorithm of described posture adjustment campaign refers to that autonomous mechanism is by n ₀=(0,0,1) ^tposture adjustment is to n=(l, m, n) ^tstate, anti-side-play amount of separating displacement and the cutter point of eight leg liftings;

Only have 3 frees degree during autonomous mechanism posture adjustment, wherein rotary freedom only has the rotation of A, B angle both direction, the pitching that is autonomous mechanism with sidewinder action, autonomous mechanism leg 2 only has a compensation rate that is parallel to the housing direction, therefore autonomous mechanism is by n ₀=(0,0,1) ^tposture adjustment is to n=(l, m, n) ^tprocess, also can think autonomous mechanism through B to pivot angle

a is to pivot angle

the pose arrived;

According to the Rotating Transition of Coordinate computing, moving coordinate system O _mwith position fixing be O _bcoordinate conversion matrix R _bmshown in (7); Because the Space Rotating matrix is orthogonal matrix, therefore R _bmthere is inverse matrix, and R _bm ^-1=R _bm ^t.

$\begin{array}{c}{R}_{\mathrm{bm}}=\left(\begin{array}{ccc}\cos \left(x_b{x}_m\right)& \cos \left(x_b{y}_m\right)& \cos \left(x_b{z}_m\right)\\ \cos \left(y_b{x}_m\right)& \cos \left(y_b{y}_m\right)& \cos \left(y_b{z}_m\right)\\ \cos \left(z_b{x}_m\right)& \cos \left(z_b{y}_m\right)& \cos \left(z_b{z}_m\right)\end{array}\right)\\ =\left(\begin{array}{ccc}{\cos \mathrm{\φ}}_y& 0& {\sin \mathrm{\φ}}_y\\ {\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y& {\cos \mathrm{\φ}}_x& {-\sin \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y\\ {-\cos \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y& {\sin \mathrm{\φ}}_x& {\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y\end{array}\right)\end{array}---\left(7\right)$

Wherein

Goal method is vowed n=(l, m, n) ^tby method arrow detection algorithm, measured, therefore by formula (8), can be obtained posture adjustment A, the B pivot angle of autonomous mechanism

can solve transition matrix R _bm;

Autonomous mechanism moving coordinate system, with respect to the position of position fixing system, is designated as

O _bO _m=(c,d,h ₁) ^T (9)

Any point D is position coordinates vector D at position fixing _bwith the position vector D at moving coordinate system _mpass is:

D _b=R _bmD _m+O _bO _m (10)

D _m=R _bm ^TD _b-R _bm ^TO _bO _m (11)

S sets up an office _iat coordinate system O _b-x _by _bz _band O _m-x _my _mz _mcoordinate be respectively S _bi=(x _sib, y _sib, z _sib) ^tand S _mi=(x _sim, y _sim, z _sim) ^t;

S _im=R _bm ^TS _ib-R _bm ^TO _bO _m (12)

S _im-S _1m=R _bm ^T(S _ib-S _1b) (13)

S _im=R _bm ^T(S _ib-S _1b)+S _1m (14)

S _im0=S _ib-O _bO _m=S _ib-(c,d,h) ^T (15)

During posture adjustment, fixing whole 1 not lifting, by S _1b=(0,0,0) ^t; S _1m=(c ,-d ,-h ₁) ^tsubstitution formula (14), can obtain each sufficient side-play amount before and after posture adjustment and be

ΔS _im=S _im-S _im0=R _bm ^TS _ib-S _ib (16)

Solving each sufficient input quantity and compensation rate after posture adjustment by formula (16) is

$\left\{\begin{array}{c}{\mathrm{\Δx}}_{\mathrm{pim}}=x_{\mathrm{sib}}{\cos \mathrm{\φ}}_y+y_{\mathrm{sib}}{\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y\\ -z_{\mathrm{sib}}{\cos \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y-x_{\mathrm{sib}}\\ \mathrm{\Δ}{y}_{\mathrm{pim}}=y_{\mathrm{sib}}{\cos \mathrm{\φ}}_x+z_{\mathrm{sib}}{\sin \mathrm{\φ}}_x-y_{\mathrm{sib}}\\ \mathrm{\Δ}{z}_{\mathrm{pim}}=x_{\mathrm{sib}}{\sin \mathrm{\φ}}_y-y_{\mathrm{sib}}{\sin \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y\\ +z_{\mathrm{sib}}{\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y-z_{\mathrm{sib}}\end{array}\right.---\left(17\right)$

Δ z wherein _pimfor autonomous mechanism arrives target vector n=(l, m, n) ^tthe time each sufficient driving amount, Δ x _pim, Δ y _pimthat each is sufficient in moving platform x, the compensation rate of y both direction;

After posture adjustment, cutter point can produce skew, for guaranteeing that point of a knife point is constant, need to provide cutter point X, Y after posture adjustment, Z adjustment amount:

Point of a knife point T, can be obtained by formula (11)

T _m=R _bm ^TT _b-R _bm ^TO _bO _m (18)

S _1m=R _bm ^TS _1b-R _bm ^TO _bO _m

Therefore can obtain

T _m-S _1m=R _bm ^T(T _b-S _1b) (19)

T _m0=T _b-(c,d,h) ^T (20)

S wherein _1b={ x _s1b, y _s1b, z _s1b} ^t={ 0,0,0} ^t, S _1m={ x _s1m, y _s1m, z _s1m} ^t={ c ,-d ,-h ₁} ^t

ΔT _m=T _m-T _m0=R _bm ^TT _b-T _b (21)

The cutter point T(of autonomous mechanism processing stand) absolute coordinate is designated as T _b={ x _tb, y _tb, z _tb} ^t, try to achieve the coordinate figure of cutter point at moving coordinate system by formula (21), can calculate thus cutter point compensation rate under X, Y, Z direction moving coordinate system after posture adjustment, as shown in formula (22).

$\left\{\begin{array}{c}{\mathrm{\Δx}}_T=x_{\mathrm{Tb}}{\cos \mathrm{\φ}}_y+y_{\mathrm{Tb}}{\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y\\ -z_{\mathrm{Tb}}{\cos \mathrm{\φ}}_x{\sin \mathrm{\φ}}_x-x_{\mathrm{Tb}}\\ {\mathrm{\Δy}}_T=y_{\mathrm{Tb}}{\cos \mathrm{\φ}}_x+z_{\mathrm{Tb}}{\sin \mathrm{\φ}}_x-y_{\mathrm{Tb}}\\ {\mathrm{\Δz}}_T=x_{\mathrm{Tb}}{\sin \mathrm{\φ}}_y-y_{\mathrm{Tb}}{\sin \mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ +z_{\mathrm{Tb}}{\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y-z_{\mathrm{Tb}}\end{array}\right.---\left(22\right).$

Beneficial effect of the present invention:

The inventive method is simple, is easy to control, and by the measured value that is arranged on four laser range sensors on autonomous mechanism end effector, can measure the drilling method arrowhead amount in the lower skin-surface zone of surveying of autonomous mechanism platform coordinate system.Vow according to the drilling method in surveyed skin-surface zone again, use the anti-resolving Algorithm of posture adjustment campaign, can solve easily autonomous mechanism and arrive each leg driving amount that the drilling method is vowed, for control system, realize the posture adjustment campaign.While vows according to the drilling method in surveyed skin-surface zone, the anti-resolving Algorithm of utilization posture adjustment campaign, and in the time of can also solving easily autonomous mechanism arrival drilling method arrow, the side-play amount of cutter, realize the tool offset correction for control system.Therefore, there is control accuracy high, efficiently advantage fast.

The accompanying drawing explanation

Fig. 1 is the perspective view of autonomous of the present invention mechanism.

Fig. 2 is the laser range sensor structure distribution figure on autonomous of the present invention mechanism end effector.

Fig. 3 is that the method be comprised of many laser range sensors of the present invention is vowed the detection system schematic diagram.

Fig. 4 is autonomous mechanism schematic diagram of mechanism involved in the present invention.

Fig. 5 is autonomous mechanism posture adjustment motion sketch involved in the present invention.

In Fig. 1: 1. housing, 2. inside casing, 3. leg, 4. pair eccentricity compensation mechanism, 5. end effector.

In Fig. 2: A, B, C, D is four laser range sensors, and AB is parallel to housing 1, and AD is perpendicular to housing 1.

In Fig. 3: T is the cutter point, A ₁, B ₁, C ₁, D ₁for the measurement point of each laser range sensor, on covering.

In Fig. 5, leg i (i=1,2 ..., 8) on the secondary position of ball be designated as S _i(i=1,2 ..., 8), on its leg, the moving sets position is designated as P _i(i=1,2 ..., 8), R _i1, R _i2for two eccentric rotaries.

The specific embodiment

Below in conjunction with drawings and Examples, the present invention is further illustrated.

As Figure 1-5.

A kind of autonomous institutionalization is vowed detection and posture adjustment movement technique, and it comprises the following steps:

At first, three or four laser range sensors are installed on autonomous mechanism end effector;

Secondly, utilize the value that three or four laser range sensors are surveyed to try to achieve surveyed drilling planar process arrow n;

The 3rd, calculate the drilling planar process of institute according to above-mentioned measurement and vow that n, by the anti-resolving Algorithm of posture adjustment campaign, obtains each leg driving amount that autonomous mechanism arrives drilling method arrow, realize the posture adjustment campaign for control system;

Finally, calculate the drilling planar process of institute according to above-mentioned measurement and vow that n is by the anti-resolving Algorithm of posture adjustment campaign, when obtaining autonomous mechanism and arriving the drilling method and vow, the side-play amount of cutter, realize the tool offset correction for control system.

Details are as follows:

The normal vector of autonomous mechanism moving platform is parallel all the time with the drilling main shaft, note n ₀=(0,0,1) ^t, laser range sensor A, B, actual 3 of C, D(get final product), the vector direction of range finding slotted line is parallel with the drilling main shaft, is also n ₀=(0,0,1) ^t.Distance measuring sensor ABCD plane parallel is in autonomous mechanism frame body, and BA is parallel with outside framework, and under the note original state, the BA direction is (1,0,0) ^t, BC is vertical with outside framework, and under the note original state, the BC direction is (0,1,0) ^t.| A ₁a|, | B ₁b|, | C ₁c|, | D ₁d| is the respective sensor measuring distance.

Detect curved surface A ₁b ₁c ₁d ₁an approximate plane (the general A of actual conditions that regards as in zone ₁b ₁c ₁and A ₁b ₁d ₁two planes), method is vowed to detect and is surveyed curved surface A ₁b ₁c ₁d ₁normal vector, i.e. plane A ₁b ₁c ₁(with plane A ₁b ₁d ₁) normal vector.

Survey the drilling method in the present invention and vow, vow also referred to as the posture adjustment goal method, be designated as n=(l, m, n) ^t.

Actual three laser range sensors can ask surveyed planar process to vow n

As Fig. 3, by known the surveyed planar process of vectorial multiplication cross, vow that n can be tried to achieve by formula (1)

n=(l,m,n) ^T=C ₁B ₁×B ₁A ₁ (1)

Wherein by vector correlation, can obtain vector C ₁b ₁as shown in Equation (2), note the direction of vector

C ₁B ₁=C ₁C+CB+BB ₁

=(|C ₁C|-|BB ₁|)·(0,0,1) ^T-|CB|·(0,1,0) ^T (2)

Can obtain vector B with the reason vector correlation ₁a ₁as shown in Equation (3), note the direction of vector

B ₁A ₁=B ₁B+BA+AA ₁

=(|B ₁B|-|AA ₁|)·(0,0,1) ^T+|BA|·(1,0,0) ^T (3)

The measured value that in formula (2) (3), parameter is a laser sensor, can try to achieve surveyed planar process by formula (2) (3) substitution formula (1) and vow n.

Method of the present invention vows that detection system, by four laser range sensor A, B, C and D, below provides under four-sensor, the planar process of the surveying arrow n of institute tried to achieve

If the 4th rangefinder D arranged, can be solved again the normal vector of cutter point by formula (4), with the results averaged of formula (1), as surveyed planar process, vow, improved certainty of measurement.

(l,m,n) ^T=D ₁C ₁×D ₁A ₁

whereD ₁C ₁=(|D ₁D|-|C ₁C|)·(0,0,1) ^T-|CD|·(1,0,0) ^T (4)

D ₁A ₁=(|D ₁D|-|A ₁A|)·(0,0,1) ^T-|AD|·(0,1,0) ^T

If there is a deviation very large in A, B, C, D, illustrate that this place breaks away from measurement category (as measured on edge or there is cavity in somewhere), the data computing method that adopts its excess-three point to.

1. when the super scope of A point, with B, C, D, calculate, as formula (5)

(l,m,n) ^T=C ₁B ₁×C ₁D ₁ (5)

2. when the super scope of B point, with A, C, D, calculate, as formula (4)

3. when the super scope of C point, with A, B, D, calculate, as formula (6)

(l,m,n) ^T=A ₁D ₁×A ₁B ₁ (6)

4. when the super scope of D point, with A, B, C, calculate, as formula (1)

After measuring posture adjustment goal method arrow, solve the driving amount of each leg while arriving object pose, below in conjunction with accompanying drawing, the posture adjustment motion algorithm is described.

As Fig. 4, shown in Fig. 5.

In normal direction posture adjustment process, eight legs of autonomous mechanism suck, and housing 1 is locked relative fixing with inside casing 2, and the pose_adjuster sketch as shown in Figure 5.Framework (comprising housing 1, inside casing 2, end effector 5) is considered as moving platform, and fuselage surface is considered as fixed platform, and eight legs are for connecting the side chain of moving fixed platform, and the moving sets of eight legs is secondary for driving.Move back pitching that the leg elongation realizes autonomous mechanism and sidewinder by eight, thereby complete the posture adjustment action, the drilling method of autonomous mechanism is vowed with goal method and vow that n overlaps, in the posture adjustment process, the side-play amount of each leg 3 ends generations is compensated by two eccentricity compensation mechanism 4.

Set up right hand orthogonal coordinate system as shown in Figure 5.Position fixing system: O _b-x _by _bz _b, be fixed in the coordinate system on covering, initial point O _bbe positioned at S ₁upper, x _bfor original state is parallel to the BA direction of autonomous mechanism housing, y _bfor original state is parallel to the BC direction of autonomous mechanism, z _bfor x _band y _bdefinite right-handed coordinate system, under original state, the drilling method of autonomous mechanism is vowed as n ₀=(0,0,1) ^t.Moving coordinate system: O _m-x _my _mz _m, be fixed in the coordinate system of autonomous mechanism, initial point O _mfor P ₁, z _mparallel with the lifting direction of leg, x _mall the time be parallel to housing BA direction, y _ball the time be parallel to the BC direction.Under original state, moving coordinate system is parallel with position fixing.

Posture adjustment motion algorithm involved in the present invention, can be described as autonomous mechanism by n ₀=(0,0,1) ^tposture adjustment is to n=(l, m, n) ^tstate, the counter displacement of separating eight leg liftings.

Autonomous mechanism only has 3 frees degree during posture adjustment, and wherein rotary freedom only has the rotation of A, B angle both direction, is the pitching of autonomous mechanism and sidewinders action.As shown in Figure 5, autonomous mechanism leg 2 only has a compensation rate that is parallel to the housing direction, therefore autonomous mechanism is by n ₀=(0,0,1) ^tposture adjustment is to n=(l, m, n) ^tprocess, also can think autonomous mechanism through B to pivot angle

a is to pivot angle the pose arrived.

According to the Rotating Transition of Coordinate computing, moving coordinate system O _mwith position fixing be O _bcoordinate conversion matrix R _bmshown in (7).Because the Space Rotating matrix is orthogonal matrix, therefore R _bmthere is inverse matrix, and R _bm ^-1=R _bm ^t.

$\begin{array}{c}{R}_{\mathrm{bm}}=\left(\begin{array}{ccc}\cos \left(x_b{x}_m\right)& \cos \left(x_b{y}_m\right)& \cos \left(x_b{z}_m\right)\\ \cos \left(y_b{x}_m\right)& \cos \left(y_b{y}_m\right)& \cos \left(y_b{z}_m\right)\\ \cos \left(z_b{x}_m\right)& \cos \left(z_b{y}_m\right)& \cos \left(z_b{z}_m\right)\end{array}\right)\\ =\left(\begin{array}{ccc}{\cos \mathrm{\φ}}_y& 0& {\sin \mathrm{\φ}}_y\\ {\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y& {\cos \mathrm{\φ}}_x& {-\sin \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y\\ {-\cos \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y& {\sin \mathrm{\φ}}_x& {\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y\end{array}\right)\end{array}---\left(7\right)$

Wherein

Goal method is vowed n=(l, m, n) ^tby method arrow detection algorithm, measured, therefore by formula (8), can be obtained posture adjustment A, the B pivot angle of autonomous mechanism

can solve transition matrix R _bm

Solve posture adjustment each leg driving amount and compensation rate, details are as follows:

According to autonomous mechanism involved in the present invention, known, moving coordinate system, with respect to the position of position fixing system, is designated as

O _bO _m=(c,d,h ₁) ^T (9)

Any point D is position coordinates vector D at position fixing _bwith the position vector D at moving coordinate system _mpass is

D _b=R _bmD _m+O _bO _m (10)

D _m=R _bm ^TD _b-R _bm ^TO _bO _m (11)

S sets up an office _iat coordinate system O _b-x _by _bz _band O _m-x _my _mz _mcoordinate be respectively S _bi=(x _sib, y _sib, z _sib) ^tand S _mi=(x _sim, y _sim, z _sim) ^t.

S _im=R _bm ^TS _ib-R _bm ^TO _bO _m (12)

S _im-S _1m=R _bm ^T(S _ib-S _1b) (13)

S _im=R _bm ^T(S _ib-S _1b)+S _1m (14)

S _im0=S _ib-O _bO _m=S _ib-(c,d,h) ^T (15)

During posture adjustment, fixing whole 1 generally not lifting, by S _1b=(0,0,0) ^t; S _1m=(c ,-d ,-h ₁) ^tsubstitution formula (14), can obtain each sufficient side-play amount before and after posture adjustment and be

ΔS _im=S _im-S _im0=R _bm ^TS _ib-S _ib (16)

Solving each sufficient input quantity and compensation rate after posture adjustment by formula (16) is

$\left\{\begin{array}{c}{\mathrm{\Δx}}_{\mathrm{pim}}=x_{\mathrm{sib}}{\cos \mathrm{\φ}}_y+y_{\mathrm{sib}}{\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y\\ -z_{\mathrm{sib}}{\cos \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y-x_{\mathrm{sib}}\\ \mathrm{\Δ}{y}_{\mathrm{pim}}=y_{\mathrm{sib}}{\cos \mathrm{\φ}}_x+z_{\mathrm{sib}}{\sin \mathrm{\φ}}_x-y_{\mathrm{sib}}\\ \mathrm{\Δ}{z}_{\mathrm{pim}}=x_{\mathrm{sib}}{\sin \mathrm{\φ}}_y-y_{\mathrm{sib}}{\sin \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y\\ +z_{\mathrm{sib}}{\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y-z_{\mathrm{sib}}\end{array}\right.---\left(17\right)$

Δ z wherein _pimfor autonomous mechanism arrives target vector n=(l, m, n) ^tthe time each sufficient driving amount, Δ x _pim, Δ y _pimthat each is sufficient in moving platform x, the compensation rate of y both direction.

After posture adjustment, cutter point can produce skew, for guaranteeing that point of a knife point is constant, need to provide cutter point X, Y after posture adjustment, Z adjustment amount.After below calculating posture adjustment, the side-play amount of cutter point.

Details are as follows:

Point of a knife point T, can be obtained by formula (11)

T _m=R _bm ^TT _b-R _bm ^TO _bO _m (18)

S _1m=R _bm ^TS _1b-R _bm ^TO _bO _m

Therefore can obtain

T _m-S _1m=R _bm ^T(T _b-S _1b) (19)

T _m0=T _b-(c,d,h) ^T (20)

S wherein _1b={ x _s1b, y _s1b, z _s1b} ^t={ 0,0,0} ^t, S _1m={ x _s1m, y _s1m, z _s1m} ^t={ c ,-d ,-h ₁} ^t

ΔT _m=T _m-T _m0=R _bm ^TT _b-T _b (21)

According to the known cutter point T(of autonomous mechanism model machine processing stand) absolute coordinate, be designated as T _b={ x _tb, y _tb, z _tb} ^t, can try to achieve the coordinate figure of cutter point at moving coordinate system by formula (21), can calculate thus the rear cutter point compensation rate under X, Y, Z direction moving coordinate system of raising wages, as shown in formula (22).

$\left\{\begin{array}{c}{\mathrm{\Δx}}_T=x_{\mathrm{Tb}}{\cos \mathrm{\φ}}_y+y_{\mathrm{Tb}}{\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y\\ -z_{\mathrm{Tb}}{\cos \mathrm{\φ}}_x{\sin \mathrm{\φ}}_x-x_{\mathrm{Tb}}\\ {\mathrm{\Δy}}_T=y_{\mathrm{Tb}}{\cos \mathrm{\φ}}_x+z_{\mathrm{Tb}}{\sin \mathrm{\φ}}_x-y_{\mathrm{Tb}}\\ {\mathrm{\Δz}}_T=x_{\mathrm{Tb}}{\sin \mathrm{\φ}}_y-y_{\mathrm{Tb}}{\sin \mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ +z_{\mathrm{Tb}}{\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y-z_{\mathrm{Tb}}\end{array}\right.---\left(22\right)$

Method involved in the present invention is vowed detection algorithm, motion posture adjustment algorithm, and the cutter compensation algorithm, all realize for the control system of autonomous mechanism, and accurate through the metrology and measurement center testing authentication of Shanghai City metrology and measurement research institute East China country.

The part that the present invention does not relate to all prior art that maybe can adopt same as the prior art is realized.

Claims (3)

1. an autonomous institutionalization is vowed and is detected and the posture adjustment movement technique, it is characterized in that it comprises the following steps:

At first, three or four laser range sensors are installed on autonomous mechanism end effector;

Secondly, utilize the value that three or four laser range sensors are surveyed to try to achieve surveyed drilling planar process arrow n;

The 3rd, calculate the drilling planar process of institute according to above-mentioned measurement and vow that n, by the anti-resolving Algorithm of posture adjustment campaign, obtains each leg driving amount that autonomous mechanism arrives drilling method arrow, realize the posture adjustment campaign for control system;

Finally, calculate the drilling planar process of institute according to above-mentioned measurement and vow that n is by the anti-resolving Algorithm of posture adjustment campaign, when obtaining autonomous mechanism and arriving the drilling method and vow, the side-play amount of cutter, realize the tool offset correction for control system.

2. method according to claim 1, is characterized in that definite method of described planar process arrow n comprises the planar process arrow n that utilizes three laser range sensors and the value of utilizing four laser range sensors to survey to determine the drilling position;

The planar process that the described value of utilizing three laser range sensors to survey is tried to achieve is vowed n:

n=(l,m,n) ^T=C ₁B ₁×B ₁A ₁ (1)

Wherein by vector correlation, can obtain vector C ₁b ₁as shown in Equation (2), note the direction of vector,

C ₁B ₁=C ₁C+CB+BB ₁

=(|C ₁C|-BB ₁|)·(0,0,1) ^T-|CB|·(0,1,0) ^T (2)

Can obtain vector B with the reason vector correlation ₁a ₁as shown in Equation (3), note the direction of vector

B ₁A ₁=B ₁B+BA+AA ₁

=(|B ₁B|-|AA ₁|)·(0,0,1) ^T+|BA|·(1,0,0) ^T (3)

The measured value that in formula (2), (3), parameter is each laser sensor, can try to achieve surveyed planar process by formula (2) (3) substitution formula (1) and vow n;

The planar process that the described value of utilizing four laser range sensors to survey is tried to achieve vows that n can solve cutter by formula (4) again

The normal vector of tool point, vow as surveyed planar process with the results averaged of formula (1), improved certainty of measurement.

(l,m,n) ^T=D ₁C ₁×D ₁A ₁

whereD ₁C ₁=(|D ₁D|-|C ₁C|)·(0,0,1) ^T-|CD|·(1,0,0) ^T (4)

D ₁A ₁=(|D ₁D|-|A ₁A|)·(0,0,1) ^T-|AD|·(0,1,0) ^T

If there is a deviation very large in A, B, C, D, illustrate that this place breaks away from measurement category, the data computing method that adopts its excess-three point to:

1. when the super scope of A point, with B, C, D, calculate, as formula (5)

(l,m,n) ^T=C ₁B ₁×C ₁D ₁ (5)

2. when the super scope of B point, with A, C, D, calculate, as formula (4)

3. when the super scope of C point, with A, B, D, calculate, as formula (6)

(l,m,n) ^T=A ₁D ₁×A ₁B ₁ (6)

4. when the super scope of D point, with A, B, C, calculate, as shown in Equation (1);

In above formula, the normal vector of establishing autonomous mechanism moving platform is parallel all the time with the drilling main shaft, note n ₀=(0,0,1) ^t, laser range sensor A, B, C, D, the vector direction of range finding slotted line is parallel with the drilling main shaft, is also n ₀=(0,0,1) ^t, distance measuring sensor ABCD plane parallel is in autonomous mechanism frame body, and BA is parallel with outside framework, and under the note original state, the BA direction is (1,0,0) ^t, BC is vertical with outside framework, and under the note original state, the BC direction is (0,1,0) ^t, | A ₁a|, | B ₁b|, | C ₁c|, | D ₁d| is the respective sensor measuring distance; Detect curved surface A ₁b ₁c ₁d ₁zone is similar to regards a plane as, and method is vowed to detect and surveyed curved surface A ₁b ₁c ₁d ₁normal vector, i.e. plane A ₁b ₁c ₁normal vector; The drilling method of surveying vow, also referred to as the posture adjustment goal method, vow, be designated as n=(l, m, n) ^t.

3. method according to claim 1, is characterized in that the anti-resolving Algorithm of described posture adjustment campaign refers to that autonomous mechanism is by n ₀=(0,0,1) ^tposture adjustment is to n=(l, m, n) ^tstate, anti-side-play amount of separating displacement and the cutter point of eight leg liftings;

Only have 3 frees degree during autonomous mechanism posture adjustment, wherein rotary freedom only has the rotation of A, B angle both direction, the pitching that is autonomous mechanism with sidewinder action, autonomous mechanism leg 2 only has a compensation rate that is parallel to the housing direction, therefore autonomous mechanism is by n ₀=(0,0,1) ^tposture adjustment is to n=(l, m, n) ^tprocess, also can think autonomous mechanism through B to pivot angle

a is to pivot angle

the pose arrived;

According to the Rotating Transition of Coordinate computing, moving coordinate system O _mwith position fixing be O _bcoordinate conversion matrix R _bmshown in (7); Because the Space Rotating matrix is orthogonal matrix, therefore R _bmthere is inverse matrix, and R _bm ^-1=R _bm ^t.

$\begin{array}{c}{R}_{\mathrm{bm}}=\left(\begin{array}{ccc}\cos \left(x_b{x}_m\right)& \cos \left(x_b{y}_m\right)& \cos \left(x_b{z}_m\right)\\ \cos \left(y_b{x}_m\right)& \cos \left(y_b{y}_m\right)& \cos \left(y_b{z}_m\right)\\ \cos \left(z_b{x}_m\right)& \cos \left(z_b{y}_m\right)& \cos \left(z_b{z}_m\right)\end{array}\right)\\ =\left(\begin{array}{ccc}{\cos \mathrm{\φ}}_y& 0& {\sin \mathrm{\φ}}_y\\ {\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y& {\cos \mathrm{\φ}}_x& {-\sin \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y\\ {-\cos \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y& {\sin \mathrm{\φ}}_x& {\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y\end{array}\right)\end{array}---\left(7\right)$

Wherein

Goal method is vowed n=(l, m, n) ^tby method arrow detection algorithm, measured, therefore by formula (8), can be obtained posture adjustment A, the B pivot angle of autonomous mechanism

can solve transition matrix R _bm;

Autonomous mechanism moving coordinate system, with respect to the position of position fixing system, is designated as

O _bO _m=(c,d,h ₁) ^T (9)

Any point D is position coordinates vector D at position fixing _bwith the position vector D at moving coordinate system _mpass is:

D _b=R _bmD _m+O _bO _m (10)

D _m=Rb _m ^TD _b-R _bm ^TO _bO _m (11)

S sets up an office _iat coordinate system O _b-x _by _bz _band O _m-x _my _mz _mcoordinate be respectively S _bi=(x _sib, y _sib, z _sib) ^tand S _mi=(x _sim, y _sim, z _sim) ^t;

S _im=R _bm ^TS _ib-R _bm ^TO _bO _m (12)

S _im-S _1m=R _bm ^T(S _ib-S _1b) (13)

S _im=R _bm ^T(S _ib-S _1b)+S _1m (14)

S _im0=S _ib-O _bO _m=S _ib-(c,d,h) ^T (15)

During posture adjustment, fixing whole 1 not lifting, by S _1b=(0,0,0) ^t; S _1m=(c ,-d ,-h ₁) ^tsubstitution formula (14), can obtain each sufficient side-play amount before and after posture adjustment and be

ΔS _im=S _im-S _im0=R _bm ^TS _ib-S ^ib (16)

Solving each sufficient input quantity and compensation rate after posture adjustment by formula (16) is

$\left\{\begin{array}{c}{\mathrm{\Δx}}_{\mathrm{pim}}=x_{\mathrm{sib}}{\cos \mathrm{\φ}}_y+y_{\mathrm{sib}}{\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y\\ -z_{\mathrm{sib}}{\cos \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y-x_{\mathrm{sib}}\\ \mathrm{\Δ}{y}_{\mathrm{pim}}=y_{\mathrm{sib}}{\cos \mathrm{\φ}}_x+z_{\mathrm{sib}}{\sin \mathrm{\φ}}_x-y_{\mathrm{sib}}\\ \mathrm{\Δ}{z}_{\mathrm{pim}}=x_{\mathrm{sib}}{\sin \mathrm{\φ}}_y-y_{\mathrm{sib}}{\sin \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y\\ +z_{\mathrm{sib}}{\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y-z_{\mathrm{sib}}\end{array}\right.---\left(17\right)$

Δ z wherein _pimfor autonomous mechanism arrives target vector n=(l, m, n) ^tthe time each sufficient driving amount, Δ x _pim, Δ y _pimthat each is sufficient in moving platform x, the compensation rate of y both direction;

After posture adjustment, cutter point can produce skew, for guaranteeing that point of a knife point is constant, need to provide cutter point X, Y after posture adjustment, Z adjustment amount:

Point of a knife point T, can be obtained by formula (11)

T _m=R _bm ^TT _b-R _bm ^TO _bO _m (18)S _1m=R _bm ^TS _1b-R _bm ^TO _bO _m

Therefore can obtain

T _m-S _1m=R _bm ^T(T _b-S _1b) (19)

T _m0=T _b-(c,d,h) ^T (20)

S wherein _1b={ x _s1b, y _s1b, z _s1b} ^t={ 0,0,0} ^t, S _1m={ x _s1m, y _s1m, z _s1m} ^t={ c ,-d ,-h ₁} ^t

ΔT _m=T _m-T _m0=R _bm ^TT _b-T _b (21)

Autonomous mechanism cutter point T absolute coordinate is designated as T _b={ x _tb, y _tb, z _tb} ^t, try to achieve the coordinate figure of cutter point at moving coordinate system by formula (21), can calculate thus cutter point compensation rate under X, Y, Z direction moving coordinate system after posture adjustment, as shown in formula (22).

$\left\{\begin{array}{c}{\mathrm{\Δx}}_T=x_{\mathrm{Tb}}{\cos \mathrm{\φ}}_y+y_{\mathrm{Tb}}{\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_y\\ -z_{\mathrm{Tb}}{\cos \mathrm{\φ}}_x{\sin \mathrm{\φ}}_x-x_{\mathrm{Tb}}\\ {\mathrm{\Δy}}_T=y_{\mathrm{Tb}}{\cos \mathrm{\φ}}_x+z_{\mathrm{Tb}}{\sin \mathrm{\φ}}_x-y_{\mathrm{Tb}}\\ {\mathrm{\Δz}}_T=x_{\mathrm{Tb}}{\sin \mathrm{\φ}}_y-y_{\mathrm{Tb}}{\sin \mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ +z_{\mathrm{Tb}}{\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y-z_{\mathrm{Tb}}\end{array}\right.---\left(22\right).$

Images