JP7653941B2 - Data processing device, data processing method and data processing program - Google Patents

Description

This disclosure relates to technology for estimating the position and orientation of a moving object.

As a self-location estimation technology for estimating the position and orientation of a moving object, there is a technology using SLAM (Simultaneous Localization and Mapping) described in Patent Literature 1. A known type of SLAM is Visual SLAM, which detects landmarks from a camera image and estimates the self-location. Visual SLAM is performed using technologies such as Visual Odometry and Visual-Inertial Odometry.
Patent Document 1 discloses a position estimation device that compares an environmental image obtained by photographing the surrounding space while moving with a plurality of reference images associated with each position in the space, and estimates the position and orientation of the moving body from the comparison results.

International Publication No. WO2019/187816

Monocular cameras and millimeter wave radars are available as means for capturing images of the space around a moving object.
A monocular camera can estimate a velocity vector by tracking feature points in a time series of images. However, monocular cameras have the problem that they are not suitable for all weather conditions and are vulnerable to disturbances from sunlight. In addition, monocular cameras have the problem that the velocity error is large for distant objects and the accuracy of distance measurement is poor at long distances.
Millimeter wave radar can measure the relative velocity vector of a highly reflective object with high accuracy in all weather conditions and at great distances. However, millimeter wave radar has the problem that it is difficult to detect low reflectivity objects. In addition, millimeter wave radar has the problem that the spatial resolution is low.

The main objective of this disclosure is to obtain a configuration that combines a monocular camera and millimeter wave radar to estimate the position and attitude of a moving object with high accuracy, regardless of the weather or the distance from the surrounding space.

The data processing device according to the present disclosure comprises:
a camera image processing unit that extracts a plurality of feature points as a plurality of camera feature points from each of a plurality of camera image data obtained by a monocular camera mounted on the moving object, tracks each camera feature point between the camera image data, and calculates a moving speed of each camera feature point between the camera image data;
a radar echo processing unit that extracts, as a plurality of radar characteristic points, a plurality of characteristic points from each of a plurality of radar echo data obtained in synchronization with the plurality of camera image data by a millimeter wave radar mounted on the moving object, tracks each of the radar characteristic points between the radar echo data, and calculates a moving speed of each of the radar characteristic points between the radar echo data;
an identification processing unit that identifies camera feature points and radar feature points between synchronized camera image data and radar echo data, and corrects the moving speed of each camera feature point with the moving speed of the identified radar feature point;
The apparatus further includes an estimation processing unit that estimates the position and orientation of the moving object using the corrected moving speed of each camera feature point for each camera image data.

The present disclosure provides a configuration that can estimate the position and attitude of a moving body with high accuracy, regardless of the weather or the distance from the surrounding space.

FIG. 2 is a diagram showing an example of a functional configuration of the data processing device according to the first embodiment. 2 illustrates an example of a hardware configuration of a data processing device according to a first embodiment. 4 is a flowchart showing an overview of the operation of the data processing device according to the first embodiment. 5 is a flowchart showing an example of camera image processing according to the first embodiment. 5 is a flowchart showing an example of a first conversion process according to the first embodiment. 5A to 5C are diagrams for explaining a first conversion process according to the first embodiment; 10 is a flowchart showing an example of a second conversion process according to the first embodiment. 6A to 6C are diagrams for explaining a second conversion process according to the first embodiment; 5 is a flowchart showing an example of radar echo processing according to the first embodiment. 4 is a flowchart showing an example of an identification process according to the first embodiment. 10 is a flowchart showing an example of a third conversion process according to the first embodiment. FIG. 2 is a diagram showing an example of the internal configuration of an estimation processing unit according to the first embodiment. FIG. 4 is a diagram showing an observation equation for a moving object according to the first embodiment. FIG. 2 is a diagram showing an example of an Unscented Kalman Filter (UKF) according to the first embodiment; FIG. 2 is a diagram showing an example of a UKF according to the first embodiment; FIG. 2 is a diagram showing an example of a UKF according to the first embodiment; FIG. 13 is a diagram showing an example of a functional configuration of a data processing device according to a second embodiment. 10 is a flowchart showing an example of radar echo processing according to the second embodiment.

The following describes the embodiments with reference to the drawings. In the following description of the embodiments and the drawings, the same reference numerals indicate the same or corresponding parts.

Embodiment 1.
***Configuration Description***
FIG. 1 shows an example of the functional configuration of a data processing device 100 according to the present embodiment.
FIG. 2 shows an example of a hardware configuration of the data processing device 100 according to the present embodiment.
The operation procedure of the data processing device 100 corresponds to a data processing method, and the program that realizes the operation of the data processing device 100 corresponds to a data processing program.

1, a data processing device 100 is mounted on a vehicle 1. The vehicle 1 is an example of a moving body.
Vehicle 1 includes transport vehicles such as passenger cars, buses, and taxis, construction vehicles such as dump trucks, industrial vehicles such as forklifts, motorcycles, bicycles, autonomous robots, personal mobility, and the like.
In addition to the data processing device 100, the vehicle 1 is equipped with a separate monocular camera 200 and a millimeter wave radar 300.
The monocular camera 200 and the millimeter wave radar 300 may form a data processing device as a higher-level system together with the data processing device 100. Of course, the monocular camera 200 and the millimeter wave radar 300 may be integrally incorporated into the data processing device 100.
In addition to the data processing device 100, the vehicle 1 may also include an IMU (inertial measurement unit), an odometer, and the like.

The monocular camera 200 is, for example, a charge-coupled device (CCD) camera, a complementary metal oxide semiconductor (CMOS) camera, or the like.
The monocular camera 200 captures, for example, an image of the space around the vehicle 1 and outputs camera image data obtained by capturing the image to the data processing device 100 .

The millimeter wave radar 300 is a radar in the frequency band of 30 GHz to 300 GHz.
Although not shown in FIG. 1, the millimeter wave radar 300 mainly includes a synthesizer, an oscillator, a millimeter wave transmitting antenna, a millimeter wave receiving antenna, a mixer, and a DSP (Digital Signal Processor).

