WO2021006026A1 - 自己位置特定方法 - Google Patents
自己位置特定方法 Download PDFInfo
- Publication number
- WO2021006026A1 WO2021006026A1 PCT/JP2020/024519 JP2020024519W WO2021006026A1 WO 2021006026 A1 WO2021006026 A1 WO 2021006026A1 JP 2020024519 W JP2020024519 W JP 2020024519W WO 2021006026 A1 WO2021006026 A1 WO 2021006026A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- moving body
- self
- projected
- image data
- plane
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 238000012545 processing Methods 0.000 claims abstract description 21
- 238000004458 analytical method Methods 0.000 claims abstract description 13
- 238000003384 imaging method Methods 0.000 claims abstract description 13
- 230000001678 irradiating effect Effects 0.000 claims abstract description 5
- 238000004364 calculation method Methods 0.000 claims description 21
- 238000006243 chemical reaction Methods 0.000 claims description 16
- 239000013598 vector Substances 0.000 claims description 9
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 238000013507 mapping Methods 0.000 claims description 7
- 239000000284 extract Substances 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 238000001514 detection method Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 4
- 238000001914 filtration Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000007689 inspection Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- RWSOTUBLDIXVET-UHFFFAOYSA-N Dihydrogen sulfide Chemical compound S RWSOTUBLDIXVET-UHFFFAOYSA-N 0.000 description 1
- QVGXLLKOCUKJST-UHFFFAOYSA-N atomic oxygen Chemical compound [O] QVGXLLKOCUKJST-UHFFFAOYSA-N 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000006866 deterioration Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000029087 digestion Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 229910000037 hydrogen sulfide Inorganic materials 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 239000001301 oxygen Substances 0.000 description 1
- 229910052760 oxygen Inorganic materials 0.000 description 1
- 238000000746 purification Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000010865 sewage Substances 0.000 description 1
- 239000010802 sludge Substances 0.000 description 1
- 238000001931 thermography Methods 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- 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
-
- 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/26—Measuring arrangements characterised by the use of optical techniques for measuring angles or tapers; for testing the alignment of axes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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/02—Control of position or course in two dimensions
-
- 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/10—Simultaneous control of position or course in three dimensions
-
- 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/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
Definitions
- the present invention relates to a method of identifying the self-position of a moving body by measuring the distance between the moving body and an obstacle around the moving body and ensuring the safety of the moving body during flight.
- a drone is an aircraft that can fly unmanned by remote control or automatic control, and is also called a multicopter.
- the applications are becoming more widespread, ranging from practical applications such as disaster relief and research on the natural environment in harsh environments for humans to entertaining applications such as aerial racing competitions.
- Patent Document 1 is obtained by imaging a structure with a distance measuring device for measuring the distance to the structure, a moving vector calculating device for calculating the moving direction and moving amount of the drone, and a time lag.
- the image information and the position detection system that uses the information are described.
- This position detection system can detect the position of the flying object by repeatedly calculating the movement direction and the movement amount of the drone.
- the position detection system described in Patent Document 1 calculates the distance to the structure and the amount of movement based on the feature points of the predetermined structure, the calculation accuracy is different for each shape of the structure. There is concern that it will be different. Further, the position detection system described in Patent Document 1 may not have sufficient features for correct matching or may provide similar features that provide erroneous data, depending on the environment.
- the present invention has been made in view of the above-mentioned actual conditions, and provides a self-positioning method capable of specifying the position of a moving body with stable accuracy regardless of the shape of the structure. Make it an issue.
- the present invention is a self-positioning method for a moving body including a laser light output unit for outputting laser light, a photographing unit, and a processing unit.
- the posture of the moving body and the distance from the wall surface are specified based on the shape of the projected light, it is possible to specify the position of the moving body with stable accuracy regardless of the shape of the structure. Become.
- the shape of the projected light is a figure in which three or more intersections are formed by three or more straight lines.
- the analysis step includes a conversion step of formulating the shape of the projected light and a conversion step.
- An associating step of associating the intersection with two-dimensional coordinates The calculation process for calculating the equation of a plane of the projected light and It includes a specific step of specifying the posture of the moving body and the distance from the wall surface based on the normal vector of the three-dimensional plane shown by the equation of a plane.
- the mapping by the matching step is determined based on the combinatorial optimization algorithm.
- the calculation step includes a distance measuring step of measuring a distance between the moving body and the wall surface by using the laser light output unit and the photographing unit and using the principle of triangulation. ..
- the calculation step uses outlier removal by RANSAC to remove outliers included in the coefficients of the equation of a plane using a plurality of equations of a plane based on a plurality of projected lights that are continuous over time. Includes steps.
- the analysis step comprises a filter step of extracting the projected light from the image data.
- a point at which the sign of the pixel value of the image data changes is extracted as an edge.
- FIGS. 1 to 9 the self-positioning method according to the embodiment of the present invention will be described with reference to FIGS. 1 to 9.
- a case where the self-positioning method is carried out by using the self-positioning device will be described in particular.
- the embodiments shown below are examples of the present invention, and the present invention is not limited to the following embodiments.
- a case where an air vehicle moving in the air is used as the moving body will be described.
- the configuration, operation, and the like of the self-positioning device will be described, but a method, a system, a computer program, a recording medium, and the like having the same configuration can also exert the same effects.
- the program may be stored in a recording medium. Using this recording medium, for example, the program can be installed on a computer.
- the recording medium in which the program is stored may be a non-transient recording medium such as a CD-ROM.
- FIG. 1 is a diagram showing an example of a functional block configuration of the self-positioning device A according to the present embodiment.
- the self-positioning device A includes a laser light output unit A1 that outputs laser light L (see FIG. 4), a photographing unit A2 that acquires image data, and a processing unit A3 that processes image data.
- the processing unit A3 functions as a filter means 101, a conversion means 102, an association means 103, a calculation means 104, a specific means 105, and an operation instruction means 106.
- the filter means 101 extracts the projected light from the image data.
- the conversion means 102 formulates the shape of the projected light.
- the associating means 103 associates the intersections included in one projected light with the intersections included in the other projected light.
- the calculation means 104 calculates the equation of a plane of the projected light.
- the specifying means 105 specifies the posture of the moving body 1 and the distance from the wall surface based on the normal vector of the three-dimensional plane indicated by the equation of a plane.
- the operation instruction means 106 gives an irradiation instruction to the laser light output unit A1 and an imaging instruction to the photographing unit A2.
- FIG. 2 is a schematic perspective view showing a moving body 1 provided with a self-positioning device A.
- the moving body 1 includes a moving body main body 11 that constitutes a body portion, and a plurality of rotary wing aircraft 12 that are radially arranged on the upper portion of the moving body main body 11. Further, a pair of legs 13 and 13 are provided at the lower part of the moving body main body 11. Further, in the space between the pair of legs 13 and 13, an inspection unit 14 equipped with a camera for inspecting external cracks and the like and an infrared thermography for detecting internal defects is provided. ..
- one laser light output unit A1 and one imaging unit A2 are provided on the upper surface and the side surface of the moving body main body 11. Each laser light output unit A1 and each imaging unit A2 are controlled by one processing unit A3. In each self-positioning device A, the output direction of the laser light L by the laser light output unit A1 and the shooting direction of the photographing unit A2 are configured to be substantially the same.
- FIG. 3 is a hardware configuration diagram of the mobile body 11.
- the mobile body 11 controls a safety management device 2 that controls the flight availability state of the mobile body 1, a battery 3 (power source) that supplies power used by the mobile body 1, and controls the flight operation of the mobile body 1.
- the main control unit 4 to be performed, the servomotor 5 for driving the rotary wing machine 12, the motor controller 6 for adjusting the amount of power supplied to the servomotor 5 based on the signal from the main control unit 4, and the operation operated by the operator.
- a wireless receiver 7 that receives an operation signal from a terminal, a telemetry communication unit 8 that communicates between an external terminal such as an operation terminal or another terminal and a mobile body 1, and state information such as the current location and speed of the mobile body 1. It is provided with a measuring device 9 for acquiring the above.
- the general mobile body 1 is not provided with the safety management device 2, and the main control unit 4 and the motor controller 6 are directly supplied with electric power from the battery 3.
- the power supply to the telemetry communication unit 8, the measuring device 9, and the like may be performed directly from the battery 3, or may be performed via the main control unit 4 as in the present embodiment. Depending on the specifications of the main control unit 4, power may be supplied from the motor controller 6.
- the moving body 1 flies inside a substantially square tubular tube R.
- the operator of the moving body 1 registers the height information and the width information of the pipe R in the moving body 1 in advance.
- the moving body 1 is arranged by the operator at substantially the center of the conduit R.
- the processing unit A3 gives an irradiation instruction to the laser light output unit A1 by the operation instruction means 106.
- the irradiation step of irradiating the side wall surface W1 and the upper wall surface W2 of the pipe R with the laser light L by the laser light output unit A1 is performed.
- Various wavelengths such as infrared rays and RGB are used as the laser light L.
- FIG. 4A is a view in which the upper wall surface W2 of the pipe R is omitted, and the moving body 1 and the pipe R are viewed from above
- FIG. 4B is a view in FIG. 4A. It is XX'line sectional view which showed the upper wall surface W2.
- the laser light L applied to the side wall surface W1 and the upper wall surface W2 is projected as lattice-shaped projected lights T1 and T2 on the wall surfaces W1 and W2, respectively.
- the projected lights T1 and T2 are formed in a substantially square shape by six straight lines to form nine intersections.
- the shape of the projected lights T1 and T2 does not necessarily have to be a grid shape, and if it is a figure in which three or more intersections are formed by three or more straight lines, it is a polygonal shape such as a triangle, a quadrangle, or a hexagon. It may be.
- the diffractive optical element is preferably used.
- DOE diffractive optical element
- the processing unit A3 gives an imaging instruction to the photographing unit A2 by the operation instructing means 106.
- the photographing unit A2 performs a photographing process in which the projected lights T1 and T2 are photographed and image data is acquired.
- a camera having a viewing angle (FoV) of 90 degrees or more is preferably used in order to acquire image data with high accuracy.
- the photographing step is continuously performed during the flight of the moving body 1 at a frame rate of, for example, 30 fps.
- the processing unit A3 performs an analysis step of analyzing the acquired image data.
- the filter step 101 performs a filter step of extracting projected lights T1 and T2 from the acquired image data. More specifically, in the filtering step, the processing unit A3 examines the pixel value of each pixel of the image subjected to the LoG filter using LoG (Laplacian of Gaussian), and the point at which the sign of the pixel value changes (zero intersection). Is extracted as edge information.
- LoG is expressed by the following equation.
- r is the distance from the pixel of interest and ⁇ is the degree of smoothing of the filter.
- the filter used in the filter means 101 is not limited to the LoG filter, and a DoG filter by DoG (Difference of Gaussian), which is an approximation of LoG, may be used.
- the conversion means 102 performs a conversion step of formulating the shapes of the projected lights T1 and T2. More specifically, in the conversion step, the processing unit A3 extracts a straight line constituting the projected light by performing a Hough transform on the edge information extracted by the filter means 101, and based on the extracted straight line. , Extract the intersection.
- FIG. 5A shows the projected light T1 on the side wall surface W1 when the moving body 1 is flying at the position p1 in FIG. 4A.
- the conversion step does not necessarily have to be a Hough transform, and the same extraction result can be output even with another line extraction algorithm.
- the two-dimensional coordinates of the projected lights T1 and T2 are determined.
- the conversion step is continuously performed during the flight of the moving body 1. That is, in the present embodiment, for example, with respect to the projected light with respect to the side wall surface W1, the two-dimensional coordinates of 30 patterns of projected light are determined per second.
- the associating means 103 performs an associative step of associating the intersections of the projected lights at each flight position of the moving body 1.
- the change in the shape of the projected light T1 on the side wall surface W1 when the moving body 1 moves from the position p1 to the position p2 is considered. That is, when the moving body 1 moves from the position p1 to the position p2 in FIG. 4 (a), the projected light T1 changes from the shape shown in FIG. 5 (a) to the shape shown in FIG. 5 (b) due to the bending of the pipe R.
- the projected light of FIG. 5B is referred to as a projected light T1'.
- the processing unit A3 determines the association of each intersection based on the combinatorial optimization algorithm in the association step. That is, since the act of finding the combination of correspondences can be understood as an allocation problem, the optimum correspondence can be found by, for example, an algorithm such as the Hungarian method.
- the left side represents the intersection points Vp0 to Vp8 of the projected light T1
- the right side represents the intersection points Vp0'to Vp8'of the projected light T1'.
- Vp2' is most likely to be associated with Vp2 (cost 5), subsequently associated with Vp1 (cost 10), and least likely to be associated with Vp6. (Cost 20).
- the Hungarian method is performed in the following four steps.
- Step 1) Subtract the minimum value of the row from each element of each row, and then further subtract the minimum value of the column from each element of each column.
- Step 2) Determine whether 0 can be selected one by one from each row and column. If it can be selected, the set of 0 coordinates will be the allocation plan. If you cannot choose, proceed to step 3.
- Step 3) Cover all 0s with as few vertical or horizontal lines as possible.
- Step 4) Subtract those minimum values from the elements that are not erased by the lines, and add the values to the elements where the vertical and horizontal lines intersect. Return to step 2.
- the calculation means 104 performs a calculation step of calculating the equation of a plane of the projected light. More specifically, the calculation step includes a distance measuring step and an outlier removing step.
- the distance measuring process uses the laser light output unit A1 and the photographing unit A2 in the distance measuring process
- the processing unit A3 uses the laser light output unit A1 and the photographing unit A2 to move between the moving body 1 and each intersection according to the principle of triangulation.
- the distance is measured and the coordinates of each intersection in the z-axis direction are calculated.
- FIG. 7 is a diagram schematically showing the positional relationship between the laser light output unit A1, the photographing unit A2, and the predetermined intersection Vpi'.
- Z is the distance from the predetermined moving body 1
- B is the distance between the lenses of the laser light output unit A1 and the photographing unit A2
- C is the difference in the position of the center of gravity obtained from the left and right images
- f is the focal length.
- the distance Z can be obtained by the following formula.
- the distance measuring process is performed using the laser light output unit A1 as a virtual camera. That is, the projected light output by the laser light output unit A1 is treated as an image captured by the camera of the laser light output unit A1 viewpoint, and triangulation is performed with the image captured by the photographing unit A2.
- the laser light output unit A1 outputs a simple pattern of projected light onto a flat surface. Then, matching is performed between the projected light projected from the laser light output unit A1 and the pattern projected on the plane imaged by the photographing unit A2, and mapping in pixel units is obtained. By this mapping, the image taken by the photographing unit A2 can be converted into the image taken by the virtual camera of the laser light output unit A1 viewpoint.
- Camera calibration is the work of finding the internal and external parameters of the camera in the image of each viewpoint.
- Internal parameters include the focal length, the center of the image, the relationship between the size of the image pickup element and the image size, and represent the relationship between the three-dimensional coordinates of the camera coordinate system and the two-dimensional coordinates of the digital image coordinate system.
- External parameters include camera rotation and translation, and represent the positional relationship of the camera in the three-dimensional coordinates of the world coordinate system. These parameters can be estimated from the corresponding point information as to which point in the image the feature point corresponds to in another image.
- camera calibration is the work of extracting feature points in an image, taking correspondence between the images, and then finding a camera parameter such that all corresponding line-of-sight vectors intersect at one point in three dimensions. is there.
- the camera calibration is performed by the following equation using the projection matrix Pm.
- the projection matrix Pm is the camera's internal parameter matrix A and rotation matrix R. By using, it can be expressed as follows.
- the outlier removing step is included in the outlier removing step by RANSAC using a plurality of equations of a plane based on a plurality of projected lights that are continuous over time.
- Outliers are coefficients that are extremely large or extremely small compared to the coefficients of other equations of a plane, and noise when averaging multiple equations of a plane to derive an approximate equation of a plane. It is caused by such causes.
- Equation 3 if the leftmost matrix is A, the rightmost matrix B, and the coefficient matrix composed of (a, b, c) is x, it can be expressed as follows.
- the outlier removal step by RANSAC is performed in the following five steps.
- Step 1 Randomly select more "small” samples from the data set than are needed to determine the model. The number of samples is preferably 5.
- Step 2 A temporary model is derived from the obtained "small number” of samples by the method of least squares or the like.
- Step 3) When the temporary model is applied to the data and the number of inlier values included in the temporary model is larger than a predetermined value set in advance, it is added to the "correct model candidate".
- Step 4) (Step 2) and (Step 3) are repeated.
- Step 5) Among the obtained "correct model candidates", the equation of a plane that best matches the data is set as an approximate equation of a plane of the projected light within a specific time.
- the number of iterations N ⁇ can be expressed by the following equation. By repeating the number of iterations N ⁇ or more, a mathematical model that is not affected by outliers can be obtained with a probability of (1- ⁇ ) or more.
- ⁇ is the probability of acquiring an "incorrect model candidate" by calculation
- ⁇ is the probability of acquiring an inlier value by calculation
- n is the number of samples to be selected.
- a threshold value is set for outliers that hinder the derivation of an approximate equation of a plane.
- the threshold value is the linear distance from the intersection of the planes serving as a tentative model to the intersection of the corresponding specific planes, and the intersections exceeding this threshold value are the outliers.
- the threshold value is preferably 3 cm.
- the specifying means 105 performs a specific step of specifying the posture of the moving body 1 and the distances from the wall surfaces W1 and W2 based on the normal vector of the three-dimensional plane indicated by the approximate equation of a plane.
- FIG. 8 is a schematic diagram showing the positional relationship between the approximate plane F of the projected light T1'and the moving body 1.
- the processing unit A3 first starts from the approximate plane F. Is extracted, and from the outer product of these two vectors, Is derived. Next, the processing unit A3 extracts one arbitrary point q from the approximate plane F and connects the point q and the moving body 1. Is derived.
- the distance D from the approximate plane F to the moving body 1 can be expressed as follows.
- the processing unit A3 And each of the x-axis, y-axis, and z-axis in the local coordinates of the moving body 1. Is used to specify the posture of the moving body 1. That is, the processing unit A3 derives the rotation angles Tx, Ty, and Tz of the pitch roll yaw in the local coordinates of the moving body 1 from the following equation.
- the pitch rotation angle Tx is derived
- the roll rotation angle Ty is derived
- the yaw rotation angle Tz is derived.
- steps after the associating step are always performed following the conversion step if the moving body 1 is in flight, not only when the shape of the projected light is changed. Further, a series of processing steps from the filter step to the specific step is performed for each frame.
- the moving body 1 can travel while maintaining a constant distance to the wall surface. That is, the operator only performs a forward or backward operation on the moving body 1, and the moving body 1 can safely travel inside the pipe R.
- the laser beam L is irradiated to the wall surfaces W1 and W2 by the irradiation step (step S1).
- step S2 the laser light L irradiated on the wall surfaces W1 and W2 is photographed, and the image data including the projected lights S1 and S2 is acquired (step S2).
- the image data is analyzed by the analysis process.
- the projected lights T1 and T2 are extracted from the image data by the filter step (step S3).
- step S4 the shape of the projected light is mathematically expressed by the conversion step (step S4).
- step S5 the intersections of the projected lights at each flight position of the moving body 1 are associated.
- the laser light output unit A1 and the photographing unit A2 are used in the distance measurement process, and the distance measurement between the moving body 1 and the wall surfaces W1 and W2 is performed by the principle of triangulation. (Step S6).
- the outlier removal step removes the outliers included in the equation of a plane by RANSAC using a plurality of equations of a plane based on a plurality of projected lights that are continuous over time (step S7). ..
- step S8 the posture of the moving body 1 and the distances from the wall surfaces W1 and W2 are specified based on the normal vector of the three-dimensional plane indicated by the equation of a plane (step S8).
- the processing unit A3 determines the posture of the moving body 1 and the distance from the wall surfaces W1 and W2 based on the shapes of the projected lights T1 and T2 included in the image data. Therefore, it is possible to specify the position of the moving body 1 with stable accuracy.
- the shapes of the projected lights T1 and T2 are lattice shapes, and the processing unit A3 moves based on the normal vector of the three-dimensional plane represented by the approximate equation of a plane based on the shapes of the projected lights T1 and T2.
- mapping by the mapping process is determined based on the combinatorial optimization algorithm, it is possible to obtain an accurate equation of a plane even for the projected light whose intersection is unclear.
- the calculation step includes a distance measuring step in which the laser light output unit A1 and the photographing unit A2 are used to measure the distance between the moving body 1 and the wall surfaces W1 and W2 according to the principle of triangulation. It is possible to accurately specify the position of the body 1 in three dimensions.
- the calculation step includes an outlier removal step of removing outliers included in the coefficients of the equation of a plane by RANSAC using a plurality of plane equations based on a plurality of projected lights that are continuous over time. It is possible to calculate an approximate equation of a plane based on the shapes of the projected lights T1 and T2 with higher accuracy.
- the analysis step includes a filter step of extracting projected lights T1 and T2 from the image data.
- the filtering process extracts the points at which the sign of the pixel value of the image data changes as an edge, so that the intersection of the projected light can be extracted with higher accuracy.
- the case where an air vehicle moving in the air is used as the moving body is shown, but the case is not limited to this, and naturally it can be applied to robotics traveling on the ground.
- one laser light output unit A1 and one imaging unit A2 are provided on the side surface and the upper surface of the moving body 1 (two in total), but the installation mode is not limited to this. Three or more may be provided depending on the purpose of the survey, the survey environment, and the like.
- the operator can preliminarily attach the pipe R to the moving body 1. Only the height information of is registered. At this time, the position of the moving body 1 in the width direction is calculated by comparing the image data acquired from the photographing unit A2 on each side surface. Further, when the laser light output unit A1 and the imaging unit A2 are provided on the upper surface, the bottom surface, the left side surface and the right side surface of the moving body 1 one by one (four in total), the operator can set the height of the conduit R in advance. There is no need to register information or width information.
- the present invention can identify the self-position of the moving body with high accuracy, it is difficult to ensure the safety of the worker (a place where oxygen is thin, a place where the amount of water is large, and sudden rainfall). It is possible to make the mobile body safely carry out the investigation in the area where the occurrence of hydrogen sulfide, etc. Further, according to the present invention, it is possible to reduce the survey cost by improving the running (navigation) speed as compared with the conventional submersible visual survey and the self-propelled TV camera vehicle, and also to carry in and out by reducing the weight of the device. And can be easily moved.
- the present invention can be expected to be applied to inspections of sludge pits and digestion tanks of sewage treatment plants, and infrastructures (water purification plants, headraces, etc.) of other businesses, in addition to sewerage pipelines. That is, the present invention has extremely high industrial applicability.
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Optics & Photonics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
構造物の形状によらず、安定した精度で移動体の位置を特定することが可能な、自己位置特定方法を提供する。 レーザー光Lを出力するレーザー光出力部A1と、撮影部A2と、処理部A3と、を備えた移動体1に対する自己位置特定方法であって、レーザー光Lを壁面W1、W2に照射する照射工程と、壁面W1、W2に投影されたレーザー光Lの投影光T1、T2が含まれる画像データを取得する撮影工程と、画像データを解析する解析工程と、を有し、解析工程は、画像データに含まれる投影光T1、T2の形状に基づき、移動体1の姿勢及び壁面W1、W2からの距離を特定する。
Description
本発明は、移動体と移動体周辺の障害物との距離を測定することで移動体の自己位置を特定し、移動体の飛行時の安全性を確保する方法に係るものである。
近年、スマートフォンやインターネットといったテクノロジーの発展を背景に、ドローンが世界的に普及している。ドローンとは、遠隔操作や自動制御によって無人で飛行できる航空機であり、マルチコプターとも呼ばれる。利用用途も幅広くなってきており、人間にとって苛酷な環境での災害救助や自然環境のリサーチといった実用的なものから、空中でのレース競技などエンターテイメント性のある活用方法まで様々である。
しかし無人飛行であるがゆえに、GPS受信環境の悪化による飛行位置の安定性の低下が危惧されている。
このような問題点を解決するために、ドローン自体に搭載されたカメラ等撮像機器により得られた2次元画像データを用いることにより、GPS信号を用いることなく、ドローンの自己位置を特定することができるシステムが提案されている。
例えば、特許文献1には、構造物との距離を測定する測距装置と、ドローンの移動方向及び移動量を算出する移動ベクトル算出装置と、時間をずらして構造物を撮像することによって得られた画像情報と、を利用する位置検出システムが記載されている。
この位置検出システムは、ドローンの移動方向及び移動量の算出を繰り返し行うことで、飛行体の位置を検出することができる。
この位置検出システムは、ドローンの移動方向及び移動量の算出を繰り返し行うことで、飛行体の位置を検出することができる。
しかしながら、特許文献1に記載の位置検出システムは、所定の構造物の特徴点に基づき、この構造物との距離や移動量の算出を行うため、構造物の形状毎に、これらの算出精度が異なったものとなってしまうことが懸念される。
また、特許文献1に記載された位置検出システムでは、環境によっては、正しくマッチングするための十分な特徴が得られない場合や、誤ったデータを提供する類似した特徴を提供する場合がある。
また、特許文献1に記載された位置検出システムでは、環境によっては、正しくマッチングするための十分な特徴が得られない場合や、誤ったデータを提供する類似した特徴を提供する場合がある。
本発明は上記のような実状に鑑みてなされたものであり、構造物の形状によらず、安定した精度で移動体の位置を特定することが可能な、自己位置特定方法を提供することを課題とする。
上記課題を解決するために、本発明は、レーザー光を出力するレーザー光出力部と、撮影部と、処理部と、を備えた移動体に対する自己位置特定方法であって、
前記レーザー光を壁面に照射する照射工程と、
前記壁面に投影された前記レーザー光の投影光が含まれる画像データを取得する撮像工程と、
前記画像データを解析する解析工程と、を有し、
前記解析工程は、前記画像データに含まれる前記投影光の形状に基づき、前記移動体の姿勢及び前記壁面からの距離を特定する。
前記レーザー光を壁面に照射する照射工程と、
前記壁面に投影された前記レーザー光の投影光が含まれる画像データを取得する撮像工程と、
前記画像データを解析する解析工程と、を有し、
前記解析工程は、前記画像データに含まれる前記投影光の形状に基づき、前記移動体の姿勢及び前記壁面からの距離を特定する。
本発明によれば、投影光の形状に基づき、移動体の姿勢及び壁面からの距離を特定するため、構造物の形状によらず、安定した精度で移動体の位置を特定することが可能となる。
本発明の好ましい形態では、前記投影光の形状は、3つ以上の直線により3つ以上の交点が形成された図形であり、
前記解析工程は、前記投影光の形状を数式化する変換工程と、
前記交点を2次元座標に対応付ける対応付け工程と、
前記投影光の平面方程式を算出する算出工程と、
前記平面方程式により示される3次元平面の法線ベクトルに基づいて前記移動体の姿勢及び前記壁面からの距離を特定する特定工程と、を含む。
前記解析工程は、前記投影光の形状を数式化する変換工程と、
前記交点を2次元座標に対応付ける対応付け工程と、
前記投影光の平面方程式を算出する算出工程と、
前記平面方程式により示される3次元平面の法線ベクトルに基づいて前記移動体の姿勢及び前記壁面からの距離を特定する特定工程と、を含む。
このような構成とすることで、移動体の姿勢及び壁面からの距離を、より高い精度で特定することが可能となる。
本発明の好ましい形態では、前記対応付け工程による対応付けは、組合せ最適化アルゴリズムに基づいて決定される。
このような構成とすることで、交点が不明確な投影光に対しても、正確な平面方程式を取得することが可能となる。
本発明の好ましい形態では、前記算出工程は、前記レーザー光出力部と前記撮影部とを用い、三角測量の原理によって、前記移動体と前記壁面との間の測距を行う測距工程を含む。
このような構成とすることで、移動体の3次元における位置を正確に特定することが可能となる。
本発明の好ましい形態では、前記算出工程は、経時的に連続する複数の投影光に基づく複数の平面方程式を用いて、RANSACにより、前記平面方程式の係数に含まれる外れ値を除去する外れ値除去工程を含む。
このような構成とすることで、投影光の形状に基づく近似的な平面方程式を、より高い精度で算出することが可能となる。
本発明の好ましい形態では、前記解析工程は、前記画像データから前記投影光を抽出するフィルタ工程を含み、
前記フィルタ工程は、前記画像データの画素値の符号が変化する点をエッジとして抽出する。
前記フィルタ工程は、前記画像データの画素値の符号が変化する点をエッジとして抽出する。
このような構成とすることで、投影光の交点を、より高い精度で抽出することが可能となる。
本発明によれば、構造物の形状によらず、安定した精度で移動体の位置を特定することが可能な、自己位置特定方法を提供することができる。
以下、図1~図9を用いて、本発明の実施形態に係る自己位置特定方法について説明する。本実施形態では特に、自己位置特定装置を用いて自己位置特定方法を実施する場合について説明する。
なお、以下に示す実施形態は本発明の一例であり、本発明を以下の実施形態に限定するものではない。本実施形態では、移動体として、空中を移動する飛行体を用いる場合について記載する。
なお、以下に示す実施形態は本発明の一例であり、本発明を以下の実施形態に限定するものではない。本実施形態では、移動体として、空中を移動する飛行体を用いる場合について記載する。
例えば、本実施形態では自己位置特定装置の構成、動作などについて説明するが、同様の構成の方法、システム、コンピュータプログラム、記録媒体なども、同様の作用効果を奏することができる。また、プログラムは、記録媒体に記憶させてもよい。この記録媒体を用いれば、例えばコンピュータに前記プログラムをインストールすることができる。ここで、前記プログラムを記憶した記録媒体は、例えばCD-ROM等の非一過性の記録媒体であっても良い。
図1は、本実施形態に係る自己位置特定装置Aの機能ブロック構成の一例を示す図である。自己位置特定装置Aは、レーザー光L(図4参照)を出力するレーザー光出力部A1と、画像データを取得する撮影部A2と、画像データを処理する処理部A3と、を備えている。
処理部A3は、フィルタ手段101と、変換手段102と、対応付け手段103と、算出手段104と、特定手段105と、動作指示手段106と、として機能する。
フィルタ手段101は、画像データから投影光を抽出する。
変換手段102は、投影光の形状を数式化する。
対応付け手段103は、一の投影光に含まれる交点を、他の投影光に含まれる交点に対応付ける。
算出手段104は、投影光の平面方程式を算出する。
特定手段105は、平面方程式により示される3次元平面の法線ベクトルに基づいて移動体1の姿勢及び壁面からの距離を特定する。
動作指示手段106は、レーザー光出力部A1への照射指示や、撮影部A2への撮影指示を行う。
変換手段102は、投影光の形状を数式化する。
対応付け手段103は、一の投影光に含まれる交点を、他の投影光に含まれる交点に対応付ける。
算出手段104は、投影光の平面方程式を算出する。
特定手段105は、平面方程式により示される3次元平面の法線ベクトルに基づいて移動体1の姿勢及び壁面からの距離を特定する。
動作指示手段106は、レーザー光出力部A1への照射指示や、撮影部A2への撮影指示を行う。
図2は、自己位置特定装置Aを備えた移動体1を示す概略斜視図である。
移動体1は、機体部分を構成する移動体本体11と、移動体本体11の上部に放射状に配置された複数の回転翼機12と、を備えている。また、移動体本体11の下部には一対の脚部13、13が設けられている。また、その一対の脚部13、13間の空間には、外観のヒビ等を検査するためのカメラや、内部欠陥を検出するための赤外線サーモグラフィ等が搭載された検査部14が設けられている。
移動体1は、機体部分を構成する移動体本体11と、移動体本体11の上部に放射状に配置された複数の回転翼機12と、を備えている。また、移動体本体11の下部には一対の脚部13、13が設けられている。また、その一対の脚部13、13間の空間には、外観のヒビ等を検査するためのカメラや、内部欠陥を検出するための赤外線サーモグラフィ等が搭載された検査部14が設けられている。
自己位置特定装置Aにおいて、レーザー光出力部A1及び撮影部A2は、移動体本体11の上面及び側面に、それぞれ一つずつ設けられている。
各レーザー光出力部A1及び各撮影部A2は、一つの処理部A3により制御される。
なお、各自己位置特定装置Aにおいて、レーザー光出力部A1によるレーザー光Lの出力方向と撮影部A2の撮影方向とは、略同一に構成されている。
各レーザー光出力部A1及び各撮影部A2は、一つの処理部A3により制御される。
なお、各自己位置特定装置Aにおいて、レーザー光出力部A1によるレーザー光Lの出力方向と撮影部A2の撮影方向とは、略同一に構成されている。
図3は、移動体本体11のハードウェア構成図である。移動体本体11は、移動体1の飛行可否状態を制御する安全管理装置2と、移動体1で利用される電力を供給するバッテリー3(電源)と、移動体1の飛行動作の制御等を行う主制御部4と、回転翼機12を駆動させるサーボモータ5と、主制御部4からの信号に基づいてサーボモータ5への給電量を調節するモータコントローラ6と、操縦者の操作する操作端末からの操作信号を受信する無線レシーバー7と、操作端末やその他の端末等の外部端末と移動体1間での通信を行うテレメトリ通信部8と、移動体1の現在地や速度等の状態情報を取得するための計測装置9と、を備えている。
通常、一般的な移動体1には、安全管理装置2は設けられておらず、主制御部4及びモータコントローラ6に対して、バッテリー3からの電力供給が直接的に行われている。テレメトリ通信部8や計測装置9等に対する電力供給はバッテリー3から直接行われてもよいし、本実施形態のように主制御部4を介して行われてもよい。なお、主制御部4の仕様によっては、モータコントローラ6から電力供給が行われる場合もある。
以下、図4~図8を用いて、自己位置特定装置Aを用いた自己位置特定方法について説明する。
本実施形態に係る自己位置特定方法では、移動体1が、略四角筒状の管渠Rの内部を飛行する場合について説明する。
なお、本実施形態では、移動体1の操縦者は、移動体1に、予め管渠Rの高さ情報及び幅情報を登録しておく。
なお、本実施形態では、移動体1の操縦者は、移動体1に、予め管渠Rの高さ情報及び幅情報を登録しておく。
図4に示すように、移動体1は、操縦者により管渠Rの略中央に配置される。そして、処理部A3が、動作指示手段106により、レーザー光出力部A1に照射指示を行う。
こうすることで、レーザー光出力部A1により、管渠Rの側壁面W1及び上壁面W2に向かってレーザー光Lを照射する、照射工程が行われる。レーザー光Lとしては、赤外線やRGB等、種々の波長が用いられる。
なお、図4(a)は、管渠Rの上壁面W2を省略し、移動体1及び管渠Rを上面から見た際の図、図4(b)は、図4(a)における、上壁面W2を示したXX´線断面図である。
こうすることで、レーザー光出力部A1により、管渠Rの側壁面W1及び上壁面W2に向かってレーザー光Lを照射する、照射工程が行われる。レーザー光Lとしては、赤外線やRGB等、種々の波長が用いられる。
なお、図4(a)は、管渠Rの上壁面W2を省略し、移動体1及び管渠Rを上面から見た際の図、図4(b)は、図4(a)における、上壁面W2を示したXX´線断面図である。
ここで、側壁面W1及び上壁面W2に照射されたレーザー光Lは、各壁面W1、W2上で格子形状の投影光T1、T2として投影される。
詳述すれば、投影光T1、T2は、6本の直線により略田字状に構成されることで、9つの交点が形成されている。
なお、投影光T1、T2の形状は、必ずしも格子形状である必要はなく、3つ以上の直線により3つ以上の交点が形成された図形であれば、三角形や四角形、六角形等の多角形状であっても良い。
詳述すれば、投影光T1、T2は、6本の直線により略田字状に構成されることで、9つの交点が形成されている。
なお、投影光T1、T2の形状は、必ずしも格子形状である必要はなく、3つ以上の直線により3つ以上の交点が形成された図形であれば、三角形や四角形、六角形等の多角形状であっても良い。
このようにレーザー光Lを格子形状の投影光T1、T2として照射する場合、回折光学素子(DOE)が好適に用いられる。
回折光学素子(DOE)とレーザー光Lとの中心を位置合わせし、回折光学素子(DOE)を介してレーザー光Lを照射することで、回折光学素子(DOE)の回折パターンに応じた所望の投影光T1、T2が、各壁面W1、W2上に投影される。
回折光学素子(DOE)とレーザー光Lとの中心を位置合わせし、回折光学素子(DOE)を介してレーザー光Lを照射することで、回折光学素子(DOE)の回折パターンに応じた所望の投影光T1、T2が、各壁面W1、W2上に投影される。
次に、処理部A3が、動作指示手段106により、撮影部A2に撮影指示を行う。こうすることで、撮影部A2により、各投影光T1、T2を撮影し画像データを取得する、撮影工程が行われる。
ここで、撮影部A2に用いられるカメラは、画像データを高い精度で取得するために、視野角(FoV)が90度以上のものが好適に用いられる。
なお、撮影工程は、本実施形態において、例えば30fpsのフレームレートで、移動体1の飛行中に連続的に行われるものとする。
ここで、撮影部A2に用いられるカメラは、画像データを高い精度で取得するために、視野角(FoV)が90度以上のものが好適に用いられる。
なお、撮影工程は、本実施形態において、例えば30fpsのフレームレートで、移動体1の飛行中に連続的に行われるものとする。
次に、処理部A3により、取得された画像データを解析する、解析工程が行われる。
本実施形態においては、まず、フィルタ手段101により、取得された画像データから投影光T1、T2を抽出する、フィルタ工程が行われる。
詳述すれば、処理部A3は、フィルタ工程において、LoG(Laplacian of Gaussian)を利用したLoGフィルタを施した画像の各画素の画素値を調べ、画素値の符号が変化する点(零交差点)を、エッジ情報として抽出する。
ここで、LoGは以下の式で表される。
詳述すれば、処理部A3は、フィルタ工程において、LoG(Laplacian of Gaussian)を利用したLoGフィルタを施した画像の各画素の画素値を調べ、画素値の符号が変化する点(零交差点)を、エッジ情報として抽出する。
ここで、LoGは以下の式で表される。
上記した式において、rは着目画素からの距離、σはフィルタの平滑化度合いを示している。
なお、フィルタ手段101で用いるフィルタは、LoGフィルタに限られず、LoGの近似となるDoG(Difference of Gaussian)によるDoGフィルタを用いても良い。
なお、フィルタ手段101で用いるフィルタは、LoGフィルタに限られず、LoGの近似となるDoG(Difference of Gaussian)によるDoGフィルタを用いても良い。
次に、変換手段102により、投影光T1、T2の形状を数式化する、変換工程が行われる。
詳述すれば、処理部A3は、変換工程において、フィルタ手段101により抽出されたエッジ情報に対して、ハフ変換を行うことにより、投影光を構成する直線を抽出し、抽出された直線に基づき、交点を抽出する。
図5(a)は、図4(a)において移動体1が位置p1を飛行している際の、側壁面W1への投影光T1を示している。
なお、変換工程の前にフィルタ工程を行ったことにより、投影光の交点は、サブピクセル単位での抽出が可能となる。また、変換工程は、必ずしもハフ変換である必要はなく、他のライン抽出アルゴリズムであっても、同様の抽出結果を出力できる。
詳述すれば、処理部A3は、変換工程において、フィルタ手段101により抽出されたエッジ情報に対して、ハフ変換を行うことにより、投影光を構成する直線を抽出し、抽出された直線に基づき、交点を抽出する。
図5(a)は、図4(a)において移動体1が位置p1を飛行している際の、側壁面W1への投影光T1を示している。
なお、変換工程の前にフィルタ工程を行ったことにより、投影光の交点は、サブピクセル単位での抽出が可能となる。また、変換工程は、必ずしもハフ変換である必要はなく、他のライン抽出アルゴリズムであっても、同様の抽出結果を出力できる。
この変換工程により、投影光T1、T2の2次元座標が決定される。
なお、変換工程は、移動体1の飛行中に連続的に行われる。即ち、本実施形態では、例えば、側壁面W1に対する投影光に関して、1秒間に30パターンの投影光の2次元座標が決定される。
なお、変換工程は、移動体1の飛行中に連続的に行われる。即ち、本実施形態では、例えば、側壁面W1に対する投影光に関して、1秒間に30パターンの投影光の2次元座標が決定される。
次に、対応付け手段103により、移動体1の各飛行位置での投影光の交点を対応付ける、対応付け工程が行われる。
ここで、対応付け工程の説明のため、移動体1が位置p1から位置p2に移動した際の、側壁面W1への投影光T1の形状の変化を考える。
即ち、図4(a)において移動体1が位置p1から位置p2に移動した際、管渠Rの屈曲により、投影光T1は、図5(a)から図5(b)に示す形状に変化したものとする。
なお、以下、説明の便宜上、図5(b)の投影光を投影光T1´と称する。
ここで、対応付け工程の説明のため、移動体1が位置p1から位置p2に移動した際の、側壁面W1への投影光T1の形状の変化を考える。
即ち、図4(a)において移動体1が位置p1から位置p2に移動した際、管渠Rの屈曲により、投影光T1は、図5(a)から図5(b)に示す形状に変化したものとする。
なお、以下、説明の便宜上、図5(b)の投影光を投影光T1´と称する。
詳述すれば、処理部A3は、対応付け工程において、各交点の対応付けを組合せ最適化アルゴリズムに基づいて決定する。
即ち、対応付けの組合せを求める行為は、割り当て問題と理解することができるので、例えば、ハンガリアン法といったアルゴリズム等により、最適な対応関係を求めることができる。
即ち、対応付けの組合せを求める行為は、割り当て問題と理解することができるので、例えば、ハンガリアン法といったアルゴリズム等により、最適な対応関係を求めることができる。
図6において、左側は投影光T1の交点Vp0~Vp8、右側は投影光T1´の交点Vp0´~Vp8´を表している。
対応付けが完了すると、投影光T1の交点Vp0~Vp8は、投影光T1´の交点Vp0´~Vp8´の何れかに1:1で対応付けられる。
対応付けが完了すると、投影光T1の交点Vp0~Vp8は、投影光T1´の交点Vp0´~Vp8´の何れかに1:1で対応付けられる。
ここで、図6において、各線上に記載された数値は、それぞれ対応付けられる場合のコストを表している。この数値が高い方が対応付けられる可能性が低く、数値が低い方が対応付けられる可能性が高い。
即ち、図6において、Vp2´はVp2と対応付けられる可能性が最も高く(コスト5)、続いてVp1と対応付けられる可能性が高く(コスト10)、Vp6と対応付けられる可能性が最も低い(コスト20)。
即ち、図6において、Vp2´はVp2と対応付けられる可能性が最も高く(コスト5)、続いてVp1と対応付けられる可能性が高く(コスト10)、Vp6と対応付けられる可能性が最も低い(コスト20)。
なお、ハンガリアン法は、以下の4ステップで行われる。
(ステップ1):各行の各要素からその行の最小値を引き、その後さらに各列の各要素からその列の最小値を引く。
(ステップ2):0を各行各列から1つずつ選ぶことができるかどうか判定する。もし選ぶことができれば、その0の座標の組みが割当案となる。選ぶことができなれければステップ3へ進む。
(ステップ3):全ての0を、できるだけ少ない数の縦又は横の線で覆う。
(ステップ4):線で消されていない要素から、それらの最小値を引き、縦横の線が交わる要素にその値を加える。ステップ2に戻る。
(ステップ1):各行の各要素からその行の最小値を引き、その後さらに各列の各要素からその列の最小値を引く。
(ステップ2):0を各行各列から1つずつ選ぶことができるかどうか判定する。もし選ぶことができれば、その0の座標の組みが割当案となる。選ぶことができなれければステップ3へ進む。
(ステップ3):全ての0を、できるだけ少ない数の縦又は横の線で覆う。
(ステップ4):線で消されていない要素から、それらの最小値を引き、縦横の線が交わる要素にその値を加える。ステップ2に戻る。
次に、算出手段104により、投影光の平面方程式を算出する、算出工程が行われる。
詳述すれば、算出工程は、測距工程と、外れ値除去工程と、を含む。
詳述すれば、算出工程は、測距工程と、外れ値除去工程と、を含む。
測距工程について、さらに詳述すれば、処理部A3は、測距工程において、レーザー光出力部A1と撮影部A2とを用い、三角測量の原理によって、移動体1と各交点との間の測距を行い、各交点のz軸方向の座標を算出する。
図7は、レーザー光出力部A1、撮影部A2及び所定の交点Vpi´の位置関係を模式的に示した図である。
Zが所定の移動体1からの距離、Bがレーザー光出力部A1と撮影部A2とのレンズ間の距離、Cが左右の画像から得られた重心位置の差、fが焦点距離を示す。
このとき、以下の式で距離Zを求めることができる。
Zが所定の移動体1からの距離、Bがレーザー光出力部A1と撮影部A2とのレンズ間の距離、Cが左右の画像から得られた重心位置の差、fが焦点距離を示す。
このとき、以下の式で距離Zを求めることができる。
なお、本実施形態では、レーザー光出力部A1を仮想のカメラとして、測距工程を行う。即ち、レーザー光出力部A1により出力される投影光を、レーザー光出力部A1視点のカメラで撮像した画像として扱い、撮影部A2により撮影した画像との間で三角測量を行う。
また、測距工程を行うにあたり、レーザー光出力部A1と撮影部A2との間でキャリブレーションを行う必要がある。
詳述すれば、例えば、レーザー光出力部A1から平面へ単純なパターンの投影光を出力する。そして、レーザー光出力部A1から投影した投影光と、撮影部A2で撮像した平面上に投影されたパターンとでマッチングを行い、ピクセル単位でのマッピングを求める。
このマッピングにより、撮影部A2で撮影した画像をレーザー光出力部A1視点の仮想カメラで撮影した画像に変換できる。
詳述すれば、例えば、レーザー光出力部A1から平面へ単純なパターンの投影光を出力する。そして、レーザー光出力部A1から投影した投影光と、撮影部A2で撮像した平面上に投影されたパターンとでマッチングを行い、ピクセル単位でのマッピングを求める。
このマッピングにより、撮影部A2で撮影した画像をレーザー光出力部A1視点の仮想カメラで撮影した画像に変換できる。
また、測距工程を行うにあたり、撮像した画像に対して、歪みを取り除き、3次元座標系に復元するための、カメラキャリブレーションを行う必要がある。
カメラキャリブレーションとは,各視点の画像におけるカメラの内部パラメータおよび外部パラメータを求める作業である。
カメラキャリブレーションとは,各視点の画像におけるカメラの内部パラメータおよび外部パラメータを求める作業である。
内部パラメータは、焦点距離や画像中心、撮像素子のサイズと画像サイズの関係などが含まれ、カメラ座標系の3次元座標とディジタル画像座標系の2次元座標の関係を表す。
外部パラメータは、カメラの回転と平行移動が含まれ、世界座標系の3次元座標におけるカメラの位置関係を表す。
これらのパラメータは、画像中の特徴点が他の画像中のどの点に対応するかという対応点情報から推定できる。
外部パラメータは、カメラの回転と平行移動が含まれ、世界座標系の3次元座標におけるカメラの位置関係を表す。
これらのパラメータは、画像中の特徴点が他の画像中のどの点に対応するかという対応点情報から推定できる。
即ち、カメラキャリブレーションとは、画像中で特徴点を抽出し、画像間で対応を取ったのち、すべての対応する視線ベクトルが、3次元上の1点で交わるようなカメラパラメータを求める作業である。
ここで、カメラキャリブレーションは、射影行列Pmを用いた、以下の式により行われる。
ここで、カメラキャリブレーションは、射影行列Pmを用いた、以下の式により行われる。
M´=[X,Y,Z,1]及びm´=[u,v,1]は、それぞれ3次元空間中の点の座標M=[X,Y,Z]及び画像上の点の座標m=[u,v]の同次座標系である。
なお、射影行列Pmは,カメラの内部パラメータ行列Aおよび回転行列Rと
を用いることで、以下のように表現できる。
なお、射影行列Pmは,カメラの内部パラメータ行列Aおよび回転行列Rと
を用いることで、以下のように表現できる。
外れ値除去工程について、さらに詳述すれば、処理部A3は、外れ値除去工程において、経時的に連続する複数の投影光に基づく複数の平面方程式を用いて、RANSACにより、平面方程式に含まれる外れ値を除去する。
外れ値とは、他の平面方程式の係数と比較して、極端に大きな係数、又は極端に小さな係数のことであり、複数の平面方程式を平均し、近似的な平面方程式を導出する際、ノイズ等の原因により生じる。
外れ値とは、他の平面方程式の係数と比較して、極端に大きな係数、又は極端に小さな係数のことであり、複数の平面方程式を平均し、近似的な平面方程式を導出する際、ノイズ等の原因により生じる。
例えば、投影光T1´の近似的な平面方程式は以下のように表すことができるものとする。
なお、投影光T1´の各交点はVpi´(xi、yi、zi)(i=0≦n≦8)とする。
なお、投影光T1´の各交点はVpi´(xi、yi、zi)(i=0≦n≦8)とする。
[数3]について、左端の行列をA、右端の行列B、(a、b、c)より構成される係数行列をxとすると、以下のように表すことができる。
ここで、投影光T1´の交点は3点以上あり、[数4]は優決定系であるから、下記の擬似逆行列の関係を用いる。
そして、[数4]をxについて解くと、以下のように表すことができる。
ここで、RANSACを用いて、係数(a、b、c)に係る連立方程式を解くことで、投影光T1´の近似的な平面方程式が導出される。
なお、RANSACによる外れ値除去工程は、以下の5ステップで行われる。
(ステップ1):データ集合から、モデルの決定に必要な数以上の「少数の」サンプルをランダムに選ぶ。なお、サンプルの数としては5が好ましい。
(ステップ2):得られた「少数の」サンプルから最小二乗法などで臨時のモデルを導出する。
(ステップ3):臨時のモデルをデータに当てはめてみて、臨時のモデルに含まれるインライア値の数が、予め設定した所定の値よりも大きい場合、「正しいモデル候補」に加える。
(ステップ4):(ステップ2)、(ステップ3)を繰り返す。
(ステップ5):得られた「正しいモデル候補」の中で、もっともデータによく合致する平面方程式を、特定の時間内での投影光の近似的な平面方程式とする。
(ステップ1):データ集合から、モデルの決定に必要な数以上の「少数の」サンプルをランダムに選ぶ。なお、サンプルの数としては5が好ましい。
(ステップ2):得られた「少数の」サンプルから最小二乗法などで臨時のモデルを導出する。
(ステップ3):臨時のモデルをデータに当てはめてみて、臨時のモデルに含まれるインライア値の数が、予め設定した所定の値よりも大きい場合、「正しいモデル候補」に加える。
(ステップ4):(ステップ2)、(ステップ3)を繰り返す。
(ステップ5):得られた「正しいモデル候補」の中で、もっともデータによく合致する平面方程式を、特定の時間内での投影光の近似的な平面方程式とする。
ここで、RANSACを用いるにあたり、上記した(ステップ4)の最大反復回数Nβを決定しておく。反復回数Nβは以下の式で表すことができる。
反復回数Nβ以上の反復を行うことで(1-ε)以上の確率で外れ値の影響のない数理モデルが得られる。
反復回数Nβ以上の反復を行うことで(1-ε)以上の確率で外れ値の影響のない数理モデルが得られる。
上記した式において、εは、計算により「正しくないモデル候補」を取得する確率、βは、計算によりインライア値を取得する確率、nは選択するサンプルの数である。
また、このとき、近似的な平面方程式を導出する妨げとなる外れ値について、閾値を設定する。閾値は、仮のモデルとなる平面の交点からこれに対応する特定の平面の交点までの直線距離であり、この閾値を上回る交点が、外れ値となる。
なお、閾値は、3cmとすることが好ましい。
なお、閾値は、3cmとすることが好ましい。
次に、特定手段105により、近似的な平面方程式により示される3次元平面の法線ベクトルに基づいて移動体1の姿勢及び各壁面W1、W2からの距離を特定する、特定工程が行われる。
図8は、投影光T1´の近似的な平面Fと移動体1との位置関係を模式図に表した図である。
移動体1の壁面W1からの距離を特定する特定工程について詳述すれば、処理部A3は、まず、近似的な平面Fから
を抽出し、これら2つのベクトルの外積から、
を導出する。
次に、処理部A3は、近似的な平面Fから任意の点qを1つ抽出し、点qと移動体1を結ぶ
を導出する。
移動体1の壁面W1からの距離を特定する特定工程について詳述すれば、処理部A3は、まず、近似的な平面Fから
を抽出し、これら2つのベクトルの外積から、
を導出する。
次に、処理部A3は、近似的な平面Fから任意の点qを1つ抽出し、点qと移動体1を結ぶ
を導出する。
これにより、近似的な平面Fから移動体1までの距離Dは、以下のように表すことができる。
姿勢を特定する特定工程について詳述すれば、処理部A3は、
と、移動体1のローカル座標のおけるx軸、y軸、z軸それぞれの
を用いて、移動体1の姿勢を特定する。
即ち、処理部A3は、移動体1のローカル座標における、ピッチ・ロール・ヨーの回転角Tx、Ty、Tzを、以下の式から導出する。
と、移動体1のローカル座標のおけるx軸、y軸、z軸それぞれの
を用いて、移動体1の姿勢を特定する。
即ち、処理部A3は、移動体1のローカル座標における、ピッチ・ロール・ヨーの回転角Tx、Ty、Tzを、以下の式から導出する。
即ち、上記(1)式で、ピッチの回転角Txが、上記(2)式で、ロールの回転角Tyが、上記(3)式で、ヨーの回転角Tzが、導出される。
なお、対応付け工程以降の工程は、投影光の形状が変化した場合のみならず、移動体1の飛行中であれば、変換工程に引き続き、常時行われる。
また、フィルタ工程~特定工程までの一連の処理工程は、フレーム毎に行われる。
また、フィルタ工程~特定工程までの一連の処理工程は、フレーム毎に行われる。
このように、移動体1の姿勢及び壁面からの距離が特定されることで、移動体1は、壁面に対する一定の距離を保って走行することができる。
即ち、操縦者は、移動体1に対して、前進又は後進の操作を行うのみで、移動体1は、管渠Rの内部を安全に走行することができる。
即ち、操縦者は、移動体1に対して、前進又は後進の操作を行うのみで、移動体1は、管渠Rの内部を安全に走行することができる。
なお、上記一連の工程により導出された移動体1の位置や姿勢の算出結果について、誤差を低減させるため、算出結果に対して、KF、EKF、UKF、UKF2、CPI、ceres-solver、GTSAM等の非線形最適化を行うアルゴリズムを適用させることが好ましい。
図9を用いて、移動体1の姿勢及び各壁面W1、W2からの距離を特定するまでの全体の流れについて説明する。
まず、照射工程により、各壁面W1、W2にレーザー光Lを照射する(ステップS1)。
次に、撮影工程により、各壁面W1、W2に照射されたレーザー光Lを撮影し、投影光S1、S2が含まれる画像データを取得する(ステップS2)。
次に、解析工程により、画像データを解析する。
詳述すれば、まず、フィルタ工程により、画像データから投影光T1、T2を抽出する(ステップS3)。
次に、変換工程により、投影光の形状を数式化する(ステップS4)。
次に、対応付け工程において、移動体1の各飛行位置での投影光の交点を対応付ける(ステップS5)。
次に、算出工程により、投影光T1、T2の平面方程式を算出する。
詳述すれば、まず、算出工程において、測距工程により、レーザー光出力部A1と撮影部A2とを用い、三角測量の原理によって、移動体1と壁面W1、W2との間の測距を行う(ステップS6)。
次に、算出工程において、外れ値除去工程により、経時的に連続する複数の投影光に基づく複数の平面方程式を用いて、RANSACにより、前記平面方程式に含まれる外れ値を除去する(ステップS7)。
最後に、特定工程により、平面方程式により示される3次元平面の法線ベクトルに基づいて、移動体1の姿勢及び壁面W1、W2からの距離を特定する(ステップS8)。
本実施形態によれば、処理部A3により、画像データに含まれる投影光T1、T2の形状に基づき、移動体1の姿勢及び壁面W1、W2からの距離を特定するため、構造物の形状によらず、安定した精度で移動体1の位置を特定することが可能となる。
また、投影光T1、T2の形状が、格子形状であり、処理部A3が、投影光T1、T2の形状に基づく近似的な平面方程式により示される3次元平面の法線ベクトルに基づいて、移動体1の姿勢及び壁面W1、W2からの距離を特定することで、移動体1の姿勢及び壁面W1、W2からの距離を、より高い精度で特定することが可能となる。
また、対応付け工程による対応付けが、組合せ最適化アルゴリズムに基づいて決定されることで、交点が不明確な投影光に対しても、正確な平面方程式を取得することが可能となる。
また、算出工程が、レーザー光出力部A1と撮影部A2とを用い、三角測量の原理によって、移動体1と壁面W1、W2との間の測距を行う測距工程を含むことで、移動体1の3次元における位置を正確に特定することが可能となる。
また、算出工程が、経時的に連続する複数の投影光に基づく複数の平面方程式を用いて、RANSACにより、前記平面方程式の係数に含まれる外れ値を除去する外れ値除去工程を含むことで、投影光T1、T2の形状に基づく近似的な平面方程式を、より高い精度で算出することが可能となる。
また、解析工程が、画像データから投影光T1、T2を抽出するフィルタ工程を含み、
フィルタ工程が、画像データの画素値の符号が変化する点をエッジとして抽出することで、投影光の交点を、より高い精度で抽出することが可能となる。
フィルタ工程が、画像データの画素値の符号が変化する点をエッジとして抽出することで、投影光の交点を、より高い精度で抽出することが可能となる。
なお、上述の実施形態において示した各構成部材の諸形状や寸法等は一例であって、設計要求等に基づき種々変更可能である。
例えば、本実施形態では、移動体として、空中を移動する飛行体を用いた場合を示したが、これに限られず、地上走行を行うロボティクスにも、当然に適用可能である
例えば、本実施形態では、レーザー光出力部A1及び撮影部A2を、移動体1の側面及び上面に1つずつ(計2つ)設けた例を示したが、設置態様はこれに限られず、調査目的や調査環境等に応じて、3つ以上設けても良い。
例えば、レーザー光出力部A1及び撮影部A2を、移動体1の上面、左側面及び右側面に1つずつ(計3つ)設けた場合、操縦者は、移動体1に、予め管渠Rの高さ情報のみを登録しておく。このとき、移動体1の幅方向の位置については、各側面の撮影部A2より取得される画像データを比較することにより、算出される。
また、レーザー光出力部A1及び撮影部A2を、移動体1の上面、底面、左側面及び右側面に1つずつ(計4つ)設けた場合、操縦者は、予め管渠Rの高さ情報や幅情報を登録する必要は無い。
また、レーザー光出力部A1及び撮影部A2を、移動体1の上面、底面、左側面及び右側面に1つずつ(計4つ)設けた場合、操縦者は、予め管渠Rの高さ情報や幅情報を登録する必要は無い。
このように、本発明は、高い精度で、移動体の自己位置を特定することができるため、作業員の安全確保が困難な状況下(酸素の薄い場所や水量の多い場所、突発的な降雨が発生する地域、硫化水素発生下等)での調査を、移動体に安全に行わせることができ、事故発生リスクを低減させることができる。
また、本発明によれば、従来の潜行目視調査や自走式TVカメラ車等に比べ、走行(航行)スピードの向上により、調査コストの低減化が実現できる上、機器の軽量化による搬入搬出や移動等の容易化を実現できる。
さらに、本発明は、下水道管路の他、下水処理場の汚泥ピットや消化タンク、さらには他事業のインフラ(浄水場、導水渠等)点検へ応用が期待できる。
即ち、本発明は、産業上の利用可能性が極めて高いものである。
また、本発明によれば、従来の潜行目視調査や自走式TVカメラ車等に比べ、走行(航行)スピードの向上により、調査コストの低減化が実現できる上、機器の軽量化による搬入搬出や移動等の容易化を実現できる。
さらに、本発明は、下水道管路の他、下水処理場の汚泥ピットや消化タンク、さらには他事業のインフラ(浄水場、導水渠等)点検へ応用が期待できる。
即ち、本発明は、産業上の利用可能性が極めて高いものである。
1 移動体
A 自己位置特定装置
A1 レーザー光出力部
A2 撮影部
A3 処理部
101 フィルタ手段
102 変換手段
103 対応付け手段
104 算出手段
105 特定手段
106 動作指示手段
R 管渠
W1 側壁面
W2 上壁面
L レーザー光
T1、T2、T1´ 投影光
F 近似的な平面
A 自己位置特定装置
A1 レーザー光出力部
A2 撮影部
A3 処理部
101 フィルタ手段
102 変換手段
103 対応付け手段
104 算出手段
105 特定手段
106 動作指示手段
R 管渠
W1 側壁面
W2 上壁面
L レーザー光
T1、T2、T1´ 投影光
F 近似的な平面
Claims (6)
- レーザー光を出力するレーザー光出力部と、撮影部と、処理部と、を備えた移動体に対する自己位置特定方法であって、
前記レーザー光を壁面に照射する照射工程と、
前記壁面に投影された前記レーザー光の投影光が含まれる画像データを取得する撮影工程と、
前記画像データを解析する解析工程と、を有し、
前記解析工程は、前記画像データに含まれる前記投影光の形状に基づき、前記移動体の姿勢及び前記壁面からの距離を特定する自己位置特定方法。 - 前記投影光の形状は、3つ以上の直線により3つ以上の交点が形成された図形であり、
前記解析工程は、前記投影光の形状を数式化する変換工程と、
前記交点を2次元座標に対応付ける対応付け工程と、
前記投影光の平面方程式を算出する算出工程と、
前記平面方程式により示される3次元平面の法線ベクトルに基づいて前記移動体の姿勢及び前記壁面からの距離を特定する特定工程と、を含む、請求項1に記載の自己位置特定方法。 - 前記対応付け工程による対応付けは、組合せ最適化アルゴリズムに基づいて決定される、請求項2に記載の自己位置特定方法。
- 前記算出工程は、前記レーザー光出力部と前記撮影部とを用い、三角測量の原理によって、前記移動体と前記壁面との間の測距を行う測距工程を含む、請求項2又は3に記載の自己位置特定方法。
- 前記算出工程は、経時的に連続する複数の投影光に基づく複数の平面方程式を用いて、RANSACにより、前記平面方程式の係数に含まれる外れ値を除去する外れ値除去工程を含む、請求項2~4の何れかに記載の自己位置特定方法。
- 前記解析工程は、前記画像データから前記投影光を抽出するフィルタ工程を含み、
前記フィルタ工程は、前記画像データの画素値の符号が変化する点をエッジ情報として抽出する、請求項1~5の何れかに記載の自己位置特定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2021530578A JP7521761B2 (ja) | 2019-07-10 | 2020-06-23 | 自己位置特定方法 |
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2019128675 | 2019-07-10 | ||
JP2019-128675 | 2019-07-10 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2021006026A1 true WO2021006026A1 (ja) | 2021-01-14 |
Family
ID=74114880
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/JP2020/024519 WO2021006026A1 (ja) | 2019-07-10 | 2020-06-23 | 自己位置特定方法 |
Country Status (2)
Country | Link |
---|---|
JP (1) | JP7521761B2 (ja) |
WO (1) | WO2021006026A1 (ja) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010092426A (ja) * | 2008-10-10 | 2010-04-22 | Canon Inc | 画像処理装置、画像処理方法およびプログラム |
JP2011075336A (ja) * | 2009-09-29 | 2011-04-14 | Panasonic Electric Works Co Ltd | 3次元形状計測装置、3次元形状計測方法 |
JP2015137897A (ja) * | 2014-01-21 | 2015-07-30 | キヤノン株式会社 | 距離計測装置及び距離計測方法 |
JP2016205837A (ja) * | 2015-04-15 | 2016-12-08 | 佐藤工業株式会社 | トンネルの管理方法 |
JP2017224123A (ja) * | 2016-06-15 | 2017-12-21 | 日本電気株式会社 | 無人飛行装置制御システム、無人飛行装置制御方法および無人飛行装置 |
-
2020
- 2020-06-23 WO PCT/JP2020/024519 patent/WO2021006026A1/ja active Application Filing
- 2020-06-23 JP JP2021530578A patent/JP7521761B2/ja active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010092426A (ja) * | 2008-10-10 | 2010-04-22 | Canon Inc | 画像処理装置、画像処理方法およびプログラム |
JP2011075336A (ja) * | 2009-09-29 | 2011-04-14 | Panasonic Electric Works Co Ltd | 3次元形状計測装置、3次元形状計測方法 |
JP2015137897A (ja) * | 2014-01-21 | 2015-07-30 | キヤノン株式会社 | 距離計測装置及び距離計測方法 |
JP2016205837A (ja) * | 2015-04-15 | 2016-12-08 | 佐藤工業株式会社 | トンネルの管理方法 |
JP2017224123A (ja) * | 2016-06-15 | 2017-12-21 | 日本電気株式会社 | 無人飛行装置制御システム、無人飛行装置制御方法および無人飛行装置 |
Also Published As
Publication number | Publication date |
---|---|
JP7521761B2 (ja) | 2024-07-24 |
JPWO2021006026A1 (ja) | 2021-01-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11361469B2 (en) | Method and system for calibrating multiple cameras | |
US11645757B2 (en) | Method of and apparatus for analyzing images | |
CN107063228B (zh) | 基于双目视觉的目标姿态解算方法 | |
CN103761737B (zh) | 基于稠密光流的机器人运动估计方法 | |
EP3818337B1 (en) | Defect detection system using a camera equipped uav for building facades on complex asset geometry with optimal automatic obstacle deconflicted flightpath | |
CN103065323B (zh) | 一种基于单应性变换矩阵的分段空间对准方法 | |
CN107677274B (zh) | 基于双目视觉的无人机自主着陆导航信息实时解算方法 | |
CN106645205A (zh) | 一种无人机桥梁底面裂纹检测方法及系统 | |
WO2019144289A1 (en) | Systems and methods for calibrating an optical system of a movable object | |
CN109472778B (zh) | 一种基于无人机的高耸结构外观检测方法 | |
EP3155369B1 (en) | System and method for measuring a displacement of a mobile platform | |
CN112146627B (zh) | 在无特征表面上使用投影图案的飞行器成像系统 | |
CN111583342B (zh) | 一种基于双目视觉的目标快速定位方法及装置 | |
CN116625258A (zh) | 链条间距测量系统及链条间距测量方法 | |
Knyaz et al. | Joint geometric calibration of color and thermal cameras for synchronized multimodal dataset creating | |
WO2021006026A1 (ja) | 自己位置特定方法 | |
Shipitko et al. | Edge detection based mobile robot indoor localization | |
CN112991372B (zh) | 一种基于多边形匹配的2d-3d相机外参标定方法 | |
CN109342439B (zh) | 基于无人机的索结构外观检测方法 | |
Popov et al. | Application of the Optical Flow as a Navigation Sensor for UAV | |
Li et al. | High‐resolution model reconstruction and bridge damage detection based on data fusion of unmanned aerial vehicles light detection and ranging data imagery | |
Brogaard et al. | Autonomous GPU-based UAS for inspection of confined spaces: Application to marine vessel classification | |
CN117274499B (zh) | 一种基于无人机倾斜摄影的钢结构加工及安装方法 | |
Li-Chee-Ming et al. | A feasibility study on using ViSP’S 3D model-based tracker for UAV pose estimation in outdoor environments | |
Igaue et al. | Cooperative 3D tunnel measurement based on 2D–3D registration of omnidirectional laser light |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 20837106 Country of ref document: EP Kind code of ref document: A1 |
|
ENP | Entry into the national phase |
Ref document number: 2021530578 Country of ref document: JP Kind code of ref document: A |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 20837106 Country of ref document: EP Kind code of ref document: A1 |