CN102338621A - Method for detecting height of obstacle for indoor visual navigation - Google Patents

Abstract

The invention discloses a method for detecting the height of a ground obstacle in indoor environment, belonging to the technical field of visual navigation. The method can help an indoor visual navigation system to judge the obstacle area and the ground area in the indoor environment correctly. The invention is realized by the following technical scheme that a camera and a laser transmitting device are arranged on an indoor ceiling, wherein the laser transmitting device can rotate randomly around the axis. The optical center of the camera and the rotating axis of the laser transmitting device are positioned in the same horizontal surface. The laser transmitting device projects a light spot to the ground, and the light spot can reach the ground if no obstacle exists; and the light spot is projected on the obstacle if the obstacle exists. According to the method, an indoor top image is shot by utilizing the camera, and the height of the obstacle is calculated by utilizing the offset of the imaging positions of the light spots projected to the ground and the obstacle.

Description

A kind of indoor Visual Navigation is used the obstacle height detection method

Technical field

The present invention belongs to the vision guided navigation technical field, relates to a kind of method that in indoor environment, detects the ground obstacle height.

Background technology

Vision guided navigation is meant the information of utilizing imageing sensor to obtain environment, realizes robot navigation's technology through the analysis environments image.Vision guided navigation obtains the mode of information with human identical, therefore is considered to have one of airmanship of development potentiality most.

In indoor environment, can be fixed on imageing sensor on the ceiling to obtain indoor overhead view image, realize the navigation of robot through analyzing overhead view image then.In this navigate mode, need cut apart the ground region of overhead view image, through the activity restriction of robot in ground region, realize hiding automatically to barrier.Existing ground region dividing method generally adopts visual signatures such as the color, texture on ground to extract ground region, has following two shortcomings:

(1), for example is placed on white ground white chair etc., then correctly the ground region of split image and barrier region if the visual signature ground region of barrier is identical;

(2) if the visual signature of ground region is inconsistent, for example there is a beach ink marks etc. on ground, can cause vision algorithm to be barrier region to the ground region erroneous judgement.

Summary of the invention

Technical matters to be solved by this invention is the above-mentioned deficiency to prior art, proposes a kind of indoor Visual Navigation and uses the obstacle height detection method.

The present invention adopts following technical scheme, comprises the following steps:

Camera and laser beam emitting device are installed on the indoor ceiling.The rotatable axle center C of laser beam emitting device rotates arbitrarily, and horizontal direction rotation corner is expressed as θ, and vertically the aspect corner is φ.The photocentre O of axle center C and camera is positioned at same surface level, and the distance that promptly arrives ground is identical.The distance table that the O point is ordered to C is shown

O is to O _fThe distance table of point is shown

With

Mode through actual measurement obtains.The focal length of camera is f _c

Known level rotational angle theta and vertical plane corner φ, laser beam emitting device is projected spot earthward.If there is not barrier, then hot spot projects the R point on ground.The coordinate that R is ordered calculates according to

and θ, φ.The R point at camera as the i that is imaged as in the plane _R, utilize aperture camera model i _RCoordinate by the focal distance f of camera _c,

And the coordinate Calculation of R obtains.If there is barrier in ground, then hot spot can not arrive ground, but projects the P point of barrier, and the P point is to the distance on ground, i.e. the height h of barrier.The P point is at the imaging i on picture plane _PUtilize camera to take the overhead view image of indoor environment, i _PCoordinate obtain through the position of analyzing hot spot in the overhead view image.Picture i _PWith picture i _RBetween distance table be shown

The height h of barrier by

f _cWith Calculate.

Because in actual application, there is the erroneous judgement phenomenon in original method to barrier region in the indoor environment and ground region, promptly is ground region to the barrier region erroneous judgement that should dodge, is barrier region to the ground region erroneous judgement that can pass through.In order to address this problem, the present invention's proposition utilizes the mode of active vision to obtain the height of indoor diverse location, and whether utilize elevation information to judge should the zone be barrier.This method can be united use with original method, can improve the accuracy rate of navigational system cognitive disorders thing.

Description of drawings

Fig. 1 illustrates the installation site of camera and laser beam emitting device;

Among the figure: 1. camera; 2. laser beam emitting device

Fig. 2 illustrates the geometric relationship of dyscalculia object height degree.

Among the figure: 1. as the plane; 2. laser beam emitting device; 3. barrier

Embodiment

1. the installation of camera and laser beam emitting device

Be installed in camera and laser beam emitting device on the ceiling, laser beam emitting device can rotate in two degree of freedom around the axle center.O representes the photocentre of camera among Fig. 1, and the optical axis of camera is perpendicular to the ground, and with the intersection point on ground be O _fC representes the axle center of laser beam emitting device rotation, and the axis of along continuous straight runs rotation is perpendicular to the ground, and with the intersection point on ground be C _fLaser beam emitting device horizontal direction rotation corner is expressed as θ, and vertically the aspect corner is φ.Photocentre O and axle center C are positioned at same surface level, and promptly 2 distances to ground equate.

The distance table that the O point is ordered to C is shown

O is to O _fThe distance table of point is shown

With

Mode through actual measurement obtains.The focal length of camera is f _c

Under the known condition of θ and φ, hot spot coordinate on the ground is R (x _R, y _R, 0), x wherein _R, and y _R, calculate by formula (1) and (2)

$x_R=\stackrel{\‾}{\mathrm{OC}}+\cos \left(\mathrm{\θ}\right)\tan \left(\mathrm{\φ}\right)\stackrel{\‾}{{\mathrm{OO}}_f}---\left(1\right)$

$y_R=\sin \left(\mathrm{\θ}\right)\tan \left(\mathrm{\φ}\right)\stackrel{\‾}{{\mathrm{OO}}_f}---\left(2\right)$

Utilize the pinhole imaging system model, can obtain R picture i in the picture plane _RCoordinate

$x_R^{\left(i\right)}=\frac{f_c}{\stackrel{\‾}{{\mathrm{OO}}_f}}{x}_R---\left(3\right)$

$y_R^{\left(i\right)}=\frac{f_c}{\stackrel{\‾}{{\mathrm{OO}}_f}}{y}_R---\left(4\right)$

2. obtain the volume coordinate of hot spot imaging

Take indoor overhead view image, when having barrier on the ground, hot spot can not arrive the R point on ground, but projects the P point of barrier.The P point is at the i that is imaged as as the plane _PUtilize image process method to obtain i _PCoordinate.

(1) under the known situation of corner φ and θ, it is (R that laser beam emitting device throws color value earthward _l, G _l, B _l) monochromatic hot spot, R _l, G _l, B _lThe red component of representing color value respectively, green component and blue component.

(2) be W as the rectangular area that is used in the plane form images, because the graphical representation that the z coordinate is therefore omits z coordinate handle of in as the plane, being had a few does

I＝[I _r(x，y)，I _g(x，y)，I _b(x，y)|(x，y)∈W]

I wherein _r(x, y), I _g(x, y), I _b(x y) is respectively image at (x, y) red component, green component and the blue component of coordinate place color value.

(3) select threshold value T, utilize following formula that light spot image is cut apart:

(4) according to the central point i of computes hot spot _PCoordinate (x _P, y _P)

$x_P^{\left(i\right)}=\frac{\underset{(x,y)\∈W}{\∫\∫}{\mathrm{xI}}_s(x,y)\mathrm{dxdy}}{\underset{(x,y)\∈W}{\∫\∫}{I}_s(x,y)\mathrm{dxdy}}---\left(5\right)$

$y_P^{\left(i\right)}=\frac{\underset{(x,y)\∈W}{\∫\∫}{\mathrm{yI}}_s(x,y)\mathrm{dxdy}}{\underset{(x,y)\∈W}{\∫\∫}{I}_s(x,y)\mathrm{dxdy}}---\left(6\right)$

3. obstacle height is calculated

In Fig. 2; The picture plane is parallel to the ground; Simultaneously optical axis is O ' with intersection point as the plane, and the distance table between O ' and O ' is shown

and is the focal length of camera.

Do vertical line earthward by the P point; Give the B point with ground; Then

representes the distance between P point and the B point, and h is the height of barrier.The extended line and the ground of O point and P point line meet at P ' point.i _RAnd i _PBe respectively R and the P picture in the picture plane

Utilize the aperture camera model to obtain

$\frac{\stackrel{\‾}{{\mathrm{RP}}^{\′}}}{\stackrel{\‾}{i_R{i}_P}}=\frac{\stackrel{\‾}{{\mathrm{OO}}_f}}{f_c}---\left(7\right)$

Similar according to triangle RPP ' with CPO, and BPP ' and O _fOP ' is similar, is concerned as follows

$\frac{\stackrel{\‾}{\mathrm{OC}}}{\stackrel{\‾}{{\mathrm{RP}}^{\′}}}=\frac{\stackrel{\‾}{\mathrm{OP}}}{\stackrel{\‾}{{\mathrm{PP}}^{\′}}}---\left(8\right)$

$\frac{\stackrel{\‾}{\mathrm{OP}}}{\stackrel{\‾}{{\mathrm{PP}}^{\′}}}=\frac{\stackrel{\‾}{{\mathrm{OO}}_f}-h}{h}---\left(9\right)$

Aggregative formula (7), (8) and (9) obtain,

$h=\frac{\stackrel{\‾}{{\mathrm{OO}}_f}\·\stackrel{\‾}{{\mathrm{OO}}_f}\·\stackrel{\‾}{i_R{i}_P}}{f_c\·\stackrel{\‾}{\mathrm{OC}}+\stackrel{\‾}{{\mathrm{OO}}_f}\·\stackrel{\‾}{i_R{i}_P}}---\left(10\right)$

Formula (3); (4),

calculated in (5) and (6) substitutions (11)

$\stackrel{\‾}{i_R{i}_P}=\sqrt[2]{{(x_R^{\left(i\right)}-x_P^{\left(i\right)})}^2+{(y_R^{\left(i\right)}-y_P^{\left(i\right)})}^2}---\left(11\right)$

Because f _c,

Be known quantity, utilize the formula (10) can dyscalculia object height h.

Claims (1)

1. a method that in indoor environment, detects the ground obstacle height comprises the following steps:

(1) be installed in camera and laser beam emitting device on the ceiling, the axis of rotation C of laser beam emitting device and the photocentre O of camera are at same surface level; The distance that the O point is ordered to C does

The optical axis of camera is perpendicular to the ground, and meets at O with ground _fThe point; O is to O _fThe distance of point does

Be the distance of O point to ground; The focal length of camera is f _c

(2) anglec of rotation of fixed laser emitter, launch spot earthward; When not having barrier on the ground, hot spot projects on the ground, at the be imaged as i of camera as the plane _RWhen on the ground barrier being arranged, hot spot projects on the barrier, at the be imaged as i of camera as the plane _Pi _RAnd i _PBetween have skew, be expressed as

Utilize the height h of the geometry imaging relations dyscalculia thing of aperture camera model:

Images