JP2021021565A - Position estimation device, position estimation method and navigation body - Google Patents

Abstract

To provide a position estimation device capable of improving the position estimation accuracy without using GPS even when navigating on flat terrain.SOLUTION: A position estimation device includes: a storage unit that stores actual sensor information and a position calculation result for each time when a specified timing is reached, which are respectively output from a sensor of the navigation body and the inertial navigation system, and map information in which reference sensor information is associated with position coordinates to identify the position by collating with the actual sensor information; a candidate route generation unit that generates multiple candidate routes that indicate time series of estimated positions based on the position calculation results at each of the multiple specified timings, including the time when the current position is estimated; an evaluation value calculation unit that calculates an evaluation value based on time series of the reference sensor information obtained based on the map information and time series of the actual sensor information at each of the plurality of estimated positions of the plurality of candidate routes; and a position identification unit that identifies the self-position based on the evaluation value.SELECTED DRAWING: Figure 1

Description

The present disclosure relates to a technique for estimating the position of a navigation body.

The inertial navigation system (INS) estimates its own position based on the integral of the inertial force, but the estimated position of the inertial navigation system alone gradually deviates from the true position due to the integration error. For this reason, conventionally, a method of estimating the current self-position using an inertial navigation system and a GPS (Global Positioning System) has been known (for example, Patent Documents 1 and 2). However, when GPS is used, GPS may not be available when the GPS signal does not reach, for example, when navigating in an area such as underwater, or when the reception of the GPS signal is obstructed.

In such a case, it is conceivable to apply a method of estimating the self-position using an inertial navigation system and a map matching technique (for example, Patent Document 3). In the map matching technology, a map in which external environmental information such as altitude, gravitational potential, and landscape at each position (coordinates) of the running area is recorded (associated) in advance is prepared. Then, at the actual position, the information detected by using a sensor (measuring instrument) such as an altimeter, a gravimeter, or a camera is collated (matched) with the map information to estimate the self-position. Since this map is prepared so that it can be used in advance, even if an external signal such as GPS cannot be used, it is calculated by the inertial navigation system based on the collation result between the detection information detected by the sensor at the site and the map information. It is possible to correct the calculation result (estimation result) of the calculated position (INS position) and improve the estimation accuracy of the self-position.

Japanese Unexamined Patent Publication No. 2001-91294 Japanese Unexamined Patent Publication No. 9-119843 Japanese Unexamined Patent Publication No. 2006-78286

As described above, in the map matching technology, the self-position is estimated by collating the information on the map (reference information) with the measured value etc. (detection information) by the sensor at the time of self-estimation at the site. When navigating in an area where the change in detection information is scarce (flat terrain, etc.), it is difficult to determine which position in the nearby area. Not only that, the result of map matching may indicate a displaced position due to the above-mentioned flat terrain, etc. In such a case, the estimated position by the inertial navigation system is the result of map matching. If it is corrected with, it can also be a factor of increasing the error. To solve such a problem, it is conceivable to discriminate minute differences and perform map matching by using a sensor with high resolution, but the cost becomes large.

In view of the above circumstances, at least one embodiment of the present invention provides a position estimation device capable of improving the position estimation accuracy even when navigating on flat terrain without using GPS. The purpose is.

(1) The position estimation device according to at least one embodiment of the present invention is
It is a position estimation device that estimates the current position of the navigation body.
A first storage unit configured to store the actual sensor information and the position calculation result for each arrival of the specified timing output from the sensor and the inertial navigation system of the navigation body, respectively.
A second storage unit configured to store the map information associated with the position coordinates by the reference sensor information for specifying the position by collating with the actual sensor information.
A candidate route generation unit configured to generate a plurality of candidate routes indicating a time series of estimated positions based on the position calculation results at each of a plurality of times when the specified timing is reached, including the time of estimating the current position. ,
Based on the time series of the reference sensor information obtained based on the map information and the time series of the actual sensor information at each of the plurality of estimated positions each of the plurality of candidate routes, the plurality of candidate routes An evaluation value calculation unit configured to calculate the evaluation value of each certainty,
A position specifying unit configured to specify the current position based on the evaluation value of each of the plurality of candidate routes is provided.

According to the configuration of (1) above, one of the plurality of candidate routes at each time when the specified timing obtained from the position calculation result by the inertial navigation is obtained for the plurality of candidate routes that the navigating body may have passed. Generate by selecting and arranging in chronological order. By collating each candidate route generated in this way with the map information, a time series of sensor information (reference sensor information) that should be obtained when the vehicle passes through each candidate route is obtained, and the reference sensor information is obtained. The certainty of each candidate route is evaluated by calculating an error sequence by comparing the time series with the time series of actual sensor information having the same specified timing such as the estimated time. Then, the current position is specified (estimated) by setting the latest position included in the candidate route having the highest evaluation value as the current position.

In other words, if the current position of the navigating body is the expected value of the actual sensor information at the time when a plurality of specified timings actually detected by measurement or the like arrives, and only the degree of agreement of the reference sensor information at the time of estimating the current position is sufficient. However, in consideration of the degree of coincidence with the reference sensor information at the time of the arrival of the specified timing that goes back one or more times, the current position of the navigation body 7 is determined based on the candidate route having a high degree of coincidence with the time series of the actual sensor information. presume. In this way, GPS is obtained by matching the entire candidate route with the map information by comparing the actual sensor information corresponding to each position of the plurality of candidate routes generated based on inertial navigation with the reference sensor information. The current position of the navigator can be estimated appropriately without using it.

In addition, when the position calculation result by inertial navigation (INS position described later) is corrected by the map matching technique, the self-position is estimated by collating the map information (reference sensor information) with the detection information by the sensor. When navigating in an area where there is little change in sensor information (flat terrain, etc.), it is difficult to narrow down the position by collation, but as described above, by estimating through the generation of candidate routes, flat terrain, etc. Even if the navigator enters the region from the middle, the current position can be estimated with higher accuracy without using a high-precision measuring instrument that can detect slight undulations.

(2) In some embodiments, in the configuration of (1) above,
The evaluation value calculation unit
The first acquisition unit that acquires the time series of the reference sensor information of the candidate route, and
A second acquisition unit that acquires the time series of the actual sensor information of the candidate route, and
An error sequence calculation unit that calculates an error sequence that is a time series of errors calculated based on a comparison between the acquired time series of the reference sensor information and the time series of the actual sensor information.
It has an evaluation unit that calculates the evaluation value based on the error sequence.

According to the configuration of (2) above, for each of the plurality of candidate routes, the time series of the reference sensor information when the navigating body passes through the candidate routes is acquired based on the map information, and the actual measurement or the like is detected. The error sequence is calculated by comparing with the time series of the actual sensor information obtained by. Then, the evaluation value of each candidate route is calculated based on the error sequence obtained by comparing the actual sensor information actually detected during the navigation of the vehicle with the time series. As a result, the evaluation value can be appropriately calculated according to the error based on the comparison of the actual sensor information with the time series, such that the smaller the error in the error sequence, the higher the evaluation value. Therefore, it is possible to select a highly accurate candidate route that matches the time series of the actual actual sensor information, and it is possible to appropriately estimate the current position.

(3) In some embodiments, in the configurations (1) and (2) above,
The candidate route generation unit
A candidate position acquisition unit that acquires a plurality of candidate positions that are candidates for the current position,
It has a candidate route calculation unit that calculates the plurality of candidate routes by calculating the estimated positions at each of the predetermined timings up to the specified number of times before each of the plurality of candidate positions.

According to the configuration of (3) above, for each of the plurality of candidate positions in the range of the estimated positions obtained by the inertial navigation system, for example, using a state transition matrix or the like, when the specified timing in the past arrives. By tracing back the position, a plurality of candidate routes corresponding to each of the plurality of candidate positions are calculated. This makes it possible to generate a plurality of suitable candidate routes.

(4) In some embodiments, in the configurations (1) and (2) above,
The candidate route generation unit
A candidate route acquisition unit for acquiring the candidate route and
It has a generation unit that calculates a new candidate route based on the acquired candidate route.

According to the configuration of (4) above, a new candidate route with a small error is calculated based on the candidate route obtained by inputting or the like. As a result, a plurality of candidate routes can be appropriately generated.

(5) In some embodiments, in the configurations (1) and (2) above,
The candidate route generation unit
A weather estimation position extraction unit that extracts a predetermined number of the estimated positions at each of the plurality of times when the specified timing arrives,
It has a combination unit that generates the plurality of candidate routes by brute force combining the predetermined number of estimated positions for each of the extracted predetermined timings at the time of arrival.

According to the configuration of (5) above, for example, all the estimated positions that are possible as the positions of the navigating body at each of the arrival of the plurality of specified timings are extracted, and each of the extracted specified timings is extracted at each time of arrival. By generating candidate routes Rs by brute force between a plurality of estimated positions, it is possible to comprehensively generate all expected candidate routes Rs.

(6) The navigating body according to at least one embodiment of the present invention is
Inertial navigation system that calculates the position of the navigating body by inertial navigation,
With at least one sensor
The above (1) to (5) for estimating the current position of the traveling body based on the position calculation result by the inertial navigation device and the actual sensor information which is the detection information detected by the at least one sensor. The position estimation device according to any one of the items and
A navigation control device for controlling the navigation of the navigation body based on the current position estimated by the position estimation device is provided.
According to the configuration of (6) above, the same effect as that of (1) above is obtained.

(7) The position estimation method according to at least one embodiment of the present invention is
It is a position estimation method that estimates the current position of the navigation body.
The first storage step for storing the actual sensor information and the position calculation result for each arrival of the specified timing output from the sensor and the inertial navigation system of the navigation body, respectively.
The second storage step of storing the map information in which the reference sensor information for identifying the position by collating with the actual sensor information is associated with the position coordinates, and
A candidate route generation step for generating a plurality of candidate routes indicating a time series of estimated positions based on the position calculation result at each of a plurality of times when the specified timing arrives, including the time of estimating the current position, and
Based on the time series of the reference sensor information obtained based on the map information and the time series of the actual sensor information at each of the plurality of estimated positions each of the plurality of candidate routes, the plurality of candidate routes Evaluation value calculation step to calculate the evaluation value of each certainty, and
A position specifying step for specifying the current position based on the evaluation value of each of the plurality of candidate routes is provided.
According to the configuration of (7) above, the same effect as that of (1) above is obtained.

According to at least one embodiment of the present invention, there is provided a position estimation device capable of improving the position estimation accuracy even when navigating on flat terrain without using GPS.

It is a figure which shows schematic function of the position estimation apparatus which concerns on one Embodiment of this invention. It is a figure which shows the state at the time of traveling of the navigation body provided with the position estimation device which concerns on one Embodiment of this invention. It is a block diagram which shows the function of the candidate route generation part which concerns on one Embodiment of this invention, and generates a candidate route based on a candidate position. It is a block diagram which shows the function of the candidate route generation part which concerns on one Embodiment of this invention, and generates a new candidate route based on a candidate route. It is a block diagram which shows the function of the candidate route generation part which concerns on one Embodiment of this invention, and generates a new candidate route by brute force of an estimated position. It is a figure which shows the position estimation method which concerns on one Embodiment of this invention.

Hereinafter, some embodiments of the present invention will be described with reference to the accompanying drawings. However, the dimensions, materials, shapes, relative arrangements, etc. of the components described as embodiments or shown in the drawings are not intended to limit the scope of the present invention to this, but are merely explanatory examples. Absent.
For example, expressions that represent relative or absolute arrangements such as "in a certain direction", "along a certain direction", "parallel", "orthogonal", "center", "concentric" or "coaxial" are exact. Not only does it represent such an arrangement, but it also represents a state of relative displacement with tolerances or angles and distances to the extent that the same function can be obtained.
For example, expressions such as "same", "equal", and "homogeneous" that indicate that things are in the same state not only represent exactly the same state, but also have tolerances or differences to the extent that the same function can be obtained. It shall also represent the state of existence.
For example, an expression representing a shape such as a quadrangular shape or a cylindrical shape not only represents a shape such as a quadrangular shape or a cylindrical shape in a geometrically strict sense, but also an uneven portion or chamfering within a range in which the same effect can be obtained. The shape including the part and the like shall also be represented.
On the other hand, the expressions "equipped", "equipped", "equipped", "included", or "have" one component are not exclusive expressions that exclude the existence of other components.

FIG. 1 is a diagram schematically showing the function of the position estimation device 1 according to the embodiment of the present invention. Further, FIG. 2 is a diagram showing a state at the time of traveling of the navigation body 7 including the position estimation device 1 according to the embodiment of the present invention.

The position estimation device 1 is a device for estimating the current position Pc of the navigation body 7 capable of navigating the desired route unmanned or manned. The navigation body 7 is, for example, a submarine, an autonomous underwater vehicle (AUV: Autonomous Underwater Vehicle), a flying body flying in the air, or the like. As shown in FIG. 1, the navigation body 7 includes the above-mentioned position estimation device 1, an inertial navigation system 8 (INS: Inertial Navigation System) that calculates the position of the navigation body 7 by inertial navigation, and, for example, an altimeter or gravity. At least one type that can be used to estimate (correct) the current position Pc by map matching, such as an altimeter, an image sensor (for example, a CMOS sensor) that a camera capable of photographing the surrounding landscape, for example, altitude, gravity, and surrounding landscape. It is provided with at least one type of sensor 7s (a plurality of types in FIG. 1) for detecting the external environment information of the vehicle, and a navigation control device 9 for controlling the navigation of the navigation body 7.

As shown in FIG. 2, the navigation body 7 includes, for example, a position calculation result (hereinafter, INS position Pb) by the inertial navigation system 8 when a periodic predetermined timing arrives, and the navigation body 7. The detection results of the sensing target (altitude, weight, etc.) by at least a part (all in FIG. 1) of one or more sensors 7s are accumulated. Then, each time the predetermined timing arrives, the position estimation device 1 uses the position calculation result by the accumulated inertial navigation system 8 and the detection result by at least one sensor 7s, and the navigation body 7 at that time is used. It is configured to perform an estimation of the current position Pc, which is the position of. Further, the navigation control device 9 controls the navigation so that the navigation body 7 navigates a predetermined route (default route) or the like while checking the current position Pc obtained from the position estimation device 1. ..

In the embodiment shown in FIGS. 1 and 2, the navigating body 7 is an AUV and underwater dives unmanned. The navigation body 7 may be operated by a person. In this case, the navigation body 7 may be navigated on a default route, or may be navigated on an arbitrary route while checking the current position Pc. Further, the inertial navigation system 8 may calculate the position by using the detection result of the sensor 7s.

Hereinafter, the position estimation device 1 will be described in detail.
As shown in FIG. 1, the position estimation device 1 includes a first storage unit 2a, a second storage unit 2b, a candidate route generation unit 3, an evaluation value calculation unit 4, and a position identification unit 5. The above-mentioned functional parts included in the position estimation device 1 will be described respectively.

The position estimation device 1 may be configured by a computer, and includes a CPU (processor) (not shown), a storage device m such as a memory such as a ROM or RAM, or an external storage device. Then, the CPU operates (data calculation, etc.) according to the instructions of the program (position estimation program) loaded in the memory (main storage device) to realize each of the above-mentioned functional units included in the position estimation device 1. In other words, the above-mentioned position estimation program is software for realizing each functional unit described later in a computer, and may be stored in a storage medium that can be read by the computer.

The first storage unit 2a includes detection information (hereinafter, actual sensor information St) by the sensors 7s at each time when the specified timing arrives, which are output from one or more sensors 7s and the inertial navigation system 8 of the navigation body 7, respectively. It is a storage unit configured to store the calculation result of the position of the navigation body 7 by the inertial navigation system 8. The specified timing is, for example, a periodic timing, and as shown in FIG. 2, each time the specified timing arrives, the detection information that is the detection result (detection value, etc.) of the sensing target by the sensor 7s at the time of arrival is the actual sensor. It is stored in the first storage unit 2a as information St. For example, when the navigation body 7 includes a plurality of types of sensors 7s, the actual sensor information St for each arrival of the specified timing may include a plurality of types of detection information by all the sensors 7s included in the navigation body 7. Alternatively, it may include one or more types of detection information by at least a part of all the sensors 7s. Similarly, each time the specified timing arrives, the INS position Pb (position calculation result) by the inertial navigation system 8 at that time is stored in the first storage unit 2a.

Further, in the first storage unit 2a, the actual sensor information St and the INS position Pb at the time of arrival of each specified timing from the latest one to before the specified number of times N are stored in a state in which the time series can be known. Specifically, each actual sensor information St may store identification information such as an ordinal number for identifying the detection time or the arrival time of the specified timing together with the time series information T. Further, the above-mentioned specified number of times N is a number smaller than the number of arrivals of the specified timing that has arrived since the navigating body 7 started estimating the self-position. In the embodiment shown in FIG. 1, the first storage unit 2a is formed in a predetermined storage area of the storage device m included in the position estimation device 1, and the external information acquisition unit 6 is formed every time a predetermined timing arrives. The latest actual sensor information St and INS position Pb are acquired and stored in the first storage unit 2a together with the time series information T.

The second storage unit 2b is a storage unit configured to store the map information M associated with the position coordinates by the reference sensor information Sr for specifying the position by collating with the above-mentioned actual sensor information St. The reference sensor information Sr is detection information obtained by detecting one or more types of external environment information in advance. Further, the map information M is information in which the reference sensor information Sr is associated with each coordinate of the coordinate system (two-dimensional or three-dimensional) virtually set in a predetermined area including the route of the navigation body 7. That is, by referring to arbitrary coordinates of the map information M, external environment information such as altitude and gravity that should be obtained at those coordinates can be known. Therefore, the actual sensor information St and the reference sensor information Sr of the map information M are collated on the assumption that the reference sensor information Sr includes the same type of external environment information as the sensing target detected by the sensor 7s of the navigation body 7. By doing (map matching), it is possible to identify (estimate) the self-position. That is, there is a high possibility that the coordinates on the map information M having the reference sensor information Sr that can be said to match the actual sensor information St are self-positions. In the embodiment shown in FIG. 1, the second storage unit 2b is formed in a predetermined storage area of the storage device m included in the position estimation device 1.

The candidate route generation unit 3 indicates a time series of the estimated position Pe based on the position calculation result of the inertial navigation system 8 at each of a plurality of (two or more) predetermined timings including the time of estimating the current position Pc. It is a functional unit configured to generate a plurality of candidate routes Rs. The candidate route Rs includes the candidate position Ps of the current position Pc and the estimated position Pe of the navigation body 7 at the time of arrival of each specified timing N times before the estimated time of the current position Pc. That is, the candidate paths Rs are information that shows the time series of the estimated position Pe at each of the above-mentioned multiple times (N + 1 times) of the predetermined timings.

Since there is an integration error in the position calculated by the inertial navigation system 8 (inertial navigation), the actual position of the navigating body 7 is any coordinate within the range considering the INS position Pb and its integration error. there is a possibility. That is, based on the position calculation result by the inertial navigation system 8, in addition to the INS position Pb, inside the error range Ae surrounded by a circle with an error radius centered on the INS position Pb (broken line circle in FIG. 2). A plurality of estimated positions Pe are obtained. Therefore, for each candidate route Rs, the candidate route Rs can be generated by arranging the position coordinates selected one by one from the error range Ae at each of the arrival of the plurality of specified timings in chronological order.

Then, as will be described later, one candidate route Rs is selected as the most probable one from the plurality of candidate routes Rs, and the candidate position Ps (latest position in time series) in the selected candidate route Rs is set. Estimated as the current position Pc. A specific method for generating the plurality of candidate paths Rs will be described later. The above error radius may be determined based on the specifications (catalog specifications, etc.) of the inertial navigation system 8. Further, ti in FIG. 2 indicates the time when the i-th arrival of the specified timing (estimated time, etc.) is reached, but the above integration error increases as the number of estimations increases, and the error range Ae (area of a broken line circle) is the time. It increases with the passage of time (t ₁ <t ₂ <t ₃ ).

The evaluation value calculation unit 4 is a time series (hereinafter, reference sensor time series Cr) and an actual sensor of the reference sensor information Sr based on the above map information M at each of the plurality of estimated positions Pe possessed by the plurality of candidate paths Rs. It is a functional unit configured to calculate the evaluation value V of the certainty of each of the plurality of candidate paths Rs based on the time series of the information St (hereinafter, the actual sensor time series Ct). More specifically, in some embodiments, as shown in FIG. 1, the evaluation value calculation unit 4 includes a first acquisition unit 41 that acquires the reference sensor time series Cr of the input candidate path Rs and the like. Comparison between the second acquisition unit 42 that acquires the actual sensor time series Ct of the candidate path Rs, the reference sensor time series Cr acquired by the first acquisition unit 41 and the second acquisition unit 42, and the actual sensor time series Ct, respectively. The error sequence calculation unit 43 that calculates the error sequence Ce, which is a time series of the error E calculated based on, and the evaluation unit that calculates the evaluation value V based on the error sequence Ce calculated by the error sequence calculation unit 43. 44 and may have.

As described above, each candidate path Rs includes time-series information of the estimated position Pe at each of a plurality of predetermined timings. Therefore, by acquiring the reference sensor information Sr at each of the plurality of estimated positions Pe possessed by the arbitrary candidate path Rs from the map information M and arranging them in time series, the reference sensor time series Cr corresponding to the candidate path Rs can be obtained. Be done. Similarly, by acquiring the actual sensor information St in which the estimated times of the plurality of estimated positions Pe of the arbitrary candidate paths Rs match from the first storage unit 2a and arranging them in chronological order, the candidate paths Rs can be set. The corresponding real sensor time series Ct is obtained. Then, by comparing the reference sensor time series Cr obtained in each candidate path Rs with the actual sensor time series Ct, an error E at each estimated position Pe (each specified timing) is calculated, and the error E is converted into a time series. By arranging them, the error sequence Ce can be obtained.

