Coordinate conversion system, coordinate conversion method and program
The coordinate conversion system addresses the inaccuracies in GPS and SLAM trajectories by dividing and correcting the SLAM data using Gaussian processes or Kalman filters, resulting in a precise estimated movement trajectory that reflects both absolute and relative changes.
Patent Information
- Application Number
- JP2020192745
- Authority / Receiving Office
- JP · JP
- Patent Type
- Patents
- Current Assignee / Owner
- Filing Date
- 2020-11-19
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2040-11-19
AI Technical Summary
Conventional methods for estimating the position of a moving body using GPS and SLAM (Simultaneous Localization and Mapping) suffer from errors and scale drift, leading to inaccurate movement trajectories due to noise and relative position changes, making it difficult to obtain a precise movement trajectory.
A coordinate conversion system that utilizes GPS to generate a first movement trajectory in an absolute coordinate system, divides the SLAM-generated second movement trajectory into segments, and applies a transformation matrix corrected using Gaussian processes or Kalman filters to align and smooth the trajectories, reducing the influence of scale drift and noise.
The system generates an estimated movement trajectory that closely resembles the actual trajectory, providing a more accurate representation by aligning and smoothing the GPS and SLAM data, incorporating both absolute position and relative changes.
Smart Images

Figure 0007591908000001 
Figure 0007591908000002 
Figure 0007591908000003
Abstract
Description
[Technical field]
[0001] The present invention relates to a coordinate transformation system, a coordinate transformation method, and a program. [Background technology]
[0002] Methods for estimating the position of a moving object include a method using a Global Positioning System (GPS) and a method using a moving image captured by an imaging device. A method using GPS directly detects a position at a specified period, and generates a time-series movement trajectory obtained as a result of the detection as a first coordinate system indicating absolute coordinates (e.g., latitude and longitude), a first movement trajectory in the first coordinate system which is an absolute coordinate system (e.g., Patent Document 1). In addition, a method using moving images, such as SLAM (Simultaneous Localization and Mapping), indicates the relative movement position at a predetermined period based on the change in size and position of the subject between images in the moving image, and generates a first movement trajectory (a movement trajectory corresponding to the relative change position) in a second coordinate system, which is a coordinate system that indicates the relative position change, from the movement trajectory obtained by linking these movement positions (for example, Patent Document 2).
[0003] However, since the position coordinates obtained by GPS have an error of several meters with respect to the actual position of the moving body, as shown in Figure 10, the error is superimposed as noise on the position coordinates in the movement trajectory 300T (hereinafter, the first movement trajectory), resulting in a problem that an accurate movement trajectory cannot be obtained. In addition, the movement trajectory 400T obtained by SLAM (hereinafter, the second movement trajectory) is obtained as a shape with less noise, as shown in Figure 11, unlike the movement trajectory 300T obtained by GPS, but since it is shown as information indicating relative position changes in a unique coordinate system (hereinafter, the local coordinate system), it is not possible to obtain a movement trajectory corresponding to the actual distance.
[0004] Therefore, conventionally, the second movement trajectory in the second coordinate system is matched to the first movement trajectory in the first coordinate system (world coordinate system), and a linear coordinate transformation (linear coordinate transformation using a linear transformation matrix, hereinafter sometimes simply referred to as coordinate transformation) is performed on the first coordinate system (world coordinate system) to ultimately determine the movement trajectory of the moving body. Here, for example, the Umeyama method (see, for example, Non-Patent Document 1) is used as the linear transformation matrix for performing the coordinate transformation. As a result, as shown in FIG. 12, a movement trajectory 400T' having a smoother shape corresponding to the actual movement trajectory can be obtained, as compared with the movement trajectory obtained by GPS in FIG. [Prior art documents] [Patent documents]
[0005] [Patent Document 1] JP 2017-156154 A [Patent Document 2] JP 2020-67439 A [Non-patent literature]
[0006] [Non-Patent Document 1] 'A Purely Algebraic Justification of the Kabsch-Umeyama Algorithm', Volume 124, Article No. 124028 (2019) https: / / doi.org / 10.6028 / jres.124.028 Journal of Research of the National Institute of Standards and Technology Summary of the Invention [Problem to be solved by the invention]
[0007] As described above, in the conventional method, when a linear transformation (for example, the above-mentioned Umeyama method) is performed on the second movement trajectory (400T) in Figure 11 used in SLAM to match the first movement trajectory (300T) obtained by GPS in Figure 10, the influence of noise is eliminated as in the case of GPS. However, the second trajectory generated by SLAM reflects the effect of scale drift, in which the scale around the trajectory gradually increases or decreases. Therefore, as shown in FIG. 13, due to the effect of scale drift in SLAM, the distance interval in the movement in the second movement trajectory (400T') resulting from the linear transformation differs from the actual trajectory (movement trajectory 501T) from the actual one.
[0008] The present invention has been made in consideration of the above-mentioned circumstances, and provides a coordinate transformation system, a coordinate transformation method, and a program that can generate an estimated movement trajectory having a shape closer to the actual movement trajectory than conventional examples, from a distorted first movement trajectory consisting of movement positions including noise generated by GPS. [Means for solving the problem]
[0009] The coordinate transformation system of the present invention is characterized in that it comprises a first movement trajectory generation unit that generates a first movement trajectory in a first coordinate system of actual distance using each of time-series movement positions of a moving body measured by a GPS (Global Positioning System); a second movement trajectory generation unit that generates a second movement trajectory in a second coordinate system indicating a change in the relative position of the moving body using each of time-series captured images of a moving landscape; a second movement trajectory division unit that divides the second movement trajectory into predetermined widths to generate each of divided second movement trajectories; and a coordinate transformation unit that performs linear coordinate transformation on each of the divided second movement trajectories to match a corresponding portion of the first movement trajectory in the first coordinate system, and generates an estimated movement trajectory that estimates the movement trajectory of the moving body.
[0010] The coordinate transformation system of the present invention further includes a transformation matrix correction unit that corrects a transformation matrix that coordinate transforms each of the divided second movement trajectories to correspond to a portion of the first movement trajectory in a first coordinate system, wherein the coordinate transformation unit generates the transformation matrix that linearly transforms each of the divided second movement trajectories from the second coordinate system to correspond to the first movement trajectory in the first coordinate system, the transformation matrix correction unit performs a correction to reduce the amount of change in each element in the transformation matrix of each adjacent divided second movement trajectory, and the coordinate transformation unit performs coordinate transformation of the divided second movement trajectory using the transformation matrix corrected by the transformation matrix correction unit to generate the estimated movement trajectory.
[0011] The coordinate transformation system of the present invention is characterized in that the coordinate transformation unit uses a Gaussian process to obtain a function that reduces the amount of change in each of the elements in the transformation matrix of each of adjacent divided second movement trajectories, and calculates the elements of the transformation matrix using the function.
[0012] The coordinate transformation system of the present invention is characterized in that the coordinate transformation unit sets elements in the transformation matrix of each of adjacent divided second movement trajectories to numerical values estimated by a Kalman filter using the original elements of the transformation matrix of each of the consecutive divided second movement trajectories.
[0013] The coordinate transformation method of the present invention is characterized in that it includes a first movement trajectory generation process in which a first movement trajectory generation unit generates a first movement trajectory in a first coordinate system of actual distance using each of time-series movement positions of a moving body measured by a GPS (Global Positioning System); a second movement trajectory generation process in which a second movement trajectory generation unit generates a second movement trajectory in a second coordinate system indicating a change in the relative position of the moving body using each of time-series captured images of a moving landscape; a second movement trajectory division process in which a second movement trajectory division unit divides the second movement trajectory into predetermined widths to generate each of divided second movement trajectories; and a coordinate transformation process in which a coordinate transformation unit performs linear coordinate transformation on each of the divided second movement trajectories to match a corresponding portion of the first movement trajectory in the first coordinate system, and generates an estimated movement trajectory that estimates the movement trajectory of the moving body.
[0014] The program of the present invention causes a computer to function as a first movement trajectory generation means for generating a first movement trajectory in a first coordinate system of actual distance using each of the time-series movement positions of a moving body measured by a GPS (Global Positioning System), a second movement trajectory generation means for generating a second movement trajectory in a second coordinate system indicating a change in the relative position of the moving body using each of the time-series captured images of a moving landscape, a second movement trajectory division means for dividing the second movement trajectory into predetermined widths to generate each of the divided second movement trajectories, and a coordinate conversion means for performing linear coordinate transformation on each of the divided second movement trajectories to match a corresponding portion of the first movement trajectory in the first coordinate system, and generating an estimated movement trajectory that estimates the movement trajectory of the moving body. Effect of the Invention
[0015] As described above, according to the present invention, it is possible to provide a coordinate transformation system, a coordinate transformation method, and a program that can generate an estimated movement trajectory having a shape closer to the actual movement trajectory than conventional examples from a distorted first movement trajectory consisting of movement positions including noise generated by GPS. [Brief description of the drawings]
[0016] [Figure 1] 1 is a block diagram showing an example of the configuration of a coordinate transformation system according to a first embodiment of the present invention. [Diagram 2] 4 is a diagram showing an example of curve data of a first movement trajectory generated by a first movement trajectory generation unit 102. FIG. [Diagram 3] 13 is a diagram showing an example of curve data of a second trajectory generated by a second trajectory generating unit 103. FIG. [Figure 4] 13 is a diagram showing the relationship between the coordinate points of the ID numbers included in the divided second movement trajectories and the transformation matrix corresponding to the divided second movement trajectories. FIG. [Diagram 5] 5 is a flowchart showing the operation of a process of performing linear coordinate transformation of a coordinate point in a second coordinate system to a first coordinate system by the coordinate transformation system in the first embodiment. [Figure 6] FIG. 11 is a block diagram showing an example of the configuration of a coordinate transformation system according to a second embodiment of the present invention. [Figure 7] FIG. 13 is a diagram illustrating a process of smoothing each of the transformation matrices corresponding to adjacent divided second trajectories by using a Gaussian process. [Figure 8] 10 is a flowchart showing the operation of a process of performing linear coordinate transformation of a coordinate point in a second coordinate system to a first coordinate system by a coordinate transformation system in a second embodiment. [Figure 9] 11 is a diagram showing the correspondence between an actually measured movement trajectory and an estimated movement trajectory generated by the coordinate transformation system of this embodiment. FIG. [Figure 10] 1 is a diagram showing an example of a first movement trajectory generated based on latitude and longitude of GPS data from a GPS device mounted on a moving object. FIG. [Figure 11] 11 is a diagram showing an example of a second movement trajectory generated from captured image data captured by an imaging device mounted on a moving object. FIG. [Figure 12] 13 is a diagram showing an example in which a second movement trajectory in a second coordinate system is linearly transformed into a first coordinate system using the Umeyama method. FIG. [Figure 13]13 is a diagram for comparing the shape of a movement trajectory generated from actually measured movement positions with the shape of a second movement trajectory after linear transformation shown in FIG. 12. FIG. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS
[0017] <First embodiment> Hereinafter, a configuration example of a coordinate conversion system according to a first embodiment of the present invention will be described with reference to the drawings. Fig. 1 is a block diagram showing a configuration example of a coordinate conversion system according to a first embodiment of the present invention. In Fig. 1, a coordinate conversion system 100 includes a data input / output unit 101, a first movement trajectory generation unit 102, a second movement trajectory generation unit 103, a second movement trajectory division unit 104, a transformation matrix generation unit 105, a coordinate conversion unit 106, a display unit 107, a GPS data storage unit 108, a captured image data storage unit 109, a movement trajectory data storage unit 110, and a transformation matrix storage unit 111.
[0018] The data input / output unit 101 inputs, from a GPS device (not shown) and an imaging device (not shown), time-series position data (data indicating a position in latitude and longitude, hereinafter referred to as GPS data) corresponding to the movement of a mobile body equipped with a GPS device, obtained by GPS, and captured image data (hereinafter referred to as captured image data) of the scenery around the mobile body, captured by an imaging device mounted on the mobile body, respectively. Furthermore, the data input / output unit 101 assigns an ID (identification) number (an example of identification information) to the input GPS data in the GPS data storage unit 108, and writes and stores the data together with the ID number in chronological order.
[0019] Similarly, the data input / output unit 101 assigns an ID number to the captured image data input to the captured image data storage unit 109, and writes and stores the data together with the ID number in chronological order. Here, the timing at which the GPS device and the imaging device acquire GPS data and captured image data, respectively, is synchronously controlled by the data input / output unit 101. Therefore, the ID number assigned to the GPS data and the ID number assigned to the captured image data are the same number.
[0020] The first movement trajectory generation unit 102 reads the GPS data from the GPS data storage unit 108, places it in a three-dimensional coordinate system (e.g., an absolute coordinate system of latitude, longitude, and altitude (first coordinate system)), and generates curve data for the first movement trajectory by connecting it in the order of ID numbers. Then, the first trajectory generating unit 102 writes the generated curve data of the first trajectory into the trajectory data storage unit 110 for storage. Fig. 2 is a diagram showing an example of curve data of a first movement trajectory generated by the first movement trajectory generating unit 102. In Fig. 2, coordinate points 201P indicated by GPS data and corresponding to ID numbers are arranged on a three-dimensional coordinate system consisting of latitude, longitude, and altitude, which is an absolute coordinate system, and each of the coordinate points 201P is connected by a line segment 201L in the order of the ID numbers.
[0021] Returning to Figure 1, the second movement trajectory generation unit 103 reads out the captured image data from the captured image data storage unit 108, calculates the amount of relative position change from the previous position in the captured image in the order of the ID numbers (for example, using the SLAM technique), places it in a three-dimensional relative coordinate system (second coordinate system) (for example, x-axis, y-axis and z-axis, where the x-axis and y-axis are axes that form the ground surface and the z-axis is the axis that indicates the height direction), and generates curve data for the second movement trajectory by connecting them in the order of the ID numbers. Then, the second trajectory generating unit 103 writes the curve data of the generated second trajectory into the trajectory data storage unit 110 to store it.
[0022] Fig. 3 is a diagram showing an example of curve data of a second movement trajectory generated by the second movement trajectory generating unit 103. In Fig. 3, coordinate points 302P indicating movement positions generated from captured image data 301 corresponding to ID numbers are arranged on a three-dimensional coordinate system (second coordinate system) which is a relative coordinate system indicating relative positions, and each of the coordinate points 302P is connected by a line segment 302L in the order of the ID numbers. Furthermore, each of frames 303, 304, and 305 is a frame for selecting movement positions to be divided in order to divide (separate) the second movement trajectory into multiple movement position units (five units in FIG. 3) to generate divided second movement trajectories (described later).
[0023] Returning to Fig. 1, the second trajectory division unit 104 divides the second trajectory generated by the second trajectory generation unit 103 into a plurality of movement position units (a group of movement positions selected by a frame in Fig. 3), thereby generating the divided second trajectories. The number of movement positions in the group of movement position units can be arbitrarily set by the user. Here, the number of movement positions in the group may be changed, and the number of movement positions at which the finally generated estimated trajectory (described later) is closest to the actual trajectory may be found by simulation, and this number may be used as a default value.
[0024] The transformation matrix generation unit 105 generates a linear transformation matrix (hereinafter simply referred to as transformation matrix) that performs a linear coordinate transformation on the coordinate values of each coordinate point of the ID number included in the divided second movement trajectory (hereinafter simply referred to as coordinate points) to positions corresponding to each coordinate point of the first movement trajectory having the same ID number as the divided second movement trajectory. At this time, the transformation matrix generating unit 105 generates a transformation matrix that performs linear coordinate transformation on the coordinate points on the divided second movement trajectory and the coordinate points on the first movement trajectory, so as to minimize the distance between the coordinate points with the same ID number, that is, the sum of the errors (or the square of the errors when the Umeyama method is used). In other words, this transformation matrix is a matrix that performs linear coordinate transformation on the coordinate points on the divided second movement trajectory from the second coordinate system to the coordinate values of the first coordinate system. Then, the transformation matrix generation unit 105 writes the generated transformation matrix into the transformation matrix storage unit 111 for storage therein.
[0025] FIG. 4 is a diagram showing the relationship between the coordinate points of the ID numbers included in the divided second movement trajectories and the transformation matrix corresponding to the divided second movement trajectories. The transformation matrix M1 is a transformation matrix that performs a linear coordinate transformation on a divided second movement trajectory (for example, a movement trajectory consisting of coordinate points divided by frame 303 in Figure 3) including coordinate values with ID numbers ID1 to ID5, with each of the coordinate values with ID numbers ID1 to ID5 corresponding to the ID numbers ID1 to ID5 of the first movement trajectory, respectively.
[0026] Similarly, transformation matrix M2 is a transformation matrix that performs linear coordinate transformation on a divided second movement trajectory (for example, a movement trajectory consisting of coordinate points divided by frame 304 in Figure 3) including coordinate values with ID numbers ID2 to ID6, with each of the coordinate values with ID numbers ID2 to ID6 corresponding to the ID numbers ID2 to ID6 of the first movement trajectory, respectively. Each of the other transformation matrices M3 to M5 is similar to each of the above-mentioned transformation matrices M1 and M2. In this embodiment, each divided second movement trajectory is generated by shifting the ID number by one in the direction of travel, so that the movement positions included in each divided second movement trajectory overlap with the divided second movement trajectories before and after the divided second movement trajectory.
[0027] Then, the coordinate transformation unit 106 performs linear transformation on the coordinate points in the second coordinate system on the second trajectory to coordinate points in the first coordinate system in correspondence with the first trajectory, thereby generating an estimated trajectory. At this time, the coordinate transformation unit 106 selects a transformation matrix to be used for linear coordinate transformation of each of the coordinate points in the second coordinate system. For example, the coordinate conversion unit 106 reads out a conversion matrix corresponding to the divided second movement trajectory in which the coordinate point to be converted in the second coordinate system is located at the center from the conversion matrix storage unit 111, and performs linear coordinate conversion of the coordinate point from the second coordinate system to the first coordinate system using the conversion matrix. In the example of Fig. 4, the coordinate conversion unit 106 reads out and uses a conversion matrix M3 from the conversion matrix storage unit 111 when performing linear coordinate conversion of the coordinate point of ID5 on the second movement trajectory from the second coordinate system to the first coordinate system.
[0028] In addition, the coordinate transformation unit 106 may be configured to read out all of the transformation matrices corresponding to the divided second movement trajectory that includes the coordinate point to be transformed in the second coordinate system from the transformation matrix storage unit 111, calculate the average value of each element that is a component at the same row and column position in the transformation matrix, generate a new average transformation matrix with the average value as a new element, and perform a linear coordinate transformation of the coordinate point to be transformed.
[0029] Furthermore, instead of simply calculating the average value, the average value may be calculated by weighting the elements of each transformation matrix (by adding a weighting factor) depending on the position of the divided second movement trajectory where the coordinate point of the target is located. For example, the weighting factor of the matrix element corresponding to the divided second movement trajectory where the coordinate point of the target is located in the center is set to be larger than the weighting factor of the matrix element corresponding to the divided second movement trajectory where the coordinate point of the target is located away from the center. In the case of the transformation matrices shown in Figure 5, for a coordinate point with an ID number of ID5 in the second coordinate system, the weighting coefficient of the elements of transformation matrix M3 is set to the largest, the weighting coefficients of transformation matrices M2 and M4 are set to be smaller than the weighting coefficient of transformation matrix M3, and the weighting coefficients of transformation matrices M1 and M5 are set to be smaller than the weighting coefficients of transformation matrices M2 and M4.
[0030] FIG. 5 is a flowchart showing the operation of a process of performing linear coordinate transformation of a coordinate point in the second coordinate system into the first coordinate system by the coordinate transformation system in the first embodiment. Step S101: The data input / output unit 101 writes and stores the GPS data supplied from the GPS device in the GPS data storage unit 108 together with the assigned ID number in chronological order.
[0031] Step S102: The data input / output unit 101 also assigns the same ID number as the GPS data supplied at the same timing to the captured image data captured by the imaging device at the same timing as the timing at which the GPS data was acquired, and writes and stores the data in chronological order in the captured image data storage unit 109.
[0032] Step S103: The first movement trajectory generation unit 102 reads the GPS data from the GPS data storage unit 108 in chronological order (for example, in order of ID numbers), places each of the GPS data in the first coordinate system, and connects each of the coordinate points with line segments in the order of their respective ID numbers to generate curve data for the first movement trajectory. Then, the first movement trajectory generating unit 102 writes the generated curve data of the first movement trajectory into the movement trajectory data storage unit 110 for storage.
[0033] Step S104: The second movement trajectory generating unit 103 reads the captured image data from the captured image data storage unit 109 in chronological order (in the order of ID numbers), and arranges the movement positions relative to the respective starting points as coordinate values in the second coordinate system. Then, the second movement trajectory generation unit 103 connects the coordinate values arranged in the second coordinate system with line segments in the order of the ID numbers, and generates curve data of the second movement trajectory. Then, the second trajectory generating unit 103 writes the curve data of the generated second trajectory into the trajectory data storage unit 110 for storage.
[0034] Step S105: The second trajectory division unit 104 reads out the curve data of the second trajectory from the trajectory data storage unit 110. Then, the second movement trajectory division unit 104 divides the second movement trajectory into divided second movement trajectories each including the coordinates of a predetermined number of movement positions by shifting the frame for dividing the movement trajectory one by one in chronological order (Figures 3 and 4). The second trajectory division unit 104 writes and stores each of the divided second trajectories generated by dividing the second trajectory in the trajectory data storage unit 110.
[0035] Step S 106 : The transformation matrix generation unit 105 reads out the curve data of the first trajectory from the trajectory data storage unit 110 .
[0036] Step S107: The transformation matrix generation unit 105 reads out data of the divided second trajectories from the trajectory data storage unit 110. Then, the transformation matrix generation unit 105 generates a transformation matrix that linearly transforms each of the coordinate values included in the divided second movement trajectory from the second coordinate system to the first coordinate system so that each of the coordinate values having the same ID number in the first movement trajectory in the first coordinate system corresponds to each of the coordinate values having the same ID number, and the distance between the coordinate values having the same ID number is minimized. The transformation matrix generation unit 105 corresponds the generated transformation matrix to a pair of ID numbers of coordinate values included in the divided second movement trajectory corresponding to the transformation matrix, and writes and stores the pair of the transformation matrix and the ID number in the transformation matrix memory unit 111.
[0037] Step S108: The transformation matrix generation unit 105 detects whether or not transformation matrices corresponding to all of the divided second movement trajectories obtained by dividing the second movement trajectory have been generated, that is, whether or not there are any divided second movement trajectories for which a transformation matrix has not been generated. At this time, if the transformation matrix generation unit 105 has generated transformation matrices corresponding to all of the divided second trajectories obtained by dividing the second trajectory, the process proceeds to step S109. On the other hand, if the transformation matrix generation unit 105 has not generated transformation matrices corresponding to all of the divided second trajectories obtained by dividing the second trajectory, the process proceeds to step S107.
[0038] Step S109: The coordinate conversion unit 106 reads out from the movement trajectory data storage unit 110 the coordinate points on the second movement trajectory in the second coordinate system together with the ID numbers. Next, the coordinate transformation unit 106 causes the transformation matrix storage unit 111 to read out, for example, each of the transformation matrices corresponding to all of the divided second trajectories that include the ID number of the coordinate point to be processed. Then, the coordinate transformation unit 106 calculates the average value of each element in the same column and row in the read transformation matrix, obtains an average transformation matrix whose elements are these average values, and prepares a transformation matrix that transforms coordinate points in the second coordinate system into the first coordinate system.
[0039] Step S110: The coordinate conversion unit 106 performs coordinate conversion to convert coordinate points on the second movement trajectory in the second coordinate system into coordinate points in the first coordinate system. The coordinate conversion unit 106 writes and stores the coordinate points obtained by converting the coordinate points in the second coordinate system into the coordinate points in the first coordinate system in the movement trajectory data storage unit 110 together with the ID number.
[0040] Step S111: The coordinate conversion unit 106 detects whether or not all coordinate points on the second movement trajectory have been subjected to coordinate conversion into the first coordinate system, that is, detects whether or not there is a second movement trajectory for which coordinate conversion has not been performed. At this time, if the coordinate conversion unit 106 has converted all the coordinate points on the second movement trajectory into the first coordinate system, the process proceeds to step S112. On the other hand, if the coordinate conversion unit 106 has not yet converted all the coordinate points on the second movement trajectory into the first coordinate system, the process proceeds to step S109.
[0041] Step S112: The coordinate conversion unit 106 reads out the coordinate points (ie, coordinate values) on the second trajectory that have been converted into the first coordinate system from the trajectory data storage unit 110, and places them at the corresponding coordinate values in the second coordinate system. Then, the coordinate conversion unit 106 connects each of the coordinate points arranged in the first coordinate system in the order of the ID numbers, and generates curve data of the estimated movement trajectory. The coordinate conversion unit 106 writes the generated curve data of the estimated movement trajectory into the movement trajectory data storage unit 110 for storage. Furthermore, the coordinate conversion unit 106 may be configured to display the generated curve data of the estimated movement trajectory on the display screen of the display unit 107 (as on the display screen shown in FIG. 9, which will be described later).
[0042] The above-mentioned coordinate transformation system divides the second movement trajectory generated from the captured image data in the second coordinate system into divided second movement trajectories composed of a predetermined number of coordinate points, generates a transformation matrix that linearly transforms the coordinate points in the divided second movement trajectories to positions in the first coordinate system where the sum of the distances (or squares of the distances) between each of the coordinate points in the divided second movement trajectories and each of the coordinate points having the same ID number in the first movement trajectory generated from the GPS data is the smallest, and transforms the coordinates of the divided second movement trajectories from the second transformation matrix to the first transformation matrix using this transformation matrix. By performing the conversion, the effect of scale drift of the second movement trajectory is eliminated in each of the divided second movement trajectories due to the absolute position information of the GPS data of the first movement trajectory, and the locally correct information of the changing shape of the movement trajectory that the second movement trajectory has is reflected to smooth out the noise in the first movement trajectory, and the distorted shape including noise due to GPS is reshaped, thereby obtaining an estimated movement trajectory of the moving body that has both absolute position information and relative changing shape (i.e., a shape with smooth relative movement changes having absolute position).
[0043] <Second embodiment> Hereinafter, a configuration example of a coordinate conversion system according to a second embodiment of the present invention will be described with reference to the drawings. FIG. 6 is a block diagram showing a configuration example of a coordinate conversion system according to a second embodiment of the present invention. In FIG. 6, the coordinate conversion system 100A includes a data input / output unit 101, a first movement trajectory generation unit 102, a second movement trajectory generation unit 103, a second movement trajectory division unit 104, a transformation matrix generation unit 105, a coordinate conversion unit 106, a display unit 107, a GPS data storage unit 108, a captured image data storage unit 109, a movement trajectory data storage unit 110, a transformation matrix storage unit 111, and a transformation matrix correction unit 112. In the coordinate conversion system 100A shown in FIG. 6, the same reference numerals are assigned to the same configurations as those in the first embodiment shown in FIG. 1. The configuration and operation different from the first embodiment will be described below.
[0044] The transformation matrix correction unit 112 corrects the elements of each of the transformation matrices generated by the transformation matrix generation unit 105 to generate a corrected transformation matrix. Here, the transformation matrix correction unit 112 performs a process of reducing the amount of change between each matrix of elements at the same column and row positions between adjacent divided second trajectories in the set of transformation matrices (M1, M2, M3, M4, M5, ..., Mn) generated by the transformation matrix generation unit 105 (hereinafter, referred to as smoothing the transformation matrix), where n is an integer.
[0045] In this embodiment, each of the first and second coordinate systems is a coordinate system that indicates a three-dimensional space, and therefore has 16 elements of 4 rows and 4 columns as transformation coordinates. Here, the coordinate transformation matrix has the property that the element in the fourth row of these 16 elements is (0,0,0,1). Therefore, in the transformation matrix Mn, there are 12 elements as scalar values used for transformation.
[0046] Each element in the same row and column is treated as a variable, and a series of variables (x1_11, x2_11, x2_11, ..., xn_11) is created, where x1_11 indicates the element in the first row and first column in transformation matrix M1, and xn_11 indicates the element in the first row and first column in transformation matrix Mn. That is, the transformation matrix correction unit 112 generates a sequence of 12 variables corresponding to the 12 elements that are scalar values.
[0047] Then, the transformation matrix correction unit 112 receives a number indicating the order of the transformation matrix M1, and generates, by a Gaussian process, a function that outputs an element in the sequence of variables corresponding to this number. FIG. 7 is a diagram illustrating a process of smoothing each of the transformation matrices corresponding to adjacent divided second trajectories using a Gaussian process. In FIG. 7, the vertical axis indicates the numerical values of variables (scalar values of elements), and the horizontal axis indicates the time-series order of the transformation matrix corresponding to the divided second trajectories. The numbers "1" through "8" indicate transformation matrices M1 through M8, respectively. Also, x1_11 through x8_11 indicate the scalar value of the element in the first row and first column of each of the transformation matrices M1 through M8, respectively.
[0048] The transformation matrix correction unit 112 uses the scalar values of elements located at the same column and row positions between each transformation matrix before correction as discrete learning data, and uses a Gaussian process to find a function F that is output corresponding to a number as a scalar value that reduces the amount of change in the scalar values of elements located at the same column and row positions between adjacent transformation matrices compared to before correction (finds a function F that reduces the amount of change due to noise in the change in the scalar value corresponding to the number and smooths the change in adjacent scalar values). That is, the transformation matrix correction unit 112 uses, for example, scalar values x1_11 to x8_11 of numbers 1 to 8, respectively, to generate a function F in which elements at the same column and row positions in adjacent transformation matrices, which are output corresponding to each input value (the transformation matrix number assigned according to the order of the progression direction of the movement trajectory), i.e., the output values (scalar values) of the function each have a relationship that has a smooth change.
[0049] Here, the transformation matrix correction unit 112, for example, sums up the differences between the scalar values of elements before correction in the same column and row position and the scalar values of elements output from the function F for all transformation matrices, and calculates a function by numerical calculation so that the relationship between the numbers and the output values is such that the sum is smaller and the total value of the amount of change in scalar values between adjacent transformation matrices output from the function is smaller. As a result, function F outputs scalar value x1_11' when number 1 is input, and outputs scalar value x2_11' when number 2 is input. Similarly, function F outputs scalar values x3_11' to x8_11' respectively when each of the other numbers 3 to 8 is input.
[0050] The transformation matrix correction unit 112 generates a corrected sequence of elements (variables) (x1_11', x2_11', x2_11', . . . , xn_11'), which is a set of scalar values of elements in the first row and first column of each of the transformation matrices M1 to M8. Then, similar to the set of scalar values of the elements in the first row and first column of each of the above-mentioned transformation matrices M1 to M8, the transformation matrix correction unit 112 creates a function that smoothes the transformation matrix, and generates a correction series for each of the elements in the i-th row (1≦i≦3, i is an integer) and j-th column (1≦j≦4, j is an integer).
[0051] The transformation matrix correction unit 112 generates corrected transformation matrices M1' to Mn' using each of the correction sequences (x1_ij', x2_ij', x2_ij', ..., xn_ij') of the elements in the i-th row and j-th column generated as described above. Moreover, the transformation matrix correction unit 112 writes each of the generated corrected transformation matrices M1' to Mn' into the transformation matrix storage unit 111 to store it therein. Coordinate conversion unit 106 converts the transformation matrices M1 to M8 generated by transformation matrix generation unit 105 from transformation matrix storage unit 111 and reads out corrected transformation matrices M1' to Mn' generated by transformation matrix correction unit 112, respectively. Then, the coordinate conversion unit 106 uses the read correction conversion matrix (e.g., M1' to M8') instead of the conversion matrix (M1 to M8) to convert the coordinate points (movement positions) of the second movement trajectory in the second coordinate system into coordinate points in the first coordinate system, as in the first embodiment.
[0052] FIG. 8 is a flowchart showing the operation of a process of performing linear coordinate transformation of a coordinate point in a second coordinate system to a first coordinate system by a coordinate transformation system in the second embodiment. In steps S201 to S208 and steps S210 to S213 in Fig. 8, the same processes as those in steps S101 to S108 and steps S109 to S112 in Fig. 5 in the first embodiment are performed. Steps S209 and S210 will be described below.
[0053] Step S209: The transformation matrix correction unit 112 reads out each of the transformation matrices M1 to Mn generated by the transformation matrix generation unit 105 from the transformation matrix storage unit 111. Then, the transformation matrix correction unit 112 uses the scalar values of elements located at the same column and row positions in the transformation matrices M1 to Mn as discrete learning data, and uses a Gaussian process to determine a function F that is output corresponding to a number as a scalar value that reduces the amount of change in the scalar values of elements between adjacent transformation matrices compared to before correction.
[0054] The transformation matrix correction unit 112 inputs the numbers of the transformation matrices M1 to Mn to the function F, and obtains, as the corresponding output, the scalar values of the elements at the same column and row positions in the transformation matrices M1 to Mn as a correction sequence. As a result, the transformation matrix correction unit 112 calculates, from the function F, scalar values of all elements in each of the transformation matrices M1 to Mn as a correction sequence, thereby generating each of the corrected transformation matrices M1' to Mn'. Then, the transformation matrix correction unit 112 writes each of the generated corrected transformation matrices M1' to Mn' into the transformation matrix storage unit 111 to store them.
[0055] Step S210: The coordinate conversion unit 106 reads out from the movement trajectory data storage unit 110 the coordinate points on the second movement trajectory in the second coordinate system together with the ID numbers. Next, the coordinate transformation unit 106, for example, reads out from the transformation matrix storage unit 111 each of the corrected transformation matrices (M1' to Mn') corresponding to all the divided second trajectories including the ID number of the coordinate point to be processed. Then, the coordinate transformation unit 106 calculates the average value of each element in the same column and row in the read correction transformation matrix, obtains an average transformation matrix whose elements are these average values, and prepares a transformation matrix that transforms coordinate points in the second coordinate system to the first coordinate system.
[0056] FIG. 9 is a diagram showing the correspondence between an actually measured movement trajectory and an estimated movement trajectory generated by the coordinate transformation system of this embodiment. In FIG. 9, coordinate point 501P is a moving point actually measured, and coordinate point 502P is a moving point on the estimated movement trajectory. Moreover, a line segment 501L is a line segment that connects the movement points that are actually measured, and a line segment 502L is a line segment that connects the movement points on the estimated movement trajectory. It can be seen from FIG. 9 that the estimated movement trajectory according to this embodiment corresponds well to the actually measured movement trajectory in terms of the positions of the moving points and the shape of the trajectory.
[0057] The coordinate transformation system described above divides a second movement trajectory generated from captured image data in a second coordinate system into divided second movement trajectories consisting of a predetermined number of coordinate points, generates a transformation matrix that linearly transforms coordinate points in the divided second movement trajectories to positions in the first coordinate system where the sum of the distances (or squares of the distances) between each of the coordinate points in the divided second movement trajectories and each of the coordinate points having the same ID number in the first movement trajectory generated from GPS data is the smallest, and generates a new correction transformation matrix by making the scalar values of elements in the same column and row positions of each of the transformation matrices into a series and reducing the amount of change in scalar values between transformation matrices corresponding to adjacent divided second movement trajectories using a Gaussian process. By generating a correction transformation matrix and performing coordinate transformation of the divided second movement trajectory from the second transformation matrix to the first transformation matrix, the influence of scale drift of the second movement trajectory is eliminated in each of the divided second movement trajectories due to the absolute position information of the GPS data of the first movement trajectory, and locally correct information of the changing shape of the movement trajectory that the second movement trajectory has is reflected, and by reducing noise using the Gaussian process, noise in the first movement trajectory is smoothed and the distorted shape including noise due to GPS is reshaped, so that an estimated movement trajectory of the moving body can be obtained that has both absolute position information and relative changing shape (i.e., a shape with smooth relative movement changes having absolute position).
[0058] <Third embodiment> A coordinate transformation system according to a third embodiment of the present invention will be described below. The configuration of the coordinate transformation system of the third embodiment is similar to that of the second embodiment shown in FIG. The following describes operations that differ from the second embodiment. In the second embodiment, the Gaussian process was used to smooth adjacent scalar values of the sequence of elements (variables) (x1_ij, x2_ij, x2_ij, ..., xn_ij) in each of the transformation matrices M1 to Mn, but in the third embodiment, a Kalman filter is used to perform the smoothing. Corresponding to each of the divided second trajectories generated from the second trajectory, each of the transformation matrices M1 to Mn has a continuous relationship in the order of the signs in the moving direction of the moving object on the second trajectory.
[0059] That is, the transformation matrix correction unit 112 inputs the estimated value r(k-1)_ij and the scalar value r(k-1)_ij to the Kalman filter, and performs processing to obtain the estimated value r(k)_ij. Here, 1≦k≦n, and k and n are integers. The scalar value r(k-1)_ij is the scalar value of the element in the i-th row and j-th column of the transformation matrix Mk-1. The estimated value r(k-1)_ij is the estimated value of the element in the i-th row and j-th column of the transformation matrix Mk. The Kalman filter then specifies and outputs an estimated value r(k)_ij of the element in the i-th row and j-th column of the transformation matrix Mk, based on the scalar value of the element in the i-th row and j-th column of the transformation matrix Mk-1 and the estimated value of the element in the i-th row and j-th column of the transformation matrix Mk-1.
[0060] As described above, the transformation matrix correction unit 112 obtains a corrected sequence (x1_ij', x2_ij, x2_ij', ..., xn_ij') from the sequence (x1_ij, x2_ij, x2_ij, ..., xn_ij) using a Kalman filter. Then, the transformation matrix correction unit 112 generates each of the corrected transformation matrices M1' to Mn' using the correction sequence for all of the elements in the rows and columns of each of the transformation matrices M1 to Mn. The transformation matrix correction unit 112 writes each of the generated corrected transformation matrices M1' to Mn' into the transformation matrix storage unit 111 to store them.
[0061] The coordinate transformation system divides the second movement trajectory generated from the captured image data in the second coordinate system into divided second movement trajectories composed of a predetermined number of coordinate points, generates a transformation matrix that linearly transforms the coordinate points in the divided second movement trajectories to positions in the first coordinate system where the sum of the distances (or squares of the distances) between each of the coordinate points in the divided second movement trajectories and each of the coordinate points having the same ID number in the first movement trajectory generated from the GPS data is the smallest, and generates a new corrected transformation matrix by making the scalar values of elements in the same column and row positions of each of the transformation matrices into a series and estimating the scalar values between the transformation matrices corresponding to adjacent divided second movement trajectories using a Kalman filter. By performing coordinate transformation of the divided second movement trajectory from the second transformation matrix to the first transformation matrix using this correction transformation matrix, the influence of scale drift of the second movement trajectory is eliminated in each of the divided second movement trajectories due to the absolute position information of the GPS data of the first movement trajectory, and locally correct information of the changing shape of the movement trajectory that the second movement trajectory has is reflected, and noise in the first movement trajectory is smoothed by noise reduction using a Kalman filter, and the distorted shape including noise due to GPS is reshaped, making it possible to obtain an estimated movement trajectory of the moving body that has both absolute position information and relative changing shape (i.e., a shape with smooth relative movement changes having absolute position).
[0062] Although the embodiment of the present invention has been described above with reference to the drawings, the specific configuration is not limited to this embodiment, and designs within the scope of the present invention that do not deviate from the gist of the present invention are also included. In this embodiment, the first coordinate system and the second coordinate system are each shown as a three-dimensional coordinate system indicating a three-dimensional space, but they may be configured as two-dimensional coordinate systems. In this case, the first coordinate system corresponds to a two-dimensional plane indicated by latitude and longitude, and the second coordinate system indicates the shape of a movement trajectory on the two-dimensional plane.
[0063] In addition, a program for realizing each function of the coordinate transformation system 100 in Fig. 1 and the coordinate transformation system 100A in Fig. 6 in the present invention may be recorded in a computer-readable recording medium, and the program recorded in the recording medium may be read into a computer system and executed to estimate the movement trajectory of a moving object using GPS data and captured image data. Note that the "computer system" referred to here includes hardware such as an OS and peripheral devices.
[0064] Additionally, "computer system" also includes WWW systems equipped with a homepage providing environment (or display environment). Additionally, "computer-readable recording medium" refers to portable media such as flexible disks, optical magnetic disks, ROMs, and CD-ROMs, as well as storage devices such as hard disks built into computer systems. Furthermore, "computer-readable recording medium" also includes devices that hold programs for a certain period of time, such as volatile memory (RAM) within computer systems that act as servers or clients when a program is transmitted via a network such as the Internet or a communication line such as a telephone line.
[0065] The above program may also be transmitted from a computer system in which the program is stored in a storage device or the like to another computer system via a transmission medium, or by transmission waves in the transmission medium. Here, the "transmission medium" that transmits the program refers to a medium that has the function of transmitting information, such as a network (communication network) such as the Internet or a communication line (communication line) such as a telephone line. The above program may also be for realizing part of the above-mentioned functions. Furthermore, it may be a so-called difference file (difference program) that can realize the above-mentioned functions in combination with a program already recorded in the computer system. [Explanation of symbols]
[0066] 100,100A...coordinate conversion system 101...Data input / output section 102...First movement trajectory generation unit 103…Second movement trajectory generation unit 104…Second movement locus division part 105...Transformation matrix generation unit 106... Coordinate conversion section 107...Display section 108…GPS data storage unit 109...Captured image data storage unit 110...Movement trajectory data storage unit 111...Transformation matrix memory unit 112...Transformation matrix correction unit
Claims
1. a first movement trajectory generation unit that generates a first movement trajectory in a first coordinate system of real distance by using each of time-series movement positions of the moving object measured by a GPS (Global Positioning System); a second movement trajectory generation unit that generates a second movement trajectory in a second coordinate system that indicates a change in a relative position of the moving object, using each of time-series captured images of a moving landscape; a second trajectory division unit that divides the second trajectory into predetermined widths to generate divided second trajectories; a transformation matrix generating unit that generates a transformation matrix for linearly transforming a coordinate point on the divided second movement trajectory having an identification number identical to an identification number of a coordinate point on the first movement trajectory to a position in a first coordinate system where a total value of distances to each of the coordinate points on the first movement trajectory or a total value of squares of distances to each of the coordinate points on the first movement trajectory is minimum; a coordinate transformation unit that performs linear coordinate transformation on each of the divided second trajectories by using the transformation matrix to match a corresponding portion of the first trajectory in a first coordinate system, thereby generating an estimated trajectory that estimates a trajectory of movement of the moving object; A coordinate transformation system comprising:
2. a transformation matrix correction unit that corrects a transformation matrix that performs coordinate transformation on each of the divided second movement trajectories so as to correspond to a portion of the first movement trajectory in a first coordinate system; The coordinate conversion unit generating the transformation matrix that linearly transforms each of the divided second movement trajectories from the second coordinate system to correspond to the first movement trajectory in the first coordinate system; the transformation matrix correction unit performs a correction to reduce an amount of change in each of the elements in the transformation matrix of each of the adjacent divided second movement trajectories; The coordinate conversion unit Using the transformation matrix corrected by the transformation matrix correction unit, coordinate transformation is performed on the divided second trajectory to generate the estimated trajectory.
2. The coordinate transformation system according to claim 1.
3. The transformation matrix correction unit: A function that sets each of the elements in the transformation matrix of each of the adjacent divided second movement trajectories to a numerical value with a smaller amount of change is obtained by a Gaussian process, and the elements of the transformation matrix are calculated by the function.
3. The coordinate transformation system according to claim 2.
4. The transformation matrix correction unit: The elements of the transformation matrix of each of the adjacent divided second trajectories are set to values estimated by a Kalman filter using the original elements of the transformation matrix of each of the consecutive divided second trajectories.
3. The coordinate transformation system according to claim 2.
5. a first movement trajectory generation step in which a first movement trajectory generation unit generates a first movement trajectory in a first coordinate system of real distance by using each of time-series movement positions of the moving object measured by a GPS (Global Positioning System); a second movement trajectory generation step in which a second movement trajectory generation unit generates a second movement trajectory in a second coordinate system indicating a change in a relative position of the moving object, using each of time-series captured images of a moving landscape; a second trajectory division step of dividing the second trajectory into a predetermined width to generate each divided second trajectory; a transformation matrix generation step in which a transformation matrix generation unit generates a transformation matrix for linearly transforming a coordinate point on the divided second movement trajectory having an identification number identical to an identification number of a coordinate point on the first movement trajectory to a position in a first coordinate system where a total value of distances to each of the coordinate points on the first movement trajectory is minimum or a total value of squares of distances is minimum; a coordinate transformation step in which a coordinate transformation unit performs linear coordinate transformation on each of the divided second trajectories by using the transformation matrix to match a corresponding portion of the first trajectory in a first coordinate system, thereby generating an estimated trajectory that estimates a trajectory of movement of the moving object; A coordinate transformation method comprising:
6. Computer, a first movement trajectory generating means for generating a first movement trajectory in a first coordinate system of real distance by using each of time-series movement positions of the moving object measured by a GPS (Global Positioning System); a second movement trajectory generating means for generating a second movement trajectory in a second coordinate system indicating a change in a relative position of the moving object, using each of time-series captured images of a moving landscape; a second movement trajectory dividing means for dividing the second movement trajectory into a predetermined width to generate each divided second movement trajectory; a transformation matrix generating means for generating a transformation matrix for linearly transforming a coordinate point on the divided second movement trajectory having the same identification number as an identification number of a coordinate point on the first movement trajectory to a position in the first coordinate system where the sum of distances to each of the coordinate points on the first movement trajectory is the smallest or the sum of squares of distances is the smallest; a coordinate transformation means for performing linear coordinate transformation on each of the divided second trajectories in accordance with the transformation matrix to match a corresponding portion of the first trajectory in a first coordinate system, thereby generating an estimated trajectory of the trajectory of the movement of the moving object; A program that functions as a
Citation Information
Patent Citations
Information processor, information processing method, and program
JP2011043419A
Systems and methods for VSLAM optimization
JP2013537995A
State estimation device, information terminal, information processing system, and program
JP2017156154A
Information processing device, data managing device, data managing system, method, and program
JP2018124787A
Method for estimating driving lane
JP2018517979A
