JP7365958B2 - Positioning systems, methods and programs - Google Patents

Description

The present invention relates to a positioning system, method, and program for positioning an object.

Various conventional methods exist as positioning techniques, and efforts are being made to improve positioning accuracy.

In Patent Document 1 ("Wide area position estimation from SLAM map"), when locally starting SLAM (Simultaneous Localization and Mapping, simultaneous execution of self-position estimation and map creation), the initial position is estimated using global coordinates. Determine by inquiring the server to be used. Patent Document 2 ("Global coordinate acquisition device using image processing") discloses global coordinates using GPS, in order to accurately measure global coordinates even under adverse conditions where GPS (Global Positioning System) cannot be used. By calculating the conversion formula for local coordinates obtained from sensors and images, it is possible to obtain the relevant coordinates even when there are no global coordinates.

Patent Document 3 (``Moving object position estimation device and method'') proposes a system that improves the positioning accuracy of a moving object even if there is an effect of vibration during driving or an error during camera calibration. SLAM coordinates are corrected. Patent Document 4 ("Position estimation device/method") discloses a method for suppressing a decrease in position estimation accuracy due to changes in surrounding environmental conditions when estimating the position of a moving object from an image captured by a camera mounted on the moving object. In order to improve accuracy, positions calculated from multiple maps corresponding to different environmental conditions are used in correction calculations.

In Non-Patent Document 1, a formula that best converts a pair of points containing noise is derived, and movement is accurately estimated as an overall average. Non-Patent Document 2 corrects the error between the global coordinates of GPS and the local coordinates determined by the sensor.

Special Publication No. 2016-528476 Japanese Patent Application Publication No. 2005-091298 Japanese Patent Application Publication No. 2019-070983 Japanese Patent Application Publication No. 2018-028489

Sorkine-Hornung, Olga, and Michael Rabinovich. "Least-squares rigid motion using svd." Computing 1.1 (2017). Accurate Global Localization Using Visual Odometry and Digital Maps on Urban Environments. : IEEE Transactions on Intelligent Transportation Systems (Volume: 13, Issue: 4, Dec. 2012)

However, in the conventional methods described above, a method is adopted to improve accuracy by mutually correcting the results of two or more positioning methods, but there are some restrictions and it is not always easy to improve accuracy. could not be improved.

According to Patent Document 1, when starting SLAM on the terminal side, the correspondence can be obtained by referring to the global coordinates. However, although it is assumed that the global coordinate inference system is correct, in reality there is a lot of noise, and once a deviation occurs, the deviation remains. Patent Document 2 uses a conversion formula between global coordinates and local coordinates so that even when global coordinates cannot be obtained, they can be obtained by converting from local coordinates, but only one pair is used, and this Calculation results are unstable because errors are not taken into account.

Patent Document 3 is a technique that improves accuracy by considering noise within local coordinates, and global coordinates are not taken into consideration. In Patent Document 4, complementary calculations are performed from other pairs when the accuracy of global position estimation is low, but there is no method for determining errors in SLAM or misidentification of global position estimation. In addition, in order to use multiple maps and use the one with the highest accuracy, it is time consuming to prepare multiple maps depending on the environmental conditions.

In Non-Patent Document 1, etc., formulas are derived to best convert pairs of noisy point clouds, but they do not take into account the errors inherent in positioning methods such as SLAM, and the result is simply the overall average. seek. However, correct results cannot be obtained in cases where errors occur suddenly in clusters, such as the behavior of errors in SLAM. Non-Patent Document 2 corrects the error between the global coordinates of the GPS and the local coordinates of the sensor, but takes a method of correcting the error so that the vehicle does not deviate from the road it is traveling on. However, for moving objects other than cars, it is difficult to define and set roads that correspond to traversable roads in the case of a car, so this method is not suitable for positioning of moving objects other than cars.

In view of the problems of the prior art described above, the present invention provides a positioning system and method that can correctly obtain the relationship between the positioning coordinates of each other and simply improve positioning accuracy when two positioning methods are used complementarily. and programs.

