ELECTRO-HYDRAULIC VARIFOCAL LENS-BASED METHOD FOR TRACKING THREE-DIMENSIONAL TRAJECTORY OF OBJECT BY USING MOBILE ROBOT

Abstract

The present disclosure discloses an electro-hydraulic varifocal lens-based method for tracking a three-dimensional (3D) trajectory of an object by using a mobile robot. By modeling an optical imaging system, a functional relation between a focusing control current of an electro-hydraulic varifocal lens and an optimal imaging object distance is obtained. Based on this functional relation, depth information of the object in focus with respect to a mobile robot camera and an average velocity of the objective within a time interval with respect to a previous moment can be obtained. With this information, 3D coordinates and motion trajectory of the object in a camera coordinate system can be calculated. At the same time, the mobile robot locates positions and attitudes in a world coordinate system in real time, and transforms the 3D coordinates of the tracked object from the camera coordinate system to the world coordinate system.

Claims

1. An electro-hydraulic varifocal lens-based method for tracking a three-dimensional (3D) trajectory of an object by using a mobile robot, comprising: step 1, calibrating and modeling an electro-hydraulic varifocal lens, selecting a to-be-tracked moving object using an object tracking algorithm, and obtaining an object tracking box; step 2, carrying out autofocusing on the tracked object using the electro-hydraulic varifocal lens, and recording a focusing control current after the autofocusing is completed, a size of the object tracking box, center point coordinates after undistortion, and four angular point coordinates, and calculating spatial coordinates of the object under a camera coordinate system; step 3, generating a mask of the moving object with the four angular point coordinates of the object tracking box, and calculating, by a Visual Simultaneous Localization and Mapping (V-SLAM) system, a transformation matrix of the camera coordinate system on the mobile robot with respect to a world coordinate system; step 4, transforming the spatial coordinates of the moving object under the camera coordinate system to the world coordinate system and recording corresponding world coordinates, calculating a movement velocity and direction of the object in combination with world coordinates of the object at a previous moment, and updating a movement velocity and direction of the mobile robot; and step 5, repeating steps 2 to 4 to continuously track the 3D trajectory of the object.

2. The electro-hydraulic varifocal lens-based method for tracking a 3D trajectory of an object by using a mobile robot according to claim 1, wherein the calibration in step 1 specifically comprises: calibrating, by a calibration method, the electro-hydraulic varifocal lens under multiple focusing control currents, and obtaining, by curve fitting, a functional relation between the focusing control currents and f.sub.x, f.sub.y:
(f.sub.x,f.sub.y)=H(I)  (1) wherein f.sub.x, f.sub.y denote equivalent focal distances of a camera in x and y directions of a pixel plane respectively, in a unit of px; and I denotes a focusing control current; and obtaining coordinates of a camera's optical center on the pixel plane, and a slant parameter between horizontal and vertical edges of a camera's photosensitive element.

3. The electro-hydraulic varifocal lens-based method for tracking a 3D trajectory of an object by using a mobile robot according to claim 2, wherein the modeling in step 1 specifically comprises: building an electro-hydraulic varifocal lens-based optical imaging system model, recording an optimal imaging object distance under multiple focusing control currents by using the model, and conducting curve fitting on the recorded focusing control currents and the corresponding optimal imaging object distance to obtain a relation between the focusing control currents and the optimal imaging object distance:
u=F(I)  (2) wherein u denotes an optimal imaging object distance.

4. The electro-hydraulic varifocal lens-based method for tracking a 3D trajectory of an object by using a mobile robot according to claim 3, wherein the carrying out autofocusing on a tracked object using the electro-hydraulic varifocal lens in step 2 comprises first autofocusing and subsequent autofocusing; wherein the first autofocusing specifically comprises: (1) searching an initial focusing control current at a certain stride, calculating a sharpness evaluation value for an internal image region of the object tracking box, obtaining a maximum sharpness evaluation value and a focusing control current corresponding to the maximum sharpness evaluation value, and setting a sharpness evaluation threshold:
K=αD.sub.max  (3) wherein α denotes a preset sharpness confidence level, and α<1; and D.sub.max denotes a maximum sharpness evaluation value; and (2) after the autofocusing is finished, recording a size of the object tracking box in an image, center point coordinates after undistortion, and four angular point coordinates.

5. The electro-hydraulic varifocal lens-based method for tracking a 3D trajectory of an object by using a mobile robot according to claim 4, wherein the subsequent autofocusing in step 2 specifically comprises: calculating a sharpness evaluation value Di of the internal image region of the object tracking box; if Di≥K, directly recording a focusing control current I.sub.i at this moment, a size size.sub.i of the object tracking box in an image, center point coordinates of the object tracking box after undistortion, and four angular point coordinates; if Di<1K reading the size size.sub.i of the object tracking box in the image at this moment, and comparing the size size.sub.i with a size size.sub.i−1 of the object tracking box at last successful focusing; it size.sub.i<size.sub.i−1 searching the focusing control current at a certain stride in the direction the optimal imaging object distance becomes longer, calculating the sharpness evaluation value in the object tracking box, and completing focusing after the sharpness evaluation value is greater than or equal to the sharpness evaluation threshold K; if size.sub.i>size.sub.i−1 searching the focusing control current at a certain stride n in the direction the optimal imaging object distance becomes shorter, calculating the sharpness evaluation value in the object tracking box, and completing focusing after the sharpness evaluation value is greater than or equal to the sharpness evaluation threshold; and after the focusing is completed, recording the searched focusing control current, the size of the object tracking box in the image after focusing, center point coordinates after undistortion, and four angular point coordinates.

6. The electro-hydraulic varifocal lens-based method for tracking a 3D trajectory of an object by using a mobile robot according to claim 5; wherein the undistortion in step 2 comprises: calculating an undistorted image in images of a current frame according to a distortion model used in the calibration method al step 1, and reading and recording center point coordinates of the object tracking box in the undistorted image.

7. The electro-hydraulic varifocal lens-based method for tracking a 3D trajectory of an object by using a mobile robot according to claim 6, wherein the calculating spatial coordinates of the object under a camera coordinate system in step 2 is implemented using a camera projection model: $\begin{array}{cc}\left(\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right)=\frac{1}{Z_{\mathrm{ci}}}\left(\begin{array}{ccc}{f}_{\mathrm{xi}}& s& c_x\\ 0& f_{\mathrm{yi}}& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{X}_{\mathrm{ci}}\\ Y_{\mathrm{ci}}\\ Z_{\mathrm{ci}}\end{array}\right)& \left(4\right)\end{array}$ wherein (x.sub.i, y.sub.i) denotes center point coordinates of the object tracking box after undistortion, c.sub.x and c.sub.y denote coordinates of the camera's optical center on the pixel plane, s denotes a slant parameter between horizontal and vertical edges of a camera's photosensitive element, and f.sub.xi and f.sub.yi denote camera's intrinsic parameters under a focusing control current I.sub.i at this moment, X.sub.ci, Y.sub.ci and Z.sub.ci denote spatial coordinates of a center point of the tracked object under the camera coordinate system, Z.sub.ci=u.sub.i, and u.sub.i denotes an optimal imaging object distance obtained under the focusing control current I.sub.i al this moment.

8. The electro-hydraulic varifocal lens-based method for tracking a 3D trajectory of an object by using a mobile robot according to claim 7, wherein the calculating a transformation matrix in step 3 comprises: generating a mask of the same size as an image captured by the camera, and processing the mask according to the four angular point coordinates of the object tracking box to assign a pixel value of 0 to an area corresponding to the object tracking box in the mask and a pixel value of 1 to other areas, and multiplying the image captured by the camera with the processed mask, and taking a product thereof as an input of the V-SLAM system to calculate the transformation matrix.

9. The electro-hydraulic varifocal lens-based method for tracking a 3D trajectory of an object by using a mobile robot according to claim 8, wherein the transforming the spatial coordinates of the moving object under the camera coordinate system to the world coordinate system and recording corresponding world coordinates in step 4 is expressed as: $\begin{array}{cc}\left(\begin{array}{c}{X}_{\mathrm{wi}}\\ Y_{\mathrm{wi}}\\ Z_{\mathrm{wi}}\\ 1\end{array}\right)=T_i\left(\begin{array}{c}{X}_{\mathrm{ci}}\\ Y_{\mathrm{ci}}\\ Z_{\mathrm{ci}}\\ 1\end{array}\right)& \left(5\right)\end{array}$ wherein (X.sub.ci, Y.sub.ci, Z.sub.ci) denote spatial coordinates of the moving object under the camera coordinate system, (x.sub.wi, Y.sub.wi, Z.sub.wi) denote world coordinates of the moving object under the world coordinate system, and Ti denotes a transformation matrix.

