CN102564416B - System and method for reconstructing and positioning three-dimensional environment for mirror cleaning robot - Google Patents

Abstract

A system for reconstructing and positioning a three-dimensional environment for a mirror cleaning robot comprises a laser distance measuring sensor, a motor, an eccentric wheel, a swing bracket of a bracket, an absolute value encoder, an MEMS (Micro-electromechanical System) inertial sensor, a host computer and a mileage recorder, wherein the bracket is arranged on the mirror cleaning robot; the motor is arranged at the front end of the bracket; the eccentric wheel is connected with the motor, and is in transmission with the bracket so as to convert the rotation movement of the motor into the swing movement of the bracket; the laser distance measuring sensor is linked with the bracket and used for obtaining the laser scanning data between obstacles in a real-time manner; the absolute value encoder is arranged on the swing shaft of the laser distance measuring sensor and used for directly measuring the swing angle of the laser distance measuring sensor relative to a vehicle body; the MEMS inertial sensor is mounted on the swing bracket and used for measuring the horizontally rolling angle and the pitching angle of the vehicle body relative to the geographic horizontal plane when the vehicle body moves on uneven ground; and the host computer is connected with the laser distance measuring sensor, the absolute value encoder, the MEMS inertial sensor and the mileage recorder, and is used for receiving the data sent by the laser distance measuring sensor and the like, building a 3D outline drawing, extracting the coplanar data points from three-dimensional data points, and defining the position of a truss.

Description

A kind of mirror surface cleaning robot carries out the system and method for three-dimensionalreconstruction and location

Invention field

The present invention relates generally to solar energy thermal-power-generating field, be specifically related to the system and method for the reconstruct of a kind of mirror surface cleaning robot three-dimensional and location.

Background technology

Solar energy thermal-power-generating, is also named focus type solar energy generating (Concentrating Solar Power is called for short CSP), by a large amount of catoptrons, in the mode focusing on, sun power direct light is gathered together, heating working medium, the steam of generation High Temperature High Pressure, steam driven steam turbine power generation.

Solar energy mirror field is mainly comprised of with the superincumbent plane mirror of installation a large amount of framves that spreads out, and is the solar thermal utilization rate that improves land area of one unit, and the adjacent frame that spreads out is arranged compact.Mirror place the annual temperature difference in highlands larger, whole mirror Ground flatness may change.Mirror surface cleaning robot (or claiming minute surface car) is for the mirror surface cleaning of high altitude localities solar light-heat power-generation Jing Chang.

Minute surface car need to be walked in adjacent gap of spreading out between frame, and utilizes vehicle-mounted wiper mechanism to clean plane mirror.For guaranteeing the safety of mirror field facility, need a kind of more accurate targeting scheme to determine car body and the relative position relation of frame that spreads out, and then make reliable and stable the executing the task of minute surface car.

Traditional indoor and outdoor locator meams mainly contains: rely on the relative positioning of laser range sensor, rely on odometer dead reckoning location, by receiving the absolute fix of gps signal, also have the integrated navigation scheme that adopts above several modes.With upper type, have unique advantage separately, laser range sensor is applied to indoor positioning can obtain stable environment profile, and odometer is a kind of simple and effective locator meams, and GPS is the extremely strong solution of a kind of versatility especially.

Yet in the application of this project, above locator meams has been proposed to new challenge.GPS civil signal positioning precision still can not meet the requirement of this application, and the larger saltus step of GPS positioning signal existence, still unable to do what one wishes for frequency applications, the risk that exists in addition gps signal to lose.Odometer only provides the displacement variable of carrier working direction, for the road surface of poor flatness, can have larger error, the failure risk that has idle running and slide when application separately.2D laser range sensor is the range finding range that is limited to sensor itself in outdoor application one, the 2nd, and outdoor environment contour feature is not remarkable, and especially the environment of this project is the baroque frame that spreads out, and has greatly increased uncertainty.

In a word, due to complex environment structure in this application, and the pavement conditions of poor flatness, conventional locator meams all can not meet the demands, and needs a kind of special targeting scheme.

Summary of the invention

The system that the first object of the present invention is to provide a kind of mirror surface cleaning robot to carry out three-dimensionalreconstruction and location, to solve the technical matters that can not meet accuracy requirement in prior art in the pavement conditions location of poor flatness.