In order to achieve the above object, the present invention is a positioning system, which includes a first positioning unit that acquires first positioning information as time-series data of the position and orientation by locally performing positioning of the target; a second positioning unit that acquires second positioning information as time-series data of position and orientation by performing positioning globally; and the first positioning information has continuous cumulative errors and sudden fluctuations as errors. Based on the fact that the second positioning information is accompanied by discontinuous fluctuations and sudden outliers as errors, the influence of errors in the first positioning information and the second positioning information is excluded. and a complementing unit that estimates a correspondence between the first positioning information and the second positioning information as the same positioning result and obtains the positioning information of the target based on the correspondence. . Further, the present invention is characterized in that it is a method and a program compatible with the positioning system.

According to the present invention, the influence of errors in the first positioning information and the second positioning information is excluded by taking into account the mutually different error behavior of the first positioning information and the second positioning information that are used to position the same target. , it is possible to simply obtain the correspondence between the first positioning information and the second positioning information of the target. Using this correspondence relationship, it is possible to correctly complement one positioning information with the other positioning information.

FIG. 1 is a configuration diagram of a positioning system according to an embodiment. FIG. 1 is a functional block diagram of a positioning system according to an embodiment. 3 is a flowchart of positioning operations performed by a positioning system according to an embodiment. It is a figure which shows typically the behavior of 1st positioning information and 2nd positioning information. 7 is a diagram illustrating a schematic example of rotational component time series data of the difference in step S3 as a schematic example for explaining the error reduction effect according to the present embodiment. FIG. 1 is a diagram showing an example of a hardware configuration in a general computer device.

FIG. 1 is a configuration diagram of a positioning system according to an embodiment, and the positioning system 10 includes a terminal 1 and a server 2 that can communicate with each other via a network NW such as the Internet. The terminal 1 is a mobile object that can be configured as a mobile device such as a smartphone or a vehicle-mounted device, and is a target for positioning in the positioning system 10. In one embodiment, the positioning system 10 may have a configuration in which only the terminal 1 is included and the server 2 is not included, so that the terminal 1 independently performs its own positioning. In one embodiment, the positioning system 10 includes a terminal 1 and a server 2, and the server 2 may be responsible for part of the processing required when positioning the terminal 1.

FIG. 2 is a functional block diagram of the positioning system 10 according to an embodiment, and the positioning system 10 includes a measuring section 11, a first positioning section 12, a first DB (database) 13, an imaging section 21, a second positioning section 22 , a second DB 23 and a complementing section 30. The complementation section 30 further includes a difference calculation section 31, a fitting section 32, a rotational component calculation section 33, a scale calculation section 34, a translational component calculation section 35, and a conversion section 36.

Each functional unit shown in FIG. 2 may be provided in either the terminal 1 or the server 2 of the positioning system 10, but in order to enable the positioning of the terminal 1 that is the target of positioning by the positioning system 10, At least the measuring section 11 and the photographing section 21 are installed in the terminal 1, and each measurement and photographing may be performed in the environment where the terminal 1 exists. Among the functional units shown in FIG. 2, those other than the measurement unit 11 and the imaging unit 21 may be provided in the server 2 instead of the terminal 1.

FIG. 3 is a flowchart of positioning operations by the positioning system 10 according to one embodiment. Each step in FIG. 3 will be explained below.

In step S1, the first positioning unit 12 obtains first positioning information P1(t) by positioning the terminal 1 using the first positioning method at the current time t (t=1,2,3...) in real time. Then, at the same time t, the second positioning unit 22 positions the terminal 1 using the second positioning method to obtain the second positioning information P2(t), and uses these positioning information P1(t), P2(t ) is output to the complementing unit 30 and recorded therein, and then the process proceeds to step S2.

In step S1, the first positioning unit 12 uses, for example, SLAM, an odometer, an acceleration/gyro sensor, etc. as a first positioning method to acquire the position and orientation of the terminal 1 as local information, and obtains the first positioning information. We can obtain P1(t). In the case of SLAM, the measurement unit 11 is configured, for example, as a LiDAR (laser image detection and ranging) sensor, and the first positioning unit 12 refers to the measurement result of this LiDAR sensor at the current time t to obtain the first positioning information P1( t). In the case of SLAM using images, the measurement unit 11 is configured as a stereo camera, for example, and the first positioning unit 12 refers to the stereo image of the stereo camera at the current time t to obtain point correspondence and perform triangulation, etc. The first positioning information P1(t) may be obtained by In the case of an odometer or an acceleration/gyro sensor, these may be configured as a sensor, and the first positioning information P1(t) may be obtained by cumulatively calculating the increase/decrease components of the position and angle obtained from this sensor. The information required to obtain the first positioning information P1(t) in the first positioning unit 12 (information such as camera parameters and feature points serving as a reference) is stored in the first DB 13, and this information can be referred to. Then, the first positioning unit 12 may calculate the first positioning information P1(t).

In step S1, the second positioning unit 22 uses, for example, VPS (Visual Positioning System) or marker detection as a second positioning method to acquire the position and orientation of the terminal 1 as global ones, and provides second positioning information. We can obtain P2(t). In the case of VPS, the photographing unit 21 is configured as a normal monocular camera, and the second positioning unit 22 refers to the image photographed by the camera at the current time t. and feature quantities), and match them with the reference feature points and feature quantities stored in the second DB 23 with global coordinates (three-dimensional world coordinates) linked in advance, and as a result of the matching, the second Positioning information P2(t) can be output. In the case of marker detection, the photographing section 21 is configured as a normal monocular camera, and the second positioning section 22 refers to the image photographed by the camera at the current time t, and the marker photographed in the image (augmented reality By detecting the square marker to be used (such as a square marker to be used), the global coordinates of the marker (three-dimensional world coordinates) are compared with reference information stored in advance in the second DB 23, and second positioning information P2(t) is obtained. Just do it like this.

In step S2, the positioning information {P1(k),P2(k)}(k=t,t-1,t-2,...) recorded and accumulated in step S1 up to the current time t is It is determined whether the records exist for a certain period of time, and if the records for a certain period of time exist, the process proceeds to step S3; if the records do not exist, the process returns to step S1, and the current time t+ of the next timing is determined. Continue positioning for 1 in the same way.

In step S2, if the records up to the current time t have been accumulated for the threshold N times (for example, N=8), (that is, N recorded positioning information {P1(k),P2 If (k)} (k=t,t-1,t-2,...,t-N+1) exists, an affirmative determination is obtained and the process proceeds to step S3; otherwise, that is, If positioning information {P1(k),P2(k)} is obtained only for N-1 or less times including the current time t, a negative determination is obtained and the process returns to step S1, where N accumulated information is obtained. Just keep recording until you get it.

The processes from steps S3 to S10 that follow step S2 are processes performed by the complementation unit 30, and these processes calculate the N recorded first and second positioning information {P1(k),P2(k )}(k=t,t-1,t-2,…,t-N+1) are integrated so that the error is reduced based on the mutual error behavior, and finally in step S10 , the position and orientation as the positioning results of the terminal 1 at the current time t will be output.

Here, before explaining the details of the processing by the complementing unit 30 after step S3, let us explain the principle of integrating the first and second positioning information to obtain the positioning result (position and orientation) of the terminal 1 in this embodiment. Briefly explain. FIG. 4 is a diagram schematically showing the behavior of the first positioning information and the second positioning information, which is a premise for explaining the principle.

In Figure 4, graph G1 shows data d1, d2, d3, d4 as a result of acquiring the first positioning information at four consecutive times, and the position is expressed as a white circle (○) in the three-dimensional xyz coordinate space. By expressing the direction with a vector arrow starting from the white circle, the time-varying behavior of the first positioning information composed of the position and direction is schematically shown. As already explained, the first positioning information is acquired as local information, does not have global coordinate information like the second positioning information, and is based on the position and orientation at the time when acquiring the first positioning information is started. This is relative positioning information. As schematically shown in the data d1, d2, d3, and d4 of graph G1, the first positioning information fluctuates less discontinuously due to the influence of noise etc. than the second positioning information, but the error is continuous. In addition, due to loss of SLAM, etc., the coordinate system suddenly shifts, although in rare cases. (This sudden deviation behavior is not shown in graph G1 of Figure 4.)

In Fig. 4, graph G2 shows data d11, d12, and d13 obtained from the second positioning information at three consecutive times.Similar to graph G1, the position is represented by a white circle and the direction is represented by a vector arrow. , schematically shows the temporal change behavior of the second positioning information composed of position and orientation. As already explained, the second positioning information is acquired as global information expressed in global coordinates. As schematically shown in data d11, d12, and d13 of graph G2, the second positioning information expressed in global coordinates is prone to discontinuous fluctuations due to the influence of noise, etc., and in rare cases This shows the behavior of mixing values.

As described above, in an ideal situation where there is no error or the like, the first positioning information and the second positioning information should represent the same positioning result for the terminal 1 at the same time. The behavior of the time change that is actually obtained has the following characteristic C1.
<Feature C1>
First positioning information: Defined by local coordinates, errors continuously accumulate, and in rare cases, sudden and large deviations occur (sudden fluctuations occur).
Second positioning information: Defined in global coordinates, has discontinuous fluctuations, and rarely has outliers (has sudden outliers).

The above feature C1 is due to the difference between the first positioning method and the second positioning method. That is, the fundamental difference is that the first positioning method is a method of accumulating movement differences, for example, integrating acceleration components and relative positions between successive images to estimate the current position. Therefore, if a sudden movement or blurring of the image occurs and it becomes impossible to compare with the previous information, there will be a discontinuous large deviation, and from there the cumulative calculation will continue. This causes accumulation of errors and sudden fluctuations.

On the other hand, in the second positioning method, there is reference information, and the position relative to the reference is estimated. Therefore, errors do not accumulate, but by performing calculations that exclude random noise components caused by RANSAC (Random Sample Consensus), even if the exact same information is given, the calculation results may be slightly different each time. It has the characteristic of producing completely different values in some cases. This is the cause of discontinuous fluctuations and sudden outliers.

In steps S3 to S10, the complementation unit 30 converts the first positioning information and the second positioning information, which should normally have the same content, by considering the difference in behavior (statistical difference) in the actual values as described above. By integrating these two positions, it is possible to obtain stable positioning results with reduced effects of errors and the like.

Hereinafter, steps S3 to S10 will be explained in detail. As a premise of the explanation, the position and orientation in the first and second positioning information {P1(k),P2(k)} (k=t,t-1,t-2,...,t-N+1) are It shall be written as follows. That is, let the position (local coordinates) in the first positioning information P1(k) at time k be LP _k , and the direction be LR _k . Further, the position (global coordinates) in the second positioning information P2(k) at time k is set as GP _k , and the direction is set as GR _k . These positions LP _k and GP _k can be given as xyz coordinate values as vectors with 3 elements, and the directions LR _k and GR _k can be given as rotation matrices of size 3 × 3, but each calculation described below In some cases, these coordinates (vectors) LP _k , GP _k and rotation matrices LR _k , GR _k may be handled using homogeneous coordinate representation, as is well known in the field of computer graphics.

In step S3, the difference calculation unit 31 calculates the first and second positioning information {P1(k),P2(k)}(k=t,t-1,t-2,...,t-N+1) _Regarding the N simultaneous orientations at After the calculation, proceed to step S4. In Equation (1), inv() represents an inverse matrix (inv(LR _k )=LR _k ⁻¹ ).
RDiff _k =GR _k・inv(LR _k ) …(1)

As already explained regarding the principle, the N rotational components RDiff _k (k=t,t-1,t-2,...,t-N+1) as the difference according to equation (1) have no errors. In an ideal situation where it is assumed that the N values are not equal, all N values should be equal, but in reality, variations occur due to errors reflecting the above-mentioned characteristic C1.

In step S4, the fitting unit applies rotational components {RDiff _k } (k=t,t-1,t-2,...,t-N+1) as the N time series data obtained in step S3. 32 calculates a regression line that allows outliers by robust estimation such as RANSAC (Random Sample Consensus), and then proceeds to step S5 after obtaining a fitting result.

In step S5, the fitting unit 32 further combines the regression line obtained in step S4 with rotational components {RDiff _k } (k=t, t-1, t-2, ..., t-N+1), and the element for which the value of the rotational component {RDiff _k } is determined to be an outlier with respect to the regression line is determined by the threshold value, It is determined whether or not the outliers are consecutive for a specified number of times TH or more (on the side closer to the current time t), and if negative (when the outliers are not consecutive for the specified number of times or more on the rear side), the process proceeds to step S51. If affirmative (if there are consecutive outliers for the specified number TH or more on the back side), the process advances to step S52.

Note that the designated number TH may be set as a predetermined threshold smaller than the number N of rotational components, and for example, when N=8, TH may be set as 2, etc. Further, in step S5, the determination may be made according to the following procedure.
(Step 1) For each of the N rotational components {RDiff _k } (k=t,t-1,t-2,...,t-N+1), compare it with the value of the regression line at the relevant time k, and calculate the difference. If (absolute value) is greater than or equal to the threshold value, it is determined that it is an outlier, and if it is not greater than the threshold value, it is determined that it is not an outlier.
(Step 2) If there is one or more outliers that were determined to be outliers in step 1 above, the outlier is within a predetermined range at the back of the N time series data (current time which is the backmost). (a predetermined range including t), and if there are a specified number TH or more of the items that belong consecutively, an affirmative determination is obtained. In other cases, a negative determination is obtained.

In step S51, the fitting unit 32 further determines outliers among the N rotational components {RDiff _k } (k=t, t-1, t-2,..., t-N+1) in step S5. Those that are considered to be outliers (outliers in step 1) are estimated to be caused by discontinuous fluctuations due to the second positioning information, and are excluded from the target set W for the subsequent calculation process. After setting as , the process proceeds to step S6. Here, RANSAC may be repeated two or more times to make the results more stable. In other words, the RANSAC regression line is calculated again using only the elements excluding outliers, and the line when the number of excluded outliers is less than the threshold is set as the target set W for calculation processing. After that, the process may proceed to step S6.

In step S52 _, the fitting unit 32 further calculates the rear side (current time t It is estimated that the elements on the side closer to the first positioning information are in the state after a large deviation has occurred in the first positioning information, and regression is performed again using only the elements on the rear side among all N pieces of data. After calculating the straight line and setting the rear element as the target set W for subsequent calculation processing, the process proceeds to step S6.

When calculating the regression line for the rear elements in step S52, RANSAC may or may not be used. If used, similar to step S51, it may be repeated two or more times to make the RANSAC results more stable. Regarding the number of elements on the rear side, a regression line may be calculated for TH samples including the current time t or a predetermined number of samples greater than or equal to TH (less than N).

In step S6, the rotational component calculation unit 33 calculates the rotational component RDiff(t) at the current time t as a value on the regression line, and then proceeds to step S7.

Note that the regression line used for the calculation in step S6 is the regression line calculated in step S4 when going from step S51 to step S6 (or the regression line calculated in step S4 when RANSAC is repeated two or more times while excluding outliers). If step S52 leads to step S6, the regression line calculated again for only the rear elements in step S52 (RANSAC is repeated two or more times while excluding outliers). the last regression line).

In step S7 , the scale calculation unit 34 calculates the local coordinates LP _k of the first positioning information and the global Using the coordinate GP _k , calculate the average value LP _ave of the local coordinate LP _k within the set W and the average value GP _ave of the global coordinate GP _k within the set W, and perform singular value decomposition of the following equation (2). I do.
U・Σ・V ^T =SVD((LP _k -LP _ave ) ^T・(GP _k -GP _ave )) …(2)

In equation (2), SVD() on the right side indicates that singular value decomposition is performed on the arguments in parentheses, and the result of the singular value decomposition becomes the left side, and the diagonal components of the matrix Σ are set W The scale calculation results for each element within will appear. For the sake of explanation, the diagonal elements of this matrix Σ are assumed to be array S. Also, the superscript T is a transposition operation, and the same applies hereafter.

In step S7, the scale calculation unit 34 further uses the above array S to calculate the scale C of the global coordinates (second positioning information) relative to the local coordinates (first positioning information) using the following equation (3). , proceed to step S8. In equation (3), sum is the sum of each element in array S, count(W) is the number of elements in set W, and average() is an average value calculation in set W.

In step S8, the translational component calculation unit 35 uses the rotational component RDiff(t) obtained in step S6 and the scale C obtained in step S7 to further calculate the difference between the local coordinates and global coordinates. A translation vector T (different from the superscript T representing a transposition operation) as a translation component of is obtained using the following equation (4), and then the process proceeds to step S9.
T=-C・RDiff(t)・(LP _ave ) ^T +(GP _ave ) ^T …(4)

In step S9, the conversion unit 36 uses the rotational component RDiff(t) obtained in step S6, the scale C obtained in step S7, and the translation vector T obtained in step S8. The projection matrix M(t) between local coordinates and global coordinates is determined by the combination process of equation (5) below. Since the rotation component RDiff(t) is a matrix of size 3 × 3 and the translation vector T is a matrix (vector) of size 1 × 3, the projection matrix M of size 3 × 4 is obtained by combining the following equation (5). (t) will be obtained.
M(t)=( C・RDiff(t), T) …(5)

In step S9, the conversion unit 36 uses the projection matrix M obtained above to convert the local coordinates (homogeneous coordinate representation of the right side) of the current time t to the global coordinates (homogeneous coordinate representation of the left side) according to the following equation (6). ), and after obtaining the positioning result regarding the position, and by converting the orientation LR in the local coordinates at the current time t to the orientation GR in the global coordinates using the following equation (7), After obtaining the positioning result, the process proceeds to step S10. The local coordinates (LP _x , LP _y , LP _z ) and the direction LR in the local coordinates in equations (6) and (7) are data acquired by the first positioning unit 12 at the current time t.
(GP _x ,GP _y ,GP _z ,1) ^T =M(t)・(LP _x ,LP _y ,LP _z ,1) ^T …(6)
GR = RDiff(t)・LR …(7)

In step S10, the interpolation unit 30 outputs the values of the global coordinates (position and orientation in equations (6) and (7)) obtained in step S9 as the interpolated positioning result in the global coordinates for the current time t. Then, return to step S1. In the returned step S1, the first and second positioning information is acquired by the first positioning unit 12 and the second positioning unit 22 for the next current time t+1, and in the same way, when step S10 is reached, Then, the positioning result for the next current time t+1 is output. These corrected global coordinates are obtained by correcting discontinuous fluctuations and outliers in the existing method with respect to positioning information obtained using the global coordinates using the existing method. Further, since the correspondence relationship between the accumulated errors occurring in the local coordinates of the correction source is recalculated at any time based on the global coordinates, the influence of the accumulated errors is suppressed to a small level.

As described above, the flow in FIG. 3 can be executed in real time, and at each time t, based on the positioning result in step S1, supplemented positioning information at that time t is output in step S10. It happens. (Note that the positioning information may be acquired in advance as time-series data and the same processing may be performed offline.) In the above, the positioning results of the first positioning unit 12 and the second positioning unit 22 are Although the description has been made on the assumption that the positions are acquired at the same time, the positioning timing (positioning frequency) of the first positioning section 12 and the positioning timing (positioning frequency) of the second positioning section 22 may be made different. For example, if the positioning of the second positioning section 22 has a higher load on the system as a whole, by setting the positioning frequency of the second positioning section 22 lower than the positioning frequency of the first positioning section 12, the system load can be suppressed. In this case, M(t) and RDiff(t) obtained at time t may be used for complementary calculations after t. For example, when the positioning process of the first positioning unit 12 is operated at 30 fps (frames per second) and the positioning process of the second positioning unit 22 is operated at 1 fps, the calculation of M(t) and RDiff(t) is performed at 1 fps. Otherwise, you can reuse the previous M or RDiff.

FIG. 5 shows rotational component time series data {RDiff _k } (k=t, t-1, t-2,... , t-N+1). As already explained, this time-series data should have a constant value in an ideal situation without errors, but in reality, as shown in the example of FIG. 5, it exhibits a behavior that varies due to the influence of errors.

The window W in FIG. 5 is at the current time t=8, and the fitting results in step S4 are targeted within the window W (k=1,2,3,...,8) where the number of samples is N=8. will be obtained. The data group DA at times t=1 to 4 and the data group DC at times t=5, 7, 8 are characterized by the fluctuation error in the second positioning information and the time progression in the first positioning information among the characteristics C. It corresponds to the cumulative error (represented by the fitting straight line LC) and the effect of . The data DB at time t=6 corresponds to the data affected by outliers in the second positioning information, but according to this embodiment, such data is excluded by robust estimation such as RANSAC, and the error is The impact can be reduced. In the example of FIG. 5, the influence of various errors can be reduced by using the value of the current time t=8 on the fitting straight line LC as the value of RDiff(8).

Figure 5 also shows the effect of a large shift in the first positioning information (sudden fluctuation due to lost SLAM, etc.) in feature C when moving from time t=8 to t=9. is appearing. In this way, after the coordinate values in the first positioning information are lost, it is necessary to discard the values before the loss and use only the time series after the loss. In this way, it is possible to use only the lost time series, and errors can be reduced. In addition, in Fig. 5, the data group DD from time t=9 to 16 after the loss is similar to the data groups DA and DB, and the cumulative error (fitting (represented by a straight line LD) and the fluctuation error in the second positioning information.

FIG. 6 is a diagram showing an example of the hardware configuration of a general computer device 70. The terminal 1 and the server 2 in the positioning system 10 can each be realized as one or more computer devices 70 having such a configuration. Note that when the positioning system 10 is implemented using two or more computer devices 70, information necessary for processing may be sent and received via a network. The computer device 70 includes a CPU (central processing unit) 71 that executes predetermined instructions, a GPU (graphics processing unit) as a dedicated processor that executes some or all of the instructions executed by the CPU 71 instead of the CPU 71 or in cooperation with the CPU 71. ) 72, RAM 73 as a main storage device that provides a work area for the CPU 71 (and GPU 72), ROM 74 as an auxiliary storage device, communication interface 75, display 76, input interface 77 that accepts user input from a mouse, keyboard, touch panel, etc. It includes a measuring section 11 and a photographing section 21 as sensing hardware for positioning, and a bus BS for exchanging data between them.

Each functional unit of the terminal 1 and the server 2 in the positioning system 10 can be realized by the CPU 71 and/or the GPU 72 that read a predetermined program corresponding to the function of each unit from the ROM 74 and execute it. Note that both the CPU 71 and the GPU 72 are a type of arithmetic unit (processor). Here, when display-related processing is performed, the display 76 also operates in conjunction, and when communication-related processing regarding data transmission and reception is performed, the communication interface 75 further operates in conjunction. The positioning results etc. obtained by the positioning system 10 may be displayed on the display 76 and output.

10...positioning system, 1...terminal, 2...server
11...Measuring section, 12...First positioning section, 13...First DB, 21...Photographing section, 22...Second positioning section, 23...Second DB, 30...Completion section, 31...Difference calculation section, 32...Fitting section, 33...Rotational component calculation section, 34...Scale calculation section, 35...Translational component calculation section, 36...Conversion section

Claims (10)

a first positioning unit that acquires first positioning information as time-series data of position and orientation by positioning a target using a first positioning method in which positioning errors accumulate in a local coordinate system ;
a second positioning unit that acquires second positioning information as time-series data of position and orientation by positioning the target using a second positioning method in which positioning errors do not accumulate in the global coordinate system ;
Comprising a complementary part,
The complementing section is
Calculating a first regression line that allows outliers for differential rotation component time series data that is the difference between the orientation time series data in the first positioning information and the orientation time series data in the second positioning information,
Determining whether the number of data corresponding to outliers from the first regression line continuously exists in data closer to the current time among the differential rotation component time series data than a threshold value;
If it is determined that the data exists, a second regression line is calculated for the data that continuously exists at or above the threshold value, and the current time data on the second regression line is determined as the current time difference rotation;
If it is determined that the above does not exist, the current time data on the first regression line is determined as the current time difference rotation,
The result of applying the current time difference rotation to the orientation of the current time acquired by the first positioning unit and complementing the orientation of the current time in the global coordinate system acquired by the second positioning unit A positioning system characterized by obtaining as follows . The first error as time series data in the first positioning information indicates the first behavior, and the first behavior includes an eleventh behavior in which the error value continues to accumulate from zero, and a sudden error in the middle of the eleventh behavior. A twelfth behavior in which the value of the error that continues to accumulate rapidly changes, which occurs in
The second error as time series data in the second positioning information indicates a second behavior, and the second behavior includes a 21st behavior in which the error value continues to fluctuate around zero, and a sudden occurrence in the middle of the 21st behavior. 2. The positioning system according to claim 1, wherein the positioning system includes a twenty-second behavior in which the error value is an outlier that deviates from the fluctuation range. 3. The positioning system according to claim 1, wherein the local coordinate system is a coordinate system that does not have global coordinate information, and the global coordinate system is a coordinate system that has global coordinate information. 4. The positioning system according to claim 1, wherein the interpolation unit uses RANSAC (Random Sample Consensus) to calculate a first regression line that allows for the outliers. In the case where the complementation unit determines that the number of data corresponding to outliers from the first regression line continuously exists in data closer to the current time among the differential rotation component time series data than a threshold value; Then, set the data closer to the current time that corresponds to the outlier as the calculation target set,
The interpolation unit determines that the number of data corresponding to outliers from the first regression line does not continuously exist in the data closer to the current time of the differential rotation component time series data than a threshold value. In this case, data obtained by excluding data corresponding to outliers from the first regression line from the differential rotation component time series data is set as a calculation target set,
The position (LP) in the local coordinate system at each time of the first positioning information at the time corresponding to the element belonging to the calculation target set _k ), and the position in the global coordinate system at each time of the second positioning information at the time corresponding to the element belonging to the calculation target set (GP _k ) and a scale representing the ratio of the length scale of the position coordinate of the second positioning information in the global coordinate system to the length scale of the position coordinate of the first positioning information in the local coordinate system. 5. The positioning system according to claim 1, wherein the positioning system estimates (C). A matrix LP _k in which time-series data of positions in the first positioning information, which is composed of time data corresponding to elements belonging to the calculation target set , are listed at each time, a matrix LP _ave of the average value of this matrix, and the above-mentioned This is obtained as a result of applying singular value decomposition SVD according to the following equation (2) using a matrix GP _k that lists the time series data of the position in the second positioning information at each time and a matrix GP _ave of the average value of this matrix. 6. The positioning system according to claim 5 , wherein the scale (C) is estimated from the diagonal components of the matrix Σ .

7. The scale (C) is estimated by the following equation (3) using a set W as the calculation target set and an array S composed of diagonal components of the matrix Σ. positioning system.

The complementation unit uses the current time difference rotation (RDiff(t)), the scale (C), the average value matrix LP _ave , and the average value matrix GP _ave to calculate the local coordinate system (LPk). Calculate the translational component (T) between and the global coordinate system (GPk),
A projection matrix ( M(t)),
The projection matrix (M(t)) is uniformly calculated using the following equation (6) for the position (LPx, LPy, LPz) in the first positioning information at the current time acquired by the first positioning unit. By applying the multiplication in the following coordinate representation , obtain (GPx, GPy, GPz) which is a complement of the position in the second positioning information at the current time acquired by the second positioning unit. The positioning system according to claim 6 or 7 , characterized by:

a first positioning step of acquiring first positioning information as time-series data of position and orientation by positioning the target using a first positioning method in which positioning errors accumulate in the local coordinate system ;
a second positioning step of acquiring second positioning information as time-series data of position and orientation by positioning the target using a second positioning method in which positioning errors do not accumulate in the global coordinate system ;
a complementation stage;
The complementation step includes:
Calculating a first regression line that allows outliers for differential rotation component time series data that is the difference between the orientation time series data in the first positioning information and the orientation time series data in the second positioning information,
Determining whether the number of data corresponding to outliers from the first regression line continuously exists in data closer to the current time among the differential rotation component time series data than a threshold value;
If it is determined that the data exists, a second regression line is calculated for the data that continuously exists at or above the threshold value, and the current time data on the second regression line is determined as the current time difference rotation;
If it is determined that the above does not exist, the current time data on the first regression line is determined as the current time difference rotation,
The result of applying the current time difference rotation to the orientation of the current time obtained in the first positioning step and complementing the orientation in the global coordinate system of the current time obtained in the second positioning step. A positioning method characterized by obtaining as follows. A program that causes a computer to function as the positioning system according to any one of claims 1 to 8 .

Images