CN109765901A - Dynamic cost digital map navigation method based on line laser and binocular vision - Google Patents
Dynamic cost digital map navigation method based on line laser and binocular vision Download PDFInfo
- Publication number
- CN109765901A CN109765901A CN201910119415.3A CN201910119415A CN109765901A CN 109765901 A CN109765901 A CN 109765901A CN 201910119415 A CN201910119415 A CN 201910119415A CN 109765901 A CN109765901 A CN 109765901A
- Authority
- CN
- China
- Prior art keywords
- map
- robot
- coordinate
- coordinate system
- dimensional
- 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.)
- Pending
Links
Landscapes
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The dynamic cost digital map navigation method based on line laser and binocular vision that the invention discloses a kind of, comprising steps of 1) establishing two-dimensional grid cartographic model；2) mobile robot pose model is established；3) laser radar data model is established；4) calibration binocular camera and acquisition three-dimensional coordinate；5) dynamic cost map is created using the three-dimensional coordinate that step 4) obtains；6) path planning is carried out using dwa algorithm and avoidance is navigated.The present invention can create cost map, the barrier that perception two-dimensional laser radar cannot detect by binocular ranging；It can be under known environment two-dimensional grid map environment, use the barrier for being above and below two-dimensional laser Radar Plane in binocular camera detection environment, and barrier is mapped in environmental map, pass through dwa algorithm, the guidance path line of avoiding obstacles is obtained, mobile robot can be made to arrive at the destination during the motion along the path for avoiding obstacle.
Description
Technical field
The present invention relates to the technical field of robot navigation's avoidance, refer in particular to a kind of based on line laser and binocular vision
Dynamic cost digital map navigation method.
Background technique
As intelligent robot technology is flourished, robot has in production and life to be more and more widely used.
According to the difference of application environment, robot is divided into plurality of classes, such as industrial robot often has multiple joints
And mechanical arm, the efficient work such as assembly, carrying is carry in factory；And service robot product, including it is wheeled
Mobile robot, anthropomorphic robot etc. can be used in household or the environment of commercialization, be developed for different using purpose
Different function, intelligent mobile trolley may be implemented unmanned, and singing and dancing entertainment interactive etc. may be implemented in anthropomorphic robot；
Unmanned plane is frequently utilized for military surveillance, remote sensing mapping etc.；It is more that there are also underwater robot, nanometer robot, agricultural robots etc.
The specialized robot of classification suffers from its more single-minded purposes.Wherein with the laser sensing for pushing mobile robot development
The continuous development and breakthrough of device technology, laser sensor are widely used in mobile robot field.In robot pair
The exploration of circumstances not known creates on map, and navigation avoidance, and laser has the critical role that do not replace.At present on the market
It is widely used and mainly two-dimensional laser at low cost, but its flatness makes robot probe flat less than lower than robot
The barrier in face and the hanging barrier for being higher than laser plane.
In order to make up deficiency of the single two-dimensional laser in mobile robot, more and more sensors are added to robot
On, wherein just there is camera.It can be with the function of ranging, naturally it is contemplated that using binocular camera using binocular camera
The barrier that two-dimensional laser can not detect is detected, to realize that mobile robot can avoid in actual scene that laser can not
The barrier detected.
In robot Real Time Obstacle Avoiding algorithm, dynamic cost map is most important, and dynamic cost map denotation is sensing
The barrier that device real-time detection arrives, in conjunction with the static map of foundation, so that it may detect the barrier letter around mobile robot
Breath, then in conjunction with path planning algorithm, calculates the shortest path that can be arrived at the destination.
And for robot software's system, the robot system frame of mainstream is based on ROS (The Robot at present
Operating System) system, which is provided with the data-interface of various mainstream sensors, also provides the control to robot
Interface processed can complete the experiment of all kinds of humanoid robots under true environment.And ROS system provides many basic functions
Packet, allows us further to do experiment and research on the basic framework of robot.
Summary of the invention
It is an object of the invention to overcome the shortcomings of that two-dimensional laser sensor can only detect single plane barrier, propose
A kind of dynamic cost digital map navigation method based on line laser and binocular vision, cannot be detected when navigating avoidance it is short, or
The hanging barrier of person can detecte these barriers using the binocular camera demarcated and calculate these barriers and arrives
The distance of head plane is imaged, then we are mapped to two-dimensional dynamic generation plus the three-dimensional generated by camera when navigating avoidance
Valence map layer manages the barrier of the detection of binocular vision by this layer, as regularly updated the barrier of vision in map
Barrier expired in map is deleted in position.In conjunction with the dynamic cost map of two laser, it is really generation that we, which can obtain,
The information of three-dimensional barrier, allows mobile robot to avoid the barrier that laser radar can't detect in boundary.
To achieve the above object, technical solution provided by the present invention are as follows: the dynamic generation based on line laser and binocular vision
Valence digital map navigation method, comprising the following steps:
1) two-dimensional grid cartographic model is established
The environmental map model using two-dimensional grid map as Environment Obstacles information is established, world coordinate system and map are established
The transformational relation of coordinate system；Wherein, model indicates Environment Obstacles information using two-dimensional grid map, and two-dimensional grid cartographic model exists
It is saved in the form of gray scale picture in system, and has recorded the height height of map picture in a model, map picture
Corresponding world coordinates (the X of width width and map lower left corner pixellowerleft,Ylowerleft), it defines in map picture
Top left corner pixel is the origin (0,0) of map coordinates system, and row coordinate is with map picture from top to bottom for positive direction, and column coordinate is with ground
Figure picture is from left to right positive direction, defines the origin of world coordinates in map reference
Place, resolution are map resolution ratio, and the x-axis direction of world coordinate system corresponds to the side that column coordinate increases in map coordinates system
To the y-axis direction of world coordinate system corresponds to the direction that row coordinate reduces in map coordinates system, and world coordinate system is with the rice in reality
For unit, each pixel has corresponded to the square two-dimensional surface region that a length and width in real world are 5cm, if pixel value
For white, then it represents that barrier is not present in the region, if pixel value is black, representing the region, there are barriers；It uses
Gmapping builds static map of the nomography building based on laser radar；
2) mobile robot pose model is established
It establishes using mobile robot center as the robot coordinate system of origin, establishes the robot position in two-dimensional coordinate plane
Appearance model, and the pose of robot in the environment is indicated in the form of coordinate system transformational relation；Wherein, the robot pose mould
For type using robot center as origin, robot center to robot positive direction is x-axis, and it is straight that right hand plane is established as unit of rice
Angular coordinate system, as robot coordinate system, and indicate with Pose (x, y, θ) pose of robot, x in formula, y represent robot
The coordinate of world coordinate system locating for the origin of coordinate system, θ indicate world coordinate system x-axis direction to robot coordinate system x-axis side
To angle, with counterclockwise for angle increase direction, then the coordinate in robot coordinate system corresponds in world coordinate system
Coordinate conversion is as follows:
In formula, x, y, θ are robot pose Pose (x, y, θ)；wx,wyFor the coordinate of world coordinate system；rx,ryFor machine
The coordinate of people's coordinate system；
3) laser radar data model is established
According to the riding position and direction of two-dimensional laser radar, laser radar data model is established, and is built according to step 1)
The data protocol of robot pose model and laser radar that vertical cartographic model and step 2) are established, realizes laser radar
The barrier data measured are simultaneously mapped in environmental map by measurement to environment distance；Wherein, the laser radar number
It is as follows according to model form:
Two-dimensional laser radar can scan the complaint message in a plane within the scope of 360 degree, two-dimensional laser radar scanning
Range can be up to 8 meters, and angular resolution is 1 degree, and in the data model of two-dimensional laser radar, transmitting data every time will be transmitted
360 floating datas counterclockwise transmit the distance of obstacle laser on every 1 angular direction since 0 degree of direction of laser radar
The distance of radar center, as unit of rice, and recording the obstacle distance radar center distance on i degree direction is ρi, when two
Dimension laser radar is centrally mounted at (0.08,0) in robot coordinate system, 0 degree of direction of laser radar and robot coordinate
The y-axis direction of system it is parallel and towards it is identical when, when robot is in position and posture Pose (x, y, θ), radar data ρiMapping
The coordinate into world coordinate system:
In formula, Xworld, Yworld are the coordinate in world coordinate system；
Also, by radar data ρiThe coordinate being finally mapped in map coordinates system:
In formula, height represents map picture row coordinate, and width represents map picture column coordinate, and resolution is represented
Map resolution ratio；
4) calibration binocular camera and acquisition three-dimensional coordinate
The photo that 10 groups of different two cameras are clapped using scaling board, by position of the angle point in image for finding scaling board
Set, come calculate two cameras internal reference and outer ginseng, the re-projection of 4 × 4 binocular camera is obtained eventually by combined calibrating
Matrixcx,cyIt is left camera as planar central coordinate, c'x,c'yIt is right camera as in plane
Heart coordinate；TxFor the centre distance of two cameras, f is camera focus；Then it is controlled using Block- matching Stereo Matching Algorithm
Disparity map, that is, depth map of two cameras gives parallax d and picture planar point (x, y), passes through formulaAcquire the three-dimensional coordinate P=(X/W, Y/W, Z/W) under camera coordinate system；
5) dynamic cost map is created using the three-dimensional coordinate that step 4) obtains
According to the definition of binocular vision system, using the optical center of the left camera in binocular camera as origin, optical axis direction
For z-axis, base direction is x-axis from left to right, as unit of rice, establishes right hand rectangular coordinate system in space, referred to as binocular coordinate system,
On obtaining image after the three-dimensional coordinate P=(x', y', z') of each pixel, what y' was indicated is exactly elevation information, and x' is corresponded to
Robot two-dimensional coordinate plane-y-axis, z' corresponding is x-axis direction, is screened according to y' lower than robot and is higher than robot
Barrier, it is then compressed to the two-dimensional surface where robot, remembers that the coordinate under robot coordinate system is r (x, y), tool
Body formula has r (x, y)=(z' ,-x'), obtains the world coordinate system coordinate (r for being compressed to robot two-dimensional surfacex,ry) after, root
According to the pose (robot of current robotx,roboty, θ), it is multiplied i.e. by rigid body translation matrix with plane coordinatesObtain coordinate (w of the binocular coordinate system in world coordinate systemx,wy)；It is creating
When building map, it is at the center of map coordinates system, according to world coordinate system that robot initial pose, which is the origin of world coordinate system,
With the relationship of map coordinates system, pose of the barrier under binocular coordinate system in map coordinates system is further calculated out, then
The barrier is added in grating map, and adds expansion radius, and this makes it possible to being created that the cost based on binocular vision
Figure；It can similarly be created that the cost map of two-dimensional laser using identical method, then merge two cost maps, it will be able to
Obtain the cost map for the barrier that laser and vision are seen in current environment；It can be kept away using the cost map robot
Barrier navigates to the target point in map；
6) finally it is exactly to select a target point, passes through the part dwa obstacle avoidance algorithm and carry out avoidance navigation: a, in robot control
The spatially carry out speed dispersion sampling of system；B, for each sample rate, look at using mobile one section of the sample rate away from
What can occur from after；C, each track of forward simulation is evaluated, evaluation criterion is: close to barrier, by close-target close to complete
Office path and speed；The illegal route is abandoned during evaluation；D, choose the track of highest scoring and send corresponding speed
To robot；E, step a-d is constantly repeated, until arriving at the destination.
Compared with prior art, the present invention have the following advantages that with the utility model has the advantages that
The method of the present invention can create cost map, the obstacle that perception two-dimensional laser radar cannot detect by binocular ranging
Object；It can be swashed under known environment two-dimensional grid map environment using two dimension is above and below in binocular camera detection environment
The barrier of optical radar plane, and barrier is mapped in environmental map, by dwa algorithm, obtain avoiding obstacles
Guidance path line can be such that mobile robot arrives at the destination during the motion along the path for avoiding obstacle.
Detailed description of the invention
Fig. 1 is two-dimensional grid map schematic diagram.
Fig. 2 is the corresponding true environment schematic diagram of map.
Fig. 3 is mobile robot hardware platform schematic diagram.
Fig. 4 is robot coordinate system's definition and 0 degree of direction schematic diagram of laser radar.
Fig. 5 is binocular calibration exemplary diagram.
Fig. 6 is the mounting means schematic diagram of laser radar and binocular camera.
Fig. 7 is actual environment obstacle schematic diagram.
Fig. 8 is the dynamic cost map that binocular camera is merged with laser radar.
Fig. 9 is the guidance path line schematic diagram being calculated in robot system.
Figure 10 avoids obstacle according to guidance path line for robot and reaches purpose result schematic diagram.
Specific embodiment
The present invention is further explained in the light of specific embodiments.
Dynamic cost digital map navigation method provided by the present invention based on line laser and binocular vision, including following step
It is rapid:
1) it establishes two-dimensional grid cartographic model: establishing the environmental map mould using two-dimensional grid map as Environment Obstacles information
Type establishes the transformational relation of world coordinate system and map coordinates system.Wherein, model indicates Environment Obstacles using two-dimensional grid map
Information, two-dimensional grid cartographic model is saved in the form of gray scale picture in systems, and has recorded map picture in a model
Height height, the width width of map picture and the corresponding world coordinates (X of map lower left corner pixellowerleft,
Ylowerleft), the origin (0,0) that top left corner pixel in map picture is map coordinates system is defined, row coordinate is with map picture from upper
To it is lower be positive direction, column coordinate, from left to right for positive direction, defines the origin of world coordinates in map reference with map picturePlace, resolution are map resolution ratio, and the x-axis direction of world coordinate system is accordingly
The direction that column coordinate increases in figure coordinate system, the y-axis direction of world coordinate system correspond to the side that row coordinate reduces in map coordinates system
To, world coordinate system as unit of the rice in reality, each pixel corresponded to a length and width in real world be 5cm just
Rectangular two-dimensional surface region, if pixel value is white, then it represents that barrier is not present in the region, if pixel value is black, generation
There are barriers in the table region.The static map based on laser radar can be constructed by building nomography using gmapping.
2) it establishes mobile robot pose model: establishing using mobile robot center as the robot coordinate system of origin, build
Robot pose model in vertical two-dimensional coordinate plane, and indicate in the form of coordinate system transformational relation robot in the environment
Pose.Wherein, the robot pose model, using robot center as origin, robot center to robot positive direction is x
Axis is established right hand plane right-angle coordinate, as robot coordinate system as unit of rice, and indicates machine with Pose (x, y, θ)
The pose of device people, wherein x, y represent the coordinate of world coordinate system locating for the origin of robot coordinate system, and θ indicates world coordinates
It is angle of the x-axis direction to robot coordinate system's x-axis direction, with the direction increased counterclockwise for angle, then robot coordinate system
In coordinate correspond to the coordinate in world coordinate system conversion it is as follows:
In formula, x, y, θ are robot pose Pose (x, y, θ)；wx,wyFor the coordinate of world coordinate system；rx,ryFor machine
The coordinate of people's coordinate system.
3) it establishes laser radar data model: according to the riding position and direction of two-dimensional laser radar, establishing laser radar
Data model, and the robot pose model and laser radar that the cartographic model and step 2) established according to step 1) are established
Data protocol, realize laser radar to the measurement of environment distance and the barrier data measured are mapped to environmental map work as
In.Wherein, the laser radar data model form is as follows:
Two-dimensional laser radar can scan the complaint message in a plane within the scope of 360 degree, general two-dimensional laser thunder
Up to scanning range up to 8 meters, angular resolution is 1 degree, and in the data model of two-dimensional laser radar, transmitting data every time will be passed
Defeated 360 floating datas, since 0 degree of direction of laser radar, the distance of obstacle counterclockwise transmitted on every 1 angular direction swashs
The distance at optical radar center, as unit of rice, and recording the obstacle distance radar center distance on i degree direction is ρi, when
Two-dimensional laser radar center is mounted at (0.08,0) in robot coordinate system, and 0 degree of direction of laser radar and robot are sat
Mark system y-axis direction it is parallel and towards it is identical when, when robot is in position and posture Pose (x, y, θ), radar data ρiIt reflects
It is mapped to coordinate in world coordinate system:
In formula, Xworld, Yworld are the coordinate in world coordinate system；
Also, by radar data ρiThe coordinate being finally mapped in map coordinates system,
In formula, height represents map picture row coordinate, and width represents map picture column coordinate, and resolution is represented
Map resolution ratio.
4) calibration binocular camera is with acquisition three-dimensional coordinate: the photo of 10 groups of different two cameras is clapped using scaling board,
By finding position of the angle point in image of scaling board, come calculate two cameras internal reference and outer ginseng, eventually by joint mark
Surely the re-projection matrix of 4 × 4 binocular camera is obtainedcx,cyIt is left camera as plane
Centre coordinate, c'x,c'yIt is right camera as planar central coordinate；TxFor the centre distance of two cameras, f is camera focus；Then
The disparity map i.e. depth map of two cameras in left and right is obtained using Block- matching Stereo Matching Algorithm, parallax d is given and a picture is flat
Face two-dimensional points (x, y), we can pass through formulaAcquire the three-dimensional coordinate P=(X/ under camera coordinate system
W,Y/W,Z/W)。
5) three-dimensional coordinate obtained using step 4) creates dynamic cost map: according to the definition of binocular vision system, with
The optical center of left camera in binocular camera is origin, and optical axis direction is z-axis, and base direction is x-axis from left to right, is with rice
Unit establishes right hand rectangular coordinate system in space, referred to as binocular coordinate system, the three-dimensional coordinate P of each pixel on obtaining image
After=(x', y', z'), what y' was indicated is exactly elevation information, x' correspond to robot two-dimensional coordinate plane-y-axis, z' is corresponding
It is x-axis direction, can be screened according to y' lower than robot and higher than the barrier of robot, it is then compressed to robot
The two-dimensional surface at place remembers that the coordinate under robot coordinate system is r (x, y), and specific formula has r (x, y)=(z' ,-x'), obtains
It is compressed to the world coordinate system coordinate (r of robot two-dimensional surfacex,ry) after, according to the pose (robot of current robotx,
roboty, θ), it is multiplied i.e. by rigid body translation matrix with plane coordinatesIt obtains double
Coordinate (w of the mesh coordinate system in world coordinate systemx,wy), when creating map, robot initial pose is world coordinate system
Origin, according to the relationship of world coordinate system and map coordinates system, can be further calculated out double at the center of map coordinates system
Pose of the barrier in map coordinates system under mesh coordinate system, then we add the barrier in grating map, and add
Add expansion radius, can thus be created that the cost map based on binocular vision.It can similarly be created using identical method
Then the cost map of two-dimensional laser out merges two cost maps, so that it may obtain laser and vision in current environment and be seen
The cost map of the barrier arrived.Using the cost map robot can avoidance navigate to the target point in map.
6) finally it is exactly to select a target point, passes through the part dwa obstacle avoidance algorithm and carry out avoidance navigation: a, in robot control
The spatially carry out speed dispersion sampling of system.B, for each sample rate, look at a bit of using sample rate movement
After what can occur.C, evaluate each track of forward simulation, evaluation criterion such as: close to barrier, by close-target close to the overall situation
Path and speed；The illegal route is abandoned during evaluation.D, choose the track of highest scoring and send corresponding speed to
Robot.E, step a-d is constantly repeated, until arriving at the destination.
Case study on implementation: the shifting based on binocular camera Yu two-dimensional laser radar is completed on KOBUKI mobile robot platform
Mobile robot navigation.
(1) two-dimensional grid cartographic model
Two-dimensional grid map as shown in Figure 1, storage information include the line number of every row number of pixels and map picture in total,
Each pixel represents the plane domain of a length and width 5cm in true environment in map, represents the region using black picture element and deposits
In obstacle, white pixel represents the region, and there is no obstacles.
In this example, it is highly 60 pixels that map picture width, which is 50 pixels, represents 2.5 meters one piece wide, long 3 meters of reality
Place is tested, as shown in Figure 2.
(2) mobile robot platform model
In the selection of mobile robot hardware platform, KOBUKI General Mobile robot base has been used, and
LSLIDAR2D laser radar and two 300,000 common pixel camera heads are as sensor acquisition range information, an X86 platform
The terminal of 4GB memory is as robot server-side, as shown in Figure 3.
To the setting of the mounting means of sensor and robot coordinate as shown in figure 4, being with robot center in this example
Origin establishes right hand plane coordinate system using robot positive direction as x-axis, and 0 degree of direction of laser radar is put and machine
The y-axis of device people's coordinate system is parallel.
(3) for the scaling board of binocular calibration, mean square error, mark calibration binocular camera and acquisition three-dimensional coordinate: are demarcated
It is as shown in Figure 5 to determine result Q.
(4) it is led based on binocular dynamic cost map and dynamic Laser cost map and combining for two-dimensional grid static map
Boat positioning.
In the implementation case: on robot platform, the binocular camera form fixed with laser radar is as shown in Figure 6.
True environment is as shown in Figure 7.And the obstacle information of global context is indicated using two-dimensional grid map in robot system, make
Barrier is detected simultaneously with binocular camera and two-dimensional laser radar, and is not interfere with each other, local environment complaint message is mapped
To in environmental map, the Global obstacle information within the scope of 5 meters of robot periphery is replaced using local disturbance's information, such as Fig. 8
Shown, the short chest of white is less than the obstacle of laser radar scanning plane in Fig. 7, it is detected to obtain by binocular camera, and
And it is mapped in the map of Fig. 8.Guidance path shown in Fig. 9 has so been obtained by dwa algorithm.According to the road of navigation programming
Diameter eventually arrives at terminal shown in Fig. 10.
Therefore, in conclusion the method for the present invention can create cost map by binocular ranging, two-dimensional laser radar is perceived
The barrier that cannot be detected；It can be under known environment two-dimensional grid map environment, using high in binocular camera detection environment
In with the barrier that is lower than two-dimensional laser Radar Plane, and barrier is mapped in environmental map, by dwa algorithm, is obtained
To the guidance path line of avoiding obstacles, mobile robot can be made to reach mesh along the path for avoiding obstacle during the motion
Ground, have actual promotional value, be worthy to be popularized.
Embodiment described above is only the preferred embodiments of the invention, and but not intended to limit the scope of the present invention, therefore
All shapes according to the present invention change made by principle, should all be included within the scope of protection of the present invention.
Claims (1)
1. the dynamic cost digital map navigation method based on line laser and binocular vision, which comprises the following steps:
1) two-dimensional grid cartographic model is established
The environmental map model using two-dimensional grid map as Environment Obstacles information is established, world coordinate system and map reference are established
The transformational relation of system；Wherein, model indicates Environment Obstacles information using two-dimensional grid map, and two-dimensional grid cartographic model is in system
In saved in the form of gray scale picture, and have recorded the height height of map picture, the width of map picture in a model
Corresponding world coordinates (the X of width and map lower left corner pixellowerleft,Ylowerleft), define upper left in map picture
Angle pixel is the origin (0,0) of map coordinates system, and row coordinate is with map picture from top to bottom for positive direction, and column coordinate is with Map
Piece is from left to right positive direction, defines the origin of world coordinates in map referencePlace,
Resolution is map resolution ratio, and the x-axis direction of world coordinate system corresponds to the direction that column coordinate increases in map coordinates system, generation
The y-axis direction of boundary's coordinate system corresponds to the direction that row coordinate reduces in map coordinates system, and world coordinate system is with the meter Wei Dan in reality
Position, each pixel have corresponded to the square two-dimensional surface region that a length and width in real world are 5cm, if pixel value is white
Color, then it represents that barrier is not present in the region, if pixel value is black, representing the region, there are barriers；It uses
Gmapping builds static map of the nomography building based on laser radar；
2) mobile robot pose model is established
It establishes using mobile robot center as the robot coordinate system of origin, establishes the robot pose mould in two-dimensional coordinate plane
Type, and the pose of robot in the environment is indicated in the form of coordinate system transformational relation；Wherein, the robot pose model with
Robot center is origin, and robot center to robot positive direction is x-axis, and right hand flat square seat is established as unit of rice
System, as robot coordinate system are marked, and indicates the pose of robot with Pose (x, y, θ), x in formula, y represent robot coordinate
The coordinate of world coordinate system locating for the origin of system, θ indicate world coordinate system x-axis direction to robot coordinate system's x-axis direction
Angle, with the direction increased counterclockwise for angle, then the coordinate in robot coordinate system corresponds to the coordinate in world coordinate system
It converts as follows:
In formula, x, y, θ are robot pose Pose (x, y, θ)；wx,wyFor the coordinate of world coordinate system；rx,ryFor robot coordinate
The coordinate of system；
3) laser radar data model is established
According to the riding position and direction of two-dimensional laser radar, laser radar data model is established, and established according to step 1)
The data protocol of robot pose model and laser radar that cartographic model and step 2) are established realizes laser radar to ring
The barrier data measured are simultaneously mapped in environmental map by the measurement of border distance；Wherein, the laser radar data mould
Type form is as follows:
Two-dimensional laser radar can scan the complaint message in a plane within the scope of 360 degree, two-dimensional laser radar scanning range
Can be up to 8 meters, angular resolution is 1 degree, and in the data model of two-dimensional laser radar, will be transmitted by transmitting data every time by 360
Floating data counterclockwise transmits the distance of obstacle laser radar on every 1 angular direction since 0 degree of direction of laser radar
The distance at center, as unit of rice, and recording the obstacle distance radar center distance on i degree direction is ρi, when two dimension swashs
Optical radar is centrally mounted at (0.08,0) in robot coordinate system, the y in 0 degree of direction and robot coordinate system of laser radar
Axis direction it is parallel and towards it is identical when, when robot is in position and posture Pose (x, y, θ), radar data ρiIt is mapped to the world
Coordinate in coordinate system:
In formula, Xworld, Yworld are the coordinate in world coordinate system；
Also, by radar data ρiThe coordinate being finally mapped in map coordinates system:
In formula, height represents map picture row coordinate, and width represents map picture column coordinate, and resolution represents map
Resolution ratio；
4) calibration binocular camera and acquisition three-dimensional coordinate
The photo that 10 groups of different two cameras are clapped using scaling board is come by finding position of the angle point in image of scaling board
Calculate two cameras internal reference and outer ginseng, obtain the re-projection matrix of 4 × 4 binocular camera eventually by combined calibratingcx,cyIt is left camera as planar central coordinate, c'x,c'yFor right camera as planar central is sat
Mark；TxFor the centre distance of two cameras, f is camera focus；Then left and right two is obtained using Block- matching Stereo Matching Algorithm
Disparity map, that is, depth map of camera gives parallax d and picture planar point (x, y), passes through formula
Acquire the three-dimensional coordinate P=(X/W, Y/W, Z/W) under camera coordinate system；
5) dynamic cost map is created using the three-dimensional coordinate that step 4) obtains
According to the definition of binocular vision system, using the optical center of the left camera in binocular camera as origin, optical axis direction z
Axis, base direction is x-axis from left to right, as unit of rice, establishes right hand rectangular coordinate system in space, referred to as binocular coordinate system,
It obtains on image after the three-dimensional coordinate P=(x', y', z') of each pixel, what y' was indicated is exactly elevation information, and x' corresponds to machine
Device people's two-dimensional coordinate plane-y-axis, z' corresponding is x-axis direction, is screened according to y' lower than robot and higher than robot
Then it is compressed to the two-dimensional surface where robot by barrier, remember that the coordinate under robot coordinate system is r (x, y), specifically
Formula has r (x, y)=(z' ,-x'), obtains the world coordinate system coordinate (r for being compressed to robot two-dimensional surfacex,ry) after, according to
Pose (the robot of current robotx,roboty, θ), it is multiplied i.e. by rigid body translation matrix with plane coordinatesObtain coordinate (w of the binocular coordinate system in world coordinate systemx,wy)；It is creating
When building map, it is at the center of map coordinates system, according to world coordinate system that robot initial pose, which is the origin of world coordinate system,
With the relationship of map coordinates system, pose of the barrier under binocular coordinate system in map coordinates system is further calculated out, then
The barrier is added in grating map, and adds expansion radius, and this makes it possible to being created that the cost based on binocular vision
Figure；It can similarly be created that the cost map of two-dimensional laser using identical method, then merge two cost maps, it will be able to
Obtain the cost map for the barrier that laser and vision are seen in current environment；It can be kept away using the cost map robot
Barrier navigates to the target point in map；
6) be finally exactly to select a target point, pass through the part dwa obstacle avoidance algorithm carry out avoidance navigation: a, robot control
Spatially carry out speed dispersion sampling；B, for each sample rate, look at using after the mobile a distance of the sample rate
What can occur；C, each track of forward simulation is evaluated, evaluation criterion is: close to barrier, leans on close-target close to global road
Diameter and speed；The illegal route is abandoned during evaluation；D, choose the track of highest scoring and send corresponding speed to machine
Device people；E, step a-d is constantly repeated, until arriving at the destination.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910119415.3A CN109765901A (en) | 2019-02-18 | 2019-02-18 | Dynamic cost digital map navigation method based on line laser and binocular vision |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910119415.3A CN109765901A (en) | 2019-02-18 | 2019-02-18 | Dynamic cost digital map navigation method based on line laser and binocular vision |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109765901A true CN109765901A (en) | 2019-05-17 |
Family
ID=66456832
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910119415.3A Pending CN109765901A (en) | 2019-02-18 | 2019-02-18 | Dynamic cost digital map navigation method based on line laser and binocular vision |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109765901A (en) |
Cited By (29)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110108269A (en) * | 2019-05-20 | 2019-08-09 | 电子科技大学 | AGV localization method based on Fusion |
| CN110502019A (en) * | 2019-09-06 | 2019-11-26 | 北京云迹科技有限公司 | A kind of barrier-avoiding method and device of Indoor Robot |
| CN110609559A (en) * | 2019-10-25 | 2019-12-24 | 江苏恒澄交科信息科技股份有限公司 | Improved DWA dynamic window method for unmanned ship path following and obstacle avoidance |
| CN110726409A (en) * | 2019-09-09 | 2020-01-24 | 杭州电子科技大学 | Map fusion method based on laser SLAM and visual SLAM |
| CN111469127A (en) * | 2020-04-14 | 2020-07-31 | 北京海益同展信息科技有限公司 | Cost map updating method and device, robot and storage medium |
| CN111726591A (en) * | 2020-06-22 | 2020-09-29 | 珠海格力电器股份有限公司 | Map updating method, map updating device, storage medium and electronic equipment |
| CN111743464A (en) * | 2020-07-06 | 2020-10-09 | 追创科技(苏州)有限公司 | Obstacle avoidance method and device based on line laser |
| CN111830977A (en) * | 2020-07-02 | 2020-10-27 | 中国兵器科学研究院 | Autonomous navigation software framework and navigation method for mobile robot |
| CN112129281A (en) * | 2019-06-25 | 2020-12-25 | 南京航空航天大学 | High-precision image navigation positioning method based on local neighborhood map |
| CN112180943A (en) * | 2020-10-19 | 2021-01-05 | 山东交通学院 | Underwater robot navigation obstacle avoidance method based on visual image and laser radar |
| CN112286194A (en) * | 2020-10-29 | 2021-01-29 | 广东杜尼智能机器人工程技术研究中心有限公司 | Cost map area division method |
| CN112294197A (en) * | 2020-11-04 | 2021-02-02 | 深圳市普森斯科技有限公司 | Sweeping control method of sweeper, electronic device and storage medium |
| CN112363498A (en) * | 2020-10-19 | 2021-02-12 | 山东交通学院 | Underwater robot intelligent motion control method based on laser radar |
| CN112378408A (en) * | 2020-11-26 | 2021-02-19 | 重庆大学 | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot |
| CN112447065A (en) * | 2019-08-16 | 2021-03-05 | 北京地平线机器人技术研发有限公司 | Trajectory planning method and device |
| CN112462768A (en) * | 2020-11-25 | 2021-03-09 | 深圳拓邦股份有限公司 | Mobile robot navigation map creating method and device and mobile robot |
| CN112489131A (en) * | 2020-12-26 | 2021-03-12 | 上海有个机器人有限公司 | Method, device, medium and robot for constructing cost map based on road surface detection |
| CN112486172A (en) * | 2020-11-30 | 2021-03-12 | 深圳市普渡科技有限公司 | Road edge detection method and robot |
| CN112506196A (en) * | 2020-12-07 | 2021-03-16 | 合肥工业大学 | Robot obstacle avoidance method and system based on priori knowledge |
| CN112505723A (en) * | 2021-02-03 | 2021-03-16 | 之江实验室 | Three-dimensional map reconstruction method based on navigation point selection |
| CN112817315A (en) * | 2020-12-31 | 2021-05-18 | 江苏集萃智能制造技术研究所有限公司 | Obstacle avoidance method and system for unmanned cleaning vehicle in dynamic environment |
| CN113239520A (en) * | 2021-04-16 | 2021-08-10 | 大连海事大学 | Near-water-bottom three-dimensional underwater terrain environment modeling method |
| CN113520246A (en) * | 2021-07-30 | 2021-10-22 | 珠海一微半导体股份有限公司 | Mobile robot compensation cleaning method and system |
| CN113607162A (en) * | 2021-10-09 | 2021-11-05 | 创泽智能机器人集团股份有限公司 | Path planning method and device based on three-dimensional map |
| CN113608532A (en) * | 2021-07-29 | 2021-11-05 | 深圳市眼科医院 | Automatic obstacle avoidance system for blind people going out and scooter thereof |
| CN114194685A (en) * | 2021-12-23 | 2022-03-18 | 山东新华医疗器械股份有限公司 | Stacking AGV control system, method and device |
| WO2022127541A1 (en) * | 2020-12-16 | 2022-06-23 | 北京极智嘉科技股份有限公司 | Robot and localization method |
| CN115406445A (en) * | 2022-08-18 | 2022-11-29 | 四川华丰科技股份有限公司 | Multi-sensor data fusion processing method and robot obstacle avoidance method |
| CN115775309A (en) * | 2023-01-31 | 2023-03-10 | 中国电子科技集团公司第二十八研究所 | Fire power range marking method and device for three-dimensional space |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103400392A (en) * | 2013-08-19 | 2013-11-20 | 山东鲁能智能技术有限公司 | Binocular vision navigation system and method based on inspection robot in transformer substation |
| CN108151728A (en) * | 2017-12-06 | 2018-06-12 | 华南理工大学 | A kind of half dense cognitive map creation method for binocular SLAM |
| US20180181141A1 (en) * | 2016-12-28 | 2018-06-28 | H.P.B Optoelectronic Co., Ltd | Robot surveillance system |
| CN108214487A (en) * | 2017-12-16 | 2018-06-29 | 广西电网有限责任公司电力科学研究院 | Based on the positioning of the robot target of binocular vision and laser radar and grasping means |
| CN108663681A (en) * | 2018-05-16 | 2018-10-16 | 华南理工大学 | Mobile Robotics Navigation method based on binocular camera Yu two-dimensional laser radar |
| CN108839055A (en) * | 2018-05-31 | 2018-11-20 | 芜湖星途机器人科技有限公司 | Robot with independent navigation |
| CN108932736A (en) * | 2018-05-30 | 2018-12-04 | 南昌大学 | Two-dimensional laser radar Processing Method of Point-clouds and dynamic robot pose calibration method |
| CN108983783A (en) * | 2018-08-03 | 2018-12-11 | 上海有个机器人有限公司 | A kind of robot movement speed control method and system, storage medium and terminal |
| CN109118542A (en) * | 2017-06-22 | 2019-01-01 | 百度在线网络技术（北京）有限公司 | Scaling method, device, equipment and storage medium between laser radar and camera |
| CN109129507A (en) * | 2018-09-10 | 2019-01-04 | 北京联合大学 | A kind of medium intelligent introduction robot and explanation method and system |
| CN109270534A (en) * | 2018-05-07 | 2019-01-25 | 西安交通大学 | A kind of intelligent vehicle laser sensor and camera online calibration method |
-
2019
- 2019-02-18 CN CN201910119415.3A patent/CN109765901A/en active Pending
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103400392A (en) * | 2013-08-19 | 2013-11-20 | 山东鲁能智能技术有限公司 | Binocular vision navigation system and method based on inspection robot in transformer substation |
| US20180181141A1 (en) * | 2016-12-28 | 2018-06-28 | H.P.B Optoelectronic Co., Ltd | Robot surveillance system |
| CN109118542A (en) * | 2017-06-22 | 2019-01-01 | 百度在线网络技术（北京）有限公司 | Scaling method, device, equipment and storage medium between laser radar and camera |
| CN108151728A (en) * | 2017-12-06 | 2018-06-12 | 华南理工大学 | A kind of half dense cognitive map creation method for binocular SLAM |
| CN108214487A (en) * | 2017-12-16 | 2018-06-29 | 广西电网有限责任公司电力科学研究院 | Based on the positioning of the robot target of binocular vision and laser radar and grasping means |
| CN109270534A (en) * | 2018-05-07 | 2019-01-25 | 西安交通大学 | A kind of intelligent vehicle laser sensor and camera online calibration method |
| CN108663681A (en) * | 2018-05-16 | 2018-10-16 | 华南理工大学 | Mobile Robotics Navigation method based on binocular camera Yu two-dimensional laser radar |
| CN108932736A (en) * | 2018-05-30 | 2018-12-04 | 南昌大学 | Two-dimensional laser radar Processing Method of Point-clouds and dynamic robot pose calibration method |
| CN108839055A (en) * | 2018-05-31 | 2018-11-20 | 芜湖星途机器人科技有限公司 | Robot with independent navigation |
| CN108983783A (en) * | 2018-08-03 | 2018-12-11 | 上海有个机器人有限公司 | A kind of robot movement speed control method and system, storage medium and terminal |
| CN109129507A (en) * | 2018-09-10 | 2019-01-04 | 北京联合大学 | A kind of medium intelligent introduction robot and explanation method and system |
Non-Patent Citations (2)
| Title |
|---|
| ROBOTDREAMSKY: "ROS机器人导航", 《ROBOTDREAMSKY HTTPS://WENKU.BAIDU.COM/VIEW/B70E5289B7360B4C2F3F6497.HTML?FROM=SEARCH》 * |
| 冯里千: "移动机器人自主探索环境定位导航技术的研究与实现", 《 中优秀国硕士学位论文全文数据库，信息科技辑》 * |
Cited By (43)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110108269B (en) * | 2019-05-20 | 2023-01-17 | 电子科技大学 | AGV positioning method based on multi-sensor data fusion |
| CN110108269A (en) * | 2019-05-20 | 2019-08-09 | 电子科技大学 | AGV localization method based on Fusion |
| CN112129281B (en) * | 2019-06-25 | 2022-02-22 | 南京航空航天大学 | High-precision image navigation positioning method based on local neighborhood map |
| CN112129281A (en) * | 2019-06-25 | 2020-12-25 | 南京航空航天大学 | High-precision image navigation positioning method based on local neighborhood map |
| CN112447065B (en) * | 2019-08-16 | 2022-04-26 | 北京地平线机器人技术研发有限公司 | Trajectory planning method and device |
| CN112447065A (en) * | 2019-08-16 | 2021-03-05 | 北京地平线机器人技术研发有限公司 | Trajectory planning method and device |
| CN110502019A (en) * | 2019-09-06 | 2019-11-26 | 北京云迹科技有限公司 | A kind of barrier-avoiding method and device of Indoor Robot |
| CN110726409A (en) * | 2019-09-09 | 2020-01-24 | 杭州电子科技大学 | Map fusion method based on laser SLAM and visual SLAM |
| CN110726409B (en) * | 2019-09-09 | 2021-06-22 | 杭州电子科技大学 | Map fusion method based on laser SLAM and visual SLAM |
| CN110609559A (en) * | 2019-10-25 | 2019-12-24 | 江苏恒澄交科信息科技股份有限公司 | Improved DWA dynamic window method for unmanned ship path following and obstacle avoidance |
| CN111469127A (en) * | 2020-04-14 | 2020-07-31 | 北京海益同展信息科技有限公司 | Cost map updating method and device, robot and storage medium |
| CN111469127B (en) * | 2020-04-14 | 2022-06-07 | 京东科技信息技术有限公司 | Cost map updating method and device, robot and storage medium |
| CN111726591A (en) * | 2020-06-22 | 2020-09-29 | 珠海格力电器股份有限公司 | Map updating method, map updating device, storage medium and electronic equipment |
| CN111726591B (en) * | 2020-06-22 | 2021-11-23 | 珠海格力电器股份有限公司 | Map updating method, map updating device, storage medium and electronic equipment |
| CN111830977A (en) * | 2020-07-02 | 2020-10-27 | 中国兵器科学研究院 | Autonomous navigation software framework and navigation method for mobile robot |
| CN111743464A (en) * | 2020-07-06 | 2020-10-09 | 追创科技(苏州)有限公司 | Obstacle avoidance method and device based on line laser |
| CN112363498A (en) * | 2020-10-19 | 2021-02-12 | 山东交通学院 | Underwater robot intelligent motion control method based on laser radar |
| CN112180943A (en) * | 2020-10-19 | 2021-01-05 | 山东交通学院 | Underwater robot navigation obstacle avoidance method based on visual image and laser radar |
| CN112286194B (en) * | 2020-10-29 | 2022-05-17 | 广东杜尼智能机器人工程技术研究中心有限公司 | Cost map area division method |
| CN112286194A (en) * | 2020-10-29 | 2021-01-29 | 广东杜尼智能机器人工程技术研究中心有限公司 | Cost map area division method |
| CN112294197A (en) * | 2020-11-04 | 2021-02-02 | 深圳市普森斯科技有限公司 | Sweeping control method of sweeper, electronic device and storage medium |
| CN112462768A (en) * | 2020-11-25 | 2021-03-09 | 深圳拓邦股份有限公司 | Mobile robot navigation map creating method and device and mobile robot |
| CN112462768B (en) * | 2020-11-25 | 2024-03-29 | 深圳拓邦股份有限公司 | Mobile robot navigation map creation method and device and mobile robot |
| CN112378408A (en) * | 2020-11-26 | 2021-02-19 | 重庆大学 | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot |
| WO2022111723A1 (en) * | 2020-11-30 | 2022-06-02 | 深圳市普渡科技有限公司 | Road edge detection method and robot |
| CN112486172A (en) * | 2020-11-30 | 2021-03-12 | 深圳市普渡科技有限公司 | Road edge detection method and robot |
| CN112506196A (en) * | 2020-12-07 | 2021-03-16 | 合肥工业大学 | Robot obstacle avoidance method and system based on priori knowledge |
| WO2022127541A1 (en) * | 2020-12-16 | 2022-06-23 | 北京极智嘉科技股份有限公司 | Robot and localization method |
| CN112489131B (en) * | 2020-12-26 | 2024-04-05 | 上海有个机器人有限公司 | Method, device, medium and robot for constructing cost map based on pavement detection |
| CN112489131A (en) * | 2020-12-26 | 2021-03-12 | 上海有个机器人有限公司 | Method, device, medium and robot for constructing cost map based on road surface detection |
| CN112817315A (en) * | 2020-12-31 | 2021-05-18 | 江苏集萃智能制造技术研究所有限公司 | Obstacle avoidance method and system for unmanned cleaning vehicle in dynamic environment |
| CN112505723B (en) * | 2021-02-03 | 2024-01-23 | 之江实验室 | Three-dimensional map reconstruction method based on navigation point selection |
| CN112505723A (en) * | 2021-02-03 | 2021-03-16 | 之江实验室 | Three-dimensional map reconstruction method based on navigation point selection |
| CN113239520A (en) * | 2021-04-16 | 2021-08-10 | 大连海事大学 | Near-water-bottom three-dimensional underwater terrain environment modeling method |
| CN113239520B (en) * | 2021-04-16 | 2023-09-08 | 大连海事大学 | Near-water three-dimensional underwater topography environment modeling method |
| CN113608532A (en) * | 2021-07-29 | 2021-11-05 | 深圳市眼科医院 | Automatic obstacle avoidance system for blind people going out and scooter thereof |
| CN113520246A (en) * | 2021-07-30 | 2021-10-22 | 珠海一微半导体股份有限公司 | Mobile robot compensation cleaning method and system |
| CN113607162A (en) * | 2021-10-09 | 2021-11-05 | 创泽智能机器人集团股份有限公司 | Path planning method and device based on three-dimensional map |
| CN114194685A (en) * | 2021-12-23 | 2022-03-18 | 山东新华医疗器械股份有限公司 | Stacking AGV control system, method and device |
| CN115406445A (en) * | 2022-08-18 | 2022-11-29 | 四川华丰科技股份有限公司 | Multi-sensor data fusion processing method and robot obstacle avoidance method |
| CN115406445B (en) * | 2022-08-18 | 2024-05-17 | 四川华丰科技股份有限公司 | Multi-sensor data fusion processing method and robot obstacle avoidance method |
| CN115775309A (en) * | 2023-01-31 | 2023-03-10 | 中国电子科技集团公司第二十八研究所 | Fire power range marking method and device for three-dimensional space |
| CN115775309B (en) * | 2023-01-31 | 2023-04-04 | 中国电子科技集团公司第二十八研究所 | Fire power range marking method and device for three-dimensional space |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109765901A (en) | Dynamic cost digital map navigation method based on line laser and binocular vision | |
| CN108663681B (en) | Mobile robot navigation method based on binocular camera and two-dimensional laser radar | |
| CN111156998B (en) | Mobile robot positioning method based on RGB-D camera and IMU information fusion | |
| EP3321888B1 (en) | Projected image generation method and device, and method for mapping image pixels and depth values | |
| AU2018295572B2 (en) | Real time position and orientation tracker | |
| Zhang et al. | Intelligent collaborative localization among air-ground robots for industrial environment perception | |
| JP4794019B2 (en) | Apparatus and method for providing a three-dimensional map representation of a region | |
| CN105354875B (en) | A kind of indoor environment is two-dimentional with the construction method and system of three-dimensional conjunctive model | |
| Maier et al. | Real-time navigation in 3D environments based on depth camera data | |
| CN104932001B (en) | A kind of 3D nuclear radiation environments in real time rebuild monitoring system | |
| Rovira-Más et al. | Creation of three-dimensional crop maps based on aerial stereoimages | |
| Bruno et al. | Augmented reality visualization of scene depth for aiding ROV pilots in underwater manipulation | |
| CN114529585A (en) | Mobile equipment autonomous positioning method based on depth vision and inertial measurement | |
| CN110202577A (en) | A kind of autonomous mobile robot that realizing detection of obstacles and its method | |
| Jensen et al. | Laser range imaging using mobile robots: From pose estimation to 3D-models | |
| Maurer et al. | Geo-referenced 3d reconstruction: Fusing public geographic data and aerial imagery | |
| CN116957360A (en) | Space observation and reconstruction method and system based on unmanned aerial vehicle | |
| Wang et al. | Micro aerial vehicle navigation with visual-inertial integration aided by structured light | |
| Lu et al. | 3-D location estimation of underwater circular features by monocular vision | |
| Allotta et al. | Thesaurus project: Design of new autonomous underwater vehicles for documentation and protection of underwater archaeological sites | |
| Brujic-Okretic et al. | Remote vehicle manoeuvring using augmented reality | |
| Bai et al. | Research on key technologies of meal delivery robot based on the fusion of lidar and machine vision | |
| Zhou et al. | A multi-binocular camera-based localization method for amphibious spherical robots | |
| Orlov et al. | Machine vision system for autonomous agricultural vehicle | |
| Amano et al. | Full color 3D point clouds from bird's-eye view using multi-view laser scanner and quadcopter |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190517 |