CN102564416A - 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 the solar energy thermal-power-generating field, be specifically related to the system and method for 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); The sun power direct light is gathered together with the mode that focuses on through a large amount of catoptrons; Heating working medium, the steam of generation HTHP, steam driven steam turbine power generation.

The solar energy mirror field mainly is made up 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.The annual temperature difference in the highlands at Jing Chang place is bigger, and whole mirror field ground flat degree may change.Mirror surface cleaning robot (or claiming the minute surface car) is used for the mirror surface cleaning of high altitude localities solar light-heat power-generation Jing Chang.

The minute surface car need be walked in adjacent gap of spreading out between the frame, and utilizes vehicle-mounted wiper mechanism cleaning plane mirror.For guaranteeing the safety of mirror field facility, need a kind of more accurate targeting scheme to confirm 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, through receiving the absolute fix of gps signal, the integrated navigation scheme of several kinds of modes more than the employing is arranged also.With upper type unique advantage is separately arranged all, laser range sensor is applied to indoor positioning can obtain stable environment profile, and odometer is a kind of simple and effective locator meams, the extremely strong solution of a kind of especially versatility of GPS.

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

In a word, because complex environment structure in should using, and the pavement conditions of poor flatness, locator meams commonly used all can not meet the demands, and needs a kind of special targeting scheme.

Summary of the invention

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

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

A kind of mirror surface cleaning robot carries out the system of three-dimensionalreconstruction and location; Be used to be installed in the headstock part of said 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 the motor, and transmission waves on the support said, is used for rotatablely moving of motor is converted into the oscillating motion of support;

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

Said absolute value encoder is arranged on waving on the axle of laser range sensor, is used for directly measuring the angle of waving with respect to car body;

The MEMS inertial sensor is used to measure car body roll angle and angle of pitch with respect to geographical surface level when the out-of-flatness ground motion;

Odometer: be installed in the mirror surface cleaning robot;

Host computer: connect laser range sensor, absolute value encoder, MEMS inertial sensor and odometer respectively; Be used to receive the transmission data that comprise the laser range sensor transmission; Make up a 3D profile diagram, extract the coplane data point in the three-dimensional data points, determine the chord position that spreads out and put.

Said MEMS inertial sensor comprises gyroscope and accelerometer, and said host computer further comprises:

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

The car body attitude merges computing unit; Be used for attitude with 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 roll angle γ and the pitching angle theta of laser scanner with respect to the actual attitude of the geographical surface level in locality;

The laser data attitude is thrown projection corrected Calculation unit, is used to calculate the projection coordinate information of laser data point at geographical surface level;

Laser data translation corrected Calculation unit is used in the universal measurement time interval increment of mileage gauge and revises, and revised data are painted into one or three paint in the coordinate system, forms the one scan point cloud chart;

The location Calculation unit is used on this analyzing spot cloud atlas, locating the point that presets.

A kind of mirror surface cleaning robot carries out the 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;

With 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 roll angle γ and the pitching angle theta of laser scanner with respect to the actual attitude of the geographical surface level in locality;

Calculate the projection coordinate information of laser data point at 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 the coordinate system, forms the one scan point cloud chart;

On this analyzing spot cloud atlas, locate the point that presets.

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

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

Description of drawings

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

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

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

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

Fig. 5 is an existing coordinate definition figure.

Embodiment

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

See also Fig. 1, Fig. 2, Fig. 3, a kind of mirror surface cleaning robot carries out the system of three-dimensionalreconstruction and location.Be used to be installed in the headstock part of said 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 the motor 11, and transmission waving on the support, is used for rotatablely moving of motor is converted into the oscillating motion of support.

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

Said absolute value encoder is arranged on waving on the axle of laser range sensor 15, is used for directly waving angle with respect to car body;

The MEMS inertial sensor is used to measure car body roll angle and angle of pitch with respect to geographical surface level when the out-of-flatness ground motion;

Odometer: be installed in the mirror surface cleaning robot;

