JPH01140014A - Estimating method of state of moving body - Google Patents

Abstract

PURPOSE:To enable the easy execution of collating a distorted observed image obtained by a sensor of a moving body making an arbitrary motion, with a map, and also making said image correspond to the map, by producing a map image from map data in accordance with prior information on the position and attitude of the moving body. CONSTITUTION:Sensed image data loaded from a sensor 1 mounted on a moving body are stored in an image memory and displayed simultaneously. Data processing device CPU 5 executes a process of estimating the position and attitude of the moving body. From the map data in a memory 6 and prior information 7 on the position and attitude of the moving body stored in the memory, a map image having distortion corresponding to an image sensed by the sensor 1 set in the state conforming to said prior information 7 is generated, and it is stored in the memory 2 and displayed 3. Next, the coordinates in an image of a characteristic point indicated by a user on the image displayed 3 are fetched, and parameters regarding the position and the posture are estimated from the coordinates, map coordinates of a corresponding point in the map data 6 and the prior information 7. The prior information 7 is updated by the values of the parameters, while the updated information is outputted outside 8 for control and operation of the moving body.

Description

[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to image processing by a sensor mounted on a moving object, and in particular to a man-machine display/processing method suitable for estimating the position and orientation of a moving object in real time. Regarding.

[Conventional technology]

Estimating the motion state of the moving object at the time of image capture, that is, the position and orientation in three-dimensional space, from the position of a feature point in an image captured by a sensor mounted on the moving object and the coordinates of the feature point in three-dimensional space, that is, map coordinates. The method is called orientation and has been known for a long time. The method is described, for example, in Japanese Patent Application No. 60-195849.

The above-mentioned conventional technology, for the purpose of correcting distortion of images taken by earth observation satellites, calculates the parameters of the orbit and attitude of the satellite by a minimum of It is estimated by multiplication. In the case of earth observation satellites, the orbit and attitude of the satellite are controlled to be constant, so the shape distortion of captured images is relatively small, and the distortion of captured images is approximately reproducible for the same area. Processing is also normally performed as offline batch processing. In this case, the position of the feature point in the image, which is necessary as an input for the estimation process, is detected by one of the following methods.

(1) Read the latitude, longitude, and altitude of the relevant feature point from the map in advance, and then cut out the relevant location from the map and a satellite image photograph or aerial photograph of the same area taken in advance. is prepared as an album. When performing estimation processing to process satellite images, the operator refers to the above-mentioned album and reads the pixel coordinates of feature points from the image to be processed displayed on the display.

(2) Cut out a small image piece (for example, 32 x 32 pixels) centered on the relevant feature point from a satellite image of the same area taken in advance and store it in a file along with the latitude, longitude, and altitude read from the map. . When performing estimation processing to process a satellite image, a small image piece corresponding to the feature point of interest is read from the above file, and corresponding points are determined by correlation between the image piece and the photographed image.

Read out the pixel coordinates.

[The problem that the invention is trying to solve]

The above-mentioned conventional technology is based on the premise that the distortion of the image with respect to the base map, such as the image of an earth observation satellite, is relatively small and that the distortion is reproducible. On the other hand, general moving objects can take any desired path and posture, so distortion is not reproducible.
It may also become larger. In addition, real-time processing is required for state estimation in order to use it for guiding and controlling moving objects. Therefore, in the above-mentioned method of associating a fixed image or map prepared in advance with an observed image, there is a problem that it is difficult to correspond to the R-side image whose distortion changes due to the movement of the moving object.

Therefore, the first object of the present invention is to obtain distorted Jl! ? The object of the present invention is to provide a means for easily collating and associating an I1g image with a map.

The second problem with the conventional technology is that the estimation accuracy decreases when there are few feature points in the observed image that serve as references whose position coordinates on the map are known, and furthermore, estimation cannot be performed when there are no reference points. be. On the other hand, although there may be a characteristic linear object in the observed image that serves as a reference whose position on the map is known, the conventional technology cannot use the position information of this characteristic line for estimation. A second object of the present invention is to provide a means for improving estimation accuracy by using characteristic lines in an image to estimate the state of a moving object.

[Means for solving problems]

The first of the above objectives is to combine a sensor mounted on a moving object, an image memory, a map data memory, a memory that stores a priori information regarding the position and orientation of the moving object, a data processing device, and an image display device. In an image processing system comprising an image coordinate input device, data processing is performed from the image coordinates of a feature point whose coordinates on a map are known, which is indicated by a human using the image coordinate input device from among the sensor images displayed on the image display device. When a device estimates the position and orientation of a moving object, this is achieved by generating a distorted map image from map data according to a priori information regarding the position and orientation of the moving object and displaying it on a display device simultaneously with the sensor image. .

The second objective is to estimate the position and orientation of the moving body by using almost characteristic lines whose positions on the map are known, in addition to or in place of the image coordinates of feature points whose coordinates on the map are known. This is achieved by using the position in the image of minutes.

[Effect]

The data processing device generates a map image having a distortion close to the image captured by the sensor mounted on the mobile object from the map data stored in the memory according to a priori information regarding the position and orientation of the mobile object,
Displayed simultaneously with the sensor image on the image display device. This makes it easy for a person to associate the map and the photographed image by looking at the display of the two images, making it easy for a person to input the position of a feature point in one image from the image coordinate input device.

In addition to feature points whose coordinates on the map are known, the coordinates of characteristic line segments whose positions on the map are known can be used in the photographed image to estimate the position and orientation of the moving object. Thereby, even if the number of feature points is small or there are no feature points, the accuracy of estimation can be improved when a characteristic line segment is present in the image.

〔Example〕

An embodiment of the present invention will be described below with reference to the drawings.

FIG. 1 is an overall configuration diagram of an image processing system according to the present invention. Photographed image data captured from a sensor 1 mounted on a moving body is stored in an image memory 2 and simultaneously displayed on an image display device 3. The data processing device 5 performs the process of estimating the position and orientation of the moving body according to the procedure shown in FIG. First, from the map data stored in the memory 6 and the a priori information regarding the position and orientation of the mobile body stored in the memory 7, a distortion corresponding to an image taken by a sensor mounted on a moving object that is in a state according to the a priori information is calculated. Generation of map image with 20
Then, the map image is written into the memory 2 and displayed 21 on the display device 3.

Next, the coordinates in the image of the feature point designated by the user on the display image of the device 3 are imported 22, and the coordinates, the map coordinates of the corresponding point in the map data read from the memory 6, and the memory 7 Estimation 23 of parameters related to position and orientation is performed using the a priori information read from . The parameter values obtained as the estimation results are used for updating 24 the a priori information in the memory 7, and are also output 25 to the external device 8 for use in controlling and manipulating the moving object. If there are multiple feature points in
may be performed once, or processes 20 to 24 may be performed for each feature point.

In the latter case, the accuracy of parameter estimation increases each time a feature point is specified, and the map image displayed using the feature point and the photographed image will have similar distortions, and the user will be able to This has the effect of making it easier and more accurate to make the correspondence and specify new feature points based on the correspondence.

FIGS. 3(a) and 3(b) are diagrams showing the structure of a priori information in the memory 7. FIG. The parameters 30 representing the state of the moving object include the coordinates (X V + V V + Z v) of a specific point on the moving object, for example, the center point of the mounted sensor, in the map coordinate system, and each axis of the seed thread fixed to the moving object with respect to the map coordinate system. The expected values of the prior probability distribution of parameters R, P, and (2) representing the rotation around are used.

Also, each element 31 of the error covariance matrix −P of these six variables
Let's have it together. This a priori information is prepared and given externally 8 as a predicted value from the measured value of a position/orientation sensor mounted on the moving object or from the estimation result of a state parameter estimated from a sensor image in advance.

FIGS. 4(a) and 4(b) are diagrams showing the structure of map data in memory. Feature point data 41 in the map is an identifier 41
1 and its map coordinates 412.

The characteristic line data 42 consists of an identifier 421 and a map coordinate sequence 422 of the node when the line is approximated by a sequence of line segments.

FIG. 5 is a diagram showing an example of displaying the photographed image 50 on the display device 3, and FIG. 6 is a diagram showing an example of displaying the map image 60 on the display device 3.

The details of the process shown in FIG. 2 will be explained below.

First, each coordinate system is defined using FIGS. 7 and 8.

The coordinates CQ*p) of the image 70 are determined as a coordinate axis 71. A coordinate system 80 fixed to a sensor mounted on the moving body, a coordinate system 81 fixed to the moving body. The map coordinate system 82 is shown in FIG. 8(a).
~(c).

