CN109801309B - Obstacle sensing method based on RGB-D camera - Google Patents
Obstacle sensing method based on RGB-D camera Download PDFInfo
- Publication number
- CN109801309B CN109801309B CN201910011207.1A CN201910011207A CN109801309B CN 109801309 B CN109801309 B CN 109801309B CN 201910011207 A CN201910011207 A CN 201910011207A CN 109801309 B CN109801309 B CN 109801309B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- image
- obtaining
- rgb
- adopting
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
The invention discloses an obstacle sensing method based on an RGB-D camera, which comprises the steps of collecting an RGB image and a depth image of an object by adopting the RGB-D camera, and calculating world coordinates of a three-dimensional space corresponding to pixel coordinates of object imaging according to a pinhole camera model; separating out the background by adopting a maximum inter-class variance method and removing; removing ground interferents based on the geometric model; removing speckle from the depth image by adopting morphological operation; and (5) obtaining convex hulls of all the obstacles by adopting a Graham's Scan method, and obtaining information such as the positions and the sizes of the obstacles. Tracking the obstacle by adopting a CamShift algorithm, obtaining an observed value of the pose of the obstacle, obtaining a predicted value of the pose according to the speed of the obstacle, optimally estimating the pose of the obstacle by adopting a Kalman filtering algorithm, and estimating the speed of the obstacle according to the optimized pose. The invention has stronger instantaneity and applicability.
Description
Technical Field
The invention relates to the field of computer vision, in particular to an obstacle sensing method based on an RGB-D camera.
Background
The working environment of the mobile robot is usually uncertain and unstructured, the mobile robot needs to realize the perception of the external environment to acquire the external environment information so as to complete path planning, and finally, safe navigation is realized. The existing obstacle sensing method based on computer vision has the defects of large calculated amount and poor real-time performance, and can not meet the navigation application requirements when the mobile robot executes tasks. The RGB-D camera is an emerging visual sensor in recent years, and compared with the traditional laser radar and ultrasonic sensor, the RGB-D camera has low cost and can provide rich information, and the RGB-D camera can be used for simultaneously acquiring the optical image of the surrounding environment and the position information of an object on the optical image, so that the RGB-D camera is increasingly valued in the field of indoor navigation of mobile robots.
Disclosure of Invention
In order to overcome the defects and shortcomings in the prior art, the invention provides an obstacle sensing method based on an RGB-D camera,
the invention adopts the following technical scheme:
an obstacle sensing method based on an RGB-D camera comprises the following steps:
s1, acquiring an RGB image and a depth image of an object according to an RGB-D camera, reading color information and distance information of the object, calculating three-dimensional world coordinates corresponding to pixel coordinates of object imaging according to a pinhole camera model, separating out a background of the depth image by adopting a maximum inter-class variance method, and removing;
s2, removing ground interference objects on the image with the background removed based on a geometric model;
s3, removing noise and speckles from the depth image by morphological operation on the image from which the ground interference is removed;
s4, obtaining convex hulls of all barriers by using a Graham' S Scan method on the images with noise and speckle removed, and obtaining information of the barriers, wherein the information comprises the positions and the sizes of the barriers;
s5, tracking the obstacle by adopting a CamShift algorithm, and obtaining an observed value of the pose of the obstacle;
s6, obtaining a predicted value of the pose of the obstacle according to the speed of the obstacle, optimally estimating the pose of the obstacle by applying a Kalman filtering algorithm, and estimating the speed of the obstacle according to the optimized pose.
The method for separating out the background by adopting the maximum inter-class variance method and removing the background comprises the following specific steps:
and obtaining an optimal threshold value of the depth image by a maximum inter-class variance method, dividing the background and the target of the depth image according to the threshold value, and removing the background.
The morphological operation comprises an open operation and a closed operation, noise is removed by adopting the open operation, and then speckle is removed by adopting the closed operation.
In S4, specifically:
s4.1, ordering the point sets: find the point (x) with the smallest abscissa value among all the points after the background is removed 0 ,y 0 ) As a reference point S [0]]Find the polar angle atan2y-y of all other points (x, y) relative to the reference point 0 ,x-x 0 ) Ordering all points according to the obtained polar angle to obtain S1],S[2],…,S[n]；
S4.2, stack scanning: establishing a stack, wherein S0, S1 and S2 are used as initial stack points, new points are added once according to a well-arranged order to obtain edges, if the left-turning relation is met with the last edge, stacking is continued, if the right-turning relation is met, stacking is released until the left-turning relation is formed, stacking is continued, and after all the points are processed, the points in the stack are convex hulls;
s4.3, threshold processing: and calculating the area of the obtained convex hull, and when the area is smaller than a certain threshold value, not taking the area into consideration, and finally outputting the convex hull coordinates of the obstacle.
The CamShift algorithm in S5 is to track the obstacle by using a color histogram according to the color information in the RGB image corresponding to the obstacle.
In the step S6, the kalman filtering uses the position of the obstacle as a state variable, and assumes that the movement of the obstacle is uniform movement in a shorter time interval, and considers the change of the acceleration of the obstacle as white gaussian noise.
The invention has the beneficial effects that:
the invention adopts the computer vision technology and the Kalman filtering algorithm to realize the identification and tracking of the obstacle, has low cost and strong real-time performance, and can improve the adaptability of the mobile robot to the dynamic environment.
Drawings
FIG. 1 is a workflow diagram of the present invention;
FIG. 2 is a pinhole camera imaging model diagram of the RGB-D camera of the present invention;
FIGS. 3 (a) -3 (d) are schematic illustrations of the Graham's Scan method of solving convex hulls according to the present invention;
fig. 4 is a flow chart of the present invention for tracking an obstacle using the CamShift algorithm.
Detailed Description
The present invention will be described in further detail with reference to examples and drawings, but embodiments of the present invention are not limited thereto.
Examples
As shown in fig. 1, 2, 3 (a) -3 (D) and 4, an RGB-D camera-based obstacle sensing method is provided, in which an optimal threshold of a depth image can be obtained by a maximum inter-class variance method, a background and a target in the depth image can be roughly divided according to the threshold, three-dimensional space coordinates of a scene point corresponding to each pixel point on the image can be calculated according to a pinhole camera model, a height threshold is set, all scene points below the threshold are considered as ground interference areas, noise is directly removed by an open operation in morphological operation, speckle in an obstacle is filled by a closed operation, and convex hull operation is performed on the obstacle by a Graham's Scan method to obtain information such as the position and the size of the obstacle. Aiming at a dynamic obstacle, tracking the obstacle by adopting a CamShift algorithm according to the corresponding color information of the extracted obstacle in an RGB image, obtaining an observed value of the pose of the obstacle, obtaining a predicted value of the pose of the obstacle according to the speed of the obstacle, optimally estimating the pose of the obstacle by adopting a Kalman filtering algorithm, wherein in the estimating process, a state variable of the obstacle consists of position coordinates of the state variable, and the movement of the obstacle is assumed to be uniform movement in a shorter time interval, so that the change of the acceleration of the obstacle is considered to be Gaussian white noise. And after the obstacle tracking is completed, estimating the speed of the obstacle according to the optimized pose.
As shown in fig. 1, the method comprises the following steps:
according to the RGB image and the depth image acquired by the RGB-D camera, color information and distance information of an object can be read, the color information and the distance information of the object can be read, and then three-dimensional world coordinates corresponding to pixel coordinates of object imaging are calculated according to a pinhole camera model.
Referring to FIG. 2, O-x-y-z is the camera coordinate system, O is the camera optical center, and is also the pinhole in the pinhole model. The three-dimensional space point P in the real world falls on the physical imaging plane O '-x' -y '-z' after being projected through the aperture O, and the distance from the physical imaging plane to the aperture is f (focal length).
The specific calculation mode is as follows: let a pixel coordinate p= [ u v ]] T The depth value is d, and the internal parameter of the known camera is f x 、f y 、c x 、c y The pixel coordinates and the three-dimensional coordinates P= [ X Y Z can be obtained according to the principle of similar triangles] T Coordinate conversion relation of (2)
Secondly, separating out the background by adopting a maximum inter-class variance method and removing the background;
let the segmentation threshold of foreground (i.e. target) and background be T, and the proportion of foreground points to the whole image be w 0 The average gray scale is mu 0 The method comprises the steps of carrying out a first treatment on the surface of the The proportion of the background point number to the whole image is w 1 The average gray scale is mu 1 The overall average gray level of the image is denoted μ, and the inter-class variance is denoted σ.
σ＝w 0 w 1 (μ 0 -μ 1 ) 2 Traversing the threshold T maximizes the variance σ, while maximizing the foreground and background differences and minimizing the misclassification probability, the threshold T at this time may be considered the optimal threshold.
The background and the target in the depth image can be approximately segmented according to the threshold.
And thirdly, removing ground interferents based on the geometric model.
The specific model is as follows: let the height of the depth camera optical center from the ground be H, and the height threshold be Δh. The three-dimensional coordinate obtained after a pixel point P in the image passes through a pinhole imaging model is P= [ X ] P Y P Z P ] T The coordinate from the available point P to the ground is h P ＝H-Z P . If h p And (3) considering that the pixel belongs to a ground interference area and directly removing the pixel.
And fourthly, removing noise and speckles of the depth image by adopting morphological operation, wherein the step of removing noise by adopting open operation and filling speckles of the barrier by adopting closed operation.
The specific definition is as follows: corresponding to the image X and the structural element S, the open operation of S on the image X is expressed as X DEG S, and the close operation of S on the image X is expressed as X.S, wherein
Fifthly, obtaining convex hulls of all the obstacles by adopting a Graham's Scan method, and obtaining information of the obstacles, wherein the information comprises positions and sizes of the obstacles, and the positions and the sizes are shown in the figures (a) -3 (d);
s5.1 point set ordering: find the point (x) with the smallest abscissa value among all the points after the background is removed 0 ，y 0 ) As a reference point S [0]]Find the polar angle atan2 (y-y) of all other points (x, y) relative to the reference point 0 ,x-x 0 ) Ordering all points according to the obtained polar angle to obtain S1],S[2],…,S[n]。
S5.2 stack scan: and building a stack, wherein S0, S1 and S2 are used as initial stack points, new points are added once according to a well-arranged order to obtain edges, if the left-turning relation is met with the previous edge, stacking is continued, if the right-turning relation is met, stacking is released until the left-turning relation is formed, and stacking is continued. And after all the points are processed, the points in the stack are convex hulls.
S5.3, thresholding, namely calculating the area of the obtained convex hull, and when the area is smaller than a certain threshold value, not taking the area into consideration, and finally outputting the convex hull coordinates of the obstacle.
And sixthly, obtaining a predicted value of the pose of the obstacle according to the speed of the obstacle.
S6.1, initializing the position and the size of a search window as a target area;
s6.2 back projection: and converting the target area image from RGB space to HSV space to obtain probabilities of different H components, replacing each pixel value in the target area with the probability of color occurrence, and replacing the non-target area pixel value with 0 to obtain the color probability distribution image.
S6.3, running a meanshift algorithm iteration: zero order moment M of color histogram through search window 00 And a first moment M 10 ,M 01 Obtaining the centroid position as
And adjusting the size of the search window according to the zero-order moment, moving the search window from the center to the mass center, if the moving distance is larger than a preset threshold value, recalculating the mass center of the window after adjustment, adjusting the size and the position of the window until the distance between the center of the search window and the mass center is within the preset threshold value range or the maximum iteration number is reached, reaching a convergence condition, and taking the position and the size of the obtained search window as a target area of the next frame of image to perform new target search.
And seventh, performing optimal estimation on the pose of the obstacle by applying a Kalman filtering algorithm, and estimating the speed of the obstacle according to the optimized pose, as shown in fig. 4.
In the estimation process, the state variable of the obstacle is constituted by its position coordinates, and the change in the acceleration of the obstacle is considered as white gaussian noise on the assumption that the movement of the obstacle is uniform movement in a short time interval. The following state and observation equations are given:
X t ＝F t,t-1 X t-1 +W t-1
Y t ＝H t X t +V t
wherein X is t And X t-1 The state variables consisting of the position coordinates and the speed in the X, y and z directions at t and t-1 respectively, namely X t ＝[x s ,y s ,z s ,x v ，y v ，z v ]；Y t As an observed variable of the target position at t, i.e. Y t ＝[x s ′，y s ′，z s ′]；F t，t-1 For state transition matrix, H t Is an observation matrix; w is state process noise, gaussian white noise and covariance Q; v is observation process noise, also Gaussian white noise, and covariance is R.
The formulas required to implement the kalman filtering are divided into a prediction group and an update group.
The prediction set includes the following formula:
Σ t，t-1 ＝F t，t-1 Σ t-1 F t，t-1 T +Q t-1
wherein the method comprises the steps of
Sigma is a predicted value based on the t-1 time state versus the t-time state t，t-1 Is a predicted value of covariance at time k based on covariance at time t-1.
The update group includes the following formula:
Σ t ＝(1-K t H t )Σ t,t-1
wherein the method comprises the steps of
To estimate the optimal state at time t, K t For filtering gain arrays, Σ t And the optimal covariance matrix at the moment t.
The obstacle sensing method based on the RGB-D camera has the advantages of good real-time performance and strong practicability.
The embodiments described above are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the embodiments described above, and any other changes, modifications, substitutions, combinations, and simplifications that do not depart from the spirit and principles of the present invention should be made in the equivalent manner, and are included in the scope of the present invention.
Claims (4)
1. An obstacle sensing method based on an RGB-D camera is characterized by comprising the following steps:
s1, acquiring an RGB image and a depth image of an object according to an RGB-D camera, reading color information and distance information of the object, calculating three-dimensional world coordinates corresponding to pixel coordinates of object imaging according to a pinhole camera model, separating out a background of the depth image by adopting a maximum inter-class variance method, and removing;
s2, removing ground interference objects on the image with the background removed based on a geometric model;
the specific model is as follows: setting the height of the optical center of the depth camera from the ground as H, and setting the height threshold as delta H; the three-dimensional coordinate obtained after a pixel point P in the image passes through a pinhole imaging model is P= [ X ] P Y P Z P ] T The coordinate from the available point P to the ground is h P ＝H-Z p； If h p And (2) considering that the pixel point belongs to a ground interference area and directly removing the pixel point;
s3, removing noise and speckles from the depth image by morphological operation on the image from which the ground interference is removed;
s4, obtaining convex hulls of all barriers by using a Graham' S Scan method on the images with noise and speckle removed, and obtaining information of the barriers, wherein the information comprises the positions and the sizes of the barriers;
the method comprises the following steps:
s4.1 ordering the point sets: find the point (x) with the smallest abscissa value among all the points after the background is removed 0 ,y 0 ) As a reference point S [0]]Find the polar angle atan2 (y-y) of all other points (x, y) relative to the reference point 0 ,x-x 0 ) Ordering all points according to the obtained polar angle to obtain S1],S[2],…,S[n]；
S4.2 stack scan: establishing a stack, wherein S0, S1 and S2 are used as initial stack points, new points are added once according to a well-arranged order to obtain edges, if the left-turning relation is met with the last edge, stacking is continued, if the right-turning relation is met, stacking is released until the left-turning relation is formed, stacking is continued, and after all the points are processed, the points in the stack are convex hulls;
s4.3, thresholding, namely calculating the area of the obtained convex hull, and when the area is smaller than a certain threshold value, not taking the area into consideration, and finally outputting the convex hull coordinates of the obstacle;
s5, tracking the obstacle by adopting a CamShift algorithm, and obtaining an observed value of the pose of the obstacle; the method comprises the following steps:
s5.1, initializing the position and the size of a search window as a target area;
s5.2, back projection: converting the target area image from RGB space to HSV space to obtain probabilities of different H components, replacing each pixel value in the target area with the probability of color occurrence, and replacing the non-target area pixel value with 0 to obtain a color probability distribution image;
s5.3, running a meanshift algorithm iteration: zero order moment M of color histogram through search window 00 And a first moment M 10 ,M 01 Obtaining the centroid position as
Adjusting the size of a search window according to the zero-order moment and moving the search window from the center to the mass center, if the moving distance is larger than a preset threshold value, recalculating the mass center of the window after adjustment, adjusting the size and the position of the window until the distance between the center of the search window and the mass center is within the preset threshold value range or the maximum iteration number is reached, reaching a convergence condition, taking the position and the size of the obtained search window as a target area of a next frame of image, and carrying out new target search;
s6, obtaining a predicted value of the pose of the obstacle according to the speed of the obstacle, optimally estimating the pose of the obstacle by applying a Kalman filtering algorithm, and estimating the speed of the obstacle according to the optimized pose.
2. The method for sensing an obstacle according to claim 1, wherein the method for separating out and rejecting the background by using the maximum inter-class variance method comprises:
and obtaining an optimal threshold value of the depth image by a maximum inter-class variance method, dividing the background and the target of the depth image according to the threshold value, and removing the background.
3. The obstacle sensing method of claim 1, wherein the morphological operations include an open operation and a closed operation, the open operation being used to remove noise, and then the closed operation being used to remove speckle.
4. The obstacle sensing method according to claim 1, wherein the kalman filter takes the position of the obstacle as a state variable, and considers the change of the acceleration of the obstacle as white gaussian noise by assuming the motion of the obstacle as uniform motion in a short time interval.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910011207.1A CN109801309B (en) | 2019-01-07 | 2019-01-07 | Obstacle sensing method based on RGB-D camera |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910011207.1A CN109801309B (en) | 2019-01-07 | 2019-01-07 | Obstacle sensing method based on RGB-D camera |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109801309A CN109801309A (en) | 2019-05-24 |
| CN109801309B true CN109801309B (en) | 2023-06-20 |
Family
ID=66558492
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910011207.1A Active CN109801309B (en) | 2019-01-07 | 2019-01-07 | Obstacle sensing method based on RGB-D camera |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109801309B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110262487B (en) * | 2019-06-12 | 2022-09-23 | 达闼机器人股份有限公司 | Obstacle detection method, terminal and computer readable storage medium |
| CN111428651B (en) * | 2020-03-26 | 2023-05-16 | 广州小鹏汽车科技有限公司 | Obstacle information acquisition method and system for vehicle and vehicle |
| CN112070700B (en) * | 2020-09-07 | 2024-03-29 | 深圳市凌云视迅科技有限责任公司 | Method and device for removing protrusion interference noise in depth image |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2015024407A1 (en) * | 2013-08-19 | 2015-02-26 | 国家电网公司 | Power robot based binocular vision navigation system and method based on |
| CN108830191A (en) * | 2018-05-30 | 2018-11-16 | 上海电力学院 | Based on the mobile robot SLAM method for improving EMM and ORB algorithm |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104899869B (en) * | 2015-05-14 | 2017-09-01 | 浙江大学 | Plane and disorder detection method based on RGB D cameras and attitude transducer |
| CN106570903B (en) * | 2016-10-13 | 2019-06-18 | 华南理工大学 | A kind of visual identity and localization method based on RGB-D camera |
-
2019
- 2019-01-07 CN CN201910011207.1A patent/CN109801309B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2015024407A1 (en) * | 2013-08-19 | 2015-02-26 | 国家电网公司 | Power robot based binocular vision navigation system and method based on |
| CN108830191A (en) * | 2018-05-30 | 2018-11-16 | 上海电力学院 | Based on the mobile robot SLAM method for improving EMM and ORB algorithm |
Non-Patent Citations (1)
| Title |
|---|
| 基于稀疏直接法和图优化的移动机器人SLAM;吴玉香 等;仪器仪表学报;第39卷(第4期);第257-263页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109801309A (en) | 2019-05-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107844750B (en) | Water surface panoramic image target detection and identification method | |
| CN109544636B (en) | Rapid monocular vision odometer navigation positioning method integrating feature point method and direct method | |
| CN109801309B (en) | Obstacle sensing method based on RGB-D camera | |
| CN111260683A (en) | Target detection and tracking method and device for three-dimensional point cloud data | |
| CN113156421A (en) | Obstacle detection method based on information fusion of millimeter wave radar and camera | |
| CN113483747B (en) | Improved AMCL (advanced metering library) positioning method based on semantic map with corner information and robot | |
| CN108171715B (en) | Image segmentation method and device | |
| CN106780542A (en) | A kind of machine fish tracking of the Camshift based on embedded Kalman filter | |
| CN113985445A (en) | 3D target detection algorithm based on data fusion of camera and laser radar | |
| CN109325444B (en) | Monocular texture-free three-dimensional object posture tracking method based on three-dimensional geometric model | |
| CN110717445B (en) | Front vehicle distance tracking system and method for automatic driving | |
| CN111897349A (en) | Underwater robot autonomous obstacle avoidance method based on binocular vision | |
| CN110021029B (en) | Real-time dynamic registration method and storage medium suitable for RGBD-SLAM | |
| CN113724299B (en) | Method for tracking three-dimensional track of target by mobile robot based on electrohydraulic adjustable focus lens | |
| Lin et al. | Construction of fisheye lens inverse perspective mapping model and its applications of obstacle detection | |
| Li et al. | Road markings extraction based on threshold segmentation | |
| CN114399675A (en) | Target detection method and device based on machine vision and laser radar fusion | |
| CN113034600A (en) | Non-texture planar structure industrial part identification and 6D pose estimation method based on template matching | |
| CN113593035A (en) | Motion control decision generation method and device, electronic equipment and storage medium | |
| CN114972427A (en) | Target tracking method based on monocular vision, terminal equipment and storage medium | |
| CN112847349B (en) | Robot walking control method and device | |
| CN114119729A (en) | Obstacle identification method and device | |
| Fucen et al. | The object recognition and adaptive threshold selection in the vision system for landing an unmanned aerial vehicle | |
| CN112733678A (en) | Ranging method, ranging device, computer equipment and storage medium | |
| CN112395985B (en) | Ground unmanned vehicle vision road detection method based on unmanned aerial vehicle image |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |