CN103065323A - Subsection space aligning method based on homography transformational matrix - Google Patents

Abstract

The invention discloses a subsection space aligning method based on a homography transformational matrix. According to the subsection space aligning method based on the homography transformational matrix, large marked distance is sectioned, the homography transformational matrix between a camera coordinate system and a millimeter-wave radar coordinate system of each subsection is acquired, errors caused by using a same homography transformational matrix to represent coordinate relations of two sensors in the prior art are avoided, and space aligning of target detection of the large marked distance can be achieved. Relations of different coordinate systems between the camera and the millimeter-wave radar are deduced and represented, and finally the relations of the coordinate systems between the camera and the millimeter-wave radar are represented by the homography transformational matrix N. The two sensors are used for obtaining target data and solving the homography transformational matrix N, and a camera internal parameter matrix and a rotation matrix which are composed of solving scaling factors, focal distance and the like are avoided and a camera external parameter matrix composed of translation vectors is avoided. Therefore, operation process is greatly simplified and operation time is saved.

Description

Segmented space alignment method based on homography transformation matrix

Technical Field

The invention relates to the technical field of unmanned vehicle multi-sensor information fusion, in particular to a segmented space alignment method based on a homography transformation matrix.

Background

The unmanned vehicle is also called as an outdoor intelligent mobile robot, is a device which integrates multiple functions of environment perception, dynamic decision and planning, behavior control and execution and the like into a whole and has high intelligent degree, and the rapidity and the accuracy of the environment perception and the multi-sensor information fusion technology are indistinguishable. The multi-sensor information fusion technology is that a computer makes full use of sensor resources, and through reasonable domination and use of various measurement information, complementary and redundant information are combined according to certain optimization criteria in space and time to generate consistent explanation or description of an observation environment and generate a new fusion result. In the environment sensing module, a vision sensor and a millimeter wave radar are two commonly used sensors. The visual sensor has wide detection range, can obtain the size and outline information of the target in the external environment, but is easily influenced by external factors, and has the problem of target loss. The millimeter wave radar has high resolution and strong anti-interference capability, can accurately obtain the distance, relative speed and azimuth angle information of the target in various weather environments, but cannot identify the shape and size of the target, so that the complementary characteristics are utilized to fuse the information of the two, and more comprehensive and reliable environment information is obtained. And spatial alignment is a prerequisite for information fusion of the two. The essence of spatial alignment is to estimate the transformation matrix relationship between the camera and the radar coordinate system. At present, in a traditional space alignment method, in a calibration distance range of 20 meters, points on a target are randomly detected on different distance points, so that coordinate system expression of the target in a camera and expression of the target in a radar coordinate system are respectively obtained, according to data obtained by two sensors, a camera internal parameter matrix formed by a scaling factor, a focal length and the like and a camera external parameter matrix formed by a rotation matrix and a translation vector are estimated, the calculation process is relatively complicated, and errors are easily introduced; in addition, when the transformation matrix is solved for the target with the calibration distance of more than 20 meters according to the algorithm, the range is large, so that huge errors are caused, and the spatial alignment fails.

Disclosure of Invention

In view of this, the invention provides a method for aligning a segmented space based on a homography transformation matrix, which can realize the spatial alignment of a camera and a millimeter wave radar loaded on an unmanned vehicle in a larger calibration distance range, and can simplify the calculation process for solving the homography transformation matrix.

The invention relates to a segmented space alignment method based on a homography transformation matrix, which comprises the following steps:

step 1: establishing a homography transformation matrix-based relation between a camera coordinate system and a millimeter wave radar coordinate system:

defining an image coordinate system O 'uv of the camera, wherein O' is located in the upper left corner of the imaging plane of the camera; the u axis is parallel to the scanning line direction of the camera; the v-axis is perpendicular to the camera scan line direction;

defining O 'rho theta as a millimeter wave radar polar coordinate system, and O' as the center of the millimeter wave radar surface; rho is the linear distance between the target and the millimeter wave radar; theta is the angle of the target deviating from the central line of the scanning plane of the millimeter wave radar, the relation between the image coordinate system O 'uv of the camera and the millimeter wave radar polar coordinate system O' rho theta is expressed as follows:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=N\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)$

wherein, $N=\left[\begin{array}{ccc}{n}_{11}& n_{12}& n_{13}\\ n_{21}& n_{22}& n_{23}\\ n_{31}& n_{32}& n_{33}\end{array}\right],$ defining a homography transformation matrix;

step 2: determining a proper calibration distance between the unmanned vehicle and a calibration target:

definition O "" X_rY_rZ_rThe rectangular coordinate system of the millimeter wave radar is represented, and O' is the center of the surface of the millimeter wave radar; y is_rThe axis is the central line of a scanning plane of the millimeter wave radar, is vertical to the surface of the millimeter wave radar and points to the right front; x_rAxis and Y_rVertical, pointing to the right; z_rAxis perpendicular to X_r、Y_rA determined plane pointing upwards;

the relationship between the millimeter wave radar rectangular coordinate system and the millimeter wave radar polar coordinate system is as follows:

$\left[\begin{array}{c}{X}_r\\ Y_r\\ 1\end{array}\right]=\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)\′$

the distance between the calibration target and the unmanned vehicle is on the longitudinal axis Y of the millimeter wave radar rectangular coordinate system_rThe projection on the image is called a calibration distance; in the millimeter waveIn the detection range of the radar, determining a proper calibration distance L according to the maximum movement speed of the unmanned vehicle; dividing the calibration distance L into a short-distance range L1 and a long-distance range L2 from near to far, dividing the calibration distance L into m1 sections in the short-distance range L1, dividing the calibration distance L into m2 sections in the long-distance range L2, and ensuring that L1/m1 is smaller than L2/m 2;

and step 3: respectively acquiring images and data information of a calibration target through a camera and a millimeter wave radar which are loaded in the unmanned vehicle:

respectively placing the calibration targets at different sections of which the calibration distance L is divided in the step 2, respectively detecting the targets at the distances of m1+ m2 by the millimeter wave radar and the camera, and aiming at the target at each section of distance during detection, enabling the target to be located along Y_rDividing the axial direction into m rows, and dividing each row along X_rEqually dividing the axial direction into h sections, and controlling the millimeter wave radar to acquire coordinate data (X) of each section^M _rk，Y^M _rk) Controlling the camera to capture image data f of each segment_k ^MWherein M ═ 1., (M1+ M2), k ═ 1, 2., (M2);

and 4, step 4: image data f for each segment in each segment obtained by the camera in step 3_k ^MSeparately calculating the coordinates of the centroid (u) of the image_k ^M,v_k ^M)；

And 5: solving a homography space transformation matrix representing the relation between the millimeter wave radar coordinate system and the camera coordinate system:

millimeter wave radar coordinate data corresponding to all small segments obtained for each segment of distance separated from the whole calibration distance L (a)

Y^M _rk) And image data (u) of the camera^M _k,v^M _k) Forming a corresponding data set in each segment, and substituting each data set into equations (7) and (7)' respectively to obtain:

$\left[\begin{array}{c}{u}_1^M\\ \·\\ \·\\ \·\\ u_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{11}^M\\ n_{12}^M\\ n_{13}^M\end{array}\right]---\left(8\right)$

$\left[\begin{array}{c}{v}_1^M\\ \·\\ \·\\ \·\\ v_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{21}^M\\ n_{22}^M\\ n_{23}^M\end{array}\right]---\left(9\right)$

and $\left[\begin{array}{c}1\\ \·\\ \·\\ \·\\ 1\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{31}^M\\ n_{32}^M\\ n_{33}^M\end{array}\right]---\left(10\right)$

definition of $P^M=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right],$ $N^M={\left[\begin{array}{ccc}{n}_{11}^M& n_{21}^M& n_{31}^M\\ n_{12}^M& n_{22}^M& n_{32}^M\\ n_{13}^M& n_{23}^M& n_{33}^M\end{array}\right]}^T,$ $U^M={\left[\begin{array}{ccc}{u}_1^M& \·\·\·& u_k^M\end{array}\right]}^T,$ $V^M={\left[\begin{array}{ccc}{v}_1^M& \·\·\·& v_k^M\end{array}\right]}^T,$ I_k×1＝[1…1]^TThen homography spatial transformation matrix N^MThe least squares solution of (d) can be expressed as: $N^M={\left[\begin{array}{ccc}{N}_1^{M^T}& N_2^{M^T}& N_3^{M^T}\end{array}\right]}^T,$ wherein,

andrespectively as follows: $N_1^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{U}^M,$ $N_2^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{V}^M$ and $N_3={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{I}_{k\×1};$

step 6: the space alignment of the vision sensor and the millimeter wave radar is realized:

and (3) judging which segment the distance is in step 2 according to the actual distance of the calibration target scanned by the millimeter wave radar, and searching a homography space transformation matrix corresponding to the distance from m1+ m2 results obtained by calculation in step 5 to realize space alignment.

In the step 2, when the calibration distance is 50 meters, 0-20 meters are in a short distance range and are divided into 4 sections; the distance range is 20-50m, and the distance range is divided into 3 sections.

The method for calculating the centroid coordinates of each small segment of image in the calibration target image in the step 4 is as follows:

s40, manually selecting a candidate area containing a calibration target;

s41, carrying out median filtering on the candidate area image to eliminate the noise in the image;

s42, performing Sobel operator edge detection on the candidate region image to obtain a binarized calibration target edge image;

s43, in an image coordinate system with pixels as units, finding u-axis pixel point coordinates u of the coordinate minimum value and the coordinate maximum value in the calibration target edge image along the u-axis_min,u_maxEdge ofv-axis pixel point coordinate v for searching and calibrating coordinate minimum value and coordinate maximum value in target edge image_min,v_maxConnecting the above 4 points with straight lines in clockwise or counterclockwise direction to form a quadrilateral region, and using formula in the quadrilateral region ${u_k}^M=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ And ${v^M}_k=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ calculating the coordinates of the centroid (u) of the calibration target_k ^M,v_k ^M) Wherein f is_k ^MAnd (i, j) represents the gray value of a pixel point (i, j) in the quadrilateral region corresponding to the kth subsection of the target on the Mth distance subsection.

The invention has the following beneficial effects:

1) by segmenting the larger calibration distance and respectively solving the homography transformation matrix between the camera and the millimeter wave radar coordinate system for each segment, the error caused by expressing the coordinate relation between two sensors by using the same homography transformation matrix in the prior art is avoided, and the space alignment of the detection of the target with the larger calibration distance can be realized;

2) by deducing the relationship between different coordinate systems representing the camera and the millimeter wave radar, and finally adopting a homography transformation matrix N to represent the relationship between the coordinate systems, and solving target data respectively obtained by two sensors to obtain the homography transformation matrix N, the internal parameter matrix of the camera formed by solving a scaling factor, a focal length and the like and the external parameter matrix of the camera formed by a rotation matrix and a translation vector are avoided, the operation process is greatly simplified, and the operation time is saved;

3) according to the fact that the attention degree of a sensor in the unmanned vehicle to a near-distance target and a far-distance target is different, the subsection fineness of the near-distance target and the subsection fineness of the far-distance target are different, the spatial alignment is guaranteed, and meanwhile the calculation amount can be reduced;

4) in an image coordinate system, after an edge image of a target is obtained, pixel points of the maximum value and the minimum value in two axial directions are respectively found, the 4 pixel points are surrounded into a quadrangle, and the centroid of the quadrangle is found.

Drawings

FIG. 1 is a schematic view of a camera pin hole model;

FIG. 2 is a schematic diagram of a millimeter wave radar coordinate system;

FIG. 3 is a schematic diagram of a mapping relationship between an image coordinate system and a millimeter wave radar rectangular coordinate system.

Detailed Description

The invention is described in detail below by way of example with reference to the accompanying drawings.

The invention provides a segmented space alignment method based on a homography transformation matrix, which comprises the following steps:

step 1: establishing a homography transformation matrix-based relation between a camera coordinate system and a millimeter wave radar coordinate system:

as shown in FIG. 1, OX_cY_cZ_cRepresenting a camera coordinate system, with an origin O located at the optical center of the camera; x_cThe axis is parallel to the scanning line direction of the camera and points to the increasing direction of the scanning pixels; y is_cThe axis is vertical to the scanning line direction of the camera and points to the increasing direction of the scanning line; z_cThe axis is perpendicular to the imaging plane and points in the direction of the camera's line of sight. O 'uv denotes an image coordinate system in units of pixels, O' being located at the upper left corner of the imaging plane; u axis and X_cParallel connection; v-axis and Y_cParallel. O "xy represents the image coordinate system in millimeters, O" isA focal point of the image plane; x axis and X_cParallel connection; y axis and Y_cParallel. f is the focal length of the camera and I denotes the imaging plane. Assuming that the point P is in the coordinate system OX_cY_cZ_cThe coordinates under O 'xy and O' uv are respectively (X)_c,Y_c,Z_c) (x, y) and (u, v) from the point P in the coordinate system OX_cY_cZ_cAnd a geometric proportional relation in a coordinate system O' xy:

the above relationship is expressed in homogeneous form as:

$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]---\left(1\right)$

the scaling and translation relations of the P point under the coordinate system O 'xy and the coordinate system O' uv are as follows: x is S (u-u)₀),y＝S(v-v₀) Expressed in homogeneous form are:

$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=S\left[\begin{array}{ccc}1& 0& -u_0\\ 0& 1& -v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]---\left(2\right)$

where S is a scaling factor, (u)₀,v₀) Is the coordinate of the coordinate system O "xy origin O" in the coordinate system O' uv. Assume world coordinate system O' X_wY_wZ_wThe coordinate of the point P in this coordinate system is (X)_w,Y_w,Z_w) The relationship between the two coordinate systems is expressed in a homogeneous form as follows:

$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(3\right)$

where R and T represent the rotation matrix and translation vector, respectively. By combining formula (1), formula (2) and formula (3), there is a relationship:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{\mathrm{\β}}\left[\begin{array}{ccc}1& 0& u_0\\ 0& 1& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(4\right)$

wherein, β ═ Z_c·S。

As shown in FIG. 2, O "" X_rY_rZ_rThe rectangular coordinate system of the millimeter wave radar is represented, and O' is the center of the surface of the millimeter wave radar; y is_rThe axis is the central line of a scanning plane of the millimeter wave radar, is vertical to the surface of the millimeter wave radar and points to the right front; x_rAxis and Y_rVertical, pointing to the right; z_rAxis perpendicular to X_r、Y_rA determined plane, pointing upwards. O "" rho θ represents the millimeter wave radar polar coordinate system, the origin and the coordinate system O "" X_rY_rZ_rThe original points of the two are overlapped; rho is the linear distance between the target and the millimeter wave radar; theta is the angle of the target deviating from the central line of the millimeter wave radar scanning plane. Point P is at O "" ρ θ and O "" X_rY_rZ_rThe coordinates of (p, θ) and (X) are_r,Y_r,Z_r) Since the scanning plane of the millimeter wave radar is a two-dimensional plane next to the coordinate system O' rho theta, Z is present_r0, point P is at O "" ρ θ and O "" X_rY_rZ_rThe following trigonometric relationship is expressed as:

$\left[\begin{array}{c}{X}_r\\ Y_r\\ Z_r\end{array}\right]=\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 0\end{array}\right]---\left(5\right)$

assuming that the rectangular coordinate system of the millimeter wave radar is a world coordinate system, and by means of the intermediate variable of the world coordinate system and combining the equations (4) and (5), the relationship between the image coordinate system O' uv and the millimeter wave polar coordinate system O "" [ rho ] is expressed as:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{\mathrm{\β}}\left[\begin{array}{ccc}1& 0& u_0\\ 0& 1& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 0\\ 1\end{array}\right]---\left(6\right)$

as shown in fig. 3, when the camera and the millimeter wave radar observe the same target, the target point scanned by the millimeter wave radar may be projected into the image collected by the camera by equation (6). However, the zoom factor S, the camera focal length f and other camera internal parameters, and the rotation matrix R, the translation vector T and other external parameters need to be estimated, the calculation process is complicated, and for simplifying the calculation, an equivalent transformation relation N is used to represent the relation between O' uv and O "") p θ, which is expressed as:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=N\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)$

wherein, $N=\left[\begin{array}{ccc}{n}_{11}& n_{12}& n_{13}\\ n_{21}& n_{22}& n_{23}\\ n_{31}& n_{32}& n_{33}\end{array}\right],$ referred to as a homography transformation matrix.

Step 2: determining a proper calibration distance and reasonably segmenting the calibration distance:

the method comprises the steps that a calibrated target distance rho measured under a millimeter wave radar polar coordinate system is called a calibrated distance, the calibrated distance L is divided into a short-distance range L1 and a long-distance range L2 from near to far, the calibrated distance L is divided into m1 sections in the short-distance range L1, the calibrated distance L is divided into m2 sections in the long-distance range L2, and the short-distance range section can be larger than the short-distance range section because the short-distance target needs to be accurately detected and only needs to be roughly judged in the unmanned vehicle detection process, wherein the L1/m1 is smaller than the L2/m 35 2;

in the embodiment, the specified maximum speed of the unmanned vehicle is 36km/h, and according to the relation between the distance area concerned by the unmanned vehicle during driving and the speed of the unmanned vehicle, 50m is determined to be a proper calibration distance. Within a calibration distance of 50m, it is found through experiments that: the single homography space transformation matrix is utilized to cause alignment failure, so that 4 sections with the interval of 5m are arranged within the range of 0-20m in the calibration distance according to the requirement of the unmanned vehicle speed and the difference of the attention distance area; within the nominal distance of 20-50m, the interval of 10m is a section of 3 sections.

And step 3: respectively acquiring images and data information of a calibration target through a camera and a millimeter wave radar which are loaded in the unmanned vehicle:

in order to ensure the accuracy of spatial alignment, five rows are respectively taken in the longitudinal direction in each segment, and 7 sets of image and data information corresponding to the calibration target are taken in the transverse direction of each row. And a camera of an IEEE1394 interface arranged on the unmanned vehicle and a millimeter wave radar of a CAN bus interface are used for acquiring the image and data information of a fixed target under the same scene, and the information is transmitted to the industrial personal computer.

And 4, step 4: image data f for each segment in each segment obtained by the camera in step 3_k ^MSeparately calculating the coordinates of the centroid (u) of the image_k ^M,v_k ^M)；

Generally, a clustering method adopted by a millimeter wave radar is a nearest field method, and therefore, calibration target data information scanned by the millimeter wave radar in a millimeter wave radar coordinate system corresponds to a coordinate value of a position of a centroid in an image coordinate system. Gray values are represented for the digital image f, f (i, j), and points in the image area are represented for (i, j). Calculating the centroid coordinates of the calibration target in the image information by adopting the following steps:

and S40, manually selecting a candidate area containing the calibration target to reduce the interference of the background on the calculation of the centroid of the calibration target.

And S41, performing median filtering on the candidate area to eliminate noise interference in the image. Median filtering is implemented by the expression f (i, j) ═ media { f (i-k, j-l). Where (k, l) ∈ ω, ω is the 3 × 3 domain of pixels.

And S42, performing Sobel operator edge detection on the candidate region image to obtain a candidate region edge image containing the calibration target. Template for Sobel operator $\mathrm{mask}=\left[\begin{array}{ccc}-1& -2& -1\\ 0& 0& 0\\ 1& 2& 1\end{array}\right],$ And performing convolution operation on the candidate area image and the mask to obtain a binary image of the candidate area.

S43, in an image coordinate system with pixels as units, finding u-axis pixel point coordinates u of the coordinate minimum value and the coordinate maximum value in the calibration target edge image along the u-axis_min,u_maxFinding the v-axis pixel point coordinate v of the coordinate minimum value and the coordinate maximum value in the calibration target edge image along the v-axis_min,v_maxConnecting the above 4 points with straight lines in clockwise or counterclockwise direction to form a quadrilateral region, and using formula in the quadrilateral region ${u_k}^M=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ And ${v^M}_k=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ calculating the coordinates of the centroid (u) of the calibration target_k ^M,v_k ^M) Wherein f is_k ^M(i, j) represents the gray value of a pixel point (i, j) in a quadrilateral region corresponding to the kth subsection of the target on the Mth distance subsection;

and 5: solving a homography space transformation matrix representing the relation between the millimeter wave radar coordinate system and the camera coordinate system:

millimeter wave radar coordinate data corresponding to all small segments obtained for each segment of distance separated from the whole calibration distance L (a)

Y^M _rk) And image data (u) of the camera^M _k,v^M _k) Forming a corresponding data set in each segment, and substituting each data set into equations (5) and (7) respectively to obtain:

$\left[\begin{array}{c}{u}_1^M\\ \·\\ \·\\ \·\\ u_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{11}^M\\ n_{12}^M\\ n_{13}^M\end{array}\right]---\left(8\right)$

$\left[\begin{array}{c}{v}_1^M\\ \·\\ \·\\ \·\\ v_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{21}^M\\ n_{22}^M\\ n_{23}^M\end{array}\right]---\left(9\right)$

and $\left[\begin{array}{c}1\\ \·\\ \·\\ \·\\ 1\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{31}^M\\ n_{32}^M\\ n_{33}^M\end{array}\right]---\left(10\right)$

definition of $P^M=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right],$ $N^M={\left[\begin{array}{ccc}{n}_{11}^M& n_{21}^M& n_{31}^M\\ n_{12}^M& n_{22}^M& n_{32}^M\\ n_{13}^M& n_{23}^M& n_{33}^M\end{array}\right]}^T,$ $U^M={\left[\begin{array}{ccc}{u}_1^M& \·\·\·& u_k^M\end{array}\right]}^T,$ $V^M={\left[\begin{array}{ccc}{v}_1^M& \·\·\·& v_k^M\end{array}\right]}^T,$ I_k×1＝[1…1]^TThen homography spatial transformation matrix N^MThe least squares solution of (d) can be expressed as: $N^M={\left[\begin{array}{ccc}{N}_1^{M^T}& N_2^{M^T}& N_3^{M^T}\end{array}\right]}^T,$ wherein,

and

respectively as follows: $N_1^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{U}^M,$ $N_2^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{V}^M$ and $N_3={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{I}_{k\×1};$

step 6: the space alignment of the vision sensor and the millimeter wave radar is realized:

according to the actual distance of the calibration target scanned by the millimeter wave radar, firstly, judging which segment the distance is in the step 2, and then, calculating N of the homography space transformation matrix in the segment according to the step 5^M(M ═ 1, 2.., 7), spatial alignment is achieved.

In summary, the above description is only a preferred embodiment of the present invention, and is not intended to limit the scope of the present invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (3)

1. A segment space alignment method based on a homography transformation matrix is characterized by comprising the following steps:

step 1: establishing a homography transformation matrix-based relation between a camera coordinate system and a millimeter wave radar coordinate system:

defining an image coordinate system O 'uv of the camera, wherein O' is located in the upper left corner of the imaging plane of the camera; the u axis is parallel to the scanning line direction of the camera; the v-axis is perpendicular to the camera scan line direction;

defining O 'rho theta as a millimeter wave radar polar coordinate system, and O' as the center of the millimeter wave radar surface; rho is the linear distance between the target and the millimeter wave radar; theta is the angle of the target deviating from the central line of the scanning plane of the millimeter wave radar, the relation between the image coordinate system O 'uv of the camera and the millimeter wave radar polar coordinate system O' rho theta is expressed as follows:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=N\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)$

wherein, $N=\left[\begin{array}{ccc}{n}_{11}& n_{12}& n_{13}\\ n_{21}& n_{22}& n_{23}\\ n_{31}& n_{32}& n_{33}\end{array}\right],$ defining a homography transformation matrix;

step 2: determining a proper calibration distance between the unmanned vehicle and a calibration target:

definition O "" X_rY_rZ_rThe rectangular coordinate system of the millimeter wave radar is represented, and O' is the center of the surface of the millimeter wave radar; y is_rThe axis is the central line of a scanning plane of the millimeter wave radar, is vertical to the surface of the millimeter wave radar and points to the right front; x_rAxis and Y_rVertical, pointing to the right; z_rAxis perpendicular to X_r、Y_rA determined plane pointing upwards;

the relationship between the millimeter wave radar rectangular coordinate system and the millimeter wave radar polar coordinate system is as follows:

$\left[\begin{array}{c}{X}_r\\ Y_r\\ 1\end{array}\right]=\left[\begin{array}{c}\mathrm{\ρ}\sin \mathrm{\θ}\\ \mathrm{\ρ}\cos \mathrm{\θ}\\ 1\end{array}\right]---\left(7\right)\′$

the distance between the calibration target and the unmanned vehicle is on the longitudinal axis Y of the millimeter wave radar rectangular coordinate system_rThe projection on the image is called a calibration distance; in the detection range of the millimeter wave radar, determining a proper calibration distance L according to the maximum movement speed of the unmanned vehicle; dividing the calibration distance L into a short-distance range L1 and a long-distance range L2 from near to far, dividing the calibration distance L into m1 sections in the short-distance range L1, dividing the calibration distance L into m2 sections in the long-distance range L2, and ensuring that L1/m1 is smaller than L2/m 2;

and step 3: respectively acquiring images and data information of a calibration target through a camera and a millimeter wave radar which are loaded in the unmanned vehicle:

respectively placing the calibration targets at different sections of which the calibration distance L is divided in the step 2, respectively detecting the targets at the distances of m1+ m2 by the millimeter wave radar and the camera, and aiming at the target at each section of distance during detection, enabling the target to be located along Y_rDividing the axial direction into m rows, and dividing each row along X_rEqually dividing the axial direction into h sections, and controlling the millimeter wave radar to acquire coordinate data (X) of each section^M _rk，Y^M _rk) Controlling the camera to capture image data f of each segment_k ^MWherein M ═ 1., (M1+ M2), k ═ 1, 2., (M2);

and 4, step 4: image data f for each segment in each segment obtained by the camera in step 3_k ^MSeparately calculating the coordinates of the centroid (u) of the image_k ^M,v_k ^M)；

And 5: solving a homography space transformation matrix representing the relation between the millimeter wave radar coordinate system and the camera coordinate system:

millimeter wave radar coordinate data corresponding to all small segments obtained for each segment of distance separated from the whole calibration distance L (a)

Y^M _rk) And image data (u) of the camera^M _k,v^M _k) Forming a corresponding data set in each segment, and substituting each data set into equations (7) and (7)' respectively to obtain:

$\left[\begin{array}{c}{u}_1^M\\ \·\\ \·\\ \·\\ u_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{11}^M\\ n_{12}^M\\ n_{13}^M\end{array}\right]---\left(8\right)$

$\left[\begin{array}{c}{v}_1^M\\ \·\\ \·\\ \·\\ v_k^M\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{21}^M\\ n_{22}^M\\ n_{23}^M\end{array}\right]---\left(9\right)$

and $\left[\begin{array}{c}1\\ \·\\ \·\\ \·\\ 1\end{array}\right]=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right]\left[\begin{array}{c}{n}_{31}^M\\ n_{32}^M\\ n_{33}^M\end{array}\right]---\left(10\right)$

definition of $P^M=\left[\begin{array}{ccc}{X}_{r1}^M& Y_{r1}^M& 1\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ X_{\mathrm{rk}}^M& Y_{\mathrm{rk}}^M& 1\end{array}\right],$ $N^M={\left[\begin{array}{ccc}{n}_{11}^M& n_{21}^M& n_{31}^M\\ n_{12}^M& n_{22}^M& n_{32}^M\\ n_{13}^M& n_{23}^M& n_{33}^M\end{array}\right]}^T,$ $U^M={\left[\begin{array}{ccc}{u}_1^M& \·\·\·& u_k^M\end{array}\right]}^T,$ $V^M={\left[\begin{array}{ccc}{v}_1^M& \·\·\·& v_k^M\end{array}\right]}^T,$ I_k×1＝[1…1]^TThen homography spatial transformation matrix N^MThe least squares solution of (d) can be expressed as: $N^M={\left[\begin{array}{ccc}{N}_1^{M^T}& N_2^{M^T}& N_3^{M^T}\end{array}\right]}^T,$ wherein,

and

respectively as follows: $N_1^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{U}^M,$ $N_2^M={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{V}^M$ and $N_3={\left(P^M{P}^{M^T}\right)}^{-1}{P}^{M^T}{I}_{k\×1;}$

step 6: the space alignment of the vision sensor and the millimeter wave radar is realized:

and (3) judging which segment the distance is in step 2 according to the actual distance of the calibration target scanned by the millimeter wave radar, and searching a homography space transformation matrix corresponding to the distance from m1+ m2 results obtained by calculation in step 5 to realize space alignment.

2. The method according to claim 1, wherein when the calibration distance in step 2 is 50m, the short distance range is 0-20m, and the segments are divided into 4 segments; the distance range is 20-50m, and the distance range is divided into 3 sections.

3. The method as claimed in claim 1, wherein the method of calculating the coordinates of the centroid of each small segment of the calibration target image in step 4 is as follows:

s40, manually selecting a candidate area containing a calibration target;

s41, carrying out median filtering on the candidate area image to eliminate the noise in the image;

s42, performing Sobel operator edge detection on the candidate region image to obtain a binarized calibration target edge image;

s43, in an image coordinate system with pixels as units, finding u-axis pixel point coordinates u of the coordinate minimum value and the coordinate maximum value in the calibration target edge image along the u-axis_min,u_maxFinding the v-axis pixel point coordinate v of the coordinate minimum value and the coordinate maximum value in the calibration target edge image along the v-axis_min,v_maxConnecting the above 4 points with straight lines in clockwise or counterclockwise direction to form a quadrilateral region, and using formula in the quadrilateral region ${u_k}^M=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ And ${v^M}_k=\frac{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{{\mathrm{if}}_k}^M(i,j)}{\underset{i}{\mathrm{\Σ}}\underset{j}{\mathrm{\Σ}}{f_k}^M(i,j)}$ calculating the coordinates of the centroid (u) of the calibration target_k ^M,v_k ^M) Wherein f is_k ^MAnd (i, j) represents the gray value of a pixel point (i, j) in the quadrilateral region corresponding to the kth subsection of the target on the Mth distance subsection.

Images