CN111983632A - Self-navigation positioning method and system based on double-line laser radar - Google Patents
Self-navigation positioning method and system based on double-line laser radar Download PDFInfo
- Publication number
- CN111983632A CN111983632A CN202010834379.1A CN202010834379A CN111983632A CN 111983632 A CN111983632 A CN 111983632A CN 202010834379 A CN202010834379 A CN 202010834379A CN 111983632 A CN111983632 A CN 111983632A
- Authority
- CN
- China
- Prior art keywords
- data
- laser radar
- self
- vehicle body
- pitching
- 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.)
- Withdrawn
Links
Images
Classifications
-
- 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/87—Combinations of systems using electromagnetic waves other than radio waves
-
- 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
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/50—Systems of measurement based on relative movement of target
-
- 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
- Navigation (AREA)
Abstract
The invention provides a self-navigation positioning system, a self-navigation positioning method and a storage medium based on double-one-line laser radar, and relates to the field of machine vision The method and the device realize quick map building of the surrounding environment and improve the self-navigation positioning precision.
Description
Technical Field
The invention relates to the field of machine vision, in particular to a self-navigation positioning system and method based on a double-line laser radar and a storage medium.
Background
The autonomous navigation mobile robot has an increasingly wide application range in production and life, is a robot which carries a specific sensor, can establish an environment model and estimate the motion of the robot in the motion process under the condition that no environment first verification information exists.
The navigation robot carrying the single-line laser radar is mainly applied in the prior art, the robot adopts the single-line laser radar to acquire data, has single direction, sparse quantity and low mapping speed, cannot realize quick scanning of the surrounding environment, and has low self-navigation and positioning accuracy.
Disclosure of Invention
Based on the existing problems, the invention provides a self-navigation positioning system, a self-navigation positioning method and a storage medium based on a double-line laser radar, which are used for solving the technical problems that the direction of data acquired by adopting a single-line laser radar is single, the quantity is sparse, the mapping speed is slow, the rapid scanning of the surrounding environment cannot be realized, and the self-navigation and positioning accuracy is not high in the prior art.
The embodiment of the invention provides a self-navigation positioning system based on a double-line laser radar, which comprises:
the system comprises a data acquisition unit 1, a self-navigation algorithm control unit 2, a vehicle body 3 and a vehicle body motion control unit 4;
the data acquisition unit 1 includes: the system comprises a front laser radar 1-1, a rear laser radar 1-2, a first pitching motor 1-3, a first pitching motor encoder 1-4, a first pitching motor driving unit 1-5, a second pitching motor 1-6, a second pitching motor driving unit 1-7, a second pitching motor encoder 1-8 and an inertia measurement IMU module 1-9;
the self-navigation algorithm control unit 2 includes: the self-navigation algorithm execution computer 2-1;
the vehicle body motion control unit 4 includes: a driving motor driving unit 4-1, a steering motor driving unit 4-2, a driving motor 4-3, a steering motor 4-4, a driving motor encoder 4-5 and a steering motor encoder 4-6;
the front laser radar 1-1 of the data acquisition unit 1 is arranged at the front part of the vehicle body 3, is connected with the first pitching motor 1-3, the first pitching motor encoder 1-4 and the first pitching motor driving unit 1-5, and is in data communication with the self-navigation algorithm execution computer 2-1;
the rear laser radar 1-2 of the data acquisition unit 1 is arranged at the rear part of the vehicle body 3, the second pitching motor 1-6, the second pitching motor driving unit 1-7 and the second pitching motor encoder 1-8 are connected and carry out data communication with the self-navigation algorithm execution computer 2-1;
an inertial measurement IMU module 1-9 of the data acquisition unit 1 is in data communication with a self-navigation algorithm execution computer 2-1;
a driving motor driving unit 4-1 of the vehicle body motion control unit 4 is connected with a driving motor 4-3 and a driving motor encoder 4-5, and is in data communication with a self-navigation algorithm execution computer 2-1;
and a steering motor driving unit 4-2 of the vehicle body motion control unit 4 is connected with a steering motor 4-4 and a steering motor encoder 4-6 and is in data communication with a self-navigation algorithm execution computer 2-1.
Further, the connection specifically includes: and (6) electrically connecting the wires.
Further, the self-navigation algorithm execution computer 2-1 of the self-navigation algorithm control unit 2 performs data communication with the data acquisition unit 1 and the devices in the vehicle body motion control unit 4, and performs mapping, vehicle body motion trajectory monitoring and motion control on the surrounding environment according to the acquired data.
Furthermore, the front laser radar 1-1 and the rear laser radar 1-2 are symmetrically arranged at the front end and the rear end of the vehicle body 3 by using the same equipment.
Compared with the prior art, the self-navigation positioning system based on the double-line laser radar provided by the embodiment of the invention has the following beneficial effects that: the vehicle body is in the process of advancing, the pitching motor encoder and the pitching motor driving unit drive front and back laser radars which are symmetrically arranged in tandem to perform reciprocating pitching motion, the laser radars and the inertia measurement IMU module complete data acquisition and correction work, a self-navigation algorithm execution computer of the self-navigation algorithm control unit acquires data acquired by the laser radars, a double-line laser radar self-navigation positioning algorithm is executed, the vehicle body advancing track is evaluated, advancing and turning parameters are given to a driving motor and a turning motor of the vehicle body motion control unit, the actual vehicle body advancing track is corrected according to a preset track, multi-angle scanning of the surrounding environment in the process of advancing the vehicle body is completed, rapid drawing construction is achieved, and self-navigation and positioning accuracy of the advancing track of the self-navigation vehicle body are improved.
The embodiment of the invention also provides a self-navigation positioning method based on the double-line laser radar, which comprises the following steps:
the system controls a first pitching motor 1-3, a second pitching motor 1-7, a pitching motor encoder and a pitching motor driving unit which are matched with each other to drive a front laser radar and a rear laser radar to reciprocate to perform pitching motion according to the same preset frequency and speed, and environmental data are collected;
coupling the collected environment data with angle data output by a pitch motor encoder to obtain 3D point cloud data;
the pitching motor drives the laser radar to complete one-time reciprocating motion, namely one frame of data is collected, the image features of the 3D point cloud data are extracted by utilizing an ORB algorithm, the image features of two continuous frames of data are matched, the displacement relation between the same feature elements of the two frames of data is calculated, the trolley displacement data in the interval period of the two frames of data is obtained, and the actual travelling track of the trolley body is obtained;
the system adopts front and rear double line laser radars, and the vehicle body performs loop monitoring when walking beyond a preset distance, so that the accumulated error is reduced;
and comparing the actual advancing track of the vehicle body with a preset advancing path to obtain a path error, carrying out steering and advancing direction parameter correction according to the preset advancing path through an incremental PID algorithm, and controlling the vehicle body to advance according to the preset path.
Further, the coupling of the acquired environment data and the angle data output by the pitch motor encoder to obtain 3D point cloud data includes:
collecting a distance l measured by a laser radar, a horizontal included angle alpha between light and a central line of the laser radar measured by the laser radar and a pitching angle beta of reciprocating swing of the laser radar;
the coordinate u of the 3D point cloud data of the object measured by the lidar is (x, y, z), which is calculated as follows:
further, the coupling of the acquired environment data and the angle data output by the pitch motor encoder to obtain 3D point cloud data further comprises:
correcting data acquired in the advancing process of the vehicle body;
acquiring acceleration data (x, y, z) of 3D point cloud data in a vehicle body movement process according to a system inertia measurement IMU module, wherein displacement data delta u (delta x, delta y, delta z) in a sampling interval delta t;
the corrected 3D point cloud data form is as follows: u + Δ u ═ x + Δ x, y + Δ y, z + Δ z.
Further, the pitching motor drives the laser radar to complete one reciprocating motion, namely, one frame of data acquisition is completed, the image features of the 3D point cloud data are extracted by utilizing an ORB algorithm, the image features of two continuous frames of data are matched, the displacement relation between the feature elements with the same data of the two frames is calculated, the trolley displacement data during the interval of the two frames of data is obtained, and the actual advancing track of the trolley body is obtained, and the method comprises the following steps:
extracting image features from adjacent data frames, and acquiring a key point FAST and a descriptor BRIEF of the 3D point cloud data by using an ORB algorithm;
adopting an image pyramid method to perform different levels of downsampling on image data so as to realize the scale transformation of key points, and adopting a gray scale centroid method to obtain the direction information of the image feature key points;
after the image characteristics of each frame are extracted, matching calculation is carried out on the characteristics between the frames, and the same characteristic points are calculated by adopting a fast approximate nearest neighbor FLANN algorithm;
after image feature matching calculation is completed, the moving distance and direction change of the trolley between two frames of images can be calculated, the iterative closest point ICP method is adopted for calculation according to the position of the departure point of the trolley body, the current position and direction of the trolley are solved, and the actual advancing track of the trolley body is obtained.
Further, the actual advancing track of the vehicle body is compared with a preset advancing path to obtain a path error, the incremental PID algorithm is used for steering and advancing direction parameter correction according to the preset advancing path, and the vehicle body is controlled to advance according to the preset path, specifically: the incremental PID control algorithm is as follows:
Δu(k)＝u(k)-u(k-1)
＝Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
kp, Ki and Kd are PID control parameters, e (k) is a k-th error value, and u (k) is a control quantity.
Embodiments of the present invention also provide a computer-readable storage medium storing one or more programs, which are executable by one or more processors to implement a dual-line lidar based self-navigation positioning method according to any of the preceding claims 5 to 9.
Compared with the prior art, the self-navigation positioning method and the storage medium based on the double-line laser radar at least realize the following beneficial effects: the invention provides a self-navigation positioning method based on double-line laser radar, wherein a front laser radar and a rear laser radar perform pitching motion in a reciprocating manner according to the same preset frequency and speed, environment data are acquired in multiple angles to obtain 3D point cloud data which are denser than the self-navigation positioning method of the laser radar, the calculation speed is high by using an ORB (object-oriented bounding box) method, the requirement of controlling the motion of a trolley in real time can be met, the same characteristic point can be calculated quickly by adopting a quick approximate nearest neighbor FLANN (nearest neighbor computing network) algorithm, and the biggest problem in the self-navigation technical scheme is the problem of accumulated error The positioning precision effectively solves the technical problems that the direction of data acquired by adopting a single-line laser radar is single, the quantity is sparse, the mapping speed is low, the rapid scanning of the surrounding environment cannot be realized, and the self-navigation and positioning precision are not high.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without inventive exercise.
Fig. 1 is a structural diagram of a self-navigation positioning system based on a dual-line lidar according to an embodiment of the present invention;
fig. 2 is a schematic diagram of the installation positions of front and rear laser radars of a dual-line laser radar-based self-navigation positioning system according to an embodiment of the present invention;
fig. 3 is a flowchart of a self-navigation positioning method based on a dual-line lidar according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention clearer, a specific implementation of an ECC scalar multiplier implementation method based on prime number preprocessing according to an embodiment of the present invention is described in detail below with reference to the accompanying drawings. It should be understood that the preferred embodiments described below are only for illustrating and explaining the present invention and are not to be used for limiting the present invention. And the embodiments and features of the embodiments in the present application may be combined with each other without conflict.
The embodiment of the invention provides a self-navigation positioning system based on a double-line laser radar, the system structure is shown in fig. 1 and fig. 2, and the self-navigation positioning system comprises:
the system comprises a data acquisition unit 1, a self-navigation algorithm control unit 2, a vehicle body 3 and a vehicle body motion control unit 4;
the data acquisition unit 1 includes: the system comprises a front laser radar 1-1, a rear laser radar 1-2, a first pitching motor 1-3, a first pitching motor encoder 1-4, a first pitching motor driving unit 1-5, a second pitching motor 1-6, a second pitching motor driving unit 1-7, a second pitching motor encoder 1-8 and an inertia measurement IMU module 1-9;
the self-navigation algorithm control unit 2 includes: the self-navigation algorithm execution computer 2-1;
the vehicle body motion control unit 4 includes: a driving motor driving unit 4-1, a steering motor driving unit 4-2, a driving motor 4-3, a steering motor 4-4, a driving motor encoder 4-5 and a steering motor encoder 4-6;
the front laser radar 1-1 of the data acquisition unit 1 is arranged at the front part of the vehicle body 3, is connected with the first pitching motor 1-3, the first pitching motor encoder 1-4 and the first pitching motor driving unit 1-5, and is in data communication with the self-navigation algorithm execution computer 2-1;
the rear laser radar 1-2 of the data acquisition unit 1 is arranged at the rear part of the vehicle body 3, the second pitching motor 1-6, the second pitching motor driving unit 1-7 and the second pitching motor encoder 1-8 are connected and carry out data communication with the self-navigation algorithm execution computer 2-1;
an inertial measurement IMU module 1-9 of the data acquisition unit 1 is in data communication with a self-navigation algorithm execution computer 2-1;
a driving motor driving unit 4-1 of the vehicle body motion control unit 4 is connected with a driving motor 4-3 and a driving motor encoder 4-5, and is in data communication with a self-navigation algorithm execution computer 2-1;
and a steering motor driving unit 4-2 of the vehicle body motion control unit 4 is connected with a steering motor 4-4 and a steering motor encoder 4-6 and is in data communication with a self-navigation algorithm execution computer 2-1.
Preferably, the connection is, in particular: and (6) electrically connecting the wires.
Preferably, the self-navigation algorithm execution computer 2-1 of the self-navigation algorithm control unit 2 performs data communication with the data acquisition unit 1 and the devices in the vehicle body motion control unit 4, and performs mapping, vehicle body motion trajectory monitoring and motion control on the surrounding environment according to the acquired data.
Preferably, the front laser radar 1-1 and the rear laser radar 1-2 are installed at the front end and the rear end of the vehicle body 3 symmetrically by using the same equipment.
Compared with the prior art, the self-navigation positioning system based on the double-line laser radar provided by the embodiment of the invention has the following beneficial effects that: the vehicle body is in the process of advancing, the pitching motor, a pitching motor encoder, a pitching motor driving unit drive front and back laser radars which are symmetrically arranged in tandem to perform reciprocating pitching motion, the laser radars and an inertia measurement IMU module complete data acquisition and correction work, a self-navigation algorithm execution computer of a self-navigation algorithm control unit acquires data acquired by the laser radars, a double-line laser radar self-navigation positioning algorithm is executed, the vehicle body advancing track is evaluated, advancing and turning parameters are given to a driving motor and a turning motor of the vehicle body motion control unit, the actual vehicle body advancing track is corrected according to a preset track, multi-angle scanning of the surrounding environment in the vehicle body advancing process is completed, rapid drawing construction is achieved, and the control precision of the self-navigation vehicle body advancing track is improved.
The embodiment of the invention also provides a self-navigation positioning method based on the double-line laser radar, and the flow chart of the method is shown in fig. 3, and the method comprises the following steps:
s301, the system controls a first pitching motor 1-3, a second pitching motor 1-7, a pitching motor encoder and a pitching motor driving unit which are matched with each other to drive a front laser radar and a rear laser radar to reciprocate to perform pitching motion according to the same preset frequency and speed, and environmental data are collected;
s302, coupling the collected environment data with angle data output by a pitch motor encoder to obtain 3D point cloud data;
s303, the pitching motor drives the laser radar to complete one-time reciprocating motion, namely one frame of data is collected, image features of the 3D point cloud data are extracted by utilizing an ORB algorithm, the image features of two continuous frames of data are matched, the displacement relation between the same feature elements of the two frames of data is calculated, trolley displacement data in the interval period of the two frames of data is obtained, and the actual travelling track of the trolley body is obtained;
s304, the system adopts front and rear double line laser radars, and the vehicle body performs loop monitoring when the vehicle body travels beyond a preset distance, so that the accumulated error is reduced;
s305, comparing the actual advancing track of the vehicle body with a preset advancing path to obtain a path error, performing steering and advancing direction parameter correction according to the preset advancing path through an incremental PID algorithm, and controlling the vehicle body to advance according to the preset path.
Preferably, the coupling the acquired environment data with the angle data output by the pitch motor encoder to obtain 3D point cloud data includes:
collecting a distance l measured by a laser radar, a horizontal included angle alpha between light and a central line of the laser radar measured by the laser radar and a pitching angle beta of reciprocating swing of the laser radar;
the coordinate u of the 3D point cloud data of the object measured by the lidar is (x, y, z), which is calculated as follows:
preferably, the coupling the acquired environment data with the angle data output by the pitch motor encoder to obtain 3D point cloud data further includes:
correcting data acquired in the advancing process of the vehicle body;
acquiring acceleration data (x, y, z) of 3D point cloud data in a vehicle body movement process according to a system inertia measurement IMU module, wherein displacement data delta u (delta x, delta y, delta z) in a sampling interval delta t;
the corrected 3D point cloud data form is as follows: u + Δ u ═ x + Δ x, y + Δ y, z + Δ z.
Preferably, the pitching motor drives the laser radar to complete one reciprocating motion, namely, one frame of data acquisition, the image features of the 3D point cloud data are extracted by using the ORB algorithm, the image features of two consecutive frames of data are matched, the displacement relationship between the same feature elements of the two frames of data is calculated, the trolley displacement data during the interval of the two frames of data is obtained, and the actual travelling track of the trolley body is obtained, which includes:
extracting image features from adjacent data frames, and acquiring a key point FAST and a descriptor BRIEF of the 3D point cloud data by using an ORB algorithm;
adopting an image pyramid method to perform different levels of downsampling on image data so as to realize the scale transformation of key points, and adopting a gray scale centroid method to obtain the direction information of the image feature key points;
after the image characteristics of each frame are extracted, matching calculation is carried out on the characteristics between the frames, and the same characteristic points are calculated by adopting a fast approximate nearest neighbor FLANN algorithm;
after image feature matching calculation is completed, the moving distance and direction change of the trolley between two frames of images can be calculated, the iterative closest point ICP method is adopted for calculation according to the position of the departure point of the trolley body, the current position and direction of the trolley are solved, and the actual advancing track of the trolley body is obtained.
Preferably, the actual advancing track of the vehicle body is compared with a preset advancing path to obtain a path error, the incremental PID algorithm is used for steering and advancing direction parameter correction according to the preset advancing path, and the vehicle body is controlled to advance according to the preset path, specifically: the incremental PID control algorithm is as follows:
Δu(k)＝u(k)-u(k-1)
＝Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
kp, Ki and Kd are PID control parameters, e (k) is a k-th error value, and u (k) is a control quantity.
Embodiments of the present invention also provide a computer-readable storage medium storing one or more programs, which are executable by one or more processors to implement a dual-line lidar based self-navigation positioning method according to any of the preceding claims 5 to 9.
Compared with the prior art, the self-navigation positioning method and the storage medium based on the double-line laser radar at least realize the following beneficial effects: the invention provides a self-navigation positioning method based on double-line laser radar, wherein a front laser radar and a rear laser radar perform pitching motion in a reciprocating manner according to the same preset frequency and speed, environment data are acquired in multiple angles to obtain 3D point cloud data which are denser than the self-navigation positioning method of the laser radar, the calculation speed is high by using an ORB (object-oriented bounding box) method, the requirement of controlling the motion of a trolley in real time can be met, the same characteristic point can be calculated quickly by adopting a quick approximate nearest neighbor FLANN (nearest neighbor computing network) algorithm, and the biggest problem in the self-navigation technical scheme is the problem of accumulated error The positioning precision effectively solves the technical problems that the direction of data acquired by adopting a single-line laser radar is single, the quantity is sparse, the mapping speed is low, the rapid scanning of the surrounding environment cannot be realized, and the self-navigation and positioning precision are not high.
Through the above description of the embodiments, those skilled in the art will clearly understand that the embodiments of the present invention may be implemented by hardware, or by a combination of software and a necessary general hardware platform. Based on such understanding, the technical solutions of the embodiments of the present invention may be embodied in the form of a software product, which may be stored in a non-volatile storage medium (which may be a CD-ROM, a usb disk, a removable hard disk, etc.), and includes several instructions for enabling a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the methods according to the embodiments of the present invention.
Those skilled in the art will appreciate that the drawings are merely schematic representations of one preferred embodiment and that the blocks or flow diagrams in the drawings are not necessarily required to practice the present invention.
Those skilled in the art will appreciate that the modules in the devices in the embodiments may be distributed in the devices in the embodiments according to the description of the embodiments, and may be correspondingly changed in one or more devices different from the embodiments. The modules of the above embodiments may be combined into one module, or further split into multiple sub-modules.
The above-mentioned serial numbers of the embodiments of the present invention are merely for description and do not represent the merits of the embodiments.
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 self-navigation positioning system based on a double-line laser radar is characterized by comprising:
the system comprises a data acquisition unit (1), a self-navigation algorithm control unit (2), a vehicle body (3) and a vehicle body motion control unit (4);
the data acquisition unit (1) comprises: the system comprises a front laser radar (1-1), a rear laser radar (1-2), a first pitching motor (1-3), a first pitching motor encoder (1-4), a first pitching motor driving unit (1-5), a second pitching motor (1-6), a second pitching motor driving unit (1-7), a second pitching motor encoder (1-8) and an inertia measurement IMU module (1-9);
the self-navigation algorithm control unit (2) comprises: a self-navigation algorithm execution computer (2-1);
the vehicle body motion control unit (4) includes: the device comprises a driving motor driving unit (4-1), a steering motor driving unit (4-2), a driving motor (4-3), a steering motor (4-4), a driving motor encoder (4-5) and a steering motor encoder (4-6);
the front laser radar (1-1) of the data acquisition unit (1) is mounted at the front part of the vehicle body (3), is connected with the first pitching motor (1-3), the first pitching motor encoder (1-4) and the first pitching motor driving unit (1-5) and is in data communication with the self-navigation algorithm execution computer (2-1);
the rear laser radar (1-2) of the data acquisition unit (1) is arranged at the rear part of the vehicle body (3), the second pitching motor (1-6), the second pitching motor driving unit (1-7) and the second pitching motor encoder (1-8) are connected and carry out data communication with the self-navigation algorithm execution computer (2-1);
an inertial measurement IMU module (1-9) of the data acquisition unit (1) is in data communication with a self-navigation algorithm execution computer (2-1);
a driving motor driving unit (4-1) of the vehicle body motion control unit (4) is connected with a driving motor (4-3) and a driving motor encoder (4-5), and is in data communication with a self-navigation algorithm execution computer (2-1);
and a steering motor driving unit (4-2) of the vehicle body motion control unit (4) is connected with the steering motor (4-4) and a steering motor encoder (4-6), and is in data communication with the self-navigation algorithm execution computer (2-1).
2. The system according to claim 1, characterized in that said connection is, in particular: and (6) electrically connecting the wires.
3. The system according to claim 1, wherein the self-navigation algorithm execution computer (2-1) of the self-navigation algorithm control unit (2) performs data communication with the data acquisition unit (1) and the equipment in the vehicle body motion control unit (4), and performs mapping, vehicle body motion trajectory monitoring and motion control on the surrounding environment according to the acquired data.
4. The system of claim 1, wherein the front laser radar (1-1) and the rear laser radar (1-2) are the same device and are symmetrically arranged at the front end and the rear end of the vehicle body (3).
5. A self-navigation positioning method based on a double-line laser radar is characterized by comprising the following steps:
the system controls a first pitching motor (1-3), a second pitching motor (1-7), a pitching motor encoder and a pitching motor driving unit which are matched with each other to drive a front laser radar and a rear laser radar to reciprocate to perform pitching motion according to the same preset frequency and speed, and environmental data are collected;
coupling the collected environment data with angle data output by a pitch motor encoder to obtain 3D point cloud data;
the pitching motor drives the laser radar to complete one-time reciprocating motion, namely one frame of data is collected, the image features of the 3D point cloud data are extracted by utilizing an ORB algorithm, the image features of two continuous frames of data are matched, the displacement relation between the same feature elements of the two frames of data is calculated, the trolley displacement data in the interval period of the two frames of data is obtained, and the actual travelling track of the trolley body is obtained;
the system adopts front and rear double line laser radars, and the vehicle body performs loop monitoring when walking beyond a preset distance, so that the accumulated error is reduced;
and comparing the actual advancing track of the vehicle body with a preset advancing path to obtain a path error, carrying out steering and advancing direction parameter correction according to the preset advancing path through an incremental PID algorithm, and controlling the vehicle body to advance according to the preset path.
6. The method of claim 5, wherein coupling the collected environment data with the angle data output by the pitch motor encoder to obtain 3D point cloud data comprises:
collecting a distance l measured by a laser radar, a horizontal included angle alpha between light and a central line of the laser radar measured by the laser radar and a pitching angle beta of reciprocating swing of the laser radar;
the coordinate u of the 3D point cloud data of the object measured by the lidar is (x, y, z), which is calculated as follows:
7. the method of claim 6, wherein coupling the collected environment data with angle data output by a pitch motor encoder to obtain 3D point cloud data further comprises:
correcting data acquired in the advancing process of the vehicle body;
acquiring acceleration data (x, y, z) of 3D point cloud data in a vehicle body movement process according to a system inertia measurement IMU module, wherein displacement data delta u (delta x, delta y, delta z) in a sampling interval delta t;
the corrected 3D point cloud data form is as follows: u + Δ u ═ x + Δ x, y + Δ y, z + Δ z.
8. The method as claimed in claim 5, wherein the pitching motor drives the laser radar to complete a reciprocating motion to complete the acquisition of one frame of data, the image features of the 3D point cloud data are extracted by using an ORB algorithm, the image features of two consecutive frames of data are matched, the displacement relationship between the same feature elements of the two frames of data is calculated, the trolley displacement data during the interval of the two frames of data is obtained, and the actual traveling track of the trolley body is obtained, comprising:
extracting image features from adjacent data frames, and acquiring a key point FAST and a descriptor BRIEF of the 3D point cloud data by using an ORB algorithm;
adopting an image pyramid method to perform different levels of downsampling on image data so as to realize the scale transformation of key points, and adopting a gray scale centroid method to obtain the direction information of the image feature key points;
after the image characteristics of each frame are extracted, matching calculation is carried out on the characteristics between the frames, and the same characteristic points are calculated by adopting a fast approximate nearest neighbor FLANN algorithm;
after image feature matching calculation is completed, the moving distance and direction change of the trolley between two frames of images can be calculated, the iterative closest point ICP method is adopted for calculation according to the position of the departure point of the trolley body, the current position and direction of the trolley are solved, and the actual advancing track of the trolley body is obtained.
9. The method of claim 5, wherein the actual vehicle body traveling track is compared with a preset traveling path to obtain a path error, and the incremental PID algorithm is used to perform steering and forward direction parameter correction according to the preset traveling path to control the vehicle body to travel according to the preset path, specifically: the incremental PID control algorithm is as follows:
Δu(k)＝u(k)-u(k-1)
＝Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
kp, Ki and Kd are PID control parameters, e (k) is a k-th error value, and u (k) is a control quantity.
10. A computer readable storage medium, storing one or more programs which are executable by one or more processors to implement a dual-line lidar self-navigation positioning method according to any of claims 5 to 9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010834379.1A CN111983632A (en) | 2020-08-19 | 2020-08-19 | Self-navigation positioning method and system based on double-line laser radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010834379.1A CN111983632A (en) | 2020-08-19 | 2020-08-19 | Self-navigation positioning method and system based on double-line laser radar |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111983632A true CN111983632A (en) | 2020-11-24 |
Family
ID=73434708
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010834379.1A Withdrawn CN111983632A (en) | 2020-08-19 | 2020-08-19 | Self-navigation positioning method and system based on double-line laser radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111983632A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112947425A (en) * | 2021-02-01 | 2021-06-11 | 湖北迈睿达供应链股份有限公司 | Indoor outdoor AGV robot of multi-line radar |
| CN113610741A (en) * | 2021-08-18 | 2021-11-05 | 梅卡曼德（北京）机器人科技有限公司 | Point cloud processing method and device based on laser line scanning |
-
2020
- 2020-08-19 CN CN202010834379.1A patent/CN111983632A/en not_active Withdrawn
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112947425A (en) * | 2021-02-01 | 2021-06-11 | 湖北迈睿达供应链股份有限公司 | Indoor outdoor AGV robot of multi-line radar |
| CN113610741A (en) * | 2021-08-18 | 2021-11-05 | 梅卡曼德（北京）机器人科技有限公司 | Point cloud processing method and device based on laser line scanning |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109521756B (en) | Obstacle motion information generation method and apparatus for unmanned vehicle | |
| CN110009718B (en) | Three-dimensional high-precision map generation method and device | |
| CN110658531B (en) | Dynamic target tracking method for port automatic driving vehicle | |
| CN107239076B (en) | AGV laser SLAM method based on virtual scanning and distance measurement matching | |
| CN107246876B (en) | Method and system for autonomous positioning and map construction of unmanned automobile | |
| CN105892471B (en) | Automatic driving method and apparatus | |
| CN112650220B (en) | Automatic vehicle driving method, vehicle-mounted controller and system | |
| CN111982137B (en) | Method, device, equipment and storage medium for generating route planning model | |
| CN103176409B (en) | Method for fast and accurately realizing concrete pump truck cantilever crane movement locus | |
| CN114862901A (en) | Road-end multi-source sensor fusion target sensing method and system for surface mine | |
| CN111708048B (en) | Method, device and system for motion compensation of point cloud | |
| Zhang et al. | A real-time curb detection and tracking method for UGVs by using a 3D-LIDAR sensor | |
| EP3992732A1 (en) | Method and apparatus for predicting motion trajectory | |
| CN111983632A (en) | Self-navigation positioning method and system based on double-line laser radar | |
| CN110345936B (en) | Track data processing method and processing system of motion device | |
| CN110705385B (en) | Method, device, equipment and medium for detecting angle of obstacle | |
| CN109073390B (en) | Positioning method and device, electronic equipment and readable storage medium | |
| Wang et al. | End-to-end interactive prediction and planning with optical flow distillation for autonomous driving | |
| CN111007534A (en) | Obstacle detection method and system using sixteen-line laser radar | |
| CN103076023A (en) | Method and device for calculating step | |
| CN116740146A (en) | Unmanned excavator dynamic target detection tracking method, device and equipment | |
| CN115540850A (en) | Unmanned vehicle mapping method combining laser radar and acceleration sensor | |
| TWI680898B (en) | Light reaching detection device and method for close obstacles | |
| CN110780325A (en) | Method and device for positioning moving object and electronic equipment | |
| CN106408593A (en) | Video-based vehicle tracking method and device |
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 | ||
| WW01 | Invention patent application withdrawn after publication | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20201124 |