CN106384353A - Target positioning method based on RGBD - Google Patents

Abstract

The invention provides a target positioning method based on a RGBD, and the main steps comprise hardware environment arrangement and device initialization; the RGBD sensor is adopted to acquire depth data and color image of a mobile robot in current frame, a depth image is segmented based on the depth data to obtain a target region in the depth image, the target image is set as SearchRect_depth and is mapped to the color image, and the corresponding color image part is recorded as SearchRectROI; in the color image SearchRectROI, a CAMSHIFT method is utilized to calculate a position (x, y) of a target in the color image; the depth data is combined to calculate world coordinates (xw, yw, zw) of the current target; the world coordinates (xw, yw, zw) of the current target is transmitted to the mobile robot through a communication system, and the robot can control its self motion. According to the invention, the method has advantage of fast calculation speed, accurate positioning, strong adaptability for environment change and good expandability for different targets.

Description

A kind of object localization method based on RGBD

Technical field

The invention belongs to vision positioning field is and in particular to a kind of object localization method based on RGBD.

Background technology

Mobile robot self poisoning ability is to realize basis and the key of real high-performance and high accuracy navigation, and it can enter Row autonomous simultaneously complete given job task it is necessary to obtain exact position and the attitude of itself in real time.Move machine at present People is provided with GPS and inertial navigation system, its technology relative maturity, has much outstanding product can easily adopt Buy.Although GPS and inertial navigation system have many advantages, such as, but also have the shortcomings that some merit attention:

(1) gps data is usually to update once within 1 second it is impossible to meet the requirement of the quick motion of robot.

(2) when robot is in some particular pose, such as under backward flight, underriding and reduced gravity situations, often cannot connect Receive gps signal.

(3) indoors in environment it is impossible to receive gps signal.

Generally require to be tested indoors during the research method such as Autonomous Mobile Robot Control Algorithm and environment sensing, and in room In the environment of interior and some GPS cannot use, own location information often cannot be known in robot, and weather condition, cloud Thickness degree is also possible to affect the reception of gps signal.In addition, the change of robot own load also can make the quality of robot Distribution and equilibrium condition change the control difficulty increase so that mobile robot.Therefore it is adaptable to interior, have accurately, soon Speed, independently, the cheap and research heat becoming current mobile robot based on machine vision localization method the features such as adaptability is good Point.

Content of the invention

For overcoming the deficiencies in the prior art, the present invention devises a kind of object localization method based on RGBD, and this method has Have the advantages that calculating speed is fast, accurate positioning, the change for environment has stronger adaptability.

For realizing technique scheme, the invention provides a kind of object localization method based on RGBD, walk including following Suddenly：

Hardware environment is set up, and equipment initializes；

Obtain depth data and the coloured image of present frame mobile robot using RGBD sensor, based on depth data pair Depth image is split, and obtains the target area in depth image, is set to SearchRect_depth, and maps that to coloured silk In color image, corresponding color image parts are designated as SearchRectROI；

Calculate position in coloured image for the target using CAMSHIFT method in coloured image SearchRectROI (x, y)；

Calculate the world coordinates (x of current goal in conjunction with depth data_w, y_w, z_w)；

World coordinates (x by current goal_w, y_w, z_w) mobile robot is sent to by communication system, robot controls Displacement.

Preferably, described hardware environment is set up, and comprises the following steps：

By fixing for RGB sensor on the ceiling so as to front panel parallel to ground and with ground distance at 0.8 meter to 4 Between rice；

Colour-coded is fixed on mobile robot center.

Preferably, described based on depth data, depth image is split, comprise the following steps：

Note former frame positioning result (x_w, y_w, z_w) in target depth value z_wFor depth_pre, scheme in present frame depth image As the depth of arbitrfary point (i, j) is depth (i, j), obtain present frame depth data；

Registered depth threshold value is T, a width of depth_cols of depth image, a length of depth_rows of depth image, depth image In a certain pixel gray value be depth_seg (i, j), present frame depth data is traveled through, as follows to depth Image carries out Range Image Segmentation：

