CN111694009B - Positioning system, method and device - Google Patents

Abstract

The invention relates to a positioning method, belonging to the field of positioning. Acquiring first data of each cell in a specific area through a scanner; the scanner determines the global position of the robot in the specific area according to the first data; acquiring second data of the cells in the global position where the first laser radar is located through the first laser radar; acquiring third data of the cell in the global position where the second laser radar is located through the second laser radar; and matching the second data with the first data, and matching the third data with the first data to determine a specific position of the robot in the global position. According to the invention, a 3D scanner is utilized to construct a global map, so that global positioning of the robot in a large range around a transformer substation is realized, the global position of the power inspection robot is given, and the robot is initially positioned; the robot is localized accurate to be positioned by utilizing the double laser radars again, so that the robot can be accurately positioned in a large range of the transformer substation, and the problem of positioning loss caused by the existence of dead zones is prevented.

Description

A positioning system, method and device

technical field

The present invention relates to the field of positioning, and more specifically, to a positioning system, method and device.

Background technique

In order to meet the increasing requirements for power supply quality, substation power inspection robots are more and more widely used in substations. The power inspection robot is mainly used in outdoor substations. Through autonomous positioning and navigation functions, the inspection robot can drive to the designated location to perform instrument recording tasks without being on duty, and can detect defects in power equipment and foreign objects hanging in time. and other abnormal phenomena. When the power inspection robot performs inspection tasks, it is very important to avoid obstacles in a correct and timely manner, which is not only related to the safety of the robot but also to the normal operation of the entire substation.

Because it is an outdoor substation, the inspection environment of the robot is more complicated than indoors. When other inspection robots pass by around the robot, there will be a blind area, resulting in loss of positioning.

Therefore, it is necessary to propose an effective solution to solve the above problems.

Contents of the invention

In order to overcome the defects of the prior art, a positioning system, method and device of the present invention solve the problem of lost positioning of the inspection robot in the prior art.

For reaching this purpose, the present invention adopts following technical scheme:

The present invention provides a positioning system, including: an outdoor robot, a scanner, a first laser radar, and a second laser radar;

Both the scanner and the first laser radar are installed on the top of the robot;

Obtaining the first data of each cell in a specific area through the scanner;

Obtaining the second data of the cell in the global position where the first laser radar is located through the first laser radar;

Obtain the third data of the cell in the global position where the second laser radar is located through the second laser radar;

The scanner determines the global position of the robot in the specific area according to the first data;

The second data is matched with the first data, and the third data is matched with the first data respectively to determine the specific position of the robot in the global position.

The present invention also provides a positioning method, which is suitable for the above positioning system, including:

Obtaining the first data of each cell in a specific area through a scanner;

The scanner determines the global position of the robot in the specific area according to the first data;

Obtaining the second data of the cell in the global position where the first laser radar is located through the first laser radar;

Obtain the third data of the cell in the global position where the second laser radar is located through the second laser radar;

The second data is matched with the first data, and the third data is matched with the first data to determine the specific position of the robot in the global position.

Preferably, according to the first data, determining the global position of the robot in the specific area includes:

The scanner constructs a map of the specific area according to the first data, and determines the global position of the robot in the area.

Preferably, matching the second data with the first data and the third data with the first data respectively to determine the specific position of the robot in the global position includes:

Matching the second data with the first data to obtain a first optimal matching probability P(X) to obtain a first best matching pose P;

Matching the third data with the first data to obtain a second optimal matching probability P'(X) to obtain a second best matching pose P';

The specific position of the robot in the global position is determined through the relationship between P and P'.

Preferably, the specific position of the robot in the global position is determined through the relationship between P and P', including:

When P is greater than P', the probability that the first laser radar matches the scanner is greater than the probability that the second laser radar matches the scanner, thereby determining the specific position of the robot in the global position The location is the location of the first lidar in the specific area;

When P is smaller than P', the probability that the second laser radar matches the scanner is greater than the probability that the first laser radar matches the scanner, thereby determining the specific position of the robot in the global position The location is the location of the second lidar in the specific area.

The present invention also provides a positioning device, which is suitable for the above-mentioned positioning method, including:

The first acquisition unit is configured to acquire the first data of each cell in a specific area through a scanner;

a first determining unit, configured for the scanner to determine the global position of the robot in the specific area according to the first data;