Two axes of the coordinate system 80 are made to coincide with the optical axis 85, and a focal plane 83 and a coordinate system 84 on the plane are considered at a position separated by a focal length f in the negative direction of the Z axis.

The map image generation process 20 is performed as follows.

Map coordinates of point P-86 in Figure 8 r = (x y z)
'' is given by the map data. At this time, the coordinates of the image point '87 on the focal plane 83 are determined.

If the line of sight vector 88 for point t is expressed as 1° in the map coordinate system and 18 in the sensor fixed coordinate system, then Q=M, M1 principal. There is a relationship as shown in equation (1). Here, M-factor is a rotation orthogonal matrix from coordinate system 80 to 81, and M2 is a rotation orthogonal matrix from coordinate system 81 to 82. Given by the angles R, P, and Y. Here, values in the memory 7 are used for R, P, and Y. On the other hand, the coordinates of Tendan in the map system are.

L = 2v + 1L It is given by the formula (3). Here, v = (Xv Yv Zv
)" is given by the position parameter of the state of the moving body. This also uses the value in memory. Q g = M 1TM 2 " (r r v) by solving equations (1) and (3).
19 given from equation (4) is (X!l Vta
Z9)T, the coordinates (X, Y) of the image point are given by. This (X, Y) can be converted into (Q, p) of the image. Here a. , a□, bo.

