JP6754518B2 - Position estimation system - Google Patents

Description

The present invention relates to a position estimation system.
The present application claims priority based on Japanese Patent Application No. 2014-248336 filed in Japan on December 8, 2014, the contents of which are incorporated herein by reference.

In order to estimate the position of the target object, a position estimation device including a motion sensor, GPS (Global Positioning System) and the like is used. This position estimation device is used for controlling a moving body such as a UAV (Unmanned Aerial Vehicle), for example.

Tsuyoshi Abe, Akira Mizushima, Noboru Noguchi, Research on Straight Tracking Control of Agricultural Vehicles Using Laser Scanner, Journal of Agricultural Machinery Society, 2005, 67 (3), 65-71 Masayuki Kamihara, Naokazu Yokoya, Prototype of Position and Posture Sensor Using Multiple Infrared Receivers Proceedings of IEICE General Conference 2004 Shunsuke Hijikata, Kazunobu Umeda, Position / Posture Estimating Method for Mobile Robots Using Infrared LEDs in the Room, 11th Robotics Symposia Proceedings, 2006.3 pp. 141-146 Iwakura et al., Indoor self-position estimation of a flying robot equipped with an infrared distance sensor (OS3 Autonomous Intelligent Unmanned Vehicle Motion and Control, "Vibration and Motion Control" Symposium Proceedings 2011 (12), 2011-06-28, 253 -258 Hitoshi Fujinaga, Hiroshi Tokutake, Shigeru Sunada Guidance control and autonomous flight test of small unmanned aerial vehicles, Proceedings of the Japan Aerospace Society, 2008, Vol. 56, No. 649, pp. 57-64 Hiroyuki Adachi, Eijun Nakayama, Makoto Sugo, Junichi Mizusawa, Report on 3D real-time measurement experiment of large joints using KINEC, IEICE Technical Report: IEICE Technical Report 2014-07-26, 114 (153), 25-29

The estimation accuracy of the conventional position estimation device may not be sufficient.
The present invention has been made in consideration of the above circumstances, and an object of the present invention is to provide a position estimation system capable of estimating a position with higher accuracy.

The present invention includes a light emitting unit that emits light, a first light receiving element that points in a first direction, a second light receiving element that points in a second direction different from the first direction, and the first light receiving element. The first light receiving element, the second light receiving element, and the third light receiving element include a third light receiving element that points in a direction different from that of the second direction and a third light receiving element that is different from the second direction. A position for estimating the position of the light emitting unit with respect to the light receiving unit using the light receiving unit provided so as not to be separated from each other and the light receiving results of the first light receiving element, the second light receiving element, and the third light receiving element. The position estimation unit includes an estimation unit, and the position estimation unit sets a geometric region for each of the first light receiving element, the second light receiving element, and the third light receiving element, and sets the geometric region with the set geometric region. , The geometric constraint condition set for the set geometric region, and the degree of attenuation of the light emitted by the light emitting unit obtained based on the information indicating the intensity of the light as the light receiving result. The position of the light emitting portion with respect to the light receiving portion is estimated based on the above, and the geometric region has a smaller radius centered on the direction direction of the light receiving element as the distance from the light receiving element increases. This is a position estimation system in which the rate of change of the radius is a three-dimensional curved surface that becomes smaller as the distance increases, and is a region in which the light emitting portion may exist.

The present invention is to provide a position estimation system capable of estimating a position with higher accuracy.

It is a conceptual diagram of the position estimation system 1 of 1st Embodiment. It is a figure which shows the functional structure of the position estimation system 1. It is a flowchart which shows the flow of processing executed by the position estimation apparatus 50. It is a conceptual diagram of a geometric area. It is a figure which shows an example of the position estimation map 60. It is a block diagram of the UAV group (flying body system) equipped with the position estimation system 1A. It is a figure which shows the functional structure of the target flying object 100 of the position estimation system 1A of the 2nd Embodiment. It is a conceptual diagram of the geometric area of the 3rd Embodiment. It is a conceptual diagram of the geometric area of 4th Embodiment. It is a schematic diagram of the method when a position estimation system estimates a position. It is a conceptual diagram of an output signal and signal propagation. It is a circuit diagram of the self-position transmitter. It is a figure which shows the result of having measured the reception time of an infrared signal by the distance change between a transmitter / receiver for each set of transmission signals using a microcomputer. It is a circuit diagram of a receiver. It is a figure which shows the result of experiment 1-2. It is a figure which shows the correlation of the distance between a transmitter / receiver, and the received light count number. It is a figure which shows the difference between the estimated distance and the true value by the difference in the number of samplings. It is a figure which shows the result of having measured the reception time by an angle change. It is a figure which shows the graph of the correlation of the number of counts and an angle at each distance. The conceptual diagram of the sensor arrangement is shown. The figure which shows the relationship between the count count and the distance / angle.

Hereinafter, embodiments of the position estimation system of the present invention will be described with reference to the drawings.

<First Embodiment>
FIG. 1 is a conceptual diagram of the position estimation system 1 of the first embodiment. The position estimation system 1 may include, but is not limited to, a light emitting device 10 and an air vehicle 20 equipped with the position estimation device 50. The position estimation device 50 receives the light emitted from the light emitting device 10 by a plurality of light receiving elements that direct directions independently of each other, and emits light based on information indicating the intensity of the light received by the plurality of light receiving elements. The position of the device 10 is derived.

FIG. 2 is a diagram showing a functional configuration of the position estimation system 1. The light emitting device 10 is equipped with light emitting units 12-1 to 12-n and a light emitting control unit 14. Hereinafter, when the light emitting units 12-1 to 12-n are not distinguished, they are simply referred to as the light emitting unit 12.

The light emitting unit 12 is provided as, for example, a fixed station or a mobile station. The light emitting unit 12 emits light having low directivity. The light emitting units 12-1 to 12-n emit light in different directions. The light emitting unit 12 is, for example, infrared light whose amplitude is modulated with time.

For example, the light emitting unit 12 has a cover unit that covers the light source of the light emitting unit 12. The cover unit scatters the light emitted from the light source of the light emitting unit 12 to reduce the directivity of the emitted light. In addition, instead of the amplitude modulation, other modulation methods such as frequency modulation and pulse modulation may be used. The light emitting control unit 14 refers to the information indicating the light emitting pattern of the light emitting unit 12 stored in the storage area of the own device, and causes the light emitting unit 12 to emit a predetermined light.

The aircraft body 20 includes a storage battery 22, an antenna 24, a communication control unit 26, a sensor 28, rotors 30-1 to 30-4, a rotor drive unit 32, a flight control unit 34, and a position estimation device 50. May include, but is not limited to.

The storage battery 22 is, for example, a lithium polymer battery or a lithium ion battery. The storage battery 22 is detachably mounted on the flying object 20. The storage battery 22 supplies electric power to each part of the flying object 20.

The antenna 24 receives radio waves transmitted from a transmission unit of a controller (not shown). A controller is a device for a user to remotely control an air vehicle. The radio wave transmitted from the transmitter of the controller includes the amount of operation input to the controller. The communication control unit 26 controls wireless communication between the controller and the flying object 20.

The sensor 28 is, for example, an altitude sensor, a distance measuring sensor, a gyro sensor, or the like. The altitude sensor projects a laser vertically below the body of the flying object 20 and receives the reflected light of the projected laser. The altitude sensor calculates the altitude of the flying object 20 based on, for example, the phase delay of the received light. The distance measuring sensor projects a laser in the horizontal direction of the airframe 20 and receives the reflected light of the projected laser. The ranging sensor detects an object existing around the flying object 20 based on, for example, a phase delay of the received light. The gyro sensor detects the attitude of the airframe 20.

Rotors 30-1 to 30-4 are rotating bodies (rotor blades). Hereinafter, rotors 30-1 to 30-4 are simply referred to as rotors 30 when they are not distinguished. The rotor drive unit 32 drives each rotor 30. In the present embodiment, the rotors 30-1 to 30-4 will be provided as an example, but the present embodiment is not limited to this. For example, there may be one rotor.

The flight control unit 34 controls the rotor drive unit 32 by integrating the processing result of the position estimation device 50, the altitude calculated by the sensor 28, the detected surrounding condition of the airframe, the attitude of the airframe, and the like. The flight control unit 34 controls, for example, the rotor drive unit 32 to increase or decrease the rotation speed of each rotor 30. As a result, the flight control unit 34 raises, lowers, changes direction, moves forward, and the like. The flight control unit 34 controls the aircraft based on, for example, a preset flight route.

The position estimation device 50 includes a storage unit 52, a light receiving unit 56 (light receiving elements 57-1 to 57-3), and a position estimation unit 58. The storage unit 52 is realized by, for example, a storage device such as a RAM (Random Access Memory), a ROM (Read Only Memory), an HDD (Hard Disk Drive), or a flash memory. The position estimation unit 58 is realized by executing a program by a processor such as a CPU (Central Processing Unit). Further, these position estimation units 58 may be realized by hardware such as LSI (Large Scale Integration), ASIC (Application Specific Integrated Circuit), and FPGA (Field-Programmable Gate Array).

The storage unit 52 has a position where the light receiving elements 57-1 to 57-3 are attached, the direction of the central axis of the light receiving direction in which the light receiving elements 57-1 to 57-3 receive light, and the light emitted by the light emitting unit 12. Information (amplitude, etc.) about is stored. Further, the storage unit 52 stores a preset flight route and flight plan.

The light receiving elements 57-1 to 57-3 are, for example, photodiodes. The light receiving elements 57-1 to 57-3 are arranged on the flying object 20 so that the light receiving surfaces face each other in different directions. The light receiving elements 57-1 to 57-3 are arranged, for example, on the lower side surface (the surface facing the ground) of the airframe. Further, the light receiving elements 57-1 to 57-3 are collected and provided at, for example, substantially one place. The light receiving elements 57-1 to 57-3 may be provided at different locations apart from each other. Hereinafter, the light receiving elements 57-1 to 57-3 of the light receiving unit 56 are simply referred to as light receiving elements 57 when they are not distinguished.

The position estimation unit 58 sets (limits) the geometric region in which the light emitting unit 12 may exist based on the information indicating the intensity of the light contained in the light received by each light receiving element 57. The position of the light emitting unit 12 is estimated based on the intersection of the geometric regions. The details of the processing of the position estimation unit 58 will be described later.

FIG. 3 is a flowchart showing a flow of processing executed by the position estimation device 50. This process is a process that is repeatedly executed. First, the position estimation unit 58 acquires information (light receiving result) indicating the intensity of the light contained in the light received by each receiving element 57 of the light receiving unit 56 (step S100).

Next, the position estimation unit 58 sets the geometric region based on the light receiving result acquired in step S100 (step S102). Next, the position estimation unit 58 derives the intersection position of the geometric region set in step S102 (step S104). The intersecting position of the geometric regions is the position of the light emitting unit 12.

FIG. 4 is a conceptual diagram of a geometric area. Geometric regions include curved surfaces, planes, spatial regions, curves, straight lines, and the like. In the figure, the X-axis coincides with the directing direction of the light receiving element 57. The directing direction is the central axis direction of the portion of the light receiving element 57 having high light receiving sensitivity. The position estimation unit 58 sets a geometric region based on the light receiving result of the light receiving element 57. The position estimation unit 58 derives a plurality of relationships (geometric regions) between the distance and the angle from the light receiving element 57 corresponding to the light receiving result (output A> B> C in the figure). The light receiving result is an output result of the light receiving element 57. The geometric region of this three-dimensional curved surface has a smaller radius about the direction facing the light receiving element 57 as the distance increases, and the rate of change of the radius decreases as the distance increases. It is a curved surface that looks like a curved surface). This is because the intensity of light becomes weaker as the distance between the light receiving element 57 and the light emitting unit 12 increases, and becomes weaker as the incident angle between the light receiving element 57 and the light emitting unit 12 increases from the facing direction. .. Reflecting this tendency, the geometric region is a set of positions where the light emitting unit 12 exists.

Further, the intersection position of the geometric regions is the intersection of the three geometric regions set by the position estimation unit 58 corresponding to each of the three light receiving elements 57. The position estimation unit 58 derives the position of the light emitting unit 12 based on the intersecting position of the derived geometric regions and the installation conditions of the light receiving element 57. Details of the position derivation method will be described in "Examples" described later.

Next, the position estimation unit 58 estimates the position of the own machine with respect to the position of the light emitting unit 12 (step S106). Next, the flight control unit 34 controls the own aircraft based on the position of the own aircraft estimated by the position estimation unit 58 and the preset flight route (step S108). As a result, the processing of one routine of this flowchart is completed, and the process returns to the processing of step S100.

For example, when the light emitting unit 12 is provided near the landing position of the aircraft body 20, the aircraft body 20 controls its own aircraft based on the estimation result of the position estimation unit 58, so that the aircraft body 20 can land more accurately. The target position can be specified. As a result, the aircraft 20 can land its own aircraft at the landing target position with higher accuracy.

In addition, instead of the processing of step S102 and step S104 of this flowchart, the position estimation unit 58 may derive the position of the light emitting unit 12 with respect to the own machine based on the position estimation map. In this case, the position estimation map is stored in the storage unit 52. FIG. 5 is a diagram showing an example of the position estimation map 60. The position estimation map 60 is a map in which the positions of the light emitting unit 12 with respect to the own machine are associated with the light receiving results of the three light receiving elements 57. The position estimation map 60 is created based on the principle of estimating the position based on the overlap of geometric regions. The position estimation map 60 is set based on the result obtained by, for example, an experiment or a simulation using the above-mentioned principle.

In the figure, α1 to α3 are output results corresponding to each of the light receiving elements 57-1 to 57-3. The position estimation unit 58 refers to the position estimation map 60 and derives the position (P1 or P2 in the figure) of the light emitting unit 12 with respect to the own machine based on the intersection of the light receiving results of the three light receiving elements 57. In the figure, positions P1 and P2 are defined by, for example, three-dimensional coordinates. Further, the position estimation unit 58 may derive the position of the light emitting unit 12 by using four arithmetic operations and approximation formulas (polynomial approximation, straight line approximation, polygonal line approximation).

The position estimation system 1 of the first embodiment described above intersects geometric regions based on the light receiving results of a plurality of light receiving elements 57 that receive the light emitted by the light emitting unit 12 that emits light and that are directed in directions independent of each other. By estimating the position as the position of the light emitting unit 12, the position of the light emitting unit 12 can be estimated with higher accuracy. As a result, the flying object 20 can control its own aircraft more accurately based on the position of the light emitting unit 12 derived by the position estimation unit 58.

(Second Embodiment)
Hereinafter, the second embodiment will be described. Here, the differences from the first embodiment will be mainly described, and the description of the functions and the like common to the first embodiment will be omitted. In the second embodiment, the light emitting unit 12 is provided on the target flying object 100 that the flying object 20 follows.

FIG. 6 is a configuration schematic diagram of a UAV group (aircraft system) equipped with the position estimation system 1A. Regarding the UAV group handled in the present embodiment, first, the target flying object (master unit) 100 and the flying object (slave unit) 20 are assigned roles. The base unit will be equipped with equipment that can recognize the absolute self-position by giving advance information. Since such equipment is expensive and heavy, it is not installed in the slave unit, but the master unit is made to follow. A light emitting device 10 is mounted on the master unit, and the light receiving element 57 mounted on the slave unit detects and follows the light emitted from the light emitting device 10 to realize colony control.

The position estimation system 1A of the second embodiment includes a flying object 20 and a target flying object 100. FIG. 7 is a diagram showing a functional configuration of the target aircraft 100 of the position estimation system 1A of the second embodiment.

The light emitting units 12-1 to 12-n included in the target aircraft 100 and the light emitting control unit 14 have the same functional configurations as the light emitting units 12-1 to 12-n included in the light emitting device 10 and the light emitting control unit 14. Therefore, the description is omitted. Further, the storage battery 22, the antenna 24, the communication control unit 26, the sensor 28, the rotors 30-1 to 30-4, the rotor drive unit 32, and the storage unit 52 included in the target aircraft 100 are the aircraft. Since the storage battery 22 included in the 20 has the same functional configuration as the storage unit 52, the description thereof will be omitted.

The flight control unit 34 of the target aircraft 100 controls the rotor 30 based on the detection result of the sensor 28 and the flight route stored in the storage unit 52. As a result, the target aircraft 100 flies toward the target point or according to the flight route. Further, the target aircraft 100 may be controlled based on the radio waves transmitted from the controller.

The operation of the flying object 20 of the second embodiment will be described. First, the light receiving element 57 of the flying object 20 receives the light emitted from the light emitting unit 12 of the target flying object 100. The position estimation unit 58 of the aircraft 20 refers to the position estimation map 60, and derives the position of the target aircraft 100 based on the light receiving result of the receiving element 57. The position estimation unit 58 estimates the position of the aircraft with respect to the target aircraft 100. The flight control unit 34 controls the own aircraft so as to follow the target aircraft 100 based on the position of the target aircraft 100 derived by the position estimation unit 58 and the position of the own aircraft.

The target aircraft 100 may be equipped with a position estimation device 50. In this case, the position estimation device 50 of the target flying object 100 derives the position of the light emitting unit 12 based on the light receiving result of the light emitted from the light emitting unit 12 of the light emitting device 10 provided on the ground or the like. The position estimation device 50 of the target aircraft 100 controls its own aircraft based on the position of the derived light emitting unit 12. Further, the flying object 20 following the target flying object 100 derives the position of the target flying object 100 based on the light receiving result of the light emitted from the light emitting unit 12 of the target flying object 100. The position estimation device 50 of the aircraft 20 controls its own aircraft based on the derived position of the target aircraft 100. As a result, the colony control of the flying object 20 and the target flying object 100 can be performed more accurately.

According to the second embodiment described above, the position estimation system 1A estimates the position of the target flying object 100 with higher accuracy based on the light receiving result of the light emitted by the light emitting unit 12 of the target flying object 100. can do. As a result, the aircraft 20 can follow the target aircraft 100 more accurately based on the positions of the target aircraft 100 and its own aircraft derived by the position estimation unit 58.

(Third Embodiment)
Hereinafter, the third embodiment will be described. Here, the differences from the first embodiment will be mainly described, and the description of the functions and the like common to the first embodiment will be omitted. In the first embodiment, the position of the light emitting unit 12 in the three-dimensional space is derived, but in the second embodiment, the position of the light emitting unit 12 in the two-dimensional space is derived. The position estimation device 50 can be applied to, for example, parking control in an automobile, a self-propelled vacuum cleaner, and the like.

The position estimation device 50 of the third embodiment derives the two-dimensional position of the light emitting unit 12 based on the intersection of the two-dimensional curves based on the light receiving results of the two light receiving elements 57. FIG. 8 is a conceptual diagram of the geometric region of the third embodiment. In the figure, the X-axis coincides with the directing direction of the light receiving element 57-1. In the figure, the Y-axis coincides with the directing direction of the light receiving element 57-2. The directing direction is the central axis direction of the portion of the light receiving element 57 having high light receiving sensitivity. The position estimation unit 58 sets the geometric region of the two-dimensional curve based on the light receiving result of the light receiving element 57. The position estimation unit 58 derives a plurality of relationships between the distance and the angle from the own machine corresponding to the light reception result (output A> B> C> D in the figure). The position estimation unit 58 derives the intersection of the two set geometric regions, and derives the derived intersection position as the position of the light emitting unit 12. For example, when the light receiving result of the light receiving element 57-1 is the output D and the light receiving result of the light receiving element 57-2 is the output B, the position estimation unit 58 has the light emitting unit 12 at the position where the output D and the output B intersect. Presumed to exist.

