Self-location estimation device, self-location estimation method, and program

Abstract

To provide a self-position estimation device capable of accurately estimating the position and posture of an autonomous moving body.SOLUTION: A self-position estimation device includes: a first acquisition section for acquiring first sensor data which makes it possible to detect a fixed object constantly present in the periphery of an autonomous moving body and an unfixed object temporarily present therein; a second acquisition section for acquiring second sensor data including acceleration and angular velocity of the autonomous moving body; a first estimation section for estimating a partial parameter that is a part of a parameter of six degrees of freedom representing the position and posture of the autonomous moving body in a three-dimensional coordinate system on the basis of the first sensor data and known information in which information including a position in a travel area of a mark, which is one fixed object selected in advance from a plurality of fixed objects is stored; and a second estimation section for estimating a self-position of the autonomous moving body on the basis of the first sensor data, the second sensor data, and the partial parameter.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present disclosure relates to a self-location estimation device, a self-location estimation method, and a program for an autonomous moving body. [Background technology]

[0002] Autonomous mobile objects used in logistics, plant inspections, and the like estimate their own position (position and orientation relative to each axis of a three-dimensional coordinate system) by comparing current sensor data with map data created in advance based on sensor data acquired by, for example, laser sensors or cameras (image sensors). However, when operating an autonomous mobile object in a facility where aisles are made up of multi-tiered cargo, such as a logistics warehouse, these cargo objects are non-fixed objects that exist temporarily and cannot be reflected in the map data in advance. As a result, the accuracy of the autonomous mobile object's estimation of its own position may decrease in areas where cargo is temporarily placed.

[0003] For this reason, in areas where it is difficult to estimate the self-position using a laser sensor or a camera, a technology has been devised for estimating the self-position using other sensors instead. For example, Patent Document 1 describes a method of acquiring sensor data measured by a plurality of different sensors such as an encoder, an inertial measurement unit (IMU), a global navigation satellite system (GNSS) receiver, a camera, and a laser sensor, estimating self-position candidates of an autonomous mobile body from each piece of sensor data, and determining the self-position candidate with the highest reliability as the self-position of the autonomous mobile body. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Japanese Patent Publication No. 2020-87307 Summary of the Invention [Problem to be solved by the invention]

[0005] However, the technology of Patent Document 1 selects one of the self-position candidates estimated independently from the individual sensor data and determines it as the self-position, so the accuracy of the self-position depends on the self-position estimation result by the sensor.

[0006] Furthermore, self-positioning technology that combines multiple sensors is being considered. Visual Inertial Odometry (VIO), a self-positioning technology that combines a camera and an IMU, uses optimization calculations (for example, nonlinear least squares) to calculate the self-position so that the amount of movement of feature points obtained from camera images matches the acceleration and angular velocity obtained from the IMU.

[0007] However, because this odometry method solves a local optimization problem viewed on a time axis, the accumulation of errors can cause drift, in which the estimated position gradually deviates from the actual position. There is a need for a technology that can reduce the effects of such accumulated errors and estimate the vehicle's self-position with high accuracy.

[0008] An object of the present disclosure is to provide a self-location estimation device, a self-location estimation method, and a program that can accurately estimate the position and attitude of an autonomous moving body. [Means for solving the problem]

[0009] According to one aspect of the present disclosure, a self-position estimation device that estimates the self-position of an autonomous mobile body in a driving area includes a first acquisition unit that acquires first sensor data that can detect fixed objects that are always present around the autonomous mobile body and non-fixed objects that are temporarily present around the autonomous mobile body; a second acquisition unit that acquires second sensor data including the acceleration and angular velocity of the autonomous mobile body; a first estimation unit that estimates partial parameters that are part of six degrees of freedom parameters that represent the position and attitude of the autonomous mobile body in a three-dimensional coordinate system based on known information that records information including the position in the driving area of ​​a landmark that is a fixed object pre-selected from a plurality of the fixed objects and the first sensor data; and a second estimation unit that estimates the self-position of the autonomous mobile body expressed by the six degrees of freedom parameters based on the first sensor data, the second sensor data, and the partial parameters.

[0010] According to one aspect of the present disclosure, a self-position estimation method for estimating the self-position of an autonomous mobile body in a travel area includes the steps of acquiring first sensor data capable of detecting fixed objects that are always present around the autonomous mobile body and non-fixed objects that are temporarily present; acquiring second sensor data including the acceleration and angular velocity of the autonomous mobile body; estimating partial parameters that are some of the six-degree-of-freedom parameters that represent the position and attitude of the autonomous mobile body in a three-dimensional coordinate system based on known information that records information including the position in the travel area of ​​a landmark that is a fixed object pre-selected from a plurality of the fixed objects and the first sensor data; and estimating the self-position of the autonomous mobile body expressed by the six-degree-of-freedom parameters based on the first sensor data, the second sensor data, and the partial parameters.

[0011] According to one aspect of the present disclosure, a program causes a self-position estimation device that estimates the self-position of an autonomous mobile body in a driving area to perform the following steps: acquiring first sensor data capable of detecting fixed objects that are always present around the autonomous mobile body and non-fixed objects that are temporarily present; acquiring second sensor data including the acceleration and angular velocity of the autonomous mobile body; estimating partial parameters that are some of the six-degree-of-freedom parameters that represent the position and attitude of the autonomous mobile body in a three-dimensional coordinate system based on known information that records information including the position in the driving area of ​​a landmark that is a fixed object pre-selected from a plurality of the fixed objects and the first sensor data; and estimating the self-position of the autonomous mobile body expressed by the six-degree-of-freedom parameters based on the first sensor data, the second sensor data, and the partial parameters. [Effects of the Invention]

[0012] According to the above aspect, the position and orientation of the autonomous moving body can be estimated with high accuracy. [Brief explanation of the drawings]

[0013] [Figure 1] 1 is a schematic diagram showing the overall configuration of an autonomous moving body according to a first embodiment. [Figure 2] 1 is a block diagram showing a functional configuration of a self-position estimation device according to a first embodiment. [Figure 3] 5 is a flowchart showing an example of processing performed by the self-position estimation device according to the first embodiment. [Figure 4] 6 is a flowchart showing an example of processing by a first estimating unit according to the first embodiment. [Figure 5] FIG. 2 is a diagram illustrating functions of the self-position estimation device according to the first embodiment. [Figure 6] 6 is a flowchart showing an example of processing by a second estimating unit according to the first embodiment. [Figure 7] FIG. 10 is a diagram illustrating functions of a self-location estimation device according to a second embodiment. [Figure 8]10 is a flowchart illustrating an example of processing by a first estimating unit according to the second embodiment. [Figure 9] FIG. 10 is a first diagram illustrating a function of a self-location estimation device according to a third embodiment. [Figure 10] FIG. 10 is a second diagram illustrating the function of the self-location estimation device according to the third embodiment. [Figure 11] 11 is a flowchart illustrating an example of processing by a first estimating unit according to the third embodiment. [Figure 12] FIG. 10 is a third diagram illustrating the function of the self-location estimation device according to the third embodiment. [Figure 13] FIG. 10 is a fourth diagram illustrating the function of the self-location estimation device according to the third embodiment. [Figure 14] FIG. 10 is a diagram illustrating functions of a self-position estimation device according to a fourth embodiment. [Figure 15] 13 is a flowchart showing an example of processing by a first estimating unit according to the fourth embodiment. [Figure 16] 13 is a flowchart showing an example of processing by a second estimating unit according to the fifth embodiment. [Figure 17] 13 is a flowchart showing an example of processing by a second estimating unit according to the sixth embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0014] First Embodiment The first embodiment will be described in detail below with reference to FIGS.

[0015] (Overall configuration of autonomous mobile body) FIG. 1 is a schematic diagram showing the overall configuration of an autonomous moving body according to the first embodiment. 1, an autonomous mobile body 1 according to this embodiment is, for example, an unmanned forklift that autonomously travels to a target point along a predetermined travel route within a travel area such as a logistics warehouse. Note that in other embodiments, the autonomous mobile body 1 may be an inspection robot used for plant inspections, etc.

[0016] The autonomous moving body 1 includes a self-position estimation device 10, a first sensor 20, and a second sensor .

[0017] The first sensor 20 is a camera that captures image data (each frame of video data) of the surroundings of the autonomous moving body 1.

[0018] The second sensor 21 is an inertial measurement unit (hereinafter also referred to as “IMU”) that measures the acceleration and angular velocity of the autonomous moving body 1.

[0019] The self-position estimation device 10 estimates the self-position of the autonomous moving body 1 based on image data (first sensor data) acquired by the first sensor 20 and the acceleration and angular velocity (second sensor data) measured by the second sensor 21.

[0020] (Functional configuration of the self-location estimation device) FIG. 2 is a block diagram showing the functional configuration of the self-location estimation device according to the first embodiment. As shown in FIG. 2, the self-location estimation device 10 includes a processor 11, a memory 12, a storage 13, and an interface 14.

[0021] The memory 12 has a memory area necessary for the operation of the processor 11 .

[0022] The storage 13 is a so-called auxiliary storage device, such as a hard disk drive (HDD) or a solid state drive (SSD).

[0023] The interface 14 is an interface for transmitting and receiving various types of information to and from external devices (for example, the first sensor 20 and the second sensor 21).

[0024] The processor 11 operates in accordance with a predetermined program to function as a first acquisition unit 110, a second acquisition unit 111, a map matching unit 112, a first estimation unit 113, a second estimation unit 114, and an output processing unit 115.

[0025] The first acquisition unit 110 acquires image data (first sensor data) from the first sensor 20 that can detect fixed objects that are always present around the autonomous moving body 1 and non-fixed objects that are temporarily present around the autonomous moving body 1. For example, fixed objects are structures whose arrangement or shape does not change within the travel area, such as ceilings, floors, shelves, signs, and paint. Non-fixed objects are luggage or pallets that are temporarily placed on floors, shelves, etc.

[0026] The second acquisition unit 111 acquires the acceleration and angular velocity of the autonomous moving body 1 from the second sensor 21 (second sensor data).

[0027] The map matching unit 112 matches the first sensor data with map data D1 pre-recorded in the storage 13 to estimate the self-position of the autonomous moving body 1. The self-position of the autonomous moving body 1 is expressed by parameters with six degrees of freedom, consisting of a position in each direction of the coordinate axes (Xw, Yw, Zw) of the world coordinate system and a rotation angle (attitude) around each coordinate axis.

[0028] The map data D1 is a collection of sample data in which first sensor data is collected in advance by test driving the autonomous mobile body 1 within a travel area, and the self-position of the autonomous mobile body 1 at the time each piece of first sensor data is attached to each piece of first sensor data. The map comparison unit 112 compares the first sensor data acquired by the first acquisition unit 110 with each piece of sample data in the map data D1, and estimates that the self-position attached to the sample data that matches the first sensor data is the self-position of the autonomous mobile body 1.

[0029] The first estimation unit 113 estimates a partial parameter, which is at least one of six-degree-of-freedom parameters that represent the position and orientation in a three-dimensional coordinate system, based on known information D2 previously recorded in the storage 13 and the first sensor data. The known information D2 is information that records in advance the position, angle, shape, etc., of a landmark, which is a fixed object previously selected from multiple fixed objects, in the travel area of ​​the autonomous mobile body 1. For example, the landmark is paint (such as a white line) applied to the floor of the travel area or a sign (such as a signboard) installed within the travel area. It is desirable that the landmark be a fixed object that can be observed at least in part even when there are non-fixed objects.

[0030] The second estimation unit 114 estimates the self-position of the autonomous moving body, which is expressed by parameters of six degrees of freedom, based on the first sensor data, the second sensor data, and the partial parameters estimated by the first estimation unit 113.

[0031] The output processing unit 115 outputs the self-position of the autonomous moving body 1 estimated by the map matching unit 112 or the second estimation unit 114 to a control device (not shown) that controls the operation of the autonomous moving body 1 or the like.

[0032] (Processing flow of the self-location estimation device) FIG. 3 is a flowchart illustrating an example of processing performed by the self-location estimation device according to the first embodiment. Hereinafter, the flow of processing in which the self-position estimation device 10 estimates the self-position of the autonomous moving body 1 will be described with reference to FIG.

[0033] First, the first acquisition unit 110 acquires first sensor data from the first sensor 20. Furthermore, the second acquisition unit 111 acquires second sensor data from the second sensor 21 (step S100). In this embodiment, the first sensor data is image data representing the latest frame of video captured by a camera, and the second sensor data is the acceleration and angular velocity of the autonomous moving body 1 measured by the IMU.

[0034] Next, the map matching unit 112 compares the first sensor data with the map data D1 to estimate the self-position of the autonomous moving body 1 (step S101). For example, the map matching unit 112 compares each sample data of the first sensor data acquired by the first acquisition unit 110 with the map data D1 using a known pattern matching process, and extracts the sample data that has the highest degree of match, at least a predetermined degree of match, as data that matches the first sensor data. The map matching unit 112 estimates that the self-position assigned to the extracted sample data is the self-position of the autonomous moving body 1 at the time the first sensor data was acquired.

[0035] It should be noted that, for example, due to luggage being temporarily placed on a shelf or floor, the scenery of the travel area when the map data D1 was generated may differ from the scenery of the travel area when the autonomous mobile body 1 actually travels. In this case, sample data that matches the first sensor data does not exist in the map data D1, and the self-position of the autonomous mobile body 1 cannot be estimated. For this reason, the self-position estimation device 10 determines whether estimation of the self-position of the autonomous mobile body 1 has been completed by map matching (step S102).

[0036] When the map matching unit 112 has completed estimation of the self-position by map matching (step S102; YES), the output processing unit 115 outputs the self-position estimated by the map matching unit 112 to a control device (not shown) of the autonomous moving body 1 (step S105). Furthermore, the self-position estimation device 10 returns to step S100 and executes the series of processes in FIG. 3 again.

[0037] On the other hand, if the map matching unit 112 is unable to estimate the self-position by map matching (step S102; NO), the first estimation unit 113 and the second estimation unit 114 proceed to the self-position estimation process.

[0038] First, the first estimation unit 113 performs a process of estimating at least one parameter among the six degrees of freedom parameters that represent the self-position of the autonomous mobile body 1 based on the landmark included in the first sensor data and the known information D2 (step S103). In this embodiment, a case will be described in which the landmark is a white line painted on the floor of the traveling area. Note that in other embodiments, paint painted on the wall or ceiling of the traveling area may be used as the landmark.

[0039] FIG. 4 is a flowchart illustrating an example of processing by the first estimating unit according to the first embodiment. The processing of the first estimation unit 113 will be described with reference to Fig. 4. First, the first estimation unit 113 performs a predetermined image conversion process on the first sensor data (step S110). This image conversion process is, for example, general distortion correction, conversion to a bird's-eye view, etc.

[0040] Next, the first estimation unit 113 performs a process of extracting white lines from the first sensor data after the image conversion process (step S111). For example, the first estimation unit 113 extracts white lines included in the first sensor data by further performing a binarization process on the first sensor data.

[0041] In addition, the first estimation unit 113 performs processing to estimate the position of the autonomous moving body 1 on the Xw axis and the rotation angle (attitude) around the Zw axis based on the white lines extracted from the first sensor data and the known information D2 (step S112).

[0042] FIG. 5 is a diagram illustrating the function of the self-position estimation device according to the first embodiment. As shown in Fig. 5, white lines M1 are provided on the floor of the travel area R. Even if a baggage B1 is temporarily placed on the floor or shelf of the travel area R, at least one of the white lines M1 (in the example of Fig. 5, the white line M1 on the right side of the autonomous moving body 1) is provided in a position where it is not hidden by the baggage B1 and can be observed by the first sensor 20. In the example of Fig. 5, the left-right direction of the autonomous moving body 1 (vehicle coordinate system) is represented by the Xv axis, the front-rear direction is represented by the Yv axis, and the vertical direction is represented by the Zv axis. Furthermore, the horizontal directions (e.g., east-west and north-south directions) of the travel area R (world coordinate system) are represented by the Xw axis and Yw axis, and the vertical direction is represented by the Zw axis.

[0043] The known information D2 includes information indicating the position and angle at which the white line M1 is provided within the travel area R, information indicating the width of the white line M1, etc. For example, the first estimation unit 113 identifies which travel lane the autonomous mobile body 1 will enter within the travel area R based on the previous self-position estimation result of the map matching unit 112, information on a predetermined travel route, etc., and extracts information regarding the white line M1 drawn on the travel lane to be entered from the known information D2. Furthermore, if travel lane identification information (such as a travel lane number or a barcode) is printed on the white line M1, the first estimation unit 113 may read this identification information from the first sensor data using known character recognition processing or barcode reading processing, etc., and extract information regarding the white line M1 identified by the identification information from the known information D2.

[0044] The first estimation unit 113 can estimate two-degree-of-freedom parameters (partial parameters) of the position Xw1 in the Xw-axis direction of the autonomous moving body 1 in the world coordinate system and the rotation angle θZw1 around the Zw-axis based on the information of the white line M1 extracted from the known information D2 and the appearance (size, inclination, etc.) of the white line M1 in the first sensor data (image data).

[0045] Next, returning to FIG. 3, the second estimation unit 114 executes a self-position estimation process using optimization calculations (step S104).

[0046] FIG. 6 is a flowchart illustrating an example of processing by the second estimating unit according to the first embodiment. The processing of the second estimating unit 114 will be described with reference to FIG.

[0047] First, the second estimation unit 114 sets the self-position (partial parameters) calculated by the first estimation unit 113 using the known information D2 (step S120). Note that the optimization calculation in self-position estimation is performed using a relative change in position and orientation (odometry) from the state immediately before (previous frame). For this reason, in this embodiment, the position Xw1 on the Xw axis and the rotation angle θZw1 around the Zw axis estimated by the first estimation unit 113 are converted into relative values ​​subtracted from Xw and θZw of the self-position estimation results for the previous frame, and then set.

[0048] Next, the second estimation unit 114 estimates relative values ​​from the previous frame by optimization calculation for the four-degree-of-freedom parameters, i.e., positions Yw1 and Zw1 in the Yw-axis and Zw-axis directions and rotation angles θXw1 and θYw1 around the Xw-axis and Yw-axis, excluding the two-degree-of-freedom parameters Xw1 and θZw1 estimated by the first estimation unit 113 (step S121). Specifically, the second estimation unit 114 obtains relative values ​​of the remaining four-degree-of-freedom parameters by solving the following equation (1) while fixing the relative values ​​of Xw1 and θZw1.

[0049]

number

[0050] In equation (1), χ is an estimate of the change in self-position (relative to the previous frame) from the previous frame f to the latest frame f+1. The first term in the curly brackets in equation (1) represents the residual between the change in position and orientation from the previous frame f to the latest frame f+1 and the IMU integrated value between them (IMU residual), and the second term represents the re-projection error on the image of the change in position and orientation from the previous frame f to the latest frame f+1 (image residual).

[0051] Next, the second estimation unit 114 calculates the self-position of the autonomous moving body 1 in the world coordinate system for the current frame based on the relative values ​​of each parameter calculated in steps S120 and S121 with respect to the previous frame and the self-position estimation result for the previous frame (step S122).

[0052] 3, the output processing unit 115 outputs the self-position (parameters of six degrees of freedom) estimated by the first estimation unit 113 and the second estimation unit 114 to a control device (not shown) of the autonomous moving body 1 (step S105). The self-position estimation device 10 also returns to step S100 and executes the series of processes in FIG. 3 again.

[0053] (Action, effect) As described above, the self-position estimation device 10 according to this embodiment includes a first acquisition unit 110 that acquires first sensor data, a second acquisition unit 111 that acquires second sensor data, a first estimation unit 113 that estimates partial parameters that are a part of the six-degree-of-freedom parameters that represent the self-position of the autonomous moving body 1 based on landmarks included in the first sensor data and known information D2, and a second estimation unit 114 that estimates the remaining parameters based on the first sensor data, the second sensor data, and the partial parameters estimated by the first estimation unit 113.

[0054] In this way, the self-position estimation device 10 can estimate some parameters by utilizing known information D2 that can be indirectly observed from the first sensor data, thereby suppressing cumulative errors in the optimization calculation and accurately estimating the self-position of the autonomous moving body 1.

[0055] Furthermore, the second estimating unit 114 estimates parameters other than the partial parameters estimated by the first estimating unit 113, out of the parameters with six degrees of freedom.

[0056] In this way, some parameters are known to the self-location estimation device 10, and only other parameters need to be calculated, so that the influence of accumulated errors can be more effectively suppressed. Also, the self-location estimation device 10 can perform optimization calculations at high speed and with low load.

[0057] The landmark is a white line M1 (painted) that has been applied in advance to the driving area R, and the first estimation unit 113 estimates, as partial parameters, the position Xw1 in the Xw-axis direction and the attitude θZw1 represented by rotation around the Zw-axis, among the six degrees of freedom parameters, based on the white line M1 extracted from the first sensor data and known information D2.

[0058] In this way, the self-location estimation device 10 can accurately estimate parameters of two of the six degrees of freedom from the white line M1 in the traveling area R. This allows the self-location estimation device 10 to more effectively suppress the influence of accumulated errors in estimating parameters of the other four degrees of freedom.

[0059] <Second embodiment> The second embodiment will be described in detail below with reference to Figures 7 and 8. Note that, among the configurations of the second embodiment, the same configurations as those of the first embodiment will be described using the same reference numerals as those of the first embodiment.

[0060] FIG. 7 is a diagram illustrating the functions of the self-position estimation device according to the second embodiment. In the above-described first embodiment, an example has been described in which the first estimation unit 113 of the self-position estimation device 10 estimates two parameters out of six degrees of freedom using a white line M1 in the travel area R as a landmark. In contrast, the first estimation unit 113 of the self-position estimation device 10 according to this embodiment estimates two parameters out of six degrees of freedom using a sign M2 (such as a signboard) installed in the travel area R as a landmark, as in the example of Fig. 8. The sign M2 is installed in a position where it can be observed by the first sensor 20 without being obscured by baggage B1 or the like.

[0061] (Processing flow of the self-location estimation device) FIG. 8 is a flowchart illustrating an example of processing by the first estimating unit according to the second embodiment. The first estimation unit 113 according to this embodiment executes the process of FIG. 8 instead of the process of the first embodiment (FIG. 4).

[0062] 8, the first estimation unit 113 performs a predetermined image conversion process on the first sensor data (step S130). This process is the same as step S110 in FIG.

[0063] Next, the first estimation unit 113 performs a process of detecting the sign M2 by performing known edge detection processing, pattern recognition processing, etc. on the first sensor data after the image conversion processing (step S131).

[0064] Furthermore, the first estimation unit 113 performs processing to estimate the position of the autonomous moving body 1 on the Yw axis and the rotation angle around the Zw axis based on the white lines extracted from the first sensor data and the known information D2 (step S132).

[0065] The known information D2 pre-records information indicating the position and angle at which the sign M2 is installed within the travel area R, information indicating the size of the sign M2, etc. As in the first embodiment, the first estimation unit 113 identifies which travel lane the autonomous mobile body 1 will enter within the travel area R based on the previous self-position estimation result of the map matching unit 112, information on a predetermined travel route, etc., and extracts information about the sign M2 installed on the travel lane to be entered from the known information D2. Furthermore, if travel lane identification information (such as a travel lane number or a barcode) is printed on the sign M2, the first estimation unit 113 may read this identification information from the first sensor data using known character recognition processing or barcode reading processing, etc., and extract information about the sign M2 identified by the identification information from the known information D2.

[0066] The first estimation unit 113 can estimate two-degree-of-freedom parameters (partial parameters) of the position Yw1 in the Yw-axis direction in the world coordinate system of the autonomous moving body 1 and the rotation angle θZw1 around the Zw-axis based on the information of the sign M2 extracted from the known information D2 and the appearance (size, inclination, etc.) of the sign M2 in the first sensor data (image data).

[0067] The subsequent processes (steps S104 to S105 in FIG. 3) are the same as those in the first embodiment.

[0068] (Action, effect) As described above, in this embodiment, the landmark is a sign M2 that has been installed in advance in the driving area R, and the first estimation unit 113 estimates, as partial parameters, the position Yw1 in the Yw-axis direction and the attitude θZw1 represented by rotation around the Zw-axis, among the six degrees of freedom parameters, based on the sign M2 extracted from the first sensor data and known information D2.

[0069] In this way, the self-location estimation device 10 can accurately estimate parameters of two of the six degrees of freedom from the sign M2 in the traveling area R. This allows the self-location estimation device 10 to more effectively suppress the influence of accumulated errors in estimating parameters of the other four degrees of freedom.

[0070] In another embodiment, the first estimation unit 113 of the self-position estimation device 10 may extract both the white line M1 and the sign M2 as landmarks and estimate two or three degrees of freedom parameters out of the six degrees of freedom parameters. For example, if the first estimation unit 113 detects only the white line M1 from the first sensor data, it estimates two degrees of freedom parameters, Xw1 and θZw1, as in the first embodiment. If the first estimation unit 113 detects only the sign M2 from the first sensor data, it estimates two degrees of freedom parameters, Yw1 and θZw1, as in the second embodiment. Furthermore, if the first estimation unit 113 detects both the white line M1 and the sign M2 from the first sensor data, it estimates three degrees of freedom parameters, Xw1, Yw1, and θZw1, based on these. This allows for flexible adaptation to various layouts of the traveling area R, since it is sufficient to install at least one of the white line M1 and the sign M2 according to the arrangement of shelves and the like within the traveling area R. Furthermore, if the first estimating unit 113 can estimate three-degree-of-freedom parameters from both the white line M1 and the sign M2, the influence of accumulated errors can be more effectively suppressed in the optimization calculations of the second estimating unit 114.

[0071] <Third embodiment> The third embodiment will be described in detail below with reference to Figures 9 to 13. Note that, among the configurations of the third embodiment, the same configurations as those of the first and second embodiments will be described using the same reference numerals as those of the first and second embodiments.

[0072] FIG. 9 is a first diagram illustrating the function of the self-location estimation device according to the third embodiment. FIG. 10 is a second diagram illustrating the function of the self-location estimation device according to the third embodiment. In the above-described first and second embodiments, an example has been described in which the first estimation unit 113 of the self-position estimation device 10 estimates two parameters out of six degrees of freedom using a white line M1 or a sign M2 in the traveling area R as a landmark. In contrast, the first estimation unit 113 of the self-position estimation device 10 according to this embodiment estimates three parameters out of six degrees of freedom using a landmark M3 attached to a fixed object in the traveling area R as a landmark, as shown in FIGS. 9 and 10 .

[0073] Fig. 9 is a view of the travel area R as seen from above in the vertical direction (+Zw), and Fig. 10 is a view of shelf B2 as seen from one side in the horizontal direction (+Xw). As shown in Figs. 9 and 10, landmark M3 is attached to the lower part (foot area) of the pillar of shelf B2 on which luggage B1 is placed so that it can be observed by first sensor 20 without being hidden by luggage B1 or the like.

[0074] In this embodiment, the first sensor 20 of the autonomous moving body 1 is a LiDAR (Light Detection and Ranging) sensor, and acquires point cloud data P of fixed objects and non-fixed objects around the autonomous moving body 1. In step S100 of FIG. 3, the first acquisition unit 110 of the self-position estimation device 10 acquires first sensor data, which is point cloud data, from the first sensor 20.

[0075] (Processing flow of the self-location estimation device) FIG. 11 is a flowchart illustrating an example of processing by the first estimating unit according to the third embodiment. The first estimation unit 113 according to this embodiment executes the process of FIG. 11 instead of the process of the first embodiment (FIG. 4) or the second embodiment (FIG. 8).

[0076] 11, the first estimation unit 113 performs a predetermined conversion process on the first sensor data (point cloud data P) (step S140). This conversion process is, for example, general distortion correction, conversion to a world coordinate system, or the like.

[0077] Next, the first estimation unit 113 performs a process of extracting a landmark M3 from the first sensor data after the conversion process (step S141).

[0078] For example, the known information D2 pre-records information indicating the position and angle at which each landmark M3 is installed within the driving area R, as well as information indicating the shape and size of each landmark M3. The known information D2 also records the position of an area (existence area A) that includes each landmark M3. As shown in FIG. 9, for example, the existence area A is a rectangular virtual area centered on each landmark M3, and is set to be larger than the horizontal size of the landmark M3 by a predetermined margin. Furthermore, if multiple landmarks M3 are installed adjacent to each other, the existence area A may be set to include these adjacent landmarks M3.

[0079] Based on the previous self-position estimation result and known information D2, the first estimation unit 113 identifies an area corresponding to the existence area A of the landmark M3 from the first sensor data, and extracts the point cloud data P contained within this existence area A as the point cloud data P of the landmark M3.

[0080] Furthermore, the first estimation unit 113 performs a process of calculating a representative point RP of the landmark M3 based on the extracted point cloud data (step S142).

[0081] FIG. 12 is a third diagram illustrating the function of the self-location estimation device according to the third embodiment. 12 shows an example in which the shape of the landmark M3 is a rectangular parallelepiped. In this case, the first estimation unit 113 fits two lines expressed by the following equations (2) and (3) to the point cloud, and extracts the intersection (corner) of these lines as the representative point RP of the landmark M3.

[0082]

number

[0083]

number

[0084] FIG. 13 is a third diagram illustrating the function of the self-location estimation device according to the third embodiment. 13 shows an example in which the shape of the landmark M3 is a cylinder. In this case, the first estimation unit 113 fits the circle equation expressed by the following formula (4) to the point group, and extracts the center (a, b) of the circle that minimizes the error as the representative point RP of the landmark M3.

[0085]

number

[0086] Next, for each landmark M3 extracted from the first sensor data, the first estimation unit 113 estimates three-degree-of-freedom parameters, namely, the positions Xw1, Yw1 of the autonomous moving body 1 in the Xw-axis direction and the Yw-axis direction, and the rotation angle θZw1 around the Zw-axis, based on the position coordinates of the representative point RP in the vehicle coordinate system calculated in step S143 and the position coordinates of the representative point of the landmark M3 in the world coordinate system recorded in the known information D2 (step S143).

[0087] Note that landmarks M3 of different shapes or landmarks M3 made of materials with different reflectance may be placed for each travel path or shelf B2 in the travel area R. In this case, the first estimation unit 113 reads information about the landmark M3 from the known information D2 that matches the shape or reflectance detected from the point cloud data P, and estimates the parameters. This allows the first estimation unit 113 to narrow down the travel path of the autonomous mobile body 1 and which shelf B2 it is traveling near, and then accurately estimate the parameters of the three degrees of freedom.

[0088] The subsequent processes (steps S104 to S105 in FIG. 3) are the same as those in the first embodiment.

[0089] (Action, effect) As described above, in the self-position estimation device 10 according to this embodiment, the first acquisition unit 110 acquires first sensor data, which is point cloud data P, from the first sensor 20. The mark is the landmark M3 attached to a fixed object (the bottom of the shelf B2), and the first estimation unit 113 estimates, as partial parameters, positions Xw1 and Yw1 in the Xw-axis direction and the Yw-axis direction and an attitude θZw1 represented by a rotation around the Zw-axis, among the parameters of the six degrees of freedom, based on the landmark M3 extracted from the first sensor data and known information D2.

[0090] In this way, the self-location estimation device 10 can estimate some of the six-degree-of-freedom parameters from the point cloud data P and the known information D2, even for an autonomous moving body 1 equipped with a LiDAR as the first sensor 20. Furthermore, the first estimation unit 113 can more accurately estimate the three-degree-of-freedom parameters based on the positional relationship and angle with respect to multiple landmarks M3 detected from the first sensor data. This allows the self-location estimation device 10 to more accurately estimate the self-location of the autonomous moving body 1.

[0091] <Fourth embodiment> The fourth embodiment will be described in detail below with reference to Figures 14 and 15. Note that, among the configurations of the fourth embodiment, the same configurations as those of the first to third embodiments will be described using the same reference numerals as those of the first to third embodiments.

[0092] FIG. 14 is a diagram illustrating the function of the self-position estimation device according to the fourth embodiment. In the third embodiment, an example was described in which the first estimation unit 113 of the self-position estimation device 10 determines the three-degree-of-freedom parameters (Xw1, Yw1, θZw1) of the autonomous moving body 1 based on the representative points RP of each landmark M3. In contrast, in the present embodiment, the first estimation unit 113 estimates the two-degree-of-freedom parameters (Xw1, θZw1) of the autonomous moving body 1 by arranging a plurality of landmarks M3, for example, in a line on a straight line and comparing a shape (straight line L) obtained by connecting point cloud data of the plurality of landmarks M3 with a virtual line, as shown in Fig. 14, with a shape obtained from the actual arrangement of the landmarks M3.

[0093] (Processing flow of the self-location estimation device) FIG. 15 is a flowchart illustrating an example of processing by the first estimating unit according to the fourth embodiment. The first estimation unit 113 according to this embodiment executes the process of FIG. 15 instead of the process of the third embodiment (FIG. 11).

[0094] 15, the first estimation unit 113 performs a predetermined conversion process on the first sensor data (point cloud data P) (step S150). The first estimation unit 113 also performs a process of extracting landmarks M3 from the first sensor data after the conversion process (step S151). These processes are the same as steps S140 to S141 in FIG.

[0095] Next, the first estimation unit 113 fits a straight line to the point cloud data P of the extracted landmark M3, and estimates the position Xw1 of the autonomous moving body 1 on the Xw axis and the rotation angle θZw1 around the Zw axis based on the distance and angle to this line L and the line obtained from the arrangement of the landmark M3 recorded in the known information D2 (step S152).

[0096] The subsequent processes (steps S104 to S105 in FIG. 3) are the same as those in the first embodiment.

[0097] 14 shows an example in which a plurality of landmarks M3 are arranged in a line, but this is not limiting. In other embodiments, the landmarks M3 may be arranged in other shapes, such as an L-shape.

[0098] (Action, effect) As described above, in the self-position estimation device 10 according to this embodiment, the first acquisition unit 110 estimates two parameters of freedom, namely, the position Xw1 in the Xw-axis direction and the rotation angle θZw1 around the Zw-axis, based on the shape obtained by connecting multiple landmarks M3 extracted from the first sensor data and the shape obtained from the arrangement of the landmarks M3 recorded in the known information D2.

[0099] In this way, the self-location estimation device 10 can more robustly estimate the self-location from a combination of a plurality of landmarks M3.

[0100] <Fifth embodiment> The fifth embodiment will be described in detail below with reference to Fig. 16. Note that, among the configurations of the fifth embodiment, the same configurations as those of the first to fourth embodiments will be described using the same reference numerals as those of the first to fourth embodiments.

[0101] In each of the above-described embodiments, an example has been described in which the second estimation unit 114 of the self-location estimation device 10 estimates only the parameters of the six degrees of freedom other than the partial parameters estimated by the first estimation unit 113. In contrast, the second estimation unit 114 according to this embodiment estimates all of the parameters of the six degrees of freedom by performing optimization calculations that add constraints based on the partial parameters estimated by the first estimation unit 113.

[0102] (Processing flow of the self-location estimation device) FIG. 16 is a flowchart illustrating an example of processing by a second estimating unit according to the fifth embodiment. The second estimating unit 114 according to this embodiment executes the process of Fig. 16 instead of the process of the first embodiment (Fig. 6). Note that the method by which the first estimating unit 113 estimates the partial parameters may be any of the methods of the first to fourth embodiments.

[0103] First, the second estimation unit 114 sets the partial parameters calculated by the first estimation unit 113 using the known information D2 as constraint terms (step S160). Here, as in the first embodiment, the relative values ​​between the previous frame f and the current frame f+1 are set.

[0104] Next, the second estimation unit 114 performs optimization calculations that add constraint terms to the partial parameters estimated by the first estimation unit 113, and estimates relative values ​​of all parameters with six degrees of freedom relative to the previous frame (step S161). Specifically, the second estimation unit 114 finds relative values ​​of all parameters with six degrees of freedom by solving the following equation (5) instead of equation (1) in the first embodiment.

[0105]

number

[0106] In equation (5), the third term in curly brackets represents the difference between the self-position χ obtained by optimization calculation and the self-position m (partial parameter) estimated by the first estimation unit 113 from the known information D2, as shown in the following equation (6). In equation (6), a represents a weighting coefficient, p represents position coordinates (x, y, z), and r represents attitude (θx, θy, θz). The weighting coefficient a is a value determined by parameter tuning before operating the autonomous mobile body 1.

[0107]

number

[0108] According to equation (6), the greater the difference between the self-position χ estimated by the optimization calculation and the self-position m estimated by the first estimation unit 113, the greater the value of the third term in equation (5).

[0109] In this way, by adding the parameters estimated by the first estimation unit 113 as constraint terms during optimization calculations rather than simply adopting them as they are, it is possible to reduce the overall cumulative error while mitigating the observation error of the first sensor data.

[0110] Furthermore, for example, as in the first embodiment, when the first estimation unit 113 estimates the self-position m (partial parameter) based on the white line M1, there is a possibility that the self-position m includes an error in the white line M1 detected from the image. For this reason, the second estimation unit 114 may change the value of the weighting coefficient α using the reliability of the first estimation unit 113's detection of the white line M1 through image processing. For example, the higher the detection reliability of the white line M1, the higher the accuracy of the estimation result of the self-position m by the first estimation unit 113 should be. Therefore, the second estimation unit 114 increases the value of the weighting coefficient α as the reliability of the detection of the white line M1 increases. By changing the weighting coefficient α in this way, an optimization calculation can be performed such that the difference from the self-position m decreases as the estimation accuracy of the self-position m by the first estimation unit 113 increases, and conversely, the self-position m is less susceptible to the influence of an error in the self-position m as the estimation accuracy of the self-position m by the first estimation unit 113 decreases.

[0111] Next, the second estimation unit 114 calculates the self-position of the autonomous moving body 1 in the world coordinate system for the current frame based on the relative values ​​of each parameter with respect to the previous frame obtained in step S161 and the self-position estimation result for the previous frame (step S162).

[0112] The subsequent processing (step S105 in FIG. 3) is the same as in the first embodiment.

[0113] (Action, effect) As described above, in the self-position estimation device 10 according to this embodiment, the second estimation unit 114 performs optimization calculations with constraints based on the partial parameters estimated by the first estimation unit 113, and estimates all parameters of the six degrees of freedom.

[0114] In this way, the self-location estimation device 10 can estimate its own location with higher accuracy by obtaining an estimated value that is consistent with other parameters through optimization calculations, while still based on the parameters estimated by the first estimation unit 113. Furthermore, the self-location estimation device 10 imposes constraints on some parameters, thereby improving the processing speed of the optimization calculations.

[0115] Sixth Embodiment The sixth embodiment will be described in detail below with reference to Fig. 17. Note that, among the configurations of the sixth embodiment, the same configurations as those of the first to fifth embodiments will be described using the same reference numerals as those of the first to fifth embodiments.

[0116] In the self-position estimation device 10 according to this embodiment, the second estimation unit 114 performs optimization calculations that add constraints based on the motion constraints of the autonomous moving body 1, and estimates all parameters of the six degrees of freedom.

[0117] (Processing flow of the self-location estimation device) FIG. 17 is a flowchart illustrating an example of processing by a second estimating unit according to the sixth embodiment. The second estimating unit 114 according to this embodiment executes the process of Fig. 17 instead of the process of the fifth embodiment (Fig. 16). Note that the method by which the first estimating unit 113 estimates the partial parameters may be any of the methods of the first to fourth embodiments.

[0118] First, the second estimating unit 114 sets the partial parameters calculated by the first estimating unit 113 using the known information D2 as constraint terms for the optimization calculation (step S170). This process is the same as step S160 in FIG.

[0119] Furthermore, the second estimation unit 114 sets the vehicle motion constraint D3 recorded in advance in the storage 13 of the autonomous moving body 1 as a constraint term for the optimization calculation (step S171).

[0120] For example, if the autonomous mobile body 1 is an unmanned forklift truck operated in a logistics warehouse, it cannot perform any motion that causes the body to rotate more than a predetermined angle in the forward / backward direction (rotation around the Xw axis) or left / right direction (rotation around the Yw axis) (such as one side of the body sinking into the floor or floating in the air). The vehicle movement constraint D3 defines the range of positions and orientations that the autonomous mobile body 1 can take, based on the body characteristics of the autonomous mobile body 1, the operating conditions of the autonomous mobile body 1, and positions to which the autonomous mobile body 1 cannot move (for example, areas with shelves).

[0121] Next, the second estimation unit 114 performs optimization calculations that add the constraint terms of the partial parameters estimated by the first estimation unit 113 and the constraint terms of the vehicle motion constraint D3, and estimates relative values ​​from the previous frame for all parameters with six degrees of freedom (step S172). Specifically, the second estimation unit 114 solves the above equation (5) to find relative values ​​for all parameters with six degrees of freedom. In this embodiment, the third term of equation (5) is expressed by the following equation (7) instead of the above equation (6).

[0122]

number

[0123] In equation (7), a represents a weighting coefficient, m1 represents the self-position (partial parameter) estimated by the first estimation unit 113, m2 represents the self-position determined from the vehicle movement constraint D3, p represents the position coordinates (x, y, z), and r represents the attitude (θx, θy, θz).

[0124] The second estimation unit 114 may also estimate the amount of movement in the vehicle's traveling direction based on measurement data from an encoder (not shown) that measures the number of wheel rotations of the autonomous moving body 1, and add this to the constraints.

[0125] According to equation (7), the value of the third term in equation (5) increases as the difference between the self-position χ estimated by the optimization calculation and the self-position m2 determined from the vehicle motion constraint D3 increases.

[0126] Next, the second estimation unit 114 calculates the self-position of the autonomous moving body 1 in the world coordinate system for the current frame based on the relative values ​​of each parameter with respect to the previous frame obtained in step S172 and the self-position estimation result for the previous frame (step S173).

[0127] The subsequent processing (step S105 in FIG. 3) is the same as in the first embodiment.

[0128] (Action, effect) As described above, in the self-position estimation device 10 according to this embodiment, the second estimation unit 114 performs optimization calculations that further add motion constraints on the autonomous moving body 1, and estimates all parameters of the six degrees of freedom.

[0129] In this way, the self-position estimation device 10 can eliminate positions and orientations that cannot occur as the autonomous moving body 1, thereby further suppressing the accumulated error.

[0130] Although one embodiment has been described in detail above with reference to the drawings, the specific configuration is not limited to the above, and various design modifications are possible. That is, in other embodiments, the order of the above-described processes may be changed as appropriate. Furthermore, some processes may be executed in parallel.

[0131] The self-location estimation device 10 according to the above-described embodiment may be configured by a single computer, or the configuration of the self-location estimation device 10 may be divided among multiple computers, and the multiple computers may cooperate with each other to function as the self-location estimation device 10. In this case, some of the computers configuring the self-location estimation device 10 may be installed inside the autonomous moving body 1, and other computers may be provided outside the autonomous moving body 1.

[0132] Furthermore, in the first and second embodiments, an example in which the first sensor 20 is a camera has been described, but the present invention is not limited to this. In the configurations of the first and second embodiments, a LiDAR may be used as the first sensor 20. Similarly, in the third and fourth embodiments, an example in which the first sensor 20 is a LiDAR has been described, but the present invention is not limited to this. In the configurations of the third and fourth embodiments, a camera may be used as the first sensor 20.

[0133] <Additional Notes> The above-described embodiment can be understood, for example, as follows.

[0134] (1) According to a first aspect, a self-position estimation device 10 that estimates the self-position of an autonomous mobile body 1 in a travel area R includes a first acquisition unit 110 that acquires first sensor data capable of detecting fixed objects that are always present around the autonomous mobile body 1 and non-fixed objects that are temporarily present around the autonomous mobile body 1; a second acquisition unit 111 that acquires second sensor data including the acceleration and angular velocity of the autonomous mobile body 1; known information D2 that records information including the position in the travel area R of landmarks M1, M2, and M3, which are fixed objects pre-selected from a plurality of fixed objects; a first estimation unit 113 that estimates partial parameters that are part of the six-degree-of-freedom parameters that represent the position and attitude of the autonomous mobile body 1 in a three-dimensional coordinate system based on the first sensor data; and a second estimation unit 114 that estimates the self-position of the autonomous mobile body 1, which is expressed by the six-degree-of-freedom parameters, based on the first sensor data, the second sensor data, and the partial parameters.

[0135] In this way, the self-position estimation device 10 can estimate some parameters by utilizing known information D2 that can be indirectly observed from the first sensor data, thereby suppressing cumulative errors in the optimization calculation and accurately estimating the self-position of the autonomous moving body 1.

[0136] (2) According to the second aspect, in the self-position estimation device 10 according to the first aspect, the second estimation unit 114 estimates parameters other than the partial parameters among the parameters of the six degrees of freedom.

[0137] In this way, some parameters are known to the self-location estimation device 10, and only other parameters need to be calculated, so that the influence of accumulated errors can be more effectively suppressed. Also, the self-location estimation device 10 can perform optimization calculations at high speed and with low load.

[0138] (3) According to the third aspect, in the self-location estimation device 10 according to the first aspect, the second estimation unit 114 performs optimization calculations with constraints based on partial parameters to estimate all parameters of the six degrees of freedom.

[0139] In this way, the self-location estimation device 10 can estimate its own location with higher accuracy by obtaining an estimated value that is consistent with other parameters through optimization calculations, while still based on the parameters estimated by the first estimation unit 113. Furthermore, the self-location estimation device 10 imposes constraints on some parameters, thereby improving the processing speed of the optimization calculations.

[0140] (4) According to the fourth aspect, in the self-position estimation device 10 according to the third aspect, the second estimation unit 114 performs an optimization calculation that further adds motion constraints on the autonomous moving body 1, and estimates all parameters of the six degrees of freedom.

[0141] In this way, the self-position estimation device 10 can eliminate positions and orientations that cannot occur as the autonomous moving body 1, thereby further suppressing the accumulated error.

[0142] (5) According to the fifth aspect, in the self-position estimation device 10 relating to any one of the first to fourth aspects, the landmark is at least one of paint M1 applied to the driving area R and a sign M2 installed in the driving area R, and the first estimation unit 113 estimates, as partial parameters, the position in at least one of the first axis direction and the second axis direction extending horizontally, and the attitude represented by rotation around the third axis extending vertically, among the six degrees of freedom parameters, based on at least one of the paint M1 and the sign M2 extracted from the first sensor data and known information D2.

[0143] In this way, the self-position estimation device 10 can accurately estimate parameters of two of the six degrees of freedom from the paint M1 or sign M2 in the travel area R. Furthermore, the self-localization estimation device 10 can accurately estimate parameters for three of the six degrees of freedom from the combination of the paint M1 and the marker M2. This allows the self-localization estimation device 10 to more effectively suppress the influence of cumulative errors in estimating parameters for the remaining four or three degrees of freedom.

[0144] (6) According to the sixth aspect, in the self-position estimation device 10 relating to any one of the first to fourth aspects, the mark is a landmark M3 attached to a part of a fixed object, and the first estimation unit 113 estimates, as partial parameters, the positions in the first axis direction and the second axis direction extending horizontally and the attitude represented by the rotation around the third axis extending vertically, among the six degrees of freedom parameters, based on the landmark M3 extracted from the first sensor data and the known information D2.

[0145] In this way, the first estimation unit 113 of the self-location estimation device 10 can more accurately estimate the parameters of the three degrees of freedom based on the positional relationship and angle with respect to the plurality of landmarks M3 detected from the first sensor data. This allows the self-location estimation device 10 to more accurately estimate the self-location of the autonomous moving body 1.

[0146] (7) According to the seventh aspect, in the self-position estimation device 10 relating to the sixth aspect, the first estimation unit 113 estimates, as partial parameters, the position in the first axis direction extending horizontally and the attitude represented by rotation around the third axis extending vertically, among the six degrees of freedom parameters, based on the shape obtained by connecting multiple landmarks M3 extracted from the first sensor data and the shape obtained from the arrangement of the landmarks M3 recorded in the known information D2.

[0147] In this way, the self-location estimation device 10 can more robustly estimate the self-location from a combination of a plurality of landmarks M3.

[0148] (8) According to the eighth aspect, a self-position estimation method for estimating the self-position of an autonomous mobile body 1 in a travel area R includes the steps of: acquiring first sensor data capable of detecting fixed objects that are always present around the autonomous mobile body 1 and non-fixed objects that are temporarily present around the autonomous mobile body 1; acquiring second sensor data including the acceleration and angular velocity of the autonomous mobile body 1; estimating partial parameters, which are some of the six-degree-of-freedom parameters that represent the position and attitude of the autonomous mobile body 1 in a three-dimensional coordinate system, based on known information that records information including the position in the travel area R of landmarks M1, M2, and M3, which are fixed objects pre-selected from a plurality of fixed objects, and the first sensor data; and estimating the self-position of the autonomous mobile body 1, which is represented by the six-degree-of-freedom parameters, based on the first sensor data, the second sensor data, and the partial parameters.

[0149] (9) According to the ninth aspect, the program causes a self-position estimation device 10, which estimates the self-position of an autonomous mobile body 1 in a travel area, to execute the following steps: acquiring first sensor data capable of detecting fixed objects that are always present around the autonomous mobile body 1 and non-fixed objects that are temporarily present; acquiring second sensor data including the acceleration and angular velocity of the autonomous mobile body 1; estimating partial parameters, which are some of the six-degree-of-freedom parameters that represent the position and attitude of the autonomous mobile body 1 in a three-dimensional coordinate system, based on known information D2 that records information including the position in the travel area R of landmarks M1, M2, and M3, which are fixed objects pre-selected from a plurality of fixed objects, and the first sensor data; and estimating the self-position of the autonomous mobile body 1, which is represented by the six-degree-of-freedom parameters, based on the first sensor data, the second sensor data, and the partial parameters. [Explanation of symbols]

[0150] 1 Autonomous Mobile Vehicle 10 Self-position estimation device 11 processors 110 First acquisition part 111 Second Acquisition Department 112 Map matching unit 113 1st estimation part 114 Second estimation part 115 Output Processing Unit 12 Memory 13. Storage 14 Interface 20 First sensor 21 Second sensor

Claims

1. A self-position estimation device that estimates a self-position of an autonomous moving body in a travel area, a first acquisition unit that acquires first sensor data capable of detecting fixed objects that are always present around the autonomous moving body and non-fixed objects that are temporarily present around the autonomous moving body; a second acquisition unit that acquires second sensor data including an acceleration and an angular velocity of the autonomous moving body; a map matching unit that matches the first sensor data with map data of the traveling area and estimates a self-position of the autonomous moving body using six-degree-of-freedom parameters that represent a position and an attitude of the autonomous moving body in a three-dimensional coordinate system; a first estimation unit that, when the map matching unit is unable to estimate the self-position of the autonomous moving body, estimates partial parameters that are part of the six degrees of freedom parameters based on known information that records information including a position in the traveling area of ​​a landmark that is a fixed object pre-selected from the plurality of fixed objects, and the first sensor data; a second estimation unit that estimates, based on the first sensor data, the second sensor data, and the partial parameters estimated by the first estimation unit, parameters other than the partial parameters estimated by the first estimation unit among the six degrees of freedom parameters, thereby estimating a self-position of the autonomous moving body represented by the six degrees of freedom parameters; A self-location estimation device comprising:

2. the second estimation unit performs optimization calculations with constraints based on the partial parameters to estimate all of the parameters of the six degrees of freedom. The self-location estimation device according to claim 1 .

3. the second estimation unit performs optimization calculations to which motion constraints of the autonomous moving body are further added, and estimates all of the parameters of the six degrees of freedom. The self-position estimation device according to claim 2 .

4. the mark is at least one of paint applied to the travel area and a sign installed in the travel area; the first estimation unit estimates, as the partial parameters, a position in at least one of a first axis direction and a second axis direction extending horizontally and an attitude represented by a rotation about a third axis extending vertically, among the six degrees of freedom parameters, based on at least one of the paint and the sign extracted from the first sensor data and the known information; The self-position estimation device according to claim 1 .

5. the mark is a landmark attached to a part of the fixed object, the first estimation unit estimates, as the partial parameters, positions in a first axis direction and a second axis direction extending horizontally and an attitude represented by a rotation about a third axis extending vertically, among the six degrees of freedom parameters, based on the landmarks extracted from the first sensor data and the known information; The self-position estimation device according to claim 1 .

6. the first estimation unit estimates, as the partial parameters, a position in a first axis direction extending horizontally and an attitude represented by a rotation about a third axis extending vertically, among the six degrees of freedom parameters, based on a shape obtained by connecting the plurality of landmarks extracted from the first sensor data and a shape obtained from an arrangement of the landmarks recorded in the known information; The self-position estimation device according to claim 5 .

7. A self-position estimation method for estimating a self-position of an autonomous moving body in a travel area, comprising: acquiring first sensor data capable of detecting fixed objects that are always present around the autonomous moving body and non-fixed objects that are temporarily present around the autonomous moving body; acquiring second sensor data including an acceleration and an angular velocity of the autonomous moving body; a step of collating the first sensor data with map data of the traveling area and estimating a self-position of the autonomous moving body using six-degree-of-freedom parameters that represent a position and an attitude of the autonomous moving body in a three-dimensional coordinate system; a step of estimating partial parameters that are a part of the six degrees of freedom parameters based on known information that records information including a position in the traveling area of ​​a landmark that is one fixed object pre-selected from the plurality of fixed objects, when the self-position of the autonomous moving body cannot be estimated in the step of estimating the self-position; a step of estimating a self-position of the autonomous moving body represented by the parameters of six degrees of freedom by estimating parameters other than the partial parameters among the parameters of six degrees of freedom based on the first sensor data, the second sensor data, and the partial parameters; A self-location estimation method comprising:

8. A self-position estimation device that estimates a self-position of an autonomous moving body in a travel area, acquiring first sensor data capable of detecting fixed objects that are always present around the autonomous moving body and non-fixed objects that are temporarily present around the autonomous moving body; acquiring second sensor data including an acceleration and an angular velocity of the autonomous moving body; a step of collating the first sensor data with map data of the traveling area and estimating a self-position of the autonomous moving body using six-degree-of-freedom parameters that represent a position and an attitude of the autonomous moving body in a three-dimensional coordinate system; a step of estimating partial parameters that are a part of the six degrees of freedom parameters based on known information that records information including a position in the traveling area of ​​a landmark that is one fixed object pre-selected from the plurality of fixed objects, when the self-position of the autonomous moving body cannot be estimated in the step of estimating the self-position; a step of estimating a self-position of the autonomous moving body represented by the parameters of six degrees of freedom by estimating parameters other than the partial parameters among the parameters of six degrees of freedom based on the first sensor data, the second sensor data, and the partial parameters; A program that executes the following.

9. A self-position estimation device that estimates the self-position of an autonomous moving body in a travel area, a first acquisition unit that acquires first sensor data capable of detecting fixed objects that are always present around the autonomous moving body and non-fixed objects that are temporarily present around the autonomous moving body; a second acquisition unit that acquires second sensor data including an acceleration and an angular velocity of the autonomous moving body; a first estimation unit that estimates partial parameters that are part of six-degree-of-freedom parameters that represent the position and attitude of the autonomous moving body in a three-dimensional coordinate system based on known information that records information including the position in the traveling area of ​​a landmark that is a fixed object pre-selected from the plurality of fixed objects and the first sensor data; a second estimation unit that estimates a self-position of the autonomous moving body expressed by parameters of six degrees of freedom based on the first sensor data, the second sensor data, and the partial parameters; Equipped with the mark is a landmark attached to a part of the fixed object, the first estimation unit estimates, as the partial parameters, a position in a first axis direction extending horizontally and an attitude represented by a rotation about a third axis extending vertically, among the six degrees of freedom parameters, based on a shape obtained by connecting the plurality of landmarks extracted from the first sensor data and a shape obtained from an arrangement of the landmarks recorded in the known information; Self-location estimation device.

Images