10. The electro-hydraulic varifocal lens-based method for tracking a 3D trajectory of an object by using a mobile robot according to claim 9, wherein in step 4, the calculating a movement velocity of the moving object comprises: calculating an average velocity of world coordinates of the moving object at a current moment and world coordinates of the moving object at a previous moment in all directions within a time interval t: $\begin{array}{cc}{v}_i=\frac{\sqrt{{\left(X_{\mathrm{wi}}-X_{\mathrm{wi}-1}\right)}^2+{\left(Y_{\mathrm{wi}}-Y_{\mathrm{wi}-1}\right)}^2+{\left(Z_{\mathrm{wi}}-Z_{\mathrm{wi}-1}\right)}^2}}{t}& \left(6\right)\end{array}$ $\begin{array}{cc}{v}_{\mathrm{xi}}=\frac{X_{\mathrm{wi}}-X_{\mathrm{wi}-1}}{t}& \left(7\right)\end{array}$ $\begin{array}{cc}{v}_{\mathrm{yi}}=\frac{Y_{\mathrm{wi}}-Y_{\mathrm{wi}-1}}{t}& \left(8\right)\end{array}$ $\begin{array}{cc}{v}_{\mathrm{Zi}}=\frac{Z_{\mathrm{wi}}-Z_{\mathrm{wi}-1}}{t}& \left(9\right)\end{array}$ wherein (X.sub.wi, Y.sub.wi, Z.sub.wi) denote world coordinates of the moving object at a current moment, (x.sub.wi−1, Y.sub.wi−1, Z.sub.wi−1) denote world coordinates of the moving object at a previous moment, vi denotes a movement velocity of the moving object in a movement direction, and v.sub.xi, v.sub.yi, and v.sub.zi denote movement velocities of the moving object in x, y and z directions, respectively and the updating a movement velocity and direction of the mobile robot comprises: taking movement velocities of the moving object in all directions at a current moment as movement velocities of the mobile robot in all directions in the world coordinate system:
v.sub.rxi=v.sub.xi  (10)
V.sub.ryi=v.sub.yi  (11)
v.sub.rzi=v.sub.zi  (12) wherein v.sub.rxi, v.sub.ryi and v.sub.rzi denote movement velocities of the mobile robot in x, y and z directions in the world coordinate system, respectively.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0037] FIG. 1 is a flow chart according to Embodiment 1 of the present disclosure.

DETAILED DESCRIPTION OF THE EMBODIMENTS

Embodiment 1

[0038] The present disclosure provides an electro-hydraulic varifocal lens-based method for tracking a three-dimensional (3D) trajectory of an object by using a mobile robot, including:

[0039] step 1, calibrate and model an electro-hydraulic varifocal lens, select a to-be-tracked moving object using an object tracking algorithm, and obtain an object tracking box as a subsequent focusing window. Generally, the object tracking algorithms can be divided into two categories: first, correlation filtering methods, such as CSK, KCF/DCF, CN, etc.; and second, depth learning methods, such as C-COT, ECO and DLT; and the KCF algorithm is selected in the present embodiment.

[0040] The calibration specifically includes: calibrating, by Zhang Zhengyou Calibration Method, the electro-hydraulic varifocal lens under multiple focusing control currents, and obtaining, by curve fitting, a functional relation between the focusing control currents and camera's intrinsic parameters f.sub.x, f.sub.y:

(f.sub.x,f.sub.y)=H(I)  (1)

[0041] where f.sub.x, f.sub.y denote parameters in the camera's intrinsic parameters that change with the focal distance, and are physically defined as equivalent focal distances of a camera in x and y directions of a pixel plane respectively, in a unit of px;

[0042] obtain, by a calibration method, camera's intrinsic parameters c.sub.x, c.sub.y, s and distortion parameters k.sub.1, k.sub.2 (the quantity of the distortion parameters depends on the calibration method used, and only radial distortion is considered in the Zhang Zhengyou Calibration Method) that do not change with the focal distance, where c.sub.x, c.sub.y are physically defined as the coordinates of a camera's optical center on the pixel plane, and s is physically defined as a slant parameter between the horizontal and vertical edges of a camera's photosensitive element, all of which are constants; and I denotes a focusing control current of an electro-hydraulic varifocal lens.

[0043] The modeling specifically includes: building, by Zemax software, an electro-hydraulic varifocal lens-based optical imaging system model, and setting the radius, thickness, curvature, material and other parameters of the electro-hydraulic varifocal lens used in the Zemax software; recording an optimal imaging object distance under multiple focusing control currents by using the optical imaging system model, and conducting curve fitting on the recorded focusing control currents and the corresponding optimal imaging object to obtain a relation between the focusing control currents of the electro-hydraulic varifocal lens and the optimal imaging object distance:

u=F(I)  (2)

[0044] where u denotes an optimal imaging object distance, and I denotes a focusing control current of the electro-hydraulic varifocal lens.

[0045] step 2, carry out autofocusing on the tracked object using the electro-hydraulic varifocal lens, and record a focusing control current I.sub.i after the autofocusing is completed, a size size.sub.i of the object tracking box, center point coordinates (x.sub.i, y.sub.i) after undistortion, and four angular point coordinates, and calculate spatial coordinates (X.sub.ci, Y.sub.ci, Z.sub.ci) of the object under a camera coordinate system, where the camera is equipped with the electro-hydraulic varifocal lens.

[0046] The carrying out autofocusing on a tracked object using the electro-hydraulic varifocal lens includes first autofocusing and subsequent autofocusing, where the first autofocusing specifically includes: (1) searching first an initial focusing control current (focus control current corresponding to a shortest or longest focal distance) at a certain stride n, calculating a sharpness evaluation value for an internal image region of the object tracking box, obtaining a maximum sharpness evaluation value D.sub.max and a focusing control current I.sub.i corresponding to the maximum sharpness evaluation value, and setting a sharpness evaluation threshold:

K=αD.sub.max  (3)

[0047] where α denotes a preset sharpness confidence level, and α<1; and K denotes a sharpness evaluation threshold used in subsequent autofocusing; and

[0048] The sharpness evaluation value is calculated by the sharpness evaluation function, and the sharpness evaluation function can be commonly used SMD function, EOG function, Roberts function, Tenengrad function, Brenner function, Laplacian function or SML function. For ease of understanding, the Laplacian function is selected for calculation in this embodiment, which is expressed as:

D(f)=Σ.sub.yΣ.sub.x|G(x,y)|  (13)

[0049] where G(x,y): denotes convolution of a Laplacian operator at a pixel point (x,y), (x,y) denote coordinates on the pixel plane,

[0050] and the Laplacian operator is expressed as:

[00004]$\begin{array}{cc}L=\frac{1}{6}\left[\begin{array}{ccc}1& 4& 1\\ 4& -20& 4\\ 1& 4& 1\end{array}\right]& \left(14\right)\end{array}$

[0051] (2) after autofocusing is finished, recording a size size.sub.1 of the object tracking box in an image, center point coordinates (x.sub.1, y.sub.1) of the object tracking box after undistortion, and four angular point coordinates.

[0052] Further, the subsequent autofocusing specifically includes: calculating a sharpness evaluation value D of the internal image region of the object tracking box; if Di≥K, directly recording a focusing control current L at this moment, a size size.sub.i of the object tracking box in an image, center point coordinates (x.sub.i, y.sub.i) of the object tracking box after undistortion, and four angular point coordinates; if Di<K, reading the size size.sub.i of the object tracking box in the image at this moment, and comparing the size size.sub.i with a size size.sub.i−1 of the object tracking box at last successful focusing (successful focusing indicates that the sharpness evaluation value is greater than or equal to the threshold); and if size.sub.i<size.sub.i−1, searching the focusing control current at a certain stride n in the direction the optimal imaging object distance becomes longer, calculating the sharpness evaluation value in the object tracking box, and completing focusing after the sharpness evaluation value is greater than or equal to the sharpness evaluation threshold K; and

[0053] if size.sub.i′ size.sub.i−1 searching the focusing control current at a certain stride n in the direction the optimal imaging object distance becomes shorter, calculating the sharpness evaluation value in the object tracking box, and completing focusing after the sharpness evaluation value is greater than or equal to the sharpness evaluation threshold K; and after the focusing is completed, recording the searched focusing control current L, the size size.sub.i of the object tracking box in the image after focusing, center point coordinates (x.sub.i, y.sub.i) of the object tracking box after undistortion, and four angular point coordinates.

[0054] The undistortion of the object tracking box specifically includes: calculating, by a distortion model used in the Zhang Zhengyou Calibration Method in step 1, an undistorted image of a current frame, and reading and recording center point coordinates (x.sub.i, y.sub.i) of the object tracking box in the undistorted image.

[0055] The radial distortion model used in the Zhang Zhengyou Calibration Method is:

x.sub.distorted=x(1+k.sub.1r.sup.2+k.sub.2r.sup.4)  (15)

Y.sub.distorted=(1+k.sub.1r.sup.2+k.sub.2r.sup.4)  (16)

r=√{square root over (x.sup.2+y.sup.2)}  (17)

[0056] where x.sub.distorted and y.sub.distorted denote pixel coordinates of an image after distortion, x and y are ideal pixel coordinates of an image without undistortion, (x,y) denote coordinates on the pixel plane, and k.sub.1, k.sub.2 denote distortion parameters obtained by calibration. Calculate, by the above distortion model, an undistorted image of a current frame, and read and record center point coordinates (x.sub.i, y.sub.i) of the object tracking box in the image after undistortion.

[0057] The calculating spatial coordinates of the object under a camera coordinate system is implemented using a camera projection model:

[00005]$\begin{array}{cc}\left(\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right)=\frac{1}{Z_{\mathrm{ci}}}\left(\begin{array}{ccc}{f}_{\mathrm{xi}}& s& c_x\\ 0& f_{\mathrm{yi}}& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{X}_{\mathrm{ci}}\\ Y_{\mathrm{ci}}\\ Z_{\mathrm{ci}}\end{array}\right)& \left(4\right)\end{array}$

[0058] where (x.sub.i, y.sub.i) denote center point coordinates of the object tracking box after undistortion, c.sub.x, c.sub.y, s denote camera's intrinsic parameters obtained during calibration and do not vary with the focal distance, and f.sub.xi and f.sub.yi denote camera's intrinsic parameters obtained by substituting a focusing control current L at this moment into calibration formula (1); and X.sub.ci, Y.sub.ci and Z.sub.ci denote spatial coordinates of a center point of the tracked object under the camera coordinate system, Z.sub.ci=u.sub.i, and u.sub.i denotes an optimal imaging object distance obtained by substituting the focusing control current I.sub.i at this moment into formula (2). The coordinates (X.sub.ci, Y.sub.ci, and Z.sub.ci) of the center point of the tracked object under the camera coordinate system can be calculated by substituting center point coordinates (x.sub.i, y.sub.i) of the object tracking box after undistortion, the camera's intrinsic parameters obtained during calibration and Z.sub.ci into the above camera projection model.

[0059] step 3, generating a mask of the same size as an image captured by the camera equipped with the electro-hydraulic varifocal lens, and processing the mask according to the four angular point coordinates of the object tracking box to assign a pixel value of 0 to an area corresponding to the object tracking box in the mask and a pixel value of 1 to other areas; and multiplying the image captured by the camera with the processed mask, and taking a product thereof as an input of the visual simultaneous localization and mapping (V-SLAM) system to calculate the transformation matrix Ti of the camera coordinate system with respect to the world coordinate system such that the influence of the moving object on the stability of a V-SLAM algorithm is reduced. The V-SLAM algorithm can be selected from ORB-SLAM2, LSD-SLAM, DSO, DTAM and other V-SLAM algorithms that support monocular cameras. For ease of understanding, the ORB-SLAM2 algorithm is selected in this embodiment.

[0060] step 4, transform the spatial coordinates (X.sub.ci, Y.sub.ci, Z.sub.ci) of the moving object under the camera coordinate system to the world coordinate system and record corresponding world coordinates (X.sub.wi, Y.sub.wi, Z.sub.wi), calculate a movement velocity and direction of the object in combination with world coordinates (X.sub.wi−1, Y.sub.wi−1, Z.sub.wi−1) of the object at a previous moment, and update a movement velocity and direction of the mobile robot.

[0061] The transforming the spatial coordinates (X.sub.ci, Y.sub.ci, Z.sub.ci) of the moving object under the camera coordinate system to the world coordinate system and recording corresponding world coordinates (X.sub.wi, Y.sub.wi, Z.sub.wi) is expressed as:

[00006]$\begin{array}{cc}\left(\begin{array}{c}{X}_{\mathrm{wi}}\\ Y_{\mathrm{wi}}\\ Z_{\mathrm{wi}}\\ 1\end{array}\right)=T_i\left(\begin{array}{c}{X}_{\mathrm{ci}}\\ Y_{\mathrm{ci}}\\ Z_{\mathrm{ci}}\\ 1\end{array}\right).& \left(5\right)\end{array}$

[0062] where Ti is the 4×4 transformation matrix obtained in step 3, which represents coordinate transformation from the camera coordinate system to the world coordinate system.

[0063] calculating a movement velocity of the object includes: calculating an average velocity of world coordinates (X.sub.wi, Y.sub.wi, Z.sub.wi) of the object at a current moment and world coordinates (X.sub.wi−1, Y.sub.wi−1, Z.sub.wi−1) of the object at a previous moment in all directions within a time interval t:

[00007]$\begin{array}{cc}{v}_i=\frac{\sqrt{{\left(X_{\mathrm{wi}}-X_{\mathrm{wi}-1}\right)}^2+{\left(Y_{\mathrm{wi}}-Y_{\mathrm{wi}-1}\right)}^2+{\left(Z_{\mathrm{wi}}-Z_{\mathrm{wi}-1}\right)}^2}}{t}& \left(6\right)\end{array}$$\begin{array}{cc}{v}_{\mathrm{xi}}=\frac{X_{\mathrm{wi}}-X_{\mathrm{wi}-1}}{t}& \left(7\right)\end{array}$$\begin{array}{cc}{v}_{\mathrm{yi}}=\frac{Y_{\mathrm{wi}}-Y_{\mathrm{wi}-1}}{t}& \left(8\right)\end{array}$$\begin{array}{cc}{v}_{\mathrm{Zi}}=\frac{Z_{\mathrm{wi}}-Z_{\mathrm{wi}-1}}{t}& \left(9\right)\end{array}$

[0064] where vi denotes a movement velocity of the object in a movement direction, and v.sub.xi, v.sub.yi, and v.sub.zi denote movement velocities of the object in x, y and z directions, respectively; and

[0065] The updating a movement velocity and direction of the mobile robot includes: taking movement velocities of the moving object in all directions at a current moment as movement velocities of the mobile robot in all directions in the world coordinate system:

v.sub.rxi=v.sub.xi  (10)

v.sub.ryi=v.sub.yi  (11)

v.sub.rzi=v.sub.zi  (12)

[0066] where v.sub.rxi, v.sub.ryi and v.sub.rzi denote movement velocities of the mobile robot in x, y and z directions in the world coordinate system, respectively.

[0067] step 5, repeating steps 2 to 4 to continuously track the 3D trajectory of the object.

[0068] The present disclosure does not require stereo vision equipment with complex structure and large size, and the mobile robot can track the 3D trajectory of the object simply using a single camera, which is less costly. The 3D trajectory of the object can be tracked by the mobile robot, in the meanwhile, the tracked object can be kept in focus in the image through autofocusing, which improves the stability of the object tracking algorithm used. In the process of using the V-SLAM technology for real-time positioning of the mobile robot, the influence of the moving object on the stability of the V-SLAM algorithm is eliminated through the mask, which improves the robustness of the V-SLAM algorithm in this application scenario.

Embodiment 2

[0069] Compared with Embodiment 1, the undistortion of the object tracking box in step 2 in this embodiment includes: directly calling an undistortion function of OpenCV, introducing distortion parameters obtained through calibration, and conducting undistorting on an image of a current frame; and reading and recording center point coordinates (x.sub.i, y.sub.i) of an object tracking box in the undistorted image.