CN103447877B - Autonomous institutionalization is vowed and is detected and posture adjustment movement technique - Google Patents

Abstract

A kind of autonomous institutionalization is vowed and is detected and posture adjustment movement technique, it is characterized in that it comprises the following steps: first, autonomous mechanism end actuator is provided with three or four laser range sensors; Secondly, the value utilizing three or four laser range sensors to survey tries to achieve surveyed drilling planar process arrow n; 3rd, according to above-mentioned survey calculation drilling planar process vow that n is by the anti-resolving Algorithm of posture adjustment campaign, obtain autonomous mechanism and arrive each leg drive volume that drilling method vows, realize posture adjustment campaign for control system; Finally, according to above-mentioned survey calculation drilling planar process vow that n is by the anti-resolving Algorithm of posture adjustment campaign, obtain autonomous mechanism and arrive drilling method when vowing, the side-play amount of cutter, realizes tool offset for control system and corrects.The inventive method is simple, is easy to control, and control accuracy is high, efficiently fast.

Description

Autonomous institutionalization is vowed and is detected and posture adjustment movement technique

Technical field

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

Background technology

At present, the main method of attachment that aircaft configuration adopts is mechanical connection, nearly 1,500,000 ~ 2,000,000 rivets and bolts on a frame large aircraft, in order to meet the requirement of present generation aircraft high life, first will ensure the safety and reliability be mechanically connected.In recent years, for ensureing transporation by plane quality, improve the fatigue life of body, realizing the low cost in producing in enormous quantities and high efficiency, is have employed Automated assembly technology in a large number in the Modern New large aircraft assembling process of representative with B787, A380, C-17 etc.The development of aircraft automatic assembly system is mainly both direction.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 to batch compared with 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.

Be utilize industrial machine arm general on market to be apparatus body based on industrial machine arm automatic setup system, coordinate the automatic assembly system that corresponding end effector forms.It has housing construction maturation, and integration realization is comparatively easy, and the advantage such as work flexible, but there is the shortcoming that scope of activities is less, structure is comparatively large and mobility is poor.Based on the automatic assembly system of flexible rail, then take flexible rail as platform, be adsorbed on the surface, mix corresponding end effector, complete the work such as automation drilling, the flexible rail multiaxis machine tool disclosed in the Chinese invention patent CN200580025525.X of Boeing's application and method.But also there is the shortcoming of the comparatively large and poor mobility of structure in this automatic assembly system based on flexible rail, the shortcomings such as especially, this system needs other attachment rail, more to the requirement of frock, and the early-stage preparations time that works is longer.

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

Summary of the invention

The object of the invention is, for existing eight problems that sufficient autonomous mechanism locates and mobile difficulty is large, 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 posture adjustment movement technique, it is characterized in that it comprises the following steps:

First, autonomous mechanism end actuator is provided with three or four laser range sensors;

Secondly, the value utilizing three or four laser range sensors to survey tries to achieve surveyed drilling planar process arrow n;

3rd, according to above-mentioned survey calculation drilling planar process vow that n is by the anti-resolving Algorithm of posture adjustment campaign, obtain autonomous mechanism and arrive each leg drive volume that drilling method vows, realize posture adjustment campaign for control system;

Finally, according to above-mentioned survey calculation drilling planar process vow that n is by the anti-resolving Algorithm of posture adjustment campaign, obtain autonomous mechanism and arrive drilling method when vowing, the side-play amount of cutter, realizes tool offset for control system and corrects.

Described planar process is vowed that the defining method of n comprises and is utilized three laser range sensors to vow n with the planar process of the value determination drilling position utilizing four laser range sensors to survey;

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

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

Wherein can obtain vector C by vector correlation ₁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)

Vector B can be obtained with reason vector correlation ₁a ₁as shown in Equation (3), the direction of vector is noted

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

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

In formula (2), (3), parameter is the measured value of each laser sensor, formula (2) (3) is substituted into formula (1) and can try to achieve surveyed planar process arrow n;

The planar process that the described value 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, vows as surveyed planar process with the results averaged of formula (1), improves 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 departs from measurement category (as measured on edge or there is cavity in somewhere), then adopt the data computing method of its excess-three point to:

1. when A point over range, then calculate, as formula (5) with B, C, D

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

2. when B point over range, then calculate, as formula (4) with A, C, D

3. when C point over range, then calculate, as formula (6) with A, B, D

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

4. when D point over range, then calculate with A, B, C, as shown in Equation (1);

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

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

3 frees degree are only had during the posture adjustment of autonomous mechanism, wherein rotary freedom only has the rotation of A, B angle both direction, the pitching being autonomous mechanism with sidewinder action, the compensation rate that autonomous mechanism leg 2 only has to be parallel to 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 Rotating Transition of Coordinate computing, moving coordinate system O _mwith position fixing system O _bcoordinate conversion matrix R _bmshown in (7); Because 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 vows n=(l, m, n) ^tvow that detection algorithm is measured by method, therefore can be obtained posture adjustment A, B pivot angle of autonomous mechanism by formula (8) transition matrix R can be solved _bm;

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

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

Any point D is at position fixing system position coordinates vector D _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)

Set up an office S _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 is not elevated, 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 is

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

After solving posture adjustment by formula (16), each sufficient input quantity and compensation rate are

$\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)$

Wherein Δ z _pimfor autonomous mechanism arrives target vector n=(l, m, n) ^ttime each sufficient drive volume, Δ x _pim, Δ y _pimthe compensation rate of each foot in moving platform x, y both direction;

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

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)

Wherein S _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(processing stand) absolute coordinate is designated as T _b={ x _tb, y _tb, z _tb} ^t, try to achieve the coordinate value of cutter point at moving coordinate system by formula (21), cutter point compensation rate under X, Y, Z-direction moving coordinate system after posture adjustment can be calculated thus, 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, by the measured value of four laser range sensors be arranged on autonomous mechanism end actuator, can measure autonomous mechanism platform coordinate system lower survey the drilling method arrowhead amount in skin-surface region.Vow according to the drilling method in surveyed skin-surface region again, use the anti-resolving Algorithm of posture adjustment campaign, each leg drive volume that autonomous mechanism arrives drilling method arrow can be solved easily, realize posture adjustment campaign for control system.Vow according to the drilling method in surveyed skin-surface region simultaneously, use the anti-resolving Algorithm of posture adjustment campaign, when can also solve autonomous mechanism arrival drilling method arrow easily, the side-play amount of cutter, realizes tool offset for control system and corrects.Therefore, there is control accuracy high, efficiently advantage fast.

Accompanying drawing explanation

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

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

Fig. 3 is that the method be made up of many laser range sensors of the present invention vows detection system schematic diagram.

Fig. 4 is autonomous mechanism sketch 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 are four laser range sensors, and AB is parallel to housing 1, AD perpendicular to housing 1.

In Fig. 3: T is 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, moving sets position is designated as P _i(i=1,2 ..., 8), R _i1, R _i2for two eccentric rotary.

Detailed description of the invention

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

As Figure 1-5.

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

First, autonomous mechanism end actuator is provided with three or four laser range sensors;

Secondly, the value utilizing three or four laser range sensors to survey tries to achieve surveyed drilling planar process arrow n;

3rd, according to above-mentioned survey calculation drilling planar process vow that n is by the anti-resolving Algorithm of posture adjustment campaign, obtain autonomous mechanism and arrive each leg drive volume that drilling method vows, realize posture adjustment campaign for control system;

Finally, according to above-mentioned survey calculation drilling planar process vow that n is by the anti-resolving Algorithm of posture adjustment campaign, obtain autonomous mechanism and arrive drilling method when vowing, the side-play amount of cutter, realizes tool offset for control system and corrects.

Details are as follows:

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

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

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

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

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

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

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

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

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

Vector B can be obtained with reason vector correlation ₁a ₁as shown in Equation (3), the direction of vector is noted

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

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

In formula (2) (3), parameter is the measured value of a laser sensor, formula (2) (3) is substituted into formula (1) and can try to achieve surveyed planar process arrow n.

Method of the present invention vows that detection system is by four laser range sensors A, B, C and D, and under providing four-sensor below, the institute of trying to achieve surveys planar process arrow n

If there be the 4th rangefinder D, the normal vector of cutter point can be solved again by formula (4), vow as surveyed planar process with the results averaged of formula (1), improve 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 departs from measurement category (as measured on edge or there is cavity in somewhere), then adopt the data computing method of its excess-three point to.

1. when A point over range, then calculate, as formula (5) with B, C, D

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

2. when B point over range, then calculate, as formula (4) with A, C, D

3. when C point over range, then calculate, as formula (6) with A, B, D

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

4. when D point over range, then calculate, as formula (1) with A, B, C

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

As shown in Fig. 4, Fig. 5.