The synthesizer generates a chirp signal that represents the amplitude of the signal from the oscillator as a function of time.
Then, the millimeter wave transmitting antenna transmits the chirp signal into the space around the vehicle 1. When an object reflects the chirp signal, the millimeter wave receiving antenna receives the reflected wave.
A mixer then combines the transmit and receive signals to generate an intermediate frequency signal.
The DSP then performs a Fourier transform on the intermediate frequency signal to detect the beat frequency. Since the beat frequency correlates with distance, the DSP can calculate the distance to the object from the beat frequency. Phase information can also be obtained by the Fourier transform. Since the velocity Doppler can be found from the phase information, the DSP can also calculate the relative velocity to the object.
The result of the signal processing by the DSP is output to the data processing device 100 as radar echo data.

In the data processing device 100, the camera image processing unit 101 repeatedly acquires camera image data from the monocular camera 200. The camera image processing unit 101 extracts a plurality of camera feature points from each acquired camera image data. The camera feature points are feature points included in the camera image data.
Furthermore, the camera image processing unit 101 tracks each camera feature point between the camera image data, and calculates the moving speed of each camera feature point between the camera image data. More specifically, it calculates the coordinate value of each camera feature point for each camera image data, and calculates the moving speed of the coordinate value of each camera feature point between the camera image data. The coordinate system in the camera image data is called an image coordinate system.

The radar echo processing unit 102 repeatedly acquires radar echo data from the millimeter wave radar 300. The radar echo processing unit 102 extracts a plurality of radar characteristic points from each acquired radar echo data. The radar characteristic points are characteristic points included in the radar echo data. The radar characteristic points are also called symbols.
Furthermore, the radar echo processing unit 102 tracks each radar characteristic point between the radar echo data, and calculates the moving speed of each radar characteristic point between the radar echo data. More specifically, it calculates the coordinate value of each radar characteristic point for each radar echo data, and calculates the moving speed of the coordinate value of each radar characteristic point between the radar echo data. The coordinate system in the radar echo data is called the radar coordinate system.

The synchronous clock generation unit 103 generates a synchronous clock signal at a predetermined period and outputs the generated synchronous clock signal to the camera image processing unit 101 and the radar echo processing unit 102.

The first transformation processing unit 104 transforms the coordinate values and moving speed of the camera feature points in the image coordinate system into coordinate values and moving speed in the vehicle body coordinate system for each camera image data. The vehicle body coordinate system is a coordinate system that has the default position of the vehicle 1 as its origin.

The second conversion processing unit 105 converts the coordinate values and movement speed of the camera feature points in the vehicle coordinate system into coordinate values and movement speed in the radar coordinate system for each camera image data.

The identification processing unit 106 identifies camera characteristic points and radar characteristic points between the synchronized camera image data and radar echo data.
Furthermore, the identification processing unit 106 corrects the moving speed of each camera characteristic point with the moving speed of the identified radar characteristic point.

The third conversion processing unit 107 converts the coordinate values of the camera feature points in the radar coordinate system and the corrected moving speed (hereinafter referred to as the corrected moving speed) into coordinate values and corrected moving speed in the vehicle body coordinate system for each camera image data.

The estimation processing unit 108 estimates the position and orientation of the moving object using the coordinate values of the camera feature points and the corrected moving speed for each camera image data.
The estimation processing unit 108 may estimate the position and orientation by using Visual SLAM in combination with techniques such as Visual Odometry and Visual-Inertial Odometry. At this time, the estimation accuracy of the position and orientation may be further improved by combining other measurement information such as an IMU (inertial measurement unit) and an odometer.
The operation performed by the estimation processing unit 108 corresponds to the estimation process.

Next, an example of the hardware configuration of the data processing device 100 according to the present embodiment will be described with reference to FIG.
The data processing device 100 according to the present embodiment is a computer.
The data processing device 100 includes, as hardware, a processor 901, a main memory device 902, an auxiliary memory device 903, a communication device 904, and an input/output device 905.
The functions of the camera image processing unit 101, the radar echo processing unit 102, the synchronization clock generation unit 103, the first conversion processing unit 104, the second conversion processing unit 105, the identification processing unit 106, the third conversion processing unit 107 and the estimation processing unit 108 shown in FIG. 1 are realized, for example, by a program.
The auxiliary storage device 903 stores programs that realize the functions of the camera image processing unit 101, the radar echo processing unit 102, the synchronization clock generation unit 103, the first conversion processing unit 104, the second conversion processing unit 105, the identification processing unit 106, the third conversion processing unit 107 and the estimation processing unit 108.
These programs are loaded from the auxiliary storage device 903 to the main storage device 902. Then, the processor 901 executes these programs to perform operations of a camera image processing unit 101, a radar echo processing unit 102, a synchronous clock generating unit 103, a first conversion processing unit 104, a second conversion processing unit 105, an identification processing unit 106, a third conversion processing unit 107, and an estimation processing unit 108, which will be described later.
FIG. 2 shows a schematic diagram of a state in which the processor 901 is executing programs that realize the functions of the camera image processing unit 101, the radar echo processing unit 102, the synchronization clock generation unit 103, the first conversion processing unit 104, the second conversion processing unit 105, the identification processing unit 106, the third conversion processing unit 107, and the estimation processing unit 108.

*** Operation Description ***
FIG. 3 shows an outline of the operation of the data processing device 100 according to the present embodiment.

When the synchronization clock generation unit 103 outputs a synchronization clock signal to the camera image processing unit 101 and the radar echo processing unit 102, steps S101 and S102 are performed.

In step S101, the camera image processing unit 101 performs camera image processing.
Specifically, the camera image processing unit 101 instructs the monocular camera 200 to capture an image of the surroundings of the vehicle 1 in accordance with a synchronous clock signal from the synchronous clock generating unit 103. The camera image processing unit 101 acquires camera image data obtained by the monocular camera 200. The camera image processing unit 101 repeatedly acquires camera image data, and extracts a plurality of camera feature points from each acquired camera image data.
Furthermore, the camera image processing unit 101 tracks each camera feature point between the camera image data, and calculates the moving speed of each camera feature point between the camera image data. More specifically, it calculates the coordinate value of each camera feature point for each camera image data, tracks each camera feature point between the camera image data, and calculates the moving speed of the coordinate value of each camera feature point between the camera image data.

