JPH1183530A - Optical flow detector for image and self-position recognizing system for mobile body - Google Patents

Abstract

PROBLEM TO BE SOLVED: To recognize a self-position by detecting, at high speed, an optical flow real time from an imaged picture with a small device which can be mounted on a mobile body. SOLUTION: A distance image is generated at a stereo-process part 30 of a hardware circuit for a picture imaged with a camera assembly 10, and then from the distance image, a histogram for evaluating a No.1 block which can be used for an optical flow is generated. Then, at an optical flow process part 60 of a hardware circuit, a city block distance is calculated to fast explore No.2 block corresponding to the No.1 block. Then, at a navigation process part 90, a speed component of a mobile body is calculated from the optical flow of a lower part image, the speed component is subtracted by a rotational speed component calculated from the optical flow of the lower part image to obtain net translational speed component, and the net translational speed component is accumulated to calculate a navigation locus.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an image optical flow detecting device for detecting an optical flow of an image at high speed by a hardware circuit, and an optical flow of an image by processing a captured image in real time. And a system for recognizing a self-position of a moving body.

[0002]

2. Description of the Related Art Conventionally, various technologies such as movement control, route detection, route detection, and location detection have been developed for mobile objects that move autonomously, such as unmanned robots, autonomous mobile work vehicles, and unmanned helicopters. Among these, self-location recognition is one of the important technologies.

[0003] As this self-position recognition technology, for example,
For mobile objects that travel autonomously on the ground, such as autonomous vehicles, two-dimensional angular velocities are detected by vibrating gyros or optical gyros, and translational speeds are detected by sensors that measure ground speed, and movement from a reference position is performed. There is a technology to calculate the self-position by calculating the amount, and in the case of a flying vehicle such as an unmanned helicopter, the inertial navigation device detects the gravitational acceleration to detect the acceleration of the flying object and integrates this acceleration There is a technology that knows the amount of movement.

Furthermore, in recent years, a technique for recognizing the self-position of a moving object by applying image processing technology has been developed. In particular, a camera is imaged on the moving object to image the surrounding environment, and the timing of the imaging timing is adjusted. The technology that recognizes the self-position by detecting the movement of a moving object by finding the optical flow between two different images enables analysis of the surrounding environment that is unique to an image with a huge amount of information, and makes it possible to analyze complex terrain. By discriminating, accurate autonomous navigation can be realized.

[0005]

However, in the case of detecting the movement of a moving body by optical flow of an image, conventionally, a processing capability equivalent to that of a workstation is required to capture a captured image and recognize its own position in real time. This leads to an increase in the size and weight of the device.

For this reason, in a conventional device that can be mounted on an autonomous mobile body, it is necessary to take measures such as processing offline after capturing a captured image, and to determine its own position in real time like an unmanned helicopter. It is difficult to apply to moving objects that need to be recognized, and the application range is extremely limited.

SUMMARY OF THE INVENTION The present invention has been made in view of the above circumstances, and has an optical flow detecting device for images capable of detecting an optical flow at high speed in real time, and a photographed image which can be mounted on a moving object without being enlarged. The object of the present invention is to provide a moving object self-position recognition system that can process the object in real time and recognize the self-position of the moving object from an optical flow of an image.

[0008]

According to the first aspect of the present invention,
An optical flow detection device for an image in which one of two images captured at different timings is set as a reference image and the other is set as a comparison image, and an optical flow between the images is searched by searching for a corresponding position between the two images. A data transfer buffer for rearranging the luminance data for each pixel in the predetermined area and the luminance data for each pixel in the search area of the comparison image into a data series of a predetermined number of bytes, and transferring each data series in a ring shape; 2 transferred from transfer buffer
After calculating the absolute value of the difference between the data of the systems in parallel by a plurality of arithmetic units, the outputs of each arithmetic unit are added in series by a plurality of adders to calculate the city block distance. An arithmetic processing circuit for outputting an address having a minimum value as a corresponding position of the comparison image with respect to a predetermined area of the reference image.

According to a second aspect of the present invention, there is provided an imaging section having a pair of stereo cameras mounted on a moving body for capturing a distant landscape and a pair of stereo cameras for capturing a downward landscape, and the stereo unit. A corresponding image in the two images captured by the camera is searched to find a pixel shift amount corresponding to a distance to the object, and a distance image obtained by quantifying the perspective information to the object obtained from the pixel shift amount is obtained. A distance image generating unit to generate, and the gradation data and frequency of the distance image,
Histogram generation that accumulates the frequency of the held gradation data and generates histogram data by a coincidence output from a comparator that holds the data in the latch and compares the newly input gradation data with the held gradation data. And a plurality of characteristic image regions suitable for navigation calculation are extracted as a first block from the captured image by referring to the histogram data generated by the histogram generation unit from the distance image and the first image. An image processing unit for setting a search range for searching for an area corresponding to the first block as a second block from an image at a different imaging timing from the image from which the block has been extracted; and an image processing unit for each pixel of the first block. Rearranging the luminance data and the luminance data for each pixel in the search range of the second block into a data sequence of a predetermined number of bytes, A data transfer buffer for transferring a data sequence in a ring shape, and an absolute value of a difference between two systems of data transferred from the transfer buffer is calculated in parallel by a plurality of arithmetic units. An optical flow processing unit having an arithmetic processing circuit for calculating the city block distance by adding in series by the adder and outputting an address at which the city block distance is the minimum value, and the optical flow for a captured image of a distant scenery After calculating an optical flow from the first block to the second block based on the output from the processing unit and the distance image to calculate a rotation speed component between frames of the moving body, the above-described method is performed on the captured image of the lower landscape. An optical signal from the first block to the second block is generated based on an output from an optical flow processing unit and the distance image. The flow component between the frames of the moving body is calculated by calculating the flow rate, and the translation speed component between the frames obtained by removing the rotation speed component from the speed component is converted into a translation speed component viewed from the distance measurement start point, and this conversion is performed. A navigation processing unit for recognizing the self-position of the moving body by calculating the navigation trajectory in the three-dimensional space by accumulating the obtained translation speed components.

That is, in the optical flow detecting apparatus for an image according to the first aspect of the present invention, the luminance data of each pixel of the predetermined area of the reference image and the pixel of the search area of the comparative image are obtained from two images taken at different timings. When each of the luminance data is input to the data transfer buffer, the data transfer buffer rearranges the data sequence into a data sequence of a predetermined number of bytes, and transfers each data sequence to the arithmetic processing circuit in a ring shape. After calculating the absolute value of the difference between the two systems of data in parallel by a plurality of arithmetic units, the outputs of each arithmetic unit are added in series by a plurality of adders to calculate a city block distance. Is output as the corresponding position of the comparison image with respect to the predetermined area of the reference image. Therefore, the optical flow can be obtained from the search result of the corresponding position between the two images.

Further, in the self-position recognition system for a moving object according to the second aspect of the present invention, a distant landscape and a lower landscape are imaged by respective stereo cameras, and the distance image generation unit performs:
The corresponding position in the two images by the stereo camera is searched to determine the amount of pixel shift generated according to the distance to the object, and a distance image is generated by digitizing the perspective information to the object obtained from the pixel shift amount. At the same time, the histogram generation unit holds the gradation data and frequency of the distance image in a latch, respectively, and uses the coincidence output from a comparator that compares the newly input gradation data with the held gradation data. The histogram data is generated by integrating the frequencies of the held gradation data.

The image processing unit extracts a plurality of characteristic picture regions suitable for navigation calculation from the captured image as a first block by referring to histogram data of the distance image, and extracts the first block. A search range is set for searching for an area corresponding to the first block as a second block from among the images having different imaging timings from the image from which is extracted.

Next, in the optical flow processing unit, a captured image of a distant landscape and a captured image of a downward landscape are
The luminance data of each pixel in the first block and the luminance data of each pixel in the search range of the second block are rearranged into a data sequence of a predetermined number of bytes by a data transfer buffer, and each data sequence is processed in a ring shape. When transferred to the circuit, the arithmetic processing circuit calculates the absolute value of the difference between the two sets of luminance data in parallel with a plurality of arithmetic units, and then adds the output of each arithmetic unit in series with a multi-stage adder. The city block distance is calculated, and the address at which the city block distance becomes the minimum value is output.

The navigation processing unit obtains an optical flow based on the output from the optical flow processing unit for the captured image of the distant landscape and the distance image, calculates the rotational speed component between frames of the moving object, and An optical flow is calculated based on the output from the optical flow processing unit for the captured image of the landscape and the distance image to calculate a speed component between frames of the moving object, and translation between frames obtained by removing a rotational speed component from the speed component Converting the speed component to the translation speed component seen from the distance measurement start point,
By accumulating the converted translation speed components, a navigation trajectory in a three-dimensional space is obtained, and the self-position of the moving body is recognized.

[0015]

Embodiments of the present invention will be described below with reference to the drawings. The drawings relate to an embodiment of the present invention, and FIG. 1 is an overall configuration diagram of a self-position recognition system.
FIG. 3 is a block diagram of a stereo processing unit, FIG. 4 is a block diagram of an optical flow processing unit, FIG. 5 is a configuration diagram of a histogram processing circuit, and FIG. 6 is a diagram of a self-addition type gradation / frequency module. 7 is a configuration diagram of a data transfer buffer, FIG. 8 is a configuration diagram of an arithmetic processing circuit, FIG. 9 is an explanatory diagram of stereo processing, FIG. 10 is an explanatory diagram of histogram processing, and FIG. 12 is a time chart of a frequency module, FIG. 12 is an explanatory diagram of an optical flow process, FIG. 13 is a flowchart of a No1 block group acquisition process routine, FIG. 14 is a flowchart of a rotation calculation process routine, and FIG. 15 is a flowchart of a translation calculation process routine. 16 is a flowchart of a navigation calculation processing routine.

FIG. 1 shows a camera assembly 10 composed of two sets of stereo cameras for three-dimensional space measurement, and an optical flow (imaging coordinates between frames) between images taken by the camera assembly 10 of the surrounding environment at regular time intervals. This is a self-position recognition system for a moving object, comprising a self-position recognition device 20 for recognizing the self-position based on a movement vector distribution representing movement on a plane. In the present embodiment, the self-position recognition system is mounted on an unmanned helicopter used for spraying pesticides, for example, and processes an image captured by the camera assembly 10 in real time to generate navigation trajectory data for a flight control computer (not shown). To control the altitude of the helicopter with respect to the ground and the flight direction along a route set in absolute coordinates or a route set for a target on the ground.

The self-position recognizing device 20 includes a stereo processing unit 30 for performing stereo processing on an image captured by the camera assembly 10, an optical flow processing unit 60 for obtaining an optical flow between time-sequential captured images, The stereo processing unit 3 is provided with a navigation processing unit 90 that calculates the amount of movement between frames and the rotation and translation components of a moving object (helicopter) based on the optical flow obtained by the processing unit 60 and calculates a navigation trajectory.
0 and the optical flow processing unit 60 are configured as hardware circuits, and the navigation processing unit 90 is configured as a multiprocessor configured to operate a plurality of RISC processors in parallel, so that it is possible to acquire navigation data online by high-speed processing. ing.

The camera assembly 10 is mounted on a moving body to capture an image of the surrounding environment at regular time intervals, to capture changes in the surrounding environment on a screen-by-screen basis, and to obtain distance information corresponding to a horizontal shift. As shown in FIG. 2, the gantry 11 has a set of a distant stereo camera 12 for capturing a distant scenery necessary for calculating a rotational speed component of the moving body. And a set of lower stereo cameras 13 for capturing an image of a lower landscape (ground surface) necessary for calculating a translation speed component of the moving body.

The distant stereo camera 12 comprises a main camera (reference camera) 12a and a sub camera 12b which are synchronized with each other and have a variable shutter speed. The main camera (reference camera) 12a and the sub camera 1a
2b are arranged in a relation of the base line length LS, and the vertical axes of the imaging planes are parallel to each other.

Similarly, the lower stereo camera 13
The main camera (reference camera) 13a and the sub camera 1 which are synchronized with each other and have a variable shutter speed
3b, a main camera (reference camera) 13a,
Sub camera 13b with the same specifications as main camera 13a
Are in relation to the base line length SS, and are arranged such that the vertical axes of the imaging planes are parallel to each other. The distance stereo camera 12 and the lower stereo camera 13 are also synchronized with each other.

In the self-position recognition system according to the present embodiment, the movement of the moving body is detected from the movement of the optical flow, that is, the movement between the images having different imaging times (timings), for each of the captured image of the distant landscape and the captured image of the lower landscape. The movement of the distant image, the movement of the lower image,
Based on each distance image, it is converted to the amount of movement in the real space, the rotational speed component due to the movement of the distant image is removed from the speed component due to the movement of the lower image, the net translation speed component is obtained, and the distance measurement start point By converting and accumulating the translational velocity component viewed from the (start point), a navigation trajectory in a three-dimensional space is obtained, and the self-position of the moving object is recognized.

In this case, in order to detect the rotation speed of the moving object by the distant stereo camera 12 and to detect the rotation / translation speed of the moving object by the downward stereo camera 13, the imaging plane of the distant stereo camera 12 must be perpendicular. Axis and the vertical axis of the imaging plane of the stereo camera 13 for the lower side are orthogonal to each other,
Ideally, the distant main camera axis and the lower main camera axis are on the same plane and their reference points coincide with each other.

However, the two main cameras 12
Since it is practically difficult to make the reference points a and 13a the same, the vertical axis of the imaging plane of the distant stereo camera 12 and the vertical axis of the imaging plane of the lower stereo camera 13 are arranged so as to be orthogonal to each other. At the same time, the main camera 12a for the distant place and the main camera 13a for the lower side are arranged close to each other, and the three axes of one camera are arranged so as to be parallel to any one of the three axes of the other camera. The movement of the distant image and the lower image obtained from the pair of stereo cameras 12 and 13 can be handled in one real space coordinate system.

In this case, the lower stereo camera 13
Although the center of the axis is set, even if the distance stereo camera 12 rotates around the lower stereo camera 13 and an offset amount between them occurs, the movement of the distance image is affected by the nature of the distance image as described later. However, since the lower camera axis and the far camera axis are in an orthogonal relationship, the process of removing the rotation speed component from the speed component including translation and rotation and the origin (XYZ coordinate system fixed in space at the distance measurement start point) It simplifies the navigation calculation process from the viewpoint of the origin and enables accurate grasp of the image movement.

The camera used for the camera assembly 10 is a monochrome CCD camera conforming to EIA, a color CCD camera, and the like.
Cameras (including 3-CCD system), infrared cameras, night-vision cameras, etc. that perform area scanning,
A camera or the like that digitally outputs information from an image sensor can be used.

The stereo processing unit 30 digitizes an analog image captured by the camera assembly 10,
By stereo matching between the image (main image) captured by the main camera 12a (13a) and the image (sub-image) captured by the sub camera 12b (13b), it has the same pattern as the imaging target area of the main camera 12a (13a). An area location is searched from the imaging coordinates of the sub-camera 12b (13b) to determine a pixel shift (= parallax) generated according to the distance from the image sensor to the object, and to obtain perspective information on the object obtained from the shift amount. Acquires three-dimensional image information (distance image) that has been digitized.

FIG. 3 shows a detailed configuration of the stereo processing unit 30 and a gain control amplifier (GCA) 31a.
, An analog I / F (interface) 31 for processing an image signal from the camera assembly 10, an A / D converter 32 for A / D converting an output from the analog I / F 31, and a luminance distortion for each pixel. A shading correction memory 33 including a ROM and a RAM for storing shading correction data for correction,
1a and a D / A converter 34 for generating a control voltage to the A / D converter 32, and an FPGA (Field Programmable Gate) having various gates for digital processing such as image correction / conversion and camera control.
Array) 35, an image memory 36 for storing distant and lower main images and sub-images after correction and conversion, a shutter control original image memory 37 for storing image data for camera shutter control, and an image data for optical flow. An optical flow memory 38 for storing, a stereo matching circuit 39 for generating a distance image by performing stereo matching between a main image and a sub image, a distance image memory 40 for storing a distance image, distance data for performing a histogram process described later. Is stored in a histogram distance data memory 41 for storing the data.

Various functions for digital signal processing included in the FPGA 35 are caused by a D / A controller 45 for controlling the D / A converter 34, a difference in output characteristics of an image sensor of each camera, and the like. Look-up table (LUT; composed of ROM) 46 for changing the gain and offset amount of the image signal in order to correct the variation of the mutual image signal, and this LUT 4
6, a multiplier 47 for multiplying the image data corrected in Step 6 by the shading correction data from the shading correction memory 33, and a log (Log) conversion table (ROM) for logarithmically converting the light and dark portions of the image to adjust the sensitivity. 48), an address controller 49, a shading controller 50 for controlling the transfer of shading correction data from the shading correction memory 33 to the multiplier 47, the cameras 12a, 12b, 13a,
There are a camera controller 51 for controlling the shutter speed of 13b and the like, an external I / F 52 for externally rewriting or reading various parameters inside the FPGA 35, and the like.

On the other hand, the optical flow processing unit 60
As shown in FIG. 4, a histogram processing circuit 70 for creating a histogram for evaluating a picture portion (hereinafter, referred to as a No. 1 block) in an image area appropriate for the navigation calculation processing, and images having different imaging times. , An FPGA 61 in which an optical flow processing circuit 80 for searching for the same location as the pattern of the No. 1 block (hereinafter, referred to as a No. 2 block) by a gate, a histogram memory 62 for storing histogram data, an address of the No. 1 block and a scan start of the No. 2 block An optical flow address memory 63 for storing addresses is provided.

The No. 1 block corresponds to the navigation processing unit 9
0, the No. 2 block can be reliably matched to each No. 1 block only by translation even if the image is slightly rotated in a moderately small area. It is said that the distance to the point can be reliably reduced. In the present embodiment, the size of the No1 block is a small area of 12 × 6 pixels.

The histogram processing circuit 70 determines that
For a 12 × 6 pixel area that is a block candidate,
A histogram of distance image values appearing in the corresponding range image region is created. The histogram is referred to when the navigation processing unit 90 determines the No. 1 block, and the reliability of the captured image is evaluated. That is, the distance image is
If the image patterns captured by the left and right cameras are the same, a distance image value corresponding to the amount of displacement in the scanning line direction can be obtained.
Utilizing this feature, if the same distance image value has a predetermined number in the periphery, it is determined that the target image is a real image that exists.

The configuration of the histogram processing circuit 70 is shown in FIG. 5, and controls the operation of the n self-addition type gradation / frequency modules 71 connected in parallel, # 1 to #n. And a preprocessing circuit 73 for calculating the frequency of valid data in advance. still,
As the gradation / frequency module 71, a number corresponding to the larger one of the number of samples and the number of gradations in the input image is used.

Each of the tone / frequency modules 71
As shown in FIG. 6, the circuit includes a gradation latch 74, a coincidence detection circuit 75 using a comparator, an OR circuit 76, a frequency latch 77, and an adder 78. D of the gradation latch 74
The input terminal, the CK terminal, the E terminal, and the SET terminal respectively have a distance data (gradation data) from the histogram distance data memory 41, a synchronization clock, a load signal (latch enable signal) from the control circuit 72, A clear signal for initialization is input, and the match detection circuit 75
The grayscale data from the histogram distance data memory 41 and the latch data from the Q terminal of the grayscale latch 74 are input to the respective input terminals of.

The D input terminal of the frequency latch 77 receives the frequency data from the pre-processing circuit 73 and the latch data from the Q terminal of the frequency latch 77 in the adder 7.
8 is input, and the frequency latch 77
The CK terminal, the E terminal, and the CL terminal are supplied with a synchronous clock, the output of the OR circuit 76, and the clear signal from the control circuit 72, respectively. In the OR circuit 76,
A match determination signal from the match detection circuit 75 is input, and a load signal from the control circuit 72 is input. Then, the coincidence determination signal of the coincidence detection circuit 75 is input to the control circuit 72, and the Q
The gradation data is output from the terminal and the frequency data is output from the Q terminal of the frequency latch 77 and stored in the histogram memory 62.

The optical flow processing circuit 80 formed inside the FPGA 61 is provided with a main image No.
A data transfer buffer 81 that reads luminance data for each pixel from the search area of the No. 2 block of the main image of one block and the next frame, rearranges the data into a data sequence of a predetermined number of bytes, and transfers the data to the next stage. It comprises an arithmetic processing circuit 85 which calculates the city block distance for the two data sequences transferred from the buffer 81, and outputs the position where the city block distance becomes the minimum value as the search result of the No. 2 block.

As shown in FIG. 7, the data transfer buffer 81 includes a reference buffer 81a and a comparison buffer corresponding to a main image as a reference image and a sub image as a comparison image when calculating a city block distance. 81b, the data is read one byte at a time, rearranged into a 6-byte data sequence, and transferred to the arithmetic processing circuit 85 at the next stage.

FIG. 7A shows the reference buffer 81a.
As shown in the figure, an input stage 82 in which a 6-byte buffer is connected to a 1-byte buffer, and a ring buffer 83 in which 12 sets of 6-byte buffers are connected in a ring shape,
The data from a is transferred to the arithmetic processing circuit 85 in a ring shape. Also, the comparison side buffer 81b is configured as shown in FIG.
As shown in the figure, the input stage 82 also includes an input stage 82 in which a 6-byte buffer is connected to a 1-byte buffer, and a ring buffer 84 in which 11 sets of 6-byte buffers are connected in a ring shape. Is discarded, the next data series is read, and similarly, data is transferred from the head output buffer 84a to the arithmetic processing circuit 85 in a ring shape.

That is, by configuring the buffer set in a ring shape, the arithmetic processing circuit 8
5 can be reduced to 1/12, and a 6 × 12 data selector can be dispensed with.

Since the arithmetic processing circuit 85 needs to calculate a city block distance of 12 pixels × 6 pixels = 72 pixels, as shown in FIG. 8, six absolute value calculators 86 and six adders are used. A minimum value detection circuit 85b in which a latch is combined with an arithmetic unit similar to the absolute value arithmetic unit 86 is connected to a city block distance calculation circuit 85a having a pipeline structure in which the units 87 are connected in a pyramid shape. .

In the city block distance calculation circuit 85a,
The first stage of the pipeline structure has an absolute value calculation block composed of six absolute value calculators 86, and the second to fourth stages each have a first addition block composed of three adders 87 and one addition. Adder block consisting of the adder 87, a third adder block consisting of one adder 87, the fifth stage (final stage)
This is a sum adder composed of a plurality of adders 87. A parallel / serial type calculation circuit that inputs six pixels simultaneously to perform parallel calculation and then performs continuous calculation achieves both processing speed and circuit scale. I have.

Next, the navigation processing unit 90 includes an image processor system SYS1 for determining the No. 1 block based on the result of the histogram processing, selecting an optical flow area (setting the scan start position of the No. 2 block), and the like.
It is broadly classified into a navigation processor system SYS2 that calculates the amount of movement between frames based on the result of the optical flow processing, rotation / translation components of the airframe, calculation of a navigation trajectory, and the like.

The image processor system SYS1 includes:
No to store the main image data of the No. 1 block as the reference side image data in the data transfer buffer 81
A one-block main image data memory 91 is provided, and the navigation processor SYS2 includes the arithmetic processing circuit 8
5 is provided with an optical flow data memory 92 for storing the result of the calculation.

In the self-position recognition system having the above configuration, when the system is started by turning on the power, the hardware components are initialized, and the variable values such as the specification constant value of the stereo camera and the navigation processing variable value are initialized. . At the same time, the shading controller 50 transfers the shading correction data in the shading correction memory 33 from the ROM to the RAM.

Next, the horizontal synchronizing signal and the vertical synchronizing signal based on the system clock are transmitted to each of the cameras 12a, 12b and 13 respectively.
a and 13b are supplied as external synchronization signals, and a shutter trigger signal is separately output from the camera controller 51 of the FPGA 35 to each camera of the camera assembly 10 with the shutter speed value written by the navigation processing unit 90.

Then, the stereo processing unit 30 corrects the variance of the two cameras and the variance of the sensitivity of the two cameras with respect to the distant image captured by the stereo cameras 12a and 12b and the lower image captured by the stereo cameras 13a and 13b, respectively. Are stored in the image memory 36, a distance image is generated by stereo matching, and a histogram for evaluating a small area (No. 1 block) usable for the optical flow is obtained from the distance image. It is created by the histogram processing circuit 70 of the processing unit 60.

When histograms are created for each of the far image and the lower image, the navigation processing unit 90 determines No. 1 blocks for each of the far main image and the lower main image with reference to the created histograms.
Further, the search start address of the No2 block is determined.
The head address of the No. 1 block and the search start address of the No. 2 block are written to the optical flow address memory 63 of the optical flow processing unit 60.

In the optical flow processing unit 60, the optical flow processing circuit 80 calculates the city block distance from the No. 1 block in the corresponding area, starting from the search start address of the No. 2 block, and sequentially calculates the city block distance. The area for taking the value is set as a No2 block, and the address of the area is output.

Thus, the navigation processing unit 90 calculates the speed component (speed component including rotation and translation) of the moving object (helicopter) from the amount of movement between the No. 1 block and No. 2 block in the lower image. A net translation speed component is obtained by subtracting the rotation speed component calculated from the movement amount between the No. 1 block and the No. 2 block of the lower image from the speed component. Then, the navigation trajectory is calculated by accumulating the net translation speed components.

The timings of the above processing are all managed by a line number which is an internal counter of the self-position recognition system, and this line number is reset at the end of the image sample. In the present embodiment, the image is counted up from 00H by the system clock, and the image of the next frame is sampled in the section from 82H to 9CH.
It is reset at the end (9CH).

Next, the operation of each unit after sampling the captured image will be described in detail. First, in the analog I / F 31, the gain of the image signal is adjusted by the GCA 31a according to the control voltage from the D / A converter 34, and the variation in the gain of the two cameras for each of the stereo cameras 12 and 13 is adjusted. Output to the A / D converter 32. A /
In the D converter 32, each of the stereo cameras 12, 13 is controlled by a reference voltage from the D / A controller 45.
Adjust the offset difference between the image signals of the two cameras every time,
The analog image is converted into a digital image having a predetermined luminance gradation (for example, 256 gray scales). The above G
As for the gain control voltage of the CA 31a and the reference voltage of the A / D converter 32, the voltage value written in the FPGA 35 by the navigation processing unit 90 is transferred to the D / A controller 45.

The digital image data from the analog I / F 31 via the A / D converter 32 and the gain / offset of each of the stereo cameras 12 and 13 are matched.
The data is supplied to the LUT 46 in the FPGA 35 as address data, and the data from the LUT 46 and the shading correction data from the shading correction memory 33 transferred according to an instruction from the shading controller 50 are multiplied by the multiplier 47. As a result, the gain and the offset are more strictly matched for each of the stereo cameras 12 and 13, and a reduction in luminance due to a shading phenomenon occurring in the optical system of each of the stereo cameras 12 and 13 is corrected.

Next, the image data output from the multiplier 47 is logarithmically converted by the log conversion table 48 to remove the effects of noise components proportional to the brightness, and the distant main image, distant sub-image, The data of the lower main image and the lower sub image are stored in a form suitable for the processing in the stereo matching circuit 39 at the next stage. The image data stored in the image memory 36 is data for every four horizontal lines. At the same time, one horizontal line of image data is stored in the original image memory 37 for shutter control, and The main image data at the far end and the lower end are all stored in the flow memory 38.

After that, the original image data in the image memory 36 is transferred to the stereo matching circuit 39 in 4-byte units.
Stereo matching between the main image and the sub image is performed up to a 100 pixel shift in the area of 4 × 4 pixels, and a distance image as shown in FIG. 9 is generated and written in the distance image memory 40 and the histogram distance data memory 41. . The processing of generating a distance image from a captured image of a stereo camera is described in Japanese Patent Application Laid-Open No. 5-114 by the present applicant.
No. 099.

When the distance image is obtained by the above-described stereo processing, the optical flow processing unit 60 generates a histogram of the distance data of the area of 12 × 6 pixels for extracting the No. 1 block from the original image as shown in FIG. It is created by the histogram processing circuit 70. This histogram creation processing is performed by shifting the distance image of 400 × 200 pixels by 4 pixels in the vertical direction and 4 pixels in the horizontal direction, and is performed for 98 horizontal lines and a total of 98 horizontal × 49 vertical regions.

More specifically, in the histogram processing circuit 70, first, a module number counter in the control circuit 72 for designating the number of the gradation / frequency module 71 for latching data is initialized to 0 by the clear signal. The frequency latches 7 in all the gradation / frequency modules 71
7 is set to 0, and the gradation latch 74 is initialized to a set value (FFH when the maximum value of the input gradation data is 64H). Further, the frequency of the data in the area of the first 4 × 4 pixels is counted by the preprocessing circuit 73.

After the initialization, when representative gradation data (distance data) of the first small area of 4 × 4 pixels is input,
The module number counter inside the control circuit 72 is set to 1 and loaded into the gradation latch 74 # 1 in the gradation / frequency module 71 # 1 of # 1 (hereinafter, the module numbers are indicated by subscripts # 1 to #n). The signal is output and the OR circuit 76 is output.
A load signal is output to frequency latch 77 # 1 via # 1. As a result, the grayscale latch 74 # 1 is enabled and the first grayscale data is written and latched by the synchronous clock, and the frequency latch 77 # 1 is enabled and the frequency data after preprocessing is performed by the synchronous clock. Is written to the frequency latch 77.

Next, when the gradation data of the second small area is input to the gradation / frequency module 71 # 1, the gradation / frequency module 71 # 1 latches the data by the internal gradation latch 74 # 1. The match detection circuit 75 # 1 compares the gradation data of the first small region and the gradation data of the second small region, and outputs a match determination signal to the OR circuit 76 # 1 and the control circuit 72.

This match determination signal is a signal of 1 when the data matches, and a signal of 0 when the data does not match. When the output of the match detection circuit 75 # 1 is 1, ie, the floor of the first small area. When the tone data matches the tone data of the second small area, the frequency latch 77 # 1 is enabled via the OR circuit 76 # 1, and the frequency data latched by the frequency latch 77 # 1 and the current The input frequency data and the adder 7
The result is added at 8 and written to the frequency latch 77 # 1.

On the other hand, when the output of the coincidence detection circuit 75 # 1 is 0, that is, the gradation data of the latched first small area does not match the gradation data of the second small area inputted this time. Occasionally, the latch data is held via the OR circuit 76 # 1 while the frequency latch 77 # 1 is in a disabled state, while the control circuit 72 sets the internal module number counter to 2 and sets the # 2 gradation / frequency Module 71 # 2
Output the load signal. As a result, the grayscale latch 74 # 2 and the frequency latch 77 # 2 inside the # 2 grayscale / frequency module 71 # 2 are enabled, and 2 is input to the grayscale latch 74 # 2.
While the gradation data of the second small area is written, the frequency data of the second small area is written to the frequency latch 77 # 1.

The above operation is repeated, and when the processing for one 12 × 6 area is completed, the gradation and frequency data are read out from the # 1 gradation / frequency module 71 # 1 in order and stored in the histogram memory 62. After that, the operation is stopped in the module having the frequency of 0, the re-initialization is performed by the clear signal, and the process proceeds to the next 12 × 6 area.

That is, as shown in the time chart of FIG. 11, the gradation data and the frequency data calculated in the preprocessing are latched in each module as input data of each module, and the inputted gradation data is stored in the past. If the data coincides with the gradation data latched in the module, the frequency data is accumulated by adding new frequency data to the frequency data latched by the module and re-latching.

On the other hand, if the data does not match in any module in which the grayscale data has been latched in the past, the module number counter in the control circuit 72 is counted up and a load signal is output to the new module, and the new grayscale data is output. Repeat the process of latching the data and frequency data in the new module, all types of gradation data of the input image and frequency data after pre-processing are latched one after another,
Frequency data is accumulated.

The histogram created by the histogram processing circuit 70 is referred to by the image processor SYS1 of the navigation processing unit 90, and the No. 1 block is determined for each of the far main image and the lower main image. The determination of the No1 block is performed by using the No1 block shown in FIG.
This is performed according to a block group acquisition processing routine.

In this routine, first, at step S30
Then, it is selected whether the captured image is to be processed far or downward. The selection of this processing is as follows. Since the processing timing of the captured image is that the distant image is first and the lower image is next, it is assumed that the previously captured image is the distant image, and the process proceeds to the distant processing after step S31. , The process proceeds to the downward process after step S33.

In the distant processing after step S31, in step S31, a scanning range in which a distant image is searched for the No. 1 block is set as initial processing. This scanning range includes, for example, a search margin i2 at both ends of the screen in the horizontal scanning direction and a search margin i2 at both ends of the screen in the vertical scanning direction in a whole area W × T where W is the horizontal scanning direction of the imaging surface and T is the vertical scanning direction. Area WA × TSL excluding search margin j2 (WA = W−2 · i2,
TSL = T−2 · j2), the width WA in the horizontal scanning direction is equally divided into ten, and ten search areas WS × TSL are set. That is, a maximum of one No1 block is extracted for each search area WS × TSL, and the extraction locations are not concentrated on a specific area of the imaging screen without extracting two or more No1 blocks from the same search area. To do.

Next, in step S32, the search area WS ×
A picture portion of 12 × 6 pixels is cut out from the TSL, the image quality of the cut portion is evaluated, and the No. 1 block is determined. The quality of the image quality of the 12 × 6 pixel picture portion is determined based on the histogram of the 12 × 6 pixel area of the corresponding distance image.

That is, if there is variation in the distance information of the 12 × 6 pixel range image area, the No. 1 block to be determined from now on does not look at a certain point (one point), but in the same window (small window). This means that a plurality of objects having different perspectives are viewed in the region, and a plurality of objects having different perspectives appear as noise in the histogram. Therefore, the distance image value maxz having the highest appearance frequency in the histogram and its maximum histogram value ma
xnum, and the maximum histogram value maxnum
By examining the frequency of appearance of other range image values in the vicinity,
Candidates for objects in the area to be used can be determined.

In this embodiment, the range image values included in the range image region of 12 × 6 pixels and the respective histogram values are examined by narrowing down to the range of maxz ± 2, and the appearance of the range of maxz ± 2 The total frequency dsum of the frequency is calculated by the following equation (1), and the frequency of “OK information” within an allowable range defined by a predetermined threshold satisfies a predetermined number, and
When the total frequency dsum in the range of maxz ± 2 occupies most of the frequency of “OK information” (for example, 80% or more), the extracted 12 × 6 pixel picture portion is determined to be a reliable image. , No. 1 block. dsum = m2 + m1 + maxnum + p1 + p2 (1) where m2: frequency of occurrence in maxz-2 m1: frequency of appearance in maxz-1 p1: frequency of appearance in maxz + 1 p2: frequency of appearance in maxz + 2

Next, when the pattern extraction of the No. 1 block is determined, the average distance image value fz1 of the distance image area of 12 × 6 pixels is calculated as follows in order to accurately know the distance value to the object required for the navigation calculation. 2) Calculate by equation. fz1 = zsum / dsum (2) where zsum = (maxz−2) · m2 + (maxz−
1) · m1 + maxz · maxnum + (maxz + 1) · p
1+ (maxz + 2) · p2

The above-described No. 1 block acquisition processing for the distant image is performed for each search area WS × TSL. When a maximum of 10 No. 1 blocks are acquired for the entire image, the processing shifts to the downward processing from step S33. In addition, the search area WS
If an appropriate No1 block is not found in × TSL, the extraction in the corresponding search area is stopped.

In the lower process, in step S33, a scan range for searching for the No. 1 block is set as an initial process for the lower image. The scanning range of this lower image is a region WS × TA in which the vertical scanning direction is TA with respect to the ten search areas WS × TSL of the distant image, and is offset by a predetermined amount from the center line Th of the screen toward the traveling direction. This is an area where the line Tt is a search start line, and the width TA in the vertical scanning direction is divided into three parts so that 30 search areas WS can be extracted so as to extract a characteristic picture dispersed throughout the screen.
X Set TSG.

In this case, the offset line Tt is
It is made to vary according to the translation speed obtained in the previous processing, and taking into account the movement of the lower image, the imaging surface is used effectively according to the forward / reverse speed.

Next, in step S34, the search area WS
A picture portion of 12 × 6 pixels is cut out from the × TSG, and the image quality of the cut portion is evaluated to determine the No. 1 block. The determination of the No. 1 block in the lower image is the same as the determination of the No. 1 block in the distant image described above, except that the threshold for selecting the “OK information” is different. That is, in order to calculate the translation speed component from the motion of the lower image, in a region where there are many distance image data,
In addition, a threshold value is determined for a planar area having a small variation as a candidate for the No. 1 block.

Then, in step S34, when a maximum of 10 No. 1 blocks are obtained in the horizontal scanning direction,
Proceeding to step S35, it is checked whether or not all the search areas in the lower image have been processed. If not all processing has been completed, the flow returns to step S34 to continue the processing for the next lower search area. . And this step
The loop of S34 → Step S35 → Step S34 is repeated three times to perform processing for 30 search areas. Also in this case, in the search area WS × TSG, an appropriate No. 1
If no block is found, the extraction in the corresponding search area is stopped.

As described above, when the No. 1 block group is acquired for each of the far image and the lower image, the image processor SYS1 further sets a search start area for searching for a No. 2 block having the same pattern as the No. 1 block for a predetermined time. The location of the No1 block after the elapse is predicted and set.

That is, when the No1 block is determined,
Since the representative coordinates of the No. 1 block on the imaging surface are determined, first, four points a, given a predetermined offset amount (xf, yf) in the vertical and horizontal directions around the representative coordinates of the No. 1 block.
b, c, d are determined. And these four points a, b, c,
d in a rectangular frame, that is, (2 · xf) × (2 · y
In the area of f), the imaging information when there is no motion on the screen is No.
A search area in which the same pattern as one block is predicted is set as a search area, and the search area is arranged in consideration of the features of the distant and lower images.

In the case of a distant image, a distance sufficiently far (for example, 30 m or more) from the measurement point is measured, so that it is hard to receive an image change due to vertical and horizontal translational movement in a short time. The motion of the No. 1 block obtained in is captured as a motion due to the rotation component. Therefore, in the distant image, the coordinate values of the four points a, b, c, and d in the search area are converted by the rotation angle α around the origin O of the imaging plane coordinates, and the four points a, b, c, and d are connected. Set a rectangular search area.

On the other hand, in the case of the lower image, it is difficult to determine whether the movement amount on the imaging surface is caused by a translation component or a rotation component. For this reason, the location of the No. 1 block after a lapse of a predetermined time is predicted, and a search area for the No. 2 block after the next frame is set as the destination. That is,
When the processing speed is sufficiently fast with respect to the movement of the helicopter, or when the actual imaging range is wide, the movement of the image for each screen is not large, so the search area may be set to the maximum change amount per unit time as the search range. However, when the helicopter moves greatly, the movement is predicted on the assumption that the processing speed and the imaging range are not sufficient, and the search area is set with reference to the past speed components.

In order to predict the location of the No. 1 block after the lapse of a certain time, a predetermined time is calculated from the roll, pitch, and yaw components obtained by the previous angular velocity calculation and the net translation amount obtained by the previous translation speed calculation. The movement destination of the subsequent No. 1 block is converted into the movement of the imaging plane coordinates (virtual optical flow). Then, the frame in which the No1 block is determined and the No.
A search area for the No. 2 block is set based on a virtual optical flow between the target frame for searching for the two blocks.

The image data of the No. 1 block determined as described above is stored in the No. 1 block main image data memory 91 of the image processing system SYS1.
The address of one block and the search start address of the No. 2 block are stored in the optical flow address memory 63 of the optical flow processing unit 60.

In the optical flow processing unit 60,
The data from the one-block main image data memory 91 and the data sampled in the next frame and stored in the optical flow memory 38 are read into the data transfer buffer 81 of the optical flow processing circuit 80,
The arithmetic processing circuit 85 calculates the city block distance and returns No.
Search for two blocks.

That is, as shown in FIG.
12 × 6 of the No1 block determined in the image at t1
Pixel data is ai, j (i = 0 to 11, j = 0 to 5),
The data of 12 × 6 pixels in the search area of the No. 2 block of the image at time t = t1 + Δt is represented by bi, j (i = 0 to 1).
(1, j = 0 to 5), the position where the city block distance H shown in the following equation (3) takes the minimum value is the position of the No. 2 block, that is, the position after the movement of the No. 1 block. still,
In the equation (3), the first Σ is the sum of j = 0 to 5, and the next Σ is i =
Shows the sum of 0-11. H = ΣΣ│ai, j-bi, j│… (3)

To calculate this city block distance,
In the data transfer buffer 81, the image data is read one byte at a time from the No. 1 block main image data memory 91 into the reference buffer 81a, and the image data is read one byte at a time from the optical flow memory 38 into the comparison buffer 81b. When the sets are completed, they are transferred to the ring buffers 83 and 84, respectively.

In each of the ring buffers 83 and 84, the first buffer set is transferred to the arithmetic processing circuit 85 and, at the same time, to the next buffer. By repeating this operation 12 times and transferring data to the arithmetic processing circuit 85, 1
Two city block distances are calculated.

When the next round of the city block is calculated after the ring has completed one round, the reference side buffer 81a stores the first data in the output buffer 83a which is the head buffer of the twelve sets of ring buffers 83, and In the buffer 81b, the second data is stored in the output buffer which is the head buffer of the 11 sets of ring buffers 84 (the first data is discarded). This time, only by rotating the ring, the second to twelfth data on the comparison side are sent to the arithmetic processing circuit 85, and the thirteenth data read during the last rotation of the ring is sent to the arithmetic processing circuit 85. Can be transferred.

The arithmetic processing circuit 85 calculates the city block distance of 12 × 6 = 72 pixels. Therefore, the six absolute value calculators 8 in the first stage of the city block distance calculation circuit 85a are used.
Data of 6 pixels is input to 6 and the absolute value of the luminance difference is calculated. That is, the luminance of the pixel on the comparison image side is subtracted from the luminance of the pixel on the reference image side, and when the result is negative, 2
Calculate the absolute value from the complement of.

Next, after passing through the first stage, the second stage
Each of the adders 87 adds two simultaneous input data from the absolute value calculator 86 at the first stage. At this time, the six absolute value calculators 86 in the first stage simultaneously calculate the absolute values of the next six pixels. Similarly, the outputs of the previous stage are added in the third and fourth stages, and the output value of the fourth stage and its own output value are added in the fifth stage to obtain the sum. When the 12 addition is completed at the fifth stage, the calculated value of the city block distance for 6 × 12 = 72 pixels is output.

The above operation is performed by calculating the city block distance by incrementing the search start address of the No. 2 block by one, and calculating the calculated value of the city block distance and the calculated value of the city block distance. The address is output to the optical flow data memory 92 of the navigation processor SYS2. When the processing for one area is completed, the address of the next No. 1 block is referred from the optical flow address memory 63, and the image data of the next No. 1 block is transferred from the optical flow memory 38 to the No. 1 block main image data memory 91. I do.

Next, in the navigation processor SYS2, the rotation / translation speed is calculated using the optical flow processing result, and the own position is recognized from the navigation trajectory based on the calculation result. In other words, the navigation processor SYS2 calculates the difference between the real distance components based on the optical flow of the distant main image, calculates the rotational speed component (the angular speed of roll, pitch, and yaw) from the difference between the real distance components. A difference between real distance components is obtained based on the optical flow of the lower main image, and a net translation speed in a three-dimensional direction (X, Y, Z directions) is obtained by removing a rotational speed component from a speed component due to the difference between the real distance components. . Then, the translation speed is converted into a three-dimensional speed as viewed from the origin,
The velocity component is accumulated from the initial point to determine the movement amount from the origin and the current position.

The process of calculating the rotation speed component from the movement of the distant image is performed by the rotation calculation processing routine of FIG. In this routine, first, in step S51, No
The array pattern of one block group is compared with the array pattern of the No. 2 block group searched by the optical flow processing unit 60 to evaluate the reliability of the optical flow.

That is, due to the nature of the distant image, the relative positional relationship (array pattern) of each block hardly changes in the ideal image motion of No. 1 block to No. 2 block due to the rotational speed component. Therefore, the arrangement pattern of the No. 1 block group is compared with the arrangement pattern of the No. 2 block group, and when the arrangement of each block is clearly different, the corresponding block is excluded from both the No. 1 and No. 2 block groups.

The arrangement pattern is evaluated by comparing the distance between each block in the No. 1 block group and the area of the triangle formed by each block with the value in the No. 2 block group. For example, focusing on one block A, in order to evaluate whether block A is selected at a correct position in the No2 block group, first, N
The distance (A1-B1) between the block A and the other block B in the o1 block group is compared with the distance (A2-B2) between the block A and the block B in the No2 block group. , And blocks A to D, the distances are also compared.

Then, for all three, the No. 1 block group and the No.
If the distances between the two blocks are equal, the distance check is OK. Otherwise, the next blocks E, G,
Change the blocks to be sequentially compared with… and three OK first
, The distance check is OK, and if three NGs are found first, the distance check is NG.

Thus, the three points (B, C,
If the check of the distance to D) is OK, then the area ABC of the triangle composed of the representative coordinates of the blocks A, B, and C is obtained for the No. 1 block group and the No. 2 block group, and the sizes are compared. I do. This comparison is also performed for the triangle ACD.
If the area is the same as that of the o2 block group, it is finally determined that the position of the block A is correctly selected in the No2 block group.

At this time, the check of the distance is made by comparing the square values according to the Pythagorean theorem, and the check of the area of the triangle is performed by calculating the cross product of the vectors and calculating a value twice as large as the area of the triangle (parallelogram). Area),
Increase calculation efficiency.

After the improper blocks are eliminated by the above evaluation of the array pattern, the process proceeds to step S52, and it is checked whether or not the number of No. 2 blocks after eliminating the improper blocks is a predetermined number. If the predetermined number required to maintain the calculation accuracy has not been reached, the above-described step S5
After branching from step 2 to step S55 and updating the memory with the previous calculation result of the angular velocity component as the value of the current process, step
Proceeding to S56, when a predetermined number of No2 blocks can be secured, the process proceeds from step S52 to step S53.

In step S53, the coordinates of the imaging surface of the No. 2 block are converted into real space coordinates, which are X, Y, and Z orthogonal coordinates fixed to the camera. Lens focal length f, main camera axis Z
If the base length LS (Ss) between m and the sub-camera axis Zs and the displacement Xr of the stereo image are given, the distance Z to the distance measurement target point M is given by the following equation (4) by triangulation. The imaging plane coordinates are converted into real space coordinates based on the relationship.

Z = LS (SS) · f / Xr (4) In the following step S54, N is calculated by the nonlinear least square method.
An angular velocity (roll, pitch, yaw) corresponding to the difference between the positions of the o1 block group and the No2 block group is obtained.
That is, the difference between the position of the No. 1 block and the position of the No. 2 block is obtained by converting the speed component of the previous rotation / translation into the initial value and calculating the difference between the positions of the No. 1 block and the No. 2 block. Partial differentiation is performed for each rotation component (Z axis), an appropriate declination is given, and N on the image sensor for each roll, pitch, and yaw
The change amount (dx, dy) of the difference between the position and the o1 block is obtained. Each component of roll, pitch, and yaw is given as the limit of this variation (dx, dy).

Therefore, the processing for obtaining the above-mentioned variation (dx, dy) is performed for all No. 1 and No. 2 blocks, and the rotation component is obtained by the least square method. In this least squares method, 3
The original linear equation is to be solved. However, since the transpose of the coefficient matrix is equal and the diagonal elements are sufficiently larger than the other elements (positive definite symmetry), the numerical calculation by the Cholesky method must be performed. This can be applied, and the respective values of roll, pitch, and yaw for the No1 block can be obtained.

Actually, the solution obtained by the above method does not become a true value because it is nonlinear. Accordingly, the true value is brought close to the true value by the non-linear least square method in which the obtained value is set as the initial value and the least square method is repeated again. When the true value is obtained, the convergence of the variation (dx, dy) is obtained. There is a need to monitor and determine the condition (ideally 0). That is, when obtaining a limited range of angular velocity components with appropriate accuracy, the number of repetition calculations is set to a predetermined number (for example, 5 times).
And the time required for processing can be reduced.

When the angular velocity component in the XYZ rectangular coordinate system fixed to the camera is obtained by the processing in step S54,
Next, the routine proceeds to step 56, where the roll component rl, pitch component pt, and yaw component yw are developed into elements of the rotation matrix Rot, and the routine exits. The rotation matrix Rot is a product of the rotation matrices for each of the roll, pitch, and yaw axes. For example, as shown by the following equations (5) to (13), the rotation matrix Rot is expanded as a 3 × 3 square matrix element. it can. Rot [0] [0] = (cy · cr) − (sy · ans) (5) Rot [1] [0] = − (cp · sr) (6) Rot [2] [0] = ( (sy · cr) + (cy · ans) (7) Rot [0] [1] = (cy · cr) + (sy · ans') (8) Rot [1] [1] = (cp · cr) ) ... (9) Rot [2] [1] = (sy.sr)-(cy.ans') ... (10) Rot [0] [2] =-(sy.cp) ... (11) Rot [1] ] [2] = sp (12) Rot [2] [2] = (cy · cp) (13) where cy = cos (yw) cr = cos (rl) cp = cos (pt) sy = sin (yw) sr = sin (rl) sp = sin (pt) ans = sp.s rans' = sp.cr

Next, a process for obtaining a pure translation speed component from the motion of the lower image including the rotation speed component and the translation speed component will be described. This processing is performed by the translation calculation processing routine of FIG. 15. First, in step S60, it is determined whether or not a predetermined number of samples required to maintain the calculation accuracy has been acquired in the No. 2 block group. Is obtained, the process proceeds from step S60 to step S61.If the predetermined number of samples cannot be obtained, step S6
From 0, the process proceeds to step S71 to update the memory with the previous calculation result of the translation speed component as the value of the current process, and exits the routine.

In step S61, the average distance image value of the No. 2 block is evaluated. This evaluation is for eliminating blocks in which the distance image value differs between the No. 1 block and the No. 2 block by a threshold value or more, and when there is a corresponding block, all the No. 2 block groups except the block were evaluated. The process jumps to step S67 to check whether or not the block whose distance image value differs by more than the threshold is excluded from the entire No. 1 block group and No. 2 block group by a loop returning from step S67 to step S61.

After that, the process goes to step S62 after the evaluation of the average distance image value of the No. 2 block in step S61, and the individual samples of the No. 1 and No. 2 block groups are corrected to the image pickup plane coordinate values corrected for the lens distortion. Update to a new No1 and No2 block group with little variation in ranging accuracy. Step S61 → Step S67 → Step S6
When the number of blocks excluded in the loop 1 is large and the number of samples necessary to maintain the calculation accuracy cannot be secured, the translation calculation processing is terminated with the previous calculation result of the translation speed component as the value of the current processing.

In the following step S63, the coordinates of the real space (XYZ orthogonal coordinates fixed to the camera) are obtained from the imaging surface coordinate values of the No. 1 and No. 2 blocks and the respective distance image values, and in step S64, the values are obtained from the No. 2 block. The real space coordinate value is converted to the real space coordinate value of the pure translation component obtained by subtracting the rotation component from the latest rotation matrix, and the process proceeds to step S65, where the motion (movement vector) of the image is calculated by the difference between the real space coordinate value. Ask.

That is, since the imaging plane of the distant image and the imaging plane of the lower image are orthogonal to each other, the amount of the movement vector between the No. 1 and No. 2 blocks including the rotational movement and the translational movement was previously determined. Rotation matrix Rot (No2 → No)
By multiplying (by rotating relatively) one block, the motion of the rotation component can be removed in the class of the real distance.

Then, the process proceeds to a step S66, wherein the real space movement amount is newly recorded and the number of blocks measured is counted.
In step S67, it is checked whether or not all of the No. 2 block group has been evaluated. When all evaluations have not been completed, the process returns to step S61. When all evaluations have been completed, the process proceeds to step S68, where the direction and size of the entire optical flow are abnormal due to statistical processing such as section estimation. Filter processing is performed to eliminate the movement vector (singular vector), and the average movement amount after removing the singular vector is calculated.

Then, in step S69, the singular vector removal result is checked. If the number of singular vectors is large and the number of data required to maintain the accuracy of the navigation calculation cannot be secured, the previous step S71 is executed. The memory is updated with the result of the translation speed component calculation as the value of the current process, and the routine exits. On the other hand, if there is almost no singular vector or the number is such that the accuracy of the navigation calculation is not impaired, the process proceeds from step S69 to step S70, where the three-dimensional translation speed | S | of the component (X1, Y1, Z1) Is obtained, the memory value is updated at the new translation speed, and the routine exits.

Subsequent to the above-described translation calculation processing, the navigation calculation processing of FIG. 16 for obtaining the navigation trajectory is executed. In this navigation calculation process, first, in step S80, the origin (distance measurement start point)
Obtain the posture state of the moving body as viewed from. The posture state of the moving object viewed from the origin is the XYZ fixed in space at the distance measurement start point.
It is represented by a rotation matrix Rots viewed from a rectangular coordinate system. For this reason, first, a rotation matrix is unitized at the distance measurement start point and initialized, so that a plane orthogonal to the imaging plane is used as a reference plane representing the real space. A matrix obtained by multiplying the rotation matrix (Rot matrix viewed from the XYZ orthogonal coordinate system fixed by the camera) Rot from the right is defined as a rotation matrix Rots representing the attitude of the moving body viewed from the origin.

That is, the rotation matrix Rots representing the attitude viewed from the origin is a square matrix obtained by integrating the rotation matrix Rot for each frame viewed from the camera-fixed coordinate system in a time series. As shown, the orientation viewed from the origin in the current process is a square matrix obtained by integrating the rotation matrix Rot between frames obtained in the current rotation calculation process from the right into the rotation matrix Rots representing the orientation state viewed from the origin up to the previous time. By updating the rotation matrix Rots to represent, the latest posture of the moving body viewed from the origin can be represented. Rots = Rots · Rot (14)

By using the rotation matrix Rots, the angular velocity components Roll (Roll), Picth (pitch), Yo as viewed from the origin can be calculated by the following equations (15) to (17).
w (yaw) can be obtained. Roll = atan (Rots [0] [1] / Rots [0] [0]) ... (15) Pitch = atan (Rots [1] [2] / Rots [2] [2]) ... (16) Youw = atan (Rots [2] [0] / Rots [2] [2])… (17)

Next, proceeding to step S81, the translation speed component between the frames is converted into a translation speed component viewed from the origin. In step S82, the speed component viewed from the origin is accumulated for each frame, and the speed component from the initial point is calculated. Find the real space coordinates of the moving object.

The translation speed So viewed from the origin is calculated by multiplying the product of the rotation matrix Rots representing the posture viewed from the origin and the three-dimensional translation speed S between frames by the previous equation as shown in the following equation (18). It can be obtained by accumulating the translation speed So seen from the origin, and by multiplying the accumulated vector amount by a predetermined distance conversion value, a navigation trajectory can be obtained, and the moving amount of the moving body from the origin can be calculated. You can know. So = So + (Rots) · S (18)

In this case, a landmark is set at an appropriate point on the ground, this landmark is identified by pattern recognition or the like, and the origin is initially set as a distance measurement start point. The position can be recognized. Furthermore, by initializing a proper point on the ground as an origin based on positioning information from a satellite positioning system such as a GPS or known map information, it is possible to recognize the absolute position of the vehicle from the navigation trajectory.

[0115]

As described above, according to the present invention, an optical flow can be detected in real time by a high-speed hardware circuit, and an optical flow can be detected online from a captured image using a small device that can be mounted on a moving body. , The self-position of the moving body can be recognized, and excellent effects can be obtained.

[Brief description of the drawings]

FIG. 1 is an overall configuration diagram of a self-position recognition system.

FIG. 2 is a configuration diagram of a camera assembly.

FIG. 3 is a block diagram of a stereo processing unit.

FIG. 4 is a block diagram of an optical flow processing unit.

FIG. 5 is a configuration diagram of a histogram processing circuit.

FIG. 6 is a circuit configuration diagram of a self-addition type gradation / frequency module.

FIG. 7 is a configuration diagram of a data transfer buffer.

FIG. 8 is a configuration diagram of an arithmetic processing circuit;

FIG. 9 is an explanatory diagram of stereo processing.

FIG. 10 is an explanatory diagram of a histogram process.

FIG. 11 is a time chart of a self-addition type gradation / frequency module.

FIG. 12 is an explanatory diagram of an optical flow process.

FIG. 13 is a flowchart of a No1 block group acquisition processing routine.

FIG. 14 is a flowchart of a rotation calculation processing routine.

FIG. 15 is a flowchart of a translation calculation processing routine.

FIG. 16 is a flowchart of a navigation calculation processing routine.

[Explanation of symbols]

 Reference Signs List 10 camera assembly 20 self-position recognition device 30 stereo processing unit 39 stereo matching circuit 60 optical flow processing unit 70 histogram processing circuit 81 data transfer buffer 85 arithmetic processing circuit 90 navigation processing unit SYS1 image processing Processor system SYS2: Navigation processor system

Claims (2)

[Claims] 1. An optical flow detection apparatus for an image, wherein one of two images taken at different timings is used as a reference image and the other is used as a comparison image, and a corresponding position is searched for an optical flow between the images. The data in which the luminance data of each pixel in the predetermined area of the reference image and the luminance data of each pixel in the search area of the comparison image are rearranged into a data sequence of a predetermined number of bytes, and each data sequence is transferred in a ring shape. The transfer buffer and the absolute value of the difference between the two systems of data transferred from the transfer buffer are calculated in parallel by a plurality of arithmetic units, and the outputs of the arithmetic units are added in series by a plurality of adders. A function of calculating a city block distance and outputting an address at which the city block distance becomes a minimum value as a corresponding position of the comparison image with respect to a predetermined area of the reference image. Optical flow detecting device for an image, characterized in that it comprises a processing circuit. 2. An imaging unit having a pair of stereo cameras mounted on a moving body to capture a distant landscape and a pair of stereo cameras capturing a lower landscape, and two images captured by the stereo camera. A distance image generation unit that searches for a corresponding position in the image of FIG. 1 and obtains a pixel shift amount that occurs according to the distance to the object, and generates a distance image in which the perspective information to the object obtained from the pixel shift amount is quantified. And holding the gradation data and the frequency of the distance image in a latch, and comparing the newly inputted gradation data with the held gradation data by a coincidence output from a comparator. A histogram generation unit that generates histogram data by integrating the frequencies of the histograms, and refers to the histogram data generated by the histogram generation unit from the distance image, A plurality of regions of a characteristic pattern suitable for navigation calculation are extracted as a first block, and a region corresponding to the first block is extracted from an image whose imaging timing is different from an image from which the first block is extracted. An image processing unit that sets a search range for searching as two blocks; and a predetermined number of bytes each of the luminance data of each pixel of the first block and the luminance data of each pixel of the search range of the second block. A data transfer buffer that rearranges the data series and transfers each data series in a ring shape, and the absolute value of the difference between the two systems of data transferred from the transfer buffer is calculated in parallel by a plurality of arithmetic units. An arithmetic processing circuit that calculates the city block distance by adding the outputs of the arithmetic units in series by a plurality of adders, and outputs an address at which the city block distance is the minimum value. An optical flow processing unit having a road; and an optical flow from the first block to the second block based on an output from the optical flow processing unit for a captured image of a distant landscape and the distance image. And calculating the optical flow from the first block to the second block based on the output from the optical flow processing unit for the captured image of the lower scenery and the distance image. A velocity component between frames of the moving body is calculated, and a translation velocity component between frames obtained by removing the rotation velocity component from the velocity component is converted into a translation velocity component viewed from a distance measurement start point, and the converted translation velocity component is converted. Accumulate 3
A self-position recognition system for a moving object, comprising: a navigation processing unit for recognizing the self-position of the moving object by obtaining a navigation trajectory in a three-dimensional space.