CN112904358B - Laser positioning method based on geometric information - Google Patents
Laser positioning method based on geometric information Download PDFInfo
- Publication number
- CN112904358B CN112904358B CN202110082545.1A CN202110082545A CN112904358B CN 112904358 B CN112904358 B CN 112904358B CN 202110082545 A CN202110082545 A CN 202110082545A CN 112904358 B CN112904358 B CN 112904358B
- Authority
- CN
- China
- Prior art keywords
- laser
- robot
- reflecting plate
- coordinate system
- under
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- 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: arranging at least three laser reflection plates in a set area in a robot working space; constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflecting plate; 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 reflecting plate installed on the robot; and based on the position of the robot under the two-dimensional scene map, predicting and estimating the pose of the robot. According to the geometric information-based laser positioning method, the laser reflecting plate is arranged in the set area needing to be accurately positioned to serve 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 calculation 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 robot technology, the mobile robot has wide application prospect in the aspects of natural disasters and nuclear leakage rescue, polar and extra-satellite exploration, military reconnaissance and combat, industrial manufacturing and logistics automation, civil vehicle intellectualization and the like, and can replace human beings to engage in various dangerous, heavy and boring works. Autonomous navigation capability is a precondition 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 profile information detected by a laser radar as a reference to perform navigation, and the accuracy and reliability of the natural navigation are relatively low due to the fact that the environment profile information is only used as the 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.
Therefore, 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 reflecting plates in a set area in a robot working space;
s2, constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflecting plate;
s3, determining the position of the robot under the two-dimensional scene map by utilizing a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflecting plate installed on the robot;
s4, based on the position of the robot under the two-dimensional scene map, predicting and estimating the pose of the robot.
In some optional embodiments, the constructing a two-dimensional scene map corresponding to a robot workspace including a laser reflector includes the steps of:
s21, setting a global coordinate system and a local coordinate system;
s22, controlling the robot to move, and scanning a working space environment by using a laser radar to obtain point data under a group of robot polar coordinate systems;
s23, carrying out data clustering and feature extraction on the point data, and determining the position information of the laser reflecting plate;
s24, completing two-dimensional scene map construction.
In some alternative embodiments, the global coordinate system and the local coordinates are set in the following manner:
selecting a set position in the working space as a coordinate origin o, selecting a set direction as an x-axis positive direction, selecting a direction perpendicular to the set direction as a y-axis positive direction, and establishing a global coordinate system xoy;
the center of the robot is selected as a coordinate origin O, the advancing direction of the robot is selected as an X-axis positive direction, the direction perpendicular to the advancing direction of the robot is selected as a Y-axis positive direction, and a local coordinate system XOY is established.
In some alternative embodiments, the point data is clustered using the following formula 1 to divide the point data into a plurality of point clusters, one point cluster corresponding to each object;
wherein r is k,k+1 ＝|r k -r k+1 I denotes the geometric distance of two points, r k And r k+1 Respectively represent the distance between two points and the laser radar, r min ＝min{r k ,r k+1 }，r min Representing the minimum value of the geometric distances of all two points in the point data, C 0 And β represents a system parameter, and φ represents a lidar angular resolution.
In some alternative embodiments, feature extraction is performed to determine laser reflector location information in the following manner:
obtaining n objects as expressed by the following formula 2 in the process of setting data clustering;
when the polar diameter of the object and the actual radius of the laser reflecting plate are within a set error range, the corresponding object is considered as the laser reflecting plate;
wherein E is seg Representing object collections, seg k Represents the kth object, (x) k ,y k ) Represents the position of the center of the kth object under the global coordinate system, D k Represents the polar diameter, theta, of the kth object k Representing the principal axis direction of the kth object.
In some optional embodiments, the determining the position of the robot under the two-dimensional scene map according to the observation information of the laser radar installed on the robot and the geometric information of the laser reflecting plate by using a positioning algorithm includes the following steps:
s31, determining the observation information of the laser reflecting plate under a local coordinate system according to the observation information of the laser radar;
s32, matching the observation information of the laser reflecting plate under the local coordinate system with the geometric information of the laser reflecting plate under the global coordinate system by utilizing an angle matching mode and/or a distance matching mode;
s33, determining the position of the robot under the two-dimensional scene map by adopting a least square iteration algorithm and/or a polygon positioning algorithm based on residual errors according to the observation information of the laser reflecting plate under the local coordinate system and the geometric information of the laser reflecting plate under the global coordinate system which are matched.
In some alternative embodiments, the matching of the observed information of the laser reflection plate in the local coordinate system and the geometric information of the laser reflection plate in the global coordinate system is performed by using an angle matching mode, and the following modes are adopted:
setting m laser reflecting plates in the working space environment, wherein the coordinates of the m laser reflecting plates under the global coordinates are l i (x i ,y i ) I is more than or equal to 1 and less than or equal to m, q laser reflection plates are observed by the laser radar, and the observation azimuth angles of the q laser reflection plates are alpha j ，1≤j≤q；
And respectively calculating theoretical azimuth angles of the m laser reflection plates and the advancing direction of the robot according to the pose of the robot at the previous moment, comparing the observed azimuth angles of the q laser reflection plates with the theoretical azimuth angles of the m laser reflection plates, and when the difference between the observed azimuth angles and the theoretical azimuth angles is in a set angle error range, indicating that the laser reflection plate corresponding to the current observed azimuth angle is matched with the laser reflection plate corresponding to the current theoretical azimuth angle.
In some alternative embodiments, the distance matching method is used for matching the observation information of the laser reflecting plate in the local coordinate system with the geometric information of the laser reflecting plate in the global coordinate system, and the following steps are adopted:
setting m laser reflecting plates in the working space environment, wherein the coordinates of the m laser reflecting plates under the global coordinates are l i (x i ,y i ) 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 p j The observation azimuth angles of the q laser reflection plates are alpha j ，1≤j≤q；
According to the distances between the q laser reflection plates and the laser radar and the observation azimuth angles of the q laser reflection plates, calculating the absolute distance between any two observed laser reflection plates, calculating the real distance between any two laser reflection plates in the m laser reflection plates according to the coordinates of the m laser reflection plates under the global coordinates, comparing the absolute distance with the real distance, and when the difference value between the absolute distance and the real distance is in a set distance error range, indicating that the two laser reflection plates corresponding to the current absolute distance are matched with the two laser reflection plates corresponding to the current real distance.
In some alternative embodiments, determining the position of the robot under the two-dimensional scene map using a residual-based least squares iterative algorithm comprises:
selecting a group of coordinates and azimuth angles of the laser reflecting plate, and establishing a group of integrated relational expressions of the selected group of coordinates and azimuth angles of the laser reflecting plate and the laser radar observation position points;
and (3) omitting a higher term by utilizing Taylor series expansion to form a residual equation set of an observed value and a theoretical value, and solving the position of the laser radar by using a least square method.
In some alternative embodiments, the pose of the robot is estimated predictively using an extended kalman filter algorithm.
The technical scheme of the invention has the main advantages that:
according to the geometric information-based laser positioning method, the laser reflecting plate is arranged in the set area needing to be accurately positioned to serve 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 calculation amount are low.
Drawings
In order to more clearly illustrate the embodiments of the invention or the technical solutions in the prior art, the drawings that are required in the embodiments or the description of the prior art will be briefly described, it being obvious that the drawings in the following description are only some embodiments of the invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a laser positioning method based on geometric information according to an embodiment of the invention;
FIG. 2 is a schematic diagram of a coordinate system according to an embodiment of the present invention;
FIG. 3 is another flow chart of a laser positioning method based on geometric information according to an embodiment of the invention;
FIG. 4 is a schematic view illustrating an observation position of a laser reflector according to an embodiment of the present invention;
FIG. 5 is a schematic diagram of a triangulation algorithm according to an embodiment of the present 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 specific embodiments of the present invention and corresponding drawings. It will be apparent that the described embodiments are only some, but not all, embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The following describes in detail the technical scheme provided by the embodiment of the invention 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, which is used to implement positioning of a robot in a working space, and includes the following steps:
s1, arranging at least three laser reflecting plates in a set area in a robot working space;
s2, constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflecting plate;
s3, determining the position of the robot under the two-dimensional scene map by utilizing a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflecting plate installed on the robot;
s4, based on the position of the robot under the two-dimensional scene map, predicting and estimating the pose of the robot.
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 a robot working space.
Specifically, according to the actual positioning requirement of the robot, at least three laser reflection plates are arranged in a set area where the environmental recognizability is poor or accurate positioning is required, for example, in the vicinity of a target point.
In order to ensure that the precise positioning of the robot can be realized, the arranged laser reflecting plates cannot form a regular polygon, and at least three laser reflecting plates can be observed by the laser radar installed on the robot in the moving process of the robot.
When the robot moves in other areas where the laser reflecting plate is not arranged in the working space, the robot can be positioned in a laser navigation mode based on the external natural environment.
S2, constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflecting plate.
Specifically, in an embodiment of the present invention, constructing a two-dimensional scene map corresponding to a robot working space including a laser reflection plate includes the following steps S21 to 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 an x-axis positive direction, a direction perpendicular to the set direction is selected as a y-axis positive direction, and a global coordinate system xoy is established; the center of the robot is selected as a coordinate origin O, the advancing direction of the robot is selected as an X-axis positive direction, the direction perpendicular to the advancing direction of the robot is selected as a Y-axis positive direction, and a local coordinate system XOY is established.
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 position of the laser reflecting plate which is arranged in the working space environment of the robot in advance and the positioning navigation calculation of the robot are both based on the set global coordinate system, and 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 rapidly and accurately determined.
The local coordinate system can also be called a mobile 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 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, and scanning a working space environment by using a laser radar to obtain point data under a group of robot polar coordinate systems;
the polar coordinate system of the robot is represented by a polar coordinate system taking the center of the robot as a pole, the advancing direction of the robot is the positive direction of a polar axis, point data under each polar coordinate of the robot comprises a polar diameter and a polar angle, the polar diameter is the distance between a 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 reflecting plate;
because the data volume obtained by the laser radar is large, and part of the data is influenced by environmental noise or is not a point of interest, the data is not convenient to further use for establishing a two-dimensional scene map, so that the data obtained by the laser radar needs to be preprocessed.
In one embodiment of the invention, data clustering is used to preprocess data acquired by the lidar.
Data clustering is Data partitioning (Data clustering), and the purpose of Data clustering is to divide the Data of a frame of original measurement points into a plurality of point clusters, wherein one point cluster corresponds to one object. When data clustering is carried out, the geometric distance between two points is calculated 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, 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, the two points belong to the same group, and if the geometric distance between the two points is not within the set threshold, the two points do not belong to the same group.
Specifically, the point data is clustered by using the following formula 1;
wherein r is k,k+1 ＝|r k -r k+1 I denotes the geometric distance of two points, r k And r k+1 Respectively represent the distance between two points and the laser radar, r min ＝min{r k ,r k+1 }，r min Representing the minimum value of the geometric distances of all two points in the point data, C 0 And beta represents a system parameter, phi represents the angular resolution of the laser radar, the system parameter beta is used for reducing the dependence of the segmented part on the distance from the laser radar to the object, the system parameter C can be obtained through actual data calculation 0 The longitudinal error for adjusting the lidar may be given by empirical values.
The distance between the laser radar and the point can be obtained through data obtained through laser radar scanning.
Further, after the data clustering of the point data is completed, feature extraction is performed in the following manner to determine the position information of the laser reflection plate:
assuming that n objects as expressed in the following equation 2 are obtained in the above data clustering process;
when the polar diameter of the object and the actual radius of the laser reflecting plate are within a set error range, the corresponding object is considered as the laser reflecting plate;
wherein E is seg Representing object collections, seg k Represents the kth object, (x) k ,y k ) Represents the position of the center of the kth object under the global coordinate system, D k Represents the polar diameter, theta, of the kth object k Representing the principal axis direction of the kth object.
In an embodiment of the invention, the characteristic extraction is performed by using the polar diameter characteristics of the object, and the object corresponding to the laser reflecting plate is determined, so that the position information of the laser reflecting plate is determined.
S24, completing two-dimensional scene map construction;
after the identification and positioning of all the laser reflecting plates and other objects are completed, the construction of the 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.
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 reflecting plate mounted on the robot.
Referring to fig. 3, specifically, determining a position of a robot under a two-dimensional scene map using a positioning algorithm according to observation information of a laser radar installed on the robot and geometric information of a laser reflection plate, includes the steps of:
s31, determining the observation information of the laser reflecting plate under a local coordinate system according to the observation information of the laser radar;
after the two-dimensional scene map corresponding to the working space of the robot is built, when the robot moves in the working space, the laser radar returns scanning data information in real time, and after the scanning data information is subjected to data processing by adopting the data clustering and the feature extraction in the step 2, the observation information of the laser reflecting plate under a local coordinate system can be obtained, wherein the observation information comprises the distance from the laser reflecting plate to the laser radar and the azimuth angle of the laser reflecting plate, and the azimuth angle of the laser reflecting plate represents an included angle formed by the connecting line of the laser reflecting plate and the laser radar and the advancing direction of the robot.
S32, matching the observation information of the laser reflecting plate under the local coordinate system with the geometric information of the laser reflecting plate under the global coordinate system by utilizing an angle matching mode and/or a distance matching mode;
in order to realize the positioning calculation of the robot, the observed information and the geometric information of the laser reflecting plates need to be matched, namely, the laser reflecting plates (possibly mixed with interference signals of non-laser reflecting plates) observed by the laser radar are corresponding to the actual reference laser reflecting plates in the working space environment one by one, effective laser reflecting plate information is extracted, and then the positioning calculation of the robot can be performed. In practice, some laser reflectors may not be visible or may be confused with other laser reflectors, and the resulting results may produce erroneous positioning results. Therefore, in an embodiment of the present invention, the observed information of the scanned laser reflector under the local coordinate system is matched with the actual geometric information of the laser reflector under the global coordinate system one by an angle matching mode and/or a distance matching mode.
The principle of the angle matching mode is that coordinate values of a laser reflecting plate under global coordinates are converted into a direction angle input program to serve as a theoretical value, when a laser radar scans a laser reflecting plate signal (possibly an interference signal), an angle value is obtained, the angle value can be called an observed value, a smaller error range is set, the observed value is compared with the theoretical value correspondingly obtained, if the difference value of the observed value and the observed value is within the set angle error range, the observed value is considered to be the laser reflecting plate, the observed value is stored, and otherwise, the observed value is considered to be the interference signal to be removed.
Specifically, the observation information of the laser reflecting plate under the local coordinate system and the geometric information of the laser reflecting plate under the global coordinate system are matched in an angle matching mode, and the method is carried out in the following mode:
setting m laser reflecting plates in the working space environment, wherein the coordinates of the m laser reflecting plates under the global coordinates are l i (x i ,y i ) I is more than or equal to 1 and less than or equal to m, q laser reflection plates are observed by the laser radar, and the observation azimuth angles of the q laser reflection plates are alpha j ，1≤j≤q；
And respectively calculating theoretical azimuth angles of the m laser reflection plates and the advancing direction of the robot according to the pose of the robot at the previous moment, comparing the observed azimuth angles of the q laser reflection plates with the theoretical azimuth angles of the m laser reflection plates, and when the difference between the observed azimuth angles and the theoretical azimuth angles is in a set angle error range, indicating that the laser reflection plate corresponding to the current observed azimuth angle is matched with the laser reflection plate corresponding to the current theoretical azimuth angle.
The purpose of laser reflector matching is to determine the observation azimuth angle alpha j In fact, the included angle between the reference reflecting plate and the advancing direction of the robot in the working space environment is the actual angle, because the observation period of the laser radar is very short and the displacement of the robot is very small in the process, the theoretical azimuth angle between 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, and if the calculated theoretical azimuth angle of the ith laser reflecting plate and the observed azimuth angle of the jth laser reflecting plate are within the set angle error range, the observed jth laser reflecting plate and the ith reference laser reflecting plate are considered to be matched with each other, so that a group of effective laser reflecting plate coordinates l are obtained i (x i ,y i ) And azimuth angle alpha j 。
Further, the distance matching mode is performed under the precondition that static pose calculation is satisfied, and the robot is required to be stationary at the moment. The principle Of the distance matching mode is that the laser radar scans laser at a certain frequency, the Time interval from the sending Of laser to the receiving Of the laser from the laser radar and the reflection Of the laser from the 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 to match with an actual reference laser reflecting plate under a global coordinate system.
Specifically, the observation information of the laser reflecting plate under the local coordinate system and the geometric information of the laser reflecting plate under the global coordinate system are matched in a distance matching mode, and the following steps are adopted:
setting m laser reflecting plates in the working space environment, wherein the coordinates of the m laser reflecting plates under the global coordinates are l i (x i ,y i ) 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 p j The observation azimuth angles of the q laser reflection plates are alpha j ，1≤j≤q；
According to the distances between the q laser reflection plates and the laser radar and the observation azimuth angles of the q laser reflection plates, calculating the absolute distance between any two observed laser reflection plates, calculating the real distance between any two laser reflection plates in the m laser reflection plates according to the coordinates of the m laser reflection plates under the global coordinates, comparing the absolute distance with the real distance, and when the difference value between the absolute distance and the real distance is in a set distance error range, indicating that the two laser reflection plates corresponding to the current absolute distance are matched with the two laser reflection plates corresponding to the current real distance.
Referring to FIG. 4, the laser radar at a certain moment observes a laser reflection plate position as shown in FIG. 4, XOY represents a local coordinate system, M j Indicating the observed laser reflection plate. The purpose of laser reflector matching is to determine which reference laser reflector in the working space environment is actually the laser reflector observed by the laser radar, and since the robot is stationary and the corresponding laser radar is stationary, the absolute distance M between any two observed laser reflectors can be calculated by cosine theorem according to the distance between the observed laser reflectors and the laser radar and the observed azimuth angle of the laser reflectors 1,2 ,M 1,3 ,…,M 1,q ,M 2,1 ,M 2,3 ,…,M 2,q ,…,M q,1 ,M q,2 ,…,M q,q-1 . Meanwhile, as 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, so that the observed laser reflectors are matched with the laser reflectors under the global coordinate system, and the effective laser reflector coordinate l is obtained i (x i ,y i ) Distance p j And azimuth angle alpha j 。
S33, utilizing the observation information of the matched laser reflecting plate under the local coordinate system and the geometric information of the laser reflecting plate under the global coordinate system, and determining the position of the robot under the two-dimensional scene map by adopting a least square iteration algorithm and/or a polygon positioning algorithm based on residual errors.
After the matching of the laser reflecting plates is completed, the position of the robot under the two-dimensional scene map can be calculated and determined by utilizing the observation information of the matched laser reflecting plates under the local coordinate system and the geometric information of the laser reflecting plates under the global coordinate system.
In an embodiment of the invention, a least square iterative algorithm based on residual errors is adopted to determine the position of the robot under the two-dimensional scene map on the basis of matching by using an angle matching mode.
Specifically, determining the position of the robot under the two-dimensional scene map by adopting a least square iterative algorithm based on residual errors comprises:
selecting a group of coordinates and azimuth angles of the laser reflecting plate, and establishing a group of integrated relational expressions of the selected group of coordinates and azimuth angles of the laser reflecting plate and the laser radar observation position points;
and (3) omitting a higher term by utilizing Taylor series expansion to form a residual equation set of an observed value and a theoretical value, and solving the position of the laser radar by using a least square method.
Since the lidar is mounted on the robot, the position of the lidar, i.e. the position of the robot, is determined.
Further, in an embodiment of the present invention, based on matching by using a distance matching method, a polygon positioning algorithm is used to determine a position of the robot under the two-dimensional scene map.
In an embodiment of the invention, the polygon positioning algorithm includes a triangle positioning algorithm and a trilateral positioning algorithm, and when the triangle positioning algorithm or the trilateral positioning algorithm is adopted to position the robot, it is necessary to ensure that the robot detects at least three laser reflection plates in the moving process.
Specifically, the triangular positioning algorithm realizes positioning by measuring the included angle between the laser reflecting plate and the longitudinal axis of the robot. Referring to fig. 5, the angle ARB between the robot and the laser reflection plates a and B and the angle BRC between the robot and the laser reflection plates B and C are measured, and two circumscribed circles and a circumscribed circle are made by using the circumscribed circle method. According to the nature of the circumscribed circle: if the robot is positioned at the intersection of two circles, two intersection points are arranged on the two circumscribed circles, one intersection point is the laser reflecting plate B, and the other intersection point R is the position of the robot.
Specifically, the trilateral algorithm realizes robot positioning by measuring the distance between the laser reflector and the robot, the global coordinates of which are known. Referring to FIG. 6, the distances from the robot to the A, B and C laser reflectors are measured as R a 、R b And R is c . According to the definition of a circle: the track of the moving point with the fixed distance to the fixed point being equal to the fixed length takes a position point A, a position point B and a position point C as circle centers respectively, and takes R a 、R b And R is c The radius is a circle, and the robot is positioned at the intersection point of the three circles. Because the trilateration algorithm is adopted, the required calculated amount and observed amount are less compared with the trilateration algorithm, and the trilateration algorithm is easier to realize, and in one embodiment of the invention, the trilateration algorithm is preferentially used.
If the current robot position is exactly equal to the distance between the plurality of laser reflecting plates, the positioning accuracy is very high according to the optimization selection criterion of the laser reflecting plates, but the fact is that the laser reflecting plates which are uniformly distributed are not always arranged around the robot, and the robot is not always arranged in the center of the laser reflecting plates which are uniformly distributed. Therefore, in the moving process of the robot, how to reasonably select the laser reflecting plates according to the surrounding environment makes the positioning accuracy highest necessary, and as the accuracy standard is obtained, for the problem of selecting the laser reflecting plates in three-point positioning, all three laser reflecting plate combinations of all effective identification points can be found out, the accuracy of the three laser reflecting plate combinations is estimated according to the given accuracy measurement standard, and then a group of optimal combinations is selected in a comparison mode.
S4, based on the position of the robot under the two-dimensional scene map, predicting and estimating the pose of the robot.
The laser positioning process of the robot is a discrete and discontinuous process by performing calculation once every turn of the laser radar, so that after the accurate position of the robot at each positioning calculation point is defined by using a positioning algorithm, errors possibly generated when the robot moves between two positioning calculation points need to be corrected in advance, the robot is ensured to reach a target position along a planning path, and the pose of the robot and the mean value and variance of an environment map need to be estimated. For this purpose, in an embodiment of the present invention, an extended kalman filter algorithm (EKF algorithm, extended Kalman Filter) is used 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, estimates the current system state according to the system state at the previous moment, and corrects the estimated state by taking the actual observed value at the current moment as feedback.
Assume that the system discrete state space model is:
where x (k) represents a state variable of the system at time k, u (k) represents a control variable of the system at time k, z (k) represents a measurement variable of the sensor at time k, w (k) represents a process noise of the system, v (k) represents an observation noise of the sensor at 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 timely estimates the next system state (prior estimation) from the current system state and the input variables through a state update equation, and the measurement update adds a new measurement signal into the prior estimated state already obtained in the state update equation, and finally obtains the posterior estimation of the system state.
For a nonlinear system, its process model and observation model are as follows:
where X (k), Z (k), and u (k) represent the state of the system at the moment, the observed quantity, and the control input of the system, respectively, w (k) and v (k) each represent gaussian white noise, f (X (k), u (k)) represent the equation of motion, and h (X (k)) represents the equation of observation.
Referring to fig. 3, the mean and variance of the pose and the environment map of the robot can be estimated using the EKF-SLAM algorithm. In the EKF-SLAM algorithm, system noise is defaulted to follow gaussian distribution, so the state output of the system is described using the mean and covariance of the system state. In the EKF-SLAM algorithm, the system state comprises the pose of the robot and the detected positions of all laser reflecting plates, and the next pose of the robot is predicted and estimated through an EKF filter by combining the input of the current pose and control variables of the robot according to the motion state equation of the robot. When the laser radar sensor detects an external laser reflecting plate, an EKF filter is adopted to update the system state according to a sensor measurement equation by combining the current pose of the robot, the environment laser reflecting plate and the measurement information of the sensor.
The specific implementation of EKF-SLAM can be described as follows: the control signal of the robot is input into a system state equation, the state of the system is estimated recursively by using the EKF, the estimation and correction are carried out based on the measurement error, the update and replacement are carried out on the augmented system state vector and the augmented system covariance matrix, the state prediction and the observation update are alternately carried out, and the actual value of the system state is continuously approximated, so that the estimation process of the EKF is completed.
According to the geometric information-based laser positioning method provided by the embodiment of the invention, the laser reflecting plate is arranged in the set area needing to be accurately positioned to serve 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 calculation amount are low.
It should be noted that in this document, relational terms such as "first" and "second" and the like are 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. Moreover, 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 this context, "front", "rear", "left", "right", "upper" and "lower" are referred to with respect to the placement state shown in the drawings.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and are not limiting thereof; although the 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 scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims (6)
1. 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 reflecting plates in a set area in a robot working space;
s2, constructing a two-dimensional scene map corresponding to the robot working space comprising the laser reflecting plate;
s3, determining the position of the robot under the two-dimensional scene map by utilizing a positioning algorithm according to the observation information of the laser radar and the geometric information of the laser reflecting plate installed on the robot;
s4, based on the position of the robot under the two-dimensional scene map, predicting and estimating the pose of the robot;
according to the observation information of the laser radar and the geometric information of the laser reflecting plate which are installed on the robot, the position of the robot under the two-dimensional scene map is determined by using a positioning algorithm, and the method comprises the following steps:
s31, according to the observation information of the laser radar, determining the observation information of the laser reflecting plate under a set local coordinate system;
s32, matching the observation information of the laser reflecting plate under the set local coordinate system with the geometric information of the laser reflecting plate under the set global coordinate system in an angle matching mode;
s33, determining the position of the robot under the two-dimensional scene map by adopting a least square iteration algorithm based on residual errors according to the observation information of the matched laser reflecting plate under the set local coordinate system and the geometric information of the laser reflecting plate under the set global coordinate system;
the method comprises the following steps of matching observation information of a laser reflecting plate under a set local coordinate system with geometric information of the laser reflecting plate under a set global coordinate system in an angle matching mode, wherein the following steps are adopted:
setting m laser reflecting plates in the working space environment, wherein the coordinates of the m laser reflecting plates under the global coordinates are l i (x i ,y i ) I is more than or equal to 1 and less than or equal to m, q laser reflection plates are observed by the laser radar, and the observation azimuth angles of the q laser reflection plates are alpha j ，1≤j≤q；
Calculating theoretical azimuth angles of the m laser reflection plates and the advancing direction of the robot respectively according to the pose of the robot at the previous moment, comparing the observed azimuth angles of the q laser reflection plates with the theoretical azimuth angles of the m laser reflection plates, and when the difference between the observed azimuth angles and the theoretical azimuth angles is in a set angle error range, indicating that the laser reflection plate corresponding to the current observed azimuth angle is matched with the laser reflection plate corresponding to the current theoretical azimuth angle;
the method for determining the position of the robot under the two-dimensional scene map by adopting the least square iterative algorithm based on the residual error comprises the following steps:
selecting a group of coordinates and azimuth angles of the laser reflecting plate, and establishing a group of integrated relational expressions of the selected group of coordinates and azimuth angles of the laser reflecting plate and the laser radar observation position points;
and (3) omitting a higher term by utilizing Taylor series expansion to form a residual equation set of an observed value and a theoretical value, and solving the position of the laser radar by using a least square method.
2. The laser positioning method based on geometric information according to claim 1, wherein the constructing of the two-dimensional scene map corresponding to the robot working space including the laser reflection plate includes the steps of:
s21, setting a global coordinate system and a local coordinate system;
s22, controlling the robot to move, and scanning a working space environment by using a laser radar to obtain point data under a group of robot polar coordinate systems;
s23, carrying out data clustering and feature extraction on the point data, and determining the position information of the laser reflecting plate;
s24, completing two-dimensional scene map construction.
3. The laser positioning method based on geometric information according to claim 2, wherein the following means are used for setting the global coordinate system and the local coordinate system:
selecting a set position in the working space as a coordinate origin o, selecting a set direction as an x-axis positive direction, selecting a direction perpendicular to the set direction as a y-axis positive direction, and establishing a global coordinate system xoy;
the center of the robot is selected as a coordinate origin O, the advancing direction of the robot is selected as an X-axis positive direction, the direction perpendicular to the advancing direction of the robot is selected as a Y-axis positive direction, and a local coordinate system XOY is established.
4. A geometric information-based laser positioning method according to claim 3, wherein the point data is clustered by using the following formula 1 to divide the point data into a plurality of point clusters, one corresponding to each object;
wherein r is k,k+1 ＝|r k -r k+1 I denotes the geometric distance of two points, r k And r k+1 Respectively represent the distance between two points and the laser radar, r min ＝min{r k ,r k+1 }，r min Representing the minimum value of the geometric distances of all two points in the point data, C 0 And β represents a system parameter, and φ represents a lidar angular resolution.
5. The method of claim 4, wherein the feature extraction is performed to determine the position information of the laser reflector by:
obtaining n objects as expressed by the following formula 2 in the process of setting data clustering;
when the polar diameter of the object and the actual radius of the laser reflecting plate are within a set error range, the corresponding object is considered as the laser reflecting plate;
wherein E is seg Representing object collections, seg k Represents the kth object, (x) k ,y k ) Represents the position of the center of the kth object under the global coordinate system, D k Represents the polar diameter, theta, of the kth object k Representing the principal axis direction of the kth object.
6. The laser positioning method based on geometric information according to any one of claims 1 to 5, characterized in that the pose of the robot is estimated predictively by using an extended kalman filter algorithm.
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 CN112904358A (en) | 2021-06-04 |
| CN112904358B true 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) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113432533B (en) * | 2021-06-18 | 2023-08-15 | 北京盈迪曼德科技有限公司 | Robot positioning method and device, robot and storage medium |
| CN113791377B (en) * | 2021-09-09 | 2024-04-12 | 中国科学院微小卫星创新研究院 | Positioning method based on angle measurement |
| CN114355383B (en) * | 2022-01-20 | 2024-04-12 | 合肥工业大学 | Positioning navigation method combining laser SLAM and laser reflecting plate |
| CN115060265B (en) * | 2022-05-27 | 2024-05-28 | 华南农业大学 | Positioning device for farmland navigation and positioning method thereof |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102402225B (en) * | 2011-11-23 | 2013-09-04 | 中国科学院自动化研究所 | Method for realizing localization and map building of mobile robot at the same time |
| CN105157697B (en) * | 2015-07-31 | 2017-05-17 | 天津大学 | Indoor mobile robot pose measurement system and measurement method based on optoelectronic scanning |
| CN106969768B (en) * | 2017-04-22 | 2020-08-11 | 深圳力子机器人有限公司 | Accurate positioning and parking method for trackless navigation AGV |
| CN110866927B (en) * | 2019-11-21 | 2021-07-20 | 哈尔滨工业大学 | Robot positioning and composition method based on EKF-SLAM algorithm combined with dotted line characteristics of foot |
| CN111781609B (en) * | 2020-04-10 | 2022-11-29 | 昆山同日智能技术有限公司 | AGV laser navigation multilateral positioning method |
| CN111678516B (en) * | 2020-05-08 | 2021-11-23 | 中山大学 | Bounded region rapid global positioning method based on laser radar |
-
2021
- 2021-01-21 CN CN202110082545.1A patent/CN112904358B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN112904358A (en) | 2021-06-04 |
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 | |
| CN106969768B (en) | Accurate positioning and parking method for trackless navigation AGV | |
| EP3427008B1 (en) | Laser scanner with real-time, online ego-motion estimation | |
| US10006772B2 (en) | Map production method, mobile robot, and map production system | |
| CN109459039B (en) | Laser positioning navigation system and method of medicine carrying robot | |
| CN103926925B (en) | Improved VFH algorithm-based positioning and obstacle avoidance method and robot | |
| WO2017028653A1 (en) | Method and system for automatically establishing map indoors by mobile robot | |
| CN109597864B (en) | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering | |
| CN110889808B (en) | Positioning method, device, equipment and storage medium | |
| CN111429574A (en) | Mobile robot positioning method and system based on three-dimensional point cloud and vision fusion | |
| CN108253958A (en) | A kind of robot real-time location method under sparse environment | |
| Park et al. | Radar localization and mapping for indoor disaster environments via multi-modal registration to prior LiDAR map | |
| CN114998276B (en) | Robot dynamic obstacle real-time detection method based on three-dimensional point cloud | |
| CN112750161B (en) | Map updating method for mobile robot | |
| CN114413909A (en) | Indoor mobile robot positioning method and system | |
| Demim et al. | Simultaneous localisation and mapping for autonomous underwater vehicle using a combined smooth variable structure filter and extended kalman filter | |
| CN112731337B (en) | Map construction method, device and equipment | |
| D’Adamo et al. | Registration of three‐dimensional scanning LiDAR sensors: An evaluation of model‐based and model‐free methods | |
| Blueml et al. | Bias compensated uwb anchor initialization using information-theoretic supported triangulation points | |
| CN115421153B (en) | Laser radar and UWB combined positioning method and system based on extended Kalman filtering | |
| Pei et al. | A decorrelated distributed EKF-SLAM system for the autonomous navigation of mobile robots | |
| Cupec et al. | Global localization based on 3d planar surface segments | |
| Guivant et al. | Simultaneous localization and map building: Test case for outdoor applications | |
| Watanabe et al. | Robust localization with architectural floor plans and depth camera |
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 |