In step S102, the radar echo processing unit 102 performs radar echo processing.
Specifically, the radar echo processing unit 102 instructs the millimeter-wave radar 300 to scan the surroundings of the vehicle 1 in accordance with the synchronous clock signal from the synchronous clock generating unit 103. The radar echo processing unit 102 acquires radar echo data obtained by the millimeter-wave radar 300. The radar echo processing unit 102 repeatedly acquires radar echo data, and extracts a plurality of radar characteristic points from each acquired radar echo data.
The radar echo processing unit 102 tracks each radar characteristic point between the radar echo data and calculates the moving speed of each radar characteristic point between the radar echo data. More specifically, the radar echo processing unit 102 calculates the coordinate value of each radar characteristic point for each radar echo data and calculates the moving speed of the coordinate value of each radar characteristic point between the radar echo data.

In step S104, the first conversion processing unit 104 performs the first conversion processing.
Specifically, the first transformation processing unit 104 transforms the coordinate values and moving speed of the camera feature points in the image coordinate system into coordinate values and moving speed in the vehicle body coordinate system for each camera image data.

In step S105, the second conversion processing unit 105 performs the second conversion processing.
Specifically, the second conversion processing unit 105 converts the coordinate values and moving speed of the camera feature points in the vehicle body coordinate system into coordinate values and moving speed in the radar coordinate system for each camera image data.

In step S106, the identification processing unit 106 performs the identification process.
Specifically, the identification processing unit 106 evaluates the degree of similarity between the coordinate value and moving speed of the camera feature point and the coordinate value and moving speed of each radar feature point between the synchronized camera image data and radar echo data, and identifies the camera feature point and the radar feature point. In other words, the identification processing unit 106 identifies the camera feature point and the radar feature point that has the highest degree of similarity to the camera feature point.
Furthermore, the identification processing unit 106 corrects the moving speed of each camera characteristic point with the moving speed of the identified radar characteristic point.

In step S107, the third conversion processing unit 107 performs the third conversion process.
Specifically, the third conversion processing unit 107 converts the coordinate values and corrected movement speed of the camera characteristic points in the radar coordinate system into coordinate values and corrected movement speed in the vehicle body coordinate system for each camera image data.

In step S108, the estimation processing unit 108 performs the estimation process.
Specifically, the estimation processing unit 108 estimates the position and attitude of the vehicle 1 using the coordinate values of the camera feature points and the corrected movement speed for each camera image data.

Next, we will explain each step in Figure 3 in detail.

Figure 4 shows the details of step S101. That is, Figure 4 shows the details of the camera image processing by the camera image processing unit 101.

In step S1011, the camera image processing unit 101 determines whether or not the data acquisition timing has arrived.
When the synchronous clock signal is output from the synchronous clock generating unit 103, the camera image processing unit 101 determines that the data acquisition timing has arrived.
If the data acquisition timing has arrived (YES in step S1011), the process proceeds to step S1012.

In step S<b>1012 , the camera image processing unit 101 acquires camera image data from the monocular camera 200 .
Specifically, the camera image processing unit 101 instructs the monocular camera 200 to capture an image of the surroundings of the vehicle 1. Then, the camera image processing unit 101 acquires camera image data from the monocular camera 200.

Next, in step S<b>1013 , the camera image processing unit 101 calculates the attitude of the monocular camera 200 .
Specifically, the camera image processing unit 101 calculates the yaw angle, roll angle, and pitch angle of the monocular camera 200 as the attitude of the monocular camera 200 .
The method for calculating the attitude of the monocular camera 200 is not important.

Next, in step S1014, the camera image processing unit 101 extracts camera feature points from the camera image data.
Furthermore, the camera image processing unit 101 calculates the coordinate values of each camera feature point in the image coordinate system. Then, the camera image processing unit 101 identifies, in the current camera image data, camera feature points that are the same as the camera feature points extracted during the previous execution of camera image processing, based on the similarity of the coordinate values. In other words, the camera image processing unit 101 tracks the camera feature points between camera image data.
The camera image processing unit 101 identifies the current camera feature point that is the same as the previous camera feature point by using, for example, a homography conversion technique, and sets the ID (Identifier) of the previous camera feature point to the tracked current camera feature point.
For a camera feature point that does not have a corresponding previous camera feature point, that is, for a camera feature point that is newly extracted from the current camera image data, the camera image processing unit 101 sets a new ID.

Next, in step S1015, the camera image processing unit 101 calculates the moving speed of the camera feature point.
That is, the camera image processing unit 101 calculates the movement speed of the camera feature point by dividing the amount of change in coordinate values between the previous camera feature point and the current camera feature point that have the same ID assigned thereto by the generation cycle of the synchronous clock signal.

Finally, in step S<b>1016 , the camera image processing unit 101 outputs the camera image data ID, the camera attitude information, and the camera feature point information to the first conversion processing unit 104 .
The camera image data ID is an identifier of the camera image data. For example, the camera image data ID is an ID of the synchronous clock signal output by the synchronous clock generating unit 103. The camera image data ID may be any information as long as it can associate the camera image data to be synchronized with the radar echo data.
The camera attitude information indicates the attitude (yaw angle, roll angle, pitch angle) of the monocular camera 200 calculated in step S1012.
The camera feature point information indicates the ID, coordinate values, and movement speed of each camera feature point.

Figure 5 shows the details of step S104. That is, Figure 5 shows the details of the first conversion process by the first conversion processing unit 104.

In step S1041, the first conversion processing unit 104 acquires the camera image data ID, camera posture information, and camera feature point information.

