CN103512579A - Map building method based on thermal infrared camera and laser range finder - Google Patents

Abstract

The invention relates to a map building method based on a thermal infrared camera and a laser range finder. The method is characterized in that the problem that in the prior art, a map cannot be correctly built under the conditions of poor light condition and existence of a shielding object is effectively solved by acquiring a thermal infrared image of a target instead of acquiring the visible image of the target, and a rectangular frame containing the target is obtained by taking a variance weighted information entropy for interest area extracting, so as to reduce the learning space and remove huge noise jamming. According to the invention, the matched corresponding points of Euclidean distance and temperature are high in reliability, an RANSAC (random sample consensus) algorithm filters the corresponding points, so that the reliability of the corresponding points is improved to the greatest extent, and the matching calculation speed based on an ICP (iterative closest point) algorithm is high.

Description

A kind of map constructing method based on thermal infrared video camera and laser range finder

Technical field

The present invention relates to the instant location of mobile robot and map structuring field, relate in particular to mobile robot's and map structuring there is shelter in the situation that very low in visibility.It can be used for mobile robot is set out by optional position at zone of ignorance, according to the perception data of laser range finder and thermal infrared video camera, estimates robot pose constructing environment map.

Background technology

Along with the development of Robotics, the intelligent mobile robot with the function that is movably walking, environment sensing ability and trajectory planning ability is more and more subject to various countries researchist's attention.Realizing instant location is one of the most basic function of intelligent robot with map structuring (SimultaneousLocalization and Mapping, SLAM), is also its important prerequisite that completes many tasks.

In the application of two-dimentional or three-dimensional map structuring, mainly adopt based on yardstick invariant features mapping algorithm (Scale-invariant feature transform, SIFT) at present.SIFT algorithm is a kind of based on metric space, to image scaling, rotate the Feature Correspondence Algorithm that even affined transformation maintains the invariance.This algorithmic match ability is stronger, can extract stable feature, can process the matching problem under translation, rotation, affined transformation, view transformation, illumination change situation occurs between two width images.But the shortcoming of this method is that ambient brightness and the random road sign occurring are had to very strong dependence.And iterative closest point algorithms (Iterative Closest Point, ICP), by iteration optimization matrix, in each iterative process, to each point on target point set, in reference point, concentrate and find closest approach, and utilize such corresponding point, and calculate corresponding rotation matrix and translation vector, use it on target point set, obtain new target point set, then enter next iterative process.Finally obtain best transition matrix, realize the accuracy registration of two point sets.

In map structuring, conventionally use depth camera or other sensors to gather the visible images of target, obtain target information.Visible images is reflected image, and radio-frequency component is many, can reflect the details of scene under certain illumination, but the contrast of visible images when illumination is not good is lower, and poor quality, cannot accurately obtain target information; Thermal infrared images is radiation image, and gray scale determines by the temperature difference of target and background, and dark or have in the environment such as cigarette, cloud, mist at light, visible images is second-rate, and the target in infrared image is but still clear and legible.And thermal infrared possesses certain penetration capacity, even still can more clearly obtain target image there being shelter to exist, be conducive in the situation that hiding, pretend and blocking sooner, the detection of a target more accurately.

Summary of the invention

The technical problem to be solved in the present invention is: a kind of map constructing method and device based on thermal infrared video camera and laser range finder is provided, solves map structuring problem very low in visibility and there is shelter in the situation that.

For solving the problems of the technologies described above, the technical solution used in the present invention is as follows:

A map constructing method based on thermal infrared video camera and laser range finder, is characterized in that comprising the steps:

Step 1, the cooperation of employing laser range finder rotate freely motor and build three-dimensional laser range finding platform, realize the scanning to three-dimensional scenic;

It is upper that step 2, the three-dimensional laser range finding platform that step 1 is built and thermal infrared video camera are installed on mobile robot, and structure can obtain the thermal infrared images of target and the scanister of three-dimensional laser point point set simultaneously; Demarcate thermal infrared video camera, obtain thermal infrared intrinsic parameters of the camera and external parameter; Adjust the position of thermal infrared video camera, make the focus of thermal infrared video camera and laser range finder consistent and do not stop scanning area each other; By calculating, be converted to the global coordinate system of device;

Step 3, utilize the scanister scanning that step 2 builds to obtain the original three-dimensional laser point data of environment, then record service robot that ultrasonic sensor records and the range data of the place ahead barrier, the two-dimensional grid map of establishing target environment;

Step 4, use thermal infrared video camera obtain several thermal infrared imagess of environment and several thermal infrared imagess that obtain are carried out to preliminary filtering noise reduction process; According to horizontal direction and vertical direction, in two steps image is carried out to lens distortion correction; Mobile robot's thermal infrared imaging mapping method is expanded to traverse measurement system, calculate the analyzing spot of three-dimensional laser stadimeter and the mathematics transformational relation of corresponding thermal infrared images pixel;

Step 5, thermal infrared images is carried out processing based on variance weighted information entropy, realize the target of area-of-interest under complex background is detected and extraction process, extract several thermal infrared images temperature information unique points and Euclidean distance unique point;

After step 6, definite coupling corresponding point, adopt RANSAC Algorithm for Solving basis matrix, then adopt basis matrix to reject the corresponding point of matching error;

Step 7, two-dimensional grid map and the temperature information unique point and the Euclidean distance unique point that at diverse location and visual angle, gather several thermal infrared imagess are carried out to repeatedly ICP Iterative matching, realize the reconstruct to unknown scene 3 d grid map.

By technique scheme, in step 1, adopt the electric rotating air connector being contacted by mercury, laser range finder can rotate freely and can not cause cable distortion; Three-dimensional laser range measurement system can free 0 °～360 ° rotations around laser scanning plane obtain three-dimensional laser data.

By technique scheme, three-dimensional laser stadimeter is first set in step 3 take 1 millisecond as sampling period collection environmental data and carries out analog to digital conversion, obtain original three-dimensional environment data, then record service robot that ultrasonic sensor records and the range data of the place ahead barrier; The three-dimensional laser point point set of obtained original three-dimensional environment data is carried out to triangular grid, to form a plurality of planes, and acquisition and each corresponding normal vector in a plurality of planes; Merge approaching normal vector to form normal vector list and normal vector list is mated with each vector in described environmental characteristic vector table; Add in environmental characteristic vector table the normal vector of failing in the normal vector table of coupling to upgrade environmental characteristic vector table, utilize the vector of coupling mutually, build local two-dimensional grid map;

Then, utilize odometer to obtain displacement and the translational speed of mobile robot's left and right wheels, employing track recursion pushes away the pose that the method obtaining obtains current robot; The projection ratio of the current pose of calculating robot in grating map; Location algorithm by EKF by the prediction of robustness, generates overall two-dimensional grid map by local two-dimensional grid map after the coupling fusion treatment of each time point environment detail.

Of the present invention focusing on: by obtaining thermal infrared images and the three-dimensional laser point set of target, then based on variance weighted information entropy, the target of area-of-interest under complex background is detected and extraction process, and use repeatedly ICP Iterative matching, reach the object of the three-dimensional map that builds circumstances not known.

With respect to prior art, beneficial effect of the present invention is:

The present invention replaces prior art to obtain the visible images of target by obtaining the thermal infrared images of target, effectively solves that prior art is poor at illumination condition, the correct problem of map structuring have shelter to exist in the situation that; Utilize variance weighted information entropy to do region of interesting extraction, obtain the rectangle frame that comprises target, dwindled solution space, rejected a large amount of noise; Euclidean distance of the present invention and Temperature Matching corresponding point reliability are high, with RANSAC algorithm, filter corresponding point, have at utmost improved the reliability of corresponding point, fast based on ICP algorithmic match computing velocity.

The present invention has good versatility and practicality, can effectively promote the widespread use of infrared image, has good economic benefit.

Accompanying drawing explanation

Fig. 1 is the process flow diagram of a kind of map constructing method based on thermal infrared video camera and laser range finder of the present invention.

Fig. 2 is that in the present invention, laser range finder coordinates the coordinate system figure that rotates freely motor structure three-dimensional laser range finding platform.

Fig. 3 is the coordinate system figure of three-dimensional laser range finding platform and the experiment porch of thermal infrared video camera structure in the present invention.

Fig. 4 is the projection model of thermal infrared video camera in the present invention.

Embodiment

For making technical scheme of the present invention and advantage clearer, with reference to accompanying drawing, the present invention is described in further detail, not the restriction to its protection domain.

Fig. 1 has provided the implementing procedure figure of a system of the present invention.As shown in Figure 1, generally speaking, method of the present invention comprises the steps:

Step 1, utilize the German SICK LMS200 of company laser range finder to coordinate to rotate freely motor and build three-dimensional laser scanner, by laser range finder, realize the scanning to three-dimensional scenic.Three-dimensional laser range measurement system is to consist of the small-sized The Cloud Terrace of built-in motor and LMS200 laser range finder, inclination mounting means, and the built-in motor of The Cloud Terrace drives laser.The present invention adopts the electric rotating air connector being contacted by mercury, and stadimeter can rotate freely and can not cause cable distortion.Three-dimensional laser range measurement system can the free 0D around laser scanning plane～360D rotation obtain three-dimensional laser data, and scan mode is perpendicular to surface level.This laser scanner is arranged on the MT-R of the Ying Ji Si company Autonomic Mobile Robot System platform of Shanghai.Coordinate the motor that rotates freely of The Cloud Terrace to select permanent type heavy duty magnetic-synchro motor, can guarantee the serviceable life that three-dimensional laser range measurement system is longer, and have stable operating function, its rotational speed is similar to uniform motion.

In actual case study on implementation, due to when the mechanical erection, spatially there is certain deviation in the rotation center of The Cloud Terrace and laser beam launching centre, and its location parameter relation can be represented by Fig. 2 structural representation.Suppose that two-dimentional stadimeter LMS200 coordinate system is Σ _l, the coordinate of initial point ο is positioned at plane of scanning motion center, X ' axle corresponds to the two-dimentional stadimeter plane of scanning motion, Y ' axle be perpendicular to scanning plane (face or axle?), Z ' axle is corresponding right-handed coordinate system.Coordinate system Σ _srepresent three-dimensional laser stadimeter, origin ο is the rotation platform that is positioned at a specific distance center.X-axis is corresponding to the front of positive three-dimensional laser stadimeter, the surface of the corresponding rotation axis of Y-axis, and Z axis is for meeting right-handed coordinate system.This scanning laser range finder setting angle is to be β with horizontal plane angle, and the rotation sweep speed of the The Cloud Terrace plane of scanning motion is

Homogeneous transformation matrix ^st _lwith coordinate system Σ _l, Σ _sbetween relationship between expression be:

$T_L^S=\left[\begin{array}{cc}R_L^S& \mathrm{<figure-callout\; id="0"\; label="t\; L\; S"\; filenames="HDA0000399749970000021.png"\; state="\{\{state\}\}">t</figure-callout>}_{LS}^{}\\ 0& 1\end{array}\right]$

$R_L^S=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}& -\sin \mathrm{\&gamma;}& 0\\ \sin \mathrm{\&gamma;}& \cos \mathrm{\&gamma;}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\&beta;}& 0& \sin \mathrm{\&beta;}\\ 0& 1& 0\\ -\sin \mathrm{\&beta;}& 0& \cos \mathrm{\&beta;}\end{array}\right]t_L^S=\mathrm{<figure-callout\; id="0"\; label="t\; L\; S"\; filenames="HDA0000399749970000021.png"\; state="\{\{state\}\}">t</figure-callout>}_{LS}^{}\left[\begin{array}{c}0\\ 0\\ L\end{array}\right]$

In the present invention ^sr _lwith ^st _lrespectively two-dimentional stadimeter coordinate system Σ _lwith respect to three-dimensional laser range finding coordinate system Σ _srotation matrix and transition matrix.

Measurement point L _pwith respect to Σ _lbe expressed as: $L_p=\left[\begin{array}{c}p_x^L\\ p_y^L\\ p_z^L\end{array}\right]=\left[\begin{array}{c}r\cos \mathrm{\&theta;}\\ r\sin \mathrm{\&theta;}\\ 0\end{array}\right],$ R represents measuring distance, and θ represents scanning angle.

Measurement point coordinate ^sp is about three-dimensional laser range measurement system coordinate system Σ _smathematical relation be:

^Sp= ^ST _L ^Lp

$\left[\begin{array}{c}p_x^S\\ p_y^S\\ p_z^S\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}& -\sin \mathrm{\&gamma;}& 0\\ \sin \mathrm{\&gamma;}& \cos \mathrm{\&gamma;}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\&beta;}& 0& \sin \mathrm{\&beta;}\\ 0& 1& 0\\ -\sin \mathrm{\&beta;}& 0& \cos \mathrm{\&beta;}\end{array}\right]\left[\begin{array}{c}r\cos \mathrm{\&theta;}\\ r\cos \mathrm{\&theta;}\\ L\end{array}\right]$

Real case is acquisition measurement point in implementing ^sthe accurate three-dimensional Cartesian coordinates of p, the anglec of rotation γ synchronous and plane of scanning motion of scan-data is extremely important.

Step 2, thermal infrared video camera and three-dimensional laser range-finding system are arranged on the MT-R of robot in the lump.For building a three-dimensional thermogram, we need full width thermal infrared images.Because the visual angle of traditional infrared camera is conventionally less, for reaching three-dimensional thermographic mapping, camera also should be arranged on the rotary head of robot.Thermal infrared video camera is carried out to parameter calibration, obtain its inner parameter and external parameter.Adjust the position of thermal infrared video camera, make the focus of thermal infrared video camera and laser range finder consistent and do not stop scanning area each other.As the coordinate system figure of the experiment porch of Fig. 3 three-dimensional laser range finding platform and thermal infrared video camera structure.By single analyzing spot ^sp and corresponding thermal infrared images pixel ^cp coupling.The coordinate system of thermal infrared video camera is Σ _c, Σ _c true origin 0 ^//the position of the camera focus being positioned at, x axle X ^//optic axis, z axle Z for camera ^//for the vertical coordinate of camera, y axle Y ^//for meeting the axis of right-handed coordinate system.

Thermal infrared picture point ^cp and coordinate system Σ _cmathematical relation be:

^Cp= ^CT _L ^Cp（1）

In the present invention ^ct _scoordinate system Σ _sand Σ _cbetween homogeneous transformation, and ^wt _swith Σ _scoordinate system and Σ _wbetween coordinate system (global coordinate system), position relationship is known, ^wt _cwith Σ _ccoordinate system and Σ _wthe relation of coordinate system is to calculate by camera position and camera rotation angle.Therefore,

$\begin{array}{c}T_S^C=T_W^{C}T_S^W\\ =T_C^{-1W}{}_{W}T_S\\ =\left[\begin{array}{ccc}R_C^T{}_W& -R_C^T{}_W& \mathrm{<figure-callout\; id="0"\; label="T\; C\; W"\; filenames="HDA0000399749970000021.png"\; state="\{\{state\}\}">T</figure-callout>}_{CW}^{}\\ 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R_S^W& \mathrm{<figure-callout\; id="0"\; label="T\; S\; W"\; filenames="HDA0000399749970000021.png"\; state="\{\{state\}\}">T</figure-callout>}_{SW}^{}\\ 0& 1\end{array}\right]\\ =\left[\begin{array}{ccc}R_C^T{}_W& R_S^W& R_C^T{}_W(T_S^W-T_C^W)\\ 0& 0& 1\end{array}\right]\\ \&DoubleRightArrow;p_C=\left[\begin{array}{ccc}R_C^T{}_W& R_S^W& R_C^T{}_W(T_S^W-T_C^W)\\ 0& 0& 1\end{array}\right]p_S\end{array}---\left(2\right)$

We can obtain global coordinate system Σ thus _wbe transformed into robot coordinate system Σ _rSwhen analyzing spot when ，Dang robot is static also can move analyzing spot model extension to robot.

The scanister scanning that step 3, utilization (step 2) build obtains the original three-dimensional laser point data of environment, then records service robot that ultrasonic sensor records and the range data of the place ahead barrier, the two-dimensional grid map of establishing target environment.

1) arrange three-dimensional laser stadimeter take 1 millisecond gather environmental data and carry out analog to digital conversion as the sampling period, obtain original three-dimensional environment data, then record service robot that ultrasonic sensor records and the range data of the place ahead barrier.Obtained three-dimensional laser point point set is carried out to triangular grid, to form a plurality of planes, and acquisition and each corresponding normal vector in a plurality of planes.Because the normal vector vertical with definite plane is unique, therefore can all represent a plane by each normal vector, i.e. that perpendicular plane.Merge approaching normal vector to form normal vector list and normal vector list is mated with each vector in described environmental characteristic vector table.Add in environmental characteristic vector table the normal vector of failing in the normal vector table of coupling to upgrade environmental characteristic vector table, utilize the vector of coupling mutually, build local two-dimensional grid map.

2) utilize odometer to obtain displacement and the translational speed of mobile robot's left and right wheels, employing track recursion pushes away the pose that the method obtaining obtains current robot.The projection ratio of the current pose of calculating robot in grating map.Location algorithm by EKF by the prediction of robustness, generates overall two-dimensional grid map by local two-dimensional grid map after the coupling fusion treatment of each time point environment detail.

Step 4, by thermal infrared video camera, obtain scanning coordinate and put corresponding thermal infrared images.As the video camera projection model of Fig. 4 demonstration.

Pass through homogeneous equation

and equation $\mathrm{\&omega;}\hat{q}=\mathrm{MQ},M=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_{\mathrm{<figure-callout\; id="0"\; label="y\; c\; y"\; filenames="HDA0000399749970000021.png"\; state="\{\{state\}\}">y</figure-callout>}}& c_y{}_{}\\ 0& 0& 1\end{array}\right]---\left(3\right)$

By three-dimensional coordinate point Q=(X, Y, Z) to the homogeneous pixel q=(x, y) that is transformed into.Wherein M is the inner parameter matrix of thermal infrared video camera, f _xand f _ythe focal length of representative, c _xand c _yit is the side-play amount of projection centre.The satisfied condition of equation (3) is there is no lens distortion, therefore the distortion correction of thermal infrared video camera is essential.The view data of obtaining from thermal infrared camera lens is carried out to the correction of horizontal direction, the view data of carrying out after the correction of horizontal direction is write to dynamic storage.Then the view data in dynamic storage is carried out the correction of vertical direction.According to horizontal direction and vertical direction, carry out in two steps lens distortion correction, realize rapidly the lens distortion of image and proofread and correct.

For map structuring fast, we expand to traverse measurement system by mobile robot's thermal infrared imaging mapping method.The method is to obtain on formula (3) basis in three-dimensional range finding.Make Σ _rSthe data that in robot coordinate system, laser range finder is caught, Σ _rCit is the image that in robot coordinate system, thermal infrared camera is caught. ^rSt _sto represent that scanner coordinate is with respect to robot coordinate homogeneous transformation matrix, ^rSt _cmean that camera coordinates is with respect to robot coordinate homogeneous transformation matrix.So homogeneous transformation matrix ^ct _s(represent Σ _sabout Σ _c) can be decomposed into:

^CT _S= ^CT _RC ^RCT _W ^WT _RS ^RST _S

While tentatively obtaining specifically implementing during moveable robot movement, the mathematics transformational relation of the corresponding thermal infrared images pixel of analyzing spot of three-dimensional laser stadimeter.

Step 5, thermal infrared images is carried out processing based on variance weighted information entropy, to the target detection based on area-of-interest and extraction under complex background.Wherein the region of interesting extraction step based on variance weighted information entropy is:

1) the initial frame image of M*N size is divided into the subimage block of m*n size, obtains subimage block set F=(u, v), wherein: m=2 ^k, n=2 ⁱ, 1≤K≤4,1≤I≤4,0≤u≤M/m-1,0≤v≤N/n-1;

2), to the every width image F=(i, j) in subimage block set F=(u, v), adopt formula:

$H\left(s\right)=-\underset{S=0}{\overset{s225}{\mathrm{\&Sigma;}}}{(s-\stackrel{\&OverBar;}{s})}^2*p_s*\log$

Calculate its variance weighted information entropy H=(i, j), obtain the entropy diagram picture of M/ (m-1) * N/ (n-1) size, and average μ and the variances sigma of computing information entropy diagram picture; Wherein: 0≤i≤M/m-1,0≤v≤N/n-1, s represents gray-scale value, p _srepresent every kind of probability that gray level is corresponding, represent the average gray of infrared image, work as p _s, make p at=0 o'clock _s* log (p _s)=0;

3) when step 2) maximum entropy H (t, r)>=H in the entropy diagram picture that obtains<sub TranNum="199">t</sub>time, the subimage F (t, r) that H (t, r) is corresponding is seed; As H (t, r)<H<sub TranNum="200">t</sub>time, get m=m/2, n=n/2, repeating step (1) and (2), until maximum entropy meets H (t, r)>=H in entropy diagram picture<sub TranNum="201">t</sub>or m=2, n=2; Wherein (t, r) represents the position of the image block of entropy maximum.

4) on entropy diagram picture, the factor obtaining with step (3), adopts the region growing method based on eight neighborhoods to increase, and obtains the region of interest ROI (centerx that comprises target, centery, w, h) result that detects as infrared small target in complex background, wherein: centerx is regional center horizontal ordinate, centery is regional center ordinate, w is the width in region, for h region height, is integer; Similarity α ∈ (0,1) in the described region growing method based on eight neighborhoods.

Step 6, extract several thermal infrared images temperature information unique points and Euclidean distance unique point.After match point is chosen, match point is rejected to error matching points with RANSAC algorithm, improve matching efficiency.RANSAC (random sample consensus) algorithm is a kind of iterative algorithm of estimating mathematical model parameter, and main thought is the strategy by sampling and verifying, solves the parameter of the mathematical model that most of sample characteristics point can be satisfied.The present invention uses the concrete steps of RANSAC algorithm as follows:

1) randomly draw 4 pairs of thermal infrared images temperature information unique points and Euclidean distance unique point as characteristic matching point (this sentence has an ambiguity, needs to clarify and revise), as initial interior some set, by this interior point set, add up to and calculate transformation matrix H.

2) in judgement, put the point outside set successively, calculate the distance between X and HX ', if be less than the I value of setting, " i " front point is joined to the set of interior point successively.

3) repeat (1) and (2) N time, choose that group that interior some number is maximum as qualified coupling point set.According to new interior some set, use least square method to upgrade transformation matrix H.

It is p that the point of supposing correct coupling accounts for total ratio, and 4 couple who randomly draws, one match point is not that the probability of correct coupling is 1-p entirely, and probability is (1-p) to extract that all to take out for N time less than 4 pairs be correct match point entirely<sup TranNum="208">4</sup>, in practice, as long as can guarantee (1-p)<sup TranNum="209">4</sup><0.05 just can meet the needs of practical application.

Step 7, by two-dimensional grid map with at diverse location and visual angle, gather the temperature information unique point of several thermal infrared imagess and Euclidean distance unique point and carry out repeatedly ICP and mate, realize the structure to scene 3 d grid map.ICP coupling in the present invention is not only used Euclidean distance but also is found corresponding point according to the temperature difference.The use of this algorithm of evaluation function is the nearest quadratic sum of iteration point but also consider the corresponding point calorific value difference of two squares not only.In each iterative step, ICP algorithm is selected immediate corresponding point and is calculated its rotation matrix R and transformed matrix t minimizes ICP algorithm equation:

$E_T(R,t)=\underset{i=1}{\overset{N_m}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{N_d}{\mathrm{\&Sigma;}}}\widehat{{\mathrm{\&omega;}}_{i,j}}({\left|\right|m_i-({\mathrm{Rd}}_j+t)\left|\right|}^2+K{|h_{\mathrm{mi}}-h_{\mathrm{dj}}|}^2)$

N wherein _mand N _dbeing respectively M place reference point quantity and D place match point quantity, is the weight of K temperature; h _mia m _iplace's temperature value, h _dja d _jplace's temperature value.When time, functional value minimizes.ICP iteration is carried out always, until E _t(R, t) convergence, now arrives iteration optimum value.Finally obtain environment 3 d grid map.

The advantage that the map constructing method of the implementation case based on thermal infrared video camera and laser range finder compared with domestic and international prior art is: first region of interest ROI is extracted, can effectively be dwindled map match region, reduce data calculated amount; Secondly match point is rejected to error matching points with RANSAC algorithm, reduce data analysis quantity, improve map match efficiency, shorten match time; Again, by the 3 d grid map of constructing environment, can embody better environmental information, contribute to the application of three-dimensional map in real work.

The map constructing method of the implementation case based on thermal infrared video camera and laser range finder, realization is in intensity of illumination, low and targeted environment exists shelter to exist in the situation that, establishing target environment three-dimensional map, improve laser robot under circumstances not known to the detectivity of environment and work efficiency.The present invention can be applied to the working environments such as field detection, earthquake search and rescue equally, has good economic and social benefit.

Above-described specific embodiment, further describes object of the present invention, technical scheme and beneficial effect, it should be understood that and the foregoing is only specific embodiments of the invention, is not limited to the present invention.Within the spirit and principles in the present invention all, any modification of making, be equal to replacement, improvement etc., within all should being included in protection scope of the present invention.

Claims (3)

1. the map constructing method based on thermal infrared video camera and laser range finder, is characterized in that comprising the steps:

Step 1, the cooperation of employing laser range finder rotate freely motor and build three-dimensional laser range finding platform, realize the scanning to three-dimensional scenic;

It is upper that step 2, the three-dimensional laser range finding platform that step 1 is built and thermal infrared video camera are installed on mobile robot, and structure can obtain the thermal infrared images of target and the scanister of three-dimensional laser point point set simultaneously; Demarcate thermal infrared video camera, obtain thermal infrared intrinsic parameters of the camera and external parameter; Adjust the position of thermal infrared video camera, make the focus of thermal infrared video camera and laser range finder consistent and do not stop scanning area each other; By calculating, be converted to the global coordinate system of device;

Step 3, utilize the scanister scanning that step 2 builds to obtain the original three-dimensional laser point data of environment, then record service robot that ultrasonic sensor records and the range data of the place ahead barrier, the two-dimensional grid map of establishing target environment;

Step 4, use thermal infrared video camera obtain several thermal infrared imagess of environment and several thermal infrared imagess that obtain are carried out to preliminary filtering noise reduction process; According to horizontal direction and vertical direction, in two steps image is carried out to lens distortion correction; Mobile robot's thermal infrared imaging mapping method is expanded to traverse measurement system, calculate the analyzing spot of three-dimensional laser stadimeter and the mathematics transformational relation of corresponding thermal infrared images pixel;

Step 5, thermal infrared images is carried out processing based on variance weighted information entropy, realize the target of area-of-interest under complex background is detected and extraction process, extract several thermal infrared images temperature information unique points and Euclidean distance unique point;

After step 6, definite coupling corresponding point, adopt RANSAC Algorithm for Solving basis matrix, then adopt basis matrix to reject the corresponding point of matching error;

Step 7, two-dimensional grid map and the temperature information unique point and the Euclidean distance unique point that at diverse location and visual angle, gather several thermal infrared imagess are carried out to repeatedly ICP Iterative matching, realize the reconstruct to unknown scene 3 d grid map.

2. the map constructing method based on thermal infrared video camera and laser range finder according to claim 1, is characterized in that: in step 1, adopt the electric rotating air connector being contacted by mercury, laser range finder to rotate freely and can not cause cable distortion; Three-dimensional laser range measurement system can free 0 °～360 ° rotations around laser scanning plane obtain three-dimensional laser data.

3. the map constructing method based on thermal infrared video camera and laser range finder according to claim 1, is characterized in that:

In step 3, first arrange three-dimensional laser stadimeter take 1 millisecond gather environmental data and carry out analog to digital conversion as the sampling period, obtain original three-dimensional environment data, then record service robot that ultrasonic sensor records and the range data of the place ahead barrier; The three-dimensional laser point point set of obtained original three-dimensional environment data is carried out to triangular grid, to form a plurality of planes, and acquisition and each corresponding normal vector in a plurality of planes; Merge approaching normal vector to form normal vector list and normal vector list is mated with each vector in described environmental characteristic vector table; Add in environmental characteristic vector table the normal vector of failing in the normal vector table of coupling to upgrade environmental characteristic vector table, utilize the vector of coupling mutually, build local two-dimensional grid map;

Then, utilize odometer to obtain displacement and the translational speed of mobile robot's left and right wheels, employing track recursion pushes away the pose that the method obtaining obtains current robot; The projection ratio of the current pose of calculating robot in grating map; Location algorithm by EKF by the prediction of robustness, generates overall two-dimensional grid map by local two-dimensional grid map after the coupling fusion treatment of each time point environment detail.

Images