The second acquisition unit is configured to acquire the second data of the cell in the global position of the first laser radar through the first laser radar;

A third acquiring unit, configured to acquire third data of a cell in the global position of the second laser radar through the second laser radar;

A second determining unit, configured to match the second data with the first data, and match the third data with the first data, to determine a specific position of the robot in the global position .

Preferably, the first determination unit is specifically used for the scanner to construct a map of the specific area according to the first data, and determine the global position of the robot in the area.

Preferably, the second determining unit is specifically configured to match the second data with the first data, obtain a first optimal matching probability P(X), and obtain a first best matching pose P ;

Matching the third data with the first data to obtain a second optimal matching probability P'(X) to obtain a second best matching pose P';

The specific position of the robot in the global position is determined through the relationship between P and P'.

Preferably, the second determination unit is specifically further configured to: when P is greater than P', the probability that the first laser radar matches the scanner is greater than the probability that the second laser radar matches the scanner , so as to determine that the specific position of the robot in the global position is the position of the first laser radar in the specific area;

When P is smaller than P', the probability that the second laser radar matches the scanner is greater than the probability that the first laser radar matches the scanner, thereby determining the specific position of the robot in the global position The location is the location of the second lidar in the specific area.

The beneficial effects of the present invention are:

The present invention provides a positioning method. The first data of each cell in a specific area is acquired through a scanner; the scanner determines the global position of the robot in the specific area according to the first data; and the first laser radar is used to obtain the first data. The second data of the cell in the global position of the laser radar; through the second laser radar, obtain the third data of the cell in the global position of the second laser radar; match the second data with the first data, and the second data The third data is matched with the first data to determine the specific position of the robot in the global position.

The present invention uses a 3D scanner to construct a global map, realizes the global positioning of the robot in a large range around the substation, gives the global position of the power inspection robot, and performs preliminary positioning of the robot; and then uses dual laser radars to realize local precise positioning of the robot, which has the advantages It can have the advantages of large positioning range of 3D scanner and high positioning accuracy of dual laser radars at the same time, which can realize the precise positioning of robots in a large range of substations and prevent the problem of positioning loss due to the existence of blind spots.

Description of drawings

Fig. 1 is a schematic structural diagram of a positioning system provided by a specific embodiment of the present invention;

Fig. 2 is a schematic flowchart of a positioning method provided by a specific embodiment of the present invention;

Fig. 3 is a schematic structural diagram of a positioning device provided by a specific embodiment of the present invention.

In the picture:

101. First laser radar; 102. Second laser radar; 103. Scanner; 104. Robot.

Detailed ways

The technical solutions of the present invention will be further described below in conjunction with the accompanying drawings and through specific implementation methods.

Fig. 1 is a schematic structural diagram of a positioning system provided by a specific embodiment of the present invention, as shown in Fig. 1; Fig. 2 is a schematic flow chart of a positioning method provided by a specific embodiment of the present invention, as shown in Fig. 2; Fig. 3 It is a schematic structural diagram of a positioning device provided by a specific embodiment of the present invention, as shown in FIG. 3 . Among them, the target objects in Fig. 1 refer to transformer boxes and obstacles in the substation environment, through laser scanning, feature information of these objects is extracted to construct a map. The present invention proposes a positioning system, including: an outdoor robot 104, a scanner 103, a first laser radar 101, and a second laser radar 102; the scanner 103 and the first laser radar 101 are installed on the top of the robot; Through the scanner 103, the first data of each cell in a specific area is obtained; through the first laser radar 101, the second data of the cell in the global position where the first laser radar 101 is located; through the first laser radar 102, obtain The third data of the cells in the global position of the first lidar 102; the scanner 103 determines the global position of the robot in a specific area according to the first data; respectively matches the second data with the first data, and the third The data is matched with the first data to determine the specific position of the robot 104 in the global position. In specific implementation, the scanner 103 is connected to the top of the bracket through a conductive slider, and the bottom of the bracket is fixedly connected to the top of the robot 104; the forward direction of the robot 104 is set to the front, and the 3D scanner 103 is installed at the upper position in the middle of the robot 104. The first laser radar 101 Installed above the front of the robot 104 , the first lidar 102 is installed above the rear of the robot 104 .

The present invention also provides a positioning method, which is suitable for the above positioning system, comprising the following steps:

S101: Obtain the first data of each cell in a specific area by using a scanner;

S102: The scanner determines the global position of the robot in a specific area according to the first data;

S103: Using the first laser radar, acquire the second data of the cell in the global position where the first laser radar is located;

S104: Obtain the third data of the cell in the global position of the second laser radar through the second laser radar;

S105: Match the second data with the first data, and match the third data with the first data, to determine a specific position of the robot in the global position.

The main function of the present invention is to determine the global position of the robot by the scanning of the scanner; the point cloud information scanned by the first laser radar and the scanned point cloud information of the scanner, and the point cloud information and the scanned point cloud information of the second laser radar The point cloud information scanned by the scanner is matched, and the one with the highest probability is selected as the position of the lidar in the global position as the actual position of the robot, that is, the specific position of the robot in the global position.

In S101, the first data of each cell in the specific area is obtained through the scanner; and in S102, the scanner determines the global position of the robot in the specific area according to the first data; specifically, through the 3D scanner 360° Rotate scanning to obtain 3D point cloud data, and the 3D scanner constructs a map of the current specific area based on the 3D point cloud data, realizes the global positioning of the current specific area, and determines the global position of the power inspection robot. Among them, each cell is pre-divided in a specific area; the specific area is the entire environment around the outdoor substation, specifically the environment within a few meters around the outdoor substation, such as 3 meters, 5 meters, etc.; the scanner is a 3D scanner ; The first data is 3D point cloud data; the robot adopts a power inspection robot. Among them, the 360° rotating scanning of the 3D scanner can obtain richer 3D point cloud information. The 3D scanner rotates and scans 360° to obtain 3D point cloud data. In the specific implementation, the 3D point cloud data can be obtained through the calculation of the scanning algorithm. The scanning frequency can be determined according to the specific implementation situation. For example, the scanning frequency can be set to 20 frames /S.

In S103, the first laser radar is used to obtain the second data of the cell in the global position of the first laser radar; specifically, the first laser radar constructs and obtains the cell in the global position of the first laser radar by scanning A map of , and acquire the second data of the cell in the global position of the first lidar. Wherein, the first lidar is 2D lidar; the second data is 2D point cloud information; in specific implementation, the scanning angle of the first lidar is 120°, and the scanning radius is R ₁ .

In S104, the second laser radar is used to obtain the third data of the cell in the global position of the second laser radar; specifically, the second laser radar constructs and obtains the cell in the global position of the second laser radar by scanning to obtain the third data of the cell in the global position of the second laser radar; in specific implementation, the scanning angle of the second laser radar is 360°, and the scanning radius is R ₂ . Wherein, R ₁ is greater than R ₂ . Likewise, the second lidar is 2D lidar; the third data is 2D point cloud information.

The present invention uses a 3D scanner to construct a global map, realizes the global positioning of the robot in a large range around the substation, gives the global position of the power inspection robot, and performs preliminary positioning of the robot; and then uses dual laser radars to realize local precise positioning of the robot, which has the advantages It can have the advantages of large positioning range of 3D scanner and high positioning accuracy of dual laser radars at the same time, which can realize the precise positioning of robots in a large range of substations and prevent the problem of positioning loss due to the existence of blind spots. In addition, lidar has excellent characteristics such as not being affected by weather, lighting and other conditions, not relying on texture and color to distinguish, and not sensitive to shadow noise. During laser radar measurement, the scanning frequency is high and the data volume is rich, and the distance value is returned, which is convenient for fast processing. Therefore, the use of laser radar to perceive the environmental information around the inspection robot has good adaptability and rapidity, but the working range of a single laser radar is relatively limited.

Preferably, determining the global position of the robot in the specific area according to the first data includes: the scanner builds a map of the specific area according to the first data, and determines the global position of the robot in the area.

Preferably, the second data is matched with the first data, and the third data is matched with the first data to determine the specific position of the robot in the global position, including:

Matching the second data with the first data to obtain the first optimal matching probability P(X) to obtain the first best matching pose P;

Matching the third data with the first data to obtain the second optimal matching probability P'(X) to obtain the second best matching pose P';

Through the relationship between P and P', determine the specific position of the robot in the global position.

Wherein, in the above implementation, the first optimal matching probability and the second optimal matching probability are respectively calculated by the following formula (1);

μ为均值，∑为方差，/>

表示一个单元格内所有的扫描点。in,

μ is the mean, ∑ is the variance, />

Indicates all scan points in a cell.

Furthermore, in specific implementation, the positioning system is also equipped with an industrial computer for realizing the data processing function. The SLAM algorithm is adopted through the industrial computer, and the SLAM algorithm is the NDT algorithm. The idea of the algorithm is to respectively construct the map of the current specific area based on the 3D point cloud data of the cell map in the global position of the first laser radar and the 3D scanner. Matching is performed, and the map of the cells in the global position where the second laser radar is located is matched with the map of the current specific area constructed by the 3D scanner based on the 3D point cloud data. First, the two-dimensional plane will be decomposed into a series of fixed-sized cells, and the probability density function is calculated based on the points of the cells. This conversion can directly scan the matching analytical expressions everywhere, without considering the correspondence between points or features, and can quickly and accurately complete the map matching, effectively solving the local position tracking and global positioning problems of the robot in the outdoor environment of the substation.

Preferably, the specific position of the robot in the global position is determined through the relationship between P and P', including:

When P is greater than P', the probability that the first laser radar matches the scanner is greater than the probability that the second laser radar matches the scanner, so that the specific position of the robot in the global position is determined as the position of the first laser radar in a specific area ;

When P is less than P', the probability that the second laser radar matches the scanner is greater than the probability that the first laser radar matches the scanner, so that the specific position of the robot in the global position is determined as the position of the second laser radar in a specific area .

In order to further clarify the matching process of the point cloud information obtained by the first laser radar and the 3D scanner and the matching process of the point cloud information obtained by the second laser radar and the 3D scanner, the point cloud information obtained by the first laser radar and the 3D scanner is used below Matching as the main description:

(1) Create a normal distribution transformation of the 3D scanner scan.

(2) Use the odometer reading to initialize the coordinate transformation parameters;

(3) For each sample scanned by the first lidar, map it to the first scanning coordinate system according to these coordinate transformation parameters;

(4) Determine the corresponding normal distribution of each mapping point;

(5) Evaluate the sum of the probability distributions of each mapping point as the fractional value of each coordinate transformation parameter;

(6) Use the Hessian matrix method to optimize these score values and calculate new parameter estimates;

(7) Go back to step 3 and continue the cycle until the convergence requirements are met. The fractional values of these coordinate transformation parameters p are expressed as:

x′_i＝T(x_i,p)

x′ _i ＝T( _xi ,p)

这里的u_i和上面的μ含义是一样的，不另外进行说明。Among them, i is the mapping point of coordinate transformation.

The u _i here has the same meaning as μ above, and no further explanation is given.

Here, as part of the scan matching algorithm, the error function -score(p) must be minimized, that is, the score(p) is maximized to ensure that the coordinate transformation according to the parameter p is optimal.

同理，第二激光雷达的最优匹配概率P'(X)也可以求得，这里就不再赘述，最后选择P(X)与P'(X)中最大值作为当前位置最优匹配位姿P^*。Optimize the first lidar and 3D scanner data through the Hessian matrix to calculate the optimal probability distribution of sensor readings given the robot's current position and map

Similarly, the optimal matching probability P'(X) of the second lidar can also be obtained, so I won't go into details here, and finally select the maximum value of P(X) and P'(X) as the optimal matching position of the current position Posture P ^* .

The present invention also provides a positioning device, which is suitable for the above positioning method, including:

201: a first acquisition unit, configured to acquire the first data of each cell in a specific area through a scanner;

202: a first determination unit, used for the scanner to determine the global position of the robot in a specific area according to the first data;

203: The second acquisition unit is configured to acquire the second data of the cell in the global position of the first laser radar through the first laser radar;

204: The third acquisition unit is configured to acquire the third data of the cell in the global position of the second laser radar through the second laser radar;

205: A second determination unit, configured to match the second data with the first data, and the third data with the first data, to determine a specific position of the robot in the global position.

Preferably, the first determination unit is specifically used for the scanner to construct a map of a specific area according to the first data, and determine the global position of the robot in the area.

Preferably, the second determination unit is specifically configured to match the second data with the first data, and obtain the first optimal matching probability P(X), so as to obtain the first best matching pose P;

Matching the third data with the first data to obtain the second optimal matching probability P'(X) to obtain the second best matching pose P';

Through the relationship between P and P', determine the specific position of the robot in the global position.

Preferably, the second determining unit is also specifically used to determine the specific position of the robot in the global position when P is greater than P', the probability that the first laser radar matches the scanner is greater than the probability that the second laser radar matches the scanner The position is the position of the first lidar in the specific area;

When P is less than P', the probability that the second laser radar matches the scanner is greater than the probability that the first laser radar matches the scanner, so that the specific position of the robot in the global position is determined as the position of the second laser radar in a specific area .

The present invention has been described through preferred embodiments, and those skilled in the art know that various changes or equivalent substitutions can be made to these features and embodiments without departing from the spirit and scope of the present invention. The present invention is not limited by the specific embodiments disclosed here, and other embodiments falling within the claims of the present application all belong to the protection scope of the present invention.

Claims (7)

1. A positioning system, comprising:

a robot, a scanner, a first lidar, and a second lidar located outdoors;

the scanner and the first laser radar are both arranged on the top of the robot;

acquiring first data of each cell in a specific area through the scanner;

acquiring second data of a cell in a global position where the first laser radar is located by the first laser radar;

acquiring third data of the cell in the global position where the second laser radar is located through the second laser radar;

the scanner determines the global position of the robot in the specific area according to the first data;

respectively matching the second data with the first data and the third data with the first data, determining a specific position of the robot in the global position, including:

matching the second data with the first data to obtain a first optimal matching probability P (X) so as to obtain a first optimal matching pose P;

matching the third data with the first data to obtain a second optimal matching probability P '(X) so as to obtain a second optimal matching pose P';

determining a specific position of the robot in the global position through the relation between the P and the P';

determining a specific position of the robot in the global position through the relation between the P and the P', wherein the specific position comprises the following steps:

when P is greater than P', the probability that the first laser radar matches the scanner is greater than the probability that the second laser radar matches the scanner, thereby determining that the specific position of the robot in the global position is the position of the first laser radar in the specific region;

when P is less than P', the probability that the second lidar matches the scanner is greater than the probability that the first lidar matches the scanner, thereby determining that the particular location of the robot in the global location is the location of the second lidar in the particular region.

2. A positioning method suitable for use in the positioning system of claim 1, comprising:

acquiring first data of each cell in a specific area through a scanner;

the scanner determines the global position of the robot in the specific area according to the first data;

acquiring second data of a cell in a global position where a first laser radar is located by the first laser radar;

acquiring third data of the cell in the global position where the second laser radar is located through the second laser radar;

and matching the second data with the first data, and matching the third data with the first data, and determining a specific position of the robot in the global position.

3. The positioning method of claim 2, wherein,

determining a global position of the robot in the specific area according to the first data, including:

and the scanner constructs a map of the specific area according to the first data, and determines the global position of the robot in the area.

4. A positioning device adapted for use in the positioning method of any one of claims 2 to 3, comprising:

a first acquisition unit configured to acquire first data of each cell in a specific area by a scanner;

a first determining unit, configured to determine, by the scanner, a global position of the robot in the specific area according to the first data;

the second acquisition unit is used for acquiring second data of the cell in the global position where the first laser radar is located through the first laser radar;

the third acquisition unit is used for acquiring third data of the cells in the global position where the second laser radar is located through the second laser radar;

and the second determining unit is used for matching the second data with the first data and matching the third data with the first data to determine the specific position of the robot in the global position.

5. The positioning device of claim 4 wherein,

the first determining unit is specifically configured to construct a map of the specific area according to the first data by using the scanner, and determine a global position of the robot in the area.

6. The positioning device of claim 4 wherein,

the second determining unit is specifically configured to match the second data with the first data, and obtain a first optimal matching probability P (X) so as to obtain a first most matching pose P;

matching the third data with the first data to obtain a second optimal matching probability P '(X) so as to obtain a second optimal matching pose P';

and determining the specific position of the robot in the global position through the relation between the P and the P'.

7. The positioning device of claim 6 wherein,

the second determining unit is specifically configured to determine that, when P is greater than P', the probability that the first lidar matches the scanner is greater than the probability that the second lidar matches the scanner, so that a specific position of the robot in the global position is a position of the first lidar in the specific area;

when P is less than P', the probability that the second lidar matches the scanner is greater than the probability that the first lidar matches the scanner, thereby determining that the particular location of the robot in the global location is the location of the second lidar in the particular region.

Images