Next, in step S1042, the first transformation processing unit 104 transforms the coordinate values of each camera feature point in the image coordinate system into coordinate values in the vehicle body coordinate system.
Specifically, the first conversion processing unit 104 converts the coordinate value Xp/c=(u,v) in the image coordinate system into the coordinate value Xp/w=(Z,Y,X) in the vehicle body coordinate system using the conversion formula shown in Fig. 6. Since the radar echo data is two-dimensional scan data of a horizontal plane, the first conversion processing unit 104 changes the Z value to Z=0 after the coordinate conversion, and makes the coordinate value in the vehicle body coordinate system two-dimensional. Note that p/c is an abbreviation for "Point to Camera".

Next, in step S1043, the first transformation processing unit 104 transforms the moving speed of each camera feature point in the image coordinate system into a moving speed in the vehicle body coordinate system.
Specifically, the first conversion processing unit 104 converts the moving speed Vp/c=(Δu, Δv) in the image coordinate system into the moving speed Vp/w=(ΔZ, ΔY, ΔX) in the vehicle body coordinate system using the conversion formula shown in Fig. 6. As with the coordinate values, the first conversion processing unit 104 changes the Z value to ΔZ=0 after the coordinate conversion, and makes the moving speed in the vehicle body coordinate system two-dimensional.

Finally, in step S<b>1044 , the first conversion processing unit 104 outputs the camera image data ID, the camera attitude information, and the camera feature point information to the second conversion processing unit 105 .
Here, the camera feature point information indicates the coordinate value and moving speed of each camera feature point in the vehicle body coordinate system.

Here, a supplementary explanation of FIG. 6 will be given.
FIG. 6 shows how to calculate where in space a point in the camera image data and its depth correspond.
"Camera Coordinate Rotation (XYZ)" in FIG. 6C indicates the coordinate axes and rotation polarity of the camera coordinates (Y, X, Z) and image coordinates (u, v).
In the drawing, Ψ is the yaw angle of the monocular camera 200. φ is the roll angle of the monocular camera 200. θ is the pitch angle of the monocular camera 200. Ψ, φ, and θ are included in the camera attitude information.
The image size W is the number of pixels in the u-axis direction, and the pixel size H is the number of pixels in the v-axis direction.
fx is the focal length in pixels in the X-axis direction. fy is the focal length in pixels in the Y-axis direction. cx is the focal position in the X-axis direction. cy is the focal position in the Y-axis direction.
Xp/c; Point in Image (x, y, z) ^T indicates an imaged point in the camera coordinate system.
"Vehicle Coordinate rotation (ZYX)" in FIG. 6(b) indicates the vehicle body coordinate system, which is the coordinate system of the vehicle 1 on which the monocular camera 200 is mounted.
Rc/v indicates the coordinate axis and rotation polarity of the vehicle body coordinate system. Note that c/v is an abbreviation for "Camera to Vehicle."
Moreover, "Word Coordinate rotation (ZYX)" in FIG. 6A indicates the world coordinate system.
Rv/w indicates the coordinate axis and rotation polarity of the world coordinate system. Note that v/w is an abbreviation for "Vehicle to World."
O _1×3 is a 1 row, 4 column (0,0,0,0) matrix.
Xp/w; Point in World (x, y, z) ^T indicates an imaged point in the world coordinate system. However, in the formula, (x, y, z, 1) ^T is treated as 4 rows and 1 column.

Figure 7 shows the details of step S105. In other words, Figure 5 shows the details of the second conversion process by the second conversion processing unit 105.

In step S1051, the second conversion processing unit 105 acquires the camera image data ID, the camera posture information, and the camera feature point information.

Next, in step S1052, the second transformation processing unit 105 transforms the coordinate values of each camera feature point in the vehicle body coordinate system into coordinate values in the radar coordinate system.
Specifically, the second conversion processing unit 105 converts the coordinate value Xp/w=(Y, X) in the vehicle body coordinate system into the coordinate value Xr=(r, θ) in the radar coordinate system using the conversion formula shown in FIG.

Next, in step S1053, the second transformation processing unit 105 transforms the moving speed of each camera feature point in the vehicle body coordinate system into a moving speed in the radar coordinate system.
Specifically, the second conversion processing unit 105 converts the moving speed Vp/w=(ΔY, ΔX) in the vehicle body coordinate system into the moving speed Vr=(Δr, Δθ) in the radar coordinate system using the conversion formula shown in FIG.

Finally, in step S1054, the second conversion processing unit 105 outputs the camera image data ID, the camera attitude information, and the camera feature point information to the identification processing unit 106.
Here, the camera feature point information indicates the coordinate value and moving speed of each camera feature point in the radar coordinate system.

Here, a supplementary explanation of FIG. 8 will be given.
FIG. 8 shows an example of a polar coordinate display of radar characteristic points of radar echo data.
Xr: (x, y) indicates the position of the radar characteristic point in the same Cartesian coordinate system as the camera image data. When Xr: (x, y) is expressed in polar coordinates, it becomes Xr: (r, θ).

Figure 9 shows the details of step S102. That is, Figure 9 shows the details of radar echo processing by the radar echo processing unit 102.

In step S1021, the radar echo processing unit 102 determines whether or not the data acquisition timing has arrived.
When the synchronous clock signal is output from the synchronous clock generating unit 103, the radar echo processing unit 102 determines that the data acquisition timing has arrived.
If the data acquisition timing has arrived (YES in step S1021), the process proceeds to step S1022.

In step S<b>1022 , the radar echo processing unit 102 acquires radar echo data from the millimeter wave radar 300 .
Specifically, the radar echo processing unit 102 instructs the millimeter wave radar 300 to scan the surroundings of the vehicle 1. Then, the radar echo processing unit 102 acquires radar echo data from the millimeter wave radar 300.

Next, in step S1023, the radar echo processing unit 102 extracts radar characteristic points from the radar echo data.
Specifically, the radar echo processing unit 102 extracts points with high reflection intensity from the radar echo data as radar characteristic points.
The radar echo processing unit 102 also calculates the coordinate values of each radar characteristic point in the radar coordinate system. The radar echo processing unit 102 then identifies, in the current radar echo data, radar characteristic points that are the same as the radar characteristic points extracted during the previous execution of the radar echo processing, based on the similarity of the coordinate values. In other words, the radar echo processing unit 102 tracks the radar characteristic points between the radar echo data.
As described later, a prediction gate is set for each radar characteristic point when the previous radar echo processing is executed. If the current radar characteristic point is within the prediction gate of any of the previous radar characteristic points, the radar echo processing unit 102 determines that these two radar characteristic points are the same. Then, the radar echo processing unit 102 sets the ID of the previous radar characteristic point to the tracked current radar characteristic point.
For a radar characteristic point that does not have a corresponding previous radar characteristic point, that is, for a radar characteristic point that is newly extracted from the current radar echo data, the radar echo processing unit 102 sets a new ID.

It is possible that a plurality of current radar characteristic points are present within the prediction gate of the previous radar characteristic point.
For this reason, the radar echo processing unit 102 calculates the following evaluation function K for each of a plurality of current radar characteristic points present within the prediction gate of the previous radar characteristic point. Then, the radar echo processing unit 102 selects the current radar characteristic point that minimizes the value of the evaluation function K. For example, if there are three current radar characteristic points present within the prediction gate, the radar echo processing unit 102 calculates the evaluation function K three times.
In addition, in formula 1, "i _n " is the reflection luminance of the radar characteristic point of the nth scan (the previous radar characteristic point), and "i _n+1 " is the reflection luminance of the radar characteristic point of the (n+1)th scan (the current radar characteristic point).

Next, in step S1024, the radar echo processing unit 102 calculates the moving speed of the radar characteristic point.
In other words, the radar echo processing unit 102 calculates the moving speed of the radar characteristic point by dividing the amount of change in coordinate values between the previous radar characteristic point and the current radar characteristic point that have the same ID assigned thereto by the generation period of the synchronous clock signal.

Next, in step S1025, the radar echo processing unit 102 sets a prediction gate for each of the current radar characteristic points.
Specifically, the radar echo processor 102 predicts the position (coordinate value) of each radar characteristic point at the next scan timing from the line-of-sight speed information, and sets an error range centered on the predicted position as a prediction gate.
The radar echo processing unit 102 stores the prediction gate of each radar characteristic point in, for example, the auxiliary storage device 903 .

Finally, in step S1025, the radar echo processing unit 102 outputs the radar echo data ID and the radar characteristic point information to the identification processing unit 106.
The radar echo data ID is an identifier of the radar echo data. The radar echo data ID is, for example, an ID of the synchronous clock signal output by the synchronous clock generating unit 103. The radar echo data ID may be any information that can associate the camera image data and the radar echo data to be synchronized.
The radar characteristic point information indicates the ID, coordinate value, and moving speed of each radar characteristic point.

Figure 10 shows the details of step S106. That is, Figure 10 shows the details of the identification process by the identification processing unit 106.

In step S1061, the identification processing unit 106 acquires the camera image data ID, the camera posture information, and the camera feature point information.

Next, in step S1062, the identification processing unit 106 acquires the radar echo data ID and radar characteristic point information.

Next, in step S1063, the identification processing unit 106 identifies camera characteristic points and radar characteristic points between the synchronized camera image data and radar echo data.
That is, the identification processing unit 106 performs identification between the camera feature points of the camera feature point information and the radar feature points of the radar feature point information, whose camera image data ID and radar echo data ID match.
Since the beam width of the millimeter wave radar 300 is several degrees, the position resolution of the radar feature points is low. For this reason, the identification processing unit 106 evaluates the degree of similarity between the coordinate values and moving speed of the camera feature point and the coordinate values and moving speed of each radar feature point for each camera feature point, and identifies the camera feature point and the radar feature point.
Specifically, the identification processing unit 106 calculates an evaluation function J shown in the following formula 2 as the degree of approximation between each camera feature point and a radar feature point. Then, the identification processing unit 106 identifies the radar feature point and the camera feature point that minimize the value of the evaluation function J. In other words, the identification processing unit 106 identifies the camera feature point and the radar feature point by taking into account the speed difference in the moving speed in addition to the separation distance between the camera feature point and the radar feature point.

In Equation 2, r _r and θ _r are the coordinate values of the camera feature point in the radar coordinate system (X _r =(r _r , θ _r )).
Furthermore, r _s and θ _s are the coordinate values of the radar characteristic point (X _s =(r _s , θ _s )).
Furthermore, _vr is the moving speed of the camera feature point in the radar coordinate system.
Moreover, v _s is the moving speed of the radar characteristic point.

Next, in step S1064, the identification processing unit 106 corrects the moving speed of the camera characteristic point with the moving speed of the identified radar characteristic point.
Specifically, the identification processing unit 106 corrects the moving speed of the camera feature point by the following equation 3.
Δv _rs in Equation 3 indicates the corrected moving speed of the camera feature point.
The identification processing unit 106 calculates the corrected moving speed Δv _rs for all camera feature points included in the camera feature point information.

Finally, in step S<b>1065 , the identification processing unit 106 outputs the camera image data ID, the camera attitude information, and the corrected camera feature point information to the third conversion processing unit 107 .
The corrected camera feature point information indicates the coordinate value of each camera feature point in the radar coordinate system and the corrected moving speed.

Figure 11 shows the details of step S107. That is, Figure 11 shows the details of the third conversion process by the third conversion processing unit 107.

In step S1071, the third conversion processing unit 107 acquires the camera image data ID, the camera posture information, and the corrected camera feature point information.

Next, in step S1072, the third transformation processing unit 107 transforms the coordinate values of each camera feature point in the radar coordinate system into coordinate values in the vehicle body coordinate system.

Next, in step S1073, the third transformation processing unit 107 transforms the corrected moving speed in the radar coordinate system of each camera characteristic point into a corrected moving speed in the vehicle body coordinate system.
Specifically, the third conversion processing unit 107 converts the corrected moving speed according to the following expressions 4 and 5.
When the number of identified camera feature points is n, corrected moving velocities in the vehicle body coordinate system are obtained for the n camera feature points.

Finally, in step S1074, the third conversion processing unit 107 outputs the camera image data ID, the camera attitude information, and the corrected camera feature point information to the estimation processing unit 108.
Here, the corrected camera feature point information indicates the coordinate value of each camera feature point in the vehicle body coordinate system and the corrected moving speed.