The error E at each estimated position Pe is the difference between the actual sensor information St and the reference sensor information Sr at each estimated position Pe included in the candidate path Rs when the actual sensor information St includes only one type of detection information. It may be a calculated value of a difference such as a difference. On the other hand, when the actual sensor information St includes a plurality of types of detection information, the error E at each estimated position Pe is for each type of sensing target between the actual sensor information St and the reference sensor information Sr at each estimated position Pe. The difference may be a calculated value obtained by adjusting the difference with a coefficient for adjusting the difference of the type and then adding the difference. This coefficient may be the same for each type of sensing target.

Then, the evaluation value calculation unit 4 calculates the evaluation value V of each candidate route Rs based on the error sequence Ce. Specifically, this evaluation value V may be calculated by summing the error E of each estimated position Pe included in the error sequence Ce. The error of each estimated position Pe may be multiplied by a weighting coefficient and summed. In this case, the evaluation value V becomes larger as the error is larger. Therefore, the value of the evaluation value V is calculated so as to be larger as the certainty is higher, by calculating the reciprocal of the total of the errors E in the error sequence Ce. You may. On the contrary, the value of the evaluation value V may be calculated so as to become smaller as the certainty is higher.

The position specifying unit 5 is a functional unit configured to specify the current position Pc based on the evaluation value V of each of the plurality of candidate paths Rs. In the present embodiment, the candidate route Rs having a higher evaluation value V is more likely to be, so for example, the candidate route Rs having the highest evaluation value V is selected from a plurality of candidate routes Rs, and the latest estimated position of the selected candidate route Rs is selected. Pe is specified as the current position Pc.

In the embodiment shown in FIG. 1, the candidate route generation unit 3 and the evaluation value calculation unit 4 are connected so that a plurality of candidate routes Rs estimated by the candidate route generation unit 3 are input to the evaluation value calculation unit 4. It has become. In the evaluation value calculation unit 4, the first acquisition unit 41 obtains the reference sensor information Sr corresponding to each of the plurality of estimated positions Pe of each candidate route Rs input from the candidate route generation unit 3 from the second storage unit 2b. The reference sensor time series Cr is acquired by acquiring and storing the reference sensor information Sr so that the time series can be understood. Similarly, the second acquisition unit 42 acquires the actual sensor information St corresponding to each of the plurality of estimated positions Pe of each candidate route Rs input from the candidate route generation unit 3 from the first storage unit 2a. The actual sensor time series Ct is acquired by storing the actual sensor information St so that the time series can be understood. The reference sensor time series Cr and the actual sensor time series Ct for one candidate path Rs thus obtained are input to the error sequence calculation unit 43 connected to the first acquisition unit 41 and the second acquisition unit 42, and the error sequence Ce is calculated. The error sequence calculation unit 43 is connected to the evaluation unit 44, and the evaluation unit 44 calculates that the evaluation value V of the error sequence Ce input from the error sequence calculation unit 43 increases as the certainty increases. To. This makes it possible to appropriately calculate the evaluation value V of each candidate route Rs.

According to the above configuration, a plurality of candidate routes Rs that the navigating body 7 may have passed are selected from one of a plurality of candidate positions at each arrival of the specified timing obtained from the position calculation result by inertial navigation. Then, it is generated by arranging them in chronological order. By collating each candidate route Rs generated in this way with the map information, a time series of sensor information (reference sensor time series Cr) that should be obtained when the traveling body 7 passes through each candidate route Rs is obtained, and at the same time, By comparing the reference sensor time series Cr with the actual sensor time series Ct having the same specified timing such as the estimated time, the error sequence Ce is calculated, and the certainty of each candidate path Rs is evaluated. Then, the current position is specified (estimated) by setting the latest position included in the candidate path Rs having the highest evaluation value V as the current position.

In other words, the current position Pc of the navigation body 7 is set as the expected value of the actual sensor information St at the time when a plurality of specified timings actually detected by measurement or the like arrives, and the reference sensor information Sr at the time of estimating the current position Considering not only the degree of coincidence but also the degree of coincidence with the reference sensor information Sr at the arrival of the specified timing that goes back one or more times, the navigation is based on the candidate route Rs having a high degree of coincidence with the time series of the actual sensor information St. The current position Pc of the running body 7 is estimated. In this way, matching the entire candidate route Rs with the map information M is performed by comparing the actual sensor information St corresponding to each position of the plurality of candidate routes generated based on the inertial navigation with the reference sensor information Sr. Therefore, the current position Pc of the navigation body 7 can be appropriately estimated without using GPS.

Further, when the position calculation result (INS position Pb) by inertial navigation is corrected by the map matching technique, the self-position is estimated by collating the map information M (reference sensor information Sr) with the detection information by the sensor 7s. , When navigating in a region where the change of reference sensor information Sr is scarce (flat terrain, etc.), it is difficult to narrow down the position by collation, but as described above, estimation is performed through the generation of candidate routes Rs. Even if the navigation body 7 enters an area such as a flat terrain from the middle, the current position can be estimated with higher accuracy without using a high-precision measuring instrument capable of detecting slight undulations.

Next, some embodiments relating to the method of generating a plurality of candidate routes Rs by the candidate route generation unit 3 described above will be described with reference to FIGS. 3 to 5. FIG. 3 is a block diagram showing a function of the candidate route generation unit 3 according to the embodiment of the present invention, and generates a candidate route based on the candidate position Ps. FIG. 4 is a block diagram showing the function of the candidate route generation unit 3 according to the embodiment of the present invention, and generates new candidate routes Rs based on the candidate route Rs. Further, FIG. 5 is a block diagram showing a function of the candidate route generation unit 3 according to the embodiment of the present invention, in which new candidate routes Rs are generated by brute force of the estimated position Pe.

In some embodiments, as shown in FIG. 3, the candidate route generation unit 3 has a candidate position acquisition unit 31 that acquires a plurality of candidate positions Ps that are candidates for the current position Pc, and each of the plurality of candidate positions Ps. Therefore, it may have a candidate route calculation unit 32 for calculating a plurality of candidate routes Rs by calculating the estimated position Pe at each time when the specified timing arrives before the specified number of times N. As described above, from the position calculation result by the inertial navigation system 8, it can be found that the navigating body 7 may exist in the error range Ae centered on the INS position Pb. Therefore, the candidate position acquisition unit 31 sets at least a part of the position coordinates included in the error range Ae of the position calculation result of the inertial navigation system 8 at the time of estimating the current position Pc as the candidate position Ps. That is, the candidate positions Ps are all obtained by superimposing the INS position Pb output from the inertial navigation system 8, the error radius based on the error specifications stored in the storage device m, and the map information M. It is extracted from the position coordinates.

Further, the candidate route calculation unit 32 calculates one or more candidate routes Rs having each of the plurality of candidate positions Ps acquired by the candidate position acquisition unit 31 as an end point.
More specifically, in some embodiments, in generating the candidate paths Rs, a state transition matrix Mt capable of calculating the estimated position Pe at the time when the predetermined timing immediately before the arbitrary estimated position Pe arrives is used. Is also good. In this case, the candidate route is calculated by using an arbitrary candidate position Ps as the initial position and calculating the position (estimated position Pe) at the arrival of the specified timing from this initial position by the state transition matrix Mt in order in the past. Rs may be generated. This makes it possible to generate candidate paths Rs whose end point is the candidate position Ps, which is the initial position.

This state transition matrix Mt can calculate where to go next (state transition) from the position of the navigation body 7, the speed and the traveling direction at that position, the weight of the navigation body 7, and other conditions during navigation. It is a possible matrix. The state transition matrix Mt may be a Kalman filter or an MHE (Moving Horizon Estimation). For example, by performing a matrix operation (Mt ^-1 × Pe) between the inverse matrix of the state transition matrix Mt and the estimated position Pe, it is possible to obtain the position coordinates at the time when the specified timing immediately before the estimated position Pe arrives. Is.

Therefore, for each of the plurality of candidate positions Ps (initial positions), the estimated position Pe at the time when the predetermined timing immediately before the candidate position Ps arrives is obtained by using the candidate position Ps and the state transition matrix Mt, respectively. Further, using the obtained estimated position Pe and the state transition matrix Mt, the estimated position Pe at the time of the arrival of the immediately preceding specified timing is obtained. By repeating the matrix operation using the state transition matrix Mt for a specified number of times N, the estimated position Pe at each time when the specified timing before the specified number of times N arrives can be obtained at an arbitrary candidate position Ps. It becomes possible to obtain the corresponding candidate routes Rs.

In this embodiment, a plurality of candidate positions Ps may be first determined, and then candidate paths Rs may be generated using the state transition matrix Mt for each candidate position Ps. Alternatively, after determining only one candidate position Ps such as INS position Pb at the time of estimating the current position Pc by the inertial navigation system 8 and creating a candidate route Rs corresponding to this candidate position Ps, the next candidate position Ps A plurality of candidate paths Rs may be generated by repeating the process of determining the above until the end condition is satisfied. When this end condition is satisfied, for example, when the error sequence Ce of the generated candidate path Rs is calculated and the evaluation value V of the error sequence Ce falls below the threshold value, or when the candidate paths Rs of a predetermined number L are generated. It may be. Further, in determining the next candidate position Ps, a predetermined algorithm as described later may be used. Thereby, a plurality of suitable candidate routes Rs can be generated.

In some other embodiments, as shown in FIG. 4, the generation of candidate routes Rs is started from the initial route for any candidate route Rs, and the following is performed by a predetermined algorithm until the end condition as described above is satisfied. Candidate routes Rs may be created, and a plurality of candidate routes Rs may be generated by repeating this. For example, the time series of the INS position Pb at the time of estimating the current position Pc and the time series of the INS position Pb at the time when the specified timing arrives before the specified number of times N may be started as the initial route of the candidate route Rs. Further, by using an optimization algorithm such as a genetic algorithm, an interior point method, or Newton's method as the above-mentioned predetermined algorithm, it is possible to create candidate paths Rs so that the evaluation value V is maximized.

In the embodiment shown in FIG. 4, the candidate route generation unit 3 has a candidate route acquisition unit 33 that acquires an arbitrary candidate route Rs, and a new candidate route based on the candidate route Rs acquired by the candidate route acquisition unit 33. It has a generation unit 34 for calculating Rs'. That is, when one candidate route Rs is input, the candidate route generation unit 3 of the present embodiment obtains the reference sensor time series Cr and the actual sensor time series Ct corresponding to the input one candidate route Rs. The error sequence Ce is calculated, and a new candidate path Rs'is created from one input candidate path Rs using the above-mentioned predetermined algorithm so that the evaluation value V based on the obtained error sequence Ce is minimized. Generate.

Since a new candidate route Rs'can be generated by inputting a candidate route Rs as an initial value to such a candidate route generation unit 3, the generated new candidate route Rs' is sent to the candidate route generation unit 3 again. A plurality of candidate routes Rs can be obtained by repeating the input and the like until the end condition as described above is satisfied. In the embodiment shown in FIG. 4, the candidate routes Rs as the initial routes are the time series of the INS position Pb, and these initial routes are input to the candidate route generation unit 3 and the evaluation value calculation unit 4, respectively, and are obtained as a result. A new candidate route Rs'and an evaluation value V are stored. At this time, if the end condition is not satisfied, the generated new candidate route Rs'is input to the candidate route acquisition unit 33, the next new candidate route Rs is generated, and then the evaluation value calculation unit. The evaluation value V according to 4 is performed and stored. By repeating this until the end condition is satisfied, a plurality of candidate routes Rs are generated. Thereby, a plurality of suitable candidate routes Rs can be generated.

In some other embodiments, as shown in FIG. 5, the candidate route generation unit 3 is obtained based on the position calculation result of the inertial navigation system 8 at each of a plurality of predetermined timings. The weather estimation position extraction unit 35 that extracts a predetermined number of estimated position Pes from all the estimated position Pes, and the predetermined number of estimated position Pes extracted by the weather estimation position extraction unit 35 at the arrival of each predetermined timing. It may have a combination unit 36 that generates a plurality of candidate paths Rs by combining them in a round-robin manner. It should be noted that all of the estimated positions Pe possessed by the plurality of candidate routes Rs are estimated positions Pe obtained by estimation by the inertial navigation system 8, and all of the candidate routes Rs may have actually passed the navigating body 7. It is a route with.

That is, one or more predetermined number of position coordinates are extracted from the error range Ae centered on the INS position Pb at the time of estimating the current position Pc and at the arrival of each specified timing N times earlier than the current position Pc. .. When the number of estimated position Pes extracted at the arrival of the i-th (i = 1, 2, ..., N, N + 1) specified timing is represented by ni, the total number of candidate paths Rs generated is n _1. × n ₂ × ・ ・ ・ × n _{N + 1} . At this time, for example, if ni is all the estimated positions Pe obtained based on the position calculation result of the inertial navigation system 8, all the candidate routes Rs that are possible as the positions of the navigation body 7 are generated.

In the embodiment shown in FIG. 5, after the weather estimation position extraction unit 35 extracts a predetermined number of estimated position Pes from each of the arrivals of a plurality of specified timings, a group of estimated position Pes for each specified timing is formed. It is input to the combination unit 36 in a state of being associated with the time series information T so that it can be determined at which specified timing. However, the present invention is not limited to the present embodiment. In some other embodiments, for example, a candidate path Rs composed of a time series of INS position Pb at each arrival of a plurality of specified timings is set as an initial path, and an arbitrary estimated position Pe of this initial path is set as an initial path. A new candidate route Rs may be generated by arbitrarily changing the candidate route Rs so as not to match the already generated candidate route Rs as a whole within the error range of the inertial navigation system 8. A new candidate route may be generated by using a predetermined algorithm as described above. Then, the candidate paths Rs may be generated until the end condition is satisfied.

As a result, for example, all the estimated positions Pe that are possible as the positions of the navigating body 7 at each of the arrival of the plurality of specified timings are extracted, and the plurality of estimated positions Pe at each time of the arrival of each of the extracted specified timings are extracted. By generating candidate routes Rs by brute force of each other, it is possible to comprehensively generate all expected candidate routes Rs.

Hereinafter, the position estimation method corresponding to the position estimation device 1 described above will be described with reference to FIG. FIG. 6 is a diagram showing a position estimation method according to an embodiment of the present invention.

The position estimation method is a method of estimating the current position Pc of the navigation body 7. As shown in FIG. 6, the position estimation method includes a first storage step (S2), a second storage step (S1), a candidate route generation step (S4), an evaluation value calculation step (S5), and a position identification. A step (S6) is provided. The position estimation method will be described according to the flow of FIG. It should be noted that steps S2 to S6 described below are executed every time the specified timing arrives.

In step S1 of FIG. 6, the second storage step is executed. The second storage step is a step of storing the map information M described above. This step is a step of preparing the map information M so that it can be used, and stores the map information M in the first storage unit 2a in advance, such as before the start of the navigation of the navigation body 7.

In step S2 of FIG. 6, the first storage step is executed. The first storage step is a step of storing the above-mentioned actual sensor information St and the position calculation result by the inertial navigation system 8 for each arrival of the specified timing. For example, the actual sensor information St detected at the arrival of the predetermined timing that arrives periodically and the INS position Pb calculated by the inertial navigation system 8 are acquired and stored in the second storage unit 2b.

In step S4 of FIG. 6, the candidate route generation step is executed. The candidate route generation step is a step of generating a plurality of candidate routes Rs described above. Since this candidate route generation step is the same as the processing content executed by the candidate route generation unit 3 already described, the details will be omitted. In the candidate route generation step, since the specified timing arrives more than the specified number of times N, the number of actual sensor information St stored by the execution of the first storage step becomes larger than the specified number of times N (step). Yes) is executed in S3.

In step S5 of FIG. 6, the evaluation value calculation step is executed. The evaluation value calculation step is a step of generating the plurality of candidate paths Rs described above. This candidate route generation step is performed on a plurality of candidate routes based on the reference sensor time series Cr and the actual sensor time series Ct based on the map information M at each of the plurality of estimated positions Pe possessed by the plurality of candidate routes Rs. This is a step of calculating the evaluation value V of each certainty of Rs. Since the evaluation value calculation step is the same as the processing content executed by the evaluation value calculation unit 4 already described, the details will be omitted.

In step S6 of FIG. 6, the position specifying step is executed. The position specifying step is a step of specifying the current position Pc based on the evaluation value V of each of the plurality of candidate paths Rs. Since this position specifying step is the same as the processing content executed by the position specifying unit 5 already described, the details will be omitted.

In the embodiment shown in FIG. 6, the map information M is stored in the second storage unit 2b in advance in the storage device m included in the navigation body 7. After that, when the navigation of the navigation body 7 is started, in step S2, every time the specified timing arrives, the actual sensor information St and the INS position Pb at that time are stored in the first storage unit 2a. Then, in step S3, the number of actual sensor information St is confirmed, and when the number exceeds the specified number of times N, step S4 and subsequent steps are executed.

In step S4, a plurality of candidate paths Rs are generated, and in step S5, each evaluation value V of the plurality of candidate paths Rs is calculated based on the reference sensor time series Cr and the actual sensor time series Ct of each candidate path Rs. To do. Specifically, the error sequence Ce may be calculated, and the total of the errors E constituting the error sequence Ce may be calculated as the evaluation value V. Then, in step S6, the current position Pc is specified based on the evaluation value V of each of the plurality of candidate routes Rs calculated in step S5, and the position estimation result of the navigation body 7 is used. After that, the position estimation result obtained in step S6 is transmitted to the navigation control device 9.

The present invention is not limited to the above-described embodiment, and includes a modified form of the above-described embodiment and a combination of these embodiments as appropriate.

1 Position estimation device 2a 1st storage unit 2b 2nd storage unit 3 Candidate route generation unit 31 Candidate position acquisition unit 32 Candidate route calculation unit 33 Candidate route acquisition unit 34 Generation unit 35 Generation unit 35 Sensor estimation position extraction unit 36 Combination unit 4 Evaluation value calculation Part 41 1st acquisition part 42 2nd acquisition part 43 Error sequence calculation part 44 Evaluation part 5 Position identification part 6 External information acquisition part 7 Navigation unit 7s Sensor 8 Inertial navigation system 9 Navigation control device Ct Actual sensor Time series Cr Refer to Sensor time series Ce Error sequence E Error V Evaluation value M Map information Mt State transition matrix N Specified number of times Rs Candidate path Pc Current position Pe Estimated position Ps Candidate position Pb INS Position Ae Error range (INS)
S Sensor information Sr Reference sensor information St Actual sensor information T Time series information m Storage device

Claims (7)

It is a position estimation device that estimates the current position of the navigation body.
A first storage unit configured to store the actual sensor information and the position calculation result for each arrival of the specified timing output from the sensor and the inertial navigation system of the navigation body, respectively.
A second storage unit configured to store the map information associated with the position coordinates by the reference sensor information for specifying the position by collating with the actual sensor information.
A candidate route generation unit configured to generate a plurality of candidate routes indicating a time series of estimated positions based on the position calculation results at each of a plurality of times when the specified timing is reached, including the time of estimating the current position. ,
Based on the time series of the reference sensor information obtained based on the map information and the time series of the actual sensor information at each of the plurality of estimated positions each of the plurality of candidate routes, the plurality of candidate routes An evaluation value calculation unit configured to calculate the evaluation value of each certainty,
A position estimation device including a position specifying unit configured to specify the current position based on the evaluation value of each of the plurality of candidate routes. The evaluation value calculation unit
The first acquisition unit that acquires the time series of the reference sensor information of the candidate route, and
A second acquisition unit that acquires the time series of the actual sensor information of the candidate route, and
An error sequence calculation unit that calculates an error sequence that is a time series of errors calculated based on a comparison between the acquired time series of the reference sensor information and the time series of the actual sensor information.
The position estimation device according to claim 1, further comprising an evaluation unit that calculates the evaluation value based on the error sequence. The candidate route generation unit
A candidate position acquisition unit that acquires a plurality of candidate positions that are candidates for the current position,
It has a candidate route calculation unit that calculates the plurality of candidate routes by calculating the estimated positions at each of the predetermined timings up to the specified number of times before each of the plurality of candidate positions. The position estimation device according to claim 1 or 2. The candidate route generation unit
A candidate route acquisition unit for acquiring the candidate route and
The position estimation device according to claim 1 or 2, further comprising a generation unit that calculates a new candidate route based on the acquired candidate route. The candidate route generation unit
A weather estimation position extraction unit that extracts a predetermined number of the estimated positions at each of the plurality of times when the specified timing arrives,
Claim 1 or claim 1, further comprising a combination unit that generates the plurality of candidate routes by brute force combining the predetermined number of estimated positions for each of the extracted predetermined timings at the time of arrival. 2. The position estimation device according to 2. Inertial navigation system that calculates the position of the navigating body by inertial navigation,
With at least one sensor
Any one of claims 1 to 5 that estimates the current position of the traveling body based on the position calculation result by the inertial navigation device and the actual sensor information which is the detection information detected by the at least one sensor. The position estimation device described in the section and
A navigation body including a navigation control device that controls the navigation of the navigation body based on the current position estimated by the position estimation device. It is a position estimation method that estimates the current position of the navigation body.
The first storage step for storing the actual sensor information and the position calculation result for each arrival of the specified timing output from the sensor and the inertial navigation system of the navigation body, respectively.
The second storage step of storing the map information in which the reference sensor information for identifying the position by collating with the actual sensor information is associated with the position coordinates, and
A candidate route generation step for generating a plurality of candidate routes indicating a time series of estimated positions based on the position calculation result at each of a plurality of times when the specified timing arrives, including the time of estimating the current position, and
Based on the time series of the reference sensor information obtained based on the map information and the time series of the actual sensor information at each of the plurality of estimated positions each of the plurality of candidate routes, the plurality of candidate routes Evaluation value calculation step to calculate the evaluation value of each certainty, and
A position estimation method comprising: a position specifying step for specifying the current position based on the evaluation value of each of the plurality of candidate routes.

Images