The method that the second object of the present invention is to provide a kind of mirror surface cleaning robot to carry out three-dimensionalreconstruction and location, to solve the technical matters that can not meet accuracy requirement in prior art in the pavement conditions location of poor flatness.

A kind of mirror surface cleaning robot carries out the system of three-dimensionalreconstruction and location, for being arranged on the headstock part of described mirror surface cleaning robot, it further comprises laser range sensor, motor, eccentric wheel, wave support, absolute value encoder, MEMS inertial sensor, host computer and odometer, wherein

Eccentric wheel is fixed on motor, and transmission waves on support described, for rotatablely moving of motor being converted into the oscillating motion of support;

Described laser range sensor, is fixedly connected with and waves support, for the oscillating motion campaign with waving support, and obtains laser scanning data;

Described absolute value encoder, is arranged on the swinging shaft of laser range sensor, for directly measuring the swing angle with respect to car body;

MEMS inertial sensor, for measuring car body roll angle and angle of pitch with respect to geographical surface level when the out-of-flatness ground motion;

Odometer: be arranged in mirror surface cleaning robot;

Host computer: connect respectively laser range sensor, absolute value encoder, MEMS inertial sensor and odometer, for receiving the transmission data that comprise laser range sensor transmission, build a 3D profile diagram, extract the coplanar data point in three-dimensional data points, determine the chord position that spreads out and put.

Described MEMS inertial sensor comprises gyroscope and accelerometer, and described host computer further comprises:

Car body attitude measurement computing unit, for according to the output of gyroscope and accelerometer, calculates the attitude of this carrier of mirror surface cleaning robot;

Car body attitude fusion calculation unit, be used for the attitude of this carrier of mirror surface cleaning robot, consider that again absolute value encoder obtains the angle of pitch α that waves The Cloud Terrace, finally calculate laser scanner with respect to roll angle γ and the pitching angle theta of the actual attitude of the geographical surface level in locality;

Laser data attitude is thrown projection modification computing unit, for calculating laser data point in projection coordinate's information of geographical surface level;

Laser data translation corrected Calculation unit, for universal measurement in the time interval increment of mileage gauge revise, and revised data be painted into one or three paint in coordinate system, form one scan point cloud chart;

Location Calculation unit, for locating the point presetting on this analyzing spot cloud atlas.

Mirror surface cleaning robot carries out a method for three-dimensionalreconstruction and location, comprising:

Receive the output of gyroscope and accelerometer, calculate the attitude of this carrier of mirror surface cleaning robot;

By the attitude of this carrier of mirror surface cleaning robot, then consider that absolute value encoder obtains the angle of pitch α that waves The Cloud Terrace, finally calculate laser scanner with respect to roll angle γ and the pitching angle theta of the actual attitude of the geographical surface level in locality;

Calculate laser data point in projection coordinate's information of geographical surface level;

The increment of mileage gauge is revised in the universal measurement time interval, and revised data are painted into one or three paint in coordinate system, forms one scan point cloud chart;

On this analyzing spot cloud atlas, locate the point presetting.

Compared with prior art, the invention provides the system that a kind of mirror surface cleaning robot carries out three-dimensionalreconstruction and location, system provided by the invention can, under the pavement conditions of complex environment structure and poor flatness, complete refactoring localization work.

Also have, the present invention can provide and utilize two-dimensional laser radar to carry out three-dimensionalreconstruction and positioning action, and cost is very low, and realizes conveniently.

Accompanying drawing explanation

Fig. 1 is the part system architecture front elevational schematic that a kind of mirror surface cleaning robot carries out three-dimensionalreconstruction and location;

Fig. 2 is the part system architecture schematic rear view that a kind of mirror surface cleaning robot carries out three-dimensionalreconstruction and location;

Fig. 3 is the part system architecture side view that a kind of mirror surface cleaning robot carries out three-dimensionalreconstruction and location;

Fig. 4 is the process flow diagram that a kind of mirror surface cleaning robot carries out three-dimensionalreconstruction and location;

Fig. 5 is an existing coordinate definition figure.

Embodiment

