Estimation device, estimation program, and estimation data generation method
By integrating dead reckoning with marker observation data and correcting for marker position deviations, the method enhances the robustness and accuracy of self-position estimation for autonomous mobile devices, addressing fluctuations in external conditions.
Patent Information
- Application Number
- JP2023080340
- Authority / Receiving Office
- JP · JP
- Patent Type
- Applications
- Current Assignee / Owner
- Filing Date
- 2023-05-15
- Publication Date
- 2024-11-27
- Estimated Expiration
- 2043-05-15
AI Technical Summary
Existing self-position estimation techniques for autonomous mobile devices lack robustness due to fluctuations in estimation accuracy caused by changes in external conditions such as weather, making it difficult to ensure consistent accuracy.
A method that combines dead reckoning based on internal world data with marker observation data to estimate the self-position, correcting the main estimated position using the deviation between predicted and actual marker positions, and fusing these estimates based on the ratio of their errors to enhance accuracy.
This approach ensures robustness in self-position estimation by offsetting the influence of estimation errors, allowing for high-accuracy and reliable position estimation data generation.
Smart Images

Figure 2024164679000001_ABST
Abstract
Description
[Technical field]
[0001] The present disclosure relates to an estimation technique for estimating the self-position of an autonomous driving device that autonomously drives in the outside world. [Background technology]
[0002] The estimation technique disclosed in Patent Document 1 switches the estimation method for estimating the self-position of the autonomous driving device according to the estimation accuracy for each area that is simulated in advance. Specifically, in the first area, the self-position is estimated using point cloud data obtained by scanning the outside world from the autonomous driving device and map data of the outside world. On the other hand, in the second area, the self-position is estimated using the detection results from the autonomous driving device of multiple guides installed in the outside world and the position information of the guides in the outside world. [Prior art documents] [Patent documents]
[0003] [Patent Document 1] JP 2022-110773 A Summary of the Invention [Problem to be solved by the invention]
[0004] However, in the estimation technology disclosed in Patent Document 1, the estimation accuracy of each estimation method in each region varies from moment to moment due to changes in external conditions such as weather, etc. Therefore, it is difficult to say that true estimation accuracy can be ensured by simply switching the estimation method, and it can also be said that it lacks robustness.
[0005] An object of the present disclosure is to provide an estimation device that ensures robustness with respect to the estimation accuracy of the self-location. Another object of the present disclosure is to provide an estimation program that ensures robustness with respect to the estimation accuracy of the self-location. Yet another object of the present disclosure is to provide an estimation data generation method that ensures robustness with respect to the estimation accuracy of the self-location. [Means for solving the problem]
[0006] The technical means of the present disclosure for solving the problems will be described below. Note that the claims and the reference characters in parentheses in this section indicate the corresponding relationship with the specific means described in the embodiments described later in detail, and do not limit the technical scope of the present disclosure.
[0007] A first aspect of the present disclosure is An estimation device (1) having a processor (12) and estimating a self-position of an autonomous mobile device (4) that autonomously travels in an external world, The processor Obtaining a main estimated position (Pd) as a self-position by dead reckoning based on internal data (Di) acquired in the internal world of the autonomous driving device; Obtaining a sub-estimated position (Pm1) as a self-position by marker matching based on marker observation data (Do) obtained by observing a plurality of route markers (6) installed in the external world from an autonomous mobile device and marker position data (Dmp) that defines marker installation positions (Pmp) that are the setting positions of each route marker in the external world; and generating position estimation data (Df) of the self-position by weighting the main estimated position and the sub estimated position based on a ratio of the estimation errors of the main estimated position and the sub estimated position by fusion. To obtain the sub-estimated position, This includes obtaining, as a sub-estimated position, the main estimated position corrected by the deviation (δP) between a marker predicted position (Pp) predicted from the marker observation data starting from the main estimated position, and a marker installation position that matches the predicted marker position in the marker position data.
[0008] A second aspect of the present disclosure is An estimation program that is stored in a storage medium (10) for estimating a self-position of an autonomous mobile device (4) that autonomously travels in an external world, and includes instructions to be executed by a processor (12), Obtaining a main estimated position (Pd) as a self-position by dead reckoning based on internal data (Di) acquired in the internal world of the autonomous driving device; Obtaining a sub-estimated position (Pm1) as a self-position by marker matching based on marker observation data (Do) obtained by observing a plurality of route markers (6) installed in the external world from an autonomous mobile device and marker position data (Dmp) that defines marker installation positions (Pmp) that are the setting positions of each route marker in the external world; and generating position estimation data (Df) of the self-position by weighting the main estimated position and the sub estimated position based on a ratio of the estimation errors of the main estimated position and the sub estimated position by fusion. To obtain the sub-estimated position, This includes obtaining, as a sub-estimated position, the main estimated position corrected by the deviation (δP) between a marker predicted position (Pp) predicted from the marker observation data starting from the main estimated position, and a marker installation position that matches the predicted marker position in the marker position data.
[0009] A third aspect of the present disclosure is An estimated data generation method executed by a processor (12) to generate position estimation data (Df) that estimates a self-position of an autonomous mobile device (4) that autonomously travels in an external world, comprising: Obtaining a main estimated position (Pd) as a self-position by dead reckoning based on internal data (Di) acquired in the internal world of the autonomous driving device; Obtaining a sub-estimated position (Pm1) as a self-position by marker matching based on marker observation data (Do) obtained by observing a plurality of route markers (6) installed in the external world from an autonomous mobile device and marker position data (Dmp) that defines marker installation positions (Pmp) that are the setting positions of each route marker in the external world; and generating position estimation data by weighting the main estimated position and the sub estimated position based on a ratio of their respective estimation errors, To obtain the sub-estimated position, This includes obtaining, as a sub-estimated position, the main estimated position corrected by the deviation (δP) between a marker predicted position (Pp) predicted from the marker observation data starting from the main estimated position, and a marker installation position that matches the predicted marker position in the marker position data.
[0010] According to the first to third aspects, a main estimated position is obtained by dead reckoning based on internal data, and a sub-estimated position is obtained by marker matching based on marker observation data and marker position data. Here, the sub-estimated position is obtained by focusing on the deviation between a marker predicted position predicted from the marker observation data starting from the main estimated position and a marker installation position matching the marker predicted position in the marker position data. According to this, the influence of the estimation error of the main estimated position can be offset from the estimation error of the sub-estimated position obtained by correcting the main estimated position by the deviation between the marker predicted position and the marker installation position. Therefore, the main estimated position and the sub-estimated position are weighted and fused based on the ratio between the respective estimation errors that can reflect changes in the external environment, thereby making it possible to generate position estimation data that ensures robustness with respect to the estimation accuracy of the self-position. [Brief description of the drawings]
[0011] [Figure 1] 1 is a block diagram showing an overall configuration of an estimation device according to a first embodiment. [Diagram 2] 1 is a perspective view showing an autonomous mobile device on which an estimation device according to a first embodiment is mounted, together with a route marker. [Diagram 3] 1 is a plan view showing an autonomous mobile device on which an estimation device according to a first embodiment is mounted, together with route markers. [Figure 4] FIG. 2 is a block diagram showing a functional configuration of the estimation device according to the first embodiment. [Diagram 5] FIG. 2 is a schematic diagram for explaining the estimation principle of the estimation device according to the first embodiment. [Figure 6] FIG. 2 is a schematic diagram for explaining the estimation principle of the estimation device according to the first embodiment. [Figure 7] FIG. 2 is a schematic diagram for explaining the estimation principle of the estimation device according to the first embodiment. [Figure 8] 4 is a flowchart showing an estimation flow according to the first embodiment. [Figure 9] 11 is a plan view showing an autonomous mobile device on which an estimation device according to a second embodiment is mounted, together with route markers. FIG. [Figure 10] FIG. 11 is a schematic diagram for explaining the estimation principle of an estimation device according to a second embodiment. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS
[0012] Hereinafter, multiple embodiments of the present disclosure will be described with reference to the drawings. In addition, by assigning the same reference numerals to corresponding components in each embodiment, duplicated descriptions may be omitted. In addition, when only a part of the configuration is described in each embodiment, the configuration of the other embodiment described above can be applied to the other parts of the configuration. Furthermore, in addition to the combination of configurations explicitly stated in the description of each embodiment, configurations of multiple embodiments can be partially combined together even if not explicitly stated, as long as there is no particular problem with the combination.
[0013] First embodiment The estimation device 1 of the first embodiment shown in FIG. 1 estimates the self-position of an autonomous mobile device 4 that autonomously travels in the outside world. Here, in this embodiment, a three-dimensional absolute coordinate system is defined in which the index is assumed to be in the vertical direction of Z, with respect to the two horizontal directions of X and Y. Furthermore, for the autonomous mobile device 4 that takes a posture required for autonomous travel in the three-dimensional orthogonal coordinate system, a three-dimensional relative coordinate system is defined in which the index is assumed to be in the vertical direction of x, the horizontal direction of y, and the height direction of z. However, it is assumed that the vertical direction (Z direction) of the three-dimensional absolute coordinate system and the height direction (z direction) of the three-dimensional relative coordinate system are substantially the same.
[0014] The autonomous mobile device 4 may be a logistics vehicle or a logistics robot that autonomously travels on a road inside and outside a warehouse in a logistics facility that is the external world of the autonomous mobile device 4 to transport luggage (example of FIG. 2). The autonomous mobile device 4 may be an automatically driven vehicle that can autonomously travel on a road in a traffic environment that is the external world of the autonomous mobile device 4. The autonomous mobile device 4 may be a food delivery robot that autonomously travels on a road inside a restaurant or hospital that is the external world of the autonomous mobile device 4 to deliver food and drink. The autonomous mobile device 4 may be a disaster support robot that autonomously travels in a disaster area that is the external world of the autonomous mobile device 4 to transport supplies or collect information. The autonomous mobile device 4 may of course be a device other than these. Furthermore, any type of autonomous mobile device 4 may receive remote driving support or driving control from an external center.
[0015] As shown in Figs. 2 and 3, a plurality of route markers 6 are installed in at least a part of the area of the travel path or road that is the subject of autonomous travel in the external world of the autonomous mobile device 4. Each route marker 6 is mainly composed of a magnetized magnetic marker, for example, made of a ferrite magnet. In the first embodiment, each route marker 6 is arranged at intervals on a travel trajectory Tr that is assumed to continuously provide representative travel positions, such as a lateral center position in a three-dimensional relative coordinate system, on the travel route on which the autonomous mobile device 4 travels autonomously. Each route marker 6 is attached to the road surface at each arrangement location, for example, by a protective sheet or is buried. All or some of the route markers 6 may be equipped with RFID (radio frequency identification).
[0016] 1, an estimation device 1 is mounted on an autonomous mobile device 4 together with a sensor unit 2 and a map unit 3. The sensor unit 2 includes a marker sensor 20, an external sensor 22, and an internal sensor .
[0017] The marker sensor 20 shown in Figs. 1, 2 and 4 acquires marker observation data Do that can be used to estimate the movement of the autonomous mobile device 4 in the external world, which is the surrounding environment of the autonomous mobile device 4. The marker sensor 20 is mainly composed of a magnetic sensor or a magnetic receiver that acquires the marker observation data Do by observing route markers 6 that exist in the external world from the autonomous mobile device 4. The marker sensor 20 may include an RFID reader. It is preferable that an attachment position and an observation reference position for such a marker sensor 20 are set in advance for the autonomous mobile device 4 so that the route markers 6 that come within an observation range of a set distance can be observed.
[0018] The external sensor 22 shown in FIGS. 1, 2, and 4 acquires, as external data De, information other than the marker observation data Do that can be used to estimate the motion of the autonomous mobile device 4 in the external world that is the surrounding environment of the autonomous mobile device 4. The external sensor 22 may acquire the external data De by detecting an object existing in the external world from the autonomous mobile device 4. The external sensor 22 of this detection type is, for example, one or more types of LiDAR (Light Detection and Ranging / Laser Imaging Detection and Ranging), a camera, a radar, a sonar, etc. The external sensor 22 may acquire the external data De by receiving, from the autonomous mobile device 4, a wireless signal from a wireless communication system existing in the external world. The external sensor 22 of this reception type is, for example, one or more types of a GNSS (Global Navigation Satellite System) receiver and an ITS (Intelligent Transport Systems) receiver. In the following, an example will be described in which the external sensor 22 is constituted by a LiDAR that scans the external world with a beam and detects the reflected beam from a target to obtain a three-dimensional point cloud image as the external world data De.
[0019] The internal sensor 24 shown in FIGS. 1 and 4 acquires information usable for estimating the motion of the autonomous mobile device 4 as internal world data Di in the internal world, which is the internal environment of the autonomous mobile device 4. The internal world sensor 24 may acquire the internal world data Di by detecting a specific motion physical quantity in the internal world of the autonomous mobile device 4. This detection type of internal world sensor 24 is one or more types of sensors, such as an inertial sensor, a speed sensor, and a steering angle sensor. In the following, an example will be described in which the internal world sensor 24 is composed of an inertial sensor that acquires the angular velocity and / or attitude angle of the autonomous mobile device 4 as the internal world data Di, and a speed sensor that acquires the speed of the autonomous mobile device 4 as the internal world data Di.
[0020] The map unit 3 shown in FIGS. 1 and 4 includes one or more types of non-transitory tangible storage media, such as a semiconductor memory, a magnetic medium, and an optical medium, that non-transitoryly stores or memorizes the map data Dm. The map unit 3 may be a locator database used for advanced driving assistance or automatic driving control of the autonomous driving device 4. The map unit 3 may be a database of a navigation device that navigates the driving of the autonomous driving device 4. The map unit 3 may be a database of a planning unit that plans the driving of the autonomous driving device 4. The map unit 3 may be configured by combining multiple types of these databases.
[0021] The map unit 3 acquires and stores the latest map data Dm, for example, by communication with an external center. The map data Dm is a three-dimensional digital map including a map point cloud that maps the external world in which the autonomous driving device 4 travels autonomously, and is, for example, a high-precision dynamic map. Such map data Dm may include road information that represents one or more types of the position, shape, size, and road surface condition of a travel path or road. The map data Dm may include marking information that represents one or more types of the position, shape, size, and operation state of signs, lane markings, and traffic lights attached to the travel path or road. The map data Dm may include structure information that represents one or more types of the position, shape, size, and operation state of buildings, structures, and plants facing the travel path or road.
[0022] The estimation device 1 shown in FIGS. 1 and 4 is connected to a sensor unit 2 and a map unit 3 via one or more of, for example, a local area network (LAN), a wire harness, an internal bus, and a wireless communication line. The estimation device 1 may be an ECU (Electronic Control Unit) dedicated to driving control, which executes advanced driving assistance or automatic driving control of the autonomous driving device 4. The estimation device 1 may be an ECU of a locator used for advanced driving assistance or automatic driving control of the autonomous driving device 4. The estimation device 1 may be an ECU of a navigation device that navigates the driving of the autonomous driving device 4. The estimation device 1 may be an ECU of a communication control device that controls communication between the autonomous driving device 4 and the outside world.
[0023] 1, the estimation device 1 is a dedicated computer including at least one memory 10 and one processor 12. The memory 10 is one or more types of non-transitory tangible storage medium, such as a semiconductor memory, a magnetic medium, an optical medium, etc., that non-temporarily stores or stores computer-readable programs, data, etc. The processor 12 includes one or more types of cores, such as a central processing unit (CPU), a graphics processing unit (GPU), a reduced instruction set computer (RISC)-CPU, etc.
[0024] The processor 12 executes a plurality of instructions included in the estimation program stored in the memory 10. In this way, the estimation device 1 constructs a plurality of functional blocks for estimating the self-position of the autonomous mobile device 4, as shown in Fig. 4. In this way, in the estimation device 1, the estimation program stored in the memory 10 causes the processor 12 to execute a plurality of instructions to estimate the self-position of the autonomous mobile device 4, thereby constructing a plurality of functional blocks.
[0025] The multiple functional blocks constructed by the estimation device 1 include a dead reckoning block 100, a marker matching block 110, a map matching block 120, and a fusion block 130. The dead reckoning block 100 estimates the self-position of the autonomous mobile device 4 at the current estimation time t by dead reckoning based on the internal data Di and a dynamics model, and acquires the estimation result as a main estimated position Pd (see FIG. 5 described later in detail). Here, the dynamics model is, for example, a two-wheel model in which the behavior of the autonomous mobile device 4 is modeled based on dynamics.
[0026] Thus, the three-dimensional position coordinates Xd, Yd, Zd constituting the main estimated position Pd at the current estimation time t are defined by Equations 1, 2, and 3 using the three-dimensional position coordinates Xf, Yf, Zf (see FIG. 5) estimated by the fusion block 130 at the previous estimation time t-1 as described later in detail. Here, the speed of the autonomous mobile device 4 is input to V in Equations 1, 2, and 3 as the internal data Di detected at the current estimation time t by the speed sensor of the internal sensors 24. The estimation interval (i.e., sampling period) between the current estimation time t and the previous estimation time t-1 is input to δt in Equations 1, 2, and 3. The pitch angle of the attitude angle of the autonomous mobile device 4 is input to ωp in Equation 3 as the internal data Di detected at the current estimation time t by the inertial sensor of the internal sensors 24. In addition, in Figure 5 and Figure 10 described below, as well as equations 1 to 3 and equations 4 to 34 described below, parameters (variables) that need to be distinguished at each estimated time t, t-1 are expressed in the functional form [t], [t-1] of the corresponding estimated times t, t-1, respectively.
number
number
number
[0027] Furthermore, θd in Equations 1 and 2 is the yaw angle among the attitude angles at the current estimation time t, and is defined by Equation 4 using the yaw angle θf (see FIG. 5) estimated by the fusion block 130 at the previous estimation time t-1 as described in detail later. Here, the yaw rate of the autonomous driving device 4 is input to γ in Equation 4 as the internal field data Di detected at the current estimation time t by the inertial sensor among the internal sensors 24. The estimation interval between the current estimation time t and the previous estimation time t-1 is input to δt in Equation 4.
number
[0028] In association with such estimation of the main estimated position Pd at the current estimation time t, the dead reckoning block 100 acquires an error predicted in the estimation as a main estimation error ΔPd shown in Fig. 4. At this time, the main estimation error ΔPd at the current estimation time t is composed of three-dimensional position errors ΔXd, ΔYd, and ΔZd related to the estimated three-dimensional position coordinates Xd, Yd, and Zd, respectively.
[0029] Therefore, the three-dimensional position errors ΔXd, ΔYd, ΔZd at the current estimation time t are defined by Equations 5, 6, and 7 using the three-dimensional position errors ΔXf, ΔYf, ΔZf estimated by the fusion block 130 at the previous estimation time t-1 as described later in detail. Here, the velocity at the current estimation time t by the velocity sensor of the internal sensors 24 is input to V in Equations 5, 6, and 7. The velocity error predicted as the error characteristic of the velocity sensor of the internal sensors 24 is input to ΔV in Equations 5, 6, and 7. The estimated interval between the current estimation time t and the previous estimation time t-1 is input to δt in Equations 5, 6, and 7. The estimated yaw angle of Equation 4 at the current estimation time t is input to θd in Equations 5 and 6. The pitch angle error predicted as the error characteristic of the inertial sensor of the internal sensors 24 is input to Δωp in Equation 7.
number
number
number
[0030] Furthermore, Δθf in Equations 5 and 6 is a yaw angle error estimated by the fusion block 130 at the previous estimation time t-1 as described later in detail, and the error Δθd of the yaw angle θd estimated at the current estimation time t is defined by Equation 8 using the previous yaw angle error Δθf. Here, the yaw rate error predicted as the error characteristic of the inertial sensor of the internal sensor 24 is input to Δγ in Equation 8. The estimation interval between the current estimation time t and the previous estimation time t-1 is input to δt in Equation 8.
number
[0031] From the above, the dead reckoning block 100 estimates the three-dimensional position coordinates Xd, Yd, Zd of Equations 1, 2, and 3 that give the main estimated position Pd, together with the yaw angle θd of Equation 4, for the current estimation time t. Accordingly, the dead reckoning block 100 estimates the three-dimensional position errors ΔXd, ΔYd, ΔZd of Equations 5, 6, and 7 that give the main estimated error ΔPd, together with the yaw angle error Δθd of Equation 8, for the current estimation time t. The dead reckoning block 100 then stores these estimation results in the memory 10 in association with the current estimation time t.
[0032] The marker matching block 110 shown in Fig. 4 estimates the self-position of the autonomous mobile device 4 at the current estimated time t by marker matching based on the marker observation data Do and the marker position data Dmp, and acquires the estimation result as a first sub-estimated position Pm1. At this time, the marker observation data Do is the observed position po of the route marker 6 when observed by the marker sensor 20 at the current estimated time t, and is composed of three-dimensional relative coordinates xo, yo, zo with respect to the autonomous mobile device 4 as shown in Fig. 5.
[0033] Therefore, the current estimated time t that is the subject of the marker matching process in the marker matching block 110 is the acquisition timing of the input marker observation data Do. Here, the acquisition timing of the marker observation data Do may be set for each observation period of the route marker 6 by the marker sensor 20 in the installation area of the route marker 6. The acquisition timing of the marker observation data Do may be set to the timing when the route marker 6 is actually observed by the marker sensor 20 in the installation area of the route marker 6.
[0034] 5 from the marker observation data Do at the current estimated time t, starting from the main estimated position Pd at the same time t by the dead reckoning block 100. For this reason, in the dead reckoning block 100, which inputs the main estimated position Pd to the marker matching block 110, the current estimated time t at which the dead reckoning process is executed is substantially synchronized with the acquisition timing of the marker observation data Do.
[0035] The three-dimensional position coordinates Xp, Yp, Zp predicted as the predicted marker position Pp are obtained by correcting the three-dimensional position coordinates Xd, Yd, Zd of the main estimated position Pd using coordinate transformation values between the three-dimensional relative coordinates xo, yo, zo of the marker observation position po and the attachment position and observation reference position of the marker sensor 20. Therefore, the prediction error for the three-dimensional position coordinates Xp, Yp, Zp of the predicted marker position Pp includes the main estimation error ΔPd for the three-dimensional position coordinates Xd, Yd, Zd of the main estimated position Pd and the observation error for the three-dimensional relative coordinates xo, yo, zo of the marker observation data Do.
[0036] For such marker observation data Do, marker position data Dmp in Fig. 4 is input as marker installation positions Pmp in Fig. 5, which are the set positions of each route marker 6 in the outside world, including the three-dimensional position coordinates Xmp, Ymp, Zmp of each of the markers 6. At this time, the marker position data Dmp may be read out from the map unit 3 as part of the map data Dm (in the case of Figs. 1 and 4). The marker position data Dmp may be stored in the map unit 3 or memory 10 in accordance with the above-mentioned map data Dm, and may be read out from the storage destination.
[0037] Therefore, the marker matching block 110 recognizes the deviation δP between the marker installation position Pmp that matches the marker predicted position Pp at the current estimated time t in the marker position data Dmp as shown in Fig. 5 and the predicted marker position Pp. As a result, the marker matching block 110 corrects the main estimated position Pd at the current estimated time t with the recognized deviation δP, thereby acquiring the first sub-estimated position Pm1 at the same time t.
[0038] At this time, the three-dimensional position coordinates Xm1, Ym1, and Zm1 in Figs. 4 and 5 constituting the first sub-estimated position Pm1 are defined by the numbers 9, 10, and 11, respectively. That is, each of the three-dimensional position coordinates Xm1, Ym1, and Zm1 is obtained by subtracting the deviation between the three-dimensional position coordinates Xp, Yp, and Zp of the predicted marker position Pp and the three-dimensional position coordinates Xmp, Ymp, and Zmp of the marker installation position Pmp as the deviation δP in Fig. 5 from the three-dimensional position coordinates Xd, Yd, and Zd of the main estimated position Pd. At the same time, the yaw angle θm1 in Figs. 4 and 5, which is one of the attitude angles at the current estimated time t, is obtained by correcting the estimated value θf at the previous estimated time t-1 based on the deviation between the coordinates Yp and Ymp of the positions Pp and Pmp and the travel distance of the autonomous mobile device 4 during the estimation interval between times t and t-1.
number
number
number
[0039] In accompaniment with this estimation of the first sub-estimated position Pm1 at the current estimation time t, the marker matching block 110 acquires an error predicted in the estimation as the first sub-estimated error ΔPm1 shown in Fig. 4. At this time, the first sub-estimated error ΔPm1 at the current estimation time t is composed of three-dimensional position errors ΔXm1, ΔYm1, and ΔZm1 related to the estimated three-dimensional position coordinates Xm1, Ym1, and Zm1, respectively.
[0040] Here, due to the relationship between the second and third terms on the right-hand sides of equations 9, 10 and 11, each of the three-dimensional position errors ΔXm1, ΔYm1 and ΔZm1 is affected by the prediction error for the three-dimensional position coordinates Xp, Yp, Zp of the predicted marker position Pp and the map error for the three-dimensional position coordinates Xmp, Ymp, Zmp of the marker installation position Pmp. Of these errors, in particular, the prediction error for the three-dimensional position coordinates Xp, Yp, Zp in the second term on the right-hand sides includes the main estimation error ΔPd for the three-dimensional position coordinates Xd, Yd, Zd of the main estimated position Pd and the observation error for the three-dimensional relative coordinates xo, yo, zo of the marker observation data Do, as described above.
[0041] Therefore, the influence of the main estimation error ΔPd on the three-dimensional position coordinates Xd, Yd, Zd on each of the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1 is substantially canceled by the relationship between the first and second terms on the right sides of the formulas 9, 10, and 11. Therefore, the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1 each include the observation error on the three-dimensional relative coordinates xo, yo, zo and the map error on the three-dimensional position coordinates Xmp, Ymp, Zmp. Therefore, each of the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1 is preset to a fixed value that is different or the same based on the observation error predicted as the error characteristic of the marker sensor 20 for the route marker 6 and the measurement error of the marker position data Dmp for the route marker 6. At this time, the error Δθm1 predicted in the estimation of the yaw angle θm1 at the current estimation time t is also preset to a similar fixed value.
[0042] From the above, the marker matching block 110 estimates the three-dimensional position coordinates Xm1, Ym1, Zm1 of numbers 9, 10, and 11 that give the first sub-estimated position Pm1, together with the yaw angle θm1, for the current estimated time t. Accordingly, the marker matching block 110 estimates the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1 that give the first sub-estimated error ΔPm1, together with the yaw angle error Δθm1, for the current estimated time t. The marker matching block 110 then stores these estimation results in the memory 10 in association with the current estimated time t.
[0043] 4 estimates the self-position of the autonomous driving device 4 at the current estimated time t by map matching based on the external world data De and map data Dm, and acquires the estimation result as a second sub-estimated position Pm2. At this time, as the external world data De, a three-dimensional point cloud image Des including a scanning point cloud Cs in a three-dimensional relative coordinate system scanned by the LiDAR of the external world sensors 22 at the current estimated time t as shown in FIGS.
[0044] Therefore, the current estimated time t at which the map matching process is executed in the map matching block 120 is the acquisition timing of the three-dimensional point cloud image Des input as the external world data De. Here, the acquisition timing of the three-dimensional point cloud image Des is set for each scanning period of the external world by the LiDAR of the external world sensor 22.
[0045] For such a three-dimensional point cloud image Des, a three-dimensional digital map Dmm including a map point group Cm in which targets in the outside world of the autonomous driving device 4 are mapped is input as the map data Dm in Figs. 6 and 7. At this time, the map point group Cm in which targets in the outside world are mapped in the three-dimensional digital map Dmm in the three-dimensional absolute coordinate system is converted into a three-dimensional relative coordinate system in terms of its position coordinates. The coordinate-converted map point group Cm is then allocated to a plurality of three-dimensional voxels Vm in which the outside world is divided in the three-dimensional relative coordinate system. In Figs. 6 and 7, the three-dimensional voxels Vm are illustrated two-dimensionally in the vertical direction (x direction) and horizontal direction (y direction), with the height direction (z direction) omitted.
[0046] Upon receiving the input involving such coordinate conversion and allocation, the map matching block 120 recognizes the map point group Cm that matches the scanning point group Cs of the three-dimensional point group image Des, together with the scanning point group Cs that has a corresponding relationship in the map matching, as compared with Figures 6 and 7. As a result, the map matching block 120 returns the position coordinates in the three-dimensional relative coordinate system of the map point group Cm that matches the scanning point group Cs to the three-dimensional absolute coordinate system, thereby acquiring the three-dimensional position coordinates Xm2, Ym2, Zm2 that constitute the second sub-estimated position Pm2 at the current estimation time t as shown in Figure 4. At this time, the yaw angle θm2 in Figure 4, which is one of the attitude angles at the current estimation time t, is also estimated by map matching.
[0047] In accompaniment with this estimation of the second sub-estimated position Pm2 at the current estimation time t, the map matching block 120 acquires an error predicted in the estimation as the second sub-estimated error ΔPm2 shown in Fig. 4. At this time, the second sub-estimated error ΔPm2 at the current estimation time t is composed of three-dimensional position errors ΔXm2, ΔYm2, and ΔZm2 relating to the estimated three-dimensional position coordinates Xm2, Ym2, and Zm2, respectively.
[0048] Therefore, the three-dimensional position errors ΔXm2, ΔYm2, ΔZm2 at the current estimation time t are defined by Equations 12, 13, and 14 using the necessary parameters from among the point cloud amount-dependent likelihoods Lxs, Lys, Lzs and the noise amount-dependent likelihood Ln in the three-dimensional relative coordinate system. Here, the functions fx, fy, and fz in Equations 12, 13, and 14 are functions that decrease the corresponding three-dimensional position errors ΔXm2, ΔYm2, and ΔZm2 as the corresponding likelihoods Lxs, Lys, Lzs, and Ln increase, respectively.
number
number
number
[0049] Furthermore, the map matching block 120 obtains an error Δθm2 predicted in the estimation of the yaw angle θm2 at the current estimation time t. At this time, the yaw angle error Δθm2 is defined by Equation 15 using the point cloud amount-dependent likelihood Lys and the noise amount-dependent likelihood Ln. Here, the function fθ in Equation 15 is a function that reduces the yaw angle error Δθm2 as the likelihoods Lys and Ln increase.
number
[0050] In such numbers 12, 13, 14, and 15, the point cloud amount-dependent likelihoods Lxs, Lys, and Lzs are input by accumulating (summing up) the inverses of the variations in each direction in the three-dimensional relative coordinate system for the matching map point cloud Cm over all voxels Vm including the map point cloud Cm that is matched with the scanning point cloud Cs, as shown with dot hunting in Fig. 6. On the other hand, in numbers 12, 13, 14, and 15, the noise amount-dependent likelihood Ln is input with the inverse of the total number for all voxels Vm including the scanning point cloud Cs that is unmatched with the map point cloud Cm, as shown with dot hunting in Fig. 7.
[0051] From the above, the map matching block 120 estimates the three-dimensional position coordinates Xm2, Ym2, Zm2 that give the second sub-estimated position Pm2, together with the yaw angle θm2, for the current estimation time t. Accordingly, the map matching block 120 estimates the three-dimensional position errors ΔXm2, ΔYm2, ΔZm2 that give the second sub-estimated error ΔPm2, together with the yaw angle error Δθm2, for the current estimation time t. The map matching block 120 then stores these estimation results in the memory 10 in association with the current estimation time t.
[0052] The fusion block 130 shown in Fig. 4 estimates the self-position Pf by adopting a fusion method according to the acquisition timing of either the marker observation data Do or the 3D point cloud image Des, which is the external data De, at the current estimation time t. Therefore, at the current estimation time t, which is the acquisition timing of the marker observation data Do, the fusion block 130 fuses the main estimated position Pd and the first sub-estimated position Pm1 by filtering through a Kalman filter. As a result, the position estimation data Df is generated so as to include the self-position Pf, which is the fusion of the main estimated position Pd and the first sub-estimated position Pm1 weighted based on the ratio between the respective estimation errors ΔPd and ΔPm1.
[0053] At this time, the three-dimensional position coordinates Xf, Yf, Zf of the self-position Pf are defined by the formulas 16, 17, and 18 using the three-dimensional position coordinates Xd, Yd, Zd of the main estimated position Pd and the three-dimensional position coordinates Xm1, Ym1, Zm1 of the first sub-estimated position Pm1. Here, the Kalman gains Kx, Ky, Kz of the formulas 16, 17, and 18 are variably set based on the ratio between the three-dimensional position errors ΔXd, ΔYd, ΔZd of the main estimated position Pd and the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1 of the first sub-estimated position Pm1. In particular, in the formulas 16, 17, and 18, the Kalman gains Kx, Ky, Kz are set to increase as the relative ratio of the three-dimensional position errors ΔXd, ΔYd, ΔZd to the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1 decreases.
number
number
number
[0054] With the estimation of the self-position Pf at the current estimation time t, which is the acquisition timing of the marker observation data Do, the three-dimensional position errors ΔXf, ΔYf, ΔZf, which are the estimation error ΔPf, are also estimated as shown in Fig. 4 and added to the position estimation data Df. The three-dimensional position errors ΔXf, ΔYf, ΔZf are defined by Equations 19, 20, and 21 using the three-dimensional position errors ΔXd, ΔYd, ΔZd of the main estimation error ΔPd and the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1 of the first sub-estimation error ΔPm1. Here, the Kalman gains Kx, Ky, Kz in Equations 19, 20, and 21 are the same as those in Equations 16, 17, and 18, respectively.
number
number
number
[0055] Furthermore, at the current estimation time t, which is the timing of acquiring the marker observation data Do, the yaw angle θf and its error Δθf shown in FIG. 4 among the attitude angles of the autonomous mobile device 4 are also estimated by fusion equivalent to the self-position Pf. At this time, the yaw angle θf is defined by Equation 22 using the yaw angles θd and θm1. At the same time, the yaw angle error Δθf is defined by Equation 23 using the yaw angle errors Δθd and Δθm1. Here, the Kalman gain Kθ, which is common to Equations 22 and 23, is variably set based on the ratio between the yaw angle errors Δθd and Δθm1. In particular, in Equations 22 and 23, the Kalman gain Kθ is set to increase as the relative ratio of the yaw angle error Δθd to the yaw angle error Δθm1 decreases.
number
number
[0056] On the other hand, at the current estimation time t, which is the acquisition timing of the 3D point cloud image Des, the fusion block 130 fuses the main estimated position Pd and the second sub-estimated position Pm2 by filtering through a Kalman filter. As a result, position estimation data Df is generated so as to include a self-position Pf in which the main estimated position Pd and the second sub-estimated position Pm2 are weighted based on the ratio between the respective estimation errors ΔPd and ΔPm2 and fused.
[0057] At this time, the three-dimensional position coordinates Xf, Yf, Zf of the self-position Pf are defined by Equations 24, 25, and 26 using the three-dimensional position coordinates Xd, Yd, Zd of the main estimated position Pd and the three-dimensional position coordinates Xm2, Ym2, Zm2 of the second sub-estimated position Pm2. Here, the Kalman gains kx, ky, kz of Equations 24, 25, and 26 are variably set based on the ratio between the three-dimensional position errors ΔXd, ΔYd, ΔZd of the main estimated position Pd and the three-dimensional position errors ΔXm2, ΔYm2, ΔZm2 of the second sub-estimated position Pm2. In particular, in Equations 24, 25, and 26, the Kalman gains kx, ky, kz are set to increase as the relative ratio of the three-dimensional position errors ΔXd, ΔYd, ΔZd to the three-dimensional position errors ΔXm2, ΔYm2, ΔZm2 decreases.
number
number
number
[0058] With the estimation of the self-position Pf at the current estimation time t, which is the acquisition timing of the three-dimensional point cloud image Des, the three-dimensional position errors ΔXf, ΔYf, ΔZf, which are the estimation error ΔPf, are also estimated as shown in Fig. 4 and added to the position estimation data Df. Here, each three-dimensional position error ΔXf, ΔYf, ΔZf is defined by Equations 27, 28, and 29 using the three-dimensional position errors ΔXd, ΔYd, ΔZd of the main estimation error ΔPd and the three-dimensional position errors ΔXm2, ΔYm2, ΔZm2 of the second sub-estimation error ΔPm2. Here, the Kalman gains kx, ky, kz of Equations 27, 28, and 29 are the same as those of Equations 24, 25, and 26, respectively.
number
number
number
[0059] Furthermore, at the current estimation time t, which is the acquisition timing of the three-dimensional point cloud image Des, the yaw angle θf and its error Δθf shown in FIG. 4 among the attitude angles of the autonomous driving device 4 are estimated by fusion equivalent to the self-position Pf. At this time, the yaw angle θf is defined by Equation 30 using the yaw angles θd and θm2. At the same time, the yaw angle error Δθf is defined by Equation 31 using the yaw angle errors Δθd and Δθm2. Here, the Kalman gain kθ common to Equations 30 and 31 is variably set based on the ratio between the yaw angle errors Δθd and Δθm2. In particular, in Equations 30 and 31, the Kalman gain kθ is set to increase as the relative ratio of the yaw angle error Δθd to the yaw angle error Δθm2 decreases.
number
number
[0060] Regardless of the above acquisition timing, position estimation data Df obtained by estimating the self-position Pf at the current estimation time t by fusion is output from the fusion block 130 as shown in Fig. 4. As a result, the three-dimensional position coordinates Xf, Yf, Zf output as the self-position Pf are stored in the memory 10 in association with the current estimation time t, and are then fed back to the dead reckoning process at the next processing time by the dead reckoning block 100. In addition, the three-dimensional position coordinates Xf, Yf, Zf output as the self-position Pf may be utilized for, for example, advanced driving assistance or automatic driving control of the autonomous driving device 4.
[0061] The following describes a flow chart (hereinafter, referred to as the estimation flow) according to Fig. 8 of an estimation data generation method for generating position estimation data Df that estimates the self-position Pf of the autonomous mobile device 4 by cooperation of the blocks 100, 110, 120, and 130 described so far. Note that this estimation flow is repeatedly executed at estimation intervals while the autonomous mobile device 4 is running. Also, in this estimation flow, "S" refers to multiple steps executed by multiple commands included in the estimation program.
[0062] In S10, the dead reckoning block 100 determines whether the current estimated time t is the timing for acquiring the marker observation data Do. If a positive determination is made, the estimation flow proceeds to S20. In S20, the dead reckoning block 100 acquires the main estimated position Pd of the autonomous mobile device 4 at the current estimated time t by dead reckoning based on the internal data Di and a dynamics model.
[0063] In S30 following S20, the marker matching block 110 obtains a first sub-estimated position Pm1 of the autonomous mobile device 4 at the current estimated time t by marker matching based on the marker observation data Do and the marker position data Dmp. At this time, the main estimated position Pd corrected by the deviation ΔP between the marker predicted position Pp predicted from the marker observation data Do starting from the main estimated position Pd and the marker installation position Pmp that matches the predicted position Pp in the marker position data Dmp becomes the first sub-estimated position Pm1.
[0064] In S40 following S30, the fusion block 130 generates position estimation data Df of the self-position Pf by weighting the main estimated position Pd and the first sub-estimated position Pm1 at the current estimation time t based on the ratio of their respective estimation errors ΔPd, ΔPm1 through fusion. The position estimation data Df thus generated is output to and stored in the memory 10, and is fed back to the dead reckoning at the next estimation time by the dead reckoning block 100. Upon completion of such execution of S40, the current execution of the estimation flow ends.
[0065] On the other hand, if a negative determination is made in S10, the estimation flow proceeds to S50. In S50, the map matching block 120 determines whether the current estimated time t is the timing for acquiring the three-dimensional point cloud image Des from the external world data De. As a result, if a negative determination is made, the current execution of the estimation flow ends. On the other hand, if a positive determination is made, the estimation flow proceeds to S60. In S60, the map matching block 120 acquires a second sub-estimated position Pm2 of the autonomous mobile device 4 at the current estimated time t by map matching based on the external world data De and the map data Dm.
[0066] In S70 following S60, the fusion block 130 generates position estimation data Df of the self-position Pf by weighting the main estimated position Pd and the second sub estimated position Pm2 at the current estimation time t based on the ratio of their respective estimation errors ΔPd, ΔPm2 through fusion. The position estimation data Df generated in this manner is also output to and stored in the memory 10, and is fed back to the dead reckoning at the next estimation time by the dead reckoning block 100. Upon completion of such execution of S70, the current execution of the estimation flow ends.
[0067] (Action and effect) The effects of the first embodiment described above will be described below.
[0068] According to the first embodiment, a main estimated position Pd is obtained by dead reckoning based on the internal data Di, and a first sub-estimated position Pm1 is obtained by marker matching based on the marker observation data Do and the marker position data Dmp. Here, in order to obtain the first sub-estimated position Pm1, attention is paid to the deviation ΔP between a marker predicted position Pp predicted from the marker observation data Do starting from the main estimated position Pd, and a marker installation position Pmp matching the marker predicted position Pp in the marker position data Dmp. According to this, the influence of the estimation error ΔPd of the main estimated position Pd can be offset by the estimation error ΔPm1 of the first sub-estimated position Pm1 obtained by correcting the main estimated position Pd by the deviation ΔP between the marker predicted position Pp and the marker installation position Pmp. Therefore, by weighting and fusing the main estimated position Pd and the first sub-estimated position Pm1 based on the ratio between their respective estimation errors ΔPd, ΔPm1, which may reflect changes in the external situation, it is possible to generate position estimation data Df that ensures robustness with respect to the estimation accuracy of the self-position Pf.
[0069] According to the first embodiment, at the timing of acquiring the marker observation data Do, the position estimation data Df is generated by fusing the main estimated position Pd by dead reckoning and the first sub-estimated position Pm1 by marker matching based on the marker observation data Do, which are weighted based on the ratio of their respective estimated errors. On the other hand, at the timing of acquiring the three-dimensional point cloud image Des as the external world data De, the position estimation data Df is generated by fusing the main estimated position Pd by dead reckoning and the second sub-estimated position Pm2 by map matching based on the acquired image Des, which are weighted based on the ratio of their respective estimated errors. According to these, at the timing of acquiring the three-dimensional point cloud image Des, the first sub-estimated position Pm1 with the estimated error correction is acquired by dead reckoning based on the past self-position Pf estimated from the second sub-estimated position Pm2 using the acquired image Des, and at the timing of acquiring the marker observation data Do, the latest self-position Pf can be estimated with high accuracy. Therefore, it is possible to increase the reliability in generating the position estimation data Df that ensures robustness with respect to the estimation accuracy of the self-position Pf.
[0070] Second Embodiment The second embodiment is a modification of the first embodiment.
[0071] As shown in Fig. 9, in the second embodiment, pairs of route markers 6 are arranged at symmetrical positions on the horizontal direction of a travel trajectory Tr assumed for the travel route of the autonomous mobile device 4 in a three-dimensional relative coordinate system, and are installed at intervals along the trajectory Tr. Therefore, in the autonomous mobile device 4 of the second embodiment, the travel direction Td on the trajectory Tr is controlled in an orthogonal direction to a virtual straight line L connecting the paired route markers 6 (hereinafter, referred to as paired markers 6 in the description of the second embodiment) as shown in Fig. 9. At this time, the actual yaw angle of the autonomous mobile device 4 is feedback-controlled based on the yaw angle θf estimated by the estimation device 1 (see the first embodiment), thereby making it possible to realize a travel direction Td aligned with an orthogonal direction to the virtual straight line L.
[0072] Therefore, in the marker matching block 110 of the second embodiment, the three-dimensional position coordinates Xm1, Ym1, Zm1 of the first sub-estimated position Pm1 at the current estimation time t shown in Fig. 10 are defined by numbers 32, 33, and 34, respectively, where each suffix of the paired marker 6 is i = 1, 2. That is, each of the three-dimensional position coordinates Xm1, Ym1, Zm1 is obtained by subtracting the deviation average δP between the three-dimensional position coordinates Xp_i, Yp_i, Zp_i which are the predicted marker position Pp_i of each paired marker 6 and the three-dimensional position coordinates Xmp_i, Ymp_i, Zmp_i which are the marker installation position Pmp_i of each paired marker 6 from the three-dimensional position coordinates Xd, Yd, Zd of the main estimated position Pd as shown in Fig. 10. At the same time, the yaw angle θm1 at the current estimated time t is acquired as an attitude angle that represents the traveling direction Td of the autonomous mobile device 4 on the trajectory Tr, that is, the orthogonal orientation with respect to the imaginary line L.
number
number
number
[0073] In the second embodiment, the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1 (see the first embodiment) that become the first sub-estimated error ΔPm1 at the current estimation time t are substantially offset by the influence of the main estimated error ΔPd for the three-dimensional position coordinates Xd, Yd, Zd, based on the relationships in the formulas 32, 33, and 34, respectively. Therefore, in each of the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1, the observation error for the three-dimensional relative coordinates xo, yo, zo of the predicted marker position Pp, and the map error for the three-dimensional position coordinates Xmp, Ymp, Zmp of the marker installation position Pmp remain, as in the first embodiment. Therefore, each of the three-dimensional position errors ΔXm1, ΔYm1, ΔZm1 is preset to a fixed value that is different or the same, based on the observation error predicted as the error characteristic of the marker sensor 20 for the paired marker 6 and the measurement error of the marker position data Dmp for the paired marker 6. At this time, the error Δθm1 (see the first embodiment) predicted in the estimation of the yaw angle θm1 at the current estimation time t is also set in advance to a similar fixed value.
[0074] The second embodiment described above can also achieve the same effects as the first embodiment. Moreover, according to the second embodiment, the traveling direction Td of the autonomous mobile device 4 is controlled to be orthogonal to the virtual straight line L connecting the paired markers 6, so that it is possible to prevent the yaw angle estimation accuracy of the autonomous mobile device 4 from affecting the estimation error ΔPm1 of the first sub-estimated position Pm1. Therefore, it is possible to increase the reliability of the generation of position estimation data Df that ensures robustness with respect to the estimation accuracy of the self-position Pf.
[0075] (Other embodiments) Although several embodiments have been described above, the present disclosure should not be construed as being limited to those embodiments, and can be applied to various embodiments and combinations within the scope not departing from the gist of the present disclosure.
[0076] In the modified example, the dedicated computer constituting the estimation device 1 may have at least one of a digital circuit and an analog circuit as a processor. Here, the digital circuit is at least one of ASIC (Application Specific Integrated Circuit), FPGA (Field Programmable Gate Array), SOC (System on a Chip), PGA (Programmable Gate Array), CPLD (Complex Programmable Logic Device), etc. Furthermore, such a digital circuit may have a memory that stores a program.
[0077] In a modified example, the map matching block 120 may be omitted, and a fusion block 130 may be provided that does not perform fusion processing at the acquisition timing of the three-dimensional point cloud image Des of the external world data De. In this case, S50 to S70 may be omitted in the estimation flow, and the current execution of the estimation flow may be terminated in response to a negative determination in S10.
[0078] In the marker matching block 110 of the modified example, a second sub-estimated position Pm2 may be input instead of the main estimated position Pd at the timing of acquisition of a three-dimensional point cloud image Des as the external world data De, and used to predict the predicted marker position Pp. In the modified example, instead of the three-dimensional absolute coordinate system and the three-dimensional relative coordinate system, a two-dimensional absolute coordinate system and a two-dimensional relative coordinate system in which the vertical direction (Z direction) and the height direction (z direction) are not assumed may be defined, and in this case, each embodiment may be implemented by reading "three-dimensional" as "two-dimensional".
[0079] In addition to the forms described so far, the above-mentioned embodiments and variants may be implemented in the form of a processing circuit (e.g., a processing ECU, etc.) or a semiconductor device (e.g., a semiconductor chip, etc.) as a control device configured to be mounted on an autonomous vehicle 4 and having at least one processor 12 and one memory 10. [Explanation of symbols]
[0080] 1: Estimation device, 4: Autonomous driving device, 6: Route marker, 10: Memory, 12: Processor, Cm: Map point cloud, Cs: Scanning point cloud, De: External data, Df: Position estimation data, Di: Internal data, Dm: Map data, Dmp: Marker position data, Do: Marker observation data, L: Virtual line, Pd: Main estimated position, Pm1: First sub-estimated position, Pm2: Second sub-estimated position, Pmp: Marker installation position, Pp: Marker predicted position, Td: Travel direction, Vm: Voxel, δP: Deviation
Claims
1. An estimation device (1) having a processor (12) and estimating a self-position of an autonomous driving device (4) that autonomously drives in an external world, The processor, Acquiring a main estimated position (Pd) as the self-position by dead reckoning based on internal world data (Di) acquired in the internal world of the autonomous driving device; acquiring a sub-estimated position (Pm1) as the self-position by marker matching based on marker observation data (Do) acquired by observing a plurality of route markers (6) installed in the external world from the autonomous driving device and marker position data (Dmp) that defines marker installation positions (Pmp) that are the setting positions of each of the route markers in the external world; and generating position estimation data (Df) of the self-position by weighting the main estimated position and the sub estimated position based on a ratio of the estimation errors of the main estimated position and the sub estimated position by fusion. The acquisition of the sub estimated position includes: and acquiring, as the sub-estimated position, the main estimated position corrected by a deviation (δP) between a marker predicted position (Pp) predicted from the marker observation data starting from the main estimated position, and the marker installation position that matches the marker predicted position in the marker position data.
2. The processor, obtaining the main position estimate; obtaining a first sub-estimated position, the first sub-estimated position being the sub-estimated position; Obtaining a second sub-estimated position (Pm2) as the self-position by map matching based on external world data (De) obtained by scanning the external world from the autonomous driving device and map data (Dm) that maps the external world; At a timing when the marker observation data is acquired, the main estimated position and the first sub-estimated position are weighted based on a ratio of their estimation errors, and the position estimation data is generated by fusion, and the position estimation data is fed back to the dead reckoning; The estimation device according to claim 1, configured to generate the position estimation data by fusion in which the main estimated position and the second sub-estimated position are weighted based on a ratio of their respective estimation errors at the timing when the external data is acquired, and feed the generated position estimation data back to the dead reckoning.
3. The external data is A scanning point cloud (Cs) obtained by scanning the external world, The map data is The estimation device according to claim 2 , further comprising a map point cloud (Cm) obtained by mapping the external world into a plurality of voxels (Vm) obtained by dividing the external world three-dimensionally.
4. The autonomous driving device, 3. The estimation device according to claim 1 or 2, wherein the autonomous driving device is controlled to control a driving direction (Td) of the autonomous driving device in a direction perpendicular to a virtual straight line (L) connecting the pair of route markers.
5. The generation of the position estimation data includes: The estimation device according to claim 1 or 2, further comprising: outputting the generated position estimation data.
6. An estimation program that is stored in a storage medium (10) for estimating a self-position of an autonomous driving device (4) that autonomously drives in an external world, and includes instructions to be executed by a processor (12), Acquiring a main estimated position (Pd) as the self-position by dead reckoning based on internal world data (Di) acquired in the internal world of the autonomous driving device; acquiring a sub-estimated position (Pm1) as the self-position by marker matching based on marker observation data (Do) acquired by observing a plurality of route markers (6) installed in the external world from the autonomous driving device and marker position data (Dmp) that defines marker installation positions (Pmp) that are the setting positions of each of the route markers in the external world; and generating position estimation data (Df) of the self-position by weighting the main estimated position and the sub estimated position based on a ratio of the estimation errors of the main estimated position and the sub estimated position by fusion, The acquisition of the sub estimated position includes: and acquiring, as the sub-estimated position, the main estimated position corrected by a deviation (δP) between a marker predicted position (Pp) predicted from the marker observation data starting from the main estimated position, and the marker installation position that matches the marker predicted position in the marker position data.
7. An estimated data generation method executed by a processor (12) for generating position estimation data (Df) that estimates a self-position of an autonomous driving device (4) that autonomously drives in an external world, comprising: Acquiring a main estimated position (Pd) as the self-position by dead reckoning based on internal world data (Di) acquired in the internal world of the autonomous driving device; acquiring a sub-estimated position (Pm1) as the self-position by marker matching based on marker observation data (Do) acquired by observing a plurality of route markers (6) installed in the external world from the autonomous driving device and marker position data (Dmp) that defines marker installation positions (Pmp) that are the setting positions of each of the route markers in the external world; generating the position estimation data by weighting the main estimated position and the sub estimated position based on a ratio of their estimation errors; The acquisition of the sub estimated position includes: and obtaining, as the sub-estimated position, the main estimated position corrected by a deviation (δP) between a marker predicted position (Pp) predicted from the marker observation data starting from the main estimated position, and the marker installation position that matches the marker predicted position in the marker position data.
Citation Information
Patent Citations
Robot control method, robot, control terminal and control system
CN113878577A
Mobile robot
JP2007200070A
Control system of work machine, work machine, and method for controlling work machine
JP2020020656A
Control method and control system
JP2023050720A
Three-dimensional semiconductor memory device and electronic system including the same
KR1020220140917A
