CN102338621B - 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 method for detecting height of obstacle for indoor visual navigation

Technical field

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

Background technology

Vision guided navigation refers to the information of utilizing imageing sensor to obtain environment, by the analysis environments image, realizes robot navigation's technology.The mode of vision guided navigation obtaining information is identical with the mankind, therefore is considered to have one of airmanship of development potentiality most.

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

(1), if the visual signature ground region of barrier is identical, such as being placed on the ground white chair of white etc., can't correctly cut apart ground region and the barrier region of image;

(2), if the visual signature of ground region is inconsistent, such as You Yi beach, ground ink marks etc., can cause vision algorithm that ground region is mistaken for to barrier region.

Summary of the invention

Technical matters to be solved by this invention is the above-mentioned deficiency for prior art, proposes a kind of method for detecting height of obstacle for indoor visual navigation.

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

Camera and laser beam emitting device are installed on 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 level, and the distance that namely 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 by 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 no barrier, dot projection is to the R point on ground.The coordinate basis that R is ordered

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

And the coordinate of R calculates.If there is barrier in ground, 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 _P.Utilize camera to take the overhead view image of indoor environment, i _PCoordinate by the position of analyzing hot spot in overhead view image, obtain.Picture i _PWith picture i _RBetween distance table be shown The height h of barrier by

f _cWith

Calculate.

Due in actual application, there is misjudgment phenomenon in original method to the barrier region in indoor environment and ground region, namely the barrier region that should dodge is mistaken for to ground region, and the ground region that can pass through is mistaken for to barrier region.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, utilizes elevation information to judge whether this zone is barrier.This method can be combined use with original method, can improve the accuracy rate of navigational system cognitive disorders thing.

The accompanying drawing explanation

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

In figure: 1. camera; 2. laser beam emitting device

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

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

Embodiment

1. the installation of camera and laser beam emitting device

Camera and laser beam emitting device are arranged on ceiling, and laser beam emitting device can rotate around axle center in two degree of freedom.In Fig. 1, O represents the photocentre of camera, and the optical axis of camera is perpendicular to the ground, and with the intersection point on ground be O _f.C represents 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 _f.Laser 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 namely 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 by actual measurement obtains.The focal length of camera is f _c.

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

$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, while on ground, having barrier, 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 plane _P.The method of utilizing image to process obtains i _PCoordinate.

(1) in the situation that corner φ and θ are known, 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 that represents respectively color value, green component and blue component.

(2) as the rectangular area for imaging in plane, be W, due in as plane z coordinate a little be

Therefore the image representation that omits z coordinate handle is

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 red component, green component and the blue component of image at (x, y) coordinate place color value.

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

(4) calculate according to the following formula the central point i of 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, parallel to the ground as plane, optical axis is O ' with the intersection point as plane simultaneously, and the distance table between O ' and O ' is shown

Focal length for camera.

By the P point, do earthward vertical line, with ground, give the B point, Distance between expression P point and B point,

H is the height of barrier.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 to CPO according to triangle RPP ', and BPP ' and O _fOP ' is similar, obtains following relation

$\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), (5) and (6) substitutions (11) are calculated

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

Due to f _c,

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

Claims (1)

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

(1) camera and laser beam emitting device are arranged on ceiling, the axis of rotation C of laser beam emitting device and the photocentre O of camera are in same level; The distance that the O point is ordered to C is 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 is Be the distance of O point to ground; Laser beam emitting device horizontal direction rotation corner is expressed as θ, and vertically the aspect corner is ф; The focal length of camera is f _c

(2) anglec of rotation of fixed laser emitter, launch spot earthward; While on ground, there is no barrier, dot projection is to ground, at the be imaged as i of camera as plane _RWhile on ground, barrier being arranged, dot projection is to barrier, at the be imaged as i of camera as plane _PCamera is W as the rectangular area for imaging in plane, and the image representation of shooting is 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 the red component that coordinate (x, y) is located the color of image value, green component and blue component; Select threshold value T, utilize threshold segmentation method to cut apart I, obtain

i _RAnd i _PBetween have skew, be expressed as $\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},$ Wherein $x_R=\stackrel{\‾}{\mathrm{OC}}+\cos \left(\mathrm{\θ}\right)\tan \left(\mathrm{\φ}\right)\stackrel{\‾}{{\mathrm{OO}}_f},$ $y_R=\sin \left(\mathrm{\θ}\right)\tan \left(\mathrm{\φ}\right)\stackrel{\‾}{{\mathrm{OO}}_f},$ $x_R^{\left(i\right)}=\frac{f_c}{\stackrel{\‾}{{\mathrm{OO}}_f}}{x}_R,$ $y_R^{\left(i\right)}=\frac{f_c}{\stackrel{\‾}{{\mathrm{OO}}_f}}{y}_R,$ $x_P^{\left(i\right)}=\frac{\underset{(x,y)\∈W}{\∫\∫}x{I}_s(x,y)\mathrm{dxdy}}{\underset{(x,y)\∈W}{\∫\∫}{I}_s(x,y)\mathrm{dxdy}},$ $y_P^{\left(i\right)}=\frac{\underset{(x,y)\∈W}{\∫\∫}y{I}_s(x,y)\mathrm{dxdy}}{\underset{(x,y)\∈W}{\∫\∫}{I}_s(x,y)\mathrm{dxdy}};$ Obtain the height of barrier $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}}.$

Images