Below in conjunction with accompanying drawing, illustrate the present invention.

Refer to Fig. 1, Fig. 2, Fig. 3, a kind of mirror surface cleaning robot carries out the system of three-dimensionalreconstruction and location.For being arranged on the headstock part of described mirror surface cleaning robot, it further comprises laser range sensor 15, motor 11, eccentric wheel 13, waves support, absolute value encoder, MEMS inertial sensor, host computer and odometer, wherein,

Eccentric wheel 13 is fixed on motor 11, and transmission waving on support, for rotatablely moving of motor being converted into the oscillating motion of support.

Laser range sensor 15, is fixedly connected with and waves support, for the oscillating motion campaign with waving support, and obtains laser scanning data;

Described absolute value encoder, is arranged on the swinging shaft of laser range sensor 15, for directly with respect to car body swing angle;

MEMS inertial sensor, for measuring car body roll angle and angle of pitch with respect to geographical surface level when the out-of-flatness ground motion;

Odometer: be arranged in mirror surface cleaning robot;

Host computer: connect respectively laser range sensor 15, absolute value encoder, MEMS inertial sensor and odometer, for receiving the transmission data that comprise laser range sensor transmission, build a 3D profile diagram, extract the coplanar data point in three-dimensional data points, determine the chord position that spreads out and put.

Laser range sensor waves along waving fulcrum 17, waves fulcrum 17 and can be fixed in mirror surface cleaning robot.Wave support and at least comprise upper bracket 141 and lower carriage 142, upper bracket 141 and lower carriage 142 form the oblong-shaped of a hollow, eccentric wheel 13 is arranged in the rectangle of described hollow, eccentric wheel 13 drops on described lower carriage 142, the perpendicular lattice framing in a left side of lower carriage can be fixedly connected with laser range sensor 15 by fulcrum 18, the bottom rail lattice framing of lower carriage 142 is connected with perpendicular lattice framing, and eccentric wheel 13 moves in the semi-surrounding circle of lower carriage, drives the oscillating motion of lower carriage 142.In this example, also comprise a support bar, one end of support bar connects the lower end of the perpendicular lattice framing in the right side of upper bracket, and the other end is flexibly connected on laser range sensor.Like this, when eccentric wheel 13 moves in the semi-surrounding circle of lower carriage, at least can supported hold.Motor rotarily drives eccentric wheel, makes support produce reciprocal oscillating motion, and the relative angle of this oscillating motion records by absolute value encoder.Laser sensor is fixed on support, in a rolling period, (amplitude that support waves is approximately 20 ° to the two-dimensional scan frame of timing sampling some, this amplitude is adjustable by eccentric wheel mounting eccentric magnitude, in a rolling period every 2 ° of collections of carrying out a laser scanning data, one wave carry out back and forth 20 frame laser scannings).

Described eccentric diameter equals the height of the perpendicular lattice framing in a left side of described lower carriage in this example.Described laser range sensor is arranged on and waves on support or one end of motor.

Laser range sensor can adopt two-dimensional laser distance measuring sensor, to reduce cost, also can directly adopt three-dimensional laser distance measuring sensor.

In this example, described MEMS inertial sensor comprises gyroscope and accelerometer, and described host computer further comprises:

Car body attitude measurement computing unit, for according to the output of gyroscope and accelerometer, calculates the attitude of this carrier of mirror surface cleaning robot;

Car body attitude fusion calculation unit, be used for the attitude of this carrier of mirror surface cleaning robot, consider that again absolute value encoder obtains the angle of pitch α that waves The Cloud Terrace, finally calculate laser scanner with respect to roll angle γ and the pitching angle theta of the actual attitude of the geographical surface level in locality;

Laser data attitude is thrown projection modification computing unit, for calculating laser data point in projection coordinate's information of geographical surface level;

Laser data translation corrected Calculation unit, for universal measurement in the time interval increment of mileage gauge revise, and revised data be painted into one or three paint in coordinate system, form one scan point cloud chart;

Location Calculation unit, for locating the point presetting on this analyzing spot cloud atlas.

For above-mentioned system, a kind of mirror surface cleaning robot carries out the method (referring to Fig. 4) of three-dimensionalreconstruction and location, comprising:

First carry out S110: receive the output of gyroscope and accelerometer, calculate the attitude of this carrier of mirror surface cleaning robot. (car body attitude measurement)

Illustrate: by inertial sensor, carrying out attitude of carrier, to measure this algorithm be a ripe algorithm, and here we just repeat to realize and be applied in project.

Gyro data is carried out the renewal of Kalman filtering time; According to the motion state of the measured value judgement car of accelerometer, whether the result of judgement determines to utilize the measuring value of accelerometer to carry out Kalman filtering measurement renewal; After more than completing, according to filter status estimated result, resolve attitude.

(object: input angle speed (gyroscope survey) and acceleration (accelerometer measures), output carrier is with respect to the attitude of the geographical surface level in locality: the angle of pitch, roll angle.

For clarity, divide three parts to say below: 1. coordinate system and angle metric definition 2. are by acceleration measuring value judgement motion state 3. system equation 4. attitude algorithms)

1. definition (referring to Fig. 5)

Carrier coordinate system b: initial point o is positioned at carrier movement center, ox _baxle is right-hand towards carrier, oy _baxle is towards carrier dead ahead, oz _bwith ox _b, oy _baxle is according to right-hand rule form right angle coordinate system o-x _by _bz _b

Local geographic coordinate system n: initial point O is that initial point overlaps with b, OX axle is towards geographical east orientation, OY axle towards geographic north to, OZ and OY, OY axle are according to right-hand rule form right angle coordinate system O-XYZ

Carrier can be represented by pitching angle theta and roll angle γ with respect to the Space Angle position of geographic coordinate system, be defined as follows:

Pitching angle theta, the carrier longitudinal axis is with respect to the angle of local ground level.

Roll angle γ, longitudinal plane of symmetry of carrier is with respect to the angle of rotating by carrier longitudinal axis vertical guide.

2. motion state judgement

The motion state of carrier is divided into two kinds:

1) dereliction motivation is moving.Under this state, by acceleration measuring value, can determine the attitude of the relatively geographical surface level of carrier, the information that therefore can be used as a kind of redundancy is proofreaied and correct attitude of carrier;

2) there is active motor-driven.With contrary above, accelerometer measures can not be for determining attitude of carrier.

Judgement principle: when carrier does not have linear acceleration, carrier is that dereliction motivation is moving.Acceleration measuring value now $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T,$ Its mould value should be 1 local gravitational acceleration (9.8m/s ²).

If g is local gravitational acceleration value.

When time, think that carrier dereliction motivation is moving;

When time, think that carrier has initiatively motor-driven;

α wherein _afor judgment threshold, generally get and be no more than 5%.

3. system equation

Input quantity: angular speed measures ${\mathrm{\ω}}_{\mathrm{nb}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{nbx}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]}^T,$ Acceleration measures $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T.$ The state vector c that consideration is relevant with θ, γ ₃

$c_3=\left[\begin{array}{c}{c}_{31}\\ c_{32}\\ c_{33}\end{array}\right]=\left[\begin{array}{c}-\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\\ \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

And equivalent gyroscopic drift ${\widetilde{\mathrm{\ϵ}}}^b=\left[\begin{array}{c}{\mathrm{\ϵ}}_x^b\\ {\mathrm{\ϵ}}_y^b\\ {\mathrm{\ϵ}}_z^b\end{array}\right]$

Its kinetics equation is ^{[list of references]}

$\left[\begin{array}{c}{\stackrel{\·}{c}}_3\\ {\stackrel{\stackrel{\·}{~}}{\mathrm{\ϵ}}}^b\end{array}\right]=\left[\begin{array}{cc}-{\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)}_{\×}& -{\left(c_3\right)}_{\×}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{c}_3\\ {\widetilde{\mathrm{\ϵ}}}^b\end{array}\right]+\left[\begin{array}{c}-{\left(c_3\right)}_{\×}\\ 0_{3\×3}\end{array}\right]w_k^b---\left[1\right]$

Wherein $w_k^n={\left[\begin{array}{ccc}{w}_{\mathrm{kx}}^b& w_{\mathrm{ky}}^b& w_{\mathrm{kz}}^b\end{array}\right]}^T$ System noise sequence, approximate satisfied (expectation), (covariance/variance), Q _kfor gyroscope noise sequence variance battle array.

${\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)}_{\×}=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{\mathrm{nbz}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b& 0& -{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ -{\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]$

${\left(c_3\right)}_{\×}=\left[\begin{array}{ccc}0& -c_{33}& c_{32}\\ c_{33}& 0& -c_{31}\\ -c_{32}& c_{31}& 0\end{array}\right]$

Above equation is arranged to order

$X=\left[\begin{array}{c}{c}_3\\ {\widetilde{\mathrm{\ϵ}}}^b\end{array}\right],$ $F=\left[\begin{array}{cc}-{\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)}_{\×}& -{\left(c_3\right)}_{\×}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],$ $G=\left[\begin{array}{c}-{\left(c_3\right)}_{\×}\\ 0_{3\×3}\end{array}\right]$

Formula [1] can be write as

$\stackrel{\·}{X}=\mathrm{FX}+{\mathrm{Gw}}^b---\left[2\right]$

When the motion state of carrier is dereliction motivation when moving, acceleration measuring value f ^bmeet relational expression

$\frac{f^b}{g}=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{c}_3\\ {\widetilde{\mathrm{\ϵ}}}^b\end{array}\right]+v_k^b---\left[3\right]$

Wherein g is local gravitational acceleration, general desirable 9.8m/s ². $v_k^b={\left[\begin{array}{ccc}{v}_{\mathrm{kx}}^b& v_{\mathrm{ky}}^b& v_{\mathrm{kz}}^b\end{array}\right]}^T$ To measure noise sequence, approximate satisfied (expectation), (covariance/variance), R _kfor accelerometer noise sequence variance battle array.

Arrange above formula, order

$Z=\frac{f^b}{g},H=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\end{array}\right]$

Formula [3] can be write as

Z＝HX+v ^b.......................................................[4]

To the system being formed by [2] and [4], adopt Kalman filtering filtering algorithm to carry out iterative computation.Kalman filtering filtering algorithm comprises that the time upgrades and two steps are upgraded in measurement, at each iteration cycle gyroscope measuring value, is used for carrying out time renewal; The result of motion state judgement is for determining whether measure renewal with accelerometer measuring value, if carrier is initiatively motion state of nothing, carries out normal measurement and upgrades, otherwise, directly utilize the result of time renewal as the result measuring after upgrading.

4. attitude algorithm

The real-time estimated state amount of Kalman filtering filtering algorithm $X=\left[\begin{array}{c}{c}_3\\ {\widetilde{\mathrm{\ϵ}}}^b\end{array}\right]$ Value, pass through c ₃resolve attitude angle

θ＝arcsin(c ₃₂)

${\mathrm{\γ}}_{\mathrm{main}}=\arctan (-\frac{c_{31}}{c_{33}})$

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_{\mathrm{main}},& c_{33}\&GreaterEqual;0\\ {\mathrm{\&gamma;}}_{\mathrm{main}}-\mathrm{\&pi;},& c_{33}<0,{\mathrm{\&gamma;}}_{\mathrm{main}}>0\\ {\mathrm{\&gamma;}}_{\mathrm{main}}+\mathrm{\&pi;},& c_{33}<0,{\mathrm{\&gamma;}}_{\mathrm{main}}<0\end{array}\right.$

Upper part can list of references [1] Henrik Rehbinder, Xiaoming Hu.Drift-free attitude estimation for accelerated rigid bodies.Automatic, Volume 40, and Issue 4, April 2004, Pages 653-659

Then carry out S120: by the attitude of this carrier of mirror surface cleaning robot, consider that again absolute value encoder obtains the angle of pitch α that waves The Cloud Terrace, finally calculate laser scanner with respect to roll angle γ and the pitching angle theta (attitude fusion) of the actual attitude of the geographical surface level in locality.

Attitude during each frame data that laser range sensor is measured is comprised of two parts: the attitude of car body and be fixed on the tilter attitude of car body, algorithm by has above obtained respectively this two-part attitude parameter, respectively carrier with respect to the attitude parameter of the geographical surface level in locality: pitching angle theta and roll angle γ, with by absolute value encoder, obtained the angle of pitch α that waves The Cloud Terrace, now laser scanner with respect to the computing formula of the actual attitude of the geographical surface level in locality is