bl is a constant determined by the definition of the coordinate system 71.

The image coordinates of the points constituting the map can be obtained by applying the above calculations to the coordinates 412 of the feature points in the memory 6 and the coordinates 422 of the node array of the feature line. For feature points, a symbol corresponding to the type 411 is written at the corresponding image coordinates in the memory 2, and for feature lines, for example, a predetermined density value is written at the pixel corresponding to the line segment connecting the image coordinates of the nodal array. This image data is displayed at 21. In Figure 6, buildings 61°, bridges 62 are symbols, roads 63. Railway line 64. JI+6
This is an example in which 5 is displayed as a line.

Parameter estimation 23 is performed as follows. Regarding the image coordinates of the first feature point, the value taken in from the user's instruction input in process 22 is calculated as χ−1=(Q p)"
, the value calculated using equations (4) to (6) is fl=(Qp)
”, where i=1°..., N, where N is the total number of feature points in the image.

If we now set the unknown parameter vector x= (RP Y xv 7v Xv), then the king is given as the one that minimizes the evaluation function J of the following equation.

Here, -XO*PO is given by the expected value and the error covariance matrix from the a priori information of the unknown parameters in the memory 7, respectively. The error vector 11 is given by the equation (8), where W is a weight matrix and it can be given as parallel columns of the error covariance matrix of el.

The deweighting of equation (7) can be obtained by the least squares method or the Kalman filter.In the case of the 6-Kalman filter, the data of the feature points are processed one by one, so the input estimation results obtained each time and the error map Image display can be updated.

Next, an example will be described in which feature lines are used for estimation instead of some or all of the feature points in an image.

In the processing procedure of FIG. 2, instead of the process 22, as shown in FIG. 9, the image coordinates of the representative point of the feature line are captured 221. This means that in the image 100 displayed on the display device as shown in FIG.
This is done by the user viewing the display and giving instructions from the input device 44 regarding the image coordinates 03 and the corresponding latter line segment 104. Regarding the latter, the processing device 5 searches the point sequence 422 of the memo U 6 to find the linear equation of the corresponding line segment in the map coordinate system. The equation is given in the form of r=rl+u (9) for a point square on a line segment. Ll is the coordinate of the representative point P□ on the straight line including the line segment, and 〇 is the unit direction vector of the straight line. The geometric relationships necessary for estimation will be explained using FIG. 11. When the representative point of the line segment 110 of the characteristic line of interest is given, and the unit direction vector 112 is given, the vector Q corresponding to the line of sight 114 is expressed as ng=M1"M2" in the sensor coordinate system as in equation (4). rt-rv) (10)
The coordinates (xt Yt) of the image -P, '115 on the sensor surface 83 of -P-0 are given by equation (5). - direction vector and the unit direction vector a of the corresponding image are a/1zXM1τM 2 T((r x r v)
X u) is given by equation (11). here I
Z= (OO1)", /ha means parallel. Now let soil be a two-dimensional vector on the plane 83, soil = (Cx Cy)
Let P, 117 be the position on the sensor surface of the representative point of the sensor image given by the user's instruction as shown in FIG. The coordinates of this point in the coordinate system 84 are given by inverse transformation of equation (6) from (Q p) given by the instruction. Let it be (X2Y2)T. At this time, the line segment image 118 predicted from the geometric model
And the indicated point -? The distance from the image 117 of −2 to the image 117 on one image is e= ((Xz Xo)Cy+ (yz Yo)C
x) ・α Equation (12) is obtained. Here α is a constant for scale.

Using the error e defined in this way, an estimation problem can be defined as follows. That is, in place of the vector 1 used in the case of feature points in equation (7), in the case of feature lines, the scalar 81 defined in equation (12) is used. Also, a weight scalar W is used instead of a 2×2 weight matrix. Here, feature points and feature lines may coexist in the image. In this case, equation (7) can be used instead of equation (7). Here, il is the error vector of the i-th feature point, and 8J is According to the zero-line method, which is an error scalar for the j-th feature line, more features that serve as the reference for estimation can be specified in one image, and the estimation accuracy can be improved.

Note that only one display device 2 may be prepared and the sensor photographed image and the map image may be displayed in an overlay manner, for example, in different colors, but two may be prepared and both images displayed on separate devices.

〔Effect of the invention〕

According to the present invention, a map image having a distortion close to that of the sensor image is displayed on the display device at the same time as the sensor image, so that the map image and the sensor image can be matched in a distorted state by associating the two images. This has the effect of making it possible to improve the accuracy of estimating the state of a moving body using feature points in one image even in real-time processing.

In addition, position information of feature lines in addition to feature points in the image can be used for estimation, which has the effect of improving estimation accuracy.

[Brief explanation of the drawing]

FIG. 1 is an overall configuration diagram of an image processing system according to an embodiment of the present invention, FIG. 2 is a diagram showing a processing procedure of a processing device constituting the system, and FIG. 3 is a configuration diagram of an a priori information memory. FIG. 4 is a block diagram of the map data memory, FIG. 5 is a diagram showing an example of displaying a sensor image, and FIG. 6 is a diagram showing an example of displaying a map image. FIG. 7 is a diagram for explaining image coordinates, FIG. 8 is a diagram for explaining the geometric relationship of imaging, FIG. 9 is a diagram for explaining the processing procedure of the processing device in the second embodiment, and FIG. FIG. 10 is a display example of a sensor image and a map image of this embodiment, and FIG. 11 is a diagram for explaining the geometrical relationship of this embodiment. 13 Figure 114I! l Group 7, Figure q

Claims (2)

[Claims] 1. A sensor mounted on the moving object, an image memory, a map data memory, a memory for storing a priori information regarding the position and orientation of the moving object, a data processing device, an image display device, and an image coordinate input device. In this image processing system, a data processing device calculates the position and location of a moving object from the image coordinates of a feature point whose coordinates on a map are known, which are specified by a human using an image coordinate input device from a sensor image displayed on an image display device. A method for estimating the state of a moving object, characterized in that when estimating the attitude, a distorted map image is generated from map data according to a priori information regarding the position and attitude of the moving object, and the map image is displayed on a display device simultaneously with a sensor image. 2. In estimating the position and orientation of the moving object, in addition to or in place of the image coordinates of feature points whose coordinates on the map are known, the positions of characteristic line segments whose positions on the map are known are used in the image. A method for estimating the state of a moving body according to claim 1, characterized in that: