Location detection device, location detection method, and program

Abstract

The present invention provides a position detection device, a position detection method, and a program that can suppress the computational load and cost when detecting a position from relative and absolute positions, and that can perform accurate and stable position detection. [Solution] The position detection device 10 detects position by using in combination the relative position coordinates of the first coordinate system output by the relative position detection unit 12 and the absolute position coordinates of the second coordinate system output by the absolute position detection unit 14. The coordinate transformation unit 16 of the position detection device 10 transforms the relative position coordinates to the position coordinates of the second coordinate system, or transforms the absolute position coordinates to the position coordinates of the first coordinate system, and outputs the position coordinates after the coordinate transformation. The coordinate transformation unit 16 uses a recurrence relation to determine a coordinate transformation formula that fits a first movement trajectory using relative position coordinates and a second movement trajectory using absolute position coordinates in an arbitrary time interval up to the latest time.

Description

[Technical Field]

[0001] The present invention relates to a position detection device, a position detection method, and a program for detecting a position from relative position and absolute position. [Background technology]

[0002] Conventionally, methods have been used to detect the absolute position of moving objects using absolute position detection devices such as GPS. However, the position estimation of moving objects using absolute position detection devices becomes unstable in urban areas or indoor environments. Therefore, a method has been proposed to detect the position of a moving object by combining an absolute position detection device with a relative position detection device that detects the position by accumulating the displacement amount of the moving object from a reference position.

[0003] For example, the position detection device disclosed in Patent Document 1 comprises a movement amount detection device, a movement direction detection device, a relative position detection device, and an absolute position detection device. The relative position detection device detects the relative position by integrating the changes in the amount of movement and direction of movement per unit time from a reference point of the moving object, based on the outputs of the movement amount detection device and the movement direction detection device. The absolute position detection device detects its absolute position, for example, by using GPS.

[0004] Note that the coordinate system of the position obtained by the absolute position detection device is different from the coordinate system of the position obtained by the relative position detection device. Therefore, in order to use these two positions in combination, a coordinate transformation is necessary.

[0005] Therefore, in the position detection device described in Patent Document 1, the position movement trajectory obtained by the relative position detection device is fitted to the position movement trajectory obtained by the absolute position detection device. Specifically, coordinate transformation is performed on the movement trajectory obtained by the absolute position detection device so as to minimize the error between the movement trajectory obtained by the relative position detection device and the movement trajectory obtained by the absolute position detection device. This fitting process is repeated each time new data is acquired.

[0006] Incidentally, in the position detection device disclosed in Patent Document 1, fitting processing is performed using all the data acquired up to that point in a specific section each time a position with a small error is acquired from the absolute position detection device. For this reason, the longer the distance traveled by the moving object, the greater the memory area used, the computation load, and the computation cost. Furthermore, in Embodiment 1 of Patent Document 1, an effort is made to keep the memory area and computation cost constant by limiting the section of the movement trajectory used, but in this case, if the error in the detection result of the position detection device used in the fitting processing is large, the accuracy will deteriorate and stable position detection will not be possible. [Prior art documents] [Patent Documents]

[0007] [Patent Document 1] Patent No. 5066367 [Overview of the Initiative] [Problems that the invention aims to solve]

[0008] The present invention aims to provide a position detection device, a position detection method, and a program that can suppress computational load and cost when detecting a position from relative position and absolute position, and that can perform accurate and stable position detection. [Means for solving the problem]

[0009] (1) A position detection device according to one embodiment of the present invention is A position detection device that detects position by using in combination the relative position coordinates of a first coordinate system output by a relative position detection unit and the absolute position coordinates of a second coordinate system output by an absolute position detection unit, The system includes a coordinate transformation unit that transforms the relative position coordinates to the position coordinates of the second coordinate system, or transforms the absolute position coordinates to the position coordinates of the first coordinate system, and outputs the position coordinates after the coordinate transformation. The coordinate transformation unit obtains a coordinate transformation equation using a recurrence relation to fit a first movement trajectory based on relative position coordinates and a second movement trajectory based on absolute position coordinates over any time interval up to the most recent time.

[0010] (2) The coordinate transformation unit may fit the first movement trajectory and the second movement trajectory based on the first reference point of the first movement trajectory, the second reference point of the second movement trajectory, and the rotation between the first movement trajectory and the second movement trajectory.

[0011] (3) The recurrence relation may include a first recurrence relation relating to the updating of the first reference point, a second recurrence relation relating to the updating of the second reference point, and a third recurrence relation relating to the rotation in the fitting.

[0012] (4) The rotation in the fitting is determined by a point cloud registration method using singular value decomposition. The third recurrence relation may also be an equation for updating the sum of tensor products used in the point cloud registration method. Here, the tensor product refers to the tensor product of the position coordinates obtained by translating the relative position coordinates so that the first reference point is the origin, and the position coordinates obtained by translating the absolute position coordinates so that the second reference point is the origin.

[0013] (5) The coordinate transformation unit may calculate and update the first reference point in any time interval up to the latest time using the first recurrence relation, based on the relative position coordinates newly output by the relative position detection unit and the first reference point that was previously updated, and may calculate and update the second reference point in any time interval up to the latest time using the second recurrence relation, based on the absolute position coordinates newly output by the absolute position detection unit and the second reference point that was previously updated.

[0014] (6) The relative position coordinates and the absolute position coordinates may be assigned a common weight for each detection time.

[0015] (7) The recurrence relation may include a fourth recurrence relation relating to the updating of the weights.

[0016] (8) The weights may be set based on the standard deviation of the output values ​​of the relative position detection unit and the absolute position detection unit, respectively.

[0017] (9) The weight may be set based on the newness of the output values ​​of the relative position detection unit and the absolute position detection unit, respectively.

[0018] (10) The coordinate transformation unit does not need to update the first reference point and the second reference point, nor update the fitting, if the output values ​​of the relative position detection unit and / or the absolute position detection unit have a standard deviation that exceeds an arbitrary threshold.

[0019] (11) When the coordinate transformation unit performs a coordinate transformation so that the first movement trajectory and the second movement trajectory fit together, if the difference between the latest positions of the first movement trajectory and the second movement trajectory exceeds an arbitrary threshold, the first reference point and the second reference point, as well as the fitting, do not need to be updated.

[0020] (12) The relative position detection unit includes an optical distance measuring device, The absolute position detection unit may include a receiving device that receives positioning information via wireless communication.

[0021] (13) The first coordinate system and the second coordinate system are each three-dimensional coordinate systems, The coordinate transformation unit may perform the coordinate transformation by specifying the three-dimensional translation vector and rotation matrix.

[0022] (14) The position detection device further comprises a position display unit that displays the detected position based on the position coordinates output from the coordinate transformation unit, The coordinate transformation unit outputs the position coordinates of the second coordinate system to the position display unit. The position display unit may convert the position coordinates of the second coordinate system input from the coordinate transformation unit into position coordinates of the world coordinate system, which are expressed in latitude and longitude on Earth, and display them as the detected position.

[0023] (15) The position detection device further comprises a position display unit that displays the detected position based on the position coordinates output from the coordinate transformation unit, The coordinate transformation unit may output the position coordinates output by the absolute position detection unit to the position display unit if it determines that the output accuracy of the relative position detection unit has decreased.

[0024] (16) A location detection method according to one embodiment of the present invention is a detection method performed by a computer, A position detection method that detects a position by using in combination the relative position coordinates of a first coordinate system output by a relative position detection unit and the absolute position coordinates of a second coordinate system output by an absolute position detection unit, When transforming the relative position coordinates to the position coordinates of the second coordinate system, or transforming the absolute position coordinates to the position coordinates of the first coordinate system, and outputting the position coordinates after the coordinate transformation, a coordinate transformation formula is obtained using a recurrence relation to fit the first movement trajectory based on the relative position coordinates and the second movement trajectory based on the absolute position coordinates over any time interval up to the latest time.

[0025] (17) A program according to one embodiment of the present invention causes a computer to execute the above-described position detection method. [Effects of the Invention]

[0026] According to the present invention, the computational load and cost can be suppressed when detecting a position from relative and absolute positions. Furthermore, according to one aspect of the present invention, it is possible to perform accurate and stable position detection even when the error in the detection result of the position detection device used in the fitting process is large, while suppressing the memory area used. [Brief explanation of the drawing]

[0027] [Figure 1] Figure 1 illustrates an example of a position detection method using relative and absolute position coordinates. [Figure 2] Figure 2 illustrates an example of a position detection method using relative and absolute position coordinates. [Figure 3] Figure 3 illustrates an example of a position detection method using relative and absolute position coordinates. [Figure 4] Figure 4 is a block diagram showing the schematic configuration of a position detection device according to one embodiment of the present invention. [Figure 5] Figure 5 is a block diagram specifically showing the configuration of a position detection device according to one embodiment of the present invention. [Figure 6] Figure 6 is a flowchart showing the operation of a position detection device according to one embodiment of the present invention. [Figure 7] Figure 7 is a block diagram showing an example of a computer that implements a position detection device according to one embodiment of the present invention. [Modes for carrying out the invention]

[0028] Before describing embodiments of the present invention, an example of a position detection method using two types of position coordinates with different coordinate systems will be briefly described.