The position estimation device 50 of the third embodiment may be mounted on a self-propelled moving body. When applied to parking control in an automobile, a light emitting unit 12 is provided near a parking target position of the automobile. The automobile equipped with the position estimation device 50 estimates the position of the own vehicle based on the light receiving result of the light emitted from the light emitting unit 12. The car can park the car accurately based on the estimation result of the position of the car.

For example, the position estimation device 50 may be mounted on a self-propelled floor cleaning device that sucks dust and the like scattered on the floor surface. For example, a self-propelled floor cleaning device includes a plurality of traveling motors, a plurality of wheels, a dust suction unit, and a traveling control unit. The wheels are rotated by the power output by the traveling motor. As a result, the self-propelled floor cleaning device travels in a predetermined direction. The travel control unit controls the travel motor based on the position of the light emitting unit 12 derived by the position estimation device 50. As a result, the self-propelled floor cleaning device can control the self-propelled floor cleaning device more accurately based on the estimation result of the position of the position estimation device 50.

According to the third embodiment described above, the position estimation device 50 estimates the two-dimensional position of the light emitting unit 12 with higher accuracy based on the light receiving result of the light emitted by the light emitting unit 12. be able to.

(Fourth Embodiment)
Hereinafter, the fourth embodiment will be described. Here, the differences from the first to third embodiments will be mainly described, and the description of the functions and the like common to the first to third embodiments will be omitted. In the first to third embodiments, the light receiving elements 57-1 to 57-3 are gathered and provided at substantially one place. On the other hand, in the fourth embodiment, the position estimation unit 58 is provided at different locations apart from each other, and the position estimation unit 58 is a light emitting unit based on information indicating the intensity of light received by the plurality of light receiving elements 57. Estimate 12 positions. In the present embodiment, the position estimation unit 58 estimates the position of the light emitting unit 12 on a plane.

FIG. 9 is a conceptual diagram of the geometric region of the fourth embodiment. In the fourth embodiment, the light receiving elements 57-1 and 57-2 are provided at different locations apart from each other. In the illustrated example, the directional direction (Y-axis direction) of the light receiving element 57-1 and the directional direction (Y-axis direction) of the light receiving element 57-2 coincide with each other. Further, the distance Lab between the light receiving element 57-1 and the light receiving element 57-2 is stored in advance in the storage unit 52. The position estimation unit 58 derives a geometric region corresponding to the light receiving result for each light receiving element 57. In the figure, outputs A (57-1) to C (57-1) indicate a geometric region based on the light receiving result of the light receiving element 57-1, that is, a region where the light emitting unit 12 may exist, and the output A The output E (57-2) from (57-2) shows a geometric region based on the light receiving result of the light receiving element 57-2.

The position estimation unit 58 derives the intersection of the two set curves (geometric regions). Then, the position estimation unit 58 derives the position of the light emitting unit 12 based on the intersection of the derived curves. For example, when the light receiving result of the light receiving element 57-1 is the output B (57-1) and the light receiving result of the light receiving element 57-2 is the output D (57-2), the position estimation unit 58 performs the output B (57-1). The light emitting unit 12 with respect to the light receiving element 57 exists based on the position where the geometric region corresponding to 57-1) and the geometric region corresponding to the output D (57-2) intersect and the distance Lab between the light receiving elements 57. Positions (eg, angle α and distance La, and angle β and distance Lb) can be estimated.

The position estimation unit 58 may derive the position of the light emitting unit 12 with respect to the own machine based on the position estimation map. The position estimation map 60 is a map in which the position of the light emitting unit 12 with respect to the own machine is associated with the light receiving result of the light receiving element 57. The position estimation map 60 is created based on the principle of estimating the position based on the overlap of geometric regions and the distance between the light receiving elements 57. Further, in the present embodiment, the position estimation unit 58 estimates the two-dimensional position of the light emitting unit 12 based on the light receiving results of the two light receiving elements 57, but is not limited to this. For example, the position estimation unit 58 may estimate the three-dimensional position of the light emitting unit 12 based on the light receiving results of the three light receiving elements 57, which are separated from each other. Further, the plurality of light receiving elements 57 may be provided at different positions apart from each other and may be directed in directions independent of each other, or may be directed in the same direction.

According to the fourth embodiment described above, the plurality of light receiving elements 57 are provided at different positions apart from each other, and the position estimation device 50 provides information indicating the intensity of light received by the plurality of light receiving elements 57. Based on the above, a geometric region in which the light emitting unit 12 may exist is set corresponding to each of the plurality of light receiving elements 57, and the intersection position of the set geometric regions is estimated as the position of the light emitting unit 12. As a result, the position of the light emitting unit 12 can be estimated with higher accuracy.

Further, the position estimation system 1 or 1A of the first to fourth embodiments may be applied to a three-dimensional display method for displaying a binocular parallax image on a liquid crystal display. When providing a three-dimensional image to a user (viewer), it is necessary to estimate the position of the user's head. Therefore, the light emitting unit 12 of the position estimation system 1 is arranged at an arbitrary position or the like on the user's head, and the light receiving unit 56 is arranged at an arbitrary position where the light emitted by the light emitting unit 12 can be received. As a result, the position estimation system 1 or 1A can estimate the position (viewpoint) of the user's head and display the binocular parallax image corresponding to the estimated head position on the liquid crystal display. The light receiving unit 56 may be arranged at an arbitrary position on the user's head, and the light emitting unit 12 may be arranged at an arbitrary position capable of delivering light to the light receiving unit 56. If the position of the user's head is uniform to some extent, the position estimation system 1 or 1A may estimate the position of the user's head in two dimensions.

(Example)
The applicant conducted the following experiment and confirmed the self-position transmission by the amplitude-modulated light and the estimation of its three-dimensional position by the position estimation system.
In this experiment, two preliminary experiments are shown as preliminary experiments before the proposed method is implemented in an actual machine. The first preliminary experiment is a preliminary experiment for distance estimation using one light emitting element and one light receiving element. The second preliminary experiment is a preliminary experiment for three-dimensional position estimation using one light emitting element and three light receiving elements.

FIG. 10 is a schematic diagram of a method when the position estimation system estimates the position. The position estimation system is, for example, a system that combines a transmitter, a receiver, and peripheral circuits. The position estimation system modulates the steplessly changed brightness of infrared light using a carrier wave with a pulse of 38 [kHz], and estimates the distance from the state of attenuation for several milliseconds. It is possible to perform high-precision self-position transmission with little influence from the environment such as external light in a short time of several tens of milliseconds. In addition, in the three-dimensional position estimation, the accuracy should be improved by making a correspondence in advance with the distribution of the infrared light reception intensity of the transmitter side sensor and adjusting the judgment time when performing the position estimation. Can be done. In addition, although three light receiving elements are used in the three-dimensional position estimation experiment performed by the position estimation system this time, the estimation range and accuracy can be improved by increasing the number of light receiving elements.

Here, infrared rays having a central wavelength of 940 [nm] are used as the self-position transmitting device, and the distance to the self-position transmitting source is estimated by detecting the light whose energy is attenuated in correlation with the distance with the IR sensor. Further, by arranging a large number of IR sensors in an arbitrary direction, the angle to the self-position source can be estimated, so that the three-dimensional position can be estimated.

FIG. 11 is a conceptual diagram of an output signal and signal propagation. The self-position transmitting device modulates the AM-modulated monotonically decreasing signal using a carrier wave of 38 [kHz], and emits the signal shown in (FIGS. 10 and 11) by infrared rays. By using the carrier wave, the output can be increased, so that the distance between the aircraft can be increased. In addition, a frequency filter is used to reduce the influence of disturbance.

The receiving device uses an IR sensor (digital output infrared remote control light receiving module: PL-IRM1261-C438) equipped with a filter circuit matched to the carrier wave. As an infrared signal propagates through space, its energy is attenuated in space. Therefore, as shown in FIG. 11, the time for the receiver to receive the signal changes according to the distance.

The distance can be estimated using the correlation between this time and the distance between the transmitter and the receiver. Furthermore, by arranging a large number of sensors in any direction and processing the distance and angle of the three-dimensional space and the reaction time of each sensor in an integrated manner and analyzing the correlation, it is possible to estimate the position in the three-dimensional space. become.

(Preliminary experiment for distance estimation)
Regarding the preliminary experiment using one light emitting element and one light receiving element, the performance was improved in four stages. Each process will be described below. FIG. 12 is a circuit diagram of the self-position transmitting device. Turn on sig1 for a certain period of time to charge the capacitor with the voltage of Vcc. By switching sig2 at 30 [kHz] 200 times, the infrared signal shown in FIG. 11 is output using the energy of the capacitor. Arbitrary distance resolution and measurement cycle (charging time + light emission time) can be obtained by changing the duty ratio of Vcc, about 30 [kHz], capacitor capacity, charging time, LED current limiting resistance, and discharging time (number of pulses). Can be done. In this experiment, the experiment was performed with Vcc5 [V], duty1 / 3, capacitor 4.7 [μF], charging time 500 [μs], resistance 330 [Ω], and 200 pulses.

(Experiment 1-1)
FIG. 13 is a diagram showing the results of measuring the reception time of an infrared signal due to a change in the distance between transmitters and receivers for each set of transmission signals using a microcomputer (AVR: 16 [MHz]). As shown in FIG. 13, it can be seen that the infrared reception time increases as the distance between the transmitters and receivers decreases. On the other hand, as the distance became smaller, the sensor sensitivity became very poor, and it took about 1 second for the output waveform to stabilize, resulting in poor responsiveness. In addition, although infrared rays are output from the transmitter side for about 7 milliseconds, the result is obtained that the receiving side can receive only about 1 millisecond even at a distance of 250 [mm]. It is considered that the cause of this is that if the signal is continuously received, the electric charge is saturated in the internal circuit and the sensor does not operate normally. In order to solve this problem, the circuit shown in FIG. 14 is used for each set of received signals. The charge inside the sensor was reset by grounding the power supply of the receiver (sensor) to the ground.

(Experiment 1-2)
In order to solve the problem of Experiment 1-1, the charge inside the sensor was reset by grounding the power supply of the receiver (sensor) to the ground for each set of received signals using the circuit shown in FIG. FIG. 14 is a circuit diagram of the receiver. Immediately after measuring the light receiving time of the infrared signal, turn off the power for 300 microseconds. Power again and wait 100 microseconds for stabilization. Measure the light receiving time of the infrared signal again. The measurement was performed according to the above flow. The results of Experiment 1-2 are shown in FIG.

With the circuit shown in FIG. 14, since each measurement can be performed in the initial state, the responsiveness is very good and the delay is about 200 microseconds. However, from FIG. 15, the result that the distance between the transmitters and receivers is between 1500 [mm] and 500 [mm] and the reception time increases as the distance decreases cannot be obtained. It is considered that the cause is that the timing of supplying power to the sensor becomes unstable due to the influence of noise and the like.

(Experiment 1-3)
In order to solve the problem that occurred in Experiment 1-2, we created a program that counts the infrared signal reception time by the number of loops of the main program of the microcomputer. Since the number of counts varies due to the influence of the power supply on the transmitter side, the environment, etc., the average number of times 500 counts are measured 1000 times is shown in FIG. Since the time required for 1000 measurements is about 7 seconds, it was confirmed that 500 counts were performed in about 7.2 milliseconds for one set of transmission signals. The measurement result is shown in FIG.

FIG. 16 is a diagram showing the correlation between the distance between transmitters and receivers and the number of received light reception counts. In addition, by initializing at regular intervals using the circuit of FIG. 11, a delay of about 200 microseconds in terms of responsiveness, which is sufficient for mounting on a UAV and performing position estimation, could be obtained. Based on this data, the distances are divided into distances of 300 [mm] to 700 [mm] (formula (1)) and 700 [mm] to 2500 [mm] (formula (2)), and the distances (l) from the count (x) A fourth-order approximation formula was created to estimate. Approximate equations are shown in equations (1) and (2).

FIG. 17 is a diagram showing the difference between the estimated distance and the true value due to the difference in the number of samplings. The larger the number of samplings, the smaller the error. If the number of samplings exceeds 100 (about 0.7 seconds), the error converges within 20 [mm]. However, the error did not converge to 0 even if the number of samplings was increased by a certain amount or more due to the error due to the approximate expression and the influence of the environment.

(Experiment 1-4)
FIG. 18 is a diagram showing the results of measuring the reception time due to an angle change from 0 [deg] to 180 [deg] with the optical axis set to 90 [deg]. From FIG. 18, it was found that the sensitivity of the sensor does not change at ± 15 [deg] from the center, and that the sensor has left-right symmetry.

(Preliminary experiment for 3D position estimation)
Under the same conditions as the above-mentioned "preliminary experiment for distance estimation", the sensor elements are expanded to three, and a preliminary experiment for three-dimensional position estimation and an evaluation experiment are performed.

(Experiment 2-1)
FIG. 19 is a graph showing a graph of the correlation between the number of counts and the angle at each distance. The distance between the transmitters and receivers was changed at intervals of 50 [mm] for measurement. The curve of the graph shows a line of a distance of 300 [mm], a line of 350 [mm], and a line of 400 [mm] in order from the upper left. From FIG. 19, a plurality of sets of distance (r) and angle (θ) corresponding to the number of counts (n) received by the sensor can be estimated (see FIG. 4).

(Evaluation experiment for 3D position estimation)
Three were arranged on the three-dimensional space so that the Cartesian coordinate system xyz axis and the optical axis of the receiver coincide with each other. FIG. 20 shows a conceptual diagram of the sensor arrangement. The distance r from the center of the sensor to the transmitter was set to 350 [mm]. The position estimation method is shown below. The number of counts acquired from the three sensors is applied to the graph (approximate formula) of FIG. 19 to calculate the estimated angle for each distance. As an example, an approximate expression of the relationship between the number of counts and the angle when the distance is 300 [mm] is shown in the equation (3).

The estimated angle is obtained by substituting the values (sensor A = 289, B = 302, C = 293) obtained from the sensor into x in the approximate expression. Similarly, by substituting into the approximate expression for angle estimation in the case of other distances, the estimated angle in the case of distances of 300 [mm], 350 [mm], and 400 [mm] is obtained. The results are shown in FIG. FIG. 21 is a diagram showing the relationship between the number of counts and the distance / angle.

Assuming that the three sensors are at one point, the distance to the transmitter is the same for all sensors. Further, since the optical axes of the three sensors are orthogonal to each other, there is a geometric binding condition as shown in FIG. Equation (4) is shown using the characters used in FIG.

From equation (4), the sum of squares of the cosine of the angle formed by the optical axis of the transmitter and each of the three sensors is 1. Substituting the estimated angle at each distance from FIG. 21, the sum of squares of the cosine of the angle at 300 [mm] is 0.8396, 0.9574 at 350 [mm], and the numerical value 1.064 at 400 [mm], which is 350. Since the case of [mm] is the closest to 1, it is a solution. Further, it is an estimated angle formed by the optical axis of the transmitter and the optical axis of each sensor at which the angle at that time is estimated.

When the three-dimensional position was estimated by this algorithm, the three-dimensional position within ± 25 [mm] of the distance accuracy of the transmitting and receiving engine and ± 3 ° of the angle accuracy with respect to the optical axis of each sensor was obtained with a sampling time of about 0.2 seconds. It was confirmed that the estimated data could be acquired. Since the characteristic data of the sensor was sampled at intervals of 50 [mm], the maximum error was ± 25 [mm], but the accuracy can be improved by collecting the data in more detail.

In this way, the position estimation system arranges three light receiving elements orthogonally as a receiving unit, defines a three-dimensional orthogonal coordinate axis along the optical axis (the origin is the light receiving element), and transmits amplitude-modulated infrared light. Estimate the position (distance, angle) of the source. With the time when the three light receiving elements react as three inputs, the position of the source (one distance, two angles) is estimated according to the conditions related to the angle and the conditions related to the distance.

In addition, the position estimation system can be used for colony control of moving bodies including UAVs. For example, one multi-rotor UAV is equipped with a transmitter and peripheral circuits, and another multi-rotor UAV is equipped with a receiver and peripheral circuits. A formation flight can be realized by following the flight of the UAV equipped with the transmitter and the flight of the other UAV equipped with the receiver.

As described above, the effectiveness of amplitude-modulated infrared light was verified toward the realization of an inexpensive, compact and lightweight three-dimensional position estimation system that can be used indoors and outdoors. It can be said that the influence of disturbance is very small because the same performance could be obtained by conducting experiments under various light quantities. It can be expected to be applied to UAVs that are operated in various environments such as outdoors. We created a transmitter and a three-dimensional position estimation algorithm, and realized a system that acquires data that enables highly accurate position estimation in a short time. The position estimation method using the position estimation system enables stable and highly accurate position estimation, and can be used in combination with GPS to perform autonomous formation flight with a short distance between aircraft. Furthermore, it is possible to systematically collect information including the search for a rescuer even in a narrow place such as in a building where GPS cannot be used or in rubble. Since the initial information can be collected quickly in the event of a disaster, it can greatly contribute to the control of damage and the saving of lives. In addition, it is considered that it can be used in various aspects such as landing guidance during autonomous control and guidance of passing points.

Although the embodiments for carrying out the present invention have been described above using the embodiments or experimental results, the present invention is not limited to these embodiments or experimental results and does not deviate from the gist of the present invention. Various modifications and substitutions can be made in.

1,1A ... position estimation system, 10 ... light emitting device, 12 ... light emitting unit, 20 ... flying object, 56 ... light receiving unit, 57 ... light receiving element, 58 ... position estimation unit

Claims (6)

A light emitting part that emits light and
A first light receiving element that points in the first direction,
A second light receiving element that points in a second direction different from the first direction,
The first light receiving element, the second light receiving element, and the third light receiving element include the first direction and a third light receiving element that points in a third direction different from the second direction. A light receiving part provided so that the elements are not separated from each other,
A position estimation unit that estimates the position of the light emitting unit with respect to the light receiving unit using the light receiving result of the first light receiving element, the second light receiving element, and the third light receiving element is provided.
The position estimation unit
Geometric regions are set for each of the first light receiving element, the second light receiving element, and the third light receiving element.
The light emitting unit obtained based on the set geometric region, the geometric constraint condition set for the set geometric region, and the information indicating the light intensity as the light receiving result emitted light. Based on the degree of light attenuation, the position of the light emitting part with respect to the light receiving part is estimated.
In the geometric region, the radius centered on the direction of direction of the light receiving element decreases as the distance from the light receiving element increases, and the rate of change of the radius decreases as the distance increases. And is a region where the light emitting portion may exist.
Position estimation system. Wherein said first direction in which the first light receiving element is directed, and the second direction in which the second light receiving element is directed, and the third direction in which the third light receiving element is oriented to each other They are orthogonal to each other and are arranged so that the distance from the light emitting unit to the first light receiving element, the distance to the second light receiving element, and the distance to the third light receiving element can be regarded as the same. Yes,
The position estimation system according to claim 1. The light emitted by the light emitting unit is amplitude-modulated light.
The position estimation system according to claim 1 or 2. The light receiving unit and the position estimation unit are mounted on the flying object.
The light emitting unit is a fixed station.
The position estimation system according to any one of claims 1 to 3.
The light emitting unit is mounted on the first flying object and is mounted on the first flying object.
The light receiving element and the position estimation unit are mounted on the second flying object.
The second flying object follows the first flying object and flies.
The position estimation system according to any one of claims 1 to 3. The first flying object is
Further, a device that recognizes the self-position, a flight control unit that controls the flight of the own aircraft based on the self-position and a preset flight route, and the light emission when the own aircraft is in flight. Equipped with a light emission control unit that radiates light to the unit
The second flying object is
Further, a flight control unit that controls the aircraft so as to follow the first flying object based on the estimation unit of the position estimation unit, regardless of the recognition result of the equipment that recognizes the self-position.
To prepare
The position estimation system according to claim 5.

Images