In normal direction posture adjustment process, autonomous mechanism eight legs suck, and housing 1 is locked relative fixing with inside casing 2, and 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 the side chain connecting dynamic fixed platform, and the moving sets of eight legs is secondary for driving.Move back leg elongation by eight realize the pitching of autonomous mechanism and sidewinder, thus complete posture adjustment action, the drilling method of autonomous mechanism is vowed and vows that n overlaps with goal method, in posture adjustment process, the side-play amount that each leg 3 end produces 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 namely fixed on the coordinate system on covering, initial point O _bbe positioned at S ₁on, 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 _bthe right-handed coordinate system determined, namely 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 namely fixed on the coordinate system of autonomous mechanism, initial point O _mfor P ₁, z _mparallel with the lifting direction of leg, x _mall the time housing BA direction is parallel to, y _ball the time BC direction is parallel to.Under original state, moving coordinate system is parallel with position fixing system.

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 anti-displacement separating eight leg liftings.

Only have 3 frees degree during the posture adjustment of autonomous mechanism, 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, the compensation rate that autonomous mechanism leg 2 only has to be parallel to 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 Rotating Transition of Coordinate computing, moving coordinate system O _mwith position fixing system O _bcoordinate conversion matrix R _bmshown in (7).Because 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 vows n=(l, m, n) ^tvow that detection algorithm is measured by method, therefore can be obtained posture adjustment A, B pivot angle of autonomous mechanism by formula (8) transition matrix R can be solved _bm

Solve each leg drive volume of posture adjustment and compensation rate, details are as follows:

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

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

Any point D is at position fixing system position coordinates vector D _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)

Set up an office S _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 is not generally elevated, 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 is

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

After solving posture adjustment by formula (16), each sufficient input quantity and compensation rate are

$\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)$

Wherein Δ z _pimfor autonomous mechanism arrives target vector n=(l, m, n) ^ttime each sufficient drive volume, Δ x _pim, Δ y _pimthe compensation rate of each foot in moving platform x, y both direction.

After posture adjustment, cutter point can produce skew, for ensureing that point of a knife point is constant, needs to provide cutter point X, Y, Z adjustment amount after posture adjustment.After calculating posture adjustment below, 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)

Wherein S _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 autonomous mechanism model machine known cutter point T(processing stand) absolute coordinate, be designated as T _b={ x _tb, y _tb, z _tb} ^t, the coordinate value of cutter point at moving coordinate system can be tried to achieve by formula (21), cutter point compensation rate under X, Y, Z-direction moving coordinate system after raising wages can be calculated thus, 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 vows detection algorithm, and motion posture adjustment algorithm, cutter compensation algorithm, the control system all for autonomous mechanism realizes, and accurate through the testing authentication of East China national measurement test center of Shanghai City metrology and measurement research institute.

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

Claims (1)

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

First, autonomous mechanism end actuator is provided with three or four laser range sensors;

Secondly, the value utilizing three or four laser range sensors to survey tries to achieve surveyed drilling planar process arrow n;

3rd, vow that n is by the anti-resolving Algorithm of posture adjustment campaign according to above-mentioned survey calculation gained drilling planar process, obtain the drive volume that each leg of autonomous mechanism arrives drilling method arrow, realize posture adjustment campaign for control system;

Finally, according to above-mentioned survey calculation drilling planar process vow that n is by the anti-resolving Algorithm of posture adjustment campaign, obtain autonomous mechanism and arrive drilling method when vowing, the side-play amount of cutter, realizes tool offset for control system and corrects;

Described planar process is vowed that the defining method of n comprises and is utilized the planar process of three laser range sensors or the value determination drilling position that utilizes four laser range sensors to survey to vow n; A, B, C, D are four laser range sensors, A ₁, B ₁, C ₁, D ₁for the measurement point of each laser range sensor;

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

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

Wherein can obtain vector C by vector correlation ₁b ₁as shown in formula (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)

Vector B can be obtained with reason vector correlation ₁a ₁as shown in formula (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)

In formula (2), (3), parameter is the measured value of each laser sensor, formula (2) (3) is substituted into formula (1) and can try to achieve surveyed planar process arrow n;

The planar process that the described value utilizing four laser range sensors to survey is tried to achieve vows that n can be solved the normal vector of cutter point again by formula (4), vows, improve certainty of measurement with the results averaged of formula (1) as surveyed planar process;

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

Wherein, D ₁c ₁=(| D ₁d|-|C ₁c|) (0,0,1) ^t-| CD| (1,0,0) ^t

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 departs from measurement category, then adopt the data computing method of its excess-three point to:

1. when A point departs from measurement category, then calculate, as formula (5) with B, C, D

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

2. when B point departs from measurement category, then calculate, as formula (4) with A, C, D

3. when C point departs from measurement category, then calculate, as formula (6) with A, B, D

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

4. when D point departs from measurement category, then calculate, as shown in formula (1) with A, B, C;

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