FIG. 12 shows an example of the internal configuration of the estimation processing unit 108.
The configuration shown in Fig. 12 is one of state estimation filters. The configuration shown in Fig. 12 is also an implementation example of an Unscented Kalman Filter (UKF) used for state estimation of a nonlinear system. The UKF is realized, for example, by the equations shown in Figs. 14, 15, and 16.

The vehicle position/attitude state quantity update unit 1081 uses the observation quantity at time t and the observation equation in FIG. 13 to obtain a state estimate value at time t+1.
That is, the vehicle position/attitude state quantity update unit 1081 applies the coordinate values of the camera feature points and the corrected movement speed at time t to the observation equation in FIG. 13, and calculates state estimates for the position and attitude of the vehicle 1 at time t+1.
Fig. 13 shows a model of a four-wheeled vehicle in which steering is performed by the front wheels. However, it can also be applied to a two-wheeled vehicle model in which both steering and driving are performed by the rotational speed difference of the driving wheels if the observation equation is simplified.
Fig. 13(a) shows a four-wheel dynamics model, and Fig. 13(b) shows a vehicle body position in a vehicle body coordinate system (movement of the rear axle center of the four-wheel dynamics model).
In Fig. 13, x and y indicate coordinate values in the vehicle body coordinate system. Ψ indicates the attitude angle of the vehicle 1. v indicates the speed of the vehicle 1. a indicates the acceleration of the vehicle 1. L indicates the wheel base of the vehicle 1. δ indicates the steering angle of the vehicle 1. R indicates the turning radius in the vehicle body coordinate system.
The vehicle position/attitude state quantity update unit 1081 acquires these values from a configuration not shown in FIG. 1 (for example, an ECU (Electronic Control Unit) or the like).

The vehicle position and attitude observation amount update unit 1082 corrects the observation amount predicted value at time t+1 based on the error between the observation amount at time t+1 and the observation amount predicted value at time t+1.
The vehicle position and attitude output unit 1083 outputs the observation quantity prediction value at time t+1 after being corrected by the vehicle position and attitude observation quantity update unit 1082 to, for example, the input/output device 905 as the position and attitude of the vehicle 1 at time t+1.

The operation of the vehicle position and attitude state quantity update unit 1081, the vehicle position and attitude observation quantity update unit 1082, and the vehicle position and attitude output unit 1083 will be described in detail below.

The vehicle position/attitude state quantity update unit 1081 averages the coordinate values Xp/w = (0, Ym, Xm) of the m camera feature points included in the corrected camera feature point information using equations 6 and 7. The vehicle position/attitude state quantity update unit 1081 then substitutes the x and y values obtained by averaging into the x and y values in the equations in FIG. 13.

The vehicle position/attitude state quantity update unit 1081 also averages the yaw angles Ψ of the m monocular cameras 200 included in the camera attitude information using Equation 8. The vehicle position/attitude state quantity update unit 1081 then substitutes the Ψ value obtained by averaging into Ψ in the equation in FIG. 13.

The vehicle position/attitude state quantity update unit 1081 also converts the n pairs of corrected movement velocities (Δxn, Δyn) included in the corrected camera feature point information into an acceleration a using Equation 9. The vehicle position/attitude state quantity update unit 1081 then substitutes the obtained acceleration a for a in the equation in FIG. 13.

For the other variables (v, δ, etc.) included in the equation in FIG. 13, the vehicle position/attitude state quantity update unit 1081 assigns 0 as the initial value. After that, the value of each variable is calculated sequentially from the observation equation.

After the observations are observed, the vehicle position/attitude observation update unit 1082 corrects the predicted observation values based on the error between the predicted observation values and the actual observations.
Normally, in self-location estimation of a moving object, the position and attitude of the moving object are predicted by calculation using current observation values. However, in reality, the current observation values (current position) used in the calculation are a position that is one calculation cycle in the past. As a result, the predicted position contains an error for one calculation cycle (movement distance for the current calculation cycle). Therefore, the vehicle position and attitude observation value update unit 1082 reduces this error by predicting the position at a time that is one calculation cycle in the future.

The vehicle position and attitude output unit 1083 outputs the corrected observation quantity prediction value to, for example, the input/output device 905. Here, the future position and attitude of vehicle 1 for the calculation time are output, so that the accurate current position of vehicle 1 can be obtained.

***Description of Effects of the Embodiment***
As described above, according to this embodiment, by combining a monocular camera and a millimeter wave radar, it is possible to obtain a configuration that can estimate position and attitude without being affected by weather or depending on the distance from the surrounding space.

The velocity vector of the camera feature points in camera image data captured by a monocular camera is the relative velocity vector of the moving body relative to stationary objects in the operating environment. Therefore, by integrating the relative velocity vector over time, the travel trajectory of the moving body with the starting point as the origin can be obtained. By calculating the difference with the known target point coordinates, steering commands and speed commands to the drive unit can be obtained. For this reason, the velocity vector of the camera feature points can be used to control moving robots, etc. However, monocular cameras are not suitable for all weather conditions and are vulnerable to disturbances from sunlight. In addition, there is a problem that the position of the moving body cannot be calculated and the position accuracy is poor due to large velocity errors for distant objects.

The velocity vectors of radar feature points in radar echo data from millimeter-wave radar can only be obtained from objects with high reflective brightness. Therefore, even if an object captured in camera image data is scanned with a millimeter-wave radar, the number of radar feature points obtained is small. However, because millimeter-wave radar has a wide frequency band, it has extremely high velocity resolution and accuracy.
In this embodiment, the position and attitude of a moving object are corrected so that the speed difference between the same object captured in the camera image data and the radar echo data becomes zero. This makes it possible to improve the accuracy of the estimated position of a moving object even at a long distance.

In addition, according to this embodiment, even if valid camera image data cannot be obtained due to the influence of sunlight, the millimeter wave radar is not affected by sunlight, so it is possible to continue estimating the position of the moving object.

In this embodiment, an example is shown in which the data processing device 100 is mounted on a vehicle 1 which is a moving body. However, the data processing device 100 does not necessarily have to be mounted on the vehicle 1.
For example, the data processing device 100 may be implemented in a mobile terminal (e.g., a smartphone) used by a user (driver or passenger) in the vehicle 1. In this case, the data processing device 100 is wirelessly connected to the monocular camera 200 and the millimeter wave radar 300.

Embodiment 2.
In the first embodiment, if the camera feature point is stationary, the velocity vector of the camera feature point correctly represents the moving velocity of the vehicle 1. However, if there is a moving object (hereinafter, referred to as a moving object) such as a person or a vehicle ahead of the vehicle 1, the relative velocity between the vehicle 1 and the moving object is erroneously measured, resulting in an error.
In view of this, in the second embodiment, an example will be described in which the position and attitude of the vehicle 1 are estimated while excluding feature points of moving objects.

In this embodiment, the functional configuration of the data processing device 100 is as shown in Fig. 18. Compared to Fig. 1, Fig. 18 differs in that speed information of the vehicle 1 is output from the estimation processing unit 108 to the radar echo processing unit 102.
The hardware configuration of the data processing device 100 is as shown in FIG.
In this embodiment, the operation of the radar echo processing unit 102 is mainly different from that of the first embodiment.
In this embodiment, the differences from the first embodiment will be described.
It should be noted that matters not explained below are the same as those in the first embodiment.

FIG. 17 shows an example of the operation of the radar echo processing unit 102 according to this embodiment.
17, compared to FIG. 9, step S1027 is performed instead of step S1023.
In the following, step S1027 will be mainly described.

Steps S1021 to S1022 are the same as those shown in Figure 9, so their explanation will be omitted.

In step S1027, the radar echo processing unit 102 extracts radar characteristic points while excluding characteristic points of moving objects.
Specifically, the radar echo processing unit 102 analyzes the radar echo data to calculate the speed difference between the relative moving speed of the object captured by the millimeter wave radar and the speed of the vehicle 1 estimated by the estimation processing unit 108. Then, based on the calculated speed difference, the radar echo processing unit 102 identifies an object having a speed difference of a certain amount or more from among the objects captured by the millimeter wave radar as a moving object, and extracts a plurality of radar feature points by excluding the feature points of the moving object from the radar echo data.
The radar characteristic points are extracted by the method shown in the first embodiment.

Steps S1024 to S1026 are the same as those shown in Figure 9, so their explanation will be omitted.

As described above, in this embodiment, radar feature points are extracted while excluding feature points of moving objects. Therefore, according to this embodiment, it is possible to extract only the velocity vectors of stationary objects. Therefore, according to this embodiment, it is possible to correctly estimate the position and attitude of vehicle 1 even when moving objects are present around vehicle 1.

Although the first and second embodiments have been described above, these two embodiments may be combined and implemented.
Alternatively, one of these two embodiments may be partially implemented.
Alternatively, these two embodiments may be partially combined.
Furthermore, the configurations and procedures described in these two embodiments may be modified as necessary.

Additional hardware configuration information
Finally, a supplementary explanation of the hardware configuration of the data processing device 100 will be given.
The processor 901 shown in FIG. 2 is an integrated circuit (IC) that performs processing.
The processor 901 is a central processing unit (CPU), a digital signal processor (DSP), or the like.
The main storage device 902 is a RAM (Random Access Memory).
The auxiliary storage device 903 is a ROM (Read Only Memory), a flash memory, a HDD (Hard Disk Drive), or the like.
The communication device 904 is an electronic circuit that performs data communication processing.
The communication device 904 is, for example, a communication chip or a NIC (Network Interface Card).
The input/output device 905 is, for example, a mouse, a keyboard, and a display.

The auxiliary storage device 903 also stores an OS (Operating System).
At least a part of the OS is executed by the processor 901 .
The processor 901 executes at least a part of the OS while executing programs that realize the functions of the camera image processing unit 101, the radar echo processing unit 102, the synchronization clock generation unit 103, the first conversion processing unit 104, the second conversion processing unit 105, the identification processing unit 106, the third conversion processing unit 107 and the estimation processing unit 108.
The processor 901 executes the OS, thereby performing task management, memory management, file management, communication control, and the like.
The data processing device 100 may also include a plurality of processors that replace the processor 901. These multiple processors share the execution of a program that realizes the functions of the "units." Each processor is an IC that performs processing, just like the processor 901.
In addition, at least one of information, data, signal values and variable values indicating the results of processing by the camera image processing unit 101, radar echo processing unit 102, synchronization clock generation unit 103, first conversion processing unit 104, second conversion processing unit 105, identification processing unit 106, third conversion processing unit 107 and estimation processing unit 108 is stored in at least one of the main memory unit 902, auxiliary memory unit 903, registers and cache memory in the processor 901.
Furthermore, the programs realizing the functions of the camera image processing unit 101, the radar echo processing unit 102, the synchronization clock generation unit 103, the first conversion processing unit 104, the second conversion processing unit 105, the identification processing unit 106, the third conversion processing unit 107, and the estimation processing unit 108 may be stored in a portable recording medium such as a magnetic disk, a flexible disk, an optical disk, a compact disk, a Blu-ray (registered trademark) disk, a DVD, etc. Then, the portable recording medium storing the programs realizing the functions of the camera image processing unit 101, the radar echo processing unit 102, the synchronization clock generation unit 103, the first conversion processing unit 104, the second conversion processing unit 105, the identification processing unit 106, the third conversion processing unit 107, and the estimation processing unit 108 may be distributed.

In addition, the "part" in the camera image processing unit 101, the radar echo processing unit 102, the synchronization clock generation unit 103, the first conversion processing unit 104, the second conversion processing unit 105, the identification processing unit 106, the third conversion processing unit 107 and the estimation processing unit 108 may be read as "circuit" or "process" or "procedure" or "processing" or "circuitry".
Moreover, the circuit 10 may be realized by a processing circuit, such as a logic IC (Integrated Circuit), a GA (Gate Array), an ASIC (Application Specific Integrated Circuit), or an FPGA (Field-Programmable Gate Array).
In this case, the camera image processing unit 101, the radar echo processing unit 102, the synchronization clock generation unit 103, the first conversion processing unit 104, the second conversion processing unit 105, the identification processing unit 106, the third conversion processing unit 107 and the estimation processing unit 108 are each realized as part of a processing circuit.
In this specification, the higher-level concept of a processor and a processing circuit is called "processing circuitry."
That is, a processor and a processing circuit are each specific examples of "processing circuitry."

1 Vehicle, 100 Data processing device, 101 Camera image processing device, 102 Radar echo processing device, 103 Synchronization clock generation device, 104 First conversion processing device, 105 Second conversion processing device, 106 Identification processing device, 107 Third conversion processing device, 108 Estimation processing device, 200 Monocular camera, 300 Millimeter wave radar, 901 Processor, 902 Main storage device, 903 Auxiliary storage device, 904 Communication device, 905 Input/output device, 1081 Vehicle position and attitude state quantity update device, 1082 Vehicle position and attitude observation quantity update device, 1083 Vehicle position and attitude output device.

Claims (8)

a camera image processing unit that extracts a plurality of feature points as a plurality of camera feature points from each of a plurality of camera image data obtained by a monocular camera mounted on the moving object, tracks each camera feature point between the camera image data, and calculates a moving speed of each camera feature point between the camera image data;
a radar echo processing unit that extracts, as a plurality of radar characteristic points, a plurality of characteristic points from each of a plurality of radar echo data obtained in synchronization with the plurality of camera image data by a millimeter wave radar mounted on the moving object, tracks each of the radar characteristic points between the radar echo data, and calculates a moving speed of each of the radar characteristic points between the radar echo data;
an identification processing unit that identifies camera feature points and radar feature points between synchronized camera image data and radar echo data, and corrects the moving speed of each camera feature point with the moving speed of the identified radar feature point;
and an estimation processing unit that estimates a position and orientation of the moving object using the corrected moving speed of each camera feature point for each camera image data. The radar echo processing unit includes:
2 . The data processing apparatus according to claim 1 , wherein the plurality of radar characteristic points are extracted by excluding characteristic points of moving objects that are moving objects captured by the millimeter wave radar from each of the plurality of radar echo data. The radar echo processing unit includes:
3. The data processing device according to claim 2, further comprising: a data processing device for processing a signal from the object captured by the millimeter wave radar based on the relative speed of the object captured by the millimeter wave radar; a data processing device for processing a signal from the object captured by the millimeter wave radar based on the relative speed of the object captured by the millimeter wave radar; and a data processing device for processing a signal from the object captured by the millimeter wave radar based on the relative speed of the object captured by the millimeter wave radar. The camera image processing unit includes:
Calculate the coordinates of each camera feature point for each camera image data;
The radar echo processing unit includes:
Calculate the coordinates of each radar characteristic point for each radar echo data.
The identification processing unit
2. The data processing device according to claim 1, wherein the camera feature points and the radar feature points are identified by evaluating the degree of similarity between the coordinate values and moving speed of the camera feature points and the coordinate values and moving speed of each radar feature point for each camera feature point between synchronized camera image data and radar echo data. The camera image processing unit includes:
Calculate the coordinates of each camera feature point for each camera image data;
The estimation processing unit:
2. The data processing device according to claim 1, wherein the position and orientation of the moving object are estimated using the corrected moving speed of each camera feature point for each camera image data and the coordinate values of each camera feature point. a computer extracts a plurality of feature points as a plurality of camera feature points from each of a plurality of camera image data obtained by a monocular camera mounted on the moving object, tracks each camera feature point between the camera image data, and calculates a moving speed of each camera feature point between the camera image data;
the computer extracts, as a plurality of radar characteristic points, a plurality of radar echo data obtained by a millimeter wave radar mounted on the moving object in synchronization with the plurality of camera image data, tracks each radar characteristic point between the radar echo data, and calculates a moving speed of each radar characteristic point between the radar echo data;
the computer identifies camera feature points and radar feature points between the synchronized camera image data and radar echo data, and corrects the moving speed of each camera feature point with the moving speed of the identified radar feature point;
A data processing method in which the computer estimates the position and orientation of the moving object using the corrected moving speed of each camera feature point for each camera image data. camera image processing for extracting a plurality of feature points as a plurality of camera feature points from each of a plurality of camera image data obtained by a monocular camera mounted on the moving object, tracking each camera feature point between the camera image data, and calculating a moving speed of each camera feature point between the camera image data;
a radar echo process for extracting a plurality of characteristic points as a plurality of radar characteristic points from each of a plurality of radar echo data obtained in synchronization with the plurality of camera image data by a millimeter wave radar mounted on the moving object, tracking each of the radar characteristic points between the radar echo data, and calculating a moving speed of each of the radar characteristic points between the radar echo data;
an identification process for identifying camera feature points and radar feature points between synchronized camera image data and radar echo data, and correcting the moving speed of each camera feature point with the moving speed of the identified radar feature point;
and an estimation process of estimating the position and orientation of the moving object using the corrected moving speed of each camera feature point for each camera image data. A monocular camera mounted on a moving object,
A millimeter wave radar mounted on a moving object,
an estimation processing unit that estimates a position and/or an attitude of the moving object using a plurality of moving speeds obtained based on association between a plurality of image data obtained from the monocular camera in response to the movement of the moving object and a plurality of radar echo data obtained from the millimeter wave radar in response to the movement of the moving object;
a camera image processing unit that extracts a plurality of feature points as a plurality of camera feature points from the image data of the monocular camera, tracks each camera feature point between the image data, and calculates a moving speed of each camera feature point between the image data;
a radar echo processing unit that extracts a plurality of characteristic points as a plurality of radar characteristic points from the radar echo data of the millimeter wave radar, tracks each of the radar characteristic points between the radar echo data, and calculates a moving speed of each of the radar characteristic points between the radar echo data;
an identification processing unit that identifies camera feature points and radar feature points between the synchronized image data and radar echo data, and corrects the moving speed of each camera feature point with the moving speed of the identified radar feature point;
A data processing device comprising :

Images