Host computer: connect laser range sensor 15, absolute value encoder, MEMS inertial sensor and odometer respectively; Be used to receive the transmission data that comprise the laser range sensor transmission; Make up a 3D profile diagram, extract the coplane data point in the 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 the mirror surface cleaning robot.Wave support and comprise upper bracket 141 and lower carriage 142 at least; The oblong-shaped of upper bracket 141 and lower carriage 142 formations one hollow, eccentric wheel 13 is arranged in the rectangle of said hollow, and eccentric wheel 13 drops on the said lower carriage 142; The perpendicular lattice framing in the left side of lower carriage can be fixedly connected with laser range sensor 15 through fulcrum 18; The following horizontal 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.Also comprise a support bar in this instance, an end of support bar connects the lower end of the perpendicular lattice framing in the right side of upper bracket, and the other end flexibly connects on the laser range sensor.Like this, when eccentric wheel 13 moves in the semi-surrounding circle of lower carriage, at least can supported hold.Motor rotating band movable eccentric wheel makes support produce reciprocal oscillating motion, and the relative angle of this oscillating motion records through absolute value encoder.Laser sensor is fixed on support; (amplitude that support waves is approximately 20 ° to the two-dimensional scan frame of timing sampling some in a rolling period; This amplitude is adjustable through eccentric wheel setting-up eccentricity amount; Every at a distance from 2 ° of collections of carrying out a laser scanning data in a rolling period, promptly one wave carry out 20 frame laser scannings back and forth).

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

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

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

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

The car body attitude merges computing unit; Be used for attitude with 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 roll angle γ and the pitching angle theta of laser scanner with respect to the actual attitude of the geographical surface level in locality;

The laser data attitude is thrown projection corrected Calculation unit, is used to calculate the projection coordinate information of laser data point at geographical surface level;

Laser data translation corrected Calculation unit is used in the universal measurement time interval increment of mileage gauge and revises, and revised data are painted into one or three paint in the coordinate system, forms the one scan point cloud chart;

The location Calculation unit is used on this analyzing spot cloud atlas, locating the point that presets.

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

At 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)

Explain: carrying out this algorithm of attitude of carrier measurement through inertial sensor is the algorithm of a maturation, and here we just repeat to realize and be applied in the project.

Gyro data is carried out the Kalman filtering time and is upgraded; Judge according to the measured value of accelerometer whether the motion state of car, result's decision of judgement are utilized the measuring value of accelerometer to carry out Kalman filtering and measured and upgrade; Resolve attitude according to the filter status estimated result after more than accomplishing.

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

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

1. definition (seeing also 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, and the OX axle is towards geographical east orientation, the 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, defines as follows:

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

Roll angle γ, vertical plane of symmetry of carrier is with respect to the angle of being rotated through carrier longitudinal axis vertical guide.

2. motion state is judged

The motion state of carrier is divided into two kinds:

1) no mover is moving.Can confirm the attitude of the geographical relatively surface level of carrier under this state through the acceleration measuring value, 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 top opposite, accelerometer measures can not be used for confirming attitude of carrier.

Judge principle: when carrier did not have linear acceleration, carrier was moving for no mover.This moment the acceleration measuring value $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 the local gravitational acceleration value.

During as

, it is moving to think that carrier does not have mover;

During as

, it is initiatively motor-driven to think that carrier has;

α wherein _aBe 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.$ Consideration and θ, the relevant state vector c of γ ₃