[0029] Figures 1 to 3 illustrate an example of a position detection method using relative coordinate system position coordinates (hereinafter referred to as relative position coordinates) and absolute coordinate system position coordinates (hereinafter referred to as absolute position coordinates). In Figures 1 and 2, (a) shows relative position coordinates, and (b) shows absolute position coordinates. Relative position coordinates are, for example, odometry coordinates calculated based on the direction and amount of movement of a moving object, while absolute position coordinates are, for example, UTM coordinates obtained by converting latitude and longitude measured by GNSS (Global Navigation Satellite System). Note that in Figures 1 and 2, (a) shows relative position coordinates p i (i is an integer greater than or equal to 1: in this example, i=1,2,3,4) and the movement trajectory based on relative position coordinates {p iReference point p of { O (In this example, the center of gravity) is shown. Also, in FIGS. 1 and 2(b), the absolute position coordinates p' i (i is an integer of 1 or more: in this example, i = 1, 2, 3, 4), and the movement locus {p' i} of the reference point p' O (In this example, the center of gravity) is shown.

[0030] The relative position coordinates shown in FIG. 1(a) and the absolute position coordinates shown in FIG. 1(b) have different coordinate systems. In order to detect the position of the moving object by using these two position coordinates in combination, it is necessary to perform coordinate transformation on one of the position coordinates to the position coordinates of the other coordinate system. Therefore, first, as shown in FIG. 2, the movement locus {p i} by relative position coordinates and the movement locus {p' i} by absolute position coordinates are each translated so that the reference points p O , p' O become the origin.

[0031] Next, as shown in FIG. 3, the movement locus {p i} by relative position coordinates and the movement locus {p' i} by absolute position coordinates are fitted. In the example of FIG. 3, by rotating the movement locus {p i} by relative position coordinates, the movement locus {p i} by relative position coordinates is fitted to the movement locus {p' i} by absolute position coordinates. As a result, it becomes possible to handle the relative position and the absolute position in a common coordinate system. Note that fitting can be executed using a point cloud registration method.

[0032] When both the relative coordinates and the absolute coordinates are coordinates of an orthogonal coordinate system, the coordinate transformation from the relative position coordinates to the absolute position coordinates can be represented by the following formula (1). In the following formula (1), p i , p O , and p' O are the above-mentioned relative position coordinates p i , the reference point p O , and the reference point p'O This indicates that Pi is the relative position coordinate p i The position coordinates are shown after transforming the relative position coordinates to absolute position coordinates, and R represents the rotation matrix. That is, equation (1) below represents the relative position coordinates p in relative coordinates. i This is a coordinate transformation formula that converts to a position coordinate Pi in absolute coordinates. (Math 1) Pi = R(p) i -p O )+p' O ...(1)

[0033] According to the position detection device, position detection method, and program according to the embodiment of the present invention, the reference point p in the above coordinate transformation formula (1) O , reference point p' O The rotation matrix R can be calculated using a recurrence relation. Below, a position detection device, position detection method, and program according to an embodiment of the present invention will be described with reference to the drawings.

[0034] [Device configuration] Figure 4 is a block diagram showing the schematic configuration of a position detection device according to one embodiment of the present invention. As shown in Figure 1, the position detection device 10 according to this embodiment includes a relative position detection unit 12, an absolute position detection unit 14, and a coordinate transformation unit 16. The position detection device 10 is a device that is installed on any moving object such as a person, robot, or automobile to detect the position of the moving object.

[0035] The relative position detection unit 12 outputs position coordinates in the relative coordinate system (hereinafter referred to as relative position coordinates). The absolute position detection unit 14 outputs position coordinates in the absolute coordinate system (hereinafter referred to as absolute position coordinates). In this embodiment, the relative coordinate system corresponds to the first coordinate system, and the absolute coordinate system corresponds to the second coordinate system. In this embodiment, both the relative coordinate system and the absolute coordinate system are Cartesian coordinate systems. As described above, the relative position coordinates are, for example, odometry coordinates, and the absolute position coordinates are, for example, UTM coordinates.

[0036] The coordinate transformation unit 16 transforms relative position coordinates to absolute position coordinates, or transforms absolute position coordinates to relative position coordinates, and outputs the position coordinates after the transformation. In this embodiment, the coordinate transformation unit 16 outputs the movement trajectory of relative position coordinates in any time interval up to the latest time {p i} and the movement trajectory using absolute position coordinates {p' i The above coordinate transformation equation (1) for fitting} and is obtained using a recurrence relation. In this embodiment, the movement trajectory {p i} corresponds to the first movement trajectory, and movement trajectory {p' i} corresponds to the second movement trajectory.

[0037] Thus, in the position detection device 10 according to this embodiment, the movement trajectory {p i} and movement trajectory {p' i Since the coordinate transformation equation for fitting {p} can be obtained using a recurrence relation, it is not necessary to use all the position coordinates obtained up to that point when performing a coordinate transformation. For example, using the data calculated during the previous coordinate transformation and the newly detected coordinate position, the movement trajectory {p} in any time interval up to the latest time can be obtained using a recurrence relation. i},{p' i Reference point p of} O ,p' O , and movement trajectory {p i} and movement trajectory {p' i A rotation matrix can be calculated to fit the object to the coordinates. This allows for accurate and stable coordinate transformation based on the entire trajectory, even when the distance traveled by the moving object is long, while suppressing the memory area used, computation load, and computation cost. In this embodiment, the reference point p O This corresponds to the first reference point, and reference point p' O This corresponds to the second reference point.

[0038] Next, the specific configuration of the position detection device 10 will be described. Figure 5 is a block diagram that specifically shows the configuration of a position detection device according to one embodiment of the present invention.

[0039] As shown in Figure 5, the position detection device 10 according to this embodiment includes a relative position detection unit 12, an absolute position detection unit 14, and a coordinate transformation unit 16, as well as a storage unit 18 and a position display unit 20.

[0040] Furthermore, in the position detection device 10 according to this embodiment, the relative position detection unit 12 includes an optical distance measuring device 12a, an IMU (Inertial Measurement Unit) 12b, and a processing unit 12c, and the absolute position detection unit 14 includes a receiving device 14a and a processing unit 14b.

[0041] The optical distance measuring device 12a is a device that measures the distance or shape of an object based on the information of the reflected light after irradiating it with laser light. In this embodiment, a known 3D-LiDAR is used as the optical distance measuring device 12a. The IMU 12b detects three-dimensional inertial motion. The processing unit 12c outputs relative position coordinates based on the detection results of the optical distance measuring device 12a and the IMU 12b. In this embodiment, the processing unit 12c outputs three-dimensional odometry coordinates (relative position coordinates). A known LIO (LiDAR Inertial Odometry) configuration can be used as the processing unit 12c.

[0042] The receiving device 14a receives positioning information via wireless communication. In this embodiment, the receiving device 14a acquires RTK-GNSS positioning information (latitude, longitude, altitude). The processing unit 14b outputs absolute position coordinates based on the positioning information acquired by the receiving device 14a. In this embodiment, the processing unit 14b converts the latitude and longitude acquired by the receiving device 14a into two-dimensional coordinates (UTM coordinates) using the Universal Transverse Mercator projection, and then adds the altitude acquired by the receiving device 14a to these UTM coordinates to generate three-dimensional absolute position coordinates.

[0043] The coordinate transformation unit 16 processes the relative position coordinates p output from the relative position detection unit 12. i , and the absolute position coordinate p' output from the absolute position detection unit 14. i Based on this, the reference point p in coordinate transformation equation (1) O , reference point p' OThe rotation matrix R is updated, and the updated values ​​are stored in the storage unit 18. Reference point p O , reference point p' O The method for updating the rotation matrix R will be described later.

[0044] In this embodiment, the coordinate transformation unit 16 uses the coordinate transformation formula (1) to transform the relative position coordinates p i The coordinates are transformed into absolute coordinate system position coordinates and output to the position display unit 20. The coordinate transformation unit 16 also outputs the absolute position coordinates output from the absolute position detection unit 14 to the position display unit 20. The coordinate transformation unit 16 may, for example, output the position coordinates output by the absolute position detection unit 14 to the position display unit 20 if it determines that the output accuracy of the relative position detection unit 12 has deteriorated (worsened below an arbitrary threshold). As a threshold for determining the deterioration of the output accuracy of the relative position detection unit 12, for example, as will be explained in the modified example described later, a value that is a constant multiple of the mean of the standard deviation included in the output value of the relative position detection unit 12 can be used.

[0045] The position display unit 20 includes a display device such as a liquid crystal display device and displays the detected position based on the position coordinates output from the coordinate transformation unit 16. In this embodiment, the position display unit 20 displays the detected position based on at least one of the position coordinates based on the output of the relative position detection unit 12 or the position coordinates based on the output of the absolute position detection unit 14. In this embodiment, the position display unit 20 converts the absolute coordinate system position coordinates input from the coordinate transformation unit 16 into world coordinate system position coordinates expressed in latitude and longitude on Earth and displays them as the detected position. The position display unit 20 may also merge the position coordinates based on the output of the relative position detection unit 12 or the position coordinates based on the output of the absolute position detection unit 14 using a known Kalman filter or the like and display them as the detected position. Furthermore, depending on the state of the relative position detection unit 12 or the absolute position detection unit 14, either the position coordinates based on the output of the relative position detection unit 12 or the position coordinates based on the output of the absolute position detection unit 14 may be selected and displayed as the detected position.

[0046] Below, reference point p O , reference point p' OThis section describes an example of how to update the rotation matrix R.

[0047] (Calculation of reference point) First, the reference point p O ,p' O The calculation method will be explained. In this embodiment, the relative position detection unit 12 and the absolute position detection unit 14 output relative position coordinates and absolute position coordinates at predetermined time intervals. The coordinate transformation unit 16 receives the i-th absolute position coordinate p' from the absolute position detection unit 14. i When you obtain the absolute position coordinates p' i The time t obtained i At the same time, the relative position coordinates obtained from the relative position detection unit 12 are relative position coordinates p i As the latest time t i Movement trajectory in any time interval up to {p i},{p' i The overall reference point (in this embodiment, the centroid) is calculated. Note that if the output time of the relative position detection unit 12 and the output time of the absolute position detection unit 14 do not match, time t i The relative position coordinates obtained immediately before or after are relative position coordinates p i This is also acceptable. Furthermore, time t i By interpolating the relative position coordinates obtained immediately before or after time t using the velocity and acceleration of the moving object, etc., at time t i relative position coordinates p i You may calculate this.

[0048] Specifically, in this embodiment, the coordinate transformation unit 16 transforms the N+1th (where N is an integer greater than or equal to 1) absolute position coordinate p' N+1 and relative position coordinate p N+1 Once obtained, the latest time t is calculated using the following equations (2) and (3). N+1 Any time interval up to (in this embodiment, from the start of measurement to time t) N+1 Movement trajectory in the time interval up to {p N+1},{p' N+1 Overall reference point p O,N+1 ,p' O,N+1 The following equation(2) corresponds to the first recurrence relation, and the following equation(3) corresponds to the second recurrence relation. [Number] [Number]

[0049] In the above formulas (2) and (3), S w,N and S w,N+1 are represented by the following formulas (4) and (5). [Number] [Number]

[0050] Also, in the above formula (4), w i,N is represented by the following formula (6). [Number]

[0051] Note that w i,N is the weight assigned to the position coordinates for each time t i (the detection time of the position coordinates). In this embodiment, the position coordinates p i , p' i output from the relative position detection unit 12 and the absolute position detection unit 14 each have a standard deviation σ i , σ' i centered on itself in the radial direction. Since the position coordinates with smaller standard deviations σ i , σ’ i are more reliable, in this embodiment, the position coordinates p i , p' i are given a weight of 1 / (σ i + σ' i ). Also, since cumulative errors almost always occur in the output value of the relative position detection unit 12, the error from the current information increases as we go back in time from the latest time t N . Therefore, for the position coordinates p i at time t​​i assigns weights of λ based on the novelty of the output value N-i Note that the λ in equations (2), (3), (5), and (6) is common. In this embodiment, λ is set to a value greater than 0.5 and less than or equal to 1. In this embodiment, the outputs of the processing unit 12c and the receiving device 14a include a covariance matrix. In this embodiment, the maximum eigenvalue of the covariance matrix output from the processing unit 12c and the receiving device 14a can be used as a representative value of the standard deviations σ and σ'. In this embodiment, the above equation (5) corresponds to the fourth recurrence formula.

[0052] In the above equation (2), for the reference point p O,N the initial value p O,1 can use the value of the relative position coordinate p1 obtained first. In the above equation (3), for the reference point p' O,N the initial value p' O,1 can use the value of the absolute position coordinate p'1 obtained first. That is, for the processing when the second relative position coordinate p2 and the absolute position coordinate p'2 are obtained, the position coordinates p1 and p'1 themselves can be used as the reference points p O,1 , p' O,1 .

[0053] Note that the displacement amounts Δp O,N+1 , Δp' O,N+1 of the reference points calculated by the following equations (7) and (8) can be used to obtain the reference points p O,N+1 , p' O,N+1 . The following equation (7) corresponds to the first recurrence formula, and the following equation (8) corresponds to the second recurrence formula.

Equation

Equation

[0054] (Calculation of rotation matrix) Next, the method for calculating the rotation matrix will be described. In this embodiment, the coordinate conversion unit 16 uses point cloud registration using singular value decomposition to obtain the movement trajectory {p based on the relative position coordinatesi} and movement trajectory using absolute position coordinates {p' i} is fitted. Specifically, the coordinate transformation unit 16 first calculates the sum S of the tensor products shown in equation (9) below. t,N+1 We perform singular value decomposition on it. In this embodiment, equation (9) below corresponds to the third recurrence relation.

number

[0055] Note that in equation (9) above, q i,N As shown in equation (10) below, time t N In this case, the movement trajectory based on relative position coordinates {p i The relative position coordinates p after} is translated so that the reference point becomes the origin. i The coordinates of are shown. Similarly, in equation (9) above, q' i,N As shown in equation (11) below, time t N In this case, the movement trajectory using absolute position coordinates {p' i The absolute position coordinates p' after} is translated so that the reference point is the origin. i The coordinates are shown.

number

number

[0056] Furthermore, in equation (9) above, S t,N Initial value S t,1 This is expressed by the following equation (12), S p,N+1 This is expressed by the following equation (13), S p',N+1 It is expressed by the following equation (14). Also, in equations (9), (13), and (14), S σ,N+1 This is expressed by the following formula (15).

number

number

number

number

[0057] Furthermore, in the above formula, S p,N Initial value S p,1 This is expressed by the following equation (16), S p',N Initial value S p',1 This is expressed by the following equation (17), S σ,N Initial value S σ,1 This is expressed by the following equation (18).

number

number

number

[0058] The singular value decomposition of equation (9) above is S t,N+1 =UΛV T Expressed as, the movement trajectory based on relative position coordinates {q i} and movement trajectory using absolute position coordinates {q' i The rotation matrix R that minimizes the discrepancy with} is calculated by the following equation (19).

number

[0059] (Updating the coordinate transformation formula) In this embodiment, each time the absolute position coordinates are output from the absolute position detection unit 14, the coordinate transformation unit 16 uses the method described above to determine the reference point p O , reference point p' O , and update the rotation matrix R, and according to equation (1) above, the relative position coordinate p on the relative coordinate system iThis is converted to absolute position coordinates Pi. In other words, in this embodiment, the coordinate transformation unit 16 identifies the translation vector and rotation matrix R each time absolute position coordinates are output from the absolute position detection unit 14, and performs the coordinate transformation.

[0060] (Effects and Benefits) In the position detection device 10 according to this embodiment, the reference point p O , reference point p' O The rotation matrix R and the reference point p can each be determined using recurrence relations. Specifically, the reference point p O This is calculated using the above formula (2), and the reference point p' O The above equation (3) is used to calculate the rotation matrix R, and the above equation (19) is used to calculate the reference point p that was updated last time. O , reference point p' O , and the sum of the tensor products S t Using the value of and the newly detected coordinate position, the movement trajectory {p} in any time interval up to the latest time can be calculated using a recurrence relation. i},{p' i Reference point p of} O ,p' O , and movement trajectory {p i} and movement trajectory {p' i The rotation matrix R for fitting {p} can be calculated. This allows for accurate coordinate transformation based on the entire movement trajectory while suppressing computation load and cost. i},{p' i Since there is no need to retain}, the capacity of the storage unit 18 can be avoided being strained. In this embodiment, the storage unit 18 stores the reference point p O , reference point p' O , the sum of tensor products S t The updated value of the rotation matrix R is also stored.

[0061] In this embodiment, a common weight is assigned to the relative position coordinates and absolute position coordinates for each detection time. In this case, weighting can be performed considering the circumstances at the time the position coordinates were detected, thus enabling appropriate fitting. In particular, in this embodiment, weights are assigned to the position coordinates based on the standard deviation of the position coordinates and the recency of the output value of the detection unit. This enables more appropriate fitting. In this embodiment, the weights can also be calculated using a recurrence relation (equation (5) above). This allows for accurate and stable position detection even when the error in the detection result is large, while suppressing the computation load and computation cost, and improving the accuracy of coordinate transformation.

[0062] [Device operation] Next, the operation of the position detection device 10 according to this embodiment will be explained with reference to Figure 7. Figure 6 is a flowchart showing the operation of the position detection device 10 according to one embodiment of the present invention. The position detection method according to this embodiment is performed by operating the position detection device 10.

[0063] As shown in Figure 6, in this embodiment, as described above, first the coordinate transformation unit 16 obtains relative position coordinates and absolute position coordinates from the relative position detection unit 12 and the absolute position detection unit 14 (step S1).

[0064] Next, as described above, the coordinate transformation unit 16 determines the movement trajectory using relative position coordinates in any time interval up to the latest time {p i} and the movement trajectory using absolute position coordinates {p' i The coordinate transformation equation (1) that fits} and is obtained using a recurrence relation (step S2). More specifically, as described above, the coordinate transformation unit 16 is the reference point p O , reference point p' O The coordinate transformation equation (1) is updated by updating the rotation matrix R.

[0065] Next, the coordinate transformation unit 16 uses the updated coordinate transformation formula (1) to determine the movement trajectory based on relative position coordinates {p i} is the movement trajectory based on absolute position coordinates {p'i The relative position coordinates are transformed into absolute position coordinates to fit the} and output to the position display unit 20 (step S3). In this embodiment, the coordinate transformation unit 16 also outputs the absolute position coordinates output from the absolute position detection unit 14 to the position display unit 20.

[0066] Finally, the position display unit 20 converts the position coordinates output from the coordinate transformation unit 16 into position coordinates in the world coordinate system, which are expressed as latitude and longitude on Earth, and displays them as the detected position (step S4). In this embodiment, steps S1 to S4 are repeatedly executed each time position coordinates are output from the relative position detection unit 12 and the absolute position detection unit 14.

[0067] [program] The program for this embodiment can be any program that causes a computer to execute steps S1 to S4 shown in Figure 6. By installing and executing this program on a computer, the position detection device and position detection method of this embodiment can be realized. In this case, the computer's processor functions as a relative position detection unit 12, an absolute position detection unit 14, a coordinate transformation unit 16, and a position display unit 20, and performs the processing.

[0068] Furthermore, in this embodiment, the storage unit 18 is realized by storing the data files that constitute it in a storage device such as a hard disk provided in a computer, or by mounting the recording medium on which the data files are stored in a reading device connected to a computer.

[0069] Furthermore, the program according to this embodiment may be executed by a computer system constructed by multiple computers. In this case, each computer may function as one of the relative position detection unit 12, absolute position detection unit 14, coordinate transformation unit 16, and position display unit 20. Also, the storage unit 18 may be constructed on a computer other than the computer that executes the program according to this embodiment.

[0070] [Physical configuration] Figure 7 is a block diagram showing an example of a computer that implements a position detection device according to one embodiment of the present invention. The computer 110 implements the position detection device 10 according to this embodiment by executing a program according to this embodiment.

[0071] As shown in Figure 7, the computer 110 comprises a CPU 111, main memory 112, storage device 113, input interface 114, display controller 115, data reader / writer 116, and communication interface 117. Each of these components is connected to each other via a bus 121 to enable data communication. In addition to the CPU 111, or in place of the CPU 111, the computer 110 may also include a GPU (Graphics Processing Unit) or an FPGA (Field-Programmable Gate Array).

[0072] The CPU 111 loads the program (code) in this embodiment, stored in the storage device 113, into the main memory 112 and performs various calculations by executing them in a predetermined order. The main memory 112 is typically a volatile storage device such as DRAM (Dynamic Random Access Memory). The program in this embodiment is provided stored in a computer-readable recording medium 120. The program in this embodiment may also be distributed over the internet connected via the communication interface 117.

[0073] Furthermore, specific examples of the storage device 113 include hard disk drives and semiconductor storage devices such as flash memory. The input interface 114 mediates data transmission between the CPU 111 and input devices 118 such as a keyboard and mouse. The display controller 115 is connected to the display device 119 and controls the display on the display device 119.

[0074] The data reader / writer 116 mediates data transmission between the CPU 111 and the recording medium 120, reads programs from the recording medium 120, and writes processing results from the computer 110 to the recording medium 120. The communication interface 117 mediates data transmission between the CPU 111 and other computers.

[0075] Furthermore, specific examples of the recording medium 120 include general-purpose semiconductor memory devices such as CF (Compact Flash®) and SD (Secure Digital), magnetic recording media such as Flexible Disks, or optical recording media such as CD-ROMs (Compact Disk Read Only Memory).

[0076] Furthermore, the position detection device 10 according to this embodiment may be implemented using hardware corresponding to each part, rather than a computer on which a program is installed. Also, the position detection device 10 may be partially implemented by a program and the remaining part by hardware.

[0077] [Differentiation] In the above embodiment, we have described a case in which the processing of steps S1 to S4 is repeatedly executed each time position coordinates are output from the relative position detection unit 12 and the absolute position detection unit 14. However, depending on the situation, the reference point p in the coordinate transformation formula (1) may be used. O , reference point p' O , and the sum of the tensor products S t You may stop updating the rotation matrix (R).

[0078] For example, the coordinate transformation unit 16 determines the reference point p if the output values ​​of the relative position detection unit 12 and / or absolute position detection unit 14 have a standard deviation that exceeds an arbitrary threshold. O and reference point p' OIt is not necessary to update the reference point or the fitting. In this case, the detection of position coordinates by the relative position detection unit 12 and the absolute position detection unit 14 is treated as if it did not occur and is not counted as a detection count. This makes it possible to remove absolute coordinates with low precision. As a threshold in this case, for example, a value that is a constant multiple of the mean of past standard deviations can be used. Note that if the reference point and fitting are not updated, it is not necessary to output the position coordinates, or the position coordinates transformed using the coordinate transformation formula updated last time may be output. The same applies to the modified examples described below.

[0079] Furthermore, for example, the coordinate transformation unit 16 determines the movement trajectory {p i} and movement trajectory {p' i When the coordinate transformation is performed so that} fits, the movement trajectory {p i} and movement trajectory {p' i The latest position p i ,p' i If the difference exceeds an arbitrary threshold, the reference point p O , reference point p' O , and the sum of the tensor products S t It is not necessary to update the rotation matrix (R). In this case, the detection of position coordinates by the relative position detection unit 12 and the absolute position detection unit 14 is treated as if it did not occur and is not counted as a detection count. This makes it possible to remove absolute coordinates with low accuracy. As a threshold in this case, for example, a value that is a constant multiple of the standard deviation of the latest position can be used. If the threshold is exceeded consecutively, the current coordinate transformation formula itself may be incorrect, so the threshold may be relaxed depending on the number of times the threshold has been exceeded n. For example, a value obtained by exponentially increasing the average of past standard deviations according to the number of times the threshold has been exceeded n may be used as the threshold.

[0080] The above describes the case of transforming an axial position into an absolute position, but it is also possible to transform an absolute position into a relative position.

[0081] In the above embodiment, the case using RTK-GNSS was described, but various other satellite positioning methods or other positioning methods using radio beacons, etc., may also be used. Furthermore, the Japanese Plane Rectangular Coordinate System may be used instead of the Universal Transverse Mercator projection, and wheel odometry may be used instead of LIO. In addition, the position detection unit may perform relative position detection using radar that uses millimeter waves or microwaves instead of or in combination with the optical ranging device.

[0082] In the above embodiment, the weights defined by equation (6) are assigned to the position coordinates detected by the relative position detection unit 12 and the absolute position detection unit 14. However, the weights assigned to the position coordinates may be changed as appropriate, or no weights may be assigned at all. Also, in the above embodiment, the movement trajectory {p i},{p' i We have explained the case where the center of gravity is used as the reference point for each, but the movement trajectory {p i},{p' i The center of each can be used as the reference point. Also, the movement trajectory {p i},{p' i If a common method is used to determine the reference point for}, the reference point does not have to be the centroid or center.

[0083] Furthermore, in the above embodiment, the movement trajectory {p i},{p' i The reference point of} and the sum of the tensor products S t The rotation matrix R is determined using a recurrence relation, but it is not necessary to use a recurrence relation when determining either the reference point or the rotation matrix R. However, from the viewpoint of reducing computational cost, it is preferable to determine both the reference point and the rotation matrix R using a recurrence relation. Similarly, when determining the weights assigned to the position coordinates, it is not necessary to use a recurrence relation, but from the viewpoint of reducing computational cost, it is preferable to use a recurrence relation. [Industrial applicability]

[0084] According to the present invention, when detecting a position from relative and absolute positions, the computational load and cost can be suppressed, and accurate and stable position detection can be achieved even when the error in the detection result is large. [Explanation of symbols]

[0085] 10 Position detection device 12 Relative position detection unit 12a Optical ranging device 12b IMU 12c Processing Unit 14 Absolute position detection unit 14a Receiving device 14b Processing Unit 16 Coordinate Transformation Unit 18 Memory section 20 Position display section

Claims

1. A position detection device that detects position by using in combination the relative position coordinates of a first coordinate system output by a relative position detection unit and the absolute position coordinates of a second coordinate system output by an absolute position detection unit, The system includes a coordinate transformation unit that transforms the relative position coordinates to the position coordinates of the second coordinate system, or transforms the absolute position coordinates to the position coordinates of the first coordinate system, and outputs the position coordinates after the coordinate transformation. The coordinate transformation unit obtains a coordinate transformation equation using a recurrence relation to fit the first movement trajectory using relative position coordinates and the second movement trajectory using absolute position coordinates over any time interval up to the most recent time. Location detection device.

2. The position detection device according to claim 1, wherein the coordinate transformation unit fits the first movement trajectory and the second movement trajectory based on a first reference point of the first movement trajectory, a second reference point of the second movement trajectory, and rotation between the first movement trajectory and the second movement trajectory.

3. The position detection device according to claim 2, wherein the recurrence relation includes a first recurrence relation relating to the updating of the first reference point, a second recurrence relation relating to the updating of the second reference point, and a third recurrence relation relating to the rotation in the fitting.

4. The rotation in the fitting is determined by a point cloud registration method using singular value decomposition. The position detection device according to claim 3, wherein the third recurrence relation is an equation for updating the sum of the tensor products of the position coordinates obtained by translating the relative position coordinates so that the first reference point is the origin, and the position coordinates obtained by translating the absolute position coordinates so that the second reference point is the origin.

5. The position detection device according to claim 3 or 4, wherein the coordinate transformation unit calculates and updates the first reference point in any time interval up to the latest time using the first recurrence relation from the relative position coordinates newly output by the relative position detection unit and the previously updated first reference point, and calculates and updates the second reference point in any time interval up to the latest time using the second recurrence relation from the absolute position coordinates newly output by the absolute position detection unit and the previously updated second reference point.

6. The position detection device according to claim 3 or 4, wherein the relative position coordinates and the absolute position coordinates are assigned a common weight for each detection time.

7. The position detection device according to claim 6, wherein the recurrence relation includes a fourth recurrence relation relating to the updating of the weights.

8. The position detection device according to claim 6, wherein the weight is set based on the standard deviation of the output values ​​of the relative position detection unit and the absolute position detection unit, respectively.

9. The position detection device according to claim 6, wherein the weight is set based on the newness of the output values ​​of the relative position detection unit and the absolute position detection unit, respectively.

10. The position detection device according to claim 5, wherein the coordinate transformation unit does not update the first reference point and the second reference point, nor the fitting, if the output values ​​of the relative position detection unit and / or the absolute position detection unit have a standard deviation exceeding an arbitrary threshold.

11. The position detection device according to claim 5, wherein when the coordinate transformation unit performs a coordinate transformation so that the first movement trajectory and the second movement trajectory fit together, if the difference between the latest positions of the first movement trajectory and the second movement trajectory exceeds an arbitrary threshold, the first reference point and the second reference point are not updated, and the fitting is not updated.

12. The relative position detection unit includes an optical distance measuring device, The position detection device according to claim 1, wherein the absolute position detection unit includes a receiving device for receiving positioning information via wireless communication.

13. The first and second coordinate systems are each three-dimensional coordinate systems. The position detection device according to claim 1, wherein the coordinate transformation unit identifies a three-dimensional translation vector and a rotation matrix and performs a coordinate transformation.

14. The system further includes a position display unit that displays the detected position based on the position coordinates output from the coordinate transformation unit. The coordinate transformation unit outputs the position coordinates of the second coordinate system to the position display unit. The position detection device according to claim 1, wherein the position display unit converts the position coordinates of the second coordinate system input from the coordinate transformation unit into position coordinates of the world coordinate system represented by latitude and longitude on Earth, and displays them as the detected position.

15. The system further includes a position display unit that displays the detected position based on the position coordinates output from the coordinate transformation unit. The position detection device according to claim 1, wherein the coordinate transformation unit outputs the position coordinates output by the absolute position detection unit to the position display unit when it determines that the output accuracy of the relative position detection unit has decreased.

16. A detection method performed by a computer, A position detection method that detects a position by using in combination the relative position coordinates of a first coordinate system output by a relative position detection unit and the absolute position coordinates of a second coordinate system output by an absolute position detection unit, A position detection method that, when transforming the relative position coordinates to the position coordinates of the second coordinate system, or transforming the absolute position coordinates to the position coordinates of the first coordinate system, and outputting the position coordinates after the coordinate transformation, obtains a coordinate transformation formula using a recurrence relation to fit the first movement trajectory based on the relative position coordinates and the second movement trajectory based on the absolute position coordinates over an arbitrary time interval up to the latest time.

17. A program that causes a computer to execute the position detection method described in claim 16.

Images