CN112904358A - Laser positioning method based on geometric information - Google Patents
Laser positioning method based on geometric information Download PDFInfo
- Publication number
- CN112904358A CN112904358A CN202110082545.1A CN202110082545A CN112904358A CN 112904358 A CN112904358 A CN 112904358A CN 202110082545 A CN202110082545 A CN 202110082545A CN 112904358 A CN112904358 A CN 112904358A
- Authority
- CN
- China
- Prior art keywords
- laser
- robot
- coordinate system
- reflector
- observation
- 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.)
- Granted
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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/23—Clustering techniques
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Biology (AREA)
- Evolutionary Computation (AREA)
- Bioinformatics & Computational Biology (AREA)
- General Engineering & Computer Science (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The invention discloses a laser positioning method based on geometric information. The method is used for realizing the positioning of the robot in the working space, and comprises the following steps: at least three laser reflecting plates are arranged in a set area in a working space of the robot; constructing a two-dimensional scene map corresponding to a robot working space comprising a laser reflecting plate; determining the position of the robot under a two-dimensional scene map by using a positioning algorithm according to observation information of a laser radar and geometric information of a laser reflector which are arranged on the robot; and predicting and estimating the pose of the robot based on the position of the robot under the two-dimensional scene map. According to the laser positioning method based on the geometric information, the laser reflecting plate is arranged in the set area needing accurate positioning to serve as the reflecting marker, so that the accurate positioning of the robot in the set area can be realized, the control accuracy of the robot is improved, the positioning reliability is high, and the algorithm complexity and the calculated amount are low.
Description
Technical Field
The invention relates to the technical field of robot positioning, in particular to a laser positioning method based on geometric information.
Background
With the continuous development of the robot technology, the mobile robot has wide application prospects in the aspects of natural disasters, nuclear leakage rescue, polar and foreign exploration, military reconnaissance and operation, industrial manufacturing, logistics automation, civil vehicle intellectualization and the like, and can replace human beings to carry out various dangerous, heavy and boring works. Autonomous navigation capability is a prerequisite for a mobile robot to perform a work task. The existing mobile robot navigation mode mainly comprises a laser navigation mode based on an external natural environment, the laser navigation mode based on the external natural environment is also called natural navigation, the navigation mode takes environment contour information detected by a laser radar as reference for navigation, and the accuracy and the reliability of the natural navigation are relatively low because only the environment contour information is taken as reference, particularly under the condition that the environment information is single, such as a long corridor, and the algorithm complexity is relatively high.
Disclosure of Invention
In order to solve part or all of the technical problems in the prior art, the invention provides a laser positioning method based on geometric information.
To this end, the invention discloses a laser positioning method based on geometric information, which is used for realizing the positioning of a robot in a working space and comprises the following steps:
s1, arranging at least three laser reflection plates in a set area in the working space of the robot;
s2, constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflector;
s3, determining the position of the robot under the two-dimensional scene map by using a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflector installed on the robot;
and S4, predicting and estimating the pose of the robot based on the position of the robot in the two-dimensional scene map.
In some optional embodiments, the constructing a two-dimensional scene map corresponding to the robot workspace comprising the laser reflector comprises the following steps:
s21, setting a global coordinate system and a local coordinate system;
s22, controlling the robot to move, scanning the working space environment by using a laser radar, and acquiring a group of point data under a robot polar coordinate system;
s23, carrying out data clustering and feature extraction on the point data, and determining the position information of the laser reflector;
and S24, completing the construction of the two-dimensional scene map.
In some alternative embodiments, the global coordinate system and the local coordinate are set as follows:
selecting a set position in a working space as a coordinate origin o, selecting a set direction as a positive x-axis direction, selecting a direction perpendicular to the set direction as a positive y-axis direction, and establishing a global coordinate system xoy;
and selecting the center of the robot as a coordinate origin O, selecting the advancing direction of the robot as the positive direction of an X axis, selecting the direction vertical to the advancing direction of the robot as the positive direction of a Y axis, and establishing a local coordinate system XOY.
In some optional embodiments, the point data is subjected to data clustering by using the following formula 1, so that the point data is divided into a plurality of point clusters, and one point cluster corresponds to one object;
wherein r isk,k+1＝|rk-rk+1I represents the geometric distance of two points, rkAnd rk+1Respectively representing the distance, r, between two points and the laser radarmin＝min{rk,rk+1}，rminRepresenting the minimum of the geometric distances of all two points in the point data, C0And β represents a system parameter and φ represents a lidar angular resolution.
In some alternative embodiments, feature extraction is performed to determine the position information of the laser reflector in the following manner:
setting n objects represented by the following formula 2 obtained in the data clustering process;
when the pole diameter of the object and the actual radius of the laser reflecting plate are within a set error range, determining the corresponding object as the laser reflecting plate;
wherein E issegRepresenting a set of objects, segkDenotes the kth object, (x)k,yk) Denotes the position of the center of the kth object in the global coordinate system, DkDenotes the pole diameter of the kth object, θkIndicating the direction of the main axis of the kth object.
In some optional embodiments, the determining the position of the robot under the two-dimensional scene map by using a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflector mounted on the robot includes the following steps:
s31, determining the observation information of the laser reflector under the local coordinate system according to the observation information of the laser radar;
s32, matching the observation information of the laser reflector in the local coordinate system with the geometric information of the laser reflector in the global coordinate system by using an angle matching mode and/or a distance matching mode;
and S33, determining the position of the robot under the two-dimensional scene map by adopting a least square iterative algorithm and/or a multilateral positioning algorithm based on residual errors according to the observation information of the matched laser reflector under the local coordinate system and the geometric information of the laser reflector under the global coordinate system.
In some optional embodiments, the observation information of the laser reflector in the local coordinate system and the geometric information of the laser reflector in the global coordinate system are matched by using an angle matching method, which is performed by the following steps:
setting working spaceM laser reflection plates are arranged in the environment, and the coordinate of the m laser reflection plates under the global coordinate is li(xi,yi) I is more than or equal to 1 and less than or equal to m, q laser reflecting plates are observed by the laser radar, and the observation azimuth angle of the q laser reflecting plates is alphaj，1≤j≤q；
And respectively calculating theoretical azimuth angles of the m laser reflectors and the advancing direction of the robot according to the pose of the robot at the previous moment, comparing the observation azimuth angles of the q laser reflectors with the theoretical azimuth angles of the m laser reflectors, and when the difference value of the observation azimuth angle and the theoretical azimuth angle is within a set angle error range, indicating that the laser reflector corresponding to the current observation azimuth angle is matched with the laser reflector corresponding to the current theoretical azimuth angle.
In some optional embodiments, the observation information of the laser reflector in the local coordinate system and the geometric information of the laser reflector in the global coordinate system are matched by using a distance matching method, which is performed by the following steps:
setting m laser reflecting plates in a working space environment, wherein the coordinate of the m laser reflecting plates under the global coordinate is li(xi,yi) I is more than or equal to 1 and less than or equal to m, q laser reflecting plates are observed by the laser radar, and the distance between the q laser reflecting plates and the laser radar is pjThe observation azimuth angles of the q laser reflection plates are alphaj，1≤j≤q；
Calculating the absolute distance between any two observed laser reflecting plates according to the distance between the q laser reflecting plates and the laser radar and the observation azimuth angles of the q laser reflecting plates, calculating the real distance between any two laser reflecting plates in the m laser reflecting plates according to the coordinates of the m laser reflecting plates under the global coordinate, comparing the absolute distance with the real distance, and when the difference value of the absolute distance and the real distance is within the set distance error range, indicating that the two laser reflecting plates corresponding to the current absolute distance are matched with the two laser reflecting plates corresponding to the current real distance.
In some optional embodiments, determining the position of the robot under the two-dimensional scene map by using a least square iterative algorithm based on a residual error comprises:
selecting coordinates and azimuth angles of a group of laser reflectors, and establishing a group of set relational expressions of the selected coordinates and azimuth angles of the group of laser reflectors and a laser radar observation position point;
and (4) omitting high-order terms by using Taylor series expansion, forming a residual equation set of the observed value and the theoretical value, and solving the position of the laser radar by using a least square method.
In some optional embodiments, an extended kalman filter algorithm is adopted to perform prediction estimation on the pose of the robot.
The technical scheme of the invention has the following main advantages:
according to the laser positioning method based on the geometric information, the laser reflecting plate is arranged in the set area needing accurate positioning to serve as the reflecting marker, so that the accurate positioning of the robot in the set area can be realized, the control accuracy of the robot is improved, the positioning reliability is high, and the algorithm complexity and the calculated amount are low.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a flow chart of a laser positioning method based on geometric information according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a coordinate system according to an embodiment of the present invention;
FIG. 3 is another flowchart of a laser positioning method based on geometric information according to an embodiment of the present invention;
FIG. 4 is a schematic view of an observation position of a laser reflector according to an embodiment of the invention;
FIG. 5 is a schematic diagram of a triangulation algorithm according to an embodiment of the invention;
FIG. 6 is a schematic diagram of a trilateration algorithm in accordance with an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below with reference to the specific embodiments of the present invention and the accompanying drawings. It is to be understood that the described embodiments are merely a few embodiments of the invention, and not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present invention without making any creative effort, shall fall within the protection scope of the present invention.
The technical scheme provided by the embodiment of the invention is described in detail below with reference to the accompanying drawings.
Referring to fig. 1, an embodiment of the present invention provides a laser positioning method based on geometric information, the method is used for realizing the positioning of a robot in a working space, and includes the following steps:
s1, arranging at least three laser reflection plates in a set area in the working space of the robot;
s2, constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflector;
s3, determining the position of the robot under the two-dimensional scene map by using a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflector installed on the robot;
and S4, predicting and estimating the pose of the robot based on the position of the robot in the two-dimensional scene map.
The following describes each step of the laser positioning method based on geometric information according to an embodiment of the present invention.
S1, arranging at least three laser reflection plates in a set area in the robot work space.
Specifically, at least three laser reflection plates are arranged in a set area with poor environment identifiability or needing accurate positioning, such as the vicinity of a target point, according to the actual positioning requirement of the robot.
Wherein, in order to guarantee that the accurate positioning of robot can be realized, a plurality of laser reflecting plates of arranging can not constitute regular polygon, and the laser radar who installs on the robot can observe three laser reflecting plates at least at the robot removes the in-process, and when laser reflecting plate is greater than three, can improve the positioning accuracy of robot, but the calculated amount also can corresponding increase.
When the robot moves in other areas where the laser reflecting plate is not arranged in the working space, the robot can be positioned by adopting a laser navigation mode based on the external natural environment.
And S2, constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflector.
Specifically, in an embodiment of the present invention, constructing a two-dimensional scene map corresponding to a robot workspace including a laser reflector includes the following steps S21-S24;
s21, setting a global coordinate system and a local coordinate system;
referring to fig. 2, in an embodiment of the present invention, a set position in a working space is selected as a coordinate origin o, a set direction is selected as a positive x-axis direction, a direction perpendicular to the set direction is selected as a positive y-axis direction, and a global coordinate system xoy is established; and selecting the center of the robot as a coordinate origin O, selecting the advancing direction of the robot as the positive direction of an X axis, selecting the direction vertical to the advancing direction of the robot as the positive direction of a Y axis, and establishing a local coordinate system XOY.
The global coordinate system is a two-dimensional coordinate system established by setting a map coordinate origin in the whole working space of the robot, the positions of laser reflecting plates pre-arranged in the working space environment of the robot and the positioning navigation calculation of the robot are based on the set global coordinate system, the positioning of the robot belongs to global positioning, namely when the robot walks in the working space provided with the global coordinate system, the pose of the robot can be quickly and accurately determined.
The local coordinate system can also be called as a moving coordinate system, the local coordinate system is a coordinate system established based on a laser radar installed on the robot, the local coordinate system can move along with the movement of the robot, the position and the angle of the laser reflecting plate relative to the robot, which are measured in the navigation and positioning process of the robot, can be obtained through the local coordinate system, the static and dynamic matching of the laser reflecting plate can be realized by combining the measured distance from the laser reflecting plate to the robot, and the setting of the local coordinate system is the basis for obtaining the current global pose of the robot.
S22, controlling the robot to move, scanning the working space environment by using a laser radar, and acquiring a group of point data under a robot polar coordinate system;
the polar coordinate system of the robot represents the polar coordinate system taking the center of the robot as a pole and the advancing direction of the robot as the positive direction of a polar axis, the point data under each polar coordinate of the robot comprises a polar diameter and a polar angle, the polar diameter is the distance between the corresponding measuring point and the laser radar, and the polar angle is the included angle between the connecting line of the corresponding measuring point and the laser radar and the advancing direction of the robot.
S23, carrying out data clustering and feature extraction on the point data, and determining the position information of the laser reflector;
because the data volume obtained by the laser radar is large, and part of the data is influenced by environmental noise or is not an interested point, the method is unfavorable for further utilizing the data to establish a two-dimensional scene map, and therefore the data obtained by the laser radar needs to be preprocessed.
In an embodiment of the invention, data clustering is adopted to preprocess data acquired by a laser radar.
Data clustering, namely Data clustering, aims to divide Data of a frame of original measuring points into a plurality of point clusters, and each point cluster corresponds to an object. When data clustering is carried out, the geometric distance between two points is firstly obtained according to the distance between the laser radar and the two points, whether the two points belong to the same group or not is judged according to whether the geometric distance between the two points is within a set threshold value or not, namely whether the two points belong to one point cluster or not, if the geometric distance between the two points is within the set threshold value, the two points belong to the same group, and if the geometric distance between the two points is not within the set threshold value, the two points do not belong to the same group.
Specifically, data clustering is performed on point data by using the following formula 1;
in the formula, rk,k+1＝|rk-rk+1I represents the geometric distance of two points, rkAnd rk+1Respectively representing the distance, r, between two points and the laser radarmin＝min{rk,rk+1}，rminRepresenting the minimum of the geometric distances of all two points in the point data, C0Beta represents a system parameter, phi represents the angular resolution of the laser radar, the system parameter beta is used for reducing the dependency of the segmented part on the distance from the laser radar to the object and can be obtained through actual data calculation, and the system parameter C0The longitudinal error for adjusting the lidar can be given by empirical values.
The distance between the laser radar and the point can be obtained through data acquired by scanning the laser radar.
Further, after data clustering of the point data is completed, feature extraction is performed in the following manner to determine the position information of the laser reflector:
it is assumed that n objects expressed as the following formula 2 are obtained in the above data clustering process;
when the pole diameter of the object and the actual radius of the laser reflecting plate are within a set error range, determining the corresponding object as the laser reflecting plate;
in the formula, EsegRepresenting a set of objects, segkDenotes the kth object, (x)k,yk) Denotes the position of the center of the kth object in the global coordinate system, DkDenotes the pole diameter of the kth object, θkIndicating the direction of the main axis of the kth object.
In an embodiment of the invention, the polar diameter characteristics of the object are utilized to perform characteristic extraction, and the object corresponding to the laser reflector is determined, so that the position information of the laser reflector is determined.
S24, completing construction of a two-dimensional scene map;
and after the identification and positioning of all the laser reflectors and other objects are completed, the construction of a two-dimensional scene map is completed, and the subsequent robot positioning and navigation are based on the two-dimensional scene map constructed in the step.
And S3, determining the position of the robot under the two-dimensional scene map by using a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflector installed on the robot.
Referring to fig. 3, specifically, determining the position of the robot under the two-dimensional scene map by using a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflector mounted on the robot, includes the following steps:
s31, determining the observation information of the laser reflector under the local coordinate system according to the observation information of the laser radar;
after the construction of the two-dimensional scene map corresponding to the working space of the robot is completed, when the robot moves in the working space, the laser radar can return scanning data information in real time, aiming at the scanning data information acquired by the laser radar, data processing is carried out on the scanning data information by adopting the data clustering and the feature extraction in the step 2, and observation information of the laser reflector under a local coordinate system can be obtained, wherein the observation information comprises the distance from the laser reflector to the laser radar and the azimuth angle of the laser reflector, and the azimuth angle of the laser reflector indicates the included angle formed by the connecting line of the laser reflector and the laser radar and the advancing direction of the robot.
S32, matching the observation information of the laser reflector in the local coordinate system with the geometric information of the laser reflector in the global coordinate system by using an angle matching mode and/or a distance matching mode;
in order to realize the positioning calculation of the robot, the observation information and the geometric information of the laser reflector need to be matched, that is, the laser reflector (possibly mixed with an interference signal which is not the laser reflector) observed by the laser radar corresponds to the actual reference laser reflector in the working space environment one by one, effective laser reflector information is extracted, and then the positioning calculation of the robot can be carried out. In practice, some laser reflectors may not be visible or confused with other laser reflectors, resulting in a result that may yield a wrong positioning result. Therefore, in an embodiment of the invention, the observation information of the laser reflector under the local coordinate system obtained by scanning is matched with the actual geometric information of the laser reflector under the global coordinate system one by one in an angle matching mode and/or a distance matching mode.
The principle of the angle matching mode is that coordinate values of the laser reflector under the global coordinate are converted into direction angles which are used as 'theoretical values' in an input program, an angle value is obtained after the laser radar scans a laser reflector signal (possibly an interference signal), the angle value can be called as 'observation value', a smaller error range is set, the observation value is compared with the corresponding theoretical value, if the difference value of the two is within the set angle error range, the laser reflector is observed, the observation value is stored, and if the difference value is not within the set angle error range, the interference signal is observed and is removed.
Specifically, the observation information of the laser reflector in the local coordinate system and the geometric information of the laser reflector in the global coordinate system are matched in an angle matching mode, and the following modes are adopted:
setting m laser reflecting plates in a working space environment, wherein the coordinate of the m laser reflecting plates under the global coordinate is li(xi,yi) I is more than or equal to 1 and less than or equal to m, q laser reflecting plates are observed by the laser radar, and the observation azimuth angle of the q laser reflecting plates is alphaj，1≤j≤q；
And respectively calculating theoretical azimuth angles of the m laser reflectors and the advancing direction of the robot according to the pose of the robot at the previous moment, comparing the observation azimuth angles of the q laser reflectors with the theoretical azimuth angles of the m laser reflectors, and when the difference value of the observation azimuth angle and the theoretical azimuth angle is within a set angle error range, indicating that the laser reflector corresponding to the current observation azimuth angle is matched with the laser reflector corresponding to the current theoretical azimuth angle.
The objective of matching the laser reflector is to determine the observation azimuth angle alphajWhich reference in the workspace environment is actuallyThe included angle between the reflecting plate and the advancing direction of the robot is small, the robot displacement is small in the process due to the fact that the observation period of the laser radar is short, the theoretical azimuth angle of the reference laser reflecting plate and the advancing direction of the robot can be calculated by means of the pose of the robot at the previous moment, if the theoretical azimuth angle of the ith laser reflecting plate obtained through calculation and the observation azimuth angle of the jth laser reflecting plate obtained through observation are within the set angle error range, the jth laser reflecting plate and the ith reference laser reflecting plate observed are determined to be matched with each other, and therefore a group of effective laser reflecting plate coordinates l is obtainedi(xi,yi) And an azimuth angle alphaj。
Further, the distance matching mode is performed on the premise that the static pose calculation is met, and the robot is required to be static at the moment. The principle Of the distance matching mode is that laser scanning is carried out by a laser radar at a certain frequency, the Time interval from the Time when the laser radar emits laser to the Time when the laser radar receives the laser and the laser is reflected back from a laser reflecting plate is measured, the distance between the laser radar and each observed laser reflecting plate is calculated by utilizing the Time Of Flight (TOF), and an observed laser reflecting plate list is generated and matched with an actual reference laser reflecting plate under a global coordinate system.
Specifically, the observation information of the laser reflector in the local coordinate system and the geometric information of the laser reflector in the global coordinate system are matched in a distance matching mode, and the following modes are adopted:
setting m laser reflecting plates in a working space environment, wherein the coordinate of the m laser reflecting plates under the global coordinate is li(xi,yi) I is more than or equal to 1 and less than or equal to m, q laser reflecting plates are observed by the laser radar, and the distance between the q laser reflecting plates and the laser radar is pjThe observation azimuth angles of the q laser reflection plates are alphaj，1≤j≤q；
Calculating the absolute distance between any two observed laser reflecting plates according to the distance between the q laser reflecting plates and the laser radar and the observation azimuth angles of the q laser reflecting plates, calculating the real distance between any two laser reflecting plates in the m laser reflecting plates according to the coordinates of the m laser reflecting plates under the global coordinate, comparing the absolute distance with the real distance, and when the difference value of the absolute distance and the real distance is within the set distance error range, indicating that the two laser reflecting plates corresponding to the current absolute distance are matched with the two laser reflecting plates corresponding to the current real distance.
Referring to FIG. 4, the position of the laser reflector observed by the laser radar at a certain time is shown in FIG. 4, XOY represents a local coordinate system, MjShowing the observed laser reflector. The aim of matching the laser reflecting plates is to determine which reference laser reflecting plate is actually observed by the laser radar in the working space environment, and since the robot is still and the corresponding laser radar is also still, the absolute distance M between any two observed laser reflecting plates can be calculated by applying the cosine law according to the observed distance between the laser reflecting plates and the laser radar and the observed azimuth angle of the laser reflecting plates1,2,M1,3,…,M1,q,M2,1,M2,3,…,M2,q,…,Mq,1,Mq,2,…,Mq,q-1. Meanwhile, because the coordinates of all the laser reflectors in the working space environment under the global coordinate system are known, the real distance between any two laser reflectors can be calculated according to the coordinates of the laser reflectors under the global coordinate system, the absolute distance is compared with the real distance, when the difference value of the absolute distance and the real distance is within the set distance error range, the two laser reflectors corresponding to the current absolute distance are matched with the two laser reflectors corresponding to the current real distance, the observed laser reflectors are matched with the laser reflectors under the global coordinate system, and the effective laser reflector coordinate l is obtainedi(xi,yi) A distance pjAnd an azimuth angle alphaj。
And S33, determining the position of the robot under the two-dimensional scene map by using the observation information of the matched laser reflector under the local coordinate system and the geometric information of the laser reflector under the global coordinate system and by using a least square iterative algorithm and/or a multilateral positioning algorithm based on residual errors.
After the matching of the laser reflector is completed, the position of the robot under the two-dimensional scene map can be calculated and determined by utilizing observation information of the matched laser reflector under a local coordinate system and geometric information of the laser reflector under a global coordinate system.
In one embodiment of the invention, on the basis of matching by using an angle matching mode, the position of the robot under a two-dimensional scene map is determined by using a least square iterative algorithm based on residual errors.
Specifically, determining the position of the robot under the two-dimensional scene map by adopting a least square iterative algorithm based on a residual error comprises the following steps:
selecting coordinates and azimuth angles of a group of laser reflectors, and establishing a group of set relational expressions of the selected coordinates and azimuth angles of the group of laser reflectors and a laser radar observation position point;
and (4) omitting high-order terms by using Taylor series expansion, forming a residual equation set of the observed value and the theoretical value, and solving the position of the laser radar by using a least square method.
Since the laser radar is mounted on the robot, the position of the laser radar is determined, i.e., the position of the robot is determined.
Further, in an embodiment of the present invention, on the basis of performing matching by using a distance matching method, a position of the robot under the two-dimensional scene map is determined by using a multilateral positioning algorithm.
In one embodiment of the invention, the multilateral positioning algorithm comprises a triangulation algorithm and a trilateral positioning algorithm, and when the robot is positioned by adopting the triangulation algorithm or the trilateral positioning algorithm, at least three laser reflecting plates must be detected by the robot in the moving process.
Specifically, the triangulation algorithm realizes positioning by measuring an included angle between a laser reflecting plate and a longitudinal axis of the robot. Referring to fig. 5, an included angle ARB between the robot and the laser reflecting plate a and the laser reflecting plate B and an included angle BRC between the robot and the laser reflecting plate B and the laser reflecting plate C are measured, and two circumscribed circles and a circumscribed circle are made by using a circumscribed circle method. According to the nature of the circumscribed circle: if three points which are not on the same straight line can be used for making a circle and only one circle is needed, the robot is positioned at the intersection of the two circles, the two circumscribed circles have two intersection points, one intersection point is the laser reflecting plate B, and the other intersection point R is the position of the robot.
Specifically, the trilateration algorithm is used for positioning the robot by measuring the distance between the laser reflecting plate and the robot, the global coordinates of which are known. Referring to fig. 6, the distances from the robot to A, B and C laser reflection plates are measured as Ra、RbAnd Rc. According to the definition of the circle: the track of the moving point with the distance to the fixed point equal to the fixed length respectively takes the position point A, the position point B and the position point C as the circle center and takes R as the circle centera、RbAnd RcIf the radius is a circle, the robot is positioned at the intersection of the three circles. Compared with the triangulation algorithm, the trilateration algorithm is less in required calculated amount and observed amount and easier to realize.
If the distance between the current robot position and a plurality of laser reflecting plates is equal, the positioning accuracy obtained according to the laser reflecting plate optimization selection criterion is very high, but the fact is that the laser reflecting plates are not always uniformly distributed around the robot, and the robot is not always in the center of the uniformly distributed laser reflecting plates. Therefore, in the moving process of the robot, how to reasonably select the laser reflector according to the surrounding environment to enable the positioning accuracy to be the highest becomes necessary, and as the accuracy standard is obtained, all three laser reflector combinations of all effective identification points can be found out for the problem of selecting the laser reflector for three-point positioning, the accuracy of the combinations is estimated according to the given accuracy measurement standard, and then a group of optimal combinations is selected by comparison.
And S4, predicting and estimating the pose of the robot based on the position of the robot in the two-dimensional scene map.
Because the laser positioning process of the robot is a discrete and discontinuous process in which the laser radar performs calculation once per circle of rotation, after the accurate position of each positioning calculation point of the robot is determined by using a positioning algorithm, errors possibly generated when the robot moves between the two positioning calculation points need to be corrected in advance, the robot is ensured to reach a target position along a planned path, and the pose of the robot and the mean value and the variance of an environment map need to be estimated. Therefore, in an embodiment of the invention, an Extended Kalman Filter algorithm (EKF algorithm) is adopted to predict and estimate the pose of the robot.
The Kalman filtering is an estimation based on the state and parameters of a probability model, the current system state is estimated according to the system state at the previous moment, and then the actual observation value at the current moment is used as feedback to correct the estimated state.
The system discrete state space model is assumed to be:
wherein x (k) represents a state variable of the system at the time k, u (k) represents a control variable of the system at the time k, z (k) represents a measurement variable of a sensor at the time k, w (k) represents process noise of the system, v (k) represents observation noise of the sensor at the time k, A and H represent a state transition matrix and a measurement matrix of the system respectively, and B represents an input control matrix of the system.
The Kalman filtering estimates the next system state (prior estimation) from the current system state and input variables in time through a state updating equation, and the measurement updating adds a new measurement signal into the prior estimation state obtained in the state updating equation to finally obtain the posterior estimation of the system state.
For a nonlinear system, the process model and observation model are as follows:
in the formula, x (k), z (k), and u (k) respectively represent the state of the system, the observed quantity, and the control input of the system at the time, w (k) and v (k) both represent white gaussian noise, f (x (k), u (k)) represents a motion equation, and h (x (k)) represents an observation equation.
Referring to fig. 3, the pose of the robot and the mean and variance of the environment map can be estimated using the EKF-SLAM algorithm. In the EKF-SLAM algorithm, the system noise is defaulted to obey a Gaussian distribution, so the state output of the system is described by the mean and covariance of the system state. In the EKF-SLAM algorithm, the system state comprises the pose of the robot and the positions of all detected laser reflecting plates, and the pose of the robot at the next moment is predicted and estimated through an EKF filter according to a motion state equation of the robot and the input of the current pose and control variables of the robot. When the laser radar sensor detects an external laser reflection plate, updating the system state by adopting an EKF filter according to a sensor measurement equation and combining the current pose of the robot, the environmental laser reflection plate and the measurement information of the sensor.
The specific implementation process of EKF-SLAM can be described as follows: inputting a control signal of the robot into a system state equation, carrying out recursive estimation on the state of the system by using the EKF, carrying out estimation and correction by taking a measurement error as a basis, and carrying out updating and replacement on an augmented system state vector and an augmented system covariance matrix, wherein the state prediction and the observation updating are alternately carried out, and the real value of the system state is continuously approached so as to complete the estimation process of the EKF.
According to the laser positioning method based on the geometric information, provided by the embodiment of the invention, the laser reflecting plate is arranged in the set area needing to be accurately positioned and is used as the reflecting marker, so that the accurate positioning of the robot in the set area can be realized, the control precision of the robot is improved, the positioning reliability is high, and the algorithm complexity and the calculated amount are low.
It is noted that, in this document, relational terms such as "first" and "second," and the like, may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. In addition, "front", "rear", "left", "right", "upper" and "lower" in this document are referred to the placement states shown in the drawings.
Finally, it should be noted that: the above examples are only for illustrating the technical solutions of the present invention, and not for limiting the same; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (10)
1. A laser positioning method based on geometric information is characterized in that the method is used for realizing the positioning of a robot in a working space, and comprises the following steps:
s1, arranging at least three laser reflection plates in a set area in the working space of the robot;
s2, constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflector;
s3, determining the position of the robot under the two-dimensional scene map by using a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflector installed on the robot;
and S4, predicting and estimating the pose of the robot based on the position of the robot in the two-dimensional scene map.
2. The laser positioning method based on geometric information according to claim 1, wherein the constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflector comprises the following steps:
s21, setting a global coordinate system and a local coordinate system;
s22, controlling the robot to move, scanning the working space environment by using a laser radar, and acquiring a group of point data under a robot polar coordinate system;
s23, carrying out data clustering and feature extraction on the point data, and determining the position information of the laser reflector;
and S24, completing the construction of the two-dimensional scene map.
3. The method of claim 2, wherein the global coordinate system and the local coordinate system are set as follows:
selecting a set position in a working space as a coordinate origin o, selecting a set direction as a positive x-axis direction, selecting a direction perpendicular to the set direction as a positive y-axis direction, and establishing a global coordinate system xoy;
and selecting the center of the robot as a coordinate origin O, selecting the advancing direction of the robot as the positive direction of an X axis, selecting the direction vertical to the advancing direction of the robot as the positive direction of a Y axis, and establishing a local coordinate system XOY.
4. The laser positioning method based on geometric information as claimed in claim 3, wherein the point data is subjected to data clustering using the following formula 1 to divide the point data into a plurality of point clusters, one point cluster corresponding to one object;
wherein r isk,k+1＝|rk-rk+1I represents the geometric distance of two points, rkAnd rk+1Respectively representing the distance, r, between two points and the laser radarmin＝min{rk,rk+1}，rminRepresenting the minimum of the geometric distances of all two points in the point data, C0And beta represents a system parameter and phi represents excitationThe optical radar angular resolution.
5. The method of claim 4, wherein the feature extraction is performed to determine the position information of the laser reflector by:
setting n objects represented by the following formula 2 obtained in the data clustering process;
when the pole diameter of the object and the actual radius of the laser reflecting plate are within a set error range, determining the corresponding object as the laser reflecting plate;
wherein E issegRepresenting a set of objects, segkDenotes the kth object, (x)k,yk) Denotes the position of the center of the kth object in the global coordinate system, DkDenotes the pole diameter of the kth object, θkIndicating the direction of the main axis of the kth object.
6. The laser positioning method based on geometric information according to claim 5, wherein the determining the position of the robot under the two-dimensional scene map by using a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflector mounted on the robot comprises the following steps:
s31, determining the observation information of the laser reflector under the local coordinate system according to the observation information of the laser radar;
s32, matching the observation information of the laser reflector in the local coordinate system with the geometric information of the laser reflector in the global coordinate system by using an angle matching mode and/or a distance matching mode;
and S33, determining the position of the robot under the two-dimensional scene map by adopting a least square iterative algorithm and/or a multilateral positioning algorithm based on residual errors according to the observation information of the matched laser reflector under the local coordinate system and the geometric information of the laser reflector under the global coordinate system.
7. The laser positioning method based on geometric information as claimed in claim 6, wherein the angle matching is used to match the observation information of the laser reflector in the local coordinate system with the geometric information of the laser reflector in the global coordinate system, and the following method is used:
setting m laser reflecting plates in a working space environment, wherein the coordinate of the m laser reflecting plates under the global coordinate is li(xi,yi) I is more than or equal to 1 and less than or equal to m, q laser reflecting plates are observed by the laser radar, and the observation azimuth angle of the q laser reflecting plates is alphaj，1≤j≤q；
And respectively calculating theoretical azimuth angles of the m laser reflectors and the advancing direction of the robot according to the pose of the robot at the previous moment, comparing the observation azimuth angles of the q laser reflectors with the theoretical azimuth angles of the m laser reflectors, and when the difference value of the observation azimuth angle and the theoretical azimuth angle is within a set angle error range, indicating that the laser reflector corresponding to the current observation azimuth angle is matched with the laser reflector corresponding to the current theoretical azimuth angle.
8. The laser positioning method based on the geometric information as claimed in claim 6 or 7, wherein the distance matching is used to match the observation information of the laser reflector in the local coordinate system with the geometric information of the laser reflector in the global coordinate system, and the following method is used:
setting m laser reflecting plates in a working space environment, wherein the coordinate of the m laser reflecting plates under the global coordinate is li(xi,yi) I is more than or equal to 1 and less than or equal to m, q laser reflecting plates are observed by the laser radar, and the distance between the q laser reflecting plates and the laser radar is pjThe observation azimuth angles of the q laser reflection plates are alphaj，1≤j≤q；
Calculating the absolute distance between any two observed laser reflecting plates according to the distance between the q laser reflecting plates and the laser radar and the observation azimuth angles of the q laser reflecting plates, calculating the real distance between any two laser reflecting plates in the m laser reflecting plates according to the coordinates of the m laser reflecting plates under the global coordinate, comparing the absolute distance with the real distance, and when the difference value of the absolute distance and the real distance is within the set distance error range, indicating that the two laser reflecting plates corresponding to the current absolute distance are matched with the two laser reflecting plates corresponding to the current real distance.
9. The laser positioning method based on geometric information according to any one of claims 6 to 8, wherein determining the position of the robot under the two-dimensional scene map by using a least square iterative algorithm based on residual errors comprises:
selecting coordinates and azimuth angles of a group of laser reflectors, and establishing a group of set relational expressions of the selected coordinates and azimuth angles of the group of laser reflectors and a laser radar observation position point;
and (4) omitting high-order terms by using Taylor series expansion, forming a residual equation set of the observed value and the theoretical value, and solving the position of the laser radar by using a least square method.
10. The laser positioning method based on geometric information according to any one of claims 1 to 9, characterized in that an extended kalman filter algorithm is used to perform predictive estimation on the pose of the robot.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110082545.1A CN112904358B (en) | 2021-01-21 | 2021-01-21 | Laser positioning method based on geometric information |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110082545.1A CN112904358B (en) | 2021-01-21 | 2021-01-21 | Laser positioning method based on geometric information |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112904358A true CN112904358A (en) | 2021-06-04 |
| CN112904358B CN112904358B (en) | 2023-05-23 |
Family
ID=76118041
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110082545.1A Active CN112904358B (en) | 2021-01-21 | 2021-01-21 | Laser positioning method based on geometric information |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112904358B (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113432533A (en) * | 2021-06-18 | 2021-09-24 | 北京盈迪曼德科技有限公司 | Robot positioning method and device, robot and storage medium |
| CN113791377A (en) * | 2021-09-09 | 2021-12-14 | 中国科学院微小卫星创新研究院 | Positioning method based on angle measurement |
| CN114355383A (en) * | 2022-01-20 | 2022-04-15 | 合肥工业大学 | Positioning and navigation method combining laser SLAM and laser reflector |
| CN114637024A (en) * | 2022-04-01 | 2022-06-17 | 北京石头世纪科技股份有限公司 | Laser positioning system and method |
| CN115060265A (en) * | 2022-05-27 | 2022-09-16 | 华南农业大学 | Positioning device and positioning method for farmland navigation |
| CN118446031A (en) * | 2024-07-04 | 2024-08-06 | 水利部交通运输部国家能源局南京水利科学研究院 | Slope soil erosion monitoring method, system and device and computer storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102402225A (en) * | 2011-11-23 | 2012-04-04 | 中国科学院自动化研究所 | Method for realizing localization and map building of mobile robot at the same time |
| CN106969768A (en) * | 2017-04-22 | 2017-07-21 | 深圳力子机器人有限公司 | A kind of trackless navigation AGV's is accurately positioned and parking method |
| US20180216941A1 (en) * | 2015-07-31 | 2018-08-02 | Tianjin University | Indoor mobile robot position and posture measurement system based on photoelectric scanning and measurement method |
| CN110866927A (en) * | 2019-11-21 | 2020-03-06 | 哈尔滨工业大学 | Robot positioning and composition method based on EKF-SLAM algorithm combined with dotted line characteristics of foot |
| CN111678516A (en) * | 2020-05-08 | 2020-09-18 | 中山大学 | Bounded region rapid global positioning method based on laser radar |
| CN111781609A (en) * | 2020-04-10 | 2020-10-16 | 昆山同孚智能技术有限公司 | AGV laser navigation multilateral positioning method |
-
2021
- 2021-01-21 CN CN202110082545.1A patent/CN112904358B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102402225A (en) * | 2011-11-23 | 2012-04-04 | 中国科学院自动化研究所 | Method for realizing localization and map building of mobile robot at the same time |
| US20180216941A1 (en) * | 2015-07-31 | 2018-08-02 | Tianjin University | Indoor mobile robot position and posture measurement system based on photoelectric scanning and measurement method |
| CN106969768A (en) * | 2017-04-22 | 2017-07-21 | 深圳力子机器人有限公司 | A kind of trackless navigation AGV's is accurately positioned and parking method |
| CN110866927A (en) * | 2019-11-21 | 2020-03-06 | 哈尔滨工业大学 | Robot positioning and composition method based on EKF-SLAM algorithm combined with dotted line characteristics of foot |
| CN111781609A (en) * | 2020-04-10 | 2020-10-16 | 昆山同孚智能技术有限公司 | AGV laser navigation multilateral positioning method |
| CN111678516A (en) * | 2020-05-08 | 2020-09-18 | 中山大学 | Bounded region rapid global positioning method based on laser radar |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113432533A (en) * | 2021-06-18 | 2021-09-24 | 北京盈迪曼德科技有限公司 | Robot positioning method and device, robot and storage medium |
| CN113432533B (en) * | 2021-06-18 | 2023-08-15 | 北京盈迪曼德科技有限公司 | Robot positioning method and device, robot and storage medium |
| CN113791377A (en) * | 2021-09-09 | 2021-12-14 | 中国科学院微小卫星创新研究院 | Positioning method based on angle measurement |
| CN113791377B (en) * | 2021-09-09 | 2024-04-12 | 中国科学院微小卫星创新研究院 | Positioning method based on angle measurement |
| CN114355383A (en) * | 2022-01-20 | 2022-04-15 | 合肥工业大学 | Positioning and navigation method combining laser SLAM and laser reflector |
| CN114355383B (en) * | 2022-01-20 | 2024-04-12 | 合肥工业大学 | Positioning navigation method combining laser SLAM and laser reflecting plate |
| CN114637024A (en) * | 2022-04-01 | 2022-06-17 | 北京石头世纪科技股份有限公司 | Laser positioning system and method |
| CN115060265A (en) * | 2022-05-27 | 2022-09-16 | 华南农业大学 | Positioning device and positioning method for farmland navigation |
| CN115060265B (en) * | 2022-05-27 | 2024-05-28 | 华南农业大学 | Positioning device for farmland navigation and positioning method thereof |
| CN118446031A (en) * | 2024-07-04 | 2024-08-06 | 水利部交通运输部国家能源局南京水利科学研究院 | Slope soil erosion monitoring method, system and device and computer storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112904358B (en) | 2023-05-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112904358B (en) | Laser positioning method based on geometric information | |
| CN110645974B (en) | Mobile robot indoor map construction method fusing multiple sensors | |
| CN111429574B (en) | Mobile robot positioning method and system based on three-dimensional point cloud and vision fusion | |
| CN107239076B (en) | AGV laser SLAM method based on virtual scanning and distance measurement matching | |
| CN108253958B (en) | Robot real-time positioning method in sparse environment | |
| CN109597864B (en) | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering | |
| CN109459039B (en) | Laser positioning navigation system and method of medicine carrying robot | |
| US20230236280A1 (en) | Method and system for positioning indoor autonomous mobile robot | |
| EP3427008A1 (en) | Laser scanner with real-time, online ego-motion estimation | |
| CN110032965B (en) | Visual positioning method based on remote sensing image | |
| JP2019207678A (en) | Unsupervised learning of metric representations from slow features | |
| CN112444246B (en) | Laser fusion positioning method in high-precision digital twin scene | |
| CN112750161A (en) | Map updating method for mobile robot and mobile robot positioning method | |
| Cui et al. | Research on simultaneous localization and mapping for AUV by an improved method: Variance reduction FastSLAM with simulated annealing | |
| CN115728803A (en) | System and method for continuously positioning urban driving vehicle | |
| Obregón et al. | Precise positioning and heading for autonomous scouting robots in a harsh environment | |
| D’Adamo et al. | Registration of three‐dimensional scanning LiDAR sensors: An evaluation of model‐based and model‐free methods | |
| CN115754977A (en) | Repositioning method for intelligent bridge crane | |
| Watanabe et al. | Robust localization with architectural floor plans and depth camera | |
| Pan et al. | LiDAR-IMU Tightly-Coupled SLAM Method Based on IEKF and Loop Closure Detection | |
| Lorenzo et al. | A Hough-based method for concurrent mapping and localization in indoor environments | |
| CN111141290B (en) | Positioning method, positioning device, equipment and storage medium of robot | |
| Wu et al. | Landmark pair based localization for intelligent vehicles using laser radar | |
| KR102364007B1 (en) | Online self-calibration system of mutiple 2D LiDARs using ling features with robust adaptive covariance | |
| Jiang et al. | Research on the Positioning and Navigation Accuracy Based on Multi-sensor Smart Car |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |