WO2017042907A1 - 航法装置および測量システム - Google Patents

航法装置および測量システム Download PDF

Info

Publication number
WO2017042907A1
WO2017042907A1 PCT/JP2015/075598 JP2015075598W WO2017042907A1 WO 2017042907 A1 WO2017042907 A1 WO 2017042907A1 JP 2015075598 W JP2015075598 W JP 2015075598W WO 2017042907 A1 WO2017042907 A1 WO 2017042907A1
Authority
WO
WIPO (PCT)
Prior art keywords
data
image
coordinate
image data
angle
Prior art date
Application number
PCT/JP2015/075598
Other languages
English (en)
French (fr)
Inventor
秀明 前原
百代 長瀬
謙二 平
純雄 加藤
Original Assignee
三菱電機株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 三菱電機株式会社 filed Critical 三菱電機株式会社
Priority to PCT/JP2015/075598 priority Critical patent/WO2017042907A1/ja
Priority to US15/561,882 priority patent/US10222210B2/en
Priority to JP2016513572A priority patent/JP6029794B1/ja
Priority to EP15903572.4A priority patent/EP3348963B1/en
Publication of WO2017042907A1 publication Critical patent/WO2017042907A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/04—Interpretation of pictures
    • G01C11/06—Interpretation of pictures by comparison of two or more pictures of the same area
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64D—EQUIPMENT FOR FITTING IN OR TO AIRCRAFT; FLIGHT SUITS; PARACHUTES; ARRANGEMENT OR MOUNTING OF POWER PLANTS OR PROPULSION TRANSMISSIONS IN AIRCRAFT
    • B64D47/00—Equipment not otherwise provided for
    • B64D47/08—Arrangements of cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G01C1/02—Theodolites
    • G01C1/04—Theodolites combined with cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/593—Depth or shape recovery from multiple images from stereo images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04N—PICTORIAL COMMUNICATION, e.g. TELEVISION
    • H04N7/00—Television systems
    • H04N7/18—Closed-circuit television [CCTV] systems, i.e. systems in which the video signal is not broadcast
    • H04N7/183—Closed-circuit television [CCTV] systems, i.e. systems in which the video signal is not broadcast for receiving images from a single remote source
    • H04N7/185—Closed-circuit television [CCTV] systems, i.e. systems in which the video signal is not broadcast for receiving images from a single remote source from a mobile camera, e.g. for remote control
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Definitions

  • the present invention relates to a navigation device for estimating the attitude of a moving body equipped with a surveying camera and a laser ranging device, and a surveying system equipped with the navigation device.
  • Patent Document 1 describes a surveying system that performs photogrammetry and aviation laser surveying using a camera mounted on a flying object and a laser transmitter / receiver.
  • the camera that captures the survey object from the flying object is supported by an attitude stabilization device called a stabilizer, and can maintain the shooting direction vertically downward regardless of the attitude of the flying object during flight.
  • the laser transmission / reception apparatus irradiates the survey target with laser light from the flying object at a predetermined cycle, and receives the reflected light from the survey target.
  • the control device in this surveying system performs aviation laser surveying using the information of the reflected light received from the survey target by the laser transmission / reception device.
  • the laser transmission / reception device corresponds to the laser distance measuring device in the present invention.
  • the three-dimensional coordinate data of the flying object is detected by a GNSS (Global Navigation Satellite System) device mounted on the flying object.
  • the GNSS device receives GNSS information from the GNSS satellite at regular intervals and analyzes the GNSS information to obtain three-dimensional coordinate data of the flying object.
  • the period in which the laser beam is irradiated onto the survey target by the laser transmission / reception apparatus is shorter than the period in which the GNSS information is received by the GNSS apparatus. For this reason, even if the reflected light from the surveying object is received by the laser transmission / reception device, the control device cannot obtain the three-dimensional coordinate data of the flying object at a constant interval other than the period in which the GNSS information is received.
  • GNSS is used by using the information on triaxial acceleration and triaxial angular acceleration measured by an IMU (Internal Measurement Unit) mounted on the aircraft. The three-dimensional coordinate data of the aircraft was obtained at regular intervals other than the period in which the information is received.
  • the surveying system described in Patent Document 1 includes an accelerometer and an angular accelerometer that are cheaper and smaller than the IMU instead of the IMU.
  • the three-dimensional acceleration from the accelerometer and the three-axis angular acceleration information from the angular accelerometer are used to determine the three-dimensional coordinates of the flying object at a fixed interval other than the reception period of the GNSS information. I have data.
  • the information indicating the attitude of the flying object is an angle of the flying object in the rolling direction, pitching direction, and yawing direction (hereinafter referred to as roll angle, pitch angle, yaw angle), and two or more different positions depending on the camera.
  • the value obtained by the bundle calculation for the corresponding points between the images taken from is used.
  • the control device uses the acceleration from the accelerometer and the angular acceleration from the angular accelerometer based on the attitude of the flying object obtained by the bundle calculation, for the laser light scanning period (other than the GNSS information reception period). The attitude of the flying object at a certain interval) is calculated.
  • the attitude of the flying object is estimated by bundle calculation using image data captured from different positions by the camera, and only the image data is used for estimating the attitude. For this reason, there is a limit to the accuracy of posture estimation.
  • This invention solves the said subject, and it aims at obtaining the navigation apparatus and surveying system which can estimate the attitude
  • the navigation device includes a data acquisition unit, a coordinate calculation unit, an image matching unit, and an attitude estimation unit.
  • the data acquisition unit includes distance data indicating a distance from a laser light irradiation reference point to a distance measuring point measured by a laser distance measuring device mounted on the moving body, angle data indicating a laser light irradiation angle,
  • the coordinate data indicating the three-dimensional coordinates of the irradiation reference point of the laser beam measured by the mounted coordinate measuring device, and the image data including the distance measuring point in the photographing target photographed by the photographing device mounted on the moving body are acquired. .
  • the coordinate calculation unit calculates the coordinates of the distance measurement point on the image of the image data based on the distance data, the angle data, the coordinate data, and the parameter indicating the posture of the moving body acquired by the data acquisition unit.
  • the image matching unit performs image matching of a pair of image data shot at different shooting positions by the shooting device, and sets the distance measurement point coordinates on the image of one image data of the pair calculated by the coordinate calculation unit.
  • the corresponding point is searched from the image of the other image data of the pair.
  • the posture estimation unit moves so that the difference between the coordinates of the distance measuring point on the image of the other image data of the pair calculated by the coordinate calculation unit and the coordinates of the corresponding point searched by the image matching unit is small.
  • the posture of the moving body is estimated by correcting the parameter value indicating the posture of the body.
  • the posture of the moving body is estimated by correcting the parameter value, the posture of the moving body can be estimated without using an IMU or a stabilizer. Further, in addition to the image data obtained by photographing the distance measuring point, the moving object using the distance from the laser light irradiation reference point to the distance measuring point, the laser light irradiation angle, and the three-dimensional coordinates of the laser light irradiation reference point Therefore, it is possible to accurately estimate the posture of the moving body.
  • FIG. 2 is a block diagram showing a functional configuration of the navigation device according to Embodiment 1.
  • FIG. 3 is a block diagram showing a hardware configuration of the navigation device according to Embodiment 1.
  • FIG. 3A shows a hardware processing circuit for realizing the function of the navigation device
  • FIG. 3B shows a hardware configuration for executing software for realizing the function of the navigation device.
  • 4 is a flowchart showing an outline of the operation of the navigation device according to the first embodiment. It is a figure which shows typically the positional relationship of a left camera, a right camera, and a laser ranging device.
  • FIG. 5A is a perspective view of a unit including a left camera, a right camera, and a laser distance measuring device
  • FIG. 5B is a diagram of the unit as viewed in the X-axis direction
  • FIG. FIG. 5 is a diagram of the unit viewed in the Z-axis direction
  • FIG. 5D is a diagram of the unit viewed in the Y-axis direction. It is a figure which shows the position change of the left camera, the right camera, and laser rangefinder accompanying flight of an aircraft.
  • FIG. 6A shows position coordinate data of the laser distance measuring device.
  • FIG. 6B is a graph in which the position coordinates of the left camera, the right camera, and the laser distance measuring device are plotted on the XZ plane
  • FIG. 6C is a graph in which these position coordinates are plotted on the YZ plane.
  • FIG. 6D is a graph in which these position coordinates are plotted on the XY plane. It is a figure which shows the change of the measurement result of the laser ranging apparatus with the flight of an aircraft.
  • FIG. 7A shows angle data and distance data at each time
  • FIG. 7B is a graph in which the data shown in FIG. 7A is plotted. It is a figure which shows the image image
  • FIG. 3 is a flowchart showing the operation of the navigation device according to the first embodiment. It is a figure which shows the calculation result of the three-dimensional coordinate of a ranging point. It is a figure which shows the projection center coordinate of a left camera and a right camera. It is a figure which shows the coordinate of the ranging point on the image image
  • FIG. 1 is a block diagram showing a configuration of a surveying system 1 according to Embodiment 1 of the present invention.
  • the surveying system 1 is a system for surveying terrain from the aircraft 2, and includes a left camera 20 a, a right camera 20 b, a laser ranging device 21, a GNSS device 22, a memory card 23, and a navigation device 3 mounted on the aircraft 2.
  • the navigation device 3 is a device that estimates the attitude of the aircraft 2 in flight, and is provided separately from the aircraft 2 as shown in FIG. However, the navigation device 3 may be mounted on the aircraft 2. Further, the attitude of the aircraft 2 is specified by three parameters of the roll angle ⁇ , the pitch angle ⁇ , and the yaw angle ⁇ that are attitude angles in the rolling direction, the pitching direction, and the yawing direction of the aircraft 2.
  • the aircraft 2 embodies the moving body in the present invention, and can fly with the left camera 20a, the right camera 20b, the laser distance measuring device 21, the GNSS device 22, and the memory card 23 mounted thereon.
  • an aircraft operated by a pilot on board may be used, or a UAV (Unmanned Aero Vehicle) may be used.
  • UAV Unmanned Aero Vehicle
  • the left camera 20a and the right camera 20b are components embodying the first photographing unit and the second photographing unit in the present invention, and photograph the ground surface including the distance measuring point of the laser distance measuring device 21.
  • a device including the left camera 20a and the right camera 20b and a control device that controls these photographing processes corresponds to the photographing device in the present invention.
  • the control device instructs the left camera 20a and the right camera 20b to shoot the ground surface at a predetermined cycle, and stores image data in which the image obtained by shooting is associated with the shooting date / time in the memory card 23.
  • a predetermined period it is conceivable to perform photographing every second.
  • the laser distance measuring device 21 receives the reflected light from the distance measuring point on the ground surface by irradiating the ground surface to be surveyed while changing the irradiation angle ⁇ of the laser light, thereby receiving the laser light irradiation reference. The distance l from the point to the distance measuring point is measured. Each time the laser distance measuring device 21 measures the distance l, the memory card 23 stores distance data indicating the distance l and angle data indicating the irradiation angle ⁇ of the laser beam from which the distance l is obtained. To do.
  • the GNSS device 22 is a component that embodies the coordinate measuring device according to the present invention, and measures the three-dimensional coordinates of the laser beam irradiation reference point in the laser distance measuring device 21. Further, the GNSS device 22 stores coordinate data indicating the three-dimensional coordinates of the irradiation reference point in the memory card 23 at a predetermined cycle. For example, the coordinates are measured every second in synchronization with photographing by the left camera 20a and the right camera 20b. Note that the difference in position between the GNSS device 22 and the irradiation reference point is within an allowable range with respect to the measurement accuracy of the GNSS device 22. That is, the GNSS device 22 is assumed to be at the same position as the irradiation reference point, and the position of the irradiation reference point has the same meaning as the position of the aircraft 2.
  • the memory card 23 is a component that embodies the storage device in the present invention, and stores distance data, angle data, image data, and coordinate data measured during the flight of the aircraft 2.
  • an SD (Secure Digital) memory card may be used as the memory card 23, for example, an SD (Secure Digital) memory card may be used.
  • FIG. 2 is a block diagram showing a functional configuration of the navigation device 3.
  • the navigation device 3 includes a data acquisition unit 30, a coordinate calculation unit 31, an image matching unit 32, and a posture estimation unit 33.
  • the data acquisition unit 30 is a component that acquires distance data, angle data, coordinate data, and image data stored in the memory card 23 of the aircraft 2. For example, the data acquisition unit 30 reads and acquires the data by connecting to the card drive of the memory card 23 by wire or wireless.
  • the coordinate calculation unit 31 is based on the distance data, angle data, coordinate data acquired by the data acquisition unit 30 and the attitude angle (roll angle ⁇ , pitch angle ⁇ , yaw angle ⁇ ) of the aircraft 2 on the image data.
  • the coordinates of the distance measuring point are calculated. For example, the distance l from the laser light irradiation reference point to the distance measuring point, the laser light irradiation angle ⁇ , the three-dimensional coordinates of the laser light irradiation reference point, the roll angle ⁇ , the pitch angle ⁇ , and the yaw angle ⁇ of the aircraft 2. Based on this, the three-dimensional coordinates of the distance measuring point are calculated.
  • the coordinates of the distance measurement points on the image of the image data photographed by the left camera 20a and the right camera 20b are calculated.
  • the image matching unit 32 performs image matching of a pair of image data photographed at different photographing positions by at least one of the left camera 20a and the right camera 20b, and one image data of the pair (hereinafter referred to as first image data).
  • a point corresponding to the coordinates of the distance measuring point on the image (denoted as image data as appropriate) is searched from the image of the other image data of the pair (hereinafter referred to as second image data as appropriate).
  • a method for image matching a well-known template matching method for examining the similarity between two images can be used.
  • the first image data is used as a template image
  • the second image data is used as target image data
  • the two image data are compared, and a point corresponding to the coordinates of a distance measuring point on the template image is determined from the target image data image.
  • the image data pair may be a pair of image data taken at different photographing positions, the image data taken at time i during the flight of the aircraft 2 and the time advanced from this time i You may use the image data image
  • the pair of image data may be a pair of image data captured at time i by the left camera 20a and the right camera 20b.
  • the pair of image data may be a pair of image data captured at time i and time j advanced from time i by at least one of the left camera 20a and the right camera 20b.
  • the change of the subject on the image according to the attitude of the aircraft 2 can be used for estimating the attitude of the aircraft 2.
  • the left camera 20a and the right camera 20b do not need a stabilizer.
  • the posture estimation unit 33 calculates the coordinates of the distance measurement points on the image of the other image data (second image data) of the pair calculated by the coordinate calculation unit 31 and the corresponding points searched by the image matching unit 32.
  • the attitude angle value of the aircraft 2 is corrected so that the difference from the coordinates is small, and the attitude of the aircraft 2 is estimated.
  • the attitude estimation unit 33 calculates the correction amount of the attitude angle value of the aircraft 2 so that the difference between the coordinates of these two points becomes small, and finally determines the attitude angle that minimizes the difference between the coordinates of the two points.
  • the attitude angle of the aircraft 2 is estimated. This makes it possible to estimate the attitude angle of the aircraft 2 with high accuracy based on the distance data, angle data, coordinate data, and image data.
  • FIG. 3 is a block diagram showing a hardware configuration of the navigation device 3.
  • FIG. 3A shows a hardware processing circuit 100 that realizes the function of the navigation apparatus 3
  • FIG. 3B shows a hardware configuration that executes software that realizes the function of the navigation apparatus 3.
  • FIG. 4 is a flowchart showing an outline of the operation of the navigation device 3.
  • Each function of the data acquisition unit 30, the coordinate calculation unit 31, the image matching unit 32, and the posture estimation unit 33 in the navigation device 3 is realized by a processing circuit. That is, the navigation apparatus 3 obtains the distance data, angle data, coordinate data, and image data shown in FIG. 4 based on the distance data, the angle data, the coordinate data, and the attitude angle of the aircraft 2 based on the attitude data of the aircraft 2.
  • Step ST2 for calculating the coordinates of the distance measuring points on the image, matching the coordinates of the distance measuring points on the image of the image data of one of the pair by performing image matching of a pair of image data photographed at different photographing positions.
  • Step ST3 for searching for the point to be searched from the image of the other image data of the pair, the coordinates of the distance measuring point on the image of the other image data of the pair, and the coordinates of the corresponding point searched by the image matching unit 32 Is provided with a processing circuit for performing step ST4 for estimating the attitude of the aircraft 2 by correcting the value of the attitude angle of the aircraft 2 so that the difference between the two is reduced.
  • the processing circuit may be dedicated hardware or a CPU (Central Processing Unit) that executes a program stored in the memory.
  • CPU Central Processing Unit
  • the processing circuit 100 when the processing circuit is a dedicated hardware processing circuit 100, the processing circuit 100 may be, for example, a single circuit, a composite circuit, a programmed processor, or a parallel programmed processor.
  • ASIC Application Specific Integrated Circuit
  • FPGA Field-Programmable Gate Array
  • the functions of the data acquisition unit 30, the coordinate calculation unit 31, the image matching unit 32, and the posture estimation unit 33 may be realized by a processing circuit, or the functions of the units may be realized by a single processing circuit. May be.
  • the functions of the data acquisition unit 30, the coordinate calculation unit 31, the image matching unit 32, and the posture estimation unit 33 are software, firmware, or software and firmware. Realized by a combination of Software and firmware are described as programs and stored in the memory 102.
  • the CPU 101 reads out and executes the program stored in the memory 102, thereby realizing the functions of each unit. That is, the navigation device 3 includes a memory 102 for storing a program that, when executed by the CPU 101, results from the processing from step ST1 to step ST4 shown in FIG.
  • these programs cause the computer to execute the procedures or methods of the data acquisition unit 30, the coordinate calculation unit 31, the image matching unit 32, and the posture estimation unit 33.
  • the memory is, for example, a RAM (Random Access Memory), ROM, flash memory, EPROM (Erasable Programmable ROM), EEPROM (Electrically Programmable EPROM), or other nonvolatile or volatile semiconductor memory, magnetic disk, flexible disk, Optical discs, compact discs, mini discs, DVDs (Digital Versatile Disk), and the like are applicable.
  • RAM Random Access Memory
  • ROM Read Only Memory
  • EPROM Erasable Programmable ROM
  • EEPROM Electrically Programmable EPROM
  • magnetic disk magnetic disk
  • flexible disk Optical discs
  • compact discs compact discs
  • mini discs mini discs
  • DVDs Digital Versatile Disk
  • the data acquisition unit 30 realizes its function by a dedicated hardware processing circuit 100, and the coordinate calculation unit 31, the image matching unit 32, and the posture estimation unit 33 are executed by the CPU 101 executing a program stored in the memory 102. The function is realized by doing.
  • the processing circuit can realize the above-described functions by hardware, software, firmware, or a combination thereof.
  • FIG. 5 is a diagram schematically showing the positional relationship among the left camera 20a, the right camera 20b, and the laser distance measuring device 21.
  • FIG. 5A is a perspective view of a unit including the left camera 20a, the right camera 20b, and the laser distance measuring device 21, and
  • FIG. 5B is a view of the unit when viewed in the X-axis direction.
  • FIG. 5C is a diagram of the unit viewed in the Z-axis direction
  • FIG. 5D is a diagram of the unit viewed in the Y-axis direction.
  • FIG. 5A is a perspective view of a unit including the left camera 20a, the right camera 20b, and the laser distance measuring device 21, and
  • FIG. 5B is a view of the unit when viewed in the X-axis direction.
  • FIG. 5C is a diagram of the unit viewed in the Z-axis direction
  • FIG. 5D is a diagram of the unit viewed in the Y-axis direction.
  • the left camera 20a is attached to the end of an arm 20c extending to the left side of the laser distance measuring device 21, and the right camera 20b is an arm 20d extending to the right side of the laser distance measuring device 21. It is attached to the end.
  • the lengths of the arms 20c and 20d on both sides are, for example, 1 m.
  • the shooting directions of the left camera 20a and the right camera 20b are directed in the direction directly below the aircraft 2 (Z-axis direction).
  • the laser distance measuring device 21, FIG. 5 (b) the distance measuring point is irradiated to the distance measuring point P 0 of the irradiation angle ⁇ to change under irradiation reference point 21a laser light ground surface from the laser light P 0 receiving reflected light from, for measuring the distance l from the irradiation reference point 21a to the distance measuring point P 0.
  • the irradiation angle ⁇ when the laser beam is irradiated directly below the irradiation reference point 21a is 90 degrees.
  • the left camera 20a and the right camera 20b obtain image data as will be described later with reference to FIG. 8 by performing imaging within a rectangular imaging range.
  • the position of the irradiation reference point 21a and the position of the aircraft 2 are the same. Accordingly, when the aircraft 2 flies horizontally in the X-axis direction, the unit including the left camera 20a, the right camera 20b, and the laser distance measuring device 21 also moves in the X-axis direction as shown in FIG. However, even if a horizontal flight is intended in the actual flight environment, the aircraft 2 cannot take a straight path due to the influence of wind or the like. That is, the aircraft 2 flies while tilting in the rolling direction, the pitching direction, and the yawing direction.
  • FIG. 6 is a diagram showing changes in the positions of the left camera 20a, the right camera 20b, and the laser distance measuring device 21 accompanying the flight of the aircraft.
  • FIG. 6A shows the position coordinate data of the laser distance measuring device 21.
  • FIG. 6B is a graph in which the position coordinates of the left camera 20a, the right camera 20b, and the laser distance measuring device 21 are plotted on the XZ plane.
  • FIG. 6C is a graph in which these position coordinates are plotted on the YZ plane
  • FIG. 6D is a graph in which these position coordinates are plotted on the XY plane.
  • the plane is flying for 3 seconds.
  • the position coordinates of the laser distance measuring device 21 are the position coordinates of the irradiation reference point 21a measured every second by the GNSS device 22.
  • the position coordinates of the left camera 20a and the right camera 20b are calculated on the assumption that they are separated from the position of the irradiation reference point 21a by the 1 m arm 20c, 20d in the Y-axis direction.
  • the position of the aircraft 2 is shifted in the Y-axis direction and the Z-axis direction in 3 seconds, and the aircraft 2 is tilted and flew.
  • FIG. 7 is a diagram showing a change in the measurement result of the laser distance measuring device 21 accompanying the flight of the aircraft 2, and shows the measurement result when the aircraft 2 flies in the state of FIG.
  • FIG. 7A shows angle data and distance data at each time
  • FIG. 7B is a graph in which the data shown in FIG. 7A is plotted.
  • FIG. 8 is a diagram showing an image taken every second by the left camera 20a and the right camera 20b, and shows an image taken when the aircraft 2 flies in the state of FIG.
  • the measurement result of the laser distance measuring device 21 is stored in the memory card 23 with the measurement time t, the irradiation angle ⁇ , and the distance l as one record.
  • the laser distance measuring device 21 performs measurement four times per second.
  • the irradiation angle ⁇ is 90 degrees immediately below the irradiation reference point 21a shown in FIG.
  • the laser distance measuring device 21 scans the laser beam by rotating the irradiation reference point 21a clockwise by 18 degrees around the X axis when viewed from the positive direction of the X axis. .
  • the laser light is also emitted from the laser distance measuring device 21 at an inclination. That is, distance data and angle data as shown in FIGS. 7A and 7B also change according to the attitude angle of the aircraft 2. Further, when the aircraft 2 is tilted and flies, the imaging directions of the left camera 20a and the right camera 20b are also tilted. As a result, the left camera image captured by the left camera 20 a and the right camera image captured by the right camera 20 b as shown in FIG. 8 also change according to the attitude angle of the aircraft 2.
  • the aircraft 2 flies horizontally, and the coordinates on the image of the distance measuring point calculated using the distance data, angle data, coordinate data, and image data are the same as those when the aircraft 2 flies at an angle.
  • An error corresponding to the attitude angle of the aircraft 2 occurs between the coordinates of the distance measuring point. Therefore, in the present invention, the attitude angle is corrected so that the error is reduced, and the attitude angle at which the error is minimized is set as the estimated value of the attitude angle of the aircraft 2.
  • Figure 9 is a diagram showing the image 100a, and 100b including the distance measuring point P 0 of the laser distance measuring device 21 taken in the left camera 20a and the right camera 20b when the aircraft 2 is flying horizontally.
  • the aircraft 2 flies horizontally along the positive direction of the X axis.
  • the lower part of the aircraft 2 is photographed by the left camera 20a and the right camera 20b every second, and the laser distance measuring device 21 sets the irradiation angle ⁇ to 90 degrees and the distance measuring point P 0 directly below the aircraft. Measure distance.
  • Figure 10 is an image 100a including the distance measuring point P 0 of the laser distance measuring device 21 taken in the left camera 20a and the right camera 20b when the aircraft 2 is flying inclined in the pitch direction, a view showing a 100c is there.
  • FIG. 11 shows the coordinates of the ranging point P 0 on the image calculated on the assumption that the aircraft 2 is flying horizontally and the corresponding on the image when the aircraft 2 is flying in the pitch direction. It is a figure which shows the difference
  • Such a difference ⁇ u L between the coordinates P 0b and the coordinates P 0b ′ is a distance measuring point P 0 on the image 100c on the assumption that the aircraft 2 flew without inclination even though the aircraft 2 was inclined in actual flight. This is because the coordinates P 0b of the above are calculated. Therefore, the attitude angle of the aircraft 2 that minimizes the difference ⁇ u L is an estimation result that appropriately represents the actual attitude of the aircraft 2. For example, in the case of FIG. 11, since the difference ⁇ u L is minimized when the aircraft 2 is tilted by the pitch angle ⁇ , the pitch angle ⁇ is obtained as the attitude estimation result. Note that the actual aircraft 2 is inclined in both the rolling direction and the yawing direction in addition to the pitching direction. In this case as well, the roll angle ⁇ and the yaw angle ⁇ may be similarly estimated.
  • FIG. 12 is a flowchart showing the operation of the navigation device 3 according to the first embodiment, and shows a series of processes for estimating the attitude angle of the aircraft 2 in flight.
  • the attitude angle of the aircraft 2 is represented by three parameters of a roll angle ⁇ , a pitch angle ⁇ , and a yaw angle ⁇ , and these angles are estimated every second.
  • ⁇ , ⁇ may be estimated. That is, a total of six posture angles, which are unknown numbers, are estimated.
  • the data acquisition unit 30 reads and acquires distance data, angle data, coordinate data, and image data from the memory card 23 mounted on the aircraft 2 (step ST1a).
  • Distance data indicates a distance l to the distance measurement point P 0 from the irradiation reference point 21a of the measurement laser light by the laser distance measuring device 21
  • angle data is data indicating an irradiation angle ⁇ of the laser beam is there.
  • the coordinate data is a three-dimensional coordinate (X 0 , Y 0 , Z 0 ) of the irradiation reference point 21 a of the laser beam measured by the GNSS device 22.
  • the image data is image data that includes a distance measuring point P 0 on a subject to be photographed by the left camera 20a and the right camera 20b. In this way, by using the data stored in the memory card 23 during the flight of the aircraft 2, the attitude of the aircraft 2 can be estimated after completion of the flight, and the survey result is corrected using the estimated attitude angle. Is also possible.
  • the coordinate calculation unit 31 uses the following formula (1).
  • three-dimensional coordinates of each distance measuring point P 0 (X, Y, Z ) is calculated (step ST2a).
  • a 11 to a 33 are elements of a 3 ⁇ 3 rotation matrix representing the inclinations of the laser distance measuring device 21, the left camera 20a, and the right camera 20b according to the attitude of the aircraft 2.
  • (X 0 , Y 0 , Z 0 ) is a three-dimensional coordinate of the irradiation reference point 21a of the laser beam indicated by the coordinate data.
  • ⁇ is the irradiation angle of the laser light shown the angle data
  • l is the distance from the irradiation reference point 21a of the laser beam indicated by the distance data to the distance measuring point P 0.
  • the irradiation angle ⁇ is 90 degrees when the aircraft 2 is vertically downward.
  • the coordinate calculation unit 31 calculates the projection center coordinates ( XL , Y) of the left camera 20a every second according to the following formula (2) and the following formula (3) based on the coordinate data and the setting value of the attitude angle.
  • L , Z L ) and the projection center coordinates (X R , Y R , Z R ) of the right camera 20b are calculated (step ST3a).
  • the coordinate calculation unit 31 based on the coordinate data, the setting value of the attitude angle, the three-dimensional coordinates of the distance measuring point P 0 , and the projection center coordinates of the left camera 20a and the right camera 20b, the coordinate calculation unit 31 according to the following equation (5), the coordinates (x L, y L) of the distance measuring point P 0 on the left camera image and the distance measuring point P 0 on the right camera image coordinates (x R, y R) is calculated ( Step ST4a).
  • c is the focal length of the left camera 20a and the right camera 20b.
  • the image matching unit 32 obtains the left camera image captured at time i and the right camera image captured at time j advanced by +1 from the time i from the image data acquired by the data acquisition unit 30. Extract as a pair. Such processing for extracting a pair of image data to be subjected to image matching is called pairing. By this pairing, a pair of image data shot at different shooting positions is obtained. Subsequently, the image matching unit 32 performs template matching between the left camera image at the time i and the right camera image at the time j, so that a distance measuring point on the left camera image at the time i calculated by the coordinate calculation unit 31 is obtained. A point corresponding to the coordinate (x L , y L ) of P 0 is searched from the right camera image at time j (step ST5a).
  • FIG. 16 shows the correspondence between the coordinates of the distance measuring point P 0 on the left camera image at time i and the coordinates of the points on the right camera image at time j obtained by the template matching corresponding thereto.
  • SCAN x (x Li , y Li ) is template matching on the right camera image at time j for a small area centered on the coordinates (x Li , y Li ) of the distance measuring point P 0 on the left camera image at time i. Is the x coordinate value of the corresponding point obtained by performing.
  • SCAN y (x Li , y Li ) is the y coordinate value of the corresponding point obtained by performing template matching in the same manner.
  • the coordinates (x Rj , y Rj ) of the distance measuring point P 0 on the right camera image at time j calculated by the coordinate calculating unit 31 and the corresponding points searched by the image matching unit 32.
  • the coordinates (SCAN x (x Li , y Li ), SCAN y (x Li , y Li ) This is because the attitude angle ( ⁇ , ⁇ , ⁇ ) of the aircraft 2 is a value other than zero. That is, if an appropriate attitude angle ( ⁇ , ⁇ , ⁇ ) indicating the attitude of the aircraft 2 in flight is set and the coordinates shown in FIG.
  • the posture estimation unit 33 uses the coordinates (x Rj , y Rj ) of the distance measuring point P 0 on the right camera image at time j and the coordinates (SCAN x (x Li , y) of the corresponding points searched by the image matching unit 32. Li ), SCAN y (x Li , y Li )) is calculated as a correction amount for the posture angle (step ST6a). For example, using the observation equation v x, v y of the following formula (6).
  • tilde omega ( ⁇ omega), tilde phi ( ⁇ phi), tilde kappa ( ⁇ kappa) is roll angle is unknown omega, pitch angle phi, is an approximate solution of the yaw angle kappa.
  • ⁇ , ⁇ , and ⁇ are correction amounts for the approximate solutions tilde ⁇ , tilde ⁇ , and tilde ⁇ .
  • ⁇ F x / ⁇ is a partial differentiation of F x at the roll angle ⁇
  • ⁇ F x / ⁇ is a partial differentiation of F x at the pitch angle ⁇ of F x , ⁇ F x / ⁇ .
  • These partial differentials are coefficients whose values are obtained by substituting the approximate solutions tilde ⁇ , tilde ⁇ , and tilde ⁇ .
  • ⁇ F y / ⁇ is a partial differential of F y at the roll angle ⁇
  • ⁇ F y / ⁇ is a partial differential of F y at the pitch angle ⁇
  • ⁇ F y / ⁇ is F This is a partial derivative of y at the yaw angle ⁇ .
  • These partial differentials are coefficients that can be obtained by substituting the approximate solutions tilde ⁇ , tilde ⁇ , and tilde ⁇ .
  • Tilde F x ( ⁇ F x) is, SCAN x (x L, y L) with respect to F x is a value obtained by substituting the approximate solution of the x R, tilde F y ( ⁇ F y) is This is a value obtained by substituting an approximate solution of SCAN y (x L , y L ) and y R for F y .
  • FIG. 18 shows a 24 ⁇ 6 design matrix composed of partial differential coefficients calculated for each observation equation in this way.
  • the posture estimation unit 33 calculates a product of the transposed matrix of the design matrix and the design matrix.
  • a calculation result using the planning matrix shown in FIG. 18 is shown in FIG.
  • the posture estimation unit 33 calculates the product of the transposed matrix of the design matrix and the constant vector shown in FIG. The result of this calculation is shown in FIG.
  • the posture estimation unit 33 calculates the product of the inverse matrix calculated from the matrix shown in FIG. 19 and the vector shown in FIG.
  • This calculation result is the posture angle correction amount ( ⁇ , ⁇ , ⁇ ) shown in FIG. Initially, assuming that the aircraft 2 is flying horizontally without tilting, (0, 0, 0) is set as the initial value of the attitude angle ( ⁇ , ⁇ , ⁇ ). The quantity is an approximate solution of the attitude angle as it is.
  • the posture estimation unit 33 adds the correction amount calculated as described above to the previous approximate solution for correction, and determines the corrected approximate solution as a setting value for the posture angle (step ST7a). At this time, if the predetermined number of repetitions has not elapsed (step ST8a; NO), the posture estimation unit 33 instructs the coordinate calculation unit 31 to perform the same coordinate calculation as described above. Thereby, the coordinate calculation unit 31 performs the processing from step ST2a to step ST4a using the corrected approximate solution as the setting value of the posture angle, and the image matching unit 32 performs the processing of step ST5a.
  • the pairing of the left camera image at time i and the right camera image at time i corresponds to fixed stereo. In motion stereo, the camera is moved and shot from different shooting positions.
  • the configuration using the left camera 20a and the right camera 20b is shown, but a single camera may be used.
  • the unknowns are the three parameters ( ⁇ , ⁇ , ⁇ ) of the attitude angle for each time has been shown so far, but the six parameters including the position coordinates (X, Y, Z) of the aircraft 2 may be added. Often, internal parameters such as the focal length c of the camera may be included.
  • the attitude of the aircraft 2 can be estimated without using an IMU or a stabilizer.
  • the attitude of the aircraft 2 is estimated using the dimensional coordinates (X, Y, Z), the attitude of the aircraft 2 can be estimated with high accuracy.
  • the pair of image data includes image data captured at time i and image data captured at time j that is ahead of time i during flight of the aircraft 2. It is.
  • the change of the subject on the image according to the attitude of the aircraft 2 can be used for the attitude estimation of the aircraft 2.
  • the image data pair is a pair of image data captured at the time i by the left camera 20a and the right camera 20b, or the left camera 20a and the right camera 20b. It is a pair of image data photographed at a time i and a time j advanced from the time i by at least one of them. Even if such image data is used, the change of the subject on the image according to the attitude of the aircraft 2 can be used for the estimation of the attitude of the aircraft 2.
  • the surveying system 1 includes a memory card 23 mounted on the aircraft 2.
  • the data acquisition unit 30 reads and acquires distance data, angle data, coordinate data, and image data stored in the memory card 23. In this way, by using the data stored in the memory card 23 during the flight of the aircraft 2, the attitude of the aircraft 2 can be estimated after the flight is completed, and the survey result can be corrected using the estimated attitude angle.
  • FIG. FIG. 23 is a block diagram showing a configuration of a surveying system 1A according to Embodiment 2 of the present invention.
  • the surveying system 1A is a system for surveying the terrain from the aircraft 2A.
  • the wireless communication device 24 transmits distance data, angle data, coordinate data, and image data obtained during the flight of the aircraft 2 ⁇ / b> A to the navigation device 3.
  • the navigation apparatus 3 is provided separately from the aircraft 2A. However, the navigation device 3 may be mounted on the aircraft 2A.
  • the data acquisition unit 30 of the navigation device 3 receives and acquires distance data, angle data, coordinate data, and image data transmitted by the wireless communication device 24.
  • the navigation device 3 estimates the attitude of the aircraft 2A based on the data acquired in this way by the same processing as in the first embodiment.
  • the surveying system 1 ⁇ / b> A includes the wireless communication device 24 mounted on the aircraft 2.
  • the data acquisition unit 30 receives and acquires distance data, angle data, coordinate data, and image data transmitted by the wireless communication device 24. In this way, by using the data wirelessly transmitted from the wireless communication device 24, it is possible to estimate the attitude during the flight of the aircraft 2A. It is also possible to correct the survey result during the flight of the aircraft 2A using the estimated attitude angle.
  • the mobile body in the present invention is a flying body such as the aircraft 2, but the present invention is not limited to this.
  • the navigation apparatus according to the present invention can be applied to a mobile mapping system, and a vehicle equipped with this system is a moving body.
  • the navigation apparatus according to the present invention can be used as an apparatus for estimating a posture of a railway vehicle, a ship, and a robot as moving bodies. Even in such a moving body, the posture angle ( ⁇ , ⁇ , ⁇ ) of the moving body can be similarly used as a parameter indicating the posture. In some cases, position information may be included in the parameter.
  • the navigation device according to the present invention is suitable as, for example, a UAV navigation device because it can accurately estimate the posture of a moving body without a configuration equipped with an IMU and a stabilizer.
  • 1,1A survey system 1,2A aircraft, 3 navigation devices, 20a left camera, 20b right camera, 20c, 20d arm, 21 laser ranging device, 21a irradiation reference point, 22 GNSS device, 23 memory card, 24 wireless communication Device, 30 data acquisition unit, 31 coordinate calculation unit, 32 image matching unit, 33 attitude estimation unit, 100 processing circuit, 100a to 100c image, 101 CPU, 102 memory.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Signal Processing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Navigation (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

異なる撮影位置でそれぞれ撮影された画像データペアの画像マッチングを行って、ペアの一方の画像データの画像上の測距点の座標に対応する点を、ペアのもう一方の画像データの画像から探索する。ペアのもう一方の画像データの画像上の測距点の座標と画像マッチングによって探索された対応する点の座標との差が小さくなるように航空機（２）の姿勢を示すパラメータの値を補正して航空機（２）の姿勢を推定する。

Description

航法装置および測量システム
　この発明は、測量用のカメラおよびレーザ測距装置を搭載した移動体の姿勢を推定する航法装置およびこれを備えた測量システムに関する。
　例えば、特許文献１には、飛行体に搭載したカメラとレーザ発信受信装置を使用して、写真測量と航空レーザ測量を行う測量システムが記載されている。
　この測量システムにおいて、飛行体から測量対象を撮影するカメラは、スタビライザーと呼ばれる姿勢安定化装置に支持されており、飛行中の飛行体の姿勢によらず、撮影方向を鉛直下向きに保つことができる。
　また、レーザ発信受信装置は、飛行体から予め定められた周期でレーザ光を測量対象に照射し、この測量対象からの反射光を受信する。この測量システムにおける制御装置は、レーザ発信受信装置によって測量対象から受信された反射光の情報を使用して航空レーザ測量を行う。なお、レーザ発信受信装置は、本発明におけるレーザ測距装置に相当する。
　航空レーザ測量では、上記の情報の他、飛行体の３次元座標データ（飛行体の水平方向の位置と標高）と飛行中の飛行体の姿勢を示す情報が必要である。これらの情報のうち、飛行体の３次元座標データは、飛行体に搭載されたＧＮＳＳ（Ｇｌｏｂａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓａｔｅｌｌｉｔｅ　Ｓｙｓｔｅｍ）装置により検出される。すなわち、ＧＮＳＳ装置は、ＧＮＳＳ衛星からＧＮＳＳ情報を一定の周期ごとに受信し、このＧＮＳＳ情報を解析して飛行体の３次元座標データを得る。
　一方、レーザ発信受信装置によってレーザ光が測量対象に照射される周期は、ＧＮＳＳ装置によってＧＮＳＳ情報が受信される周期よりも短い周期である。このため、制御装置は、レーザ発信受信装置によって測量対象からの反射光が受信されても、ＧＮＳＳ情報が受信される周期以外の一定の間隔で飛行体の３次元座標データを得ることができない。
　これに対して、従来の一般的な航空レーザ測量においては、飛行体に搭載されたＩＭＵ（Ｉｎｅｒｔｉａｌ　Ｍｅａｓｕｒｅｍｅｎｔ　Ｕｎｉｔ）により計測された３軸の加速度および３軸の角加速度の情報を使用して、ＧＮＳＳ情報が受信される周期以外の一定の間隔における飛行体の３次元座標データを得ていた。
　しかしながら、ＩＭＵは、非常に高価である上、比較的重い装置であることから、これを搭載可能な飛行体の種類も限定されてしまう。
　そこで、特許文献１に記載される測量システムでは、ＩＭＵの代わりに、ＩＭＵよりも安価でかつ小型な加速度計および角加速度計を備えている。
　すなわち、この測量システムでは、加速度計からの３軸の加速度および角加速度計からの３軸の角加速度の情報を使用して、ＧＮＳＳ情報の受信周期以外の一定の間隔における飛行体の３次元座標データを得ている。
　また、飛行体の姿勢を示す情報は、飛行体のローリング方向、ピッチング方向、ヨーイング方向の角度（以下、ロール角、ピッチ角、ヨー角と記載する）であり、カメラによって２つ以上の異なる位置から撮影された画像間の対応点を対象としたバンドル計算で得られた値が使用される。制御装置は、バンドル計算で得られた飛行体の姿勢に基づいて、加速度計からの加速度と角加速度計からの角加速度とを利用して、レーザ光の走査周期分（ＧＮＳＳ情報の受信周期以外の一定の間隔分）の飛行体の姿勢を算出している。
特開２０１４－１４５７６２号公報
　特許文献１に記載される測量システムでは、カメラによって異なる位置から撮像された画像データを用いたバンドル計算で飛行体の姿勢を推定しており、画像データのみを姿勢の推定に用いている。このため、姿勢の推定精度に限界があった。
　また、上記測量システムにおいて、飛行体の姿勢の推定に用いられる画像情報は、飛行体の姿勢に関わらず、飛行体の鉛直下向きで撮影されたことを前提としている。
　このため、カメラの撮影方向を常に鉛直下向きに維持するスタビライザーを備える必要があり、システム構成が複雑化する。
　この発明は、上記課題を解決するもので、ＩＭＵ、スタビライザーを搭載しない構成で移動体の姿勢を精度よく推定することができる航法装置および測量システムを得ることを目的とする。
　この発明に係る航法装置は、データ取得部、座標算出部、画像マッチング部および姿勢推定部を備える。データ取得部は、移動体に搭載されたレーザ測距装置により測定されたレーザ光の照射基準点から測距点までの距離を示す距離データとレーザ光の照射角度を示す角度データ、移動体に搭載された座標測定装置により測定されたレーザ光の照射基準点の３次元座標を示す座標データ、移動体に搭載された撮影装置により撮影された撮影対象に測距点を含む画像データを取得する。座標算出部は、データ取得部により取得された距離データ、角度データ、座標データおよび移動体の姿勢を示すパラメータに基づいて、画像データの画像上の測距点の座標を算出する。画像マッチング部は、撮影装置により異なる撮影位置でそれぞれ撮影された画像データのペアの画像マッチングを行って、座標算出部により算出されたペアの一方の画像データの画像上の測距点の座標に対応する点を、ペアのもう一方の画像データの画像から探索する。姿勢推定部は、座標算出部により算出されたペアのもう一方の画像データの画像上の測距点の座標と画像マッチング部により探索された対応する点の座標との差が小さくなるように移動体の姿勢を示すパラメータの値を補正して当該移動体の姿勢を推定する。
　この発明によれば、異なる撮影位置でそれぞれ撮影された画像間の対応する点の座標が移動体の姿勢によってずれることに着目し、これらの座標の差が小さくなるように移動体の姿勢を示すパラメータの値を補正して移動体の姿勢を推定するので、ＩＭＵ、スタビライザーを使用しなくても移動体の姿勢を推定することができる。
　また、測距点が撮影された画像データに加え、レーザ光の照射基準点から測距点までの距離、レーザ光の照射角度、レーザ光の照射基準点の３次元座標を使用して移動体の姿勢を推定するので、移動体の姿勢を精度よく推定することが可能である。
この発明の実施の形態１に係る測量システムの構成を示すブロック図である。 実施の形態１に係る航法装置の機能構成を示すブロック図である。 実施の形態１に係る航法装置のハードウェア構成を示すブロック図である。図３（ａ）は、航法装置の機能を実現するハードウェアの処理回路を示し、図３（ｂ）は、航法装置の機能を実現するソフトウェアを実行するハードウェア構成を示している。 実施の形態１に係る航法装置の動作の概要を示すフローチャートである。 左カメラと右カメラとレーザ測距装置との位置関係を模式的に示す図である。図５（ａ）は、左カメラと右カメラとレーザ測距装置とからなるユニットの斜視図、図５（ｂ）は、当該ユニットをＸ軸方向に見た図であり、図５（ｃ）は、当該ユニットをＺ軸方向に見た図、図５（ｄ）は、当該ユニットをＹ軸方向に見た図である。 航空機の飛行に伴う左カメラ、右カメラおよびレーザ測距装置の位置変化を示す図である。図６（ａ）は、レーザ測距装置の位置座標データを示している。図６（ｂ）は、左カメラ、右カメラおよびレーザ測距装置の位置座標をＸＺ平面にプロットしたグラフであり、図６（ｃ）は、これらの位置座標をＹＺ平面にプロットしたグラフであり、図６（ｄ）は、これらの位置座標をＸＹ平面にプロットしたグラフである。 航空機の飛行に伴ったレーザ測距装置の測定結果の変化を示す図である。図７（ａ）は、各時刻における角度データと距離データを示しており、図７（ｂ）は、図７（ａ）に示すデータをプロットしたグラフである。 左カメラと右カメラによって１秒ごとに撮影された画像を示す図である。 航空機が水平に飛行しているときに左カメラおよび右カメラに撮影されたレーザ測距装置の測距点を含む画像を示す図である。 航空機がピッチ方向に傾いて飛行しているときに左カメラおよび右カメラに撮影されたレーザ測距装置の測距点を含む画像を示す図である。 航空機が水平に飛行していると仮定して算出された画像上の測距点の座標と航空機がピッチ方向に傾いて飛行しているときの画像上の対応する点の座標との間の誤差を示す図である。 実施の形態１に係る航法装置の動作を示すフローチャートである。 測距点の３次元座標の算出結果を示す図である。 左カメラと右カメラの投影中心座標を示す図である。 左カメラと右カメラによって撮影された画像上の測距点の座標を示す図である。 時刻ｉの左カメラ画像上の測距点の座標と、これに対応する画像マッチングにより探索された時刻ｊの右カメラ画像上の点の座標を示す図である。 観測方程式の定数ベクトルを示す図である。 計画行列を示す図である。 図１８の計画行列の転置行列と計画行列の積を示す図である。 図１８の計画行列を転置したものと図１７の定数ベクトルの積によって得られる行列と、図１９の行列から計算した逆行列との積を示す図である。 姿勢角の補正量を示す図である。 最終的な姿勢角の推定結果を示す図である。 この発明の実施の形態２に係る測量システムの構成を示すブロック図である。
　以下、この発明をより詳細に説明するため、この発明を実施するための形態について、添付の図面に従って説明する。
実施の形態１．
　図１は、この発明の実施の形態１に係る測量システム１の構成を示すブロック図である。測量システム１は、航空機２から地形を測量するシステムであり、航空機２に搭載された左カメラ２０ａ、右カメラ２０ｂ、レーザ測距装置２１、ＧＮＳＳ装置２２およびメモリカード２３と、航法装置３とを備える。航法装置３は、飛行中の航空機２の姿勢を推定する装置であって、図１に示すように航空機２とは別に設けられる。ただし、航法装置３を航空機２に搭載してもよい。また、航空機２の姿勢は、航空機２のローリング方向、ピッチング方向、ヨーイング方向の姿勢角であるロール角ω、ピッチ角φ、ヨー角κの３つのパラメータによって特定される。
　航空機２は、この発明における移動体を具体化したものであり、左カメラ２０ａ、右カメラ２０ｂ、レーザ測距装置２１、ＧＮＳＳ装置２２およびメモリカード２３を搭載して飛行することができる。例えば、搭乗したパイロットが操縦する航空機であってもよく、ＵＡＶ（Ｕｎｍａｎｎｅｄ　Ａｅｒｉａｌ　Ｖｅｈｉｃｌｅ）であってもよい。
　左カメラ２０ａおよび右カメラ２０ｂは、この発明における第１の撮影部および第２の撮影部を具体化した構成要素であり、レーザ測距装置２１の測距点を含んだ地表面を撮影する。この場合、左カメラ２０ａおよび右カメラ２０ｂと、これらの撮影処理を制御する制御装置とを含む装置が、この発明における撮影装置に相当する。例えば、制御装置は、左カメラ２０ａおよび右カメラ２０ｂに指示して予め定めた周期で地表面の撮影を行い、撮影により得られた画像と撮影日時とを対応付けた画像データをメモリカード２３に記憶する。なお、予め定めた周期としては、１秒ごとに撮影を行うことが考えられる。
　レーザ測距装置２１は、レーザ光の照射角度θを変えながらレーザ光を測量対象である地表面へ照射して地表面の測距点からの反射光を受信することで、レーザ光の照射基準点から測距点までの距離ｌを測定する。
　また、レーザ測距装置２１は、距離ｌを測定するごとに、この距離ｌを示す距離データと、この距離ｌが得られたレーザ光の照射角度θを示す角度データとをメモリカード２３に記憶する。
　ＧＮＳＳ装置２２は、この発明における座標測定装置を具体化した構成要素であって、レーザ測距装置２１におけるレーザ光の照射基準点の３次元座標を測定する。
　また、ＧＮＳＳ装置２２は、予め定めた周期で照射基準点の３次元座標を示す座標データをメモリカード２３に記憶する。例えば、左カメラ２０ａおよび右カメラ２０ｂによる撮影に同期して１秒ごとに座標を測定する。
　なお、ＧＮＳＳ装置２２と照射基準点の位置の違いは、ＧＮＳＳ装置２２の測定精度に対して許容範囲内である。すなわち、ＧＮＳＳ装置２２は照射基準点と同一の位置にあるものとし、さらに当該照射基準点の位置は航空機２の位置と同じ意味であるものとする。
　メモリカード２３は、この発明における記憶装置を具体化した構成要素であり、航空機２の飛行中に測定された距離データ、角度データ、画像データ、座標データを記憶する。
　メモリカード２３として、例えばＳＤ（Ｓｅｃｕｒｅ　Ｄｉｇｉｔａｌ）メモリカードを使用してもよい。
　図２は、航法装置３の機能構成を示すブロック図である。航法装置３は、図２に示すように、データ取得部３０、座標算出部３１、画像マッチング部３２および姿勢推定部３３を備える。データ取得部３０は、航空機２のメモリカード２３に蓄えられた距離データ、角度データ、座標データおよび画像データを取得する構成要素である。
　例えば、データ取得部３０は、メモリカード２３のカードドライブに有線または無線で接続して上記データを読み出して取得する。
　座標算出部３１は、データ取得部３０により取得された距離データ、角度データ、座標データおよび航空機２の姿勢角（ロール角ω、ピッチ角φ、ヨー角κ）に基づいて、画像データの画像上の測距点の座標を算出する。例えば、レーザ光の照射基準点から測距点までの距離ｌ、レーザ光の照射角度θ、レーザ光の照射基準点の３次元座標、航空機２のロール角ω、ピッチ角φ、ヨー角κに基づいて、測距点の３次元座標を算出する。そして、測距点の３次元座標と左カメラ２０ａおよび右カメラ２０ｂの投影中心座標に基づいて、左カメラ２０ａおよび右カメラ２０ｂに撮影された画像データの画像上の測距点の座標を算出する。
　なお、航空機２のロール角ω、ピッチ角φ、ヨー角κは未知であり、最初は、姿勢角の補正量が算出されていないため、ロール角ω＝０、ピッチ角φ＝０、ヨー角κ＝０を初期値として座標の計算を行う。この座標算出の詳細は後述する。
　画像マッチング部３２は、左カメラ２０ａおよび右カメラ２０ｂのうちの少なくとも一方により異なる撮影位置でそれぞれ撮影された画像データのペアの画像マッチングを行って、ペアの一方の画像データ（以下、第１の画像データと適宜記載する）の画像上の測距点の座標に対応する点を、ペアのもう一方の画像データ（以下、第２の画像データと適宜記載する）の画像から探索する。
　画像マッチングの方法としては、２つの画像の類似度などを調べる周知のテンプレートマッチング法を用いることができる。例えば、第１の画像データをテンプレート画像とし、第２の画像データを対象画像データとして双方の画像データを比較し、テンプレート画像上の測距点の座標に対応する点を対象画像データの画像から探索する。
　また、画像データのペアとしては、異なる撮影位置でそれぞれ撮影された画像データのペアであればよいので、航空機２の飛行中に時刻ｉで撮影された画像データとこの時刻ｉよりも進んだ時刻ｊで撮影された画像データとを用いてもよい。
　さらに、画像データのペアとしては、左カメラ２０ａおよび右カメラ２０ｂとによって時刻ｉにそれぞれ撮影された画像データのペアであってもよい。
　さらに、画像データのペアは、左カメラ２０ａおよび右カメラ２０ｂのうちの少なくとも一方によって時刻ｉと時刻ｉよりも進んだ時刻ｊとにそれぞれ撮影された画像データのペアであってもよい。
　このような画像データのペアを用いることで、航空機２の姿勢に応じた画像上の被写体の変化を航空機２の姿勢推定に利用することができる。
　換言すると、この発明では、航空機２の姿勢に応じた画像上の被写体の変化を航空機２の姿勢推定に利用するため、左カメラ２０ａと右カメラ２０ｂにスタビライザーが不要である。
　姿勢推定部３３は、座標算出部３１によって算出されたペアのもう一方の画像データ（第２の画像データ）の画像上の測距点の座標と画像マッチング部３２によって探索された対応する点の座標との差が小さくなるように航空機２の姿勢角の値を補正して、航空機２の姿勢を推定する。前述したように、座標算出部３１は、姿勢角（ω，φ，κ）＝（０，０，０）を初期値として画像上の測距点の座標を算出している。このため、航空機２が傾いて飛行していた場合、座標算出部３１によって算出された上記測距点の座標と画像マッチング部３２によって探索された上記対応する点の座標が一致しなくなる。
　そこで、姿勢推定部３３は、これら２点の座標の差が小さくなるように航空機２の姿勢角の値の補正量を算出し、２点の座標の差が最小となる姿勢角を最終的な航空機２の姿勢角と推定する。これにより、距離データ、角度データ、座標データ、画像データに基づいて、航空機２の姿勢角を高精度に推定することが可能となる。
　図３は、航法装置３のハードウェア構成を示すブロック図である。図３（ａ）は航法装置３の機能を実現するハードウェアの処理回路１００を示し、図３（ｂ）は航法装置３の機能を実現するソフトウェアを実行するハードウェア構成を示している。また、図４は、航法装置３の動作の概要を示すフローチャートである。
　航法装置３におけるデータ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の各機能は、処理回路により実現される。
　すなわち、航法装置３は、図４に示す、距離データ、角度データ、座標データ、画像データを取得するステップＳＴ１、距離データ、角度データ、座標データおよび航空機２の姿勢角に基づいて、画像データの画像上の測距点の座標を算出するステップＳＴ２、異なる撮影位置でそれぞれ撮影された画像データのペアの画像マッチングを行って、ペアの一方の画像データの画像上の測距点の座標に対応する点を、ペアのもう一方の画像データの画像から探索するステップＳＴ３、ペアのもう一方の画像データの画像上の測距点の座標と画像マッチング部３２によって探索された対応する点の座標との差が小さくなるように航空機２の姿勢角の値を補正して、航空機２の姿勢を推定するステップＳＴ４を行うための処理回路を備えている。
　処理回路は、専用のハードウェアであっても、メモリに格納されるプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）であってもよい。
　図３（ａ）に示すように、上記処理回路が専用のハードウェアの処理回路１００である場合、処理回路１００は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ　Ｓｐｅｃｉｆｉｃ　Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ　Ｇａｔｅ　Ａｒｒａｙ）、または、これらを組み合わせたものが該当する。
　また、データ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の各部の機能をそれぞれ処理回路で実現してもよいし、各部の機能をまとめて１つの処理回路で実現してもよい。
　図３（ｂ）に示すように、上記処理回路がＣＰＵ１０１である場合、データ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の機能は、ソフトウェア、ファームウェアまたはソフトウェアとファームウェアとの組み合わせにより実現される。
　ソフトウェアとファームウェアはプログラムとして記述され、メモリ１０２に格納される。ＣＰＵ１０１は、メモリ１０２に格納されたプログラムを読み出して実行することにより、各部の機能を実現する。
　すなわち、航法装置３は、ＣＰＵ１０１によって実行されるときに、図４に示すステップＳＴ１からステップＳＴ４までの処理が結果的に実行されるプログラムを格納するためのメモリ１０２を備える。また、これらのプログラムは、データ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の手順または方法をコンピュータに実行させるものである。
　ここで、メモリとは、例えば、ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）、ＲＯＭ、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ　Ｐｒｏｇｒａｍｍａｂｌｅ　ＲＯＭ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ　ＥＰＲＯＭ）などの不揮発性または揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ　Ｖｅｒｓａｔｉｌｅ　Ｄｉｓｋ）などが該当する。
　なお、データ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の各機能について一部を専用のハードウェアで実現し、一部をソフトウェアまたはファームウェアで実現してもよい。
　例えば、データ取得部３０は、専用のハードウェアの処理回路１００でその機能を実現し、座標算出部３１、画像マッチング部３２および姿勢推定部３３は、ＣＰＵ１０１が、メモリ１０２に格納されたプログラム実行することによりその機能を実現する。
　このように、上記処理回路は、ハードウェア、ソフトウェア、ファームウェア、またはこれらの組み合わせによって前述の機能を実現することができる。
　図５は、左カメラ２０ａと右カメラ２０ｂとレーザ測距装置２１の位置関係を模式的に示す図である。図５（ａ）は、左カメラ２０ａと右カメラ２０ｂとレーザ測距装置２１とからなるユニットの斜視図、図５（ｂ）は、当該ユニットをＸ軸方向に見た図である。図５（ｃ）は、当該ユニットをＺ軸方向に見た図、図５（ｄ）は、当該ユニットをＹ軸方向に見た図である。図５（ａ）に示すように、左カメラ２０ａは、レーザ測距装置２１の左側に延びるアーム２０ｃの端部に取り付けられ、右カメラ２０ｂは、レーザ測距装置２１の右側に延びるアーム２０ｄの端部に取り付けられている。両側のアーム２０ｃ，２０ｄのそれぞれの長さは、例えば、１ｍとする。また、左カメラ２０ａおよび右カメラ２０ｂの撮影方向は、航空機２の直下方向（Ｚ軸方向）に向いている。
　レーザ測距装置２１は、図５（ｂ）に示すように、レーザ光の照射角度θを変えながら照射基準点２１ａからレーザ光を地表面の測距点Ｐ０に照射し測距点Ｐ０からの反射光を受信して、照射基準点２１ａから測距点Ｐ０までの距離ｌを測定する。なお、照射基準点２１ａから直下にレーザ光を照射したときの照射角度θを９０度とする。
　左カメラ２０ａおよび右カメラ２０ｂは、図５（ｃ）に示すように、矩形の撮影範囲で撮影を行うことにより、図８を用いて後述するような画像データを得る。
　ここで、照射基準点２１ａの位置と航空機２の位置は同じであると仮定する。従って、航空機２がＸ軸方向に水平飛行する場合、左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１からなるユニットも、図５（ｄ）に示すようにＸ軸方向に移動する。
　しかしながら、実際の飛行環境では水平飛行を意図しても、航空機２は、風などの影響によって直線状の進路をとることはできない。すなわち、航空機２は、ローリング方向、ピッチング方向、ヨーイング方向のそれぞれに傾いた状態で飛行する。
　図６は、航空機の飛行に伴う左カメラ２０ａと右カメラ２０ｂとレーザ測距装置２１の位置変化を示す図である。図６（ａ）は、レーザ測距装置２１の位置座標データを示している。図６（ｂ）は、左カメラ２０ａと右カメラ２０ｂとレーザ測距装置２１の位置座標をＸＺ平面にプロットしたグラフである。図６（ｃ）は、これらの位置座標をＹＺ平面にプロットしたグラフであり、図６（ｄ）は、これらの位置座標をＸＹ平面にプロットしたグラフである。図６（ａ）に示すように、航空機２は、時刻ｔ＝０でＸＹＺ座標系の原点（０，０，０）に位置し、図６（ｂ）に示すＸ軸方向に一定の速度で３秒間の水平飛行を行っている。
　なお、レーザ測距装置２１の位置座標は、ＧＮＳＳ装置２２により１秒ごとに測定された照射基準点２１ａの位置座標である。左カメラ２０ａと右カメラ２０ｂの位置座標は、照射基準点２１ａの位置から１ｍのアーム２０ｃ，２０ｄの分だけＹ軸方向に離れているものとして計算している。
　図６（ｂ）から図６（ｄ）において、大きな四角形のプロットは、時刻ｔ＝０における左カメラ２０ａの位置座標である。大きな逆三角形のプロットは、時刻ｔ＝０における右カメラ２０ｂの位置座標であり、小さな四角形のプロットは、時刻ｔ＝０におけるレーザ測距装置２１の位置座標である。
　また、点のある大きな四角形のプロットは、時刻ｔ＝１における左カメラ２０ａの位置座標である。点のある大きな逆三角形のプロットは、時刻ｔ＝１における右カメラ２０ｂの位置座標であり、点のある小さな四角形のプロットは、時刻ｔ＝１におけるレーザ測距装置２１の位置座標である。
　大きな円形のプロットは、時刻ｔ＝２における左カメラ２０ａの位置座標である。大きな三角形のプロットは、時刻ｔ＝２における右カメラ２０ｂの位置座標であり、小さな円形のプロットは、時刻ｔ＝２におけるレーザ測距装置２１の位置座標である。
　点のある大きな円形のプロットは、時刻ｔ＝３における左カメラ２０ａの位置座標である。点のある大きな三角形のプロットは、時刻ｔ＝３における右カメラ２０ｂの位置座標であり、点のある小さな円形のプロットは、時刻ｔ＝３におけるレーザ測距装置２１の位置座標である。
　図５を用いて説明したように、左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１からなるユニットは、左カメラ２０ａ、レーザ測距装置２１、右カメラ２０ｂの順でＹ軸方向に沿って連結されている。このため、図６（ｂ）に示すように左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１の各位置はＹ軸方向に重なっている。
　一方、時刻ｔ＝１を過ぎると、左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１の位置がＺ軸方向に逸れている。
　また、図６（ｃ）および図６（ｄ）に示すように、時刻ｔ＝１～２において、左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１の位置は、Ｙ軸方向に０．５ｍ逸れてから、時刻ｔ＝３で原点と同じ位置に戻っている。
　図６（ｂ）から図６（ｄ）までのグラフを考慮すると、航空機２の位置が３秒間でＹ軸方向とＺ軸方向にずれており、航空機２が傾いて飛行したことがわかる。
　図７は航空機２の飛行に伴ったレーザ測距装置２１の測定結果の変化を示す図であり、図６の状態で航空機２が飛行した場合における測定結果を示している。
　図７（ａ）は、各時刻における角度データと距離データを示しており、図７（ｂ）は、図７（ａ）に示すデータをプロットしたグラフである。
　また、図８は、左カメラ２０ａと右カメラ２０ｂによって１秒ごとに撮影された画像を示す図であり、図６の状態で航空機２が飛行したときに撮影された画像を示している。
　図７（ａ）に示すように、レーザ測距装置２１の測定結果は、測定時刻ｔ、照射角度θおよび距離ｌを１レコードとしてメモリカード２３に記憶される。ここでは、レーザ測距装置２１が１秒当たり４回の測定を行うものとする。また、照射角度θは、図５（ｂ）に示した照射基準点２１ａの直下を９０度とする。
　レーザ測距装置２１は、図５（ｂ）に示したように、Ｘ軸の正方向から見てＸ軸を中心として時計回りに照射基準点２１ａを１８度ずつ回転させてレーザ光を走査する。
　図７（ｂ）において、白い三角形のプロットは、時刻ｔ＝０．００～０．１５における角度データおよび距離データ、黒い三角形のプロットは時刻ｔ＝１．００～１．１５における角度データおよび距離データ、白い四角形のプロットは時刻ｔ＝２．００～２．１５における角度データおよび距離データである。
　図６で説明したように航空機２が傾いて飛行した場合、レーザ光もレーザ測距装置２１から傾いて照射される。すなわち、図７（ａ）および図７（ｂ）に示すような距離データおよび角度データも、航空機２の姿勢角に応じて変化する。
　また、航空機２が傾いて飛行すると、左カメラ２０ａおよび右カメラ２０ｂの撮像方向も傾く。これにより、図８に示すような左カメラ２０ａに撮影された左カメラ画像および右カメラ２０ｂに撮影された右カメラ画像も、航空機２の姿勢角に応じて変化する。
　従って、航空機２が水平に飛行したと仮定して距離データ、角度データ、座標データ、画像データを用いて算出した測距点の画像上の座標と、航空機２が傾いて飛行したときの同一の測距点の座標との間には、航空機２の姿勢角に応じた誤差が生じる。
　そこで、この発明では、上記誤差が小さくなるように姿勢角を補正していき、上記誤差が最小となる姿勢角を航空機２の姿勢角の推定値とする。以下、この発明における姿勢角の推定処理の概要を説明する。
　図９は、航空機２が水平に飛行しているときに左カメラ２０ａおよび右カメラ２０ｂに撮影されたレーザ測距装置２１の測距点Ｐ０を含む画像１００ａ，１００ｂを示す図である。図９において、航空機２は、Ｘ軸の正方向に沿って水平に飛行したと仮定する。
　ここでは、１秒ごとに、左カメラ２０ａおよび右カメラ２０ｂによって航空機２の下方が撮影され、レーザ測距装置２１が、照射角度θを９０度として機体の直下にある測距点Ｐ０との距離を測定する。
　また、左カメラ２０ａによって時刻ｔ＝０に撮影された画像１００ａ上の測距点Ｐ０の座標Ｐ０ａは、時刻ｔ＝０におけるレーザ光の照射基準点２１ａの３次元座標と航空機２の姿勢角とから算出することができる。
　同様に、右カメラ２０ｂによって時刻ｔ＝１に撮影された画像１００ｂ上の測距点Ｐ０の座標Ｐ０ｂは、時刻ｔ＝１における照射基準点２１ａの３次元座標と航空機２の姿勢角とから算出することができる。
　なお、図９では、航空機２が水平に飛行すると仮定しているので、姿勢角は０である。
　図１０は、航空機２がピッチ方向に傾いて飛行しているときに左カメラ２０ａおよび右カメラ２０ｂに撮影されたレーザ測距装置２１の測距点Ｐ０を含む画像１００ａ，１００ｃを示す図である。図１０において、時刻ｔ＝１に航空機２がピッチ角φだけ傾いて飛行したと仮定している。このとき、右カメラ２０ｂによって時刻ｔ＝１に撮影された画像１００ｃでは、鎖線で示すように、図９の場合よりも被写体が全体的に右寄りになった画像となる。
　図１１は、航空機２が水平に飛行していると仮定して算出された画像上の測距点Ｐ０の座標と航空機２がピッチ方向に傾いて飛行しているときの画像上の対応する点の座標との間の誤差を示す図である。図１１における画像１００ｃ上の測距点Ｐ０点の座標Ｐ０ｂは、図９と同様に航空機２が時刻ｔ＝１においても傾き無く飛行していると仮定して算出した座標である。一方、画像１００ｃ上の点の座標Ｐ０ｂ’は、左カメラ２０ａによって時刻ｔ＝０に撮影された画像１００ａ上の測距点Ｐ０の座標Ｐ０ａに対応する点を、画像マッチングによって画像１００ｃから探索した結果として得られた点の座標である。
　このような座標Ｐ０ｂと座標Ｐ０ｂ’との差ΔｕＬは、実際の飛行では航空機２が傾いていたにも拘わらず、傾きなく飛行したと仮定して画像１００ｃ上の測距点Ｐ０の座標Ｐ０ｂを算出したために生じたものである。従って、差ΔｕＬが最小となる航空機２の姿勢角は実際の航空機２の姿勢を適切に表した推定結果となる。
　例えば、図１１の場合、航空機２をピッチ角φだけ傾けたときに差ΔｕＬが最小となるため、ピッチ角φが姿勢の推定結果として得られる。
　なお、実際の航空機２では、ピッチング方向に加え、ローリング方向とヨーイング方向のそれぞれにも傾くが、この場合も同様にロール角ω、ヨー角κを推定すればよい。
　次に動作について説明する。
　図１２は、実施の形態１に係る航法装置３の動作を示すフローチャートであり、飛行中の航空機２の姿勢角を推定する一連の処理を示している。
　以降では、航空機２の姿勢角がロール角ω、ピッチ角φ、ヨー角κの３つのパラメータで表され、これらの角度は１秒ごとに推定される。
　便宜上、時刻ｔ＝０および時刻ｔ＝３の姿勢角（ω，φ，κ）を（０，０，０）と仮定するので、時刻ｔ＝１および時刻ｔ＝２における未知の姿勢角（ω，φ，κ）を推定すればよい。すなわち、未知数である計６つの姿勢角を推定する。
　まず、データ取得部３０が、航空機２に搭載されたメモリカード２３から距離データ、角度データ、座標データ、画像データを読み出して取得する（ステップＳＴ１ａ）。
　距離データは、レーザ測距装置２１によって測定されたレーザ光の照射基準点２１ａから測距点Ｐ０までの距離ｌを示すデータであり、角度データは、レーザ光の照射角度θを示すデータである。座標データは、ＧＮＳＳ装置２２によって測定されたレーザ光の照射基準点２１ａの３次元座標（Ｘ０，Ｙ０，Ｚ０）である。画像データは、左カメラ２０ａと右カメラ２０ｂによって撮影された撮影対象に測距点Ｐ０を含む画像データである。
　このように航空機２の飛行中にメモリカード２３に蓄積されたデータを用いることで、航空機２の姿勢を飛行終了後に推定することができ、推定された姿勢角を用いて測量結果を補正することも可能である。
　次に、座標算出部３１が、データ取得部３０により取得された距離データ、角度データ、座標データおよび航空機２の姿勢角（ω，φ，κ）の設定値に基づいて、下記式（１）に従い、測距点Ｐ０ごとの３次元座標（Ｘ，Ｙ，Ｚ）を算出する（ステップＳＴ２ａ）。
　下記式（１）において、ａ１１～ａ３３は、航空機２の姿勢に応じたレーザ測距装置２１、左カメラ２０ａおよび右カメラ２０ｂの傾きを表す３×３の回転行列の要素である。
Figure JPOXMLDOC01-appb-I000001
　また、上記式（１）において、（Ｘ０，Ｙ０，Ｚ０）は、上記座標データが示すレーザ光の照射基準点２１ａの３次元座標である。θは、上記角度データが示すレーザ光の照射角度であり、ｌは、上記距離データが示すレーザ光の照射基準点２１ａから測距点Ｐ０までの距離である。なお、照射角度θは、航空機２の鉛直下向きを９０度としている。
　また、姿勢角の設定値は、航空機２が水平に飛行していると仮定して（ω，φ，κ）＝（０，０，０）とする。
　図６（ａ）に示した時刻ｔ＝０～２における座標データおよび図７（ａ）に示した時刻ｔ＝０～２における測距点Ｐ０ごとの角度データと距離データを用いて、測距点Ｐ０ごとの３次元座標（Ｘ，Ｙ，Ｚ）を算出した結果を図１３に示す。
　次に、座標算出部３１が、座標データと姿勢角の設定値に基づいて、下記式（２）と下記式（３）に従って、１秒ごとの左カメラ２０ａの投影中心座標（ＸＬ，ＹＬ，ＺＬ）および右カメラ２０ｂの投影中心座標（ＸＲ，ＹＲ，ＺＲ）を算出する（ステップＳＴ３ａ）。
　姿勢角の設定値を（ω，φ，κ）＝（０，０，０）とし、図６（ａ）に示した時刻ｔ＝０～２における座標データを用いて投影中心座標を算出した結果を図１４に示す。
Figure JPOXMLDOC01-appb-I000002

Figure JPOXMLDOC01-appb-I000003
　次に、座標算出部３１は、座標データ、姿勢角の設定値、測距点Ｐ０の３次元座標、左カメラ２０ａと右カメラ２０ｂの各投影中心座標に基づいて、下記式（４）と下記式（５）に従って、左カメラ画像上の測距点Ｐ０の座標（ｘＬ，ｙＬ）と右カメラ画像上の測距点Ｐ０の座標（ｘＲ，ｙＲ）を算出する（ステップＳＴ４ａ）。
　下記式（４）と下記式（５）において、ｃは、左カメラ２０ａと右カメラ２０ｂの焦点距離である。
Figure JPOXMLDOC01-appb-I000004

Figure JPOXMLDOC01-appb-I000005
　ただし、
Figure JPOXMLDOC01-appb-I000006
　図６（ａ）に示した時刻ｔ＝０～２における座標データ、図１３に示した測距点Ｐ０ごとの３次元座標（Ｘ，Ｙ，Ｚ）および図１４に示した投影中心座標を用いて、左カメラ画像上の測距点Ｐ０の座標（ｘＬ，ｙＬ）と右カメラ画像上の座標（ｘＲ，ｙＲ）を算出した結果を図１５に示す。
　次に、画像マッチング部３２は、データ取得部３０により取得された画像データから、時刻ｉに撮影された左カメラ画像とこの時刻ｉよりも＋１だけ進んだ時刻ｊに撮影された右カメラ画像をペアとして抽出する。このように画像マッチングの対象となる画像データのペアを抽出する処理をペアリングと呼ぶ。このペアリングによって、異なる撮影位置で撮影された画像データのペアが得られる。
　続いて、画像マッチング部３２は、時刻ｉの左カメラ画像と時刻ｊの右カメラ画像とのテンプレートマッチングを行うことにより、座標算出部３１によって算出された時刻ｉの左カメラ画像上の測距点Ｐ０の座標（ｘＬ，ｙＬ）に対応する点を、時刻ｊの右カメラ画像から探索する（ステップＳＴ５ａ）。
　図１６は、時刻ｉの左カメラ画像上の測距点Ｐ０の座標と、これに対応するテンプレートマッチングで得られた時刻ｊの右カメラ画像上の点の座標との対応関係を示している。
　ＳＣＡＮｘ（ｘＬｉ，ｙＬｉ）は、時刻ｉの左カメラ画像上の測距点Ｐ０の座標（ｘＬｉ，ｙＬｉ）を中心とした小領域について時刻ｊの右カメラ画像上でテンプレートマッチングを行って得られた対応する点のｘ座標値である。また、ＳＣＡＮｙ（ｘＬｉ，ｙＬｉ）は、同様にテンプレートマッチングを行って得られた対応する点のｙ座標値である。
　図１６に示すように、座標算出部３１によって算出された時刻ｊの右カメラ画像上の測距点Ｐ０の座標（ｘＲｊ，ｙＲｊ）と、画像マッチング部３２によって探索された対応点の座標（ＳＣＡＮｘ（ｘＬｉ，ｙＬｉ），ＳＣＡＮｙ（ｘＬｉ，ｙＬｉ））との間には乖離が生じている。これは、航空機２の姿勢角（ω，φ，κ）が０以外の値であることに起因する。
　すなわち、飛行中の航空機２の姿勢を示す適切な姿勢角（ω，φ，κ）を設定して、図１６に示した座標を再計算すれば、上記座標（ｘＲｊ，ｙＲｊ）と上記座標（ＳＣＡＮｘ（ｘＬｉ，ｙＬｉ），ＳＣＡＮｙ（ｘＬｉ，ｙＬｉ））は一致する。
　従って、上記座標の差が最小化される姿勢角が航空機２の姿勢角の推定値となる。
　図１２の説明に戻る。以降では、姿勢推定部３３が、非線形最小二乗法に基づく手順で航空機２の姿勢角を推定する場合について説明する。
　姿勢推定部３３は、時刻ｊの右カメラ画像上の測距点Ｐ０の座標（ｘＲｊ，ｙＲｊ）と、画像マッチング部３２によって探索された対応点の座標（ＳＣＡＮｘ（ｘＬｉ，ｙＬｉ），ＳＣＡＮｙ（ｘＬｉ，ｙＬｉ））との差が小さくなる姿勢角の補正量を算出する（ステップＳＴ６ａ）。例えば、下記式（６）に示す観測方程式ｖｘ，ｖｙを用いる。

Figure JPOXMLDOC01-appb-I000007
　上記式（６）において、チルダω（～ω）、チルダφ（～φ）、チルダκ（～κ）は、未知数であるロール角ω、ピッチ角φ、ヨー角κの近似解である。δω、δφ、δκは近似解チルダω、チルダφ、チルダκに対する補正量である。
　また、∂Ｆｘ／∂ωは、Ｆｘのロール角ωでの偏微分、∂Ｆｘ／∂φは、Ｆｘのピッチ角φでの偏微分されたＦｘ、∂Ｆｘ／∂κは、Ｆｘのヨー角κでの偏微分である。これらの偏微分は、近似解チルダω、チルダφ、チルダκを代入して値が得られる係数である。
　同様に、∂Ｆｙ／∂ωは、Ｆｙのロール角ωでの偏微分、∂Ｆｙ／∂φは、Ｆｙのピッチ角φでの偏微分、∂Ｆｙ／∂κは、Ｆｙのヨー角κでの偏微分である。これらの偏微分も近似解チルダω、チルダφ、チルダκを代入して値が得られる係数である。
　チルダＦｘ（～Ｆｘ）は、Ｆｘに対してＳＣＡＮｘ（ｘＬ，ｙＬ）とｘＲの近似解を代入して得られる値であり、チルダＦｙ（～Ｆｙ）は、Ｆｙに対してＳＣＡＮｙ（ｘＬ，ｙＬ）とｙＲの近似解を代入して得られる値である。
　図１６に示したデータを用いる場合、時刻ｔ＝ｉの左カメラ画像と時刻ｔ＝ｉ＋１の右カメラ画像の各ペアにおいて４個ずつ、ｘとｙについての観測方程式ｖｘ，ｖｙが得られる。従って、観測方程式の数は３×４×２＝２４となる。このときの観測方程式の定数ベクトルを図１７に示す。
　次に、姿勢推定部３３は、観測方程式を６つの未知数のそれぞれについて偏微分する。例えば、時刻ｔ＝１におけるロール角ω、ピッチ角φ、ヨー角κで偏微分してから、時刻ｔ＝２におけるロール角ω、ピッチ角φ、ヨー角κで偏微分する。
　このようにして観測方程式ごとに算出された偏微分係数からなる２４×６の計画行列を図１８に示す。
　続いて、姿勢推定部３３は、この計画行列の転置行列と計画行列との積を算出する。図１８に示した計画行列を用いた算出結果を図１９に示す。
　さらに、姿勢推定部３３は、この計画行列の転置行列と図１７に示した定数ベクトルの積を算出する。この算出の結果を図２０に示す。
　この後、姿勢推定部３３は、図１９に示す行列から算定した逆行列と図２０に示すベクトルの積を算出する。この算出結果が、図２１に示す姿勢角の補正量（δω，δφ，δκ）である。
　なお、最初は、航空機２が傾き無く水平に飛行していると仮定して、姿勢角（ω，φ，κ）の初期値として（０，０，０）を設定しているので、上記補正量がそのまま姿勢角の近似解となる。
　姿勢推定部３３は、上記のように算出した補正量を従前の近似解に加算して補正し、補正後の近似解を姿勢角の設定値に決定する（ステップＳＴ７ａ）。このとき、予め定めた繰り返し回数が経過していなければ（ステップＳＴ８ａ；ＮＯ）、姿勢推定部３３は、上記と同様な座標計算を行うように座標算出部３１に指示する。
　これにより、座標算出部３１が、補正後の近似解を姿勢角の設定値としてステップＳＴ２ａからステップＳＴ４ａまでの処理を行い、画像マッチング部３２がステップＳＴ５ａの処理を行う。
　上記のような一連の処理を繰り返して算出される補正量（δω，δφ，δκ）によって、時刻ｊの右カメラ画像上の測距点Ｐ０の座標（ｘＲｊ，ｙＲｊ）と画像マッチング部３２により探索された対応点の座標（ＳＣＡＮｘ（ｘＬｉ，ｙＬｉ），ＳＣＡＮｙ（ｘＬｉ，ｙＬｉ））との差が小さくなっていく。
　上記一連の処理が予め定めた繰り返し回数だけ実行されて（ステップＳＴ８ａ；ＹＥＳ）、上記座標の差が最小となる補正量が得られると、姿勢推定部３３は、この補正量で補正した近似解を、最終的な姿勢角の推定結果として出力する（ステップＳＴ９ａ）。
　このようにして得られた時刻ｔ＝０．００～３．００の姿勢角の推定結果を図２２に示す。
　これまで、画像データのペアとして、時刻ｉの左カメラ画像と時刻ｊ（＝ｉ＋１）の右カメラ画像とのペアを用いる場合を示したが、時刻ｉの左カメラ画像と時刻ｉの右カメラ画像とのペアであってもよい。すなわち、この発明は、異なる撮影位置で撮影された画像データのペアであればよい。
　なお、異なる位置のカメラにより撮影された画像間で対応点を探索して観測対象までの距離および奥行き情報などの３次元情報を取得するステレオ画像処理には、固定ステレオと呼ばれる方法と、モーションステレオと呼ばれる方法がある。
　固定ステレオは、２つのカメラを間隔を空けて配置して撮影するものである。時刻ｉの左カメラ画像と時刻ｉの右カメラ画像のペアリングは、固定ステレオに相当する。
　モーションステレオでは、カメラを移動させて異なる撮影位置から撮影する。時刻ｉの左カメラ画像と時刻ｊ（＝ｉ＋１）の右カメラ画像のペアリングは、モーションステレオに相当する。
　また、これまでの説明では、左カメラ２０ａと右カメラ２０ｂを利用する構成を示したが、１台のカメラであってもよい。この場合、時刻ｉのカメラ画像と時刻ｊ（＝ｉ＋１）のカメラ画像のペアを用いることになる。
　さらに、これまで未知数が時刻ごとの姿勢角の３つのパラメータ（ω，φ，κ）である場合を示したが、航空機２の位置座標（Ｘ，Ｙ，Ｚ）を加えた６つのパラメータとしてもよく、カメラの焦点距離ｃなどの内部パラメータを含めてよい。
　以上のように、実施の形態１に係る航法装置３では、異なる撮影位置でそれぞれ撮影された画像間の対応する点の座標が航空機２の姿勢によってずれることに着目し、これらの座標の差が小さくなるように航空機２の姿勢を示すパラメータ（ω，φ，κ）の値を補正して航空機２の姿勢を推定する。これにより、ＩＭＵ、スタビライザーを使用しなくても航空機２の姿勢を推定することができる。
　また、測距点Ｐ０が撮影された画像データに加え、レーザ光の照射基準点２１ａから測距点Ｐ０までの距離ｌ、レーザ光の照射角度θ、レーザ光の照射基準点２１ａの３次元座標（Ｘ，Ｙ，Ｚ）を使用して航空機２の姿勢を推定するので、航空機２の姿勢を精度よく推定することが可能である。
　また、実施の形態１に係る航法装置３において、画像データのペアは、航空機２の飛行中に、時刻ｉに撮影された画像データとこの時刻ｉよりも進んだ時刻ｊに撮影された画像データである。このような画像データのペアを用いることにより、航空機２の姿勢に応じた画像上の被写体の変化を航空機２の姿勢推定に利用することができる。
　さらに、実施の形態１に係る航法装置３において、画像データのペアは、左カメラ２０ａと右カメラ２０ｂによって時刻ｉに撮影された画像データのペア、または、左カメラ２０ａおよび右カメラ２０ｂのうちの少なくとも一方によって時刻ｉと当該時刻ｉよりも進んだ時刻ｊとに撮影された画像データのペアである。
　このような画像データを用いても、航空機２の姿勢に応じた画像上の被写体の変化を航空機２の姿勢推定に利用することができる。
　さらに、実施の形態１に係る航法装置３において、ヨー角κ＝０を初期値として座標の計算を行うように実施したが、ヨー角κの近似解は、ＧＮＳＳ装置２２によって測定された時系列の３次元座標から得ることもできる。従って、ヨー角κの初期値としてＧＮＳＳ装置２２によって測定された時系列の３次元座標から算定された値を用いてもよい。
　さらに、実施の形態１に係る測量システム１は、航空機２に搭載されたメモリカード２３を備える。データ取得部３０は、メモリカード２３に記憶された距離データ、角度データ、座標データおよび画像データを読み出して取得する。
　このように航空機２の飛行中にメモリカード２３に蓄積されたデータを用いることで、航空機２の姿勢を飛行終了後に推定でき、推定された姿勢角を用いて測量結果を補正することもできる。
実施の形態２．
　図２３は、この発明の実施の形態２に係る測量システム１Ａの構成を示すブロック図である。測量システム１Ａは、航空機２Ａから地形を測量するシステムであり、航空機２Ａに搭載された左カメラ２０ａ、右カメラ２０ｂ、レーザ測距装置２１、ＧＮＳＳ装置２２および無線通信装置２４と、航法装置３とを備える。
　無線通信装置２４は、航空機２Ａの飛行中に得られた距離データ、角度データ、座標データおよび画像データを航法装置３へ送信する。
　航法装置３は、図２３に示すように、航空機２Ａとは別に設けられる。ただし、航法装置３を航空機２Ａに搭載してもよい。
　航法装置３のデータ取得部３０は、無線通信装置２４によって送信された距離データ、角度データ、座標データおよび画像データを受信して取得する。
　航法装置３は、このようにして取得された上記データに基づいて、実施の形態１と同様の処理で航空機２Ａの姿勢を推定する。
　以上のように、実施の形態２に係る測量システム１Ａは、航空機２に搭載された無線通信装置２４を備える。データ取得部３０は、無線通信装置２４によって送信された距離データ、角度データ、座標データおよび画像データを受信して取得する。
　このように無線通信装置２４から無線送信されたデータを用いることで、航空機２Ａの飛行中に姿勢を推定することができる。推定された姿勢角を用いて航空機２Ａの飛行中に測量結果を補正することも可能である。
　なお、これまでの説明では、この発明における移動体が航空機２のような飛行体である場合を示したが、これに限定されるものではない。例えば、この発明に係る航法装置は、モービルマッピングシステムに適用することができ、このシステムを搭載する車両が移動体となる。また、この発明に係る航法装置は、鉄道車両、船舶、ロボットを移動体とし、これらの姿勢を推定する装置として利用することも可能である。このような移動体においても、姿勢を示すパラメータは同様に移動体の姿勢角（ω，φ，κ）を用いることができ、場合によっては位置情報をパラメータに含めてもよい。
　なお、本発明はその発明の範囲内において、各実施の形態の自由な組み合わせあるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
　この発明に係る航法装置は、ＩＭＵ、スタビライザーを搭載しない構成で移動体の姿勢を精度よく推定することができるので、例えば、ＵＡＶの航法装置として好適である。
　１，１Ａ　測量システム、２，２Ａ　航空機、３　航法装置、２０ａ　左カメラ、２０ｂ　右カメラ、２０ｃ，２０ｄ　アーム、２１　レーザ測距装置、２１ａ　照射基準点、２２　ＧＮＳＳ装置、２３　メモリカード、２４　無線通信装置、３０　データ取得部、３１　座標算出部、３２　画像マッチング部、３３　姿勢推定部、１００　処理回路、１００ａ～１００ｃ　画像、１０１　ＣＰＵ、１０２　メモリ。

Claims (6)

  1. 　移動体に搭載されたレーザ測距装置により測定されたレーザ光の照射基準点から測距点までの距離を示す距離データと前記レーザ光の照射角度を示す角度データ、前記移動体に搭載された座標測定装置により測定された前記レーザ光の照射基準点の３次元座標を示す座標データおよび前記移動体に搭載された撮影装置により撮影された撮影対象に測距点を含む画像データを取得するデータ取得部と、
    　前記データ取得部により取得された前記距離データ、前記角度データ、前記座標データおよび前記移動体の姿勢を示すパラメータに基づいて、前記画像データの画像上の測距点の座標を算出する座標算出部と、
    　前記撮影装置により異なる撮影位置でそれぞれ撮影された前記画像データのペアの画像マッチングを行って、前記座標算出部により算出された前記ペアの一方の前記画像データの画像上の測距点の座標に対応する点を前記ペアのもう一方の前記画像データの画像から探索する画像マッチング部と、
    　前記座標算出部により算出された前記ペアのもう一方の前記画像データの画像上の測距点の座標と前記画像マッチング部により探索された前記対応する点の座標との差が小さくなるように前記移動体の姿勢を示すパラメータの値を補正して当該移動体の姿勢を推定する姿勢推定部と
    を備えたことを特徴とする航法装置。
  2. 　前記ペアは、前記移動体の移動中に、前記撮影装置により時刻ｉに撮影された前記画像データと前記時刻ｉよりも進んだ時刻ｊに撮影された前記画像データであることを特徴とする請求項１記載の航法装置。
  3. 　前記撮影装置は、前記移動体に搭載された第１の撮影部および第２の撮影部を備え、
    　前記ペアは、前記第１の撮影部と前記第２の撮影部により時刻ｉにそれぞれ撮影された前記画像データまたは前記第１の撮影部および前記第２の撮影部のうちの少なくとも一方により前記時刻ｉと当該時刻ｉよりも進んだ時刻ｊとにそれぞれ撮影された前記画像データであることを特徴とする請求項１記載の航法装置。
  4. 　移動体に搭載されて、レーザ光の照射基準点から測距点までの距離を示す距離データと前記レーザ光の照射角度を示す角度データを測定するレーザ測距装置と、
    　前記移動体に搭載されて、前記レーザ光の照射基準点の３次元座標を示す座標データを測定する座標測定装置と、
    　前記移動体に搭載されて、撮影対象に測距点を含む画像データを撮影する撮影装置と、
    　前記距離データ、前記角度データ、前記座標データおよび前記画像データを取得するデータ取得部と、前記データ取得部により取得された前記距離データ、前記角度データ、前記座標データおよび前記移動体の姿勢を示すパラメータに基づいて、前記画像データの画像上の測距点の座標を算出する座標算出部と、前記撮影装置により異なる撮影位置でそれぞれ撮影された前記画像データのペアの画像マッチングを行って、前記座標算出部により算出された前記ペアの一方の前記画像データの画像上の測距点の座標に対応する点を前記ペアのもう一方の前記画像データの画像から探索する画像マッチング部と、前記座標算出部により算出された前記ペアのもう一方の前記画像データの画像上の測距点の座標と前記画像マッチング部により探索された前記対応する点の座標との差が小さくなるように前記移動体の姿勢を示すパラメータの値を補正して当該移動体の姿勢を推定する姿勢推定部とを有する航法装置と
    を備えたことを特徴とする測量システム。
  5. 　前記移動体に搭載され、前記距離データ、前記角度データ、前記座標データおよび前記画像データを記憶する記憶装置を備え、
    　前記データ取得部は、前記記憶装置に記憶された前記距離データ、前記角度データ、前記座標データおよび前記画像データを読み出して取得することを特徴とする請求項４記載の測量システム。
  6. 　前記移動体に搭載され、前記距離データ、前記角度データ、前記座標データおよび前記画像データを送信する無線通信装置を備え、
    　前記データ取得部は、前記無線通信装置により送信された前記距離データ、前記角度データ、前記座標データおよび前記画像データを受信して取得することを特徴とする請求項４記載の測量システム。
PCT/JP2015/075598 2015-09-09 2015-09-09 航法装置および測量システム WO2017042907A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
PCT/JP2015/075598 WO2017042907A1 (ja) 2015-09-09 2015-09-09 航法装置および測量システム
US15/561,882 US10222210B2 (en) 2015-09-09 2015-09-09 Navigation system and survey system
JP2016513572A JP6029794B1 (ja) 2015-09-09 2015-09-09 航法装置および測量システム
EP15903572.4A EP3348963B1 (en) 2015-09-09 2015-09-09 Navigation system and survey system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2015/075598 WO2017042907A1 (ja) 2015-09-09 2015-09-09 航法装置および測量システム

Publications (1)

Publication Number Publication Date
WO2017042907A1 true WO2017042907A1 (ja) 2017-03-16

Family

ID=57358823

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2015/075598 WO2017042907A1 (ja) 2015-09-09 2015-09-09 航法装置および測量システム

Country Status (4)

Country Link
US (1) US10222210B2 (ja)
EP (1) EP3348963B1 (ja)
JP (1) JP6029794B1 (ja)
WO (1) WO2017042907A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022118513A1 (ja) * 2020-12-02 2022-06-09 三菱電機株式会社 位置姿勢算出装置、位置姿勢算出方法及び測量装置

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6861592B2 (ja) * 2017-07-14 2021-04-21 三菱電機株式会社 データ間引き装置、測量装置、測量システム及びデータ間引き方法
CN107146447A (zh) * 2017-07-16 2017-09-08 汤庆佳 一种基于无人机的智能车辆管理***及其方法
CN107478195A (zh) * 2017-09-15 2017-12-15 哈尔滨工程大学 一种基于光学的空间物体姿态测量装置及其测量方法
CN110268710B (zh) * 2018-01-07 2022-01-04 深圳市大疆创新科技有限公司 图像数据处理方法、设备、平台及存储介质
JP7188687B2 (ja) * 2018-07-17 2022-12-13 エアロセンス株式会社 情報処理方法、プログラム、および情報処理システム
CN109345589A (zh) * 2018-09-11 2019-02-15 百度在线网络技术（北京）有限公司 基于自动驾驶车辆的位置检测方法、装置、设备及介质
GB201818357D0 (en) * 2018-11-12 2018-12-26 Forsberg Services Ltd Locating system
JP7241517B2 (ja) * 2018-12-04 2023-03-17 三菱電機株式会社 航法装置、航法パラメータ計算方法およびプログラム
CN110672117B (zh) * 2019-11-04 2022-02-18 中国人民解放军空军工程大学 基于单观察哨数字望远镜的小航路捷径运动目标航迹获取方法
CN112099521B (zh) * 2020-10-09 2022-05-17 北京邮电大学 一种无人机路径规划方法及装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05118850A (ja) * 1991-10-25 1993-05-14 Shimizu Corp 模型ヘリコプターを用いる３次元測量システム
JP2001133256A (ja) * 1999-11-05 2001-05-18 Asia Air Survey Co Ltd 空中写真の位置及び姿勢の計算方法
JP2007278844A (ja) * 2006-04-06 2007-10-25 Topcon Corp 画像処理装置及びその処理方法
JP2013187862A (ja) * 2012-03-09 2013-09-19 Topcon Corp 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
JP2014145762A (ja) * 2013-01-07 2014-08-14 Amuse Oneself Inc 制御装置、測量システム、プログラム及び記録媒体並びに計測方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4335520A (en) * 1980-09-22 1982-06-22 The United States Of America As Represented By The Secretary Of The Navy Survey spar system for precision offshore seafloor surveys
JPH1151611A (ja) * 1997-07-31 1999-02-26 Tokyo Electric Power Co Inc:The 認識対象物体の位置姿勢認識装置および位置姿勢認識方法
EP1934632A1 (en) * 2005-10-13 2008-06-25 Atlantic Inertial Systems Limited Terrain mapping
JP2007240506A (ja) * 2006-03-06 2007-09-20 Giyourin Cho ３次元形状と３次元地形計測法
WO2009081177A2 (en) * 2007-12-21 2009-07-02 Bae Systems Plc Apparatus and method for landing a rotary wing aircraft
US10212687B2 (en) * 2010-09-30 2019-02-19 Echo Ridge Llc System and method for robust navigation and geolocation using measurements of opportunity
US9684076B1 (en) * 2013-03-15 2017-06-20 Daniel Feldkhun Frequency multiplexed ranging
US10643351B2 (en) * 2013-03-20 2020-05-05 Trimble Inc. Indoor navigation via multi beam laser projection
US9911189B1 (en) * 2016-06-30 2018-03-06 Kitty Hawk Corporation Navigation based on downward facing sensors

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05118850A (ja) * 1991-10-25 1993-05-14 Shimizu Corp 模型ヘリコプターを用いる３次元測量システム
JP2001133256A (ja) * 1999-11-05 2001-05-18 Asia Air Survey Co Ltd 空中写真の位置及び姿勢の計算方法
JP2007278844A (ja) * 2006-04-06 2007-10-25 Topcon Corp 画像処理装置及びその処理方法
JP2013187862A (ja) * 2012-03-09 2013-09-19 Topcon Corp 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
JP2014145762A (ja) * 2013-01-07 2014-08-14 Amuse Oneself Inc 制御装置、測量システム、プログラム及び記録媒体並びに計測方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP3348963A4 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022118513A1 (ja) * 2020-12-02 2022-06-09 三菱電機株式会社 位置姿勢算出装置、位置姿勢算出方法及び測量装置

Also Published As

Publication number Publication date
EP3348963A1 (en) 2018-07-18
JP6029794B1 (ja) 2016-11-24
EP3348963A4 (en) 2019-05-29
US20180120107A1 (en) 2018-05-03
JPWO2017042907A1 (ja) 2017-09-07
EP3348963B1 (en) 2020-05-20
US10222210B2 (en) 2019-03-05

Similar Documents

Publication Publication Date Title
JP6029794B1 (ja) 航法装置および測量システム
CN109461190B (zh) 测量数据处理装置及测量数据处理方法
US10970873B2 (en) Method and device to determine the camera position and angle
US11099030B2 (en) Attitude estimation apparatus, attitude estimation method, and observation system
US20190370983A1 (en) System and method for real-time location tracking of a drone
JP5134784B2 (ja) 空中写真測量方法
KR101614338B1 (ko) 카메라 캘리브레이션 방법, 카메라 캘리브레이션 프로그램을 기록한 컴퓨터 판독가능한 기록매체 및 카메라 캘리브레이션 장치
JP6988197B2 (ja) 制御装置、飛行体、および制御プログラム
JP6138326B1 (ja) 移動体、移動体の制御方法、移動体を制御するプログラム、制御システム、及び情報処理装置
JP2008186145A (ja) 空撮画像処理装置および空撮画像処理方法
CN112005077A (zh) 无人航空器的设置台、测量方法、测量装置、测量***和程序
JP3808833B2 (ja) 空中写真測量方法
JP2016223934A (ja) 位置補正システム、位置補正方法、および位置補正プログラム
CN113340272A (zh) 一种基于无人机微群的地面目标实时定位方法
Skaloud et al. Mapping with MAV: experimental study on the contribution of absolute and relative aerial position control
JP6305501B2 (ja) キャリブレーション方法、プログラムおよびコンピュータ
US20200116482A1 (en) Data thinning device, surveying device, surveying system, and data thinning method
Jia et al. Modeling image motion in airborne three-line-array (TLA) push-broom cameras
JP5409451B2 (ja) ３次元変化検出装置
JPH0524591A (ja) 垂直離着陸航空機の機***置測定方法
CN115118876B (zh) 拍摄参数的确定方法、装置及计算机可读存储介质
JP6974290B2 (ja) 位置推定装置、位置推定方法、プログラム、及び記録媒体
Ruzgienė Analysis of camera orientation variation in airborne photogrammetry: images under tilt (roll‑pitch‑yaw) angles
JP6062020B2 (ja) 指定点投影方法、プログラムおよびコンピュータ
JP2024021131A (ja) 姿勢角補正装置および姿勢角補正方法

Legal Events

Date Code Title Description
ENP Entry into the national phase

Ref document number: 2016513572

Country of ref document: JP

Kind code of ref document: A

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 15903572

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 15561882

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

WWE Wipo information: entry into national phase

Ref document number: 2015903572

Country of ref document: EP