$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 does ^{[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$ Be the system noise sequence, approximate satisfied

(expectation),

(covariance/variance), Q _kBe gyroscope noise sequence variance battle array.

${\left({\mathrm{\&omega;}}_{\mathrm{nb}}^b\right)}_{\&times;}=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_{\mathrm{nbz}}^b& {\mathrm{\&omega;}}_{\mathrm{nby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{nbz}}^b& 0& -{\mathrm{\&omega;}}_{\mathrm{nbx}}^b\\ -{\mathrm{\&omega;}}_{\mathrm{nby}}^b& {\mathrm{\&omega;}}_{\mathrm{nbx}}^{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="111230150731.png"\; state="\{\{state\}\}">b</figure-callout>}}& 0\end{array}\right]$

${\left(c_3\right)}_{\&times;}=\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 put in order order

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

Then formula [1] can be write as

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

When the motion state of carrier is moved for no mover, acceleration measuring value f ^bSatisfy relational expression

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

Wherein g is a 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$ Be to measure noise sequence, approximate satisfied

(expectation),

(covariance/variance), R _kBe accelerometer noise sequence variance battle array.

Put following formula in order, order

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

Then formula [3] can be write as

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

To the system that forms by [2] and [4], adopt the Kalman filtering filtering algorithm to carry out iterative computation.The Kalman filtering filtering algorithm comprises that the time upgrades and measurement upgraded for two steps, is used to carry out the time renewal at each iteration cycle gyroscope measuring value; The result that motion state is judged is used for decision and whether measures renewal with the accelerometer measuring value, for there not being initiatively motion state, then measures renewal as if carrier normally, otherwise, the result after the result who directly utilizes the time to upgrade upgrades as measurement.

4. attitude algorithm

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

θ＝arcsin(c ₃₂)

${\mathrm{\&gamma;}}_{\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.$

Top 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: with 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 roll angle γ and the pitching angle theta (attitude fusion) of laser scanner with respect to the actual attitude of the geographical surface level in locality.

Attitude during each frame data that laser range sensor is measured is made up of two parts: the attitude of car body and the tilter attitude that is fixed on car body; Algorithm through the front has obtained this two-part attitude parameter respectively; Be respectively the attitude parameter of carrier: pitching angle theta and roll angle γ with respect to the geographical surface level in locality; With obtain the angle of pitch α that waves The Cloud Terrace by absolute value encoder, this moment, laser scanner with respect to the computing formula of the actual attitude of the geographical surface level in locality did

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

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

Carry out S130 again: calculate the projection coordinate information (be laser data attitude projection correction) of laser data point at geographical surface level.

Being the local n of Department of Geography from navigation system realizes through the rotation around three axles to the conversion of carrier coordinate system b

The result who rotates for three times representes that with direction cosine matrix the note navigation is that the direction cosine matrix

that is has to carrier

$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 the laser data to (a _iDist _i), at first be transformed into the rectangular coordinate system coordinate with following formula

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

Increase the contrary projection coordinate of laser data point that promptly obtain of multiply by direction cosine matrix after z component 0 value at 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^{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="111230150731.png"\; state="\{\{state\}\}">b</figure-callout>}}\\ 0\end{array}\right]$

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

In a measuring period, car body might also be subjected to displacement when record pursued the frame laser data, and this laser data that just causes record need be carried out the translation correction through the increment of mileage gauge in the measuring intervals of TIME not in same position.

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

Do one and simplify to handle, the 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;<figure-callout\; id="0"\; label="y"\; filenames="111230150731.png"\; state="\{\{state\}\}">y</figure-callout>}\\ 0\end{array}\right]$

To back one frame laser data then according to parameter (δ x ⁿδ y ⁿδ a) carries out translation and rotation, calculates by following formula

$\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 in the measuring period N frame laser data, agreement is according to the processing of above-mentioned two steps, and each frame laser data is snapped to: 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 in a measuring period is aimed on room and time; These revised data are painted in the same three-dimensional system of coordinate, and such width of cloth analyzing spot cloud atlas is the 3D profile of describe environment fully just.

Carry out S150 at last: on this analyzing spot cloud atlas, locate the point that presets.

On the basis that obtains environment 3D profile; Though consider the frame complex structure that spreads out; The profile of laser scanning has very big uncertainty; But the shelf structure that spreads out still has the characteristic on a lot of planes on 3d space, on 3D profile cloud atlas, show as the data point of some coplanes, adopts Hough transformation to extract the coplane data point in the three-dimensional data points; The result is carried out determine the chord position that spreads out after the operations such as cluster and put, through this notable feature can confirm to spread out relative position relation of frame and laser range sensor.

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

1. extraction characteristic plane

The space plane equation:

z＝a _xx+a _yy+d

For the 3D profile cloud atlas that builds, 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.N point arranged in the 3D cloud atlas; N plane just arranged in parameter space; These Plane intersects and a point (can just in time not intersect at a point, but concentrate on a less zone relatively), the plane that the Hough inverse transformation of this point is formed with regard to coplane data point in the corresponding 3D cloud atlas.

But top algorithm 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. relative position is located

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

z＝a _xx+a _yy+d

Laser center calculates according to following formula to plan range:

$\mathrm{dis}\tan \mathrm{ce}=\frac{\left|d\right|}{\sqrt{a_{\mathrm{<figure-callout\; id="2"\; label="x"\; filenames="111230150731.png"\; state="\{\{state\}\}">x</figure-callout>}}^2+a_{\mathrm{<figure-callout\; id="2"\; label="y"\; filenames="111230150731.png"\; state="\{\{state\}\}">y</figure-callout>}}^2+1}}$

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

More than the disclosed several specific embodiments that are merely the application, but the application is not limited thereto, any those skilled in the art can think variation, all should drop in the application's the protection domain.

Claims (10)

1. a mirror surface cleaning robot carries out the system of three-dimensionalreconstruction and location; It is characterized in that; Be used to be installed in the headstock part of said 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 the motor, and transmission waves on the support said, is used for rotatablely moving of motor is converted into the oscillating motion of support;

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

Said absolute value encoder is arranged on waving on the axle of laser range sensor, is used for directly measuring the angle of waving with respect to car body;

The MEMS inertial sensor is used to measure car body roll angle and angle of pitch with respect to geographical surface level when the out-of-flatness ground motion;

Odometer: be installed in the mirror surface cleaning robot;

Host computer: connect laser range sensor, absolute value encoder, MEMS inertial sensor and odometer respectively; Be used to receive the transmission data that comprise the laser range sensor transmission; Make up a 3D profile diagram, extract the coplane data point in the three-dimensional data points, determine the chord position that spreads out and put.

2. the system of claim 1 is characterized in that, said laser range sensor waves along waving fulcrum, and the said fulcrum that waves is fixed in the said mirror surface cleaning robot.

3. according to claim 1 or claim 2 system; It is characterized in that the said support that waves comprises upper bracket and lower carriage at least, said upper bracket and lower carriage constitute the oblong-shaped of a hollow; Said eccentric wheel is arranged in the rectangle of said hollow; Said eccentric wheel drops on the said lower carriage, and the perpendicular lattice framing in the left side of lower carriage is fixedly connected with said laser range sensor, and the following horizontal lattice framing of said lower carriage is connected with the perpendicular lattice framing in a said left side; Said eccentric wheel moves in the semi-surrounding circle of lower carriage, drives the oscillating motion of lower carriage.

4. system as claimed in claim 3 is characterized in that, said eccentric diameter equals the height of the perpendicular lattice framing in a left side of said lower carriage.

5. system as claimed in claim 3 is characterized in that, also comprises a support bar, and an end of said support bar connects the lower end of the perpendicular lattice framing in the right side of said upper bracket, and the other end flexibly connects on the laser range sensor.

6. the system of claim 1 is characterized in that, said laser range sensor is arranged on and waves on the support or an end of motor.

7. system as claimed in claim 2 is characterized in that, said laser range sensor adopts the two-dimensional laser distance measuring sensor.

8. the system of claim 1 is characterized in that,

Said MEMS inertial sensor comprises gyroscope and accelerometer, and said host computer further comprises:

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

The car body attitude merges computing unit; Be used for attitude with 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 roll angle γ and the pitching angle theta of laser scanner with respect to the actual attitude of the geographical surface level in locality;

The laser data attitude is thrown projection corrected Calculation unit, is used to calculate the projection coordinate information of laser data point at geographical surface level;

Laser data translation corrected Calculation unit is used in the universal measurement time interval increment of mileage gauge and revises, and revised data are painted into one or three paint in the coordinate system, forms the one scan point cloud chart;

The location Calculation unit is used on this analyzing spot cloud atlas, locating the point that presets.

9. a 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;

With 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 roll angle γ and the pitching angle theta of laser scanner with respect to the actual attitude of the geographical surface level in locality;

Calculate the projection coordinate information of laser data point at 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 the coordinate system, forms the one scan point cloud chart;

On this analyzing spot cloud atlas, locate the point that presets.

10. method as claimed in claim 9 is characterized in that, on this analyzing spot cloud atlas, locatees the point that presets and further comprises:

On 3D profile cloud atlas, extract characteristic plane;

Confirm the relative both sides of carrier spread out position and the carrier forward direction of frame and the angle of the frame direction of spreading out.

Images