$depth\_seg(i,j)=\left\{\begin{array}{c}0,\left|depth\right(i,j)-depth\_pre|>T\\ 1,\left|depth\right(i,j)-depth\_pre|<T\end{array}\right.$

Wherein i=0,1,2 ... depth_cols, j=0,1,2 ... depth_rows；

Carry out connected domain detection in depth image depth_seg (i, j) through over-segmentation, choose the maximum connection of area Domain, and obtain the boundary rectangle of the maximum connected domain of area, the target area SearchRect_depth as in depth image；

By depth image and coloured image coordinate corresponding relation, obtain from coloured image and SearchRect_depth Corresponding search rectangular region SearchRectROI.

Preferably, described depth image is corrected by camera lens for RGB sensor with coloured image coordinate corresponding relation and is obtained Depth data and color image pixel point between mapping relations it is known that a bit (i, j) in coloured image, reflected by this The relation of penetrating can obtain depth information depth (i, j) of this point.

Preferably, calculated using CAMSHIFT method in described coloured image SearchRectROI and follow the tracks of target's center position Put O, i.e. (x, y), comprise the following steps：

Run if first, then need to carry out object initialization, choose target following rectangle frame manually TrackWindow_init, follows the tracks of target and should have specific color characteristic, and following the tracks of target in the present invention is to be fixed on robot The colour-coded at center；

To carrying out CAMSHIFT search in SearchRectROI, obtain new target location trackWindow, calculate The area trackWindow.area of trackWindow, if trackWindow.area is more than threshold value T_area then it represents that following the tracks of Success, obtains trackWindow centre of form coordinate (x, y), if trackWindow.area is less than threshold value T_area then it represents that following the tracks of Failure, reinitializes tracking result trackWindow_pre that trackWindow_init is previous frame, and re-starts CAMSHIFT searches for, and obtains new target location trackWindow.

Preferably, described threshold value is the 30% of initial trackWindow_init area, that is,

T_area=0.3*trackWindow_init.area.

Preferably, described combination depth data calculates the world coordinates (x of current goal_w, y_w, z_w), comprise the following steps：

Note W and H is respectively width and the height of RGB sensor color image, and image coordinate is converted into image coordinate Coordinate value (x for initial point₁, y₁), it is calculated as follows:

$y_1=\frac{H}{2}-y,x_1=\frac{W}{2}-x;$

The field range of note RGBD colour imagery shot is horizontal view angle α_h, vertical angle of view α_v, thus can try to achieve every pixel and correspond to Field of view angle be:

Level：Vertically：

And then try to achieve current pixel point (x₁, y₁) corresponding to level angle θ_hWith vertical rotational angle theta_v：

$\begin{array}{cc}{\mathrm{\&theta;}}_h=\stackrel{\&OverBar;}{{\mathrm{\&alpha;}}_h}\&CenterDot;x_1& {\mathrm{\&theta;}}_v=\stackrel{\&OverBar;}{{\mathrm{\&alpha;}}_v}\&CenterDot;y_1\end{array};$

According to depth information z_wWorld coordinates (the x of current goal can be tried to achieve_w, y_w, z_w)：

x_w=z_w·tanθ_hy_w=z_w·tanθ_v.

Compared with prior art, the technical solution used in the present invention has following beneficial effects：

The hardware device that the method for the invention is used is simple, with low cost, only relates to RGBD sensor and outfit There are Windows operating system and the computer of corresponding RGBD sensor interface.In terms of target following, by calculating to CAMSHIFT Method carries out the improvement of depth data fusion, greatly reduces the sensitiveness to illumination variation for the algorithm, enhances the Shandong of track algorithm Rod and real-time, have significantly attenuated the background object interference close with color of object.Compare other contact robot localization Method, this method adopts contactless measurement, and calculating speed, up to 25 frames per second, has that calculating speed is fast, accurate positioning Advantage；Change for environment has stronger adaptability, is directed to different target, it is strong that this method has extensibility Advantage.

Brief description

Fig. 1 is the inventive method overall flow figure.

Fig. 2 merges the image segmentation flow chart of depth data for the inventive method.

Fig. 3 improves CAMSHIFT tracking flow chart for the inventive method.

Specific embodiment

Below in conjunction with the accompanying drawing in the embodiment of the present invention, the technical scheme in the embodiment of the present invention is carried out clear, complete Whole description is it is clear that described embodiment is only a part of embodiment of the present invention, rather than whole embodiments.Ability The every other embodiment that domain ordinary person is obtained under the premise of not making creative work, belongs to the protection of the present invention Scope.

Embodiment：A kind of object localization method based on RGBD.

In the present embodiment, using second generation Kinect sensor, including RGB camera, infrared receiver, infrared Line transmitter and microphone array, note RGBD sensor world coordinate system is O_nX_nY_nZ_n, abbreviation N coordinate system, N coordinate origin Sensor color camera central point, the Z axis of N coordinate system perpendicular to Kinect front panel, X-axis along front panel to the left, Y-axis edge Front panel is downward.

Shown in reference Fig. 1, a kind of object localization method based on RGBD, comprise the following steps：

Hardware environment is set up, and equipment initializes；

Obtain depth image in flight course for the present frame aircraft and coloured image using RGBD sensor, based on depth Degrees of data is split to depth image, obtains the target area in depth image, is set to SearchRect_depth, and by its It is mapped in coloured image, corresponding color image parts are designated as SearchRectROI；

Calculate position in coloured image for the target using CAMSHIFT method in coloured image SearchRectROI (x, y)；

Calculate the world coordinates (x of current goal in conjunction with depth data_w, y_w, z_w)；

The world coordinates of current goal is sent to aircraft by communication system ZigBee, flying vehicles control itself is transported Dynamic.

In the present embodiment, described hardware environment is set up, and comprises the following steps：

By fixing for RGB sensor on the ceiling so as to front panel parallel to ground and with ground distance at 0.8 meter to 4 Between rice；

Colour-coded is fixed on mobile robot center.

With reference to shown in Fig. 2, described based on depth data, depth image is split, comprise the following steps：

Note former frame positioning result (x_w, y_w, z_w) in target depth value z_wFor depth_pre, scheme in present frame depth image As the depth of arbitrfary point (i, j) is depth (i, j), obtain present frame depth data；

Registered depth threshold value is T, a width of depth_cols of depth image, a length of depth_rows of depth image, depth image In a certain pixel gray value be depth_seg (i, j), present frame depth data is traveled through, as follows to depth Image carries out Range Image Segmentation：

$depth\_seg(i,j)=\left\{\begin{array}{c}0,\left|depth\right(i,j)-depth\_pre|>T\\ 1,\left|depth\right(i,j)-depth\_pre|<T\end{array}\right.$

Wherein i=0,1,2 ... depth_cols, j=0,1,2 ... depth_rows；

Carry out connected domain detection in depth image depth_seg (i, j) through over-segmentation, choose the maximum connection of area Domain, and obtain the boundary rectangle of the maximum connected domain of area, the target area SearchRect_depth as in depth image；

By depth image and coloured image coordinate corresponding relation, obtain from coloured image and SearchRect_depth Corresponding search rectangular region SearchRectROI.

In the present embodiment, described depth image is corrected by camera lens for RGB sensor with coloured image coordinate corresponding relation Mapping relations between the depth data obtaining and color image pixel point it is known that in coloured image a bit (i, j), by this Plant depth information depth (i, j) that mapping relations can obtain this point.

With reference to shown in Fig. 3, calculated using CAMSHIFT method in described coloured image SearchRectROI and follow the tracks of in target Heart position O, remembers its image coordinate (x, y), comprises the following steps：

Object initialization, chooses target following rectangle frame trackWindow_init manually, follows the tracks of target position in the present invention The green panel at quadrotor center；

To carrying out CAMSHIFT search in SearchRectROI, obtain new target location trackWindow, calculate The area trackWindow.area of trackWindow, if trackWindow.area is more than threshold value T_area then it represents that following the tracks of Success, is the central point of trackWindow rectangle frame according to position in coloured image for the target's center, i.e. (x, y), if TrackWindow.area is less than threshold value T_area then it represents that following the tracks of unsuccessfully, and it is upper for reinitializing trackWindow_init Tracking result trackWindow_pre of one frame, and re-start CAMSHIFT search, obtain new target location trackWindow.

In the present embodiment, described threshold value is the 30% of initial trackWindow_init area, that is,

T_area=0.3*trackWindow_init.area.

In the present embodiment, described combination depth data calculates coordinate (x in N coordinate system for the current goal_w, y_w, z_w), bag Include following steps：

Note W and H is respectively width and height, W=1920, the H=1080 in Kinect of RGB sensor color image, will scheme As Coordinate Conversion becomes with image coordinateCoordinate value (x for initial point₁, y₁), it is calculated as follows:

$y_1=\frac{H}{2}-y,x_1=\frac{W}{2}-x;$

The field range of note RGBD colour imagery shot is horizontal view angle α_h, vertical angle of view α_v, α in Kinect_h=70 °, α_v =60 °, thus can try to achieve the corresponding field of view angle of every pixel is:

Level：Vertically：

And then try to achieve current pixel point (x₁, y₁) corresponding to level angle θ_hWith vertical rotational angle theta_v：

$\begin{array}{cc}{\mathrm{\&theta;}}_h=\stackrel{\&OverBar;}{{\mathrm{\&alpha;}}_h}\&CenterDot;x_1& {\mathrm{\&theta;}}_v=\stackrel{\&OverBar;}{{\mathrm{\&alpha;}}_v}\&CenterDot;y_1\end{array};$

According to Kinect depth information z_wWorld coordinates (the x of current goal can be tried to achieve_w, y_w, z_w)：

x_w=z_w·tanθ_hy_w=z_w·tanθ_v.

The above is presently preferred embodiments of the present invention, but the present invention should not be limited to this embodiment and accompanying drawing institute is public The content opened, thus every without departing from complete equivalent or modification under spirit disclosed in this invention, both fall within present invention protection Scope.

Claims (7)

1. a kind of object localization method based on RGBD is it is characterised in that comprise the following steps：

Hardware environment is set up, and equipment initializes；

Obtain depth data and the coloured image of present frame mobile robot using RGBD sensor, based on depth data to depth Image is split, and obtains the target area in depth image, is set to SearchRect_depth, and maps that to cromogram In picture, corresponding color image parts are designated as SearchRectROI；

Calculate position (x, y) in coloured image for the target using CAMSHIFT method in coloured image SearchRectROI；

Calculate the world coordinates (x of current goal in conjunction with depth data_w, y_w, z_w)；

World coordinates (x by current goal_w, y_w, z_w) mobile robot is sent to by communication system, robot controls itself Motion.

2. the object localization method based on RGBD as claimed in claim 1 is it is characterised in that described hardware environment is set up, including Following steps：

By fixing for RGB sensor on the ceiling so as to front panel parallel to ground and with ground distance 0.8 meter to 4 meters it Between；

Colour-coded is fixed on mobile robot center.

3. as claimed in claim 1 the object localization method based on RGBD it is characterised in that described based on depth data to depth Image is split, and comprises the following steps：

Note former frame positioning result (x_w, y_w, z_w) in target depth value z_wFor depth_pre, in present frame depth image, image is appointed The depth of meaning point (i, j) is depth (i, j), obtains present frame depth data；

Registered depth threshold value be T, a width of depth_cols of depth image, a length of depth_rows of depth image, in depth image certain One pixel gray value is depth_seg (i, j), present frame depth data is traveled through, as follows to depth image Carry out Range Image Segmentation：

$depth\_seg(i,j)=\left\{\begin{array}{c}0,\left|depth\right(i,j)-depth\_pre|>T\\ 1,\left|depth\right(i,j)-depth\_pre|<T\end{array}\right.$

Wherein i=0,1,2 ... depth_cols, j=0,1,2 ... depth_rows；

Carry out connected domain detection in depth image depth_seg (i, j) through over-segmentation, choose the maximum connected domain of area, And obtain the boundary rectangle of the maximum connected domain of area, the target area SearchRect_depth as in depth image；

By depth image and coloured image coordinate corresponding relation, obtain corresponding with SearchRect_depth from coloured image Search rectangular region SearchRectROI.

4. the object localization method based on RGBD as claimed in claim 3 is it is characterised in that described depth image and cromogram As coordinate corresponding relation corrects the mapping between the depth data obtaining and color image pixel point for RGB sensor by camera lens Relation it is known that a bit (i, j) in coloured image, by this mapping relations can obtain this point depth information depth (i, j).

5. the object localization method based on RGBD as claimed in claim 1 is it is characterised in that in described coloured image Calculated using CAMSHIFT method in SearchRectROI and follow the tracks of target's center position O, i.e. (x, y), comprise the following steps：

Run if first, then need to carry out object initialization, choose target following rectangle frame trackWindow_ manually Init, follows the tracks of target and should have specific color characteristic, and following the tracks of target in the present invention is to be fixed on the colored mark at robot center Note；

To carrying out CAMSHIFT search in SearchRectROI, obtain new target location trackWindow, calculate The area trackWindow.area of trackWindow, if trackWindow.area is more than threshold value T_area then it represents that following the tracks of Success, obtains trackWindow centre of form coordinate (x, y), if trackWindow.area is less than threshold value T_area then it represents that following the tracks of Failure, reinitializes tracking result trackWindow_pre that trackWindow_init is previous frame, and re-starts CAMSHIFT searches for, and obtains new target location trackWindow.

6. the object localization method based on RGBD as claimed in claim 5 is it is characterised in that described threshold value is initial The 30% of trackWindow_init area, that is,

T_area=0.3*trackWindow_init.area.

7. the object localization method based on RGBD as claimed in claim 1 is it is characterised in that described combination depth data calculates World coordinates (the x of current goal_w, y_w, z_w), comprise the following steps：

Note W and H is respectively width and the height of RGB sensor color image, and image coordinate is converted into image coordinateFor former Coordinate value (the x of point₁, y₁), it is calculated as follows:

$y_1=\frac{H}{2}-y,x_1=\frac{W}{2}-x;$

The field range of note RGBD colour imagery shot is horizontal view angle α_h, vertical angle of view α_v, thus can try to achieve that every pixel is corresponding to be regarded Rink corner degree is:

Level：Vertically：

And then try to achieve current pixel point (x₁, y₁) corresponding to level angle θ_hWith vertical rotational angle theta_v：

$\begin{array}{cc}{\mathrm{\&theta;}}_h=\stackrel{\&OverBar;}{{\mathrm{\&alpha;}}_h}\&CenterDot;x_1& {\mathrm{\&theta;}}_v=\stackrel{\&OverBar;}{{\mathrm{\&alpha;}}_v}\&CenterDot;y_1\end{array};$

According to depth information z_wWorld coordinates (the x of current goal can be tried to achieve_w, y_w, z_w)：

x_w=z_w·tanθ_hy_w=z_w·tanθ_v.