Self position estimation system

Abstract

A system for estimating a self-position of a moving body is provided with: a first self-position estimation unit for estimating a first self-position in a first coordinate system using a first positioning sensor mounted on the moving body; a second self-position estimation unit that estimates a second self-position in a second coordinate system using a second positioning sensor mounted on the moving body; a self-position conversion unit that converts the first self-position represented by the first coordinate system estimated by the first self-position estimation unit into a representation form in the second coordinate system; and a self-position correction unit that corrects the first self-position converted to a representation in the second coordinate system using correction information map data that stores, as correction information, a positional shift amount of a recording point in an actual space represented by the first coordinate system with respect to the second coordinate system.

Description

Technical Field

[0001] The present disclosure relates to a self-position estimation system. Background Art

[0002] As a measure to save manpower, expectations for autonomous mobile robots are increasing.

[0003] During autonomous movement, the robot needs to always know where it is currently located, which is achieved through various algorithms for estimating its own position.

[0004] In existing indoor autonomous mobility, the self-position estimation based on Lidar and grid maps is generally used. Outdoors, for example, in large-scale agricultural autonomous driving, although the driving area becomes wider, the sky is open in the driving area as a whole, and it is easy to receive radio waves from satellites, so it is easy to implement GNSS-based position estimation at a relatively low cost. On the other hand, even outdoors, such as in last-mile delivery, the driving area is centered on residential streets, building streets, etc., the sky is not necessarily open, and the driving area includes areas where GNSS cannot be used.

[0005] In view of this, in order to make the autonomous mobile robot more flexible in driving, a plurality of self-position estimation units are generally used in a complementary manner. For example, Patent Document 1 describes a method for estimating the self-position and measuring the shape of a mobile body by aligning the measurement results of each of a plurality of measurement units using coordinate transformation.

[0006] Prior Art Literature

[0007] Patent Literature

[0008] Patent Document 1: Japanese Patent Application Publication No. 2017-150977 Summary of the invention

[0009] However, in the prior art such as Patent Document 1, there is a problem that due to the deformation of the coordinate system, an offset occurs between two estimated self-positions derived by different self-position estimation algorithms, and the self-position becomes unstable. For example, the self-position estimation in the grid map is based on the relative self-position alignment of the grid map, so it is easy to produce coordinate system deformation unlike GNSS which always obtains absolute coordinates.

[0010] When attempting to align two estimated self-positions derived from different self-position estimation algorithms in such a state including a distorted coordinate system, a large difference arises between the two, and when the robot switches its self-position estimation algorithm, there is a concern that its self-position may be lost or misidentified.

[0011] The present invention has been made in view of the above-mentioned problems, and an object of the present invention is to provide a self-position estimating system capable of suppressing positional deviation of self-positions estimated by two self-position estimating algorithms when two self-position estimating algorithms are used complementarily.

[0012] The present invention mainly solves the above-mentioned problems and is a self-position estimation system for a mobile object, the self-position estimation system comprising:

[0013] A first self-position estimating unit estimates a first self-position in a first coordinate system using a first positioning sensor carried by the mobile body;

[0014] a second self-position estimating unit that estimates a second self-position in a second coordinate system using a second positioning sensor carried by the mobile body;

[0015] a self-position conversion unit that converts the first self-position estimated by the first self-position estimating unit and expressed in the first coordinate system into an expression form in the second coordinate system; and

[0016] The self-position correction unit corrects the first self-position converted into a representation form in the second coordinate system using correction information map data storing, as correction information, a position offset of a recording point in the actual space represented by the first coordinate system relative to the second coordinate system.

[0017] According to the position estimation system of the present disclosure, when two self-position estimation algorithms are used complementarily, it is possible to suppress the positional deviation of the self-position estimated by the two self-position estimation algorithms. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 This is a diagram that explains the definition of its own position.

[0019] Figure 2 This is a diagram showing a configuration of an autonomous mobile robot including a self-position estimating system according to an embodiment of the present invention.

[0020] Figure 3 This is a diagram showing functional blocks of a self-position estimating system of an autonomous mobile robot according to an embodiment of the present invention.

[0021] Figure 4 This is a diagram for explaining the definition of the position derived by the first self-position estimating unit.

[0022] Figure 5 This is a diagram for explaining the definition of the position derived by the second self-position estimating unit.

[0023] Figure 6This is a diagram for explaining the conversion of the coordinate system performed by the self-position conversion unit.

[0024] Figure 7 : is a flowchart showing the correction processing performed by the own position correction unit.

[0025] Figure 8 Yes Figure 7 The process of S530 is broken down into detailed steps of a subroutine flowchart.

[0026] Fig. 9 Yes Figure 7 The process of S530 is schematically described in the following figure. DETAILED DESCRIPTION

[0027] Hereinafter, embodiments of the present invention will be described with reference to the drawings.

[0028] First, use Figure 1 Describe the definition of your own position.

[0029] The so-called self-position refers to the scalar value of the robot's angle (i.e., posture) and the coordinates of the robot. The so-called coordinates refer to the values ​​of the X-coordinate and Y-coordinate on the plane where the robot moves. In other words, it is defined as shown in the following formula (1). In addition, for the sake of convenience, the concept of "position" and "posture (i.e., orientation)" of the robot (moving body) is referred to as "position".

[0030] [Mathematical formula 1]

[0031]

[0032] Next, the autonomous mobile robot will be described.

[0033] Figure 2 1 is a diagram showing a configuration of an autonomous mobile robot 10 including a self-position estimating system according to an embodiment of the present invention. The autonomous mobile robot 10 (corresponding to the "mobile object" of the present invention) estimates its own position and moves autonomously.

[0034] The autonomous mobile robot 10 is equipped with a wheel speed sensor 110 , a range sensor 120 , a GNSS sensor 130 , and a control unit 15 .

[0035] The wheel speed sensor 110 is a sensor for measuring the rotation speed of each left and right wheel. Based on the time change of the vehicle speed and yaw rate estimated from the wheel speed sensor 110, the movement amount of the autonomous mobile robot 10 from the reference position is calculated, and based on the movement amount, the current position of the autonomous mobile robot 10 can be estimated.

[0036] The range sensor 120 is a two-dimensional scanning optical distance sensor that uses laser to scan and measure the distance to an external detection object, such as LIDAR (Light Detection and Ranging). The data obtained from the range sensor 120 is data that obtains distance information to obstacles and walls at a certain angular resolution.

[0037] The GNSS sensor 130 calculates the position of the receiving point using, for example, ranging signals from a plurality of GPS satellites. The data obtained from the GNSS sensor 130 is the latitude and longitude information of the sensor position. However, the GNSS sensor 130 uses radio waves transmitted by GNSS artificial satellites to measure the position, so the estimated accuracy varies depending on the configuration of artificial satellites at the time of measurement, weather, and the presence or absence of surrounding buildings.

[0038] Next, the self-position estimating system U of the autonomous mobile robot 10 will be described.

[0039] Figure 3 2 is a diagram showing functional blocks of the self-position estimating system U of the autonomous mobile robot 10 .

[0040] The control unit 15 estimates the self-position of the autonomous mobile robot 10. At this time, the self-position estimation in this patent is roughly divided into the following two processes.

[0041] (1) Processing of Estimating the Self-Position of the First Self-Position Estimating Unit 210 and the Second Self-Position Estimating Unit 220 (described later)

[0042] (2) Processing of correcting the self-position estimated by the first self-position estimating unit 210 using the correction information map data 320

[0043] The control unit 15 is a so-called computer including a CPU, a memory, etc. The control unit 15 has functional blocks such as a calculation unit 200 and a storage unit 300. The control unit 15 is connected to the wheel speed sensor 110, the range sensor 120, and the GNSS sensor 130 by wire or wireless.

[0044] The storage unit 300 includes travel route map data 310 and correction information map data 320. The storage unit 300 is a non-volatile storage device such as a so-called HDD mounted on a PC.

[0045] The driving path map data 310 uses data called an occupancy grid map, which is a bitmap format data that represents the area where the robot can move in white and fills in black the areas where there are fixed objects such as walls and billboards (i.e., areas where the robot cannot move). In addition, the driving path map data 310 is created by, for example, using SLAM (Simultaneous Localization and Mapping) processing that sequentially estimates the robot's position and creates a map using the range sensor 120.

[0046] The correction information map data 320 is data in which correction information is arranged at each position in the actual space (i.e., each recording point where correction information is actually measured) so as to cover the area in which the robot can move in the first coordinate system used by the first self-position estimating unit 210 (see Fig. 9 ).

[0047] The correction information is correction data for converting the position and posture on the first coordinate system estimated by the first self-position estimating unit 210 into the position and posture of the second coordinate system used in the second self-position estimating unit 220 with high accuracy. The correction information is, for example, data that associates the position on the first coordinate system with the correction amount related to the x direction, y direction, and rotation direction of the position as shown in the following formula (2). As the correction information, information on coordinate deformation obtained in advance by actual measurement at each recording point in the actual space is used (the details will be described later).

[0048] Furthermore, the correction information set at a certain point in the first coordinate system is expressed in the formula (2), and this correction information is set individually for each position in the region in which the robot can move in the first coordinate system.

[0049] [Mathematical formula 2]

[0050]

[0051] The calculation unit 200 includes a first self-position estimating unit 210 , a second self-position estimating unit 220 , a self-position converting unit 230 , and a self-position correcting unit 240 .

[0052] The first self-position estimating unit 210 estimates the self-position of the autonomous mobile robot 10 using the value of the range sensor 120 and the driving path map data 310 contained in the storage unit 300. Hereinafter, the self-position of the autonomous mobile robot 10 estimated by the first self-position estimating unit 210 is referred to as "position P1" or "first self-position P1".

[0053] In the first self-position estimating method performed by the first self-position estimating unit 210 , position estimation is performed by performing a so-called scan matching process using, for example, the measurement values ​​of the wheel speed sensor 110 , the measurement values ​​of the range sensor 120 , and the travel path map data 310 .

[0054] use Figure 4 , the definition of the position P1 derived by the first self-position estimating unit 210 will be described. For example, as shown in the following equation, the position P1 is in the data format of the following equation (3).

[0055] [Mathematical formula 3]

[0056]

[0057] In addition, each element on the right side of the formula (3) is a scalar value, and the angle is an angle value with the Z axis as the center and the vector direction (x, y) = (1, 0) as the angle value 0. In addition, the coordinate system is the first coordinate system.

[0058] Second self-position estimating unit 220 estimates the self-position of autonomous mobile robot 10 using GNSS sensor 130 and wheel speed sensor 110. The self-position of autonomous mobile robot 10 estimated by second self-position estimating unit 220 is referred to as "position P2" or "second self-position P2".

[0059] In the second self-position estimating method performed by the second self-position estimating unit 220 , the measurement values ​​of the wheel speed sensor 110 and the GNSS sensor 130 are used to perform position estimation based on the temporal evolution of the state space model by using a so-called Kalman filter or the like.

[0060] The second self-position estimation method is described in more detail. First, the second self-position estimation unit 220 uses the values ​​of longitude and latitude measured by the GNSS sensor 130 and a geodetic system such as WGS84 to calculate the coordinate points on the plane. Next, the second self-position estimation unit 220 uses the values ​​of wheel speed measured by the wheel speed sensor 110 to calculate the successive movement amount. In addition, the second self-position estimation unit 220 uses a method such as a Kalman filter to integrate the coordinate points on the above-mentioned plane and the successive movement amount, and outputs a position P2 as the self-position estimation result.

[0061] use Figure 5 The definition of the position P2 estimated by the second self-position estimating unit 220 will be described. The position P2 is in the data format of the following equation (4).

[0062] [Formula 4]

[0063]

[0064] Each element on the right side of equation (4) is a scalar value, and the angle is an angle value with the Z axis as the center and the vector direction (x, y)=(1, 0) as the angle value 0. In addition, the position P2 is expressed by the second coordinate system.

[0065] The self-position converter 230 converts the self-position estimation result in the first coordinate system output by the first self-position estimator 210 into a self-position estimation value in the second coordinate system.

[0066] Here, use Figure 6 The conversion of the coordinate system performed by the self-position conversion unit 230 will be described.

[0067] The positional relationship of the origin of the positional relationship between the first coordinate system and the second coordinate system is defined by rotation and parallel translation. Therefore, the coordinate conversion vector P used to convert the position P1 represented by the first coordinate system into the coordinate conversion vector P of the second coordinate system is offset Defined by rotation and parallel translation. Figure 6 In the case of the positional relationship shown, the coordinate conversion vector is as shown in the following equation (5).

[0068] [Formula 5]

[0069]

[0070] By using P offset , the position estimation result estimated by the first coordinate system can be converted to the second coordinate system. That is, the position P1 coordinates are converted to the second coordinate system. The posture after the coordinate conversion of the position P1 is called the position P transed Position P transed The data is expressed, for example, as in the following formula (6).

[0071] [Mathematical formula 6]

[0072]

[0073] In addition, the x-coordinate position and the y-coordinate position on the right side of equation (6) are scalar values, and the angle θ is an angle value with the Z-axis as the center and the vector direction (x, y) = (1, 0) as the angle value 0. Here, the x-coordinate position, the y-coordinate position, and the angle θ are values ​​expressed by the second coordinate system.

[0074] Here, it is assumed that the first self-position estimation method and the second self-position estimation method produce ideal estimation results. When the autonomous mobile robot 10 is present at a certain location, the position P2 and the position P transed for the same location.

[0075] However, if there is a deformation in each location of the first coordinate system in the estimation result, an error will occur depending on where the autonomous mobile robot 10 is located in the driving area, so there is a concern that due to the deformation, the position P1 measured by the first coordinate system converted into the position of the second coordinate system will not be consistent with the position P2 measured by the second coordinate system. In this way, for example, when the robot switches its own position estimation algorithm, its own position may be lost or misidentified, which may have an adverse effect on driving.

[0076] For example, when the driving path map data 310 used for position estimation by the first self-position estimation method is deformed relative to the real environment, such a phenomenon occurs. In addition, the driving path map data 310 is often created by using the so-called SLAM process that sequentially performs self-position estimation and map creation using the range sensor 120, and it is difficult to eliminate the deformation of the real environment and the driving path map data 310 for the entire driving environment.

[0077] On the other hand, the self-position estimation in the SLAM process for creating the driving road map and the self-position estimation based on GNSS can be performed in parallel, and the self-position estimation results at each point can be easily compared. The comparison results can be used for posture correction.

[0078] That is, the correction information map data 320 is created by using the positional offset information of two self-position data acquired by the parallel implementation of the self-position estimation based on the range sensor 120 and the self-position estimation based on the GNSS sensor 130 at each position (i.e., each recording point) in the actual space in advance. And, the self-position estimation system U involved in the present disclosure corrects the deformation included when the coordinates of the position P1 measured in the first coordinate system are converted into the second coordinate system by using the correction information calculated in advance at each position (i.e., each recording point) in the actual space.

[0079] This allows the self-position P1 expressed in the first coordinate system and estimated by the first self-position estimating unit 210 to be converted into the expression format of the second coordinate system with high accuracy. That is, this allows the two self-position estimating algorithms to be used complementarily.

[0080] Next, a description will be given of a process of correcting the own position using the correction information map. Figure 7 2 is a flowchart showing the correction process performed by the self-position correction unit 240 .

[0081] First, in S510 , the control unit 15 calculates a position P1 which is a self-position estimation result using the first self-position estimation method.

[0082] Next, at S520, the control unit 15 converts the position P1 expressed on the first coordinate system into the position P expressed on the second coordinate system. transed This process uses the coordinate system transformation vector P mentioned above. offset to implement.

[0083] Next, in S530 , the control unit 15 determines the correction information group ANCuse used for calculating the correction value.

[0084] Here, use Figure 8 A process of determining the correction information group ANCuse used for calculating the correction value will be described.

[0085] Figure 8 This is a flowchart of a subroutine that breaks down the processing of S530 into detailed steps. Fig. 9 Yes Figure 7 The processing of S530 is schematically illustrated in FIG. Fig. 9 Each point in is a record point where correction information is stored in the correction information map data 320 .

[0086] First, in S530 - 1 , the control unit 15 checks whether or not undetermined correction information remains in the correction information map data 320 . If so, one of the undetermined correction information is selected ( S530 - 2 ).

[0087] Next, in S530 - 3 , the control unit 15 calculates the distance between the position of the recording point associated with the correction information selected in S530 - 2 and the position P1 . The calculated distance value is referred to as the distance value A. The calculation method of the distance value A is shown in the following formula (7).

[0088] [Formula 7]

[0089]

[0090] Next, in S530-4, the control unit 15 compares the distance value A calculated in S530-3 with the distance threshold. If the distance value A is less than the distance threshold, the process proceeds to S530-5. If the distance value A is greater than the distance threshold, the process of the correction information is terminated and the process returns to S530-1.

[0091] Next, in S530-5, the control unit 15 adds the correction information selected in S530-2 to the correction information group ANCuse used in the calculation of the correction value. After the addition, the control unit 15 returns to S530-1 and performs the determination process related to other correction information in the correction information map data 320 (i.e., the determination process of whether to use it for the calculation of the correction value).

[0092] When the control unit 15 determines in S530 - 1 that the confirmation of all the correction information in the correction information map data 320 has been completed, the control unit 15 outputs the created correction information group ANCuse and ends the process of S530 .

[0093] Next, in S540, the control unit 15 corrects the value P hosei The correction value P is calculated by taking the weighted average of the correction value of the correction information of each recording point, with the distance from the position P1 to the position of each recording point as the weight. hosei The specific formula is as follows (8).

[0094] [Mathematical formula 8]

[0095]

[0096] Next, in S550, the control unit 15 uses the position P1 converted into the position P of the second coordinate system. transed and correction value P hosei , and the self position P as the correction result is correced The calculation of is specifically implemented by the following formula (9).

[0097] [Mathematical formula 9]

[0098]

[0099] By following the above steps S500 to S550, the second self-position estimating method calculated on the second coordinate system and the first self-position estimating method calculated on the first coordinate system are both used as the self-position estimation result on the second coordinate system, and the two self-position estimation results can be used switchably.

[0100] Furthermore, since the higher the arrangement density of the recording points in the correction information map data 320 is, the more accurate the self-position estimation can be performed, it is preferable to generate in advance the correction information map data 320 in which more recording points are arranged.

[0101] By using the method of the present disclosure, for example, the autonomous mobile robot 10 can switch the self-position estimation algorithm during autonomous movement and travel smoothly. That is, the control unit 15 involved in the present disclosure interactively switches between a first movement control mode for controlling the movement of the autonomous mobile robot 10 using the first self-position estimated by the first self-position estimation unit 210 and a second movement control mode for controlling the movement of the autonomous mobile robot 10 using the second self-position estimated by the second self-position estimation unit 220, and controls the movement of the autonomous mobile robot 10. The control unit 15 switches the self-position estimation function to be used, for example, based on the estimation accuracy of the second self-position estimation method output by the second self-position estimation unit 220.

[0102] Although the specific examples of the present invention have been described in detail above, these are merely illustrative and do not limit the claims. The technology described in the claims includes various modifications and changes of the specific examples described above.

[0103] Industrial Applicability

[0104] The present disclosure relates to a position estimation system and is particularly useful in a method for estimating the position of an autonomous mobile robot.

[0105] Explanation of symbols

[0106] U Self-position estimation system

[0107] 10 Autonomous Mobile Robots

[0108] 15 Control Unit

[0109] 110 Wheel speed sensor

[0110] 120 Range Sensor

[0111] 130 GNSS Sensors

[0112] 200 Operation unit

[0113] 210 First self position estimation unit

[0114] 220 Second self position estimation unit

[0115] 230 Self-position conversion unit

[0116] 240 Self-position correction unit

[0117] 300 Storage

[0118] 310 Driving route map data

[0119] 320 Correction information map data

[0120] P1 Own position (first own position)

[0121] P2 Own position (second own position).

Claims

1. A self-position estimation system is a self-position estimation system of a mobile object, comprising: A first self-position estimating unit estimates a first self-position in a first coordinate system using a first positioning sensor carried by the mobile body; a second self-position estimating unit that estimates a second self-position in a second coordinate system using a second positioning sensor carried by the mobile body; a self-position conversion unit that converts the first self-position estimated by the first self-position estimating unit and expressed in the first coordinate system into an expression form in the second coordinate system; and The self-position correction unit corrects the first self-position converted into a representation form in the second coordinate system using correction information map data storing, as correction information, a position offset of a recording point in the actual space represented by the first coordinate system relative to the second coordinate system.

2. The self-position estimation system according to claim 1, wherein: The self-position correction unit performs the following processing: extracting the correction information associated with and stored in the recording point whose distance from the first self position is less than a given distance from the correction information map data, Based on the extracted correction information, the first self position is corrected.

3. The self-position estimation system according to claim 2, wherein: In the case where there are multiple recording points, The self-position correction unit calculates a correction value to be applied to the first self-position by weighted averaging the correction information associated with and stored in the plurality of recording points based on distances between the first self-position and respective positions of the plurality of recording points.

4. The self-position estimation system according to claim 1, wherein: The first self position and the second self position respectively include a coordinate position and a direction of the moving object.

5. The self-position estimation system according to claim 1, wherein: The first self-position estimating unit estimates the first self-position by using a position estimating method using a range sensor. The second self-position estimating unit estimates the second self-position by using a position estimation method using a satellite positioning system.

6. The self-position estimation system according to claim 1, wherein: The mobile object is an autonomous mobile robot.

7. The self-position estimation system according to claim 1, wherein: The self-position estimation system further includes: a control unit, which switches between a first movement control mode for controlling the movement of the mobile body using the first self-position estimated by the first self-position estimation unit and a second movement control mode for controlling the movement of the mobile body using the second self-position estimated by the second self-position estimation unit, and controls the movement of the mobile body.

Images