JP2007132853A - System and method for detecting position - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a system and a method which are excellent in reliability and can exactly detect a position of a mobile object while suppressing influences of errors to the minimum, even if the errors exist in measured angles. <P>SOLUTION: Angles between respective direction lines which connect three optical beacons to the mobile object 2, are measured, and two or more combinations each of which includes two angles among measured angles as measurement angles, are selected. Then, positions of the mobile object 2 are computed in selected combinations respectively by using the measurement angles and known positions of each optical beacon. Furthermore, percentage changes in above computed positions corresponding to changes in measurement angles are estimated in the selected combinations respectively. When carrying out above computation, the position of the mobile object is computed by using the combination of measurement angles which is selected from above-mentioned selected combinations such that the estimated percentage change is minimum. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

  The present invention relates to a position detection system and a position detection method for detecting the position of a moving body.

Conventionally, triangulation is known as a method for detecting the position of a moving object.
As an example of a system for detecting the position of a moving body by triangulation, at least three indicators are provided, and angles between the direction lines connecting these indicators and the moving body (three angles if there are three indicators). , And two of the measured angles are selected, and the position of the moving body is detected by calculation using the selected two angles and the position of each index (for example, Patent Document 1).
JP 2001-33244 A

  In the above system, the angle between each direction line connecting each index and the moving body is measured. If the measurement angle includes an error, the error adversely affects the position detection. .

  In consideration of the above circumstances, the present invention is a reliability that can accurately detect the position of a moving object while minimizing the influence of the error even if the measurement angle includes an error. It is an object to provide an excellent position detection system and position detection method.

  The position detection system according to the first aspect of the present invention is provided with three or more indicators for detecting the position of the moving body, and each of the direction lines connecting the three indexes and the moving body among the indicators. Measuring means for measuring the angle between them, computing means for calculating the position of the moving body from the known position of each index and the angle measured by the measuring means, and each of the measuring angles of the measuring means is 2 Selection means for selecting a plurality of combinations of one measurement angle, and estimation means for estimating, for each combination selected by the selection means, the rate of change of the position calculated by the calculation means with respect to the change of the measurement angle And. And the said calculating means calculates the position of a moving body using the measurement angle of the combination from which the change rate estimated by the said estimation means becomes the minimum among the measurement angles of each combination selected by the said selection means. .

  According to the position detection system and the position detection method of the present invention, even if an error is included in the measurement angle, it is possible to accurately detect the position of the moving body while minimizing the influence of the error. This greatly improves the reliability of position detection.

Hereinafter, an embodiment of the present invention will be described with reference to the drawings.
As shown in FIG. 1, reference numeral 1 denotes a building such as a large store, which is covered with a floor, a wall, and a ceiling. A moving body 2 is movably mounted on the floor of the building 1. Then, six optical beacons # 0 to # 6 are dispersed and attached as at least three or more indexes on the inner wall upper part or ceiling of the building 1. These optical beacons # 0 to # 6 use light emitting diodes that emit infrared light as light emitting elements, and their mounting positions (X and Y coordinates on the plane) are known at the time of installation.

  Infrared light emitted from the optical beacons # 0 to # 6 moves up and down within the range of 180 degrees in the horizontal direction on the plan view (90 or 270 degrees for those attached to corners) when mounted on the wall. It expands in the left-right direction, and when it is attached to the ceiling, it expands downward in a range of a maximum of 360 degrees on the plan view.

  In particular, the optical beacons # 0 to # 6 exist in the arrival area of light emitted from at least one optical beacon other than itself.

In these optical beacons # 0 to # 6, the corresponding light emission order is determined in advance in the order of the codes. The optical beacon # 0 having the first light emission order periodically emits light as a reference optical beacon. As shown in FIG. 2, the control circuit of the optical beacon # 0 includes a light emission pattern generation unit 10, a timer 11, an ID setting unit 12, and a light emitting element (light emitting diode) 13. Among these, the light emission pattern generation unit 10 and the timer 11 constitute a light emission control unit.
The timer 11 counts a predetermined time t1 for setting the periodic light emission operation of the optical beacon # 0. The ID setting unit 12 is for variably setting identification information so-called ID unique to the optical beacon # 0 by an artificial operation.

  As shown in the time chart of FIG. 3, the light emission pattern generation unit 10 modulates a carrier signal having a predetermined frequency every time the timer 11 counts a predetermined time t1, and uses the modulation signal (pulse signal) to generate a specific light emission. A start code having a pattern is generated, an ID code corresponding to the setting of the ID setting unit 12 is subsequently generated, and the light emitting element 13 is caused to emit light according to the generated start signal and ID code. A certain time ta is secured from the rise of the start signal to the end of the ID code. The ID code is a 3-bit binary value that fits within a predetermined time tb, and a binary value “0” is set for the optical beacon # 0. The fixed times ta and tb are set by counting the clock signal in the light emission pattern generation unit 10.

  The remaining optical beacons # 1 to # 6 are driven optical beacons that use light emitted from an optical beacon having a predetermined light emission order one of the light emitted from optical beacons other than itself. It discriminate | determines from the included ID code, and it operates in sequence by receiving the discriminated light and emits light.

The control circuits of the optical beacons # 1 to # 6 are the same as each other, and the control circuit of the optical beacon # 1 is representatively shown in FIG. That is, a light receiving element (photodiode) 20 that receives light emitted from an optical beacon other than itself, a signal determining unit 21 that determines a start signal and an ID code included in the light receiving signal of the light receiving element 20, and this signal determining unit When the start signal and the ID code are determined in 21, the comparison unit 22 that compares the determined ID code with the specific ID code in the ID memory 23 (the ID code of the optical beacon that is the previous light emission order) Is provided. The comparison result of the comparison unit 22 is supplied to the light emission pattern generation unit 24. A timer 25, an ID setting unit 26, and a light emitting element (light emitting diode) 27 are connected to the light emitting pattern generation unit 24.
The timer 25 is prepared for counting a certain time t2 until the light emission operation starts when the comparison result of the comparison unit 22 is coincident. The ID setting unit 26 is for variably setting an ID, which is identification information unique to the optical beacon # 1, by an artificial operation.

  As shown in FIG. 2, the light emission pattern generation unit 24 operates the timer 25 when the comparison result of the comparison unit 22 matches, and after the count of the predetermined time t2 by the timer 25 ends, A carrier signal having a frequency is modulated, a start signal having a specific light emission pattern is generated based on the modulated signal (pulse signal), an ID code corresponding to the setting of the ID setting unit 26 is generated, and the generated start signal is generated. The light emitting element 27 is caused to emit light according to the ID code. A certain time ta is secured from the rise of the start signal to the end of the ID code. The ID code is a 3-bit binary value that fits within a predetermined time tb, and a binary value “1” is set for the optical beacon # 1. The fixed times ta and tb are set by counting the clock signal in the light emission pattern generation unit 24.

  The signal determination unit 21, the comparison unit 22, the ID memory 23, the light emission pattern generation unit 24, and the timer 25 constitute a light emission control unit.

  As ID codes of the other optical beacons # 2 to # 6, binary values “2”, “3”, “4”, “5”, and “6” are set, respectively. FIG. 5 shows the relationship between the ID codes of the optical beacons # 0 to # 6 and the specific ID code (the ID code of the optical beacon one before the light emission order) stored in the ID memory 23 of the optical beacons # 0 to # 6. It shows.

  Thus, the optical beacon # 0 emits light every predetermined time t1 to generate the start signal and the ID code. The optical beacon # 1 monitors the ID code of the light received from the other optical beacons, and starts the light emission operation after a predetermined time t2 if the light emission order is the specific ID code of the optical beacon # 0 immediately before. Issue a signal and ID code. The optical beacon # 2 monitors the ID code of light received from other optical beacons, and starts emitting light after a predetermined time t2 if the light emission order is the specific ID code of the previous optical beacon # 1. Issue a signal and ID code. Thereafter, similarly, the optical beacons # 3 to # 6 emit light. The timing when the light emitting operations of all the optical beacons # 0 to # 6 are finished is before the timing when the counting of the predetermined time t1 is finished. As a result, the sequential light emission operations of the optical beacons # 0 to # 6 are repeated every certain time t1.

  On the other hand, the moving body 2 calculates the incident angle of light incident from the optical beacons # 0 to # 6 and determines the positions of the optical beacons # 0 to # 6 corresponding to the ID codes included in the respective incident lights. It has a function of discriminating and detecting its own position according to these calculation results and discrimination results. As shown in the control circuit of FIG. 6, as a detecting means, a light receiving unit 30, a light spot position measuring unit 33, an incident point An angle calculation unit 34, a self-position calculation unit 35, a code detection unit 36, and a position data memory 37 are provided.

  The light receiving unit 30 condenses the light that arrives from the optical beacons # 0 to # 6 and enters the incident unit 31 on the two-dimensional optical sensor 32. As shown in FIG. 7, the incident portion 31 has a diaphragm plate 31a. Light incident on the aperture (diaphragm) of the diaphragm plate 31a is condensed on the two-dimensional optical sensor 32 by the lens 31b, and two-dimensional optical A condensing point is formed on the upper surface of the sensor 32. The light spot position measurement unit 33 measures each condensing point in the two-dimensional optical sensor 32. The incident angle calculation unit 34 is directed to the incident unit 31 based on the distance between each condensing point measured by the light spot position measurement unit 33 and the central axis standing at the center position of the upper surface of the two-dimensional optical sensor 32. The incident angle of each incident light (the light source direction of each incident light) is calculated.

As the two-dimensional optical sensor 32, for example, a position sensor called PSD (Position Sensitive Detector) is employed. This position sensor detects the barycentric position of the light reception intensity at the condensing point by using the surface resistance of the photodiode, and outputs a signal of a voltage level corresponding to the barycentric position. That is, if the X and Y coordinates of the barycentric position of the light reception intensity at the condensing point are Xc and Yc, signals of voltage levels VXc and VYc are output from the position sensor. And it turns out that the optical beacon of the light emission origin exists in the direction of the angle obtained by the following formula using these gravity center positions Xc and Yc.
tan ^-1 (Yc / Xc) ± π
The code detection unit 36 detects the start code and ID code included in each incident light to the incident unit 31 based on the output of the two-dimensional optical sensor 32. The position data memory 37 stores the position data of the optical beacons # 0 to # 6 in association with the ID codes of the optical beacons # 0 to # 6.

The self-position calculator 35 has the following (1) to (4) as main functions.
(1) Three of the incident angles of each incident light, which is the calculation result of the incident angle calculation unit 34, are appropriately selected, and three optical beacons A and B as shown in FIG. 8 based on the selected three incident angles. , C (any three of the optical beacons # 0 to # 6) and the moving body 2 are recognized, and the three directional lines La, Lb, and Lc are recognized, and between the recognized directional lines La, Lb, and Lc. Measuring means for measuring angles (= θ ₁ , θ ₂ , θ ₃ ).

(2) Calculate the X coordinate Px and Y coordinate Py of the position of the moving body 2 from the positions of the optical beacons A, B, C and the angles (= θ ₁ , θ ₂ , θ ₃ ) measured by the measuring means. Computing means.

(3) A plurality of measurement angles α and β (= [θ ₁ , θ ₂ ] [θ ₂ , θ ₃ ] [θ ₁ , θ ₃ ]) among the measurement angles measured by the measurement means. Selection means for selecting a combination of

  (3) By referring to the position data memory 37 based on each ID code detected by the code detection unit 36, the position (XY coordinate) of each optical beacon that is a light emission source of each incident light to the incident unit 31 Discriminating means for discriminating.

  (4) Estimating means for estimating, for each combination selected by the selecting means, the rate of change Δx, Δy of the X coordinate Px and Y coordinate Py calculated by the calculating means with respect to changes in the measurement angles α, β. .

  The computing means (2) calculates the measurement angles α, β of the combinations that minimize the change rate Δx estimated by the estimation means among the measurement angles α, β of the combinations selected by the selection means. And the X coordinate Px of the position of the moving body 2 is calculated, and the Y coordinate of the position of the moving body 2 is calculated using the combination of the measurement angles α and β that minimize the change rate Δy estimated by the estimation means. Py is calculated.

  In addition, the moving body 2 has an accumulation memory 38 for accumulating and storing the positions detected by the self-position calculating unit 35. Based on the stored contents of the storage memory 38, the moving path of the moving body 2 can be analyzed.

  Further, the mobile body 2 includes a controller 40, a travel unit 41, a map data memory 42, and a travel route program memory 43 to enable use as, for example, a mobile robot capable of autonomous travel. The map data memory 42 stores map data of the moving space in the building 1. The travel route program memory 43 stores a travel route program for designating the travel route of the mobile body 2. The controller 40 controls driving of the autonomous traveling unit 41 according to the moving route program in the moving route program memory 43 and by comparing the moving body position specified by the self-position calculating unit 35 with the map data in the map data memory 42. To do.

Here, the function of the self-position calculator 35 will be described in detail.
Assume that the XY coordinates of the positions of the three optical beacons A, B, and C are A (x _a , y _a ), B (x _b , y _b ), and C (x _c , y _c ).

In order to obtain the position P of the moving body 2, any _two of the angles θ ₁ , θ ₂ , and θ ₃ between the direction lines La, Lb, and Lc need to be known. A plurality of combinations of (= [θ ₁ , θ ₂ ] [θ ₂ , θ ₃ ] [θ ₁ , θ ₃ ]) are selected.

A method of obtaining the X coordinate Px that is the X direction position and the Y coordinate Py that is the Y direction position of the moving body 2 using the combination of [θ ₁ , θ ₂ ] among these combinations will be described.

An XY coordinate of the center of the circle passing through the optical beacons A and B and having a circumferential angle θ ₁ is O ₁ (x ₁ , y ₁ ).
The XY coordinates of the center of the circle passing through the optical beacons A and C and having a circumferential angle of θ ₂ are O ₂ (x ₂ , y ₂ ).
An angle formed by the optical beacons A and B and the center XY coordinates O ₁ (x ₁ , y ₁ ) is represented by ∠BAO ₁ = φ.
An angle formed by the optical beacons A and C and the center XY coordinates O ₂ (x ₂ , y ₂ ) is set as ∠CAO ₂ = η.

The position of the moving body 2 is obtained as an intersection of a circle passing through the optical beacons A and B and having a circumferential angle of θ ₁ and a circle passing through the optical beacons A and C and having a circumferential angle of θ ₂ . Normally, these circles intersect at two points, but one intersection coincides with the position of the optical beacon A, so the remaining intersection P is determined as the position of the moving body 2.

Since the position of the optical beacon A is the base point, for convenience, the XY coordinates (xB, yB) of the position of the optical beacon B and the XY coordinates (xC, yC) of the position of the optical beacon C are
(XB, yB) = (xb−xa, yb−ya)
(XC, yC) = (xc−xa, yc−ya)
Then, the XY coordinates O ₁ and O ₂ of the center of the circle can be obtained by the following equations (1) and (2), respectively. Using this result, the X coordinate Px and the Y coordinate Py of the position of the moving body 2 can be obtained by the following expression (4) which is the first arithmetic expression.

Parameters for determining the X coordinate Px and the Y coordinate Py of the position of the moving body 2 are the measurement angles θ ₁ and θ ₂ . If the measurement angles θ ₁ and θ ₂ are changed, the X coordinate Px and the Y coordinate Py of the position of the moving body 2 are also changed.

The rate of change Δx of the X coordinate Px with respect to changes in the measurement angles θ ₁ and θ ₂ can be estimated by the following equation (6), which is the second arithmetic expression, by partial differentiation of the equation (4).

The change rate Δy of the Y coordinate Py with respect to changes in the measurement angles θ ₁ and θ ₂ can be estimated by the following equation (7), which is the second arithmetic expression, based on the partial differentiation of the equation (4).

On the other hand, the measurement angle theta _2, the case of detecting the position of the moving body 2 with a combination of theta _3, the measured angle theta _2, if there is a change in the theta _3, the position of the moving body 2 X coordinate Px and Y A change occurs in the coordinates Py.

The change rate Δx of the X coordinate Px with respect to changes in the measurement angles θ ₂ and θ ₃ can be estimated by the following equation (8), which is the second arithmetic expression, based on the partial differentiation of the equation (4).

The change rate Δy of the Y coordinate Py with respect to changes in the measurement angles θ ₂ and θ ₃ can be estimated by the following equation (9), which is the second arithmetic expression, by partial differentiation of the equation (4).

The measurement angle theta _3, even when detecting the position of the moving body 2 by using a combination of theta _1, its measured angle theta _3, if there is a change in the theta _1, the position of the moving body 2 X coordinate Px and Y A change occurs in the coordinates Py.

The change rate Δx of the X coordinate Px with respect to changes in the measurement angles θ ₃ and θ ₁ can be estimated by the following equation (10), which is the second arithmetic expression, based on the partial differentiation of the equation (4).

The rate of change Δy of the Y coordinate Py with respect to changes in the measurement angles θ ₃ and θ ₁ can be estimated by the following equation (11), which is the second arithmetic expression, by partial differentiation of the equation (4).

Assuming that the same measurement error is included in the measurement angles θ ₁ , θ ₂ , and θ ₃ , the estimated change rate Δx among the X coordinates Px calculated using a combination of two measurement angles. The X coordinate Px calculated using the combination of measurement angles that minimizes can be specified as the X coordinate Px with the smallest influence of measurement errors.

Similarly, if the same measurement error is included in the measurement angles θ ₁ , θ ₂ , and θ ₃ , the Y coordinate Py calculated using a combination of two measurement angles is estimated. The Y coordinate Py calculated using the combination of measurement angles that minimizes the change rate Δy can be specified as the Y coordinate Py having the smallest influence of the measurement error.

As specific conditions, as shown in FIG. 9, the X and Y coordinates (Ax, Ay), (Bx, By), and (Cx, Cy) of the three optical beacons A, B, and C are (0, 0), respectively. ), (0, 100), (86.6, 50), the X and Y coordinates (Px, Py) of the point P as the position of the moving body 2 are (49.43, 120.71), and the measurement angles θ ₁ , θ ₂ , When θ ₃ is θ ₁ = 90 °, θ ₂ = 125 °, and θ ₃ = 145 °, the above change rates are as follows.
First, the change rate Δx of the X coordinate Px with respect to the change of the measurement angles θ ₁ and θ ₂ is Δx = 0.81 from the above equation (6).

The change rate Δx of the X coordinate Px with respect to changes in the measurement angles θ ₂ and θ ₃ is Δx = 1.76 from the above equation (8).

The change rate Δx of the X coordinate Px with respect to changes in the measurement angles θ ₃ and θ ₁ is Δx = 0.95 from the above equation (10).
Among these, the combination of measurement angles that minimizes the change rate Δx is a combination of measurement angles θ ₁ and θ ₂ .

Subsequently, the change rate Δy of the X coordinate Py with respect to the change of the measurement angles θ ₁ and θ ₂ is Δy = 0.64 from the above equation (7).

The change rate Δy of the X coordinate Py with respect to changes in the measurement angles θ ₂ and θ ₃ is Δy = 0.16 from the above equation (9).

The change rate Δy of the X coordinate Py with respect to changes in the measurement angles θ ₃ and θ ₁ is Δy = 0.80 from the above equation (11).
Among these, the combination of the measurement angles that minimizes the change rate Δy is a combination of the measurement angles θ ₂ and θ ₃ .

Therefore, the combination of the measurement angles θ ₁ and θ ₂ is selected to detect the X coordinate Px of the P point of the moving body 2, and the combination of the measurement angles θ ₂ and θ ₃ is selected to detect the Y coordinate Py of the P point. By selecting, even if an error is included in the measurement angle, it is possible to accurately detect the position of the moving body 2 while minimizing the influence of the error. This greatly improves the reliability of position detection.

Assuming that the measurement angle includes an error of + 1 °, the following detection results are obtained as the X coordinate Px and the Y coordinate Py of the position P of the moving body 2.
When the combination of the measurement angles θ ₁ and θ ₂ is used for the calculation, Px = 48.97 (−0.81) and Py = 54.07 (−0.64). The numbers in parentheses are errors.

When the combination of the measurement angles θ ₂ and θ ₃ is used for the calculation, Px = 51.57 (1.79) and Py = 54.55 (−0.16).

When the combination of the measurement angles θ ₃ and θ ₁ is used for the calculation, Px = 48.83 (−0.95) and Py = 55.53 (0.82).

When the combination of the measurement angles θ ₁ and θ ₂ is selected for the X coordinate Px, and the combination of the measurement angles θ ₂ and θ ₃ is selected for the Y coordinate Py, the detection position error is minimized.

As shown in FIG. 10, the X and Y coordinates (Ax, Ay), (Bx, By), and (Cx, Cy) of the three optical beacons A, B, and C are (0, 0), (0 , 100), (86.6, 50), the X and Y coordinates (Px, Py) of the point P which is the position of the moving body 2 are (49.43, 120.71), and the measurement angles θ ₁ , θ ₂ , θ ₃ are θ _{When 1} = 45 °, θ ₂ = 50 °, and θ ₃ = 95 °, the above change rates are as follows.
First, the change rate Δx of the X coordinate Px with respect to the change of the measurement angles θ ₁ and θ ₂ is Δx = 1.67 from the above equation (6).

The change rate Δx of the X coordinate Px with respect to changes in the measurement angles θ ₂ and θ ₃ is Δx = 4.30 from the above equation (8).

The change rate Δx of the X coordinate Px with respect to changes in the measurement angles θ ₃ and θ ₁ is Δx = 2.63 from the above equation (10).
Among these, the combination of measurement angles that minimizes the change rate Δx is a combination of measurement angles θ ₁ and θ ₂ .

Subsequently, the change rate Δy of the X coordinate Py with respect to the change of the measurement angles θ ₁ and θ ₂ is Δy = 1.23 from the above equation (7).

The change rate Δy of the X coordinate Py with respect to changes in the measurement angles θ ₂ and θ ₃ is Δy = 0.04 from the above equation (9).

The change rate Δy of the X coordinate Py with respect to changes in the measurement angles θ ₃ and θ ₁ is Δy = 1.19 from the above equation (11).
Among these, the combination of the measurement angles that minimizes the change rate Δy is a combination of the measurement angles θ ₂ and θ ₃ .

Therefore, the combination of the measurement angles θ ₁ and θ ₂ is selected to detect the X coordinate Px of the P point of the moving body 2, and the combination of the measurement angles θ ₂ and θ ₃ is selected to detect the Y coordinate Py of the P point. By selecting, even if an error is included in the measurement angle, it is possible to accurately detect the position of the moving body 2 while minimizing the influence of the error. This greatly improves the reliability of position detection.

Assuming that the measurement angle includes an error of + 1 °, the following detection results are obtained as the X coordinate Px and the Y coordinate Py of the position P of the moving body 2.
When a combination of the measurement angles θ ₁ and θ ₂ is used for the calculation, Px = 47.67 (−1.76) and Py = 119.51 (−1.20). The numbers in parentheses are errors.

When the combination of the measurement angles θ ₂ and θ ₃ is used for the calculation, Px = 45.02 (−4.41) and Py = 120.54 (−0.17).

When the combination of the measurement angles θ ₃ and θ ₁ is used for the calculation, Px = 52.12 (2.69) and Py = 119.4 (−1.31).

When the combination of the measurement angles θ ₁ and θ ₂ is selected for the X coordinate Px, and the combination of the measurement angles θ ₂ and θ ₃ is selected for the Y coordinate Py, the detection position error is minimized.

Hereinafter, modified examples will be described.
There is a slight difference between the combination of measurement angles selected using the so-called partial differential equations of the above formulas (6) to (11) and the combination of measurement angles selected by obtaining an error from the position coordinates. Occurs. For example, the combination having the second smallest influence of the measurement error with respect to the Y coordinate Yp of the point P is a combination of the measurement angles θ ₃ and θ ₁ when selected from the differential equation, whereas the error is obtained from the position coordinates. The measurement angles θ ₁ and θ ₂ are combined and are different.

Therefore, in order to perform more accurate position detection, there is the following method.
Of the measurement angles θ ₁ , θ ₂ , θ ₃ , two measurement angles forming each combination are regarded as α and β, the actual value of the measurement angle α is Θ (deg), and the actual value of the measurement angle β is Ψ ( deg), when the measurement angles α and β include a measurement error k (deg; arbitrary constant), the change rates Δx ₁ and Δy ₁ are calculated as actual values of the estimated change rates Δx and Δy ( First calculation means).

Further, when the actual value of the measurement angle α is Θ (deg), the actual value of the measurement angle β is Ψ (deg), and the measurement angles α and β do not include the measurement error k (deg; any constant), As the actual values of the estimated change rates Δx and Δy, the change rates Δx ₂ and Δy ₂ are calculated (second calculation means).

Then, a difference (= | Δx ₁ | − | Δx ₂ |) between the calculated change rate Δx ₁ and the calculated change rate Δx ₂ among the respective coordinates Px calculated by the calculation means is obtained. . The X coordinate Px corresponding to the combination that minimizes this difference is specified as the X coordinate Px of the position of the moving body 2.

Further, the difference (= | Δy ₁ | − | Δy ₂ |) between the calculated change rate Δy ₁ and the calculated change rate Δy ₂ among the Y coordinates Py calculated by the calculation means. Ask. The Y coordinate Py corresponding to the combination that minimizes this difference is specified as the Y coordinate Py of the position of the moving body 2.

Specifically, for example, when k = 1 (deg), the following calculation result is obtained for each combination of measurement angles θ ₁ , θ ₂ , and θ ₃ .

  That is, when the measurement angle α = Θ (deg) and β = Ψ (deg) and the measurement error k (deg) is included in each measurement angle, the actual values and detected values of the coordinates Px and Py of the P point Has a property that the smaller k is, the closer it is to the following equation.

  Therefore, when considered in the example of FIG. 10, the rate of change of the coordinate Px and the rate of change of the coordinate Py of the point P of the moving body 2 can be calculated as follows.

When a combination of the measurement angles θ ₁ and θ ₂ is used, the change rate of the coordinate Px = −1.76 and the change rate of the coordinate Py = −1.20.

When the combination of the measurement angles θ ₂ and θ ₃ is selected, the change rate of the coordinate Px = −4.41 and the change rate of the coordinate Py = −0.18.

When the combination of the measurement angles θ ₃ and θ ₁ is selected, the change rate of the coordinate Px = −2.70 and the change rate of the coordinate Py = −1.31.

  In this case, that is, the combination of measurement angles selected using the partial differential equation is substantially the same as the combination of measurement angles selected by obtaining an error from the position coordinates.

  In the above embodiment, the case where the position of the moving body 2 is detected while the light emitted from the plurality of optical beacons # 0 to # 6 is captured on the moving body 2 side is described. The same can be applied to the case where the laser beam is emitted to the surroundings and the position of the moving body 2 is detected while the moving body 2 captures the laser beams reflected by the plurality of surrounding reflecting members.

  In addition, the present invention is not limited to the above-described embodiments as they are, and can be embodied by modifying the constituent elements without departing from the scope of the invention in the implementation stage. In addition, various inventions can be formed by appropriately combining a plurality of components disclosed in the embodiment. For example, some components may be deleted from all the components shown in the embodiment. Furthermore, you may combine the component covering different embodiment suitably.

The figure which shows the whole structure of one Embodiment. The block diagram of the control circuit of the optical beacon of the 1st rank of the light emission order of one Embodiment. The time chart which shows the light emission operation | movement of each optical beacon in one Embodiment. The block diagram of the control circuit of each remaining optical beacon of one Embodiment. The figure which shows the relationship between the ID code of each optical beacon in one Embodiment, and the specific ID code memorize | stored in each optical beacon. The block diagram of the control circuit of the moving body in one Embodiment. The figure which shows the structure of the light-receiving part in one Embodiment. The figure which shows the relationship between the position of three optical beacons and a mobile body in one Embodiment. The figure which shows the relationship between XY coordinate of three optical beacons and XY coordinate of the position of a moving body in one Embodiment. The figure which shows another example of the relationship between XY coordinate of three optical beacons in one embodiment, and XY coordinate of the position of a moving body.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Building, 2 ... Mobile body, # 0- # 6 ... Optical beacon, 10 ... Light emission pattern production | generation part, 11 ... Timer, 12 ... ID setting part, 13 ... Light emitting element, 20 ... Light receiving element, 21 ... Signal determination part , 22 ... comparison unit, 23 ... ID memory, 24 ... light emission pattern generation unit, 25 ... timer, 26 ... ID setting unit, 27 ... light emitting element, 30 ... light receiving unit, 32 ... two-dimensional optical sensor, 33 ... light spot position Measurement unit, 34 ... Incident angle calculation unit, 35 ... Self-position calculation unit, 36 ... Code detection unit,

Claims (5)

In a position detection system having three or more indices for detecting the position of a moving object,
Measuring means for measuring an angle between each of the direction lines connecting three of the indicators and the moving body;
Calculation means for calculating the position of the moving body from the known position of each index and the angle measured by the measurement means;
A selection means for selecting a plurality of combinations each consisting of two measurement angles among the measurement angles of the measurement means;
For each combination selected by the selection means, an estimation means for estimating a change rate of the position calculated by the calculation means with respect to a change in the measurement angle;
With
The computing means computes the position of the moving body using the measurement angle of the combination that minimizes the rate of change estimated by the estimation means among the measurement angles of each combination selected by the selection means. A position detection system characterized by The estimation means uses the value obtained by substituting the measurement angle of the combination selected by the selection means for the second calculation expression obtained by partial differentiation of the calculation expression used by the calculation means by the angle, and the change rate. A position detection system characterized by estimating. In a position detection system having three or more indices for detecting the position of a moving object,
Measuring means for measuring an angle between each of the direction lines connecting three of the indicators and the moving body;
Calculation means for calculating the X coordinate Px and the Y coordinate Py of the position of the moving body from the known position of each index and the angle measured by the measurement means;
A selection means for selecting a plurality of combinations each consisting of two measurement angles α and β among the measurement angles of the measurement means;
Estimating means for estimating, for each combination selected by the selecting means, the rate of change Δx, Δy of the X-coordinate Px and Y-coordinate Py calculated by the calculating means with respect to changes in the measurement angles α, β;
With
The calculation means uses the measurement angles α and β of the combination that minimizes the change rate Δx estimated by the estimation means among the measurement angles α and β of the combinations selected by the selection means, and moves the movement. Calculating the X coordinate Px of the position of the body, and calculating the Y coordinate Py of the position of the moving body using the measurement angles α and β of the combination that minimizes the rate of change Δy estimated by the estimation means. A position detection system characterized by In a position detection system having three or more indices for detecting the position of a moving object,
Measuring means for measuring an angle between each of the direction lines connecting three of the indicators and the moving body;
Calculation means for calculating the X coordinate Px and the Y coordinate Py of the position of the moving body from the position of each index and the angle measured by the measurement means;
A selection means for selecting a plurality of combinations each consisting of two measurement angles α and β among the measurement angles of the measurement means;
Estimating means for estimating, for each combination selected by the selecting means, the rate of change Δx, Δy of the X-coordinate Px and Y-coordinate Py calculated by the calculating means for each change in the measurement angles α, β;
The actual value when the actual value of the measurement angle α is Θ, the actual value of the measurement angle β is Ψ, and the measurement angles α and β include the measurement error k with respect to the change rates Δx and Δy estimated by the estimation means. First calculation means for calculating change rates Δx ₁ and Δy ₁ of
The actual value of the measurement angle α is Θ, the actual value of the measurement angle β is Ψ, and the measurement angles α and β do not include a measurement error with respect to the change rates Δx and Δy estimated by the estimation unit. A second calculating means for calculating actual change rates Δx ₂ and Δy ₂ ;
With
The calculation means includes an actual change rate Δx ₁ calculated by the first calculation means and a change rate calculated by the second calculation means among the measurement angles α and β of each combination selected by the selection means. The X coordinate Px of the position of the movable body is calculated using the measurement angles α and β of the combination that minimizes the difference from Δx ₂ (= | Δx ₁ | − | Δx ₂ |), and the first calculation is performed. The measurement angles α and β of the combinations that minimize the difference (= | Δy ₁ | − | Δy ₂ |) between the change rate Δy ₁ calculated by the means and the change rate Δy ₂ calculated by the second calculation means. And a Y coordinate Py of the position of the moving body is calculated. In a position detection system having three or more indices for detecting the position of a moving object,
Measuring the angle between the direction lines connecting three of the indicators and the moving body;
Calculating the position of the moving body from the known position of each index and the measured angle;
Selecting a plurality of combinations each consisting of two measurement angles among the measurement angles of the measurement means;
Estimating the rate of change of the calculated position with respect to the change of the measurement angle for each selected combination;
With
The calculating step calculates the position of the moving body using the measurement angle of the combination that minimizes the estimated rate of change among the measurement angles of the selected combinations. Detection method.

Images