WO2012160630A1 - 軌跡補正方法、軌跡補正装置および移動体装置 - Google Patents

軌跡補正方法、軌跡補正装置および移動体装置 Download PDF

Info

Publication number
WO2012160630A1
WO2012160630A1 PCT/JP2011/061685 JP2011061685W WO2012160630A1 WO 2012160630 A1 WO2012160630 A1 WO 2012160630A1 JP 2011061685 W JP2011061685 W JP 2011061685W WO 2012160630 A1 WO2012160630 A1 WO 2012160630A1
Authority
WO
WIPO (PCT)
Prior art keywords
node
trajectory
measurement
unit
probability
Prior art date
Application number
PCT/JP2011/061685
Other languages
English (en)
French (fr)
Inventor
祥尭 原
大島　章
山本　健次郎
細田　祐司
Original Assignee
株式会社日立製作所
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 株式会社日立製作所 filed Critical 株式会社日立製作所
Priority to DE112011105210.3T priority Critical patent/DE112011105210T5/de
Priority to JP2013516092A priority patent/JP5852645B2/ja
Priority to US14/118,899 priority patent/US9182235B2/en
Priority to PCT/JP2011/061685 priority patent/WO2012160630A1/ja
Publication of WO2012160630A1 publication Critical patent/WO2012160630A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching

Definitions

  • the present invention relates to a trajectory correction method for correcting a trajectory of a moving body, a trajectory correction apparatus, and a technology of the mobile body apparatus.
  • Patent Document 1 discloses an environment identification device, an environment identification method, a program, a recording medium, and a robot that use a camera as an external sensor, recognize a specific shape as a landmark, and perform autonomous movement while generating a map.
  • An apparatus is disclosed.
  • Patent Document 2 using a laser scanner as an external sensor, the shape data of a peripheral object acquired at the current time and the shape data of the peripheral object acquired at a position different from the current time at the previous time are sequentially recorded.
  • An environment map generation method and a mobile robot are disclosed in which a map is generated by enlarging a region where shape data of peripheral objects is measured by matching (superimposing) to a target object.
  • Measured error occurs in the acquired trajectory when the moving body travels. Therefore, the sequential map generation method as in the prior art has a problem that the cumulative error increases as the area of the map to be created increases. In particular, when passing a previously visited point again with a different route or when generating a map with multiple vehicles, the same point is stored as a different point on the map due to measurement error, and consistency can be maintained. May cause wrinkles. As a result, there is a problem that the accuracy of the map is deteriorated, the moving body loses its own position or the target route, and it is difficult to continue the movement.
  • the present invention has been made in view of such a background, and an object of the present invention is to calculate a highly accurate trajectory.
  • the present invention provides a trajectory correction method by a trajectory correction apparatus that corrects a trajectory traveled by a mobile object, the trajectory correction apparatus including trajectory data of the mobile object acquired by a single measurement unit.
  • the trajectory correction apparatus including trajectory data of the mobile object acquired by a single measurement unit.
  • a plurality of nodes are set, and the position data of the moving body acquired by the first measuring unit is associated with the node, and the moving body acquired by another measuring unit different from the first measuring unit
  • the position data is associated with the node, the position where the node is likely to occur is represented by probability, the position where the position data associated with the node is likely to occur is represented by probability, and each probability
  • An evaluation function including the node and the position data as variables is calculated based on the above, and a trajectory having the highest probability of occurrence of each node is calculated based on the evaluation function.
  • a highly accurate trajectory can be calculated.
  • 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 in-vehicle unit 10 that collects data necessary for map creation, and a management unit 20 that performs trajectory correction based on the collected data.
  • the in-vehicle unit 10 is mounted on a moving body V that is a moving body device such as an autonomous mobile robot or a vehicle.
  • the management unit 20 is installed in a management facility such as an office building.
  • the in-vehicle unit 10 and the management unit 20 can communicate via a wireless network or the like.
  • the in-vehicle unit 10 includes a peripheral object shape measuring unit 11, a peripheral object shape matching unit 12, a geographic information system data acquiring unit 13, a geographic information system matching unit 14, a GNSS (Global Navigation Satellite System) positioning unit 15, and a wheel rotation amount measuring unit. 16, a control unit 17 and the like.
  • the peripheral object shape matching unit 12, the geographic information system matching unit 14, the GNSS positioning unit 15, and the wheel rotation amount measuring unit 16 may be collectively referred to as measurement means.
  • the peripheral object shape measuring unit 11 measures the shape of an object (a building, a road tree, a road pole such as a power pole, a person, or another moving object V) existing around the moving object V.
  • a laser scanner, a stereo camera, a TOF (Time Of Flight) distance image camera, or the like can be used as the peripheral object shape measuring unit 11.
  • the peripheral object shape matching unit 12 receives, at the current position, the peripheral object shape data (structure shape data) measured by the peripheral object shape measurement unit 11 of the moving object V and the peripheral object shape data measured at different positions. By matching (superimposing), the difference from the previous time is calculated for the relative position between the two points of the moving object V.
  • matching for example, the paper “Distance Data Processing-Shape Model Generation Technology from Multiple Distance Images” (authors: Ken Masuda, Kyoko Okaya, Riksho Sagawa, Conference Meeting: Proc. Of the 146th CVIM, year held : 2004) can be used.
  • the geographic information system data acquisition unit 13 is a part that acquires map shape data of road structures in the city, road structures such as buildings, roadside trees, and utility poles from a geographic information system having map data.
  • map shape data As a data format of the map shape data, a city three-dimensional model used in a recent car navigation system, City GML (Geography Markup Language) standardized by OGC (Open Geospatial Consortium), or the like can be used.
  • the geographic information system matching unit 14 maps the shape data of the surrounding object measured by the surrounding object shape measuring unit 11 of the moving object V at the current position and the surrounding object map near the current position acquired by the geographic information system data acquiring unit 13. By matching (superimposing) the shape data, the difference from the previous time is calculated for the relative position of the current position of the moving object V to the map of the geographic information system. For the matching, the same method as that of the peripheral object shape matching unit 12 can be used.
  • the GNSS positioning unit 15 uses a positioning system such as GPS (Global Positioning System) to calculate the current position of the moving object V with respect to a reference coordinate system such as a planar rectangular coordinate system.
  • GPS Global Positioning System
  • the wheel rotation amount measuring unit 16 calculates the relative difference of the current position with respect to the position of the moving object V at the previous time by accumulating the wheel rotation amount.
  • an inertial sensor or gyro sensor called IMU (Inertial Measurement Unit) is used. The method described in the conference: Proc. OfRA ICRA '96, held year: 1996) can be used.
  • the in-vehicle unit 10 does not have to include all of the units 11 to 16 and only needs to have at least two of these units.
  • the control unit 17 performs overall control of the units 11 to 16.
  • Each unit 11 to 17 is realized by a program stored in a ROM (Read Only Memory) or the like being executed by a CPU (Central Processing Unit).
  • ROM Read Only Memory
  • CPU Central Processing Unit
  • the management unit 20 includes an evaluation function generation unit 21, a trajectory optimization calculation unit 22, and a shape map data generation unit 23.
  • the evaluation function generation unit 21 uses the difference in relative position (details will be described later) of the moving body V with respect to the previous time calculated by each of the measuring units 11 to 16 to move the moving body V when traveling in the traveling environment. This is a part for generating an evaluation function expressed by a probability function using a difference of each measured relative position as a variable.
  • the trajectory error is corrected by obtaining a variable (that is, a relative position difference) when the evaluation function is optimized. Details will be described later.
  • the trajectory optimization calculation unit 22 optimizes the evaluation function generated by the evaluation function generation unit 21 using a relative position difference as a variable. As a result, the trajectory optimization calculation unit 22 corrects the trajectory of the moving object V in which the accumulated error has occurred to the trajectory of the moving object V that has no accumulated error and maintains consistency throughout. Details will be described later.
  • the shape map data generation unit 23 affixes the shape data of the peripheral object measured by the peripheral object shape measurement unit 11 to the trajectory of the moving object V corrected by the trajectory optimization calculation unit 22 so that there is no accumulated error. Generate a map.
  • the management unit 20 is realized by a PC (Personal Computer) or the like, and each of the units 21 to 23 is a program stored in a ROM or HDD (Hard Disk Drive) is expanded in a RAM (Random Access Memory) and executed by the CPU. By embodying it.
  • 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 the exact trajectory by executing the processing of the flowchart shown in FIG. 2, and further generates a map based on this trajectory, so that the moving body V does not lose sight of its own position or target route. You can reach your destination.
  • the surrounding object shape measuring unit 11 is an object (a road structure such as a building, a roadside tree, or a power pole, a person or another mobile object) that exists around the mobile object V.
  • the shape (object shape) of the body V etc. is measured (S101).
  • the traveling here may be autonomous traveling by the moving body V or may be steered by a pilot.
  • the surrounding object shape measuring unit 11 measures the shape of an object such as a road structure 411 that falls within its own measurement range 401 with the moving body V traveling in the travelable region (road) 412 in FIG.
  • the peripheral object shape measuring unit 11 emits a laser to the surroundings, and measures the shape of the surrounding objects by reflection thereof.
  • the peripheral object shape measuring unit 11 is configured so that the periphery of the region 501 having a predetermined height as shown in FIG.
  • Object shape data may be extracted.
  • the peripheral object shape measurement unit 11 may extract characteristic peripheral object shape data such as a plane or a cylinder.
  • each of the measuring means 12, 14 to 16 acquires measurement data (S102). At this time, each measurement means 12, 14 to 16 transmits measurement data to the management unit 20.
  • the control unit 17 determines whether or not a predetermined amount of measurement data has been accumulated (S103). As a result of step S103, when a predetermined amount of measurement data is not accumulated (S103 ⁇ No), the control unit 17 returns the process to step S101.
  • the evaluation function generation unit 21 sets a node for the trajectory traveled by the moving object V created based on the transmitted data.
  • a graph structure is generated by generating an arc (S104).
  • FIG. 5 the same elements as those in FIG. A travel locus t is a diagram illustrating an example of a travel locus when the moving body V actually travels. As shown in FIG. 5, it is assumed that the moving object V is traveling along an actual travel locus t. The travel locus t is calculated based on the distance measured from the number of wheel rotations by the wheel rotation amount measuring unit 16. The travel locus t is actually closed.
  • FIG. 6 is a diagram for explaining the relationship between nodes and arcs.
  • the evaluation function generation unit 21 divides the travel locus t (FIG. 5) for each predetermined length, sets the divided point as a node p, and connects the nodes p with a straight arc g. Then, a graph structure is generated and a trajectory X is generated. At this time, the evaluation function generation unit 21 generates the node p and the arc g with the position of the moving body V calculated by the wheel rotation amount measurement unit 16 accumulating the wheel rotation amount as an initial position. Since a cumulative error occurs in the measurement of the wheel rotation amount measurement unit 16, the graph structure (trajectory X) generated by the evaluation function generation unit 21 shown in FIG.
  • FIG. 6 shows the travel locus t in the actual environment shown in FIG. Measurement error (deviation) has occurred.
  • the autonomous mobile system 1 executes the processing procedure (S105 to S109) of the subsequent flowchart shown in FIG.
  • the i-th node p is expressed as a node p i and its position is expressed as a vector x i .
  • the vector x i that is the position of the node p i is expressed as in Expression (1).
  • u i and v i are, for example, world coordinates
  • ⁇ i is the posture (orientation) of the moving object V.
  • the trajectory X represented by the node p and the arc g is expressed as a set of vectors x i representing the positions of the n nodes p i as shown in Expression (2).
  • n is the number of nodes.
  • the peripheral function shape matching unit 12, the geographic information system matching unit 14, the GNSS positioning unit 15, the wheel rotation are applied to the graph structure composed of the node p and the arc g generated by the evaluation function generation unit 21 in step S 104.
  • Each of the quantity measuring units 16 calculates the correspondence between the measurement z and the node p, and calculates the probability distribution of the difference measurement error between the acquired measurement z and the node p (S105).
  • FIG. 7 is a diagram for explaining definitions of terms according to the present embodiment.
  • FIG. 7 (a) shown as the position of the wheel rotation quantity measuring unit 16 has measured, the example using the node p 1 and the node p 2.
  • the measurement error distribution (elliptical distribution y 1 ) at the node p 2 is represented by a normal distribution accuracy matrix ⁇ .
  • the ellipse y 1 is a covariance ellipse represented by the accuracy matrix ⁇ .
  • the accuracy matrix ⁇ is also called an information matrix and corresponds to an inverse matrix of a normal distribution covariance matrix.
  • elliptic y1 in FIG. 7 (a) the normal distribution centered on node p 2
  • the small arrow q indicates the orientation of the moving body V and corresponds to ⁇ i in the equation (1).
  • FIG. 7B a method of associating a position measured by another measuring unit with a node measured by the wheel rotation amount measuring unit 16 will be described.
  • a position where there is a measuring means (for example, the GNSS positioning unit 15) other than the wheel rotation amount measuring unit 16 is measured.
  • This position is described as measurement z (m 1 ) (position data).
  • “m 1 ” is an identification number indicating the measurement means (for example, the GNSS positioning unit 15) that measured the measurement z. That is, the measurement z (m 1 ) means the measurement z measured by the measurement unit m 1 .
  • the evaluation function generation unit 21 determines which node p is associated with the measurement z (m 1 ). For example, if the evaluation function generation unit 21 determines that the measurement z (m 1 ) corresponds to the node p 2 based on the time and the like, the measurement z is expressed as measurement z 2 (m 1 ).
  • the node p 1 and the measurement z 2 (m 1) and the measurement of the relative position of the difference of the line connecting the (hereinafter, referred to as differential measurement) is and be expressed as Z 12 (m 1).
  • the component of the difference measurement Z 12 (m 1 ) is represented by the difference between the measurement z 2 and the node p 1 .
  • the correspondence relationship is that the covariance ellipse of the measurement z 2 (m 1 ) is associated with the node p 2 (in other words, the difference measurement Z 12 (m 1 ) and the arc g 12 are associated).
  • the node p measured by the wheel rotation amount measuring unit 16 becomes an initial value of the correspondence relationship with the measurement z measured by other measuring means, that is, a reference value.
  • the measurement z 2 (m 1 ) Since the measurement z 2 (m 1 ) is also considered to contain an error, the measurement z 2 (m 1 ) has a covariance ellipse y2 with an accuracy matrix ⁇ as in FIG.
  • the difference source for the difference measurement is arbitrarily set by the user. For example, as shown in FIG. 7D, a predetermined number of nodes p before the node p associated with the measurement z may be set as the difference source.
  • the previous node p is the difference source. That is, a difference source of differential measurement generated for measurement z 3 (m 1) (Z 13) is in two previous node p measurement z 3 (m 1) and are associated with the node p 3 and it has a certain node p 1. Further, a difference source of differential measurements are generated for measurement z 4 (m 1) (Z 24) is a measurement z 4 2 preceding node (m 1) and are associated with the node p 4 p and it has a certain node p 2.
  • the method of determining the difference source for the difference measurement is not limited to the method of FIG.
  • a node p previously associated with the measurement z may be set as the difference source.
  • the measurement z 1 (m 1 ) and the node p 1 are associated
  • the measurement z 3 (m 1 ) and the node p 3 are associated
  • the measurement z 4 Assume that (m 1 ) and node p 4 are associated with each other.
  • the difference source of the difference measurement (Z 13 ) generated for the measurement z 3 (m 1 ) is the node p 1 associated with the measurement z before that.
  • the difference source of the difference measurement (Z 34 ) generated for the measurement z 4 (m 1 ) is the node p 3 associated with the measurement z before that.
  • the measurement z 4 (m 1) is a difference source of other differential measurement.
  • the node p is also measured z.
  • the correspondence relationship c 1,2 (m 0 ) can be defined (not shown).
  • each of the measuring means 12 and 14 to 16 calculates the measurement z j (m k ), the difference measurement Z ij (m k ), and the correspondence relationship c i, j (m k ) measured by itself.
  • i, j, and k are integers, and i ⁇ j.
  • FIG. 8 is a diagram for explaining locus determination.
  • the trajectory in FIG. 8 is the same as the trajectory X in FIG. In FIG. 8, nodes p 0 , p 1 , p 2 ,... Are calculated, and the corresponding measurements z 0 (m 1 ), z 1 (m 1 ), z 2 (m 1 ), z 5 are calculated. (M 1 ),... Are calculated ((m 1 ) is omitted in FIG.
  • the difference source node is a node three nodes before the node for which the correspondence is defined.
  • the nodes p 0 , p 1 , p 2 ,... Can also be measured as z 0 (m 0 ), z 1 (m 0 ), and z 2 (m 0 ), respectively.
  • the illustration is omitted here (“m 0 ” is an identification number indicating the wheel rotation amount measuring unit 16).
  • the probability of each position x ⁇ X of all the nodes p under the condition where the correspondence c i, j (m k ) (not shown in FIG.
  • ⁇ i, j> means the combination of all i, j for which the correspondence c i, j (m k ) is defined, and C is a possible correspondence c i, j (m k ).
  • M is the set of all measurement means used.
  • equation (3) is the probability that the locus X will occur when the correspondence c i, j (m k ) occurs. For example, taking the measurement z j (m 1 ) related to the identification number m 1 of the measuring means in FIG.
  • the correspondence between the nodes is ⁇ z 0 (m 1 ), z 1 ( m 1 ), z 2 (m 1 ), z 5 (m 1 ), z 6 (m 1 ), z 9 (m 1 ), z 10 (m 1 ), z 13 (m 1 ), z 14 (m 1 ) ⁇
  • the probability density function multiplied by equation (3) is ⁇ p (x
  • the description regarding m 1 is omitted.
  • the evaluation function generation unit 21 maximizes the probability density function p (x) expressed by Equation (3) to obtain the trajectory Xc of the moving object V that has no accumulated error and maintains consistency throughout.
  • the trajectory X indicates a trajectory before correction
  • the trajectory Xc indicates a trajectory after correction. That is, Equation (3) probability density function p (x) represented by the fact that the greatest is that each position x j is occurring at the highest probability.
  • the evaluation function generation unit 21 obtains each position x as described above, and obtains a trajectory Xc of the moving object V that has no accumulated error and maintains consistency throughout by connecting the obtained positions x as nodes.
  • N (•) is a probability density function representing a normal distribution.
  • the formula (4) is generated by the method described in each reference described later. This equation (4) is a probability distribution of difference measurement errors.
  • the evaluation function generation unit 21 Based on the probability distribution of the difference measurement error, the evaluation function generation unit 21 generates an evaluation function (S106).
  • the evaluation function generation unit 21 derives the evaluation function F (x) by the following procedure. First, when Expression (4) is substituted into Expression (3) and expanded according to the normal distribution formula, the following Expression (5) is derived.
  • ⁇ ij is a normalization variable
  • d ij (x) is a relative position difference (difference measurement Z ij ) of the measurement z j corresponding to the j-th node p j viewed from the i-th node p i.
  • This is a function to obtain.
  • This function is a function of x.
  • ⁇ ij is an accuracy matrix in the measurement z j (m k ) having the correspondence c i, j .
  • ⁇ ij is calculated by means such as a technique described in each reference described later. If the natural logarithm of both sides is calculated
  • Equation (7) e ij is represented by the following equation (8).
  • maximizing the probability density function p (x) shown in Expression (3) is equivalent to minimizing the evaluation function F (x) shown in Expression (7). Therefore, minimizing the evaluation function F (x) corresponds to optimizing the evaluation function F (x). That is, in step S106 of FIG. 2, the evaluation function generation unit 21 measures the measurement z j (m acquired by the surrounding object shape matching unit 12, the geographic information system matching unit 14, the GNSS positioning unit 15, and the wheel rotation amount measuring unit 16. k )), an evaluation function F (x) shown in Expression (7) is calculated.
  • control unit 17 determines whether or not the traveling in the traveling environment targeted by the moving object V has been completed, that is, whether or not the traveling of the moving object V has been completed (S107), so that the peripheral object It is determined whether or not the shape data has been collected. If the result of step S107 is that the travel has not ended (S107 ⁇ No), the control unit 17 returns the process to step S101.
  • step S107 when the traveling is finished (S107 ⁇ Yes), the trajectory optimization calculating unit 22 optimizes the evaluation function F (x) shown in the equation (7), thereby generating a cumulative error.
  • the trajectory X of the moving object V is corrected to the trajectory Xc of the moving object V that has no accumulated error and maintains consistency throughout (S108).
  • the trajectory optimization calculation unit 22 optimizes the evaluation function F (x) by obtaining the position x of the node p that minimizes the evaluation function F (x) by the following equation (9). Accordingly, the trajectory optimization calculation unit 22 uses the trajectory X (FIG. 6) acquired by the wheel rotation amount measurement unit 16 as a moving object V having no accumulated error and maintaining consistency as shown in FIG. 9. To the locus Xc. The trajectory optimization calculation unit 22 minimizes the evaluation function F (x) by a simultaneous equation solving method or a nonlinear optimization method.
  • the shape map data generation unit 23 generates a map by attaching the shape data of the peripheral object measured by the peripheral object shape measurement unit 11 to the corrected trajectory Xc of the moving body V (S109).
  • the shape map data generation unit 23 instead of generating a map while estimating its own position by sequential matching, all the shape data of surrounding objects in the driving environment for which the map is to be generated are collected and accumulated over the entire measurement data
  • An accurate map can be generated by estimating the trajectory Xc of the moving object V having no error and maintaining consistency, and pasting the measured shape data of the surrounding objects to the trajectory Xc.
  • the node p is generated from the data measured by the wheel rotation amount measuring unit 16, but the node p may be generated from the data measured by other measuring means. That is, data measured by other measuring means may be used as the reference value.
  • FIG. 10 is a flowchart showing the procedure of the process of step S105 by the peripheral object shape matching unit.
  • the peripheral object shape matching unit 12 has the shape data of the peripheral object measured by the peripheral object shape measuring unit 11 at the point of the i-th node p i for the i-th node p i and the j-th node p j ,
  • the correspondence between the measurement z acquired by the surrounding object shape matching unit 12 and the nodes p i and p j is expressed as follows. calculate.
  • the peripheral object shape measuring unit 11 searches for a node p that is within a predetermined threshold distance from the i-th node p i that is currently targeted (S201), and determines a corresponding j-th node p j .
  • the peripheral object shape measuring unit 11 measures the peripheral object shape data measured by the peripheral object shape measuring unit 11 itself at the point of the i-th node p i and the peripheral object measured at the point of the j-th node p j. Are matched with each other (S202). The result of this matching is measurement z.
  • matching for example, "Distance data processing-Shape model generation technology from multiple distance images" (author: Takeshi Masuda, Kyoko Okaya (Shimizu), Ritsumasa Sagawa, conference: Proc. Of the 146th CVIM, 2004) can be used.
  • the peripheral object shape measurement unit 11 calculates a probability distribution (probability density function: equation (4)) of the difference measurement error in the measurement z j by using the matching result in step S202 (S203).
  • the probability distribution of the difference measurement error in the measurement z j is, for example, the paper “Calculation of likelihood distribution of robot posture in scan matching” (author: Masahiro Tomono, conference: Proc. Of RSJ'10, held year: 2010) Can be used.
  • the peripheral object shape measuring unit 11 determines whether or not the processing of steps S201 to S203 has been completed for all nodes in the travel environment that are targets of map generation (S204). If the result of step S204 is not complete (S204 ⁇ No), the peripheral object shape measuring unit 11 returns the process to step S201. If the result of step S204 is complete (S204 ⁇ Yes), the peripheral object shape measuring unit 11 ends the process.
  • FIG. 11 is a flowchart showing the procedure of the process of step S105 by the geographic information system matching unit.
  • the geographic information system matching unit 14 matches (superimposes) the shape data of the surrounding objects measured by the surrounding object shape measuring unit 11 at the target node p with reference to the map shape data obtained by the geographic information system data obtaining unit 13. ) To calculate the correspondence relationship between the map data of the geographic information system and the probability distribution of the difference measurement error (probability density function: equation (4)).
  • geographical information system matching unit 14 searches for data of the geographic information system from the node p i which is currently the subject via the geographic information system data acquiring unit 13 within a distance of a predetermined threshold value (S301). Then, the geographic information system matching unit 14 uses the searched object data with the map shape data acquired by the geographic information system data acquisition unit 13 as a reference, and the shape of the peripheral object measured by the peripheral object shape measurement unit 11 at the target node p. Data is matched (S302). The result of this matching is measurement z. For the matching, a method similar to the processing in step S202 of FIG. 10 can be used.
  • the geographic information system matching unit 14 calculates the correspondence relationship between the measurement z and the node p using the matching result in step S302, and calculates the probability distribution of the difference measurement error (probability density function: equation (4). ) Is calculated (S303).
  • the calculation of the probability distribution of the difference measurement error can use the same method as in step S203 of FIG.
  • FIG. 12 is a flowchart showing the procedure of the process of step S105 by the GNSS positioning unit.
  • the GNSS positioning unit 15 calculates a correspondence c i, j with respect to a reference coordinate system such as a planar rectangular coordinate system, for example, using a positioning system.
  • the GNSS positioning unit 15 measures the measurement z with respect to the reference coordinate system of the current target node p using GNSS (S401). Then, the GNSS positioning unit 15 calculates the correspondence relationship with the node p and the probability distribution of the error of the measurement z using the positioning result (measurement z) in step S401 (S402).
  • the probability distribution of the error of the measurement z for example, information on the GST sentence in the NMEA-0183 format, which is a communication protocol used in GNSS, can be used.
  • FIG. 13 is a flowchart showing the procedure of the process of step S105 by the wheel rotation amount measuring unit.
  • the wheel rotation amount measuring unit 16 accumulates the wheel rotation amount, thereby obtaining a correspondence relationship c i, j when the node p at the current position with respect to the node p at which the moving object V was at the previous time is measured z. calculate.
  • the wheel rotation amount measurement unit 16 sets the node p j at the current position as the measurement z with reference to the node p i at which the moving object V was at the previous time, and the difference measurement Z ij as the wheel rotation amount. Measurement is performed by accumulating (S501). At this time, using an inertial sensor or gyro sensor called IMU, the paper “Gyrodometry: A New Method for Combining Data from Gyros and Odometry in Mobile Robots” (Author: Johann Borenstein, and Liqiang Feng, Conference Meeting: Proc. Of The method described in ICRA '96, held year: 1996) can be used.
  • the wheel rotation quantity measuring unit 16 uses the measurement result in the step S501, the node p j corresponding relation c i of the measurement z for, j and the probability distribution of the error in the difference measurement Z ij (probability density function: Equation (4)) is calculated (S502).
  • the method described in the book “Vehicle” (author: Kimio Kanai et al., Publisher: Corona, publication year: 2003) can be used to calculate the probability density distribution of measurement z errors.
  • FIG. 14 is a diagram illustrating another embodiment of the autonomous traveling system according to the present embodiment.
  • one mobile unit V and the management unit 20 communicate with each other.
  • the in-vehicle unit 10 mounted on a plurality of mobile units V1, V2 (V) Communication with the management unit 20 may be performed.
  • the management unit 20 generates an evaluation function based on the measurement data collected from each in-vehicle unit 10 and obtains the trajectory Xc.
  • each in-vehicle unit 10 may not have the respective units 11 to 15 (however, all the in-vehicle units 10 need to have the wheel rotation amount measuring unit 16).
  • the management unit 20 may integrate the measurement data collected from each in-vehicle unit 10 to generate an evaluation function and obtain the trajectory Xc.
  • FIG. 15 is a diagram showing another embodiment of the autonomous traveling system according to the present embodiment.
  • the functions of the in-vehicle unit 10 and the management unit 20 in FIG. 1 may be provided in the in-vehicle unit 10 a of the moving body V. Thereby, it is possible to correct the trajectory and create a map only by the moving body Va.
  • an evaluation function is generated based on data obtained from a plurality of measurement means, and the evaluation function is optimized, so that the trajectory X of the moving object V including a measurement error can be obtained throughout. It is possible to estimate the trajectory Xc of the moving object V that has no accumulated error and maintains consistency. Then, by pasting the shape data of the surrounding objects on the locus Xc, it is possible to generate an accurate map without accumulating errors. Thereby, the moving body V can reach the destination without losing sight of its own position or the target route by moving the moving body V autonomously based on such a map.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)
  • Instructional Devices (AREA)

Abstract

　精度の高い軌跡を算出することを目的とする。　移動体（Ｖ）が走行した軌跡を補正する管理部（２０）であって、車輪回転量測定部（１６）が取得した移動体（Ｖ）の軌跡データに、複数のノードを設定し、車輪回転量測定部（１６）が取得した移動体（Ｖ）の位置データを、ノードに対応付けるとともに、車輪回転量測定部（１６）とは別の他の測定手段が取得した移動体（Ｖ）の位置データを、ノードに対応付け、ノードが生起する可能性がある位置を確率で示し、ノードに対応付けられた位置データが生起する可能性がある位置を確率で示し、各確率を基に、ノードおよび位置データを変数として含む評価関数を算出する評価関数生成部（２１）と、評価関数を基に、各ノードが生起する確率が最も大きい軌跡を算出する軌跡最適化計算部（２２）と、を有することを特徴とする。

Description

軌跡補正方法、軌跡補正装置および移動体装置
　本発明は、移動体の軌跡を補正する軌跡補正方法、軌跡補正装置および移動体装置の技術に関する。
　従来、移動体に搭載された測定装置（内界センサや外界センサ）によって測定されたデータに基づいて、実環境に対応した地図を生成しながら、自己位置を推定して目標経路に従って移動する自律移動システム（例えば、特許文献１，２を参照）が開示されている。
　特許文献１には、カメラを外界センサとして用いて、特定の形状をランドマーク（目印）として認識し、地図を生成しながら自律移動を行う環境同定装置、環境同定方法、プログラム、記録媒体およびロボット装置が開示されている。
　特許文献２には、レーザスキャナを外界センサとして用いて、現在時刻に取得した周辺物体の形状データと、１つ前の時刻に現在とは異なる位置で取得した周辺物体の形状データとを逐次的にマッチング（重ね合わせ）することにより、周辺物体の形状データを測定した領域を拡大していくことで地図を生成する環境地図生成方法および移動ロボットが開示されている。
特開２００４－１１０８０２号公報 特開２００８－２７６３４８号公報
　移動体が走行し、取得した軌跡において、測定誤差（ずれ）が発生する。そのため、従来技術のような逐次的な地図生成の手法は、作成する地図の領域が拡大するにつれて累積誤差が大きくなるという問題がある。特に、以前に訪れた地点を異なる道順で再度通過した際や、複数台の車両で地図を生成した際に、測定誤差によって同一の地点が地図上では異なる地点として記憶され、整合性が保てずに齟齬が発生する場合がある。その結果、地図の正確性が悪化し、移動体が自己位置や目標経路を見失い、移動の継続が困難になるなどの問題点がある。
　このような背景に鑑みて本発明がなされたのであり、本発明は、精度の高い軌跡を算出することを目的とする。
　前記課題を解決するため、本発明は、移動体が走行した軌跡を補正する軌跡補正装置による軌跡補正方法であって、前記軌跡補正装置は、１の測定手段が取得した前記移動体の軌跡データに、複数のノードを設定し、前記１の測定手段が取得した前記移動体の位置データを、前記ノードに対応付けるとともに、前記１の測定手段とは別の他の測定手段が取得した前記移動体の位置データを、前記ノードに対応付け、前記ノードが生起する可能性がある位置を確率で示し、前記ノードに対応付けられた位置データが生起する可能性がある位置を確率で示し、各確率を基に、前記ノードおよび前記位置データを変数として含む評価関数を算出し、前記評価関数を基に、各ノードが生起する確率が最も大きい軌跡を算出することを特徴とする。
　本発明によれば、精度の高い軌跡を算出することができる。
本実施形態に係る自律移動システムの構成例を示す図である。 本実施形態に係る自律移動システムの処理手順を示すフローチャートである。 周辺物体形状測定部による物体形状測定の説明を行うための図である（その１）。 周辺物体形状測定部による物体形状測定の説明を行うための図である（その２）。 移動体の走行軌跡の例を示す図である。 ノードとアークで構成される軌跡の例を示す図である。 本実施形態に係る各用語の定義を説明するための図である。 軌跡確定を説明するための図である。 修正された軌跡の例を示す図である。 周辺物体形状マッチング部による対応関係と差分測定の誤差の確率分布を算出する処理の手順を示すフローチャートである。 地理情報システムマッチング部による対応関係と差分測定の誤差の確率分布を算出する処理の手順を示すフローチャートである。 ＧＮＳＳ測位部による対応関係と差分測定の誤差の確率分布を算出する処理の手順を示すフローチャートである。 車輪回転量測定部による対応関係と差分測定の誤差の確率分布を算出する処理の手順を示すフローチャートである。 本実施形態に係る自律走行システムの別の実施形態を示す図である（その１）。 本実施形態に係る自律走行システムの別の実施形態を示す図である（その２）。
　次に、本発明を実施するための形態（「実施形態」という）について、適宜図面を参照しながら詳細に説明する。
［システム構成］
　図１は、本実施形態に係る自律移動システムの構成例を示す図である。
　自律移動システム１は、地図作成に必要な各データを収集する車載部１０と、収集された各データを基に、軌跡補正を行う管理部２０とを有する。
　車載部１０は、例えば、自律移動ロボットや乗物などの移動体装置である移動体Ｖに搭載されている。また、管理部２０は、例えば、オフィスビルなどの管理設備に設置されている。
　車載部１０と、管理部２０とは、無線ネットワークなどを介して通信可能である。
（車載部）
　車載部１０は、周辺物体形状測定部１１、周辺物体形状マッチング部１２、地理情報システムデータ取得部１３、地理情報システムマッチング部１４、ＧＮＳＳ（Global Navigation Satellite System）測位部１５、車輪回転量測定部１６、制御部１７などを有している。なお、周辺物体形状マッチング部１２、地理情報システムマッチング部１４、ＧＮＳＳ測位部１５、車輪回転量測定部１６をまとめて、測定手段と称することがある。
　周辺物体形状測定部１１は、移動体Ｖの周辺に存在する物体（建物や街路樹や電柱などの道路構造物、人や他の移動体Ｖなど）の形状を測定する。周辺物体形状測定部１１として、例えば、レーザスキャナやステレオカメラやＴＯＦ（Time Of Flight）距離画像カメラなどを用いることができる。
　周辺物体形状マッチング部１２は、現在位置において、移動体Ｖの周辺物体形状測定部１１が測定した周辺物体の形状データ（構造物の形状データ）と、異なる位置で測定した周辺物体の形状データをマッチング（重ね合わせ）することにより、移動体Ｖの２地点間の相対的な位置について、前時刻からの差分を算出する。マッチングには、例えば、論文「距離データ処理　－　複数距離画像からの形状モデル生成技術」（著者：増田　健，　岡谷（清水）郁子，　佐川　立昌、学会会議：Proc. of the 146th CVIM、開催年：２００４年）に記載の方法などを用いることができる。
　地理情報システムデータ取得部１３は、都市内の道路形状や、建物や、街路樹や、電柱などの道路構造物の地図形状データを、地図データなどを有する地理情報システムから取得する部分である。地図形状データのデータ形式として、近年のカーナビゲーションシステムにおいて利用されている都市３次元モデルや、ＯＧＣ（Open Geospatial Consortium）により世界標準化されているＣｉｔｙＧＭＬ（Geography Markup Language）などを用いることができる。
　地理情報システムマッチング部１４は、現在位置において、移動体Ｖの周辺物体形状測定部１１が測定した周辺物体の形状データと、地理情報システムデータ取得部１３が取得した現在位置付近の周辺物体の地図形状データとをマッチング（重ね合わせ）することにより、地理情報システムが有している地図に対する移動体Ｖの現在位置の相対的な位置について、前時刻からの差分を算出する。マッチングには、周辺物体形状マッチング部１２と同様の方法を用いることができる。
　ＧＮＳＳ測位部１５は、ＧＰＳ（Global Posioning System）などの測位システムを用いて、例えば、平面直角座標系などの基準座標系に対する移動体Ｖの現在位置を算出する。
　車輪回転量測定部１６は、車輪回転量を累積することで、前時刻における移動体Ｖの位置に対する現在位置の相対的な差分を算出する。算出には、ＩＭＵ（Inertial Measurement Unit）と呼ばれる慣性センサやジャイロセンサを用いて、論文「Gyrodometry : A New Method for Combining Data from Gyros and Odometry in Mobile Robots」（著者：Johann Borenstein, and Liqiang Feng、学会会議：Proc. of ICRA’96、開催年：１９９６年）に記載の方法などを用いることができる。
　なお、車載部１０は、各部１１～１６のすべてを有していなくてもよく、これらのうち、最低２つを有していればよい。
　制御部１７は、各部１１～１６を統括制御する。
　各部１１～１７は、ＲＯＭ（Read Only Memory）などに格納されたプログラムが、ＣＰＵ（Central Processing Unit）によって実行されることによって具現化する。
（管理部）
　管理部２０は、評価関数生成部２１、軌跡最適化計算部２２、形状地図データ生成部２３を有している。
　評価関数生成部２１は、各測定手段１１～１６によって算出された移動体Ｖの前時刻に対する相対的な位置の差分（詳細は後記）を用いて、走行環境中を走行した際の移動体Ｖの軌跡に対して、それぞれの測定された相対的な位置の差分を変数とし、確率関数で表現した評価関数を生成する部分である。この評価関数を最適化したときの変数（つまり、相対的な位置の差）を求めることで、軌跡の誤差を訂正する。詳細は後記する。
　軌跡最適化計算部２２は、評価関数生成部２１が生成した評価関数を、相対的な位置の差を変数として最適化する。これにより、軌跡最適化計算部２２は、累積誤差が発生している移動体Ｖの軌跡を、全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡に修正する。詳細に関しては後記する。
　形状地図データ生成部２３は、軌跡最適化計算部２２によって修正された移動体Ｖの軌跡に対して周辺物体形状測定部１１が測定した周辺物体の形状データを貼付することで、累積誤差のない地図を生成する。
　管理部２０は、ＰＣ（Personal Computer）などで実現され、各部２１～２３はＲＯＭや、ＨＤＤ（Hard Disk Drive）に格納されたプログラムが、ＲＡＭ（Random Access Memory）に展開され、ＣＰＵによって実行されることによって具現化する。
［フローチャート］
　次に、図１を参照しつつ、図２から図９に沿って本実施形態に係る自律移動システム１の具体的な処理内容について説明する。
　図２は、本実施形態に係る自律移動システムの処理手順を示すフローチャートである。
　自律移動システム１が図２に示すフローチャートの処理を実行することで正確な軌跡を算出し、さらにこの軌跡を基に地図を生成することで、移動体Ｖが自己位置や目標経路を見失うことなく目的地に到達することができる。
　まず、移動体Ｖが対象となる走行環境を走行しつつ、周辺物体形状測定部１１が移動体Ｖの周辺に存在する物体（建物や街路樹や電柱などの道路構造物、人や他の移動体Ｖなど）の形状（物体形状）を測定する（Ｓ１０１）。なお、ここでの走行とは、移動体Ｖによる自律走行でもよいし、操縦者によって操縦されてもよい。
　図３および図４を参照して、周辺物体形状測定部による物体形状測定の説明を行う。
　図３における走行可能領域（道路）４１２を走行している移動体Ｖを中心とし、周辺物体形状測定部１１が、自身の測定範囲４０１に入る道路構造物４１１などの物体形状を測定する。
　これは、例えば、周辺物体形状測定部１１がレーザを周囲に放射し、その反射によって周囲の物体の形状を測定する。
　この際、人や他の移動体Ｖなどの移動体Ｖを地図に記憶してしまうことを避けるため、周辺物体形状測定部１１は、図４に示すように所定の高さの領域５０１の周辺物体の形状データを抽出してもよい。あるいは、周辺物体形状測定部１１は平面や円柱などの、特徴的な周辺物体の形状データを抽出するようにしてもよい。
　そして、各測定手段１２，１４～１６が、測定のデータを取得する（Ｓ１０２）。このとき、各測定手段１２，１４～１６は管理部２０へ測定のデータを送信する。
　次に、制御部１７が、測定のデータが所定量蓄積されたか否かを判定する（Ｓ１０３）。
　ステップＳ１０３の結果、測定のデータが所定量蓄積されていない場合（Ｓ１０３→Ｎｏ）、制御部１７はステップＳ１０１へ処理を戻す。
　ステップＳ１０３の結果、測定のデータが所定量蓄積されている場合（Ｓ１０３→Ｙｅｓ）、送信されたデータを基に作製される移動体Ｖが走行した軌跡に対して、評価関数生成部２１がノードとアークを生成する（Ｓ１０４）ことによって、グラフ構造を生成する。
　ここで、図５および図６を参照して、評価関数生成部２１によるノードとアークの生成を説明する。図５において、図３と同様の要素については、同一の符号を付して説明を省略する。
　走行軌跡ｔは移動体Ｖが実際に走行した際の走行軌跡の例を示す図である。
　図５に示すように、移動体Ｖが実際の走行軌跡ｔに沿って走行しているものとする。走行軌跡ｔは、車輪回転量測定部１６による車輪の回転数から測定される距離を基に算出されるものである。なお、走行軌跡ｔは実際には閉じている。
　図６は、ノードとアークの関係を説明するための図である。
　図６に示すように、評価関数生成部２１は、走行軌跡ｔ（図５）を所定の長さ毎に区切り、区切った地点をノードｐとし、ノードｐ間を直線のアークｇで繋ぐことで、グラフ構造を生成し、軌跡Ｘを生成する。
　この際、評価関数生成部２１は、車輪回転量測定部１６が車輪回転量を累積することで算出した移動体Ｖの位置を初期位置として、ノードｐとアークｇを生成する。
　車輪回転量測定部１６の測定には累積誤差が発生するため、図６に示す評価関数生成部２１が生成したグラフ構造（軌跡Ｘ）には、図５に示す実際の環境での走行軌跡ｔとは測定誤差（ずれ）が発生してしまっている。自律移動システム１は、この測定誤差（ずれ）を修正して正確な地図を生成するために、図２に示す以降のフローチャートの処理手順（Ｓ１０５～Ｓ１０９）を実行する。
　ｉ番目のノードｐをノードｐｉと表現し、その位置をベクトルｘｉで表現することとする。例えば２次元の地図を生成する場合、ノードｐｉの位置であるベクトルｘｉは式（１）のように表現される。
ｘｉ＝（ｕｉ，ｖｉ，θｉ）　・・・　（１）
　ここで、ｕｉ，ｖｉは、例えば、世界座標であり、θｉは移動体Ｖの姿勢（向き）である。
　このとき、ノードｐとアークｇで表される軌跡Ｘは、式（２）に示すようにｎ個のノードｐｉの位置を表すベクトルｘｉの集合として表現される。
Ｘ＝（ｘ１，・・・，ｘｎ）Ｔ　・・・　（２）
　ここで、ｎはノード数である。
　次に、評価関数生成部２１によって、ステップＳ１０４で生成されたノードｐとアークｇから成るグラフ構造に対して、周辺物体形状マッチング部１２、地理情報システムマッチング部１４、ＧＮＳＳ測位部１５、車輪回転量測定部１６のそれぞれが、測定ｚと、ノードｐとの対応関係を算出し、取得した測定ｚと、ノードｐ間の差分測定の誤差の確率分布を算出する（Ｓ１０５）。
　以下、図７を参照して、各測定手段による測定ｚと、ノードｐｉとの対応関係の算出、および、その確率的表現について説明する。
　図７は、本実施形態に係る各用語の定義を説明するための図である。
　まず、図７（ａ）に示すように、車輪回転量測定部１６が測定した位置として、ノードｐ１とノードｐ２を用いた例を示す。
　ノードｐ１と、ノードｐ２とを結ぶ線をアークｇ１２と表現する。ノードｐ２は、測定誤差を含んでいるので、その測定誤差は正規分布に従うと仮定すると、真のノードｐ２は楕円ｙ１の範囲内に存在すると考えられる。ここで、ノードｐ２の測定誤差の分布（楕円分布ｙ１）を正規分布の精度行列Ωによって表現される。ここで、楕円ｙ１は、精度行列Ωが表す共分散楕円である。精度行列Ωは情報行列とも呼ばれ、正規分布の共分散行列の逆行列に相当する。具体的には、図７（ａ）における楕円ｙ１は、正規分布において、ノードｐ２を中心とし、その中心から標準偏差σの範囲を示している。
　なお、小さな矢印ｑは、移動体Ｖの姿勢の向きを示すものであり、式（１）のθｉに相当する。
　次に、図７（ｂ）を参照して、他の測定手段が測定した位置を車輪回転量測定部１６が測定したノードに対応付ける方法を示す。
　図７（ｂ）に示すように、車輪回転量測定部１６以外の測定手段（例えば、ＧＮＳＳ測位部１５）がある位置を測定したとする。この位置を測定ｚ（ｍ１）（位置データ）と記載する。ここで、「ｍ１」とは、測定ｚを測定した測定手段（例えば、ＧＮＳＳ測位部１５）を示す識別番号である。つまり、測定ｚ（ｍ１）とは、測定手段ｍ１によって測定された測定ｚを意味する。
　そして、測定ｚ（ｍ１）が、どのノードｐに対応付けられるのかを評価関数生成部２１が判定する。例えば、時刻などを基に、測定ｚ（ｍ１）がノードｐ２に対応すると評価関数生成部２１によって判定されたとすると、この測定ｚを測定ｚ２（ｍ１）と表現することとする。
　ここで、ノードｐ１と測定ｚ２（ｍ１）とを結ぶ線を相対的な位置の差分の測定（以下、差分測定と称する）をＺ１２（ｍ１）と表現することとする。差分測定Ｚ１２（ｍ１）の成分は、測定ｚ２と、ノードｐ１との差で表現される。
　また、測定ｚ２（ｍ１）の共分散楕円がノードｐ２と対応付けられている（言い換えれば、差分測定Ｚ１２（ｍ１）とアークｇ１２が対応付けられている）ことを対応関係ｃ１，２（ｍ１）と示すこととする。
　このように、車輪回転量測定部１６によって測定されたノードｐは、その他の測定手段によって測定された測定ｚとの対応関係の初期値、つまり基準値となる。
　測定ｚ２（ｍ１）も誤差を含んでいると考えられるので、測定ｚ２（ｍ１）は図７（ａ）と同様に精度行列Ωによる共分散楕円ｙ２を有している。
　なお、図７（ｃ）に示すように、すべての測定ｚｉ（ｍ１）が、すべてのノードｐｉと対応付けられるわけではない。これは、各測定手段の測定間隔がまちまちであることに由来する。
　図７（ｃ）のように、測定ｚ４（ｍ１）がノードｐ４と対応付けられ、ノードｐ２，ｐ３と対応付けられる測定ｚ（ｍ１）が得られなかった場合、この測定ｚ（ｍ１）は、測定ｚ４（ｍ１）となり、差分測定は、ノードｐ１に対する差分測定はＺ１４（ｍ１）となる。
　なお、差分測定の差分元は、ユーザが任意に設定する。
　例えば、図７（ｄ）に示すように、測定ｚと対応付けられているノードｐの所定個前のノードｐが差分元とされてもよい。図７（ｄ）の例では、２つ前のノードｐが差分元となっている。つまり、測定ｚ３（ｍ１）に対して生成される差分測定（Ｚ１３）の差分元は、測定ｚ３（ｍ１）と対応付けられているノードｐ３の２つ前のノードｐであるノードｐ１となっている。また、測定ｚ４（ｍ１）に対して生成される差分測定（Ｚ２４）の差分元は、測定ｚ４（ｍ１）と対応付けられているノードｐ４の２つ前のノードｐであるノードｐ２となっている。
　差分測定の差分元の決定方法は、図７（ｄ）の方法に限らない。
　例えば、図７（ｅ）に示すように、その前に測定ｚとが対応付けられたノードｐが差分元とされてもよい。図７（ｅ）に示されているように、測定ｚ１（ｍ１）とノードｐ１とが対応付けられ、測定ｚ３（ｍ１）とノードｐ３とが対応付けられ、測定ｚ４（ｍ１）とノードｐ４とが対応付けられているとする。ここで、測定ｚ３（ｍ１）に対して生成される差分測定（Ｚ１３）の差分元は、その前に測定ｚと対応付けられているノードｐ１となる。同様に、測定ｚ４（ｍ１）に対して生成される差分測定（Ｚ３４）の差分元は、その前に測定ｚと対応付けられているノードｐ３となる。そして、測定ｚ４（ｍ１）は、他の差分測定の差分元となる。
　なお、車輪回転量測定部１６も測定手段の１つであるので、ノードｐも測定ｚとなる。例えば、図７（ａ）において、車輪回転量測定部１６の識別番号をｍ０とすると、ノードｐ１は測定ｚ１（ｍ０）となる。つまり、測定ｚ１（ｍ０）と、ノードｐ１とが対応付けられる（このとき、測定ｚ２（ｍ０）＝ノードｐ２）。この場合、測定ｚ１（ｍ０）と、ノードｐ１とが対応付けられているので、対応関係ｃ１，２（ｍ０）を定義することができる（図示せず）。
　以上のように、各測定手段１２，１４～１６は自身が測定した測定ｚｊ（ｍｋ）、差分測定Ｚｉｊ（ｍｋ）、対応関係ｃｉ，ｊ（ｍｋ）を算出する。ここで、ｉ，ｊ，ｋは整数であり、ｉ＜ｊである。
　以下、差分測定の誤差の確率分布の生成について説明する。
　図７で説明した定義を基に、評価関数生成部２１は軌跡Ｘを確定するための評価関数を算出し、この評価関数が最大となる各ノードｐの各位置ｘを算出することにより、最も尤もらしい軌跡Ｘｃを確定する。
　図８は、軌跡確定を説明するための図である。なお、図８における軌跡は、図６における軌跡Ｘと同じものである。
　図８では、ノードｐ０，ｐ１，ｐ２，・・・が算出されており、それに対応する測定ｚ０（ｍ１），ｚ１（ｍ１），ｚ２（ｍ１），ｚ５（ｍ１），・・・が算出されている（図８では（ｍ１）は省略）。ここで、差分元のノードは、対応関係が定義されたノードから３つ前のノードとしている。
　なお、前記したようにノードｐ０，ｐ１，ｐ２，・・・も、それぞれ測定ｚ０（ｍ０），ｚ１（ｍ０），ｚ２（ｍ０）とすることができるが、煩雑になるのを避けるため、ここでは図示省略してある（「ｍ０」は車輪回転量測定部１６を示す識別番号）。
　ここで、対応関係ｃｉ，ｊ（ｍｋ）（図８において図示せず）が定義されている条件下におけるすべてのノードｐの各位置ｘ∈Ｘの確率を確率密度関数ｐ（ｘ｜ｃｉ，ｊ（ｍｋ））で表すと、対応関係ｃｉ，ｊ（ｍｋ）は独立に生起すると見なされるため、位置ｘが生起する確率は、対応関係ｃｉ，ｊ（ｍｋ）が生起する条件におけるｘｊ（ノードｐｊの位置）が生起する確率の混合分布で表現され、その式は式（３）で示される確率密度関数ｐ（ｘ）で示される。ここで、ｘは式（２）で定義されるｘである。
Figure JPOXMLDOC01-appb-M000001
  
　ここで、＜ｉ，ｊ＞は、対応関係ｃｉ，ｊ（ｍｋ）が定義されるすべてのｉ，ｊの組み合わせの意味であり、Ｃは生じ得る対応関係ｃｉ，ｊ（ｍｋ）の集合であり、Ｍは、使用されているすべての測定手段の集合である。
　式（３）が意味するところは、対応関係ｃｉ，ｊ（ｍｋ）が生じたときに軌跡Ｘが生起する確率である。
　例えば、図８における測定手段の識別番号ｍ１に関する測定ｚｊ（ｍ１）を例にすると、ノードとの間で対応関係がとれているのは、｛ｚ０（ｍ１），ｚ１（ｍ１），ｚ２（ｍ１），ｚ５（ｍ１），ｚ６（ｍ１），ｚ９（ｍ１），ｚ１０（ｍ１），ｚ１３（ｍ１），ｚ１４（ｍ１）｝であるので、式（３）にて乗算される確率密度関数は、｛ｐ（ｘ｜ｃ１６，０（ｍ１）），ｐ（ｘ｜ｃ１４，１（ｍ１）），ｐ（ｘ｜ｃ１５，２（ｍ１）），ｐ（ｘ｜ｃ２，５（ｍ１）），ｐ（ｘ｜ｃ３，６（ｍ１）），ｐ（ｘ｜ｃ６，９（ｍ１）），ｐ（ｘ｜ｃ７，１０（ｍ１）），ｐ（ｘ｜ｃ１０，１３（ｍ１）），ｐ（ｘ｜ｃ１１，１４（ｍ１））｝となる。なお、図８ではｍ１に関する記載を省略している。
　つまり、対応関係ｃｉ，ｊ（ｍｋ）を条件とした差分測定Ｚｉｊは独立に生起するため、各位置ｘが同時に生起する確率の確率密度関数は、式（３）に示すようにｐ（ｘｊ｜ｃｉ，ｊ（ｍｋ））をすべての測定手段による測定に関して乗算することで示される。
　なお、前記したように車輪回転量測定部１６によるノードｐｊも測定ｚｊ（ｍ０）となるため、式（３）ではｐ（ｘｊ｜ｃｉ，ｊ（ｍ０））も乗算される。
　評価関数生成部２１は、式（３）で示される確率密度関数ｐ（ｘ）を最大化することで、全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃを求める。ここで、軌跡Ｘは補正前の軌跡を示し、軌跡Ｘｃは補正後の軌跡を示している。つまり、式（３）で示される確率密度関数ｐ（ｘ）が最も大きいということは、各位置ｘｊが最も高い確率で生起することである。評価関数生成部２１は、このような各位置ｘを求め、求められた各位置ｘをノードとして結ぶことで全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃを求める。
　以下、最尤推定法による軌跡Ｘｃの算出方法を説明する。
　図７（ｂ）で説明したように、軌跡Ｘにおける各ノードｐの位置ｘが存在する確率的位置を、精度行列Ωの正規分布で表現すると、式（３）における確率密度関数ｐ（ｘ｜ｃｉ，ｊ（ｍｋ））は、以下の式（４）で示される。
Figure JPOXMLDOC01-appb-M000002
  
　ここでＮ（・）は、正規分布を表す確率密度関数である。具体的には、後記する各参考文献に記載の方法で、式（４）が生成される。この式（４）が、差分測定の誤差の確率分布となる。
　次に、図２の説明へ戻り、周辺物体形状マッチング部１２、地理情報システムマッチング部１４、ＧＮＳＳ測位部１５、車輪回転量測定部１６のそれぞれによって生成された測定ｚとノードｐとの対応関係、および、差分測定の誤差の確率分布を基に、評価関数生成部２１が評価関数を生成する（Ｓ１０６）。
　評価関数生成部２１は、評価関数Ｆ（ｘ）を以下のような手順で導出する。
　まず、式（３）に式（４）を代入して正規分布の公式に従って展開すると、以下の式（５）が導かれる。
Figure JPOXMLDOC01-appb-M000003
  
　ここでηｉｊは正規化変数、ｄｉｊ（ｘ）はｉ番目のノードｐｉから見たｊ番目のノードｐｊに対応する測定ｚｊの相対的な位置の差分（差分測定Ｚｉｊ）を求めるための関数である。この関数はｘの関数となっている。また、Ωｉｊは、対応関係ｃｉ，ｊを有する測定ｚｊ（ｍｋ）における精度行列である。Ωｉｊは、後記する各参考文献に記載の技術などの手段で算出される。
　式（５）に関して両辺の自然対数を求めると、式（６）が導かれる。
Figure JPOXMLDOC01-appb-M000004
  
　ここで「ｃｏｎｓｔ．」は定数である。式（３）に示した確率密度関数ｐ（ｘ）を最大化することは、式（６）に示した確率密度関数ｐ（ｘ）の自然対数（尤度関数）を最大化することと等価である。このため、式（６）から定数とマイナスの係数を除去することで、式（７）のように評価関数Ｆ（ｘ）を定式化できる。
Figure JPOXMLDOC01-appb-M000005
  
　なお、式（７）において、ｅｉｊは、以下の式（８）で示されるものである。
ｅｉｊ＝ｄｉｊ（ｘ）－Ｚｉｊ（ｍｋ）　・・・　（８）
　ここで、式（３）に示した確率密度関数ｐ（ｘ）を最大化することは、式（７）に示す評価関数Ｆ（ｘ）を最小化することと等価となる。よって、評価関数Ｆ（ｘ）を最小化することが、評価関数Ｆ（ｘ）を最適化することに相当する。
　すなわち、図２のステップＳ１０６において、評価関数生成部２１は、周辺物体形状マッチング部１２、地理情報システムマッチング部１４、ＧＮＳＳ測位部１５、車輪回転量測定部１６によって取得された測定ｚｊ（ｍｋ）を基に、式（７）に示す評価関数Ｆ（ｘ）を算出する。
　次に、制御部１７が移動体Ｖが対象とする走行環境中の走行を完了したか否か、すなわち、移動体Ｖの走行が終了したか否かを判定する（Ｓ１０７）ことで、周辺物体の形状データを収集し終えたか否かを判定する。
　ステップＳ１０７の結果、走行終了していない場合（Ｓ１０７→Ｎｏ）、制御部１７は、ステップＳ１０１へ処理を戻す。
　ステップＳ１０７の結果、走行終了している場合（Ｓ１０７→Ｙｅｓ）、軌跡最適化計算部２２が、式（７）に示される評価関数Ｆ（ｘ）を最適化することで、累積誤差が発生している移動体Ｖの軌跡Ｘを、全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃに修正する（Ｓ１０８）。
　前記したように、式（３）の確率密度関数ｐ（ｘ）を最大化することは、式（７）および式（８）に示す評価関数Ｆ（ｘ）を最小化することと等価となる。すなわち、軌跡最適化計算部２２が、以下の式（９）によって、評価関数Ｆ（ｘ）を最小化するノードｐの位置ｘを求めることで、評価関数Ｆ（ｘ）を最適化を行う。これにより、軌跡最適化計算部２２は、車輪回転量測定部１６によって取得された軌跡Ｘ（図６）を、図９に示されるような全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃに修正する。なお、軌跡最適化計算部２２は、連立方程式解法や、非線形最適化法によって評価関数Ｆ（ｘ）を最小化する。
Figure JPOXMLDOC01-appb-M000006
  
　そして、形状地図データ生成部２３は、修正された移動体Ｖの軌跡Ｘｃに対して周辺物体形状測定部１１が測定した周辺物体の形状データを貼付することで、地図生成を行う（Ｓ１０９）。これにより、逐次的なマッチングにより自己位置を推定しながら地図を生成するのではなく、地図を生成する対象とする走行環境中における周辺物体の形状データをすべて収集した後に、測定データの全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃを推定し、この軌跡Ｘｃに対して測定した周辺物体の形状データを貼付することで正確な地図を生成することができる。
　なお、本実施形態では、車輪回転量測定部１６による測定のデータからノードｐを生成したが、他の測定手段による測定のデータからノードｐを生成してもよい。つまり、他の測定手段による測定のデータを基準値としてもよい。
（各測位手段における対応関係および差分測定の誤差の確率分布の算出）
　次に、図１０～図１５を参照して、図２のステップＳ１０５における各測定手段の処理を説明する。
（周辺物体形状マッチング部）
　図１０は、周辺物体形状マッチング部によるステップＳ１０５の処理の手順を示すフローチャートである。
　周辺物体形状マッチング部１２は、ｉ番目のノードｐｉとｊ番目のノードｐｊに対して、ｉ番目のノードｐｉの地点で周辺物体形状測定部１１が測定した周辺物体の形状データと、ｊ番目のノードｐｊの地点で測定した周辺物体の形状データをマッチング（重ね合わせ）することにより、周辺物体形状マッチング部１２が取得した測定ｚと、ノードｐｉ，ｐｊとの対応関係を算出する。
　周辺物体形状測定部１１は、現在対象としているｉ番目のノードｐｉから所定の閾値以内の距離にあるノードｐを探索し（Ｓ２０１）、対応するｊ番目のノードｐｊを決定する。
　次に、周辺物体形状測定部１１は、ｉ番目のノードｐｉの地点で周辺物体形状測定部１１自身が測定した周辺物体の形状データと、ｊ番目のノードｐｊの地点で測定した周辺物体の形状データをマッチングする（Ｓ２０２）。このマッチングの結果が測定ｚとなる。マッチングには、例えば、「距離データ処理　－　複数距離画像からの形状モデル生成技術」（著者：増田　健，　岡谷（清水）郁子，　佐川　立昌、学会会議：Proc. of the 146th CVIM,開催年：２００４年）に記載の方法などを用いることができる。
　次に、周辺物体形状測定部１１は、ステップＳ２０２でのマッチング結果を用いて、測定ｚｊにおける差分測定の誤差の確率分布（確率密度関数：式（４））を算出する（Ｓ２０３）。測定ｚｊにおける差分測定の誤差の確率分布は、例えば、論文「スキャンマッチングにおけるロボット姿勢の尤度分布計算」（著者：友納　正裕、学会会議：Ｐｒｏｃ．　ｏｆ　ＲＳＪ’１０、開催年：２０１０年）に記載の方法などを用いることができる。
　そして、周辺物体形状測定部１１は、地図生成の対象である走行環境中のすべてのノードついて、ステップＳ２０１～Ｓ２０３の処理が完了したか否かを判定する（Ｓ２０４）。
　ステップＳ２０４の結果、完了していない場合（Ｓ２０４→Ｎｏ）、周辺物体形状測定部１１はステップＳ２０１へ処理を戻す。
　ステップＳ２０４の結果、完了している場合（Ｓ２０４→Ｙｅｓ）、周辺物体形状測定部１１は処理を終了する。
（地理情報システムマッチング部）
　図１１は、地理情報システムマッチング部によるステップＳ１０５の処理の手順を示すフローチャートである。
　地理情報システムマッチング部１４は、地理情報システムデータ取得部１３が取得した地図形状データを基準として、対象とするノードｐで周辺物体形状測定部１１が測定した周辺物体の形状データをマッチング（重ね合わせ）することにより、地理情報システムが有する地図データに対するノードとの対応関係および差分測定の誤差の確率分布（確率密度関数：式（４））を算出する。
　まず、地理情報システムマッチング部１４は、地理情報システムデータ取得部１３を介して現在対象としているノードｐｉから所定の閾値以内の距離にある地理情報システムのデータを探索する（Ｓ３０１）。
　そして、地理情報システムマッチング部１４は、探索したデータを地理情報システムデータ取得部１３が取得した地図形状データを基準として、対象とするノードｐで周辺物体形状測定部１１が測定した周辺物体の形状データをマッチングする（Ｓ３０２）。このマッチングの結果が測定ｚとなる。マッチングには、図１０のステップＳ２０２の処理と同様の方法を用いることができる。
　次に、地理情報システムマッチング部１４は、ステップＳ３０２でのマッチング結果を用いて、測定ｚとノードｐとの対応関係を算出し、差分測定の誤差の確率分布（確率密度関数：式（４））を算出する（Ｓ３０３）。差分測定の誤差の確率分布の算出は、図１０のステップＳ２０３と同様の方法を用いることができる。
（ＧＮＳＳ測位部）
　図１２は、ＧＮＳＳ測位部によるステップＳ１０５の処理の手順を示すフローチャートである。
　ＧＮＳＳ測位部１５は、測位システムを用いて、例えば、平面直角座標系などの基準座標系に対する対応関係ｃｉ，ｊを算出する。
　まず、ＧＮＳＳ測位部１５は、現在対象としているノードｐの基準座標系に対する測定ｚを、ＧＮＳＳを用いて測位する（Ｓ４０１）。
　そして、ＧＮＳＳ測位部１５は、ステップＳ４０１での測位結果(測定ｚ）を用いて、ノードｐに対する対応関係、および、測定ｚの誤差の確率分布を算出する（Ｓ４０２）。測定ｚの誤差の確率分布は、例えば、ＧＮＳＳで用いられる通信プロトコルであるＮＭＥＡ‐０１８３フォーマットのＧＳＴセンテンスの情報などを用いることができる。
（車輪回転量測定部）
　図１３は、車輪回転量測定部によるステップＳ１０５の処理の手順を示すフローチャートである。
　車輪回転量測定部１６は、車輪回転量を累積することで、移動体Ｖが１つ前の時刻にいたノードｐに対する現在位置のノードｐを測定ｚとしたときの対応関係ｃｉ，ｊを算出する。
　まず、車輪回転量測定部１６は、移動体Ｖが１つ前の時刻にいたノードｐｉを基準として、現在位置のノードｐｊを測定ｚとし、その差分測定Ｚｉｊを、車輪回転量を累積することで測定する（Ｓ５０１）。この際に、ＩＭＵと呼ばれる慣性センサやジャイロセンサを用いて、論文「Gyrodometry : A New Method for Combining Data from Gyros and Odometry in Mobile Robots」（著者：Johann Borenstein, and Liqiang Feng、学会会議：Proc. of ICRA’96、開催年：１９９６年）に記載の方法などを用いることができる。
　次に、車輪回転量測定部１６は、ステップＳ５０１での測定結果を用いて、ノードｐｊに対する測定ｚの対応関係ｃｉ，ｊと、差分測定Ｚｉｊの誤差の確率分布（確率密度関数：式（４））を算出する（Ｓ５０２）。測定ｚの誤差の確率密度分布の算出は、例えば、書籍「ビークル」（著者：金井　喜美雄　ほか、出版社：コロナ社、出版年：２００３年）に記載の方法などを用いることができる。
［別の実施形態］
　図１４は、本実施形態に係る自律走行システムの別の実施形態を示す図である。
　図１では、１台の移動体Ｖと、管理部２０とが通信を行っているが、図１４に示すように複数台の移動体Ｖ１，Ｖ２（Ｖ）に搭載された車載部１０が、管理部２０と通信を行うようにしてもよい。この場合、管理部２０は、各々の車載部１０から収集した測定のデータを基に、評価関数を生成し、軌跡Ｘｃを求める。
　あるいは、各々の車載部１０が、各部１１～１５を有していなくてもよい（ただし、車輪回転量測定部１６は、すべての車載部１０が有する必要がある）。この場合、管理部２０は、各々の車載部１０から収集した測定のデータを統合して、評価関数を生成し、軌跡Ｘｃを求めるようにしてもよい。
　図１５は、本実施形態に係る自律走行システムの別の実施形態を示す図である。
　図１５に示すように、移動体Ｖの車載部１０ａに図１の車載部１０および管理部２０の機能が設けられてもよい。
　これにより、移動体Ｖａのみで軌跡の補正および地図の作成が可能となる。
［まとめ］
　本実施形態によれば、複数の測定手段から得られたデータを基に、評価関数を生成し、その評価関数を最適化することにより、測定誤差を含む移動体Ｖの軌跡Ｘを、全体にわたって累積誤差がなく整合性を保った移動体Ｖの軌跡Ｘｃを推定することができる。そして、その軌跡Ｘｃに、周辺物体の形状データを貼付することで、誤差が累積しない正確な地図の生成が可能となる。
　これにより、このような地図を基に、移動体Ｖが自律移動することで、移動体Ｖは自己位置や目標経路を見失うことなく目的地に到達することができる。
　つまり、本実施形態によれば、逐次的なマッチングにより自己位置を推定しながら地図を生成するのではなく、地図を生成する対象とする走行環境中の形状データをすべて収集した後に、測定データの全体にわたって累積誤差がなく整合性を保った移動体の軌跡を推定し、当該軌跡に対して測定した周辺物体の形状データを貼付することで正確な地図を生成し、自己位置や目標経路を見失うことなく目的地に到達する自律移動システムを実現する。
　１　　　自律移動システム
　１０，１０ａ　車載部
　１１　　周辺物体形状測定部
　１２　　周辺物体形状マッチング部
　１３　　地理情報システムデータ取得部
　１４　　地理情報システムマッチング部
　１５　　ＧＮＳＳ測位部
　１６　　車輪回転量測定部
　１７　　制御部
　２０　　管理部（軌跡補正装置）
　２１　　評価関数生成部
　２２　　軌跡最適化計算部
　２３　　形状地図データ生成部
　Ｖ，Ｖ１，Ｖ２，Ｖａ　移動体（移動体装置）

Claims (5)

  1. 　移動体が走行した軌跡を補正する軌跡補正装置による軌跡補正方法であって、
    　前記軌跡補正装置は、
    　１の測定手段が取得した前記移動体の軌跡データに、複数のノードを設定し、
    　前記１の測定手段が取得した前記移動体の位置データを、前記ノードに対応付けるとともに、前記１の測定手段とは別の他の測定手段が取得した前記移動体の位置データを、前記ノードに対応付け、
    　前記ノードが生起する可能性がある位置を確率で示し、
    　前記ノードに対応付けられた位置データが生起する可能性がある位置を確率で示し、
    　各確率を基に、前記ノードおよび前記位置データを変数として含む評価関数を算出し、
    　前記評価関数を基に、各ノードが生起する確率が最も大きい軌跡を算出する
    　ことを特徴とする軌跡補正方法。
  2. 　前記確率は、確率密度関数で示されており、
    　前記評価関数は、尤度関数であり、
    　前記軌跡補正装置は、
    　最尤推定法によって、前記各ノードが生起する確率が最も大きい軌跡を算出する
    　ことを特徴とする請求の範囲第１項に記載の軌跡補正方法。
  3. 　前記軌跡補正装置は、
    　前記算出された軌跡の周囲に、構造物の形状データを貼付することにより、地図データを作成する
    　ことを特徴とする請求の範囲第１項または請求の範囲第２項に記載の軌跡補正方法。
  4. 　移動体が走行した軌跡を補正する軌跡補正装置であって、
    　１の測定手段が取得した前記移動体の軌跡データに、複数のノードを設定し、
    　前記１の測定手段が取得した前記移動体の位置データを、前記ノードに対応付けるとともに、前記１の測定手段とは別の他の測定手段が取得した前記移動体の位置データを、前記ノードに対応付け、
    　前記ノードが生起する可能性がある位置を確率で示し、
    　前記ノードに対応付けられた位置データが生起する可能性がある位置を確率で示し、
    　各確率を基に、前記ノードおよび前記位置データを変数として含む評価関数を算出する評価関数生成部と、
    　前記評価関数を基に、各ノードが生起する確率が最も大きい軌跡を算出する軌跡最適化計算部と、
    　を有することを特徴とする軌跡補正装置。
  5. 　請求の範囲第４項に記載の軌跡補正装置を搭載している
    　ことを特徴とする移動体装置。
PCT/JP2011/061685 2011-05-20 2011-05-20 軌跡補正方法、軌跡補正装置および移動体装置 WO2012160630A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
DE112011105210.3T DE112011105210T5 (de) 2011-05-20 2011-05-20 Ortskurvenkorrekturverfahren, Ortskurvenkorrekturvorrichtung und mobiles Objektgerät
JP2013516092A JP5852645B2 (ja) 2011-05-20 2011-05-20 軌跡補正方法、軌跡補正装置および移動体装置
US14/118,899 US9182235B2 (en) 2011-05-20 2011-05-20 Locus correcting method, locus correcting apparatus, and mobile object equipment
PCT/JP2011/061685 WO2012160630A1 (ja) 2011-05-20 2011-05-20 軌跡補正方法、軌跡補正装置および移動体装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2011/061685 WO2012160630A1 (ja) 2011-05-20 2011-05-20 軌跡補正方法、軌跡補正装置および移動体装置

Publications (1)

Publication Number Publication Date
WO2012160630A1 true WO2012160630A1 (ja) 2012-11-29

Family

ID=47216735

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2011/061685 WO2012160630A1 (ja) 2011-05-20 2011-05-20 軌跡補正方法、軌跡補正装置および移動体装置

Country Status (4)

Country Link
US (1) US9182235B2 (ja)
JP (1) JP5852645B2 (ja)
DE (1) DE112011105210T5 (ja)
WO (1) WO2012160630A1 (ja)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014122894A (ja) * 2012-12-19 2014-07-03 Toyota Motor Engineering & Manufacturing North America Inc 車両のための装置及び方法、及び、その方法を実行するための命令を含む記憶媒体
JP2018504650A (ja) * 2014-12-26 2018-02-15 ヘーレ グローバル ベスローテン フェンノートシャップ 装置の位置特定のための幾何学的指紋法
CN108303075A (zh) * 2017-12-29 2018-07-20 广州斯马特信息科技有限公司 轨迹生成方法和系统

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6878045B2 (ja) * 2017-02-28 2021-05-26 国立研究開発法人理化学研究所 点群データの抽出方法、及び点群データの抽出装置
DE102017212603A1 (de) * 2017-07-21 2019-01-24 Robert Bosch Gmbh Verfahren zum Bereitstellen und zum Verbessern einer Positionswahrscheinlichkeitsverteilung für GNSS-Empfangsdaten
KR102063534B1 (ko) * 2017-11-30 2020-01-09 주식회사 모빌테크 라이다를 이용한 지도 생성 방법
JP6974189B2 (ja) * 2018-01-16 2021-12-01 株式会社豊田中央研究所 地図作成装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002357437A (ja) * 1984-06-07 2002-12-13 Tele Atlas North America Inc 移動体追跡情報提供装置
JP2008002906A (ja) * 2006-06-21 2008-01-10 Toyota Motor Corp 測位装置
JP2008026282A (ja) * 2006-07-25 2008-02-07 Toyota Motor Corp 測位装置、ナビゲーションシステム
JP2008175786A (ja) * 2007-01-22 2008-07-31 Zhencheng Hu 移動体位置検出方法および移動体位置検出装置

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0779998B1 (de) * 1994-09-06 1998-03-25 Siemens Aktiengesellschaft Verfahren zur bestimmung der position einer landmarke in der umgebungskarte einer selbstbeweglichen einheit, deren abstand zur einheit dynamisch von dieser ermittelt wird
JP2004110802A (ja) 2002-08-26 2004-04-08 Sony Corp 環境同定装置、環境同定方法、プログラム及び記録媒体、並びにロボット装置
JP2008076348A (ja) 2006-09-25 2008-04-03 Dainippon Toryo Co Ltd 塗料中の六価クロムの分析方法
JP4788722B2 (ja) * 2008-02-26 2011-10-05 トヨタ自動車株式会社 自律移動ロボット、自己位置推定方法、環境地図の生成方法、環境地図の生成装置、及び環境地図のデータ構造

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002357437A (ja) * 1984-06-07 2002-12-13 Tele Atlas North America Inc 移動体追跡情報提供装置
JP2008002906A (ja) * 2006-06-21 2008-01-10 Toyota Motor Corp 測位装置
JP2008026282A (ja) * 2006-07-25 2008-02-07 Toyota Motor Corp 測位装置、ナビゲーションシステム
JP2008175786A (ja) * 2007-01-22 2008-07-31 Zhencheng Hu 移動体位置検出方法および移動体位置検出装置

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014122894A (ja) * 2012-12-19 2014-07-03 Toyota Motor Engineering & Manufacturing North America Inc 車両のための装置及び方法、及び、その方法を実行するための命令を含む記憶媒体
JP2018504650A (ja) * 2014-12-26 2018-02-15 ヘーレ グローバル ベスローテン フェンノートシャップ 装置の位置特定のための幾何学的指紋法
US10145956B2 (en) 2014-12-26 2018-12-04 Here Global B.V. Geometric fingerprinting for localization of a device
CN108303075A (zh) * 2017-12-29 2018-07-20 广州斯马特信息科技有限公司 轨迹生成方法和系统

Also Published As

Publication number Publication date
DE112011105210T5 (de) 2014-05-28
JP5852645B2 (ja) 2016-02-03
JPWO2012160630A1 (ja) 2014-07-31
US9182235B2 (en) 2015-11-10
US20140088863A1 (en) 2014-03-27

Similar Documents

Publication Publication Date Title
JP5852645B2 (ja) 軌跡補正方法、軌跡補正装置および移動体装置
KR102425272B1 (ko) 디지털 지도에 대한 위치를 판별하기 위한 방법 및 시스템
US11726208B2 (en) Autonomous vehicle localization using a Lidar intensity map
US20190073794A1 (en) Survey data processing device, survey data processing method, and survey data processing program
JP5927735B2 (ja) 地図データ作成装置、自律移動システムおよび自律移動制御装置
US9098088B2 (en) Method for building outdoor map for moving object and apparatus thereof
WO2012086029A1 (ja) 自律移動システム
US20230083965A1 (en) Map construction method, apparatus and storage medium
CN105044668A (zh) 一种基于多传感器装置的wifi指纹数据库构建方法
CN110887493B (zh) 基于局部地图匹配的轨迹推算方法、介质、终端和装置
US20200364883A1 (en) Localization of a mobile unit by means of a multi-hypothesis kalman filter method
US11798225B2 (en) 3D building generation using topology
JP2017090239A (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
WO2024027350A1 (zh) 车辆定位方法、装置、计算机设备、存储介质
US11578991B2 (en) Method and system for generating and updating digital maps
US11670169B2 (en) Systems and methods for distributed cooperative localization in a connected vehicular platform
WO2017175433A1 (ja) 地図データ生成システムおよび地図データ生成方法
CN112487234B (zh) 基于分裂型层次聚类的轨迹分析方法、设备及存储介质
JP2019174191A (ja) データ構造、情報送信装置、制御方法、プログラム及び記憶媒体
EP3663973A1 (en) Automatic detection and positioning of structure faces
KR102506411B1 (ko) 차량의 위치 및 자세 추정 방법, 장치 및 이를 위한 기록매체
CN112747752A (zh) 基于激光里程计的车辆定位方法、装置、设备和存储介质
WO2019188874A1 (ja) データ構造、情報処理装置、及び地図データ生成装置
Tang et al. Enhanced indoor positioning through human-robot collaboration
CN118244785B (zh) 空地双模的两栖无人机及其定位方法、装置、存储介质

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 11865972

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2013516092

Country of ref document: JP

Kind code of ref document: A

WWE Wipo information: entry into national phase

Ref document number: 14118899

Country of ref document: US

Ref document number: 1120111052103

Country of ref document: DE

Ref document number: 112011105210

Country of ref document: DE

122 Ep: pct application non-entry in european phase

Ref document number: 11865972

Country of ref document: EP

Kind code of ref document: A1