Locus correction method, locus correction device and mobile object device

Abstract

A locus correction method for correcting a movement locus of a mobile object (V) by a locus correction device (20), the locus correction method being characterized in that: the locus correction device performs the following: Setting (S104) a plurality of nodes on a locus of the mobile object based on locus data of the mobile object obtained by a unit (16) of a measuring device, Correlating (S105) position data of the mobile object obtained by the one unit of the measuring device with the plurality of nodes and correlating position data of the mobile object obtained by at least two other units (12, 14, 15) of the measuring device than the one unit of the measuring device with the plurality of nodes, Calculating (S106) an evaluation function with differences of nodes and differences of position data as variables based on probability representations for positions at which nodes can occur and for positions at which position data correlated with nodes can occur, wherein the differences of nodes and of position data are differences in the relative positions of the locus data and the position data of the mobile object, respectively, with respect to a respective previous point in time, and Calculating (S108) a corrected locus on which a plurality of corrected nodes occur with the highest probability by determining the plurality of corrected nodes as a solution to the problem of optimizing the evaluation function given position data.

Description

Technical FieldThe present invention relates to a technique including a locus correction method, a locus correction apparatus, and a mobile object apparatus for correcting the locus of a mobile object.Background ArtAn autonomous mobile system has already been disclosed that estimates its location and moves along an intended path while generating a map that is adapted to the actual environment based on data measured by measurement devices (internal and external sensors) mounted in a mobile object (see, for example, Patent Documents 1 and 2).Patent Document 1 discloses an environment identifying device, an environment identifying method, a program, a recording medium, and a robot device for autonomous movement while recognizing a certain shape as an landmark and generating a map by using a camera as an external sensor.Patent Document 2 discloses an environment map creating method and a mobile robot for creating a map in such a manner as to extend an area in which surrounding object shape data has been measured by sequentially comparing (superimposing) surrounding object shape data obtained at the current time with surrounding object shape data obtained at the previous time and at a position different from the current position.Citation ListPatent LiteraturePatent Document 1: Japanese Unexamined Patent Application Publication No. 2004-110 802 APatent Document 2: Japanese Unexamined Patent Application Publication No. 2008-276,348A fusion method for odometry and GPS data for robot navigation by means of iterative Kalman smoothing is described in K. Ohno et al., Outdoor map building based on odometry and RTK-GPS positioning fusion, Robotics and Automation 2004, Proc. Of IEEE Conf. ICRA 2004, pp. 684-690. Further methods for robot navigation based on odometry and / or GPS data are also described in the following articles: K. Ohno, Outdoor navigation of a mobile robot between buildings based on DGPS and odometry data fusion, Proc. of 2003 IEEE International Conference on Robotics and Automation, 2003, pp. 1978-1984; Y. Hosoda, Autonomous moving technology for future urban transport, Hitachi Review, Next-Generation Cities for a New Era of Urban Development Vol. 60, No. 2, April 2011; and A. Henschel et al., A GPS and laser-based localization for urban and nonurban outdoor environments, Proc. of 2008 IEEE / RSJ International Conference on Intelligent Robots and System, 2008, pp. 149-154.SUMMARY OF THE INVENTIONTechnical ProblemWhen a mobile object moves and its locus is obtained, a measurement error (deviation) occurs in the obtained locus. Therefore, the time-sequential map creating path used in the related art has a problem that the cumulative error becomes larger as the area of the created map extends. In particular, when the mobile object passes a previously visited location once again by moving along another path, or when a map having multiple vehicles has been formed, measurement errors may cause the same location to be stored as another location on the map, consistency may not be maintained, and variation may occur. Consequently, the accuracy of the map decreases, and there arises a problem that the mobile object may lose the view of its location and have difficulty in moving further.The present invention has been made in view of the background described above, and is intended to calculate a highly accurate locus.Solution of the ProblemTo solve the above-mentioned problems, the present invention provides the locus correction method defined in claim 1 for correcting a moving locus of a mobile object by a locus correction device, and the locus correction device defined in claim 4. Further advantageous features are set out in the dependent claims.Advantageous Effects of the InventionAccording to the present invention, a highly accurate locus can be calculated.Brief Description of the DrawingsThe following are shown: FIG. 1 is a diagram of a configuration example of an autonomous mobile system according to an embodiment described herein, FIG. 2 is a flowchart of a processing procedure of the autonomous mobile system according to the present embodiment, FIG. 3 is a diagram for describing object shape measurement by a surrounding object shape measurement unit (part 1), FIG. 4 is a diagram for describing object shape measurement by the surrounding object shape measurement unit (part 2), FIG. 5 is a diagram showing an example of a moving locus of a mobile object, FIG. 6 is a diagram of a movement locus of a locus consisting of nodes and arcs, FIG. 7 is a diagram for explaining the definition of terms related to the present embodiment, FIG. 8 is a diagram for explaining the correction of the locus, FIG. 9 is a diagram showing an example of a corrected locus, FIG. 10 is a flowchart of a processing procedure for calculating correlations and probability distribution of errors in difference measurements by a surrounding object shape comparing unit, FIG. 11 is a flow chart of a processing procedure for calculating correlations and a probability distribution of errors in difference measurements by a comparing unit of the geographical information system, FIG. 12 is a flowchart of a processing procedure for calculating correlations and probability distribution of errors in difference measurements by a GNSS positioning unit, FIG. 13 is a flowchart of a processing procedure for calculating correlations and probability distribution of errors in difference measurements by a wheel rotation amount measurement unit, FIG. 14 is a diagram of another embodiment of an autonomous moving system according to the present embodiment (part 1); and FIG. 15 is a diagram of another embodiment of an autonomous moving system according to the present embodiment (part 2).DESCRIPTION OF EMBODIMENTSHereinafter, an embodiment for carrying out the present invention (hereinafter referred to as "embodiment") will be described in detail with appropriate reference to the drawings.[System Configuration]FIG. 1 is a diagram illustrating a configuration example of an autonomous mobile system according to the present embodiment.The autonomous mobile system 1 includes an onboard part 10 that collects all data required for constructing a map, and a managing part 20 that performs locus correction based on all the collected data.The onboard part 10 is mounted in a mobile object V that is a mobile object device such as an autonomous mobile robot and a vehicle. The management part 20 is installed in a management device such as an office building.The onboard part 10 and the management part 20 may communicate with each other via a wireless network or the like.Flange partThe onboard part 10 includes a surrounding object shape measuring unit 11, a surrounding object shape comparing unit 12, a geographical information system data obtaining unit 13, a geographical information system comparing unit 14, a GNSS (Global Navigation Satellite System) positioning unit 15, a wheel rotation amount measuring unit 16, a control unit 17, and others. In some cases, the surrounding object shape comparing unit 12, the geographical information system comparing unit 14, the GNSS positioning unit 15, and the wheel rotation amount measuring unit 16 are collectively referred to as a measurement device.The surrounding object shape measuring unit 11 measures the shape of objects (roadside structures such as buildings, alle trees, and power poles, people, other mobile objects V, etc.) existing around a mobile object V. As the surrounding object shape measuring unit 11, for example, a laser scanner, a stereo camera, a TOF (time-of-flight) range image camera, and others may be used.The surrounding object shape comparing unit 12 calculates a difference of the relative point-to-point position of the mobile object V from a previous time point by comparing (superimposing) by the surrounding object shape measuring unit 11 of the mobile object V at the current position of measured surrounding object shape data (structural shape data) with surrounding object shape data measured (over) at another position. For the comparison, for example, a method described in a publication "Range Data Processing; Technique for Generating a Shape Model from Multiple Range Images" (by Ken Masuda, Ikuko Okaya (Shimizu), Tituaki Sagawa, Negotiations of 2004, 146 may be used, among others. CVIM Conference).The geographical information system data obtaining unit 13 is a portion that obtains shape data on a map for road configurations in a city and roadside structures such as buildings, all trees, and power towers from a geographical information system including map data or the like. As the data format of the shape data on a map, a 3D city model which is recently used in a car navigation system and a CityGML (Geographic Markup Language) which is standardized by the OGC (Open Geospatial Consortium) worldwide, among others, can be used.The geographical information system comparing unit 14 calculates a difference of the relative position of the current position of the mobile object V from a previous time on a map possessed by the geographical information system by comparing (superimposing) by the surrounding object shape measuring unit 11 of the mobile object V at the current position of measured surrounding object shape data with (over) shape data on the surrounding object map in the vicinity of the current position obtained by the geographical information system data obtaining unit 13. For the comparison, the same method as used by the surrounding object shape comparing unit 12 may be used.The GNSS positioning unit 15 calculates the current position of the mobile object V in a normal coordinate system such as a planar rectangular coordinate system by using a positioning system such as GPS (Global Positioning System).The wheel rotation amount measurement unit 16 calculates a relative difference of the current position of the mobile object V from the position of the mobile object V at a previous time by accumulating wheel rotations. For the calculation, a method described in a publication "Gyrodometry: A New Method for Combining Data from Gyros and Ovary in Mobile Robots" (by Johann Borenstein and Liqiang Feng, negotiations of the 1996-held ICRA'-96 conference) can be used by using an inertial sensor called an IMU (Inertial Measurement Unit) or a gyroscopic sensor, among others.The onboard part 10 may not include all of the respective units 11 to 16, and may include at least two of them.The control part 17 performs overall control of the respective units 11 to 16.The respective units 11 to 17 are realized by executing programs stored in a ROM (Read Only Memory) or the like by a CPU (Central Processing Unit).Management PartThe management part 20 includes an evaluation function generation unit 21, a locus optimization calculation unit 22, and a shape map data generation unit 23.The evaluation function generation unit 21 is a portion that uses a difference of the relative position of the mobile object V calculated by each unit of the measurement devices 11 to 16 from a previous time point (details thereof will be described later) and generates an evaluation function with respect to a locus of the mobile object V moving in a moving environment. The locus is represented by a probability function in which a difference of each measured relative position is assigned as a variable. By determining a variable (i.e., a difference in relative position) when the evaluation function is optimized, an error of the locus is corrected. Details thereof will be described later.The locus optimization calculation unit 22 optimizes the evaluation function generated by the evaluation function generation unit 21 by assigning a difference in relative position as a variable. Thereby, the locus optimization calculation unit 22 corrects the locus of the mobile object V having a cumulative error occurring therein to a locus of the mobile object V in which no cumulative error exists, and the consistency is maintained throughout. Details thereof will be described later.The shape map data generation unit 23 generates a map without a cumulative error by copying shape data of surrounding objects measured by the surrounding object shape measurement unit 11 to the locus of the mobile object V corrected by the locus optimization calculation unit 22.The management part 20 is realized by a PC (Personal Computer) or the like, and the respective units 21 to 23 are realized by loading programs stored in a ROM or an HDD (Hard Disk Drive) into a RAM (Random Access Memory) and executing the programs by the CPU.[Flow Chart]Hereinafter, concrete details of the processing of the autonomous mobile system 1 according to the present embodiment will be described with reference to the following FIGS. 2 to 9 while referring to FIG. 1.FIG. 2 is a flowchart showing a processing procedure of the autonomous mobile system according to the present embodiment.The autonomous mobile system 1 calculates an accurate locus and generates a map based on the locus by executing the process of the flowchart illustrated in FIG. 2, so that the mobile object V can reach its destination without losing the view of its location and an intended path.While the mobile object V is moving in a moving environment during a designated interval, the surrounding object shape measuring unit 11 first measures the shapes of objects (roadside structures such as buildings, alley trees and utility poles, people, other mobile objects V, etc.) (Object shapes) present around the mobile object V (S 101). The moving mentioned here may mean autonomous moving by the mobile object V or driving of the mobile object by its driver.With reference to FIGS. 3 and 4, measurement of object shapes by the surrounding object shape measurement unit will be described.The surrounding object shape measuring unit 11 measures the shapes of objects such as roadside structures 411 that are within its measurement range 401 centered around the mobile object V moving in an area (road) 412, where it can move in FIG. 3.This is done, for example, in such a way that the surrounding object shape measuring unit 11 emits a laser beam to the environment and measures the surrounding object shapes by reflecting the laser.In order to prevent mobile objects V such as persons and other mobile objects V from being stored as map data during this measurement, the surrounding object shape measuring unit 11 may extract only surrounding object shape data within a given height range 501 as illustrated in FIG. 4. Alternatively, the surrounding object shape measuring unit 11 may extract only surrounding object shape data such as a planar surface and a pillar.Then, each unit of the measurement device 12 and 14 to 16 acquires measurement data (S 102). At the same time, each unit of the measurement devices 12 and 14 to 16 sends the measurement data to the management part 20.Next, the control unit 17 determines whether a predetermined measurement data amount has been stored (S 103).If a predetermined amount of measurement data has not been stored as a result of step 103 (No, S 103), the control unit 17 returns the process to step S 101.As a result of step 103, if a predetermined measurement data amount has been stored (Yes, S 103), the evaluation function generation unit 21 generates a graph geometry by generating nodes and arcs on a moving locus of the mobile object V generated based on the transmitted data.Now, with reference to FIGS. 5 and 6, generation of nodes and arcs by the evaluation function generation unit 21 will be described. In FIG. 5, elements corresponding to those in FIG. 3 are assigned the same reference numerals, and description thereof is omitted.In FIG. 5, a moving locus t represents an example of a moving locus along which the mobile object V has actually moved.As illustrated in FIG. 5, it is assumed that the mobile object V moves along the actual movement locus t. The movement locus t is calculated based on a distance measured from the number of wheel rotations measured by the wheel rotation amount measurement unit 16. In fact, the movement locus t is closed.FIG. 6 is a diagram for explaining a relationship between nodes and arcs.As illustrated in FIG. 6, the evaluation function generation unit 21 divides the movement locus t (FIG. 5 ) into segments of a given length, represents segment points as nodes p, generates a graph geometry by connecting the nodes p by linear arcs g, and generates a locus X.Here, the evaluation function generation unit 21 takes the position of the mobile object V calculated by accumulating wheel rotations by the wheel rotation amount measurement unit 16 as an initial position, and generates the nodes p and the arcs g.Since a cumulative error occurs in the measurements taken by the wheel rotation amount measurement unit 16, it is found that a measurement error (a deviation) occurs in the graph geometry (locus X) generated by the evaluation function generation unit 21 shown in FIG. 6 from the movement locus t shown in FIG. 5 in the actual environment. In order to correct this measurement error (this deviation) and generate an accurate map, the autonomous mobile system 1 executes subsequent steps (S 105 to S 109) of the processing procedure in the flowchart presented in FIG. 2.Let an ithnode p be called node p i and its position by vector x i. For example, if a 2D map is generated, the vector x i, the position of the node p i, is generated as expressed in Equation (1):Here, u i and v i are world coordinates, for example, and θ i is the orientation of the mobile object V.Then, the locus X represented by the nodes p and arcs g is expressed as a set of vectors x i representing the positions of n nodes p i as in equation (2):Here, n is the number of nodes.Next, with respect to the graph geometry made up of the nodes p and the arcs g generated by the evaluation function generation unit 21 in step S 101, the surrounding object shape comparison unit 12, the geographical information system comparison unit 14, the GNSS positioning unit 15, and the wheel rotation amount measurement unit 16 calculate correlations between measurements z and the nodes p, respectively, and calculate a probability distribution of errors in difference measurements between the obtained measurements z and the nodes p (S 105).Next, with reference to FIG. 7, the calculation of correlations between the measurements z and the nodes p i by each unit of the measurement device and their probability representation will be described.FIG. 7 is a diagram for explaining the definition of terms related to the present embodiment.First, as illustrated in FIG. 7( a), an example using the node p 1 and the node p 2 as positions measured by the wheel rotation amount measurement unit 16 is presented.A line connecting node p 1 and node p 2 is shown as arc g 12. Because the node p 2 has a measurement error, assuming that the measurement error satisfies a normal distribution, a true node p 2 is assumed to be within a range of the ellipse y 1. Here, a measurement error distribution (elliptic distribution y 1) of the node p 2 is represented by an accuracy matrix Ω of the normal distribution. Here, the ellipse y 1 is a covariance ellipse given by the accuracy matrix Ω. The accuracy matrix Ω is also referred to as an information matrix and corresponds to an inverse matrix of a covariance matrix of the normal distribution. Specifically, the ellipse y 1 in FIG. 7( a) defines a range of the standard deviation σ from the center around the node p 2 in the normal distribution.A small arrow q indicates the orientation of the mobile object V and corresponds to θ i in Equation (1).Next, with reference to FIG. 7( b), it is explained how to correlate a position measured by another unit of the measurement device with a node measured by the wheel rotation amount measurement unit 16.As illustrated in FIG. 7( b), it is assumed that a unit of the measurement device other than the wheel rotation amount measurement unit 16 (for example, the GNSS position determination unit 15) has measured a position. This position is referred to as measurement z(m 1) ( position data). Here, "m 1" is an identification number that identifies a unit of the measurement device (for example, the GNSS position determination unit 15) that has made the measurement z. This means that the measurement z(m 1) means the measurement z carried out by the unit m 1 of the measuring device.Then, the evaluation function generation unit 21 determines the node p to which the measurement z(m 1) should be correlated. Assuming that, for example, a correlation of the measurement z(m 1) with the node p 2 has been determined by the evaluation function generation unit 21 based on time points, this measurement z is referred to as a measurement z 2( m 1).Here, let Z 12( m 1) a line connecting the node p 1 and the measurement z 2( m 1) be a measurement of a difference in relative position (which will be referred to as difference measurement hereinafter).A component of a difference measurement Z 12( m 1) is represented by a difference between the measurement z 2 and the node p 1.The covariance ellipse of the measurement z 2( m 1) is correlated with the node p 2 (in other words, the difference measurement Z 12( m 1) and the arc g 12 are correlated). This correlation is referred to as correlation c 1,2( m 1).In this way, a node p measured by the wheel rotation amount measuring unit 16 serves as an initial value, namely, a reference value for correlation with a measurement made by another unit of the measuring device, for example.Also, since the measurement z 2( m 1) is assumed to have an error, the measurement z 2( m 1) has a covariance ellipse y 2, which is given by the accuracy matrix Ω as in the case of FIG. 7( a).As shown in FIG. 7( c), not all measurements z i( m 1) are correlated with all nodes p i. This is because each unit of the measuring device makes measurements at different intervals.As shown in FIG. 7( c), if the measurement z 4( m 1) is correlated with the node p 4 but no measurements z(m 1) are obtained to be correlated with the nodes p 2, p 3 the measurement z(m 1) in this case becomes the measurement z 4( m 1), and a differential measurement against the node p 1 becomes Z14(m1).A user arbitrarily sets a base point node from which a difference measurement is made.For example, as shown in FIG. 7( d), a given nthnode p before a node p correlated with a measurement z may be defined as a base point node. In an example of FIG. 7( d), a second node before a node correlated with a measurement z is a base point node. That is, a base point node from which a difference measurement (Z 13) to the measurement z 3( m 1) is generated is the node p 1 which is the second node before the node p 3 correlated with the measurement z 3( m 1). Also, a base point node from which a difference measurement (Z 24) to the measurement z 4( m 1) is generated is a node p 2, which is the second node before the node p 4 correlated with the measurement z 4( m 1).A path for determining a base point node from which a difference measurement is made is not limited to the path shown in FIG. 7( d).For example, as shown in FIG. 7( e), a node p last correlated with a measurement z may be defined as a base point node. As shown in FIG. 7( e), assume that the measurement z 1( m 1) and the node p 1 are correlated, the measurement z 3( m 1) and the node p 3 are correlated, and the measurement z 4( m 1) and the node p 4 are correlated. Here, a base point node from which a difference measurement (Z 13) to the measurement z 3( m 1) is generated becomes a node p 1, which was last correlated with a measurement z. Likewise, a base point node from which a difference measurement (Z 34) to the measurement z 4( m 1) was generated becomes a node p 3, which was last correlated with a measurement z. The measurement z 4( m 1) in turn becomes a base point node for a further difference measurement.Since the wheel rotation amount measurement unit 16 is also a unit of the measurement device, a node p also becomes a measurement, for example, if it is assumed in FIG. 7( a) that the identification number of the wheel rotation amount measurement unit 16 m is 0 the node p becomes 1 for measurement z 1( m 0). That is, the measurement z 1( m 0) and the node p 1 are correlated (simultaneously, measurement z 2( m 0) = node p 2). In this case, the correlation c 1,2( m 0) can be defined (not shown) because the measurement z 1( m 0) and the node p 1 are correlated.As described above, each unit of the measuring device 12 and 14 to 16 calculates measurements made therefrom z j( m k), difference measurements Zi j( m k) and correlations c i,j( m k), where i, j, k are natural numbers and i<j.The generation of a probability distribution of errors in difference measurements is described below.Based on the definition described with reference to FIG. 7, the evaluation function generation unit 21 calculates an evaluation function for correcting the locus X and correcting a most likely locus Xc by calculating the positions x of the nodes p that maximize the evaluation function.FIG. 8 is a diagram for explaining the correction of the locus. A locus shown in FIG. 8 is similar to the locus X in FIG. 6.In FIG. 8, the nodes p 0, p 1, p 2 etc. are calculated, and the measurements z 0( m 1), z 1( m 1), z 2( m 1), z 5( m1), etc. correlated with the nodes are calculated ((m1) is omitted in FIG. 8). Here, a base point node from which a difference measurement is made is defined as a third node before a node defined as correlated with a measurement.As described above, the nodes p 0, p 1, p 2 etc. can also be regarded as measurements z 0( m 0), z 1( m 0) and z 2( m 0) respectively, but their illustration is omitted to avoid complication ("m 0" is an identification number identifying the wheel rotational amount measuring unit 16).Here, if a probability of x ∈X, where x denotes the positions of all nodes under the condition that correlations c i,j( m k) ( not shown in FIG. 8 ) are defined for them, is defined by a probability density function p(x|c i,j( m k)) because it is assumed that the correlations c i,j( m k) occur independently, a probability at which the positions x occur is represented by a mixed distribution of a probability at which x j( the position of the node p j) occurs under the condition, that the correlations c i,j( m k) occur, and an equation therefor is expressed by a probability density function p(x) provided in equation (3). Here, x is x defined in equation (2).Here, <i, j> means all combinations of i, j for which correlations c i,j( m k) are defined, C is a set of correlations c i,j( m k), which may occur, and M is a set of all units of the measurement device that are used.Equation 3 means a probability with which the locus X occurs when the correlations c i,j( m k) occur.For example, when measurements z j( m 1) with respect to the identification number m 1 of a unit of the measurement device in FIG. 8 is taken as an example, the measurements correlated with nodes are {z 0( m 1), z 1( m 1), z 2( m 1), z 5( m1), z6(m1), z9(m1), z10(m1), z 13( m 1) and z 14( m 1)}. Therefore, the probability density function multiplied in equation (3) yields {p(x|c 16,0( m 1)), p(x|c 14,1( m 1)), p(x|c 15,2( m 1)), p(x|c 2,5( m 1)), p(x|c 3,6( m 1)), p(x|c6,9(m1)), p(x|c7,10(m1)), p(x|c 10,13( m 1)) and p(x|c 11,14( m 1))}. In FIG. 8, the designation m 1 is omitted.That is, because difference measurements Z ij, which relate to the correlations c i,j( m k) occur independently, the probability density function for determining a probability with which the positions x occur simultaneously is represented by multiplying p(x j| c i,j( m k)) with respect to measurements made at all units of the measurement device, as shown in Equation (3).Since the nodes p j taken by the wheel rotation amount measuring unit 16 also become measurements z j( m 0) as described above, p(x j| c i,j( m 0)) is also multiplied in equation (3).By maximizing the probability density function p(x) shown in Equation (3), the evaluation function generation unit 21 obtains a locus Xc in which no cumulative error exists and consistency is maintained throughout. Here, the locus X denotes a locus before correction and the locus Xc denotes a locus after correction. That is, maximizing the probability density function p(x) represented in Equation (3) means that the positions xjare the highest probability. The evaluation function generation unit 21 obtains these positions x and connects the obtained positions x as nodes, thereby obtaining the locus Xc of the mobile object V in which no cumulative error exists and consistency is maintained throughout.Next, a method of calculating the locus Xc by a maximum likelihood estimation method will be described.As described with reference to FIG. 7( b), if probability distributions at which the positions x of the nodes p exist on the locus X are represented by a normal distribution of the accuracy matrix Ω, the probability density function p(x|c i,j( m k)) in Equation (3) can be expressed by the following Equation (4).Here, N(·) is a probability density function representing a normal distribution. Specifically, Equation (4) is produced by a method described in a cited document as mentioned later. This equation (4) yields a probability distribution of errors in difference measurements.Returning to the description of FIG. 2, the evaluation function generation unit 21 then generates an evaluation function (S 106) based on the correlations between the measurements z and the nodes p generated by each of the surrounding object shape comparing unit 12, the geographic information system comparing unit 14, the GNSS position determining unit 15, and the wheel rotation amount measuring unit 16, and the probability distribution of errors in difference measurements.The evaluation function generation unit 21 derives an evaluation function F(x) by the following procedure.First, by assigning equation (4) to equation (3) and developing the latter according to a normal distribution formula, the following equation (5) is derived.Here, η ij is a normalized variable, and d ij( x) is a function of obtaining a difference of a relative position of the measurement z j, which is correlated with a j-th node, with respect to an i-th node (difference measurement Z ij). This function is a function of x.Ω ij is an accuracy matrix for measurements z j( m k) with correlations c i,j. Ω ij is calculated by a technique described in a cited document, as mentioned later.By obtaining natural logarithms of both sides of equation (5), equation (6) is derived.Here, "constant." is a constant. Maximizing the probability density function p(x) shown in equation (3) corresponds to maximizing the natural logarithms (likelihood function) of the probability density function p(x) shown in equation (6). Therefore, by removing the constants and negative coefficients from equation (6), an evaluation function F(x) can be formulated as in equation (7).In Equation (7), e ij is expressed by the following Equation (8).Here, maximizing the probability density function p(x) shown in Equation (3) corresponds to minimizing the evaluation function F(x) shown in Equation (7). Therefore, minimizing the evaluation function F(x) corresponds to optimizing the evaluation function F(x).That is, in step 106 in FIG. 2, the evaluation function generation unit 21 calculates the evaluation function F(x) represented in equation (7) on the basis of the measurements z j( m k) obtained from the surrounding object shape comparison unit 12, the geographical information system comparison unit 14, the GNSS position determination unit 15, and the wheel rotation amount measurement unit 16.Next, the control unit 17 determines whether the mobile object V has completed moving in the moving environment during the designated interval, i.e., determines whether the movement of the mobile object V has been completed (S 107), thereby determining whether the obtaining of the surrounding object shape data has been completed.If the result of step S 107 is that the movement has not been completed (No, S 107), the control unit 17 returns the process to step S 101.If the result of step S 107 is that the movement has been completed (Yes, S 107), the locus optimization calculation unit 22 corrects the locus X of the mobile object V having a cumulative error present therein to the locus Xc of the mobile object V in which no cumulative error is present and the consistency is maintained throughout, by optimizing the evaluation function F(x) (S 108) represented in Equation (7).As described above, maximizing the probability density function p(x) of the equation corresponds to minimizing the evaluation function F(x) represented in equations (7) and (8). That is, the locus optimization calculation unit 22 optimizes the evaluation function F(x) by obtaining the positions x of the nodes p that minimize the evaluation function F(x) by the following equation (9). At this time, the locus optimization calculation unit 22 corrects the locus X (FIG. 6 ) obtained by the wheel rotation amount measurement unit 16 to the locus Xc of the mobile object V in which no cumulative error exists and the consistency is maintained throughout, as illustrated in FIG. 9. The locus optimization calculation unit 22 minimizes the evaluation function F(x) by a equation system solving method and a nonlinear optimization method.Then, the shape map data generation unit 23 generates a map by copying, by the surrounding object shape measurement unit 11, surrounding object shape data measured to the corrected locus Xc of the mobile object V (S 109). Consequently, it is possible to generate an accurate map by estimating, after obtaining all surrounding object shape data in the moving environment for a designated interval for generating a map, the locus Xc of the mobile object V in which no cumulative error exists and maintaining the consistency over all the measurement data, and copying the measured surrounding object shape data to this locus Xc, instead of generating a map while estimating the own location by time-sequential mapping.Although according to the present embodiment, the nodes p are generated from measurement data taken by the wheel rotation amount measurement unit 16, the nodes p may also be generated from measurement data taken by another unit of the measurement device. That is, measurement data taken from another unit of the measurement device can be used as reference values.calculating the correlations and the probability distribution of errors in difference measurements by each unit of the measuring deviceNext, with reference to FIGS. 10 to 15, the processing by each unit of the measurement device in step S105 in FIG. 2 will be described.Unit for Comparing Shape of Surrounding ObjectsFIG. 10 is a flowchart showing a processing procedure of step S 105 by the surrounding object shape comparing unit.The surrounding object shape comparing unit 12 concentrates on an i-th node p i and a j-th node p j and calculates correlations of a measurement z obtained by the surrounding object shape comparing unit 12 with the node p i and the node p j by comparing (superimposing) shape data of surrounding objects measured by the surrounding object shape measuring unit 11 at the point of the i-th node p i with shape data of surrounding objects measured (over) at the point of the j-th node p j.The surrounding object shape measuring unit 11 searches for a node p present within a predetermined threshold distance from the i-th node p i, which is now targeted (S 201), and determines the matching j-th node p j.Then, the surrounding object shape measuring unit 11 compares surrounding object shape data measured at the point of the i-th node p i with surrounding object shape data measured at the point of the j-th node p j (S 202). The result of this comparison is a measurement, for example, a method held in "Range Data Processing; Technique for Generating a Shape Model from Multiple Range Images" (by Ken Masuda, Ikuko Okaya (Shimizu), Tatuaki Sagawa, Negotiations of 2004 146 may be used for this comparison, among others. CVIM Conference).Then, the surrounding object shape measuring unit 11 calculates, using the comparison result in step S 202, the probability distribution (probability density function; equation (4)) of an error in a difference measurement with respect to the measurement z j( S 203). For calculating the probability distribution of an error in a difference measurement, for example, a method described in a publication "Likelihood Distribution Calculation for Robot Orientation in Scan Matching" (by Tadahiro Tomonou, negotiations of the 2010-held RSJ'10 conference) may be used, among others.Again, the surrounding object shape measuring unit 11 determines whether the processing of steps 201 to 203 has been completed for all nodes in the moving environment for a designated map generation interval (S 204).If the result of step S 204 is that the processing has not been completed (No, step 204), the surrounding object shape measuring unit 11 returns the process to step S 201.If the result of step S 204 is that the processing has been completed (Yes, step 204), the surrounding object shape measuring unit 11 ends the process.Comparison unit 14 of the Geographical Information SystemFIG. 11 is a flowchart showing a processing procedure of step S 105 by the comparing unit of the geographical information system.The geographic information system comparing unit 14 calculates a correlation with a node and the probability distribution (probability density function; equation (4)) of an error in differential measurement of map data of the geographic information system by comparing (superimposing) by the surrounding object shape measuring unit 11 at the targeted surrounding object shape data measured node p with shape data on the map obtained by the geographic information system data obtaining unit 13.First, the geographic information system comparing unit 14 searches, via the geographic information system data obtaining unit 13, for data in the geographic information system that are present within a predetermined threshold distance from the now targeted node p i (S301).Again, the geographic information system comparing unit 14 compares surrounding object shape data measured by the surrounding object shape measuring unit 11 at the targeted node p with the searched data, i.e., shape data on the map obtained by the geographic information system data obtaining unit 13 (S302). This comparison results in measurement, for example, the same method as in the processing of step S 202 in FIG. 10 may be used for the comparison.Then, the geographic information system comparing unit 14 calculates a correlation between the measurement z and the node p using the comparison result in step S 302, and calculates the probability distribution (probability density function; equation (4)) of an error in a difference measurement (S 303). For calculating the probability distribution of an error in a difference measurement, the same method as in step S 203 in FIG. 10 may be used.GNSS Position Determination UnitFIG. 12 is a flowchart showing a processing procedure of step S 105 by the GNSS position determination unit.The GNSS position determination unit 15 calculates a correlation c i,j in a normal coordinate system such as a planar rectangular coordinate system.First, the GNSS positioning unit 15 makes a position measurement z on the now targeted node p in the normal coordinate system using GNSS (S 401).The GNSS position determination unit 15 in turn calculates a correlation with the node p and the probability distribution of an error in the measurement z (S 402) using the position measurement result (measurement z) in step S 401. For calculating the probability distribution of an error in the measurement z, information relevant to GST sets of an NMEA-0183 format, which is a communication protocol for use in GNSS, may be used, among others.RaddrehbetragsmesseinheitFIG. 13 is a flowchart showing a processing procedure of step S 105 by the wheel rotation amount measurement unit.The wheel rotation amount measurement unit 16 calculates a correlation c i,j, when a measurement z is made at a node p, namely, the current position of the mobile object V, from a node p at which the mobile object V was present at the previous time, by accumulating wheel rotations.First, the wheel rotation amount measurement unit 16 makes a measurement z at the node p j namely, the current position of the mobile object V, and forms the difference measurement Z ij with respect to the node p i, at which the mobile object V has been located at the previous time, by accumulating wheel rotations (S 501). Here, a method described in a publication "Gyrodometry: A New Method for Combining Data from Gyros and Odometry in Mobile Robots" (by Johann Borenstein and Liqiang Feng, negotiations of the 1996-held ICRA'96 conference) can be used by using an inertial sensor called an IMU (Inertial Measurement Unit) or a gyroscopic sensor, among others.Then, using the measurement result in step S 501, the wheel rotation amount measurement unit 16 calculates a correlation c i,j of the measurement z with the node p j and the probability distribution (probability density function; equation (4)) of an error in the difference measurement Z ij( S 502). For calculating the probability distribution of an error in the measurement z, for example, a method described in a book "Vehicle" (by Kimio Kanai et al., 2003 published by Corona Publishing Co., Ltd.), among others, may be used.[Other Embodiments]FIG. 14 is a diagram showing another embodiment of an autonomous moving system according to the present embodiment.Although one mobile object V and the management part 20 communicate with each other in FIG. 1, onboard parts 10 mounted in a plurality of mobile objects V 1, V 2(V) may communicate with the management part 20 as illustrated in FIG. 14. In this case, the management part 20 generates an evaluation function and obtains a locus Xc based on data corrected by the respective onboard parts 10.Alternatively, the respective onboard parts 10 may not include all the units 11 to 15 (however, all the onboard parts 10 need to include the wheel rotation amount measurement unit 16). In this case, the management part 20 may integrate measurement data collected from the respective onboard parts 10, generate an evaluation function, and obtain a locus Xc.FIG. 15 is a diagram showing another embodiment of an autonomous moving system according to the present embodiment.As illustrated in FIG. 15, the functions of the onboard part 10 and the management part 20 in FIG. 1 may be provided in an onboard part 10 aof a mobile object V.This makes it possible to correct a locus and generate a map only by the mobile object Va.[Conclusion]According to the present embodiment, by generating an evaluation function and optimizing the evaluation function on the basis of data obtained from a plurality of units of measurement equipment, it is possible to estimate a locus Xc of the mobile object V in which no cumulative error exists, and the consistency is maintained throughout the locus X of the mobile object V having an error.By copying shape data of surrounding objects to the locus Xc, an accurate map can be produced without error accumulation.Consequently, the mobile object V moves autonomously based on such a map, so that the mobile object V can reach its destination without losing the vision of its location and its intended path.In other words, according to the present embodiment, an autonomous mobile system is realized that generates an accurate map by estimating, after obtaining all shape data in the moving environment for a designated interval for generating a map, a locus of the mobile object V in which no cumulative error exists and consistency is maintained over all measurement data, and copying the shape data of surrounding objects to the locus instead of generating a map while estimating the own location by time-sequential imaging, thereby achieving the goal without losing vision of the location and a designated path.List of reference characters1 Autonomous mobile system 10, 10 a onboard part 11 surrounding object shape measuring unit 12 surrounding object shape comparing unit 13 geographical information system data obtaining unit 14 geographical information system comparing unit 15 GNSS position determining unit 16 wheel rotation amount measuring unit 17 control unit 20 management part (locus correcting device) 21 evaluation function generating unit 22 locus optimization calculating unit 23 map data generating unit V, V 1, V 2, VA mobile object (mobile object device)

Claims

A locus correction method for correcting a moving locus of a mobile object (V) by a locus correction device (20), the locus correction method being characterized in that: the locus correction device performs: setting (S104) a plurality of nodes on a locus of the mobile object based on locus data of the mobile object obtained by one unit (16) of measurement means; correlating (S105) position data of the mobile object obtained by the one unit of measurement means with the plurality of nodes; and correlating position data of the mobile object obtained by at least two units (12, 14, 15) of measurement means other than the one unit of measurement means with the plurality of nodes, calculating (S 106) an evaluation function having differences of nodes and differences of position data as variables based on probability representations for positions at which nodes may occur and for positions at which position data correlated with nodes may occur, the differences of nodes and of position data being differences in the relative positions of the locus data and the position data of the mobile object with respect to a respective preceding time, and calculating (S 108) a corrected locus on which a plurality of corrected nodes having the highest probability occur by determining the plurality of corrected nodes as a solution to the problem of optimizing the evaluation function at the given position data.The locus correction method according to claim 1, characterized in that: the probability representations are probability density functions, the evaluation function is a likelihood function, and the locus correction device (20) calculates a locus on which the plurality of nodes having the highest probability appear according to a maximum likelihood estimation method.The locus correction method according to claim 1 or 2, characterized in that: the locus correction device (20) creates a map by copying shape data of patterns around the calculated locus (S109).A locus correction device for correcting a moving locus of a mobile object (V), the locus correction device (20) characterized by comprising: an evaluation function generation unit (21) that executes: setting a plurality of nodes on a locus of the mobile object based on locus data of the mobile object obtained by a measurement device unit (16); correlating position data of the mobile object obtained by the measurement device unit with the plurality of nodes; and correlating position data of the mobile object obtained by at least two measurement device units (12, 14, 15) other than the measurement device unit with the plurality of nodes; calculating an evaluation function having differences of nodes and differences of position data as variables based on probability representations for positions, at which nodes may occur and for positions at which node-correlated position data may occur, the differences of nodes and position data being differences in the relative positions of the locus data and the position data of the mobile object with respect to a preceding time, respectively, and a locus optimization calculation unit (22) that calculates a corrected locus on which a plurality of corrected nodes are most likely to occur by determining the plurality of corrected nodes as a solution to the problem of optimizing the evaluation function at given position data.A mobile object apparatus characterized by being provided with the locus correcting device according to claim 4.

Images