CN112611374A - Path planning and obstacle avoidance method and system based on laser radar and depth camera - Google Patents
Path planning and obstacle avoidance method and system based on laser radar and depth camera Download PDFInfo
- Publication number
- CN112611374A CN112611374A CN202011179495.0A CN202011179495A CN112611374A CN 112611374 A CN112611374 A CN 112611374A CN 202011179495 A CN202011179495 A CN 202011179495A CN 112611374 A CN112611374 A CN 112611374A
- Authority
- CN
- China
- Prior art keywords
- agv
- laser radar
- depth camera
- moving platform
- acquiring
- 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
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000012545 processing Methods 0.000 claims abstract description 24
- 238000012544 monitoring process Methods 0.000 claims abstract description 11
- 230000033001 locomotion Effects 0.000 claims description 67
- 239000000126 substance Substances 0.000 claims description 24
- 238000005457 optimization Methods 0.000 claims description 13
- 230000008859 change Effects 0.000 claims description 7
- 238000011156 evaluation Methods 0.000 claims description 7
- 238000009825 accumulation Methods 0.000 claims description 6
- 230000029777 axis specification Effects 0.000 claims description 6
- 238000009795 derivation Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000007613 environmental effect Effects 0.000 claims 2
- 230000009286 beneficial effect Effects 0.000 description 6
- 230000009471 action Effects 0.000 description 4
- 238000005259 measurement Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 239000003795 chemical substances by application Substances 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 239000003638 chemical reducing agent Substances 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000005283 ground state Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
- G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
Abstract
The invention provides a method and a system for path planning and obstacle avoidance based on a laser radar and a depth camera, wherein the method comprises the steps of arranging the laser radar and the depth camera on an AGV trolley moving platform, and acquiring an AGV trolley moving track model according to a central processing system preset in the AGV trolley moving platform; according to the laser radar, reconstructing a position environment and acquiring environment layout data; monitoring the overall running condition of the surrounding environment of the AGV trolley moving platform according to the depth camera; and constructing an autonomous obstacle avoidance algorithm of the AGV trolley moving platform according to the environment layout data and the overall operation condition based on a laser radar and a depth camera, and determining an optimal target path.
Description
Technical Field
The invention relates to the technologies in the fields of laser SLAM, path optimization algorithm, autonomous obstacle avoidance algorithm and the like, and aims to develop a path planning and obstacle avoidance method and system based on a laser radar and a depth camera.
Background
At present, the simultaneous positioning and map building generally refers to a system for generating positioning and scene map information of the position and posture of an AGV by collecting and calculating data of various sensors on the AGV or other carriers. The SLAM technology is critical to the action and interaction capacity of an AGV or other intelligent agents, has wide application in the fields of automatic driving, service type AGV, unmanned aerial vehicles, AR/VR and the like, and can be said that all intelligent agents with certain action capacity have SLAM systems in a certain form. The appearance and the popularization of laser radar make the measurement faster more accurate, and the information is abundanter, and laser radar distance measurement is more accurate, and the error model is simple, and the operation is stable in the environment beyond the highlight direct irradiation, and the processing of point cloud is also easier. Meanwhile, the laser SLAM theoretical research is relatively mature, and the products falling to the ground are more abundant. However, with the development of the digital industry, the simple SLAM mapping cannot meet the requirements of high-level functions such as autonomous and intelligent AGV in various industrial scenes, for example, in an unmanned workshop, the AGV is required to transport and allocate goods according to a fixed track, and the AGV is further required to be adjusted and scheduled in time when an accident occurs, so that the economic loss caused by the accident is reduced as much as possible, and the AGV is required to have all-around intelligent functions such as path autonomous planning, autonomous obstacle avoidance and intelligent control.
In view of the above requirements for the AGV car autonomy and intellectualization, the invention aims to develop a path planning and obstacle avoidance method and method based on a laser radar and a depth camera.
Disclosure of Invention
The invention aims to develop a method and a system for path planning and obstacle avoidance based on a laser radar and a depth camera, wherein the system comprises the laser radar, the depth camera, an AGV trolley moving platform and a central processing unit, the laser radar can reconstruct the position environment and provide environment layout data for the safe operation of the AGV trolley; the depth camera acquires scene pictures at different visual angles in real time and monitors the overall operation condition of the equipment in real time; the AGV trolley moving platform has the main functions of carrying a depth camera and a laser radar, and providing core functions of a power supply, a brake, steering and the like used by all hardware. All the modules jointly realize map reconstruction, path planning and dynamic obstacle avoidance of the AGV moving platform in an unknown environment.
As an embodiment of the present invention, a method for path planning and dynamic obstacle avoidance based on a laser radar and a depth camera is characterized by comprising:
arranging a laser radar and a depth camera on an AGV trolley moving platform, and acquiring an AGV trolley moving track model according to a central processing system preset in the AGV trolley moving platform;
according to the laser radar, reconstructing a position environment and acquiring environment layout data;
monitoring the overall running condition of the surrounding environment of the AGV trolley moving platform according to the depth camera;
and constructing an autonomous obstacle avoidance algorithm of the AGV trolley moving platform according to the environment layout data and the overall operation condition based on a laser radar and a depth camera, and determining an optimal target path.
As an embodiment of the present invention, the installing a laser radar and a depth camera on an AGV moving platform, and acquiring an AGV moving track model according to a central processing system preset in the AGV moving platform includes:
arranging a laser radar and a depth camera on an AGV trolley moving platform; wherein the content of the first and second substances,
the laser radar consists of a single-beam narrow-band laser and a receiver and is arranged in the top platform area of the AGV; wherein the content of the first and second substances,
the single-beam narrow-band laser is used for emitting light pulses to obtain emission time t 1;
the receiver is used for receiving the light pulse and acquiring a receiving time t 2;
the depth camera consists of an RGB camera and a depth camera; wherein the content of the first and second substances,
the RGB camera is used for collecting a light ray image of the shot object;
the depth camera is used for acquiring a three-dimensional structure of an image;
determining a dynamic window algorithm according to a central processing system preset in the AGV trolley moving platform;
determining a motion model of the AGV according to a dynamic window algorithm; wherein the content of the first and second substances,
assuming that the track of the two-wheeled AGV is a section of arc or straight line (when the rotating speed is 0), a pair of (vt, wt) represents an arc track, and the specific derivation is as follows:
r＝v/w
when the rotation speed w is not equal to 0, the AGV trolley coordinates are:
θt＝θt+WΔi
and simulating the track of the AGV according to the motion model of the AGV, and acquiring a motion track model of the AGV.
As an embodiment of the present invention, the monitoring the overall operation of the environment surrounding the AGV trolley moving platform according to the depth camera includes:
acquiring light signals of the surrounding environment of the AGV trolley moving platform according to the RGB camera of the depth camera;
acquiring image information of the surrounding environment of the AGV trolley moving platform according to a depth camera of the depth camera;
acquiring images of different depth areas of the surrounding environment image according to the light signal and the image information to acquire different image phase information;
determining phase data according to the different image phase information;
calculating the phase data through a preset algorithm, and determining the depth information of the surrounding environment of the AGV trolley moving platform;
determining a three-dimensional structure of the surrounding environment of the AGV trolley moving platform according to the depth information;
and determining the overall operation condition of the surrounding environment of the AGV trolley moving platform according to the three-dimensional structure.
As an embodiment of the present invention, the reconstructing a location environment according to the laser radar and acquiring environment layout data includes:
based on laser radar, the running state of the AGV trolley moving platform is modeled, and a state estimation algorithm is as follows:
xk＝f(xk-1,uk)+vk(1)
zk＝h(xk)+wk(2)
the formula (1) is a motion equation, the position (x (k)) of the AGV moving platform at the time k is determined by the position (x (k-1)) at the time k-1 and the motion input (u (k)) at the time k, and because an error is always introduced in the actual physical environment, a noise amount (v (k)) is added to form certain constraint on state change, and the formula (2) is an observation equation, represents the AGV moving platform sensor observation (z (k)) at the time k and is determined by the AGV moving platform position at the current time. Similarly, a certain observation error, namely w (k), is brought in due to the influence of the physical environment;
determining a three-dimensional navigation coordinate axis through an inertial navigation system or a Beidou satellite GPS positioning system in a nearby ground station;
according to the three-dimensional navigation coordinate axis, determining position coordinate information of the laser radar and the surrounding environment, and acquiring the height of the laser radar from the ground and the scanning angle of the laser radar to scan the direction;
transmitting light pulses to the surrounding environment through the laser radar, and acquiring the propagation time according to the transmitting time and the receiving time;
determining the distance between the laser radar and surrounding objects according to the propagation time;
determining a target three-dimensional navigation coordinate axis with the laser radar as an origin according to the height of the laser radar from the ground, and the scanning angle and the scanning direction of the laser radar;
determining target position coordinate information of the surrounding environment of the laser radar according to the target three-dimensional navigation coordinate axis;
and reconstructing a position environment according to the coordinate information of the target position, and acquiring environment layout data.
As an embodiment of the present invention, the constructing an autonomous obstacle avoidance algorithm of an AGV moving platform based on a laser radar and a depth camera according to the environment layout data and the overall operation condition to determine an optimal target path includes:
the method comprises the steps of obtaining a slam scheme based on a laser radar and a depth camera, dividing the slam scheme into two modules, and obtaining a front-end module and a rear-end module;
acquiring the position of the continuous frame of the AGV under different running states according to a front end module;
comparing laser changes of front and back frames in the AGV trolley movement to acquire the position data of the relative movement of the AGV trolley;
decomposing the fuse into three dimensions, namely an x axis, a y axis and an angle axis, constructing three layers of for cycles, and respectively performing traversal search on the x direction, the y direction and the rotation angle direction to obtain an optimal fuse traversal result;
according to the optimal fuse traversal result, matching the fuse data of the relative motion to obtain a state estimation result;
judging the position of the AGV trolley and historical position coincidence data according to a rear-end module, and constructing closed-loop constraint;
acquiring errors in the movement process through nonlinear optimization, dispersing the errors into each position participating in optimization, eliminating error accumulation, and acquiring the running speed of the AGV;
according to the AGV trolley running speed, each track formed by each speed is evaluated, and the evaluation function is as follows:
G(V,W)＝σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w))
and the central processing unit judges whether the track is the optimal path according to the G value, and starts a path re-planning algorithm to obtain the target optimal path.
As shown in fig. 2, a method for path planning and obstacle avoidance based on a laser radar and a depth camera is characterized by comprising:
a track acquisition module: arranging a laser radar and a depth camera on an AGV trolley moving platform, and acquiring an AGV trolley moving track model according to a central processing system preset in the AGV trolley moving platform;
a laser radar module: according to the laser radar, reconstructing a position environment and acquiring environment layout data;
a depth camera module: monitoring the overall running condition of the surrounding environment of the AGV trolley moving platform according to the depth camera;
an autonomous obstacle avoidance and optimal path module: and constructing an autonomous obstacle avoidance algorithm of the AGV trolley moving platform according to the environment layout data and the overall operation condition based on a laser radar and a depth camera, and determining an optimal target path.
As an embodiment of the present invention, the track acquiring module includes:
AGV dolly motion platform unit: arranging a laser radar and a depth camera on an AGV trolley moving platform; wherein the content of the first and second substances,
laser radar first subunit: the laser radar consists of a single-beam narrow-band laser and a receiver and is arranged in the top platform area of the AGV; wherein the content of the first and second substances,
a single-beam narrow-band laser second subunit: the single-beam narrow-band laser is used for emitting light pulses to obtain emission time t 1;
a receiver second subunit: the receiver is used for receiving the light pulse and acquiring a receiving time t 2;
a depth camera unit: the depth camera consists of an RGB camera and a depth camera; wherein the content of the first and second substances,
RGB camera subunit: the RGB camera is used for collecting a light ray image of the shot object;
depth camera subunit: the depth camera is used for acquiring a three-dimensional structure of an image;
a dynamic window algorithm unit: determining a dynamic window algorithm according to a central processing system preset in the AGV trolley moving platform;
a motion model unit: determining a motion model of the AGV according to a dynamic window algorithm; wherein the content of the first and second substances,
assuming that the track of the two-wheeled AGV is a section of arc or straight line (when the rotating speed is 0), a pair of (vt, wt) represents an arc track, and the specific derivation is as follows:
r＝v/w
when the rotation speed w is not equal to 0, the AGV trolley coordinates are:
θt＝θt+WΔi
a motion trail model unit: and simulating the track of the AGV according to the motion model of the AGV, and acquiring a motion track model of the AGV.
As an embodiment of the present invention, the laser radar module includes:
a light signal acquisition unit: acquiring light signals of the surrounding environment of the AGV trolley moving platform according to the RGB camera of the depth camera;
an image information acquisition unit: acquiring image information of the surrounding environment of the AGV trolley moving platform according to a depth camera of the depth camera;
an image phase information acquisition unit: acquiring images of different depth areas of the surrounding environment image according to the light signal and the image information to acquire different image phase information;
a phase data determination unit: determining phase data according to the different image phase information;
a depth information determination unit: calculating the phase data through a preset algorithm, and determining the depth information of the surrounding environment of the AGV trolley moving platform;
a three-dimensional structure determination unit: determining a three-dimensional structure of the surrounding environment of the AGV trolley moving platform according to the depth information;
an overall operation condition determination unit: and determining the overall operation condition of the surrounding environment of the AGV trolley moving platform according to the three-dimensional structure.
As an embodiment of the present invention, the depth camera module includes:
a modeling unit: based on laser radar, the running state of the AGV trolley moving platform is modeled, and a state estimation algorithm is as follows:
xk＝f(xk-1,uk)+vk(1)
zk＝h(xk)+wk(2)
the formula (1) is a motion equation, the position (x (k)) of the AGV moving platform at the time k is determined by the position (x (k-1)) at the time k-1 and the motion input (u (k)) at the time k, and because an error is always introduced in the actual physical environment, a noise amount (v (k)) is added to form certain constraint on state change, and the formula (2) is an observation equation, represents the AGV moving platform sensor observation (z (k)) at the time k and is determined by the AGV moving platform position at the current time. Similarly, a certain observation error, namely w (k), is brought in due to the influence of the physical environment;
three-dimensional navigation coordinate axis determination unit: determining a three-dimensional navigation coordinate axis through an inertial navigation system or a Beidou satellite GPS positioning system in a nearby ground station;
an acquisition information unit: according to the three-dimensional navigation coordinate axis, determining position coordinate information of the laser radar and the surrounding environment, and acquiring the height of the laser radar from the ground and the scanning angle of the laser radar to scan the direction;
acquiring a propagation time unit: transmitting light pulses to the surrounding environment through the laser radar, and acquiring the propagation time according to the transmitting time and the receiving time;
determining a distance unit: determining the distance between the laser radar and surrounding objects according to the propagation time;
the target three-dimensional navigation coordinate axis determination unit: determining a target three-dimensional navigation coordinate axis with the laser radar as an origin according to the height of the laser radar from the ground, and the scanning angle and the scanning direction of the laser radar;
target position coordinate information determination unit: determining target position coordinate information of the surrounding environment of the laser radar according to the target three-dimensional navigation coordinate axis;
acquiring an environment layout data unit: and reconstructing a position environment according to the coordinate information of the target position, and acquiring environment layout data.
As an embodiment of the present invention, the autonomous obstacle avoidance and optimal path module includes:
a slam dividing unit: the method comprises the steps of obtaining a slam scheme based on a laser radar and a depth camera, dividing the slam scheme into two modules, and obtaining a front-end module and a rear-end module;
a dose unit: acquiring the position of the continuous frame of the AGV under different running states according to a front end module;
a dose data unit: comparing laser changes of front and back frames in the AGV trolley movement to acquire the position data of the relative movement of the AGV trolley;
a pos traversal result unit: decomposing the fuse into three dimensions, namely an x axis, a y axis and an angle axis, constructing three layers of for cycles, and respectively performing traversal search on the x direction, the y direction and the rotation angle direction to obtain an optimal fuse traversal result;
a state estimation result unit: according to the optimal fuse traversal result, matching the fuse data of the relative motion to obtain a state estimation result;
the closed-loop constraint unit is used for judging the position of the AGV trolley and the historical position coincidence data according to a rear-end module and constructing closed-loop constraint;
AGV dolly functioning speed unit: acquiring errors in the movement process through nonlinear optimization and closed-loop constraint, dispersing the errors into each position participating in optimization, eliminating error accumulation, and acquiring the running speed of the AGV;
target optimal path unit: according to the AGV trolley running speed, each track formed by each speed is evaluated, a path re-planning algorithm is determined, and the adopted evaluation functions are as follows:
G(V,W)＝σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w))
and judging whether the track of the G value is the optimal path or not, and acquiring the optimal path of the target.
The invention has the beneficial effects that: the invention discloses a high-precision positioning system and method based on an IMU (inertial measurement Unit) and a Beidou system. The system can collect positioning data of a Beidou satellite through a signal antenna, improves positioning accuracy by combining motion system parameters collected by an inertial unit (IMU), and meanwhile utilizes a laser radar to sense and reconstruct the surrounding environment in real time, so that the motion system can sense the surrounding environment in real time, and avoid obstacles on a path and perform timely safety processing, and high-precision positioning and autonomous obstacle avoidance are realized.
Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. The objectives and other advantages of the invention will be realized and attained by the structure particularly pointed out in the written description and drawings.
The technical solution of the present invention is further described in detail by the accompanying drawings and embodiments.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the principles of the invention and not to limit the invention. In the drawings:
fig. 1 is a flowchart of a method for path planning and obstacle avoidance based on a laser radar and a depth camera according to an embodiment of the present invention;
fig. 2 is a system composition diagram of a system for path planning and obstacle avoidance based on a laser radar and a depth camera according to an embodiment of the present invention;
Detailed Description
The preferred embodiments of the present invention will be described in conjunction with the accompanying drawings, and it will be understood that they are described herein for the purpose of illustration and explanation and not limitation.
Example 1:
as shown in fig. 1, a method for path planning and obstacle avoidance based on a laser radar and a depth camera includes the following steps:
step 100: arranging a laser radar and a depth camera on an AGV trolley moving platform, and acquiring an AGV trolley moving track model according to a central processing system preset in the AGV trolley moving platform;
step 101: according to the laser radar, reconstructing a position environment and acquiring environment layout data;
step 102: monitoring the overall running condition of the surrounding environment of the AGV trolley moving platform according to the depth camera;
step 103: and constructing an autonomous obstacle avoidance algorithm of the AGV trolley moving platform according to the environment layout data and the overall operation condition based on a laser radar and a depth camera, and determining an optimal target path.
The principle and the beneficial effects of the invention are as follows: the laser radar can reconstruct the position environment and provide environment layout data for the safe operation of the AGV; the depth camera acquires scene pictures at different visual angles in real time and monitors the overall operation condition of the equipment in real time; the AGV trolley moving platform has the main functions of carrying a depth camera and a laser radar and providing core functions of a power supply, a brake, steering and the like used by all hardware; all the modules jointly realize map reconstruction, path planning and dynamic obstacle avoidance of the AGV moving platform in an unknown environment.
As an embodiment of the present invention, the installing a laser radar and a depth camera on an AGV moving platform, and acquiring an AGV moving track model according to a central processing system preset in the AGV moving platform includes:
arranging a laser radar and a depth camera on an AGV trolley moving platform; wherein the content of the first and second substances,
the laser radar consists of a single-beam narrow-band laser and a receiver and is arranged in the top platform area of the AGV; wherein the content of the first and second substances,
the single-beam narrow-band laser is used for emitting light pulses to obtain emission time t 1;
the receiver is used for receiving the light pulse and acquiring a receiving time t 2;
the depth camera consists of an RGB camera and a depth camera; wherein the content of the first and second substances,
the RGB camera is used for collecting a light ray image of the shot object;
the depth camera is used for acquiring a three-dimensional structure of an image;
determining a dynamic window algorithm according to a central processing system preset in the AGV trolley moving platform;
determining a motion model of the AGV according to a dynamic window algorithm; wherein the content of the first and second substances,
assuming that the track of the two-wheeled AGV is a section of arc or straight line (when the rotating speed is 0), a pair of (vt, wt) represents an arc track, and the specific derivation is as follows:
r＝v/w
when the rotation speed w is not equal to 0, the AGV trolley coordinates are:
θt＝θt+WΔi
and simulating the track of the AGV according to the motion model of the AGV, and acquiring a motion track model of the AGV.
The principle and the beneficial effects of the invention are as follows: the AGV trolley body consists of a frame and a corresponding mechanical device, and is a basic part of the AGV and an installation foundation of other assembly parts; an electric storage and charging device, namely an AGV usually adopts 24V and 48V direct current storage batteries as power, and the power supply of the storage batteries is generally required to be kept for continuously working for more than 8 hours; the driving device consists of wheels, a reducer, a brake, a driving motor, a speed controller and the like and is used for controlling the AGV to normally run; the running instruction is sent out simultaneously by computer or manual control, and the regulation of running speed, direction and brake are respectively controlled by computer. For safety, the braking device can realize braking mechanically when power is off; the guiding device receives the direction information of the guiding system and realizes the steering action through the steering device; the on-vehicle controller receives the instruction of the control center and executes the corresponding instruction, and simultaneously feeds the state (such as position, speed and the like) of the on-vehicle controller back to the control center in time; communication device-realizes the information exchange between AGV and ground control station and ground monitoring equipment; safety protection devices-including the protection of the AGV itself, protection of people or other equipment, etc.; the transfer device is a device which is directly contacted with the transported goods to realize the transfer of the goods; information transmission and processing device-monitors the AGV, monitors the ground state of the AGV, and transmits information with the ground control station in real time.
As an embodiment of the present invention, the monitoring the overall operation of the environment surrounding the AGV trolley moving platform according to the depth camera includes:
acquiring light signals of the surrounding environment of the AGV trolley moving platform according to the RGB camera of the depth camera;
acquiring image information of the surrounding environment of the AGV trolley moving platform according to a depth camera of the depth camera;
acquiring images of different depth areas of the surrounding environment image according to the light signal and the image information to acquire different image phase information;
determining phase data according to the different image phase information;
calculating the phase data through a preset algorithm, and determining the depth information of the surrounding environment of the AGV trolley moving platform;
determining a three-dimensional structure of the surrounding environment of the AGV trolley moving platform according to the depth information;
and determining the overall operation condition of the surrounding environment of the AGV trolley moving platform according to the three-dimensional structure.
The principle and the beneficial effects of the invention are as follows: the depth camera is composed of an RGB camera and a depth camera, and Structured light, English is called Structured light. The light with a certain structure can acquire different image phase information according to different depth areas of a shot object, and then the change of the structure is converted into depth information through an arithmetic unit, so that a three-dimensional structure is obtained. In short, the three-dimensional structure of the object to be photographed is acquired by an optical means, and the acquired information is applied more deeply. Invisible infrared laser with specific wavelength is generally used as a light source, light emitted by the invisible infrared laser is projected on an object through a certain code, and the position and depth information of the object is obtained by calculating the distortion of a returned code pattern through a certain algorithm.
As an embodiment of the present invention, the reconstructing a location environment according to the laser radar and acquiring environment layout data includes:
based on laser radar, the running state of the AGV trolley moving platform is modeled, and a state estimation algorithm is as follows:
xk＝f(xk-1,uk)+vk(1)
zk＝h(xk)+wk(2)
the formula (1) is a motion equation, the position (x (k)) of the AGV moving platform at the time k is determined by the position (x (k-1)) at the time k-1 and the motion input (u (k)) at the time k, and because an error is always introduced in the actual physical environment, a noise amount (v (k)) is added to form certain constraint on state change, and the formula (2) is an observation equation, represents the AGV moving platform sensor observation (z (k)) at the time k and is determined by the AGV moving platform position at the current time. Similarly, a certain observation error, namely w (k), is brought in due to the influence of the physical environment;
determining a three-dimensional navigation coordinate axis through an inertial navigation system or a Beidou satellite GPS positioning system in a nearby ground station;
according to the three-dimensional navigation coordinate axis, determining position coordinate information of the laser radar and the surrounding environment, and acquiring the height of the laser radar from the ground and the scanning angle of the laser radar to scan the direction;
transmitting light pulses to the surrounding environment through the laser radar, and acquiring the propagation time according to the transmitting time and the receiving time;
determining the distance between the laser radar and surrounding objects according to the propagation time;
determining a target three-dimensional navigation coordinate axis with the laser radar as an origin according to the height of the laser radar from the ground, and the scanning angle and the scanning direction of the laser radar;
determining target position coordinate information of the surrounding environment of the laser radar according to the target three-dimensional navigation coordinate axis;
and reconstructing a position environment according to the coordinate information of the target position, and acquiring environment layout data.
The principle and the beneficial effects of the invention are as follows: the LIDAR system comprises a single-beam narrow-band laser and a receiving system. The laser generates and emits a light pulse, which strikes an object and is reflected back, and finally is received by the receiver. The receiver accurately measures the travel time of the light pulse from emission to reflection. (t) because the light pulse travels at the speed of light, the receiver will always receive the previous reflected pulse before the next pulse is sent out. Given that the speed of light is known, the travel time can be converted into a measure of distance. The coordinates X, Y and Z of each ground light spot can be accurately calculated by combining the height of the laser, the laser scanning angle, the position of the laser obtained from the GPS and the laser emission direction obtained from the INS. The frequency of laser beam emission can range from a few pulses per second to tens of thousands of pulses per second.
As an embodiment of the present invention, the constructing an autonomous obstacle avoidance algorithm of an AGV moving platform based on a laser radar and a depth camera according to the environment layout data and the overall operation condition to determine an optimal target path includes:
the method comprises the steps of obtaining a slam scheme based on a laser radar and a depth camera, dividing the slam scheme into two modules, and obtaining a front-end module and a rear-end module;
acquiring the position of the continuous frame of the AGV under different running states according to a front end module;
comparing laser changes of front and back frames in the AGV trolley movement to acquire the position data of the relative movement of the AGV trolley;
decomposing the fuse into three dimensions, namely an x axis, a y axis and an angle axis, constructing three layers of for cycles, and respectively performing traversal search on the x direction, the y direction and the rotation angle direction to obtain an optimal fuse traversal result;
according to the optimal fuse traversal result, matching the fuse data of the relative motion to obtain a state estimation result;
judging the position of the AGV trolley and historical position coincidence data according to a rear-end module, and constructing closed-loop constraint;
acquiring errors in the movement process through nonlinear optimization, dispersing the errors into each position participating in optimization, eliminating error accumulation, and acquiring the running speed of the AGV;
according to the AGV trolley running speed, each track formed by each speed is evaluated, and the evaluation function is as follows:
G(V,W)＝σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w))
and the central processing unit judges whether the track is the optimal path according to the G value, and starts a path re-planning algorithm to obtain the target optimal path.
The principle and the beneficial effects of the invention are as follows: after the AGV trolley obtains the destination information, firstly, an approximately feasible route is planned through the global route, and then a local route planner is called to plan that the AGV trolley makes a specific action strategy when being local according to the route and the costmap information. The DWA algorithm is called dynamic window approach, and the principle is that a plurality of groups of speeds are sampled in a speed space (v, w), the movement tracks of the speeds in a certain time are simulated, the tracks are scored through an evaluation function, and the optimal speed is selected and sent to a lower computer. The trajectory of the AGV trolley is calculated according to the speed after the trajectory motion model of the AGV trolley is provided. Therefore, only a plurality of speeds are sampled to calculate the track. The method comprises the following steps that (I) the moving AGV is limited by the maximum speed and the minimum speed of the moving AGV, and (II) the moving AGV is influenced by the performance of a motor, because the motor moment is limited and the maximum speed increasing and decreasing limitation exists, a dynamic window exists in a forward simulation period sim _ period of the moving AGV trace, and the speed in the window is the speed which can be actually reached by the AGV:
vd＝{(v,w)|v∈[vc-vbΔtc+vaΔt]∩w∈[wbΔtcwc+waΔt]}
(III) based on the safety considerations of moving the AGV vehicle, in order to be able to stop before encountering an obstacle, there is a range of speeds under maximum deceleration conditions:
an optimized path is generated, but due to external uncontrollable factors, when the moving trolley normally runs under the original specified path, obstacles appear on the path, and the sudden appearance of the obstacles can be captured by a laser radar and a depth camera system of the trolley in time to obtain a point cloud picture. At this time, the braking system of the trolley is activated and sends a braking signal to stop the trolley in place in time. And simultaneously, the central processing unit starts a path replanning algorithm, and the system replans the path in the steps 1) to 3) so that the trolley reaches the destination according to the new optimized path.
As an embodiment of the present invention: the utility model provides a path planning and obstacle avoidance system based on laser radar and depth camera which characterized in that includes:
a track acquisition module: arranging a laser radar and a depth camera on an AGV trolley moving platform, and acquiring an AGV trolley moving track model according to a central processing system preset in the AGV trolley moving platform;
a laser radar module: according to the laser radar, reconstructing a position environment and acquiring environment layout data;
a depth camera module: monitoring the overall running condition of the surrounding environment of the AGV trolley moving platform according to the depth camera;
an autonomous obstacle avoidance and optimal path module: and constructing an autonomous obstacle avoidance algorithm of the AGV trolley moving platform according to the environment layout data and the overall operation condition based on a laser radar and a depth camera, and determining an optimal target path.
As an embodiment of the present invention, the track acquiring module includes:
AGV dolly motion platform unit: arranging a laser radar and a depth camera on an AGV trolley moving platform; wherein the content of the first and second substances,
laser radar first subunit: the laser radar consists of a single-beam narrow-band laser and a receiver and is arranged in the top platform area of the AGV; wherein the content of the first and second substances,
a single-beam narrow-band laser second subunit: the single-beam narrow-band laser is used for emitting light pulses to obtain emission time t 1;
a receiver second subunit: the receiver is used for receiving the light pulse and acquiring a receiving time t 2;
a depth camera unit: the depth camera consists of an RGB camera and a depth camera; wherein the content of the first and second substances,
RGB camera subunit: the RGB camera is used for collecting a light ray image of the shot object;
depth camera subunit: the depth camera is used for acquiring a three-dimensional structure of an image;
a dynamic window algorithm unit: determining a dynamic window algorithm according to a central processing system preset in the AGV trolley moving platform;
a motion model unit: determining a motion model of the AGV according to a dynamic window algorithm; wherein the content of the first and second substances,
assuming that the track of the two-wheeled AGV is a section of arc or straight line (when the rotating speed is 0), a pair of (vt, wt) represents an arc track, and the specific derivation is as follows:
r＝v/w
when the rotation speed w is not equal to 0, the AGV trolley coordinates are:
θt＝θt+WΔi
a motion trail model unit: and simulating the track of the AGV according to the motion model of the AGV, and acquiring a motion track model of the AGV.
As an embodiment of the present invention, the laser radar module includes:
a light signal acquisition unit: acquiring light signals of the surrounding environment of the AGV trolley moving platform according to the RGB camera of the depth camera;
an image information acquisition unit: acquiring image information of the surrounding environment of the AGV trolley moving platform according to a depth camera of the depth camera;
an image phase information acquisition unit: acquiring images of different depth areas of the surrounding environment image according to the light signal and the image information to acquire different image phase information;
a phase data determination unit: determining phase data according to the different image phase information;
a depth information determination unit: calculating the phase data through a preset algorithm, and determining the depth information of the surrounding environment of the AGV trolley moving platform;
a three-dimensional structure determination unit: determining a three-dimensional structure of the surrounding environment of the AGV trolley moving platform according to the depth information;
an overall operation condition determination unit: and determining the overall operation condition of the surrounding environment of the AGV trolley moving platform according to the three-dimensional structure.
As an embodiment of the present invention, the depth camera module includes:
a modeling unit: based on laser radar, the running state of the AGV trolley moving platform is modeled, and a state estimation algorithm is as follows:
xk＝f(xk-1,uk)+vk(1)
zk＝h(xk)+wk(2)
the formula (1) is a motion equation, the position (x (k)) of the AGV moving platform at the time k is determined by the position (x (k-1)) at the time k-1 and the motion input (u (k)) at the time k, and because an error is always introduced in the actual physical environment, a noise amount (v (k)) is added to form certain constraint on state change, and the formula (2) is an observation equation, represents the AGV moving platform sensor observation (z (k)) at the time k and is determined by the AGV moving platform position at the current time. Similarly, a certain observation error, namely w (k), is brought in due to the influence of the physical environment;
three-dimensional navigation coordinate axis determination unit: determining a three-dimensional navigation coordinate axis through an inertial navigation system or a Beidou satellite GPS positioning system in a nearby ground station;
an acquisition information unit: according to the three-dimensional navigation coordinate axis, determining position coordinate information of the laser radar and the surrounding environment, and acquiring the height of the laser radar from the ground and the scanning angle of the laser radar to scan the direction;
acquiring a propagation time unit: transmitting light pulses to the surrounding environment through the laser radar, and acquiring the propagation time according to the transmitting time and the receiving time;
determining a distance unit: determining the distance between the laser radar and surrounding objects according to the propagation time;
the target three-dimensional navigation coordinate axis determination unit: determining a target three-dimensional navigation coordinate axis with the laser radar as an origin according to the height of the laser radar from the ground, and the scanning angle and the scanning direction of the laser radar;
target position coordinate information determination unit: determining target position coordinate information of the surrounding environment of the laser radar according to the target three-dimensional navigation coordinate axis;
acquiring an environment layout data unit: and reconstructing a position environment according to the coordinate information of the target position, and acquiring environment layout data.
As an embodiment of the present invention, the autonomous obstacle avoidance and optimal path module includes:
a slam dividing unit: the method comprises the steps of obtaining a slam scheme based on a laser radar and a depth camera, dividing the slam scheme into two modules, and obtaining a front-end module and a rear-end module;
a dose unit: acquiring the position of the continuous frame of the AGV under different running states according to a front end module;
a dose data unit: comparing laser changes of front and back frames in the AGV trolley movement to acquire the position data of the relative movement of the AGV trolley;
a pos traversal result unit: decomposing the fuse into three dimensions, namely an x axis, a y axis and an angle axis, constructing three layers of for cycles, and respectively performing traversal search on the x direction, the y direction and the rotation angle direction to obtain an optimal fuse traversal result;
a state estimation result unit: according to the optimal fuse traversal result, matching the fuse data of the relative motion to obtain a state estimation result;
the closed-loop constraint unit is used for judging the position of the AGV trolley and the historical position coincidence data according to a rear-end module and constructing closed-loop constraint;
AGV dolly functioning speed unit: acquiring errors in the movement process through nonlinear optimization and closed-loop constraint, dispersing the errors into each position participating in optimization, eliminating error accumulation, and acquiring the running speed of the AGV;
target optimal path unit: according to the AGV trolley running speed, each track formed by each speed is evaluated, a path re-planning algorithm is determined, and the adopted evaluation functions are as follows:
G(V,W)＝σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w))
and judging whether the track of the G value is the optimal path or not, and acquiring the optimal path of the target.
It will be apparent to those skilled in the art that various changes and modifications may be made in the present invention without departing from the spirit and scope of the invention. Thus, if such modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include such modifications and variations.
Claims (10)
1. A path planning and obstacle avoidance method based on a laser radar and a depth camera is characterized by comprising the following steps:
installing a laser radar and a depth camera on an AGV trolley moving platform, and acquiring an AGV trolley moving track model according to a central processing system preset in the AGV trolley moving platform;
according to the laser radar, reconstructing a position environment and acquiring environment layout data;
monitoring the overall running condition of the surrounding environment of the AGV trolley moving platform according to the depth camera;
and constructing an autonomous obstacle avoidance algorithm of the AGV trolley moving platform according to the environment layout data and the overall operation condition based on a laser radar and a depth camera, and determining an optimal target path.
2. The method for path planning and obstacle avoidance based on the laser radar and the depth camera as claimed in claim 1, wherein the laser radar and the depth camera are mounted on an AGV trolley moving platform, and a AGV trolley moving track model is obtained according to a central processing system preset in the AGV trolley moving platform, further comprising:
arranging a laser radar and a depth camera on an AGV trolley moving platform; wherein the content of the first and second substances,
the laser radar consists of a single-beam narrow-band laser and a receiver and is arranged in the top platform area of the AGV; wherein the content of the first and second substances,
the single-beam narrow-band laser is used for emitting light pulses to obtain emission time t 1;
the receiver is used for receiving the light pulse and acquiring a receiving time t 2;
the depth camera consists of an RGB camera and a depth camera; wherein the content of the first and second substances,
the RGB camera is used for collecting a light ray image of the shot object;
the depth camera is used for acquiring a three-dimensional structure of an image;
determining a dynamic window algorithm according to a central processing system preset in the AGV trolley moving platform;
determining a motion model of the AGV according to a dynamic window algorithm; wherein the content of the first and second substances,
assuming that the track of the two-wheeled AGV is a section of arc or straight line (when the rotating speed is 0), a pair of (vt, wt) represents an arc track, and the specific derivation is as follows:
r＝v/w
when the rotation speed w is not equal to 0, the AGV trolley coordinates are:
θt＝θt+WΔi
and simulating the track of the AGV according to the motion model of the AGV, and acquiring a motion track model of the AGV.
3. The method for path planning and obstacle avoidance based on a laser radar and a depth camera as claimed in claim 2, wherein the monitoring of the overall operation of the surrounding environment of the AGV vehicle moving platform according to the depth camera comprises:
acquiring light signals of the surrounding environment of the AGV trolley moving platform according to the RGB camera of the depth camera;
acquiring image information of the surrounding environment of the AGV trolley moving platform according to a depth camera of the depth camera;
acquiring images of different depth areas of the surrounding environment image according to the light signal and the image information to acquire different image phase information;
determining phase data according to the different image phase information;
calculating the phase data through a preset algorithm, and determining the depth information of the surrounding environment of the AGV trolley moving platform;
determining a three-dimensional structure of the surrounding environment of the AGV trolley moving platform according to the depth information;
and determining the overall operation condition of the surrounding environment of the AGV trolley moving platform according to the three-dimensional structure.
4. The method of claim 3, wherein reconstructing a location environment and obtaining environmental layout data according to the lidar comprises:
based on laser radar, the running state of the AGV trolley moving platform is modeled, and a state estimation algorithm is as follows:
xk＝f(xk-1,uk)+vk (1)
zk＝h(xk)+wk (2)
the formula (1) is a motion equation, the position (x (k)) of the AGV moving platform at the time k is determined by the position (x (k-1)) at the time k-1 and the motion input (u (k)) at the time k, and because an error is always introduced in the actual physical environment, a noise amount (v (k)) is added to form certain constraint on state change, and the formula (2) is an observation equation, represents the AGV moving platform sensor observation (z (k)) at the time k and is determined by the AGV moving platform position at the current time. Similarly, a certain observation error, namely w (k), is brought in due to the influence of the physical environment;
determining a three-dimensional navigation coordinate axis through an inertial navigation system or a Beidou satellite GPS positioning system in a nearby ground station;
according to the three-dimensional navigation coordinate axis, determining position coordinate information of the laser radar and the surrounding environment, and acquiring the height of the laser radar from the ground and the scanning angle of the laser radar to scan the direction;
transmitting light pulses to the surrounding environment through the laser radar, and acquiring propagation time according to the transmitting time and the receiving time;
determining the distance between the laser radar and surrounding objects according to the propagation time;
determining a target three-dimensional navigation coordinate axis with the laser radar as an origin according to the height of the laser radar from the ground, and the scanning angle and the scanning direction of the laser radar;
determining coordinate information of a target position of the surrounding environment of the laser radar according to the target three-dimensional navigation coordinate axis;
and reconstructing a position environment according to the coordinate information of the target position, and acquiring environment layout data.
5. The method for path planning and obstacle avoidance based on the laser radar and the depth camera as claimed in claim 2, wherein the method for establishing the AGV moving platform autonomous obstacle avoidance algorithm based on the laser radar and the depth camera according to the environmental layout data and the overall operation condition to determine the optimal path of the target comprises:
the method comprises the steps of obtaining a slam scheme based on a laser radar and a depth camera, dividing the slam scheme into two modules, and obtaining a front-end module and a rear-end module;
acquiring the position of the continuous frame of the AGV under different running states according to a front end module;
comparing laser changes of front and back frames in the AGV trolley movement to acquire the position data of the relative movement of the AGV trolley;
decomposing the fuse into three dimensions, namely an x axis, a y axis and an angle axis, constructing three layers of for cycles, and respectively performing traversal search on the x direction, the y direction and the rotation angle direction to obtain an optimal fuse traversal result;
according to the optimal fuse traversal result, matching the fuse data of the relative motion to obtain a state estimation result;
judging the position of the AGV trolley and historical position coincidence data according to a rear-end module, and constructing closed-loop constraint;
acquiring errors in the movement process through nonlinear optimization, dispersing the errors into each position participating in optimization, eliminating error accumulation, and acquiring the running speed of the AGV;
according to the AGV trolley running speed, each track formed by each speed is evaluated, and the evaluation function is as follows:
G(V,W)＝σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w))
and the central processing unit judges whether the track is the optimal path according to the G value, and starts a path re-planning algorithm to obtain the target optimal path.
6. The utility model provides a path planning and obstacle avoidance system based on laser radar and depth camera which characterized in that includes:
a track acquisition module: arranging a laser radar and a depth camera on an AGV trolley moving platform, and acquiring an AGV trolley moving track model according to a central processing system preset in the AGV trolley moving platform;
a laser radar module: according to the laser radar, reconstructing a position environment and acquiring environment layout data;
a depth camera module: monitoring the overall running condition of the surrounding environment of the AGV trolley moving platform according to the depth camera;
an autonomous obstacle avoidance and optimal path module: and constructing an autonomous obstacle avoidance algorithm of the AGV trolley moving platform according to the environment layout data and the overall operation condition based on a laser radar and a depth camera, and determining an optimal target path.
7. The system for path planning and obstacle avoidance based on lidar and a depth camera of claim 6, wherein the track acquisition module comprises:
AGV dolly motion platform unit: arranging a laser radar and a depth camera on an AGV trolley moving platform; wherein the content of the first and second substances,
laser radar first subunit: the laser radar consists of a single-beam narrow-band laser and a receiver and is arranged in the top platform area of the AGV; wherein the content of the first and second substances,
a single-beam narrow-band laser second subunit: the single-beam narrow-band laser is used for emitting light pulses to obtain emission time t 1;
a receiver second subunit: the receiver is used for receiving the light pulse and acquiring a receiving time t 2;
a depth camera unit: the depth camera consists of an RGB camera and a depth camera; wherein the content of the first and second substances,
RGB camera subunit: the RGB camera is used for collecting a light ray image of the shot object;
depth camera subunit: the depth camera is used for acquiring a three-dimensional structure of an image;
a dynamic window algorithm unit: determining a dynamic window algorithm according to a central processing system preset in the AGV trolley moving platform;
a motion model unit: determining a motion model of the AGV according to a dynamic window algorithm; wherein the content of the first and second substances,
assuming that the track of the two-wheeled AGV is a section of arc or straight line (when the rotating speed is 0), a pair of (vt, wt) represents an arc track, and the specific derivation is as follows:
r＝v/w
when the rotation speed w is not equal to 0, the AGV trolley coordinates are:
θt＝θt+WΔi
a motion trail model unit: and simulating the track of the AGV according to the motion model of the AGV, and acquiring a motion track model of the AGV.
8. The lidar and depth camera based path planning and obstacle avoidance system of claim 6, wherein the lidar module comprises:
a light signal acquisition unit: acquiring light signals of the surrounding environment of the AGV trolley moving platform according to the RGB camera of the depth camera;
an image information acquisition unit: acquiring image information of the surrounding environment of the AGV trolley moving platform according to a depth camera of the depth camera;
an image phase information acquisition unit: acquiring images of different depth areas of the surrounding environment image according to the light signal and the image information to acquire different image phase information;
a phase data determination unit: determining phase data according to the different image phase information;
a depth information determination unit: calculating the phase data through a preset algorithm, and determining the depth information of the surrounding environment of the AGV trolley moving platform;
a three-dimensional structure determination unit: determining a three-dimensional structure of the surrounding environment of the AGV trolley moving platform according to the depth information;
an overall operation condition determination unit: and determining the overall operation condition of the surrounding environment of the AGV trolley moving platform according to the three-dimensional structure.
9. The lidar and depth camera based path planning and obstacle avoidance system of claim 6, wherein the depth camera module comprises:
a modeling unit: based on laser radar, the running state of the AGV trolley moving platform is modeled, and a state estimation algorithm is as follows:
xk＝f(xk-1,uk)+vk (1)
zk＝h(xk)+wk (2)
the formula (1) is a motion equation, the position (x (k)) of the AGV moving platform at the time k is determined by the position (x (k-1)) at the time k-1 and the motion input (u (k)) at the time k, and because an error is always introduced in the actual physical environment, a noise amount (v (k)) is added to form certain constraint on state change, and the formula (2) is an observation equation, represents the AGV moving platform sensor observation (z (k)) at the time k and is determined by the AGV moving platform position at the current time. Similarly, a certain observation error, namely w (k), is brought in due to the influence of the physical environment;
three-dimensional navigation coordinate axis determination unit: determining a three-dimensional navigation coordinate axis through an inertial navigation system or a Beidou satellite GPS positioning system in a nearby ground station;
an acquisition information unit: according to the three-dimensional navigation coordinate axis, determining position coordinate information of the laser radar and the surrounding environment, and acquiring the height of the laser radar from the ground and the scanning angle of the laser radar to scan the direction;
acquiring a propagation time unit: transmitting light pulses to the surrounding environment through the laser radar, and acquiring the propagation time according to the transmitting time and the receiving time;
determining a distance unit: determining the distance between the laser radar and surrounding objects according to the propagation time;
the target three-dimensional navigation coordinate axis determination unit: determining a target three-dimensional navigation coordinate axis with the laser radar as an origin according to the height of the laser radar from the ground, and the scanning angle and the scanning direction of the laser radar;
target position coordinate information determination unit: determining target position coordinate information of the surrounding environment of the laser radar according to the target three-dimensional navigation coordinate axis;
acquiring an environment layout data unit: and reconstructing a position environment according to the coordinate information of the target position, and acquiring environment layout data.
10. The lidar and depth camera based path planning and obstacle avoidance system of claim 6, wherein the autonomous obstacle avoidance and optimal path module comprises:
a slam dividing unit: the method comprises the steps of obtaining a slam scheme based on a laser radar and a depth camera, dividing the slam scheme into two modules, and obtaining a front-end module and a rear-end module;
a dose unit: acquiring the position of the continuous frame of the AGV under different running states according to a front end module;
a dose data unit: comparing laser changes of front and back frames in the AGV trolley movement to acquire the position data of the relative movement of the AGV trolley;
a pos traversal result unit: decomposing the fuse into three dimensions, namely an x axis, a y axis and an angle axis, constructing three layers of for cycles, and respectively performing traversal search on the x direction, the y direction and the rotation angle direction to obtain an optimal fuse traversal result;
a state estimation result unit: according to the optimal fuse traversal result, matching the fuse data of the relative motion to obtain a state estimation result;
the closed-loop constraint unit is used for judging the position of the AGV trolley and the historical position coincidence data according to a rear-end module and constructing closed-loop constraint;
AGV dolly functioning speed unit: acquiring errors in the movement process through nonlinear optimization and closed-loop constraint, dispersing the errors into each position participating in optimization, eliminating error accumulation, and acquiring the running speed of the AGV;
target optimal path unit: according to the AGV trolley running speed, each track formed by each speed is evaluated, a path re-planning algorithm is determined, and the adopted evaluation functions are as follows: g (V, W) ═ σ (α, leading (V, W) + β · dist (V, W) + γ · velocity (V, W))
And judging whether the track of the G value is the optimal path or not, and acquiring the optimal path of the target.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011179495.0A CN112611374A (en) | 2020-10-29 | 2020-10-29 | Path planning and obstacle avoidance method and system based on laser radar and depth camera |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011179495.0A CN112611374A (en) | 2020-10-29 | 2020-10-29 | Path planning and obstacle avoidance method and system based on laser radar and depth camera |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112611374A true CN112611374A (en) | 2021-04-06 |
Family
ID=75224519
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011179495.0A Pending CN112611374A (en) | 2020-10-29 | 2020-10-29 | Path planning and obstacle avoidance method and system based on laser radar and depth camera |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112611374A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113436241A (en) * | 2021-06-25 | 2021-09-24 | 兰剑智能科技股份有限公司 | Interference checking method and system adopting depth information |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109343537A (en) * | 2018-11-22 | 2019-02-15 | 东南大学 | Full autonomous driving racing trolley and operation method |
| CN109900280A (en) * | 2019-03-27 | 2019-06-18 | 浙江大学 | A kind of livestock and poultry information Perception robot and map constructing method based on independent navigation |
| CN110058591A (en) * | 2019-04-24 | 2019-07-26 | 合肥柯金自动化科技股份有限公司 | A kind of AGV system based on laser radar Yu depth camera hybrid navigation |
| CN111596657A (en) * | 2020-05-09 | 2020-08-28 | 浙江工业大学 | AGV and track motion method thereof |
| CN111679673A (en) * | 2020-06-12 | 2020-09-18 | 昆明理工大学 | Charging trolley for wireless chargeable sensing network |
| CN111781929A (en) * | 2020-07-08 | 2020-10-16 | 苏州索亚机器人技术有限公司 | AGV trolley and 3D laser radar positioning and navigation method |
-
2020
- 2020-10-29 CN CN202011179495.0A patent/CN112611374A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109343537A (en) * | 2018-11-22 | 2019-02-15 | 东南大学 | Full autonomous driving racing trolley and operation method |
| CN109900280A (en) * | 2019-03-27 | 2019-06-18 | 浙江大学 | A kind of livestock and poultry information Perception robot and map constructing method based on independent navigation |
| CN110058591A (en) * | 2019-04-24 | 2019-07-26 | 合肥柯金自动化科技股份有限公司 | A kind of AGV system based on laser radar Yu depth camera hybrid navigation |
| CN111596657A (en) * | 2020-05-09 | 2020-08-28 | 浙江工业大学 | AGV and track motion method thereof |
| CN111679673A (en) * | 2020-06-12 | 2020-09-18 | 昆明理工大学 | Charging trolley for wireless chargeable sensing network |
| CN111781929A (en) * | 2020-07-08 | 2020-10-16 | 苏州索亚机器人技术有限公司 | AGV trolley and 3D laser radar positioning and navigation method |
Non-Patent Citations (1)
| Title |
|---|
| 杨振 等，北京理工大学出版社, 东北财经大学出版社 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113436241A (en) * | 2021-06-25 | 2021-09-24 | 兰剑智能科技股份有限公司 | Interference checking method and system adopting depth information |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Montemerlo et al. | Junior: The stanford entry in the urban challenge | |
| US8412449B2 (en) | Control and systems for autonomously driven vehicles | |
| CN111522339A (en) | Automatic path planning and positioning method and device for inspection robot of livestock and poultry house | |
| CN108958250A (en) | Multisensor mobile platform and navigation and barrier-avoiding method based on known map | |
| US10131446B1 (en) | Addressing multiple time around (MTA) ambiguities, particularly for lidar systems, and particularly for autonomous aircraft | |
| CN112558608A (en) | Vehicle-mounted machine cooperative control and path optimization method based on unmanned aerial vehicle assistance | |
| CN214520204U (en) | Port area intelligent inspection robot based on depth camera and laser radar | |
| CN113791621B (en) | Automatic steering tractor and airplane docking method and system | |
| US20190163201A1 (en) | Autonomous Vehicle Sensor Compensation Using Displacement Sensor | |
| CN112068574A (en) | Control method and system for unmanned vehicle in dynamic complex environment | |
| US20210394781A1 (en) | Dual lidar sensor for annotated point cloud generation | |
| US20190163189A1 (en) | Autonomous Vehicle Sensor Compensation By Monitoring Acceleration | |
| CN115993825A (en) | Unmanned vehicle cluster control system based on air-ground cooperation | |
| EP4148385A1 (en) | Vehicle navigation positioning method and apparatus, and base station, system and readable storage medium | |
| CN112611374A (en) | Path planning and obstacle avoidance method and system based on laser radar and depth camera | |
| WO2022081399A1 (en) | System for anticipating future state of an autonomous vehicle | |
| CN113758482B (en) | Vehicle navigation positioning method, device, base station, system and readable storage medium | |
| Chen et al. | Split covariance intersection filter based front-vehicle track estimation for vehicle platooning without communication | |
| US20220212694A1 (en) | Methods and systems for generating a longitudinal plan for an autonomous vehicle based on behavior of uncertain road users | |
| Berlin | Spirit of berlin: An autonomous car for the DARPA urban challenge hardware and software architecture | |
| CN114625122A (en) | Robot following and obstacle avoiding method and system based on UWB and laser radar | |
| CN113341968A (en) | Accurate parking system and method for multi-axis flat car | |
| US20230237793A1 (en) | False track mitigation in object detection systems | |
| US20240069205A1 (en) | Systems and methods for clock-skew search to improve depth accuracy in geiger mode lidar | |
| US20220067399A1 (en) | Autonomous vehicle system for performing object detections using a logistic cylinder pedestrian model |
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: 20210406 |