Roll angle γ, consistent with carrier roll angle, because wave The Cloud Terrace, only in pitch orientation, there is oscillating motion, do not affect roll angle

Angle of pitch β: sin β=sin α cos γ+θ, β=arcsin (sin α cos γ+θ) (letter definition is shown in Fig. 5 and above-mentioned explanation)

Carry out again S130: calculate laser data point in projection coordinate's information (being laser data attitude projection modification) of geographical surface level.

From navigation system, being the local n of Department of Geography realizes by the rotation around three axles to the conversion of carrier coordinate system b

The result of rotating for three times represents with direction cosine matrix, remembers that navigation is the direction cosine matrix to carrier system have

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&beta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&beta;}\\ 0& \cos \mathrm{\&beta;}& \sin \mathrm{\&beta;}\\ \sin \mathrm{\&gamma;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&beta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&beta;}\end{array}\right]$

For each the angle-distance in laser data to (a _idist _i), first with formula below, be transformed into rectangular coordinate system coordinate

$\left\{\begin{array}{c}{x}_i^b={\mathrm{dist}}_i\&CenterDot;\sin a_1\\ y_i^b={\mathrm{dist}}_i\&CenterDot;\cos a_i\end{array}\right.$

After increase z component 0 value, be multiplied by the contrary of direction cosine matrix and obtain laser data point in the projection coordinate of geographical surface level

$\left[\begin{array}{c}{x}_j^n\\ y_j^n\\ z_j^n\end{array}\right]={\left(C_n^b\right)}^{-1}\left[\begin{array}{c}{x}_i^b\\ y_i^b\\ 0\end{array}\right]$

Carry out subsequently S140: the increment of mileage gauge is revised in the universal measurement time interval, and revised data are painted into one or three paint in coordinate system, form one scan point cloud chart (laser data translation correction)

Within a measuring period, when recording frame by frame laser data, car body is likely also subjected to displacement, and this just causes the laser data of record not in same position, need to carry out translation correction by the increment of mileage gauge in measuring intervals of TIME.

If measurement attitude corresponding to two frame laser data is respectively (β ₁γ ₁) and (β ₂γ ₂), between two frames the increment of odometer be (δ x δ y δ is a).

Do one and simplify processing, mileage increment is projected to geographical surface level according to the laser attitude of former frame:

$\left[\begin{array}{c}\mathrm{\&delta;}{x}^n\\ \mathrm{\&delta;}{y}^n\\ *\end{array}\right]={\left(C_n^{b1}\right)}^{-1}\left[\begin{array}{c}\mathrm{\&delta;x}\\ \mathrm{\&delta;y}\\ 0\end{array}\right]$

Then by a rear frame laser data according to parameter (δ x ⁿδ y ⁿδ a) carries out translation and rotation, is calculated as follows

$\left[\begin{array}{c}{x}_i^{n0}\\ y_i^{n0}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\&delta;a}& \sin \mathrm{\&delta;a}\\ -\sin \mathrm{\&delta;a}& \cos \mathrm{\&delta;a}\end{array}\right]\left[\begin{array}{c}{x}_i^n\\ y_i^n\end{array}\right]+\left[\begin{array}{c}-\mathrm{\&delta;}{x}^n\\ -\mathrm{\&delta;}{y}^n\end{array}\right]$

Suppose to obtain altogether N frame laser data in the measuring period, agreement, according to the processing of above-mentioned two steps, snaps to each frame laser data: initial point is positioned at the initial point of the 1st frame laser data, and projects to local level.

Processing through above algorithm steps, each frame laser data within a measuring period is aimed on room and time, these revised data are painted in same three-dimensional system of coordinate, and such width analyzing spot cloud atlas is the 3D profile of describe environment completely just.

Finally carry out S150: on this analyzing spot cloud atlas, locate the point presetting.

Obtaining on the basis of environment 3D profile, although consider the frame complex structure that spreads out, the profile of laser scanning has very large uncertainty, but the shelf structure that spreads out still has very multilevel feature on 3d space, on 3D profile cloud atlas, show as some coplanar data points, adopt Hough transformation to extract the coplanar data point in three-dimensional data points, result is carried out to determine the chord position that spreads out after the operations such as cluster and put, by this notable feature, can determine the relative position relation of spread out frame and laser range sensor.

This part comprises: on 3D profile cloud atlas, extract the angle that characteristic plane 2. is determined carrier Yan Jia position, relative both sides and carrier forward direction and the frame direction of spreading out 1..

1. extract characteristic plane

Space plane equation:

z＝a _xx+a _yy+d

For the 3D profile cloud atlas building, to each data point [x wherein _iy _iz _i] ^tcarry out Hough transformation, a space plane in the corresponding parameter space of each data point.In 3D cloud atlas, there is N point, in parameter space, just there is N plane, these Plane intersects and a point (can just in time not intersect at a point, but relatively concentrate on a less region), the Hough inverse transformation of this point is with regard to the plane of coplanar data point composition in corresponding 3D cloud atlas.

Upper some algorithm can list of references:

[1]http://en.wikipedia.org/wiki/Hough_transform

[2] http://homepages.inf.ed.ac.uk/rbf/HIPR2/hough.htm

[3]Vosselman，G.，Dijkman，S.3D?Building?Model?Reconstruction?from?Point?Clouds?and?Ground?Plans.?International?Archives?of?the?Photogrammetry，Remote?Sensing?and?Spatial?Information?Sciences，vol34，part?3/W4，October?22?24，2001，Annapolis，MA，USA，pp.37-44.

2. Relatively orientation

If obtain representing the spread out equation of frame plane of a side from previous step:

z＝a _xx+a _yy+d

Laser center formula below plan range basis is calculated:

$\mathrm{dis}\tan \mathrm{ce}=\frac{\left|d\right|}{\sqrt{a_x^2+a_y^2+1}}$

Carrier forward direction and the angle that spreads out between frame plane calculate by formula below:

Disclosed is above only several specific embodiments of the application, but the application is not limited thereto, and the changes that any person skilled in the art can think of, all should drop in the application's protection domain.

Claims (2)

1.Yi Zhong mirror surface cleaning robot carries out the system of three-dimensionalreconstruction and location, it is characterized in that, comprise car body attitude measurement computing unit, car body attitude fusion calculation unit, laser data attitude projection modification computing unit, laser data translation corrected Calculation unit and location Calculation unit; Wherein

Described car body attitude measurement computing unit: for according to the output of gyroscope and accelerometer, calculate the attitude of this carrier of mirror surface cleaning robot;

Described car body attitude fusion calculation unit: for by the attitude of this carrier of mirror surface cleaning robot, consider that again absolute value encoder obtains the angle of pitch that waves The Cloud Terrace, finally calculate laser scanner with respect to roll angle and the angle of pitch of the actual attitude of the geographical surface level in locality;

Described laser data attitude is thrown projection modification computing unit: for calculating laser data point in projection coordinate's information of geographical surface level;

Described laser data translation corrected Calculation unit: for universal measurement in the time interval increment of mileage gauge revise, and revised data are painted in a three-dimensional system of coordinate, form one scan point cloud chart;

Described location Calculation unit: for extract characteristic plane on this analyzing spot cloud atlas, determine carrier Yan Jia position, relative both sides and carrier forward direction and the angle of the frame direction of spreading out.

2.Yi Zhong mirror surface cleaning robot carries out the method for three-dimensionalreconstruction and location, it is characterized in that, comprising:

Receive the output of gyroscope and accelerometer, calculate the attitude of this carrier of mirror surface cleaning robot;

By the attitude of this carrier of mirror surface cleaning robot, then consider that absolute value encoder obtains the angle of pitch that waves The Cloud Terrace, finally calculate laser scanner with respect to roll angle and the angle of pitch of the actual attitude of the geographical surface level in locality;

Calculate laser data point in projection coordinate's information of geographical surface level;

In the universal measurement time interval, the increment of mileage gauge is revised, and revised data are painted in a three-dimensional system of coordinate, forms one scan point cloud chart;

On this analyzing spot cloud atlas, extract characteristic plane; Determine carrier Yan Jia position, relative both sides and carrier forward direction and the angle of the frame direction of spreading out.