Binocular-vision-based method for tracking fruit space attitude and fruit space motion

Abstract

A binocular-vision-based method for tracking fruit space attitude and fruit space motion, the method comprising: establishing a connected base coordinate system by taking a junction of a fruit and a fruit stem as an origin; statically photographing a feature point on the surface of the fruit and a point of the connected base coordinate system established at the junction of the fruit and the fruit stem; storing a photographed image; acquiring an inherent relationship between the feature point and the connected base coordinate system; photographing dynamic motion of the fruit; acquiring absolute coordinates of the feature point on the surface of the fruit; calculating, according to the inherent relationship between the feature point and the connected base coordinate system, absolute coordinates of a point of the connected base coordinate system at each moment corresponding to each frame of image; and respectively calculating the displacement, instantaneous speed and instantaneous acceleration of the fruit, calculating swing angular displacement and swing angular acceleration of the fruit, and calculating a fruit torsion angular speed and a fruit torsion angular acceleration at the moment t. The study of a fruit motion state in the field of forest fruit harvest through vibration is performed, so that the motion of fruits can be better tracked.

Description

FIELD OF THE INVENTION

The present invention belongs to the field of fruit tree harvesting in agriculture and forestry, and specifically relates to a binocular-vision-based method for tracking fruit space attitude and fruit space motion, which is used in the study of fruit motion states in the field of fruit vibration harvesting.

BACKGROUND OF THE INVENTION

Fruit harvesting is the most time-consuming and laborious part of fruit production. For dried fruits such as red dates, walnuts, and ginkgo, the most effective harvesting method is mechanical vibration harvesting. The harvesting effect of its vibration harvesting machinery is related to many factors, including the growth characteristics of fruit trees and the working parameters of mechanical vibration. The research on the vibratory harvesting of forest fruits is more about viewing the tree itself as a second-order system, detecting the relationship between excitation and response, and measuring the overall stiffness and damping ratio of the tree. In recent years, more detailed research has been carried out in China and a simplified mechanical model of tree trunk and branch vibration has been established. Through experiments and theoretical calculations, it is believed that the cantilever straight beam model is more accurate, which provides a theoretical basis for the research of vibratory harvesting machinery. However, there are still gaps in the research on the movement of fruits during the vibration harvesting process, in order to explain the principle of vibratory fruit dropping in more detail, it is necessary to study the movement of fruits.

SUMMARY OF THE INVENTION

The technical problem to be solved by the present invention is to provide a binocular-vision-based method for tracking fruit space attitude and fruit space motion in response to the above-mentioned shortcomings of the prior art. This binocular-vision-based method for tracking fruit space attitude and fruit space motion method is aimed at the research of fruit movement state in the field of forest fruit vibration harvesting, which can better track the movement of fruit.

In order to achieve the above-mentioned technical objectives, the technical solutions adopted by the present invention are:

a binocular-vision-based method for tracking fruit space attitude and fruit space motion, comprising:

(1) marking three feature points C₃ on a surface of a fruit;

(2) establishing a connected base coordinate system by taking a junction of the fruit and a fruit stem as an origin, a X axis, a Y axis, and a Z axis of the connected base coordinate system are marked as a X connected-axis, a Y connected-axis and a Z connected-axis, respectively, and the origin of the connected base coordinate system is marked as O₁, an end point of the unit vector in a positive direction of the X connected-axis is marked as X₁, an end point of the unit vector in a positive direction of the Y connected-axis is marked as Y₁, an end point of the unit vector in a positive direction of the Z connected-axis is marked as Z₁;

(3) establishing a public reference base coordinate system;

(4) statically photographing the three feature points on the surface of the fruit and connected base coordinate system points Z₁ established at the junction of the fruit and the fruit stem, using Phantom software to store the photographed images, and using TEMA software to process the feature points and connected base coordinate system points Z₁ in the images, acquiring a absolute coordinate of the feature points C₃ and the connected base coordinate system points Z₁, and then acquiring an inherit relationship between the feature points and the connected base coordinate system;

(5) photographing the fruit dynamic motion, using Phantom software to store each frame of the image in the photographed video, using TEMA software to process the feature points in the images to acquiring the absolute coordinate of the feature points on the fruit surface, according to an inherent relationship between the feature points and the connected base coordinate system, the absolute coordinate of each frame of image corresponding to the connected base coordinate system points Z₁ at each moment being calculated by inverse rotation transformation, and calculating an angle between the vector {right arrow over (O₁Z₁)} at each moment and the Z axis in the public reference base coordinate system through the space vector angle formula, and the angle representing a swing posture of the fruit;

(6) rotating the {right arrow over (O₁Z₁)} at each moment to a position that coincides with the Z axis in the public reference base coordinate system, and calculating an angle between the rotated {right arrow over (O₁X₁)} and the X axis in the public reference base coordinate system, the angle representing a twisting posture of the fruit; and

(7) setting moments corresponding to two adjacent frames to t+1 moment and t moment, and calculating a displacement of the point O₁ along the X axis, Y axis and Z axis of the public reference base coordinate system at t+1 moment and t moment respectively, calculating an instantaneous speed along the X axis, an instantaneous speed along the Y axis, and an instantaneous speed along the Z axis of the fruit at t moment according to the displacement, calculating an instantaneous acceleration along the X axis, an instantaneous acceleration along the Y axis and an instantaneous acceleration along the Z axis of the fruit at t moment according to the instantaneous speed, calculating a swing angular displacement of the fruit in a time interval between the two adjacent frames of images, sequentially calculating a swing angular speed of the fruit at t moment and a swing angular acceleration of the fruit at t moment via the swing angular displacement, and calculating a torsion angular speed of the fruit at t moment and a torsion angular acceleration of the fruit at t moment according to the torsion angular degree from t moment to t+1 moment.

As a further improved technical solution for the present invention, the step (2) includes:

establishing the connected base coordinate system by taking a junction of the fruit and the fruit stem as the origin, and taking a line between the junction of the fruit and the fruit stem and a center point of the fruit as a center line, the center line being the Z axis of the connected base coordinate system, which being marked as the Z connected-axis, taking a direction from the central point pointing to the joining point as the positive direction of the Z connected-axis, establishing the X axis and Y axis of the connected base coordinate system on a plane perpendicular to the Z axis of the connected base coordinate system, and marking them as X connected-axis and Y connected-axis respectively; marking the origin of the connected base coordinate system as O₁, marking the end point of the unit vector in the positive direction of the X connected-axis as X₁, marking the end point of the unit vector in the positive direction of the Y₁ connected-axis as Y₁, and marking the end point of the unit vector in the positive direction of the Z connected-axis as Z1.

As a further improved technical solution for the present invention, the absolute coordinates are spatial coordinates in the public reference base coordinate system, and the unit vectors of the coordinate axis of the public reference base coordinate system are:

Z=(0 0 1)^T.

As a further improved technical solution for the present invention, the step (4) includes:

(A) under static conditions, using two high-speed cameras to statically photograph the three feature points on the surface of the fruit and the points Z₁ of the connected base coordinate system established at the junction of the fruit and the fruit stem, using Phantom software to store the photographed images, and using TEMA software to process the feature points C₃ and the connected base coordinate system points Z₁ in the images, acquiring the absolute coordinate of the feature points C₃ and the absolute coordinate of the connected base coordinate system points Z₁;

(b) creating vectors {right arrow over (C₁C₂)} and {right arrow over (C₁C₃)} via absolute coordinate, unitizing {right arrow over (C₁C₂)} and {right arrow over (C₁C₃)}, creating vector vector vector {right arrow over (C₁Y₁)} and vector {right arrow over (C₁Z₁)};

(c) doing a vector product of unitized {right arrow over (C₁C₂)} and {right arrow over (C₁C₃)} to get: {right arrow over (Y)}={right arrow over (C₁C₂)}×{right arrow over (C₁C₃)}, doing a vector product of {right arrow over (X)}={right arrow over (C₁C₂)} and {right arrow over (Y)} to get: {right arrow over (Z)}={right arrow over (X)}× {right arrow over (Y)}, thus, establishing a coordinate system C_xyz by taking feature point C₁ as an origin, the coordinate axis vector of the coordinate system C_xyz is: {right arrow over (X_Cxyz)}={right arrow over (Y_Cxyz)}={right arrow over (Z_Cxyz)}=

calculating angles γ_x between the vector {right arrow over (X_Cxyz)} and the X axis, Y axis and Z axis of the public reference base coordinate system through the calculation formula of the space vector angle,

calculating angles γ_y between the vector {right arrow over (Y_Cxyz)} and the X axis, Y axis and Z axis of the public reference base coordinate system,

calculating angles γ_z between the vector {right arrow over (Z_Cxyz)} and the X-axis, Y-axis and Z-axis of the public reference base coordinate system, then the coordinate conversion matrix is:

$\begin{array}{cc}A=\left(\begin{array}{ccc}\cos {\alpha }_x& \cos {\beta }_x& \cos {\gamma }_x\\ \cos {\alpha }_y& \cos {\beta }_y& \cos {\gamma }_z\\ \cos {\alpha }_z& \cos {\beta }_z& \cos {\gamma }_z\end{array}\right);& \left(1\right)\end{array}$

the coordinates of point O₁ under the coordinate system C_xyz are obtained by formula (2):

$\begin{array}{cc}{O}_{1_{C_{\mathrm{xyz}}}}=A*\overrightarrow{C_1{O}_1}=\left(\begin{array}{ccc}\cos {\alpha }_x& \cos {\beta }_x& \cos {\gamma }_x\\ \cos {\alpha }_y& \cos {\beta }_y& \cos {\gamma }_z\\ \cos {\alpha }_z& \cos {\beta }_z& \cos {\gamma }_z\end{array}\right)\left(\begin{array}{c}{x}_{o_1}^{\prime }\\ y_{o_1}^{\prime }\\ z_{o_1}^{\prime }\end{array}\right)=\left(\begin{array}{c}{x}_{O_{1_{C_{\mathrm{xyz}}}}}\\ y_{O_{1_{C_{\mathrm{xyz}}}}}\\ z_{O_{1_{C_{\mathrm{xyz}}}}}\end{array}\right);& \left(2\right)\end{array}$

wherein, {right arrow over (C₁O₁)}=(x_O1′ y_O1′ z_O1′)^T, and obtained from (b) in step (4);

similarly,

$\begin{array}{cc}{X}_{1_{C_{\mathrm{xyz}}}}=A^{*}\overrightarrow{C_1{X}_1}& \end{array}$

$Y_{1_{C_{\mathrm{xyz}}}}=A^{*}\overrightarrow{C_1{X}_1}\mathrm{and}{Z}_{1_{C_{\mathrm{xyz}}}}=A^{*}\overrightarrow{C_1{Z}_1}$ can be obtained, wherein the coordinates of

$\begin{array}{cc}{X}_{1_{C_{\mathrm{xyz}}}}{Y}_{1_{C_{\mathrm{xyz}}}}{Z}_{1_{C_{\mathrm{xyz}}}}& \end{array}$ are the coordinates of points Z₁ under the coordinate system C_xyz;

the coordinates of

$O_{1_{C_{\mathrm{xyz}}}}{X}_{1_{C_{\mathrm{xyz}}}}{Y}_{1_{C_{\mathrm{xyz}}}}{Z}_{1_{C_{\mathrm{xyz}}}}$ are the coordinates of points Z₁ under the coordinate system C_xyz, that is, representing the inherent relationship between the feature point and the connected base coordinate system.

As a further improved technical solution for the present invention, the step (5) includes:

(a) using two high-speed cameras to photograph the fruit dynamic motion, using Phantom software to store each frame of image of the photographed video, using TEMA software to process the feature points in the images, and obtaining the absolute coordinate of the feature points on the surface of the fruit;

(b) calculating angles γ_xt between the vector

$\overrightarrow{X_{C_{{\mathrm{xyz}}_t}}}$ and the X axis, Y axis, and Z axis of the public reference base coordinate system at t moment through the calculation formula of the space vector angle, calculating angles γ_yt, between the vector

$\overrightarrow{Y_{C_{{\mathrm{xyz}}_t}}}$ and me x axis, Y axis and Z axis of the public reference base coordinate system, calculating angles γ_zt between the vector

$\overrightarrow{Z_{C_{{\mathrm{xyz}}_t}}}$ and the X axis, Y axis and Z axis of the public reference base coordinate system, wherein,

$\overrightarrow{X_{C_{{\mathrm{xyz}}_t}}}\overrightarrow{Y_{C_{{\mathrm{xyz}}_t}}}\overrightarrow{Z_{C_{{\mathrm{xyz}}_t}}}$ are the coordinate axis vector of the coordinate system C_xyz established by taking the feature point C₁ in one frame of image corresponding to the t moment as an origin, then the coordinate conversion matrix is:

$\begin{array}{cc}{A}_1=\left(\begin{array}{ccc}\cos {\alpha }_{x_t}& \cos {{\beta }_x}_t& \cos {\gamma }_{x_t}\\ \cos {{\alpha }_y}_t& \cos {\beta }_{y_t}& \cos {\gamma }_{z_t}\\ \cos {\alpha }_{z_t}& \cos {\beta }_{z_t}& \cos {\gamma }_{z_t}\end{array}\right);& \left(3\right)\end{array}$

the coordinates of point O₁ in the public reference base coordinate system, that is, the absolute coordinate is:

$\begin{array}{cc}\left(\begin{array}{c}{x}_{O_1}\\ y_{O_1}\\ z_{O_1}\end{array}\right)=\left(\begin{array}{c}{x}_{C_1}\\ y_{C_1}\\ z_{C_1}\end{array}\right)+A_i^{-1}*\left(\begin{array}{c}{x}_{O_{1_{C_{\mathrm{xyz}}}}}\\ y_{O_{1_{C_{\mathrm{xyz}}}}}\\ z_{O_{1_{C_{\mathrm{xyz}}}}}\end{array}\right);& \left(4\right)\end{array}$

wherein ( Z_O1) are the absolute coordinate of the point O₁ under t moment, ( z_O1) are the absolute coordinate of the point C₁ under t moment,

$\left(x_{O_{1_{C_{\mathrm{xyz}}}}}{y}_{O_{1_{C_{\mathrm{xyz}}}}}{z}_{O_{1_{C_{\mathrm{xyz}}}}}\right)$ are the coordinate of point O₁ in the coordinate system C_xyz; similarly, calculating the absolute coordinate of the points X₁, Y₁, Z₁ under t moment;

each frame of image is independent of each other, and corresponds to a moment respectively, and solves the absolute coordinates of the points Z₁ of the connected base coordinate system frame by frame;

calculating the angle θ between the vector {right arrow over (O₁Z₁)} at each moment and the Z axis in the public reference base coordinate system through the space vector angle formula:

$\begin{array}{cc}\theta =\arccos \left(\frac{x_{z_1}{x}_z+y_{z_1}{y}_z+z_{z_1}{z}_z}{\sqrt{x_{z_1}^2+y_{z_1}^2+z_{z_1}^2}\times \sqrt{x_z^2+y_z^2+z_z^2}}\right);& \left(5\right)\end{array}$

wherein {right arrow over (O₁Z₁)}=(x_z1 y_z1 Z_z1)^T, is the unit vector of the Z axis in the public reference base coordinate system, the angle θ between the vector {right arrow over (O₁Z₁)} and the Z axis in the public reference base coordinate system represents a swing posture of the fruit.

As a further improved technical solution for the present invention, the step (6) includes:

(a) using the axis rotation matrix to rotate {right arrow over (O₁Z₁)} at the moment corresponding to a certain frame of image to a position that coincides with the Z axis in the public reference base coordinate system, calculating the angle between the rotated {right arrow over (O₁X₁)} and the X axis in the public reference base coordinate system, the angle represents a twisting posture of the fruit at the moment corresponding to the frame image, detail as follows:

the Z connected-axis direction vector of the frame of image is: ={right arrow over (O₁Z₁)}=(x_z1 y_z1 z_z1)^T, |{right arrow over (O₁Z₁)}|=√{square root over (x_Z1²+y_Z1²+z_Z1²)}, calculating the angle φ between the Z connected-axis and the Z axis in the public reference base coordinate system:

$\begin{array}{cc}\phi =\arccos \left(\frac{Z_{Z_1}}{\stackrel{\_}{O_1{Z}_1}}\right);& \left(6\right)\end{array}$

calculating the angle ψ between the projection of the Z connected-axis in the X axis and Y axis of the public reference base coordinate system and the Y axis of the public reference base coordinate system:

$\begin{array}{cc}\psi =\arccos \left(\frac{y_{Z_1}}{\sqrt{y_{Z_1}^2+x_{Z_1}^2}}\right);& \left(7\right)\end{array}$

calculating the axis rotation matrix:

$\begin{array}{cc}{T}_1=\left(\begin{array}{ccc}\cos \phi & 0& \sin \phi \\ 0& 0& 0\\ -\sin \phi & 0& \cos \phi \end{array}\right);& \left(8\right)\\ T_2=\left(\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right);& \left(9\right)\end{array}$

the absolute coordinate of the rotated point X₁ is consistent with the coordinate of the rotated vector {right arrow over (O₁X₁)}, and the rotated vector {right arrow over (O₁X₁)} is:

$\begin{array}{cc}\left(\begin{array}{c}{x_{X_1}}^{\prime }\\ {y_{X_1}}^{\prime }\\ {z_{X_1}}^{\prime }\end{array}\right)=T_2^{-1}*T_1^{-1_{*}}\left(\begin{array}{c}{x}_{x_1}\\ y_{X_1}\\ z_{X_1}\end{array}\right);& \left(10\right)\end{array}$

wherein, is the vector {right arrow over (O₁X₁)} before the {right arrow over (O₁Z₁)} of the frame of image is rotated;

the torsion angular degree is:

$\begin{array}{cc}\varphi =\arccos \left(\frac{{x_{X_1}}^{\prime }{x}_X+{y_{X_1}}^{\prime }{y}_X+{z_{X_1}}^{\prime }{z}_X}{\sqrt{{x_{X_1}}^{,2}+{y_{X_1}}^{,2}+{z_{x_1}}^{,2}}\times \sqrt{{x_X}^2+{y_X}^2+{z_X}^2}}\right);& \left(11\right)\end{array}$

the torsion angular degree is the angle between the rotated vector {right arrow over (O₁X₁)} and the X axis in the public reference base coordinate system, which represents the torsion posture of the fruit at the moment corresponding to the frame image,

wherein is the unit vector of the X axis in the public reference base coordinate system;

(b) Similarly, in (a) of step (6), calculating the twisting posture of the fruit at each moment corresponding to each frame image; and

As a further improved technical solution for the present invention, the step (7) includes:

(a) setting moments corresponding to two adjacent frames of images to t+1 moment and t moment, based on the point O₁ of the connected base coordinate system in adjacent two frames of images, and calculating the displacement of the point O₁ along the X axis of the public reference base coordinate system at t+1 moment and t moment respectively:S_x=x_Ot+1−x_O1  (12);

wherein x_O1 are the coordinates x of the point O₁ under the public reference base coordinate system at t+1 moment and t moment; similarly, calculating the displacement S_y along the Y axis and the displacement S, along the Z axis of the point O₁ of the public reference base coordinate system at t+1 moment and t moment, and the combined displacement is:S=√{square root over (S_x²+S_y²+S_z²)}  (13);

(b) using the average speed of the point O₁ moving along the X axis of the public reference base coordinate system from t moment to t+1 moment to express the instantaneous speed v_x(t) of the fruit along the X axis at t moment:

$\begin{array}{cc}{v}_{x\left(t\right)}=\frac{S_x}{\Delta t};& \left(14\right)\end{array}$

wherein Δt is the interval time between two frames of images; similarly, the instantaneous speed v_{y (t)} of the fruit along the Y axis of the public reference base coordinate system at t moment and the instantaneous speed v_{z (t)} of the fruit along the Z axis of the public reference base coordinate system at t moment can be obtained, then the combined speed of the fruit at t moment is:v=√{square root over (v_x(t)²+v_y(t)²+v_z(t)²)}  (15);

(c) using the acceleration of the point O₁ moving along the X axis of the public reference base coordinate system from t moment to t+1 moment to express the instantaneous acceleration a_x(t) of the fruit along the X axis at the t moment:

$\begin{array}{cc}{a}_{x\left(t\right)}=\frac{v_{x\left(t+1\right)}-v_{x\left(t\right)}}{\Delta t};& \left(16\right)\end{array}$

wherein v_{x (t+1)} is the instantaneous speed of the fruit along the X axis at t+1 moment, and v_x(t) is the instantaneous speed of the fruit along the X axis at t moment; similarly, the instantaneous acceleration a_{y (t)} of the fruit along the Y axis of the public reference base coordinate system at t moment and the instantaneous acceleration a_{z (t)} of the fruit along the Z axis of the public reference base coordinate system at t moment can be obtained; then the combined acceleration of the fruit at t moment is:a=√{square root over (a_x(t)²+a_y(t)²+a_z(t)²)}  (17);

(d) using the angle from the Z connected-axis at t moment to the Z connected-axis at t+1 moment to represent the swing angular displacement of the fruit in Δt:

$\begin{array}{cc}\Delta \theta =\arccos \left(\frac{x_{Z_{1_{t+1}}}{x}_{Z_{1_t}}+y_{Z_{1_{t+1}}}{y}_{Z_{1_t}}+z_{Z_{1_{t+1}}}{z}_{Z_{1_t}}}{\sqrt{{x_{Z_{1_{t+1}}}}^2+{y_{Z_{1_{t+1}}}}^2+{z_{Z_{1_{t+1}}}}^2}\times \sqrt{{x_{Z_{1_t}}}^2+{y_{Z_{1_t}}}^2+{z_{Z_{1_t}}}^2}}\right);& \left(18\right)\end{array}$

wherein

${\left(x_{Z_{1_t}}{y}_{Z_{1_t}}{z}_{Z_{1_t}}\right)}^T$ is the direction vector of Z connected-axis at t moment, that is t moment vector {right arrow over (O₁Z₁)};

${\left(x_{Z_{1_{t+1}}}{y}_{Z_{1_{t+1}}}{z}_{Z_{1_{t+1}}}\right)}^T$ is the direction vector of Z connected-axis at t+1 moment, that is t+1 moment vector {right arrow over (O₁Z₁)};

(e) using the average swing angular speed from t moment to t+1 moment to express the t swing angular speed ω_θt of the fruit at t moment:

$\begin{array}{cc}{\omega }_{{\theta }_t}=\frac{\mathrm{\Delta \theta }}{\Delta t};& \left(19\right)\end{array}$

(f) using the amount of change of the swing angular speed from t moment to t+1 moment to express the swing angular acceleration a_θt of the fruit at t moment:

$\begin{array}{cc}{a}_{{\theta }_t}=\frac{{\omega }_{{\theta }_{t+1}}-{\omega }_{{\theta }_t}}{\Delta t};& \left(20\right)\end{array}$

(g) using the axis rotation matrix to rotate the Z connected-axis at t moment and the Z connected-axis at t+1 moment to the position that coincides with the Z connected-axis in the public reference base coordinate system, using formula (6) to formula (10) in step (a) in step (6) to calculate the rotated vector {right arrow over (O₁X₁)} at t moment and the rotated vector {right arrow over (O₁X₁)} at t+1 moment, then the torsion angular degree from t moment to t+1 moment is:

$\begin{array}{cc}\mathrm{\Delta \varphi }=\arccos \left(\frac{{x_{X_{1_{t+1}}}}^{,}{x}_{X_t}+{y_{X_{1_{t+1}}}}^{,}{y}_{X_t}+{z_{X_{1_{t+1}}}}^{,}{z}_{X_t}}{\sqrt{{x_{X_{1_{t+1}}}}^{,2}+{y_{X_{1_{t+1}}}}^{,2}{z_{X_{1_{t+1}}}}^{,2}}\times \sqrt{{x_{X_t}}^2+{y_{X_t}}^2+{z_{X_t}}^2}}\right);& \left(21\right)\end{array}$

wherein

$\begin{array}{cc}{\left({x_{X_{1_t}}}^{\prime }{y_{X_{1_t}}}^{\prime }{z_{X_{1_t}}}^{\prime }\right)}^T& \end{array}$ is the rotated vector {right arrow over (O₁X₁)} at t moment,

${\left({x_{X_{1_{t+1}}}}^{\prime }{y_{X_{1_{t+1}}}}^{\prime }{z_{X_{1_{t+1}}}}^{\prime }\right)}^T$ is the rotated vector {right arrow over (O₁X₁)} at t+1 moment;

the torsion angular speed of the fruit at t moment is:

$\begin{array}{cc}{\omega }_{{\varphi }_t}=\frac{\mathrm{\Delta \varphi }}{\Delta t};& \left(22\right)\end{array}$ and

the torsion angular acceleration of the fruit at t moment is:

$\begin{array}{cc}{a}_{{\varphi }_t}=\frac{{\omega }_{{\varphi }_{t-1}}-{\omega }_{{\varphi }_t}}{\Delta t}.& (23\end{array}$

The beneficial effect of the present invention is: when the inertial force generated by the vibration of vibrating fruit picking exceeds the separation force between the fruit and the fruit stem, the fruit is separated from the fruit branch or the fruit stem at the weakest connection point to complete the picking. The effect of fruit separation ultimately depends on the maximum inertial force generated when the fruit vibrates. Inertial force comes from acceleration, so acceleration response is one of the most important characteristics in fruit tree dynamics. This method can construct the trajectory of the fruit in the vibration process through the relationship between the conjoined base at the junction of the fruit stem and the fruit, and then solve the displacement, speed and acceleration of fruit at each moment, which is beneficial to explore the law of fruit movement. And the present invention can also obtain the swing posture and the torsion posture in the process of fruit vibration. This method is aimed at the study of fruit movement status in the field of fruit vibration harvesting, and can better track the movement of fruits.

BRIEF DESCRIPTION OF THE DRAWINGS

Various other features and attendant advantages of the present invention will be more fully appreciated from the following detailed description when considered in connection with the accompanying drawings in which like reference characters designate like or corresponding parts throughout the several views, and wherein:

FIG. 1 shows a flowchart of the method;

FIG. 2 shows a relationship diagram of a coordinate system in a fruit space;

FIG. 3 shows a coordinate curve of a feature point x;

FIG. 4 shows a coordinate curve of a feature point y;

FIG. 5 shows a coordinate curve of a feature point z;

FIG. 6 shows a schematic diagram of a fruit space motion;

FIG. 7 shows a translation trajectory of a fruit spatial;

FIG. 8 shows a position diagram of a conjoined base coordinate of two adjacent moments; and

FIG. 9 shows a schematic diagram of torsion angular.

DETAILED DESCRIPTION OF THE ILLUSTRATED EMBODIMENT

The following further describes the specific implementation of the present invention based on FIGS. 1 to 9.

This embodiment provides a binocular-vision-based method for tracking fruit space attitude and fruit space motion. FIG. 1 is an application step diagram of this embodiment. The content of FIG. 1 will be expanded in detail below.

Mark three feature points C₃ on the surface of the fruit.

Establish a method for setting the fruit posture in space. The fruit is regarded as a rigid body, which does not deform in any state, and the fruit is regarded as a standard rotating body. establishing the connected base coordinate system by taking a junction of the fruit and the fruit stem as the origin, and taking a line between the junction of the fruit and the fruit stem and a center point of the fruit as a center line, the center line being the Z axis of the connected base coordinate system, which being marked as the Z connected-axis, taking a direction from the central point pointing to the joining point as the positive direction of the Z connected-axis, establishing the X-axis and Y-axis of the connected base coordinate system on a plane perpendicular to the Z-axis of the connected base coordinate system, and marking them as X connected-axis and Y connected-axis respectively; marking the origin of the connected base coordinate system as O₁, marking the end point of the unit vector in the positive direction of the X connected-axis as X₁, marking the end point of the unit vector in the positive direction of the Y connected-axis as Y₁, and marking the end point of the unit vector in the positive direction of the Z connected-axis as Z₁. Using this method, the movement of the fruit in space can be decomposed into translation, swinging around the junction of the fruit stem and the fruit, and twisting around the centerline. Using this method, the movement of the fruit in space can be decomposed into translation, swinging around the junction of the fruit stem and the fruit, and twisting around the centerline. Using this method, the movement of the fruit in space can be decomposed into translation, swinging around the junction of the fruit stem and the fruit, and torsion movement around the centerline.

Establish a public reference base coordinate system, the unit vector of the coordinate axis of the public reference base coordinate system is:

Z=(0 0 1)^T.

The absolute coordinates described in this article are all spatial coordinates in the public reference base coordinate system. The vectors described in this article are all vectors in the public reference base coordinate system.

Since the feature points on the fruit surface tracked by the binocular camera cannot directly reflect the spatial posture of the fruit, this embodiment invented a method for creating a fixed relationship between the conjoined base and the feature points on the fruit surface. First, using two high-speed cameras 1-1 (M310, VEO 410) to statically photograph the three feature points on the surface of the fruit and the points Z₁ of the connected base coordinate system established at the junction of the fruit and the fruit stem, using Phantom software to store the photographed images, and using TEMA software to process the feature points and the connected base coordinate system points Z₁ in the images, export the spatial absolute coordinate of the feature points C₃ and the points Z₁ of the connected base coordinate system and then establish an inherent relationship between the feature points and the connected base coordinate system. Referring to FIG. 2, this embodiment uses two high-speed cameras 1-1 to photograph the fruit 2-1. Two high-speed cameras 1-1 can obtain depth information by binocular visual tracking, and obtain the spatial coordinates of the fruit 2-1. Reference numeral 1-1 in FIG. 2 represents a high-speed camera, reference numeral 2-1 represents a fruit, X, Y, and Z in FIG. 2 represent a public reference base coordinate system, and X connected-axis, Y connected-axis, Z connected-axis represent a connected base coordinate system, X_cxyz, Y_cxyz, Z_cxyz represent coordinate system C_xyz. Specifically, it includes the following steps:

(a) under static conditions, using two high-speed cameras (M310, VEO 410) to statically photograph the three feature points C₃ on the surface of the fruit and the points Z₁ of the connected base coordinate system established at the junction of the fruit and the fruit stem, using Phantom software to store the photographed images, and using TEMA software to process the feature points C₃ and the connected base coordinate system points Z₁ in the images, acquiring the absolute coordinate of the feature points C₃ and the absolute coordinate of the connected base coordinate system points Z₁; the absolute coordinate of feature point C₂ is ( z_C2)^T, the absolute coordinate of feature point C₁ is ( z_C1)^T, the

absolute coordinate of feature point C₃ is ( z_C3)^T;

(b) creating vectors {right arrow over (C₁C₂)} and {right arrow over (C₁C₃)} via absolute coordinate, unitizing {right arrow over (C₁C₂)} and {right arrow over (C₁C₃)}, {right arrow over (C₁C₂)} and {right arrow over (C₁C₃)} are:

{right arrow over (C₁C₂)}=(X_C2− y_C2− z_C2−Z_C1)^T, {right arrow over (C₁C₃)}=(x_C3− y_C3− z_C3−z_C1)^T; establish vector {right arrow over (C₁O₁)}, {right arrow over (C₁O₁)}=(x_O1− y_O1− z_O1−z_C1)^T=(x_O1′ y_O1′ z_O1′)^T;

is the coordinate of the point O₁ of the connected base coordinate system in the public reference base coordinate system; and use the same method to create vector vector {right arrow over (C₁Y₁)} and vector {right arrow over (C₁Z₁)};

(c) doing a vector product of unitized {right arrow over (C₁C₂)} and {right arrow over (C₁C₃)} to get: {right arrow over (Y)}={right arrow over (C₁C₂)}× {right arrow over (C₁C₃)}, doing a vector product of {right arrow over (X)}={right arrow over (C₁C₂)} and {right arrow over (Y)} to get: {right arrow over (Z)}={right arrow over (X)}×{right arrow over (Y)}, thus, establishing a coordinate system C_xyz by taking feature point C₁ as an origin, the coordinate axis vector of the coordinate system C_xyz is: X_Cxyz===( z_Cz)^T, calculating angles γ_x between the vector {right arrow over (X_Cxyz)} and the X axis, Y axis and Z axis of the public reference base coordinate system through the calculation formula of the space vector angle, calculating angles γ_y between the vector Y_Cxyz and the X axis, Y axis and Z axis of the public reference base coordinate system, calculating angles γ_z between the vector {right arrow over (Z_Cxyz )} and the X axis, Y axis and Z axis of the public reference base coordinate system; wherein the calculation formula for the angle of the space vector is:

$\cos \phi =\frac{m_1{m}_2+n_1{n}_2+p_1{p}_2}{\sqrt{m_1^2+n_1^2+p_1^2}\times \sqrt{m_2^2+n_2^2+p_2^2}};$

wherein p₂ respectively correspond to the X coordinate, Y coordinate, and Z coordinate of the space vector;

the coordinate conversion matrix is:

$\begin{array}{cc}A=\left(\begin{array}{ccc}\cos {\alpha }_x& \cos {\beta }_x& \cos {\gamma }_x\\ \cos {\alpha }_y& \cos {\beta }_y& \cos {\gamma }_z\\ \cos {\alpha }_z& \cos {\beta }_z& \cos {\gamma }_z\end{array}\right);& \left(1\right)\end{array}$

the coordinates of point O₁ under the coordinate system C_xyz are obtained by formula (2):

$\begin{array}{cc}{O}_{1_{C_{\mathrm{xyz}}}}=A^{*}\overrightarrow{C_1{O}_1}=\left(\begin{array}{ccc}\cos {\alpha }_x& \cos {\beta }_x& \cos {\gamma }_x\\ \cos {\alpha }_y& \cos {\beta }_y& \cos {\gamma }_z\\ \cos {\alpha }_z& \cos {\beta }_z& \cos {\gamma }_z\end{array}\right)\left(\begin{array}{c}{x_{o_I}}^{{}^{\prime }}\\ {y_{o_1}}^{{}^{\prime }}\\ {z_{o_1}}^{{}^{\prime }}\end{array}\right)=\left(\begin{array}{c}{x}_{O_{1C_{\mathrm{xy}}}}\\ y_{O_{{\mathrm{IC}}_{\mathrm{xyz}}}}\\ z_{O_{1C_{\mathrm{xyz}}}}\end{array}\right);& \left(2\right)\end{array}$

wherein, {right arrow over (C₁O₁)}=(x_O1′ y_O1′ z_O1′)^T, and obtained from step (b);

similarly,

$X_{1_{C_{\mathrm{xyz}}}}=A^{*}{\overrightarrow{C_{1}X}}_1,Y_{1_{C_{\mathrm{xyz}}}}=A^{*}\overrightarrow{C_1{Y}_1}\mathrm{and}{Z}_{1_{C_{\mathrm{xyz}}}}=A^{*}\overrightarrow{C_1{Z}_1},$ wherein the coordinates corresponding to

$X_{1_{C_{\mathrm{xyz}}}},Y_{1_{C_{\mathrm{xyz}}}},Z_{1_{C_{\mathrm{xyz}}}}$ are the coordinates of points X₁, Z₁ under the coordinate system C_xyz; the vector {right arrow over (C₁X₁)}, the vector {right arrow over (C₁Y₁)}, and the vector {right arrow over (C₁Z₁)} are all obtained from the above-mentioned step (b); the coordinates corresponding to

$O_{1_{C_{\mathrm{xyz}}}}{X}_{1_{C_{\mathrm{xyz}}}}{Y}_{1_{C_{\mathrm{xyz}}}}{Z}_{1_{C_{\mathrm{xyz}}}}$ are use coordinates of points Z₁ under the feature point coordinate system C_xyz, that is, it represents the inherent relationship between the feature point and the connected base coordinate system. The inherent relationship in this embodiment is:

$S_e=\left(O_{1_{C_{\mathrm{xyz}}}}{X}_{1_{C_{\mathrm{xyz}}}}{Y}_{1_{C_{\mathrm{xyz}}}}{Z}_{1_{C_{\mathrm{xyz}}}}\right)=\left(\begin{array}{cccc}8.3580& 7.3782& 8.3534& 8.2740\\ -12.4324& -12.3469& -12.4959& -13.4267\\ -2.3046& -2.3054& -3.2860& -2.2420\end{array}\right)\circ$

The fruit space motion process is divided into translation, swing and torsion, and the motion process is shown in FIG. 6. The binocular-vision-based method of tracking fruit space attitude first uses two high-speed cameras 1-1 (M310, VEO 410) to photograph the fruit dynamic motion, and the movement of the fruit under a certain frequency of excitation. Use Phantom software to store each frame of image in the photographed video, and use TEMA software to process the feature points in each frame of image, export the absolute coordinate of the feature point on the fruit surface of each frame of image in the video, as shown in FIGS. 3 to 5, FIG. 3 is the x-coordinate diagram of the feature point C₁, FIG. 4 is the y-coordinate diagram of the feature point C₁, and FIG. 5 is the z-coordinate diagram of the feature point C₁, which represents the feature point motion curve in the video. The abscissas in FIGS. 3 to 5 represent each moment corresponding to each frame of image. According to the inherent relationship between the feature points and the connected base coordinate system, the absolute coordinate of each frame of image corresponding to the connected base coordinate system points Z₁ at each moment being calculated by the inverse rotation transformation, and calculating an angle between the vector {right arrow over (O₁Z₁)} at each moment and the Z axis in the public reference base coordinate system through the space vector angle formula, and the angle representing the swing posture of the fruit. Rotate the {right arrow over (O₁Z₁)} at each moment to a position that coincides with the Z axis in the public reference base coordinate system, and calculating an angle between the rotated {right arrow over (O₁X₁)} and the X axis in the public reference base coordinate system, the angle representing the twisting posture of the fruit, details as follows:

(a1) Use two high-speed cameras 1-1 to photograph fruit dynamic motion, use Phantom software to store each frame of image of the photographed video, use TEMA software to process the feature point in the image, and export the absolute coordinate of the feature point on the fruit surface frame by frame;

(b1) Follow the above-mentioned steps (b) and (c) to establish a coordinate system C_xyz by taking the feature point C₁ in one frame of image corresponding to t moment as an origin;

$\overrightarrow{X_{C_{{\mathrm{xyz}}_t}}}\overrightarrow{Y_{C_{{\mathrm{xyz}}_t}}},\overrightarrow{Z_{C_{{\mathrm{xyz}}_t}}}$ are the coordinate axis vector of the coordinate system C_xyz established by taking the feature point C₁ in one frame of image corresponding to t moment as an origin;

calculating angles γ_xt between the vector

$\overrightarrow{X_{C_{{\mathrm{xyz}}_t}}}$ and the X axis, Y axis, and Z axis of the public reference base coordinate system at t moment through the calculation formula of the space vector angle, calculating angles γ_yt between the vector

$\overrightarrow{Y_{C_{{\mathrm{xyz}}_t}}}$ and the X axis, Y axis and Z axis of the public reference base coordinate system, calculating angles γ_zt between the vector

$\overrightarrow{Z_{C_{{\mathrm{xyz}}_t}}}$ and the X axis, Y axis and Z axis of the public reference base coordinate system, then the coordinate conversion matrix at t moment is:

$\begin{array}{cc}{A}_t=\left(\begin{array}{ccc}\cos {\alpha }_{\mathrm{xt}}& \cos {\beta }_{\mathrm{xt}}& \cos {\gamma }_{\mathrm{xt}}\\ \cos {\alpha }_{\mathrm{yt}}& \cos {\beta }_{\mathrm{yt}}& \cos {\gamma }_{\mathrm{yt}}\\ \cos {\alpha }_{\mathrm{zt}}& \cos {\beta }_{\mathrm{zt}}& \cos {\gamma }_{\mathrm{zt}}\end{array}\right);& \left(3\right)\end{array}$

the coordinates of point O₁ in the public reference base coordinate system, that is, the absolute coordinate is:

$\begin{array}{cc}\left(\begin{array}{c}{x}_{O_1}\\ y_{O_1}\\ z_{O_1}\end{array}\right)=\left(\begin{array}{c}{x}_{C_1}\\ y_{C_1}\\ z_{C_1}\end{array}\right)+A_t^{-1*}\left(\begin{array}{c}{x}_{O_{1C_{\mathrm{xyz}}}}\\ y_{O_{1C_{\mathrm{xyz}}}}\\ z_{O_{1C_{\mathrm{xyz}}}}\end{array}\right);& \left(4\right)\end{array}$

wherein ( z_O1) are the absolute coordinate of the point O₁ under t moment, ( Z_O1) are the absolute coordinate of the point C₁ under t moment,

$\left(x_{O_{1C_{\mathrm{xyz}}}}{y}_{O_{1C_{\mathrm{xyz}}}}{z}_{O_{1C_{\mathrm{xyz}}}}\right)$ are the coordinate of point O₁ in the coordinate system C_xyz; similarly, calculating the absolute coordinate of the points Z₁ under t moment;

wherein, the vector corresponding to the absolute coordinate of point X₁ at t moment is equal to the vector corresponding to the absolute coordinate of point C₁ at t moment plus the vector of A_t⁻¹ multiplied by the coordinates of point X₁ under the coordinate system C_xyz; the absolute coordinate calculation method of point Y₁ and point Z₁ can be deduced by analogy;

each frame of image is independent of each other, and corresponds to a moment respectively, and solves the absolute coordinates of the points Z₁ of the connected base coordinate system at each moment frame by frame; calculate the angle θ between the vector {right arrow over (O₁Z₁)} at each moment and the Z axis in the public reference base coordinate system through the space vector angle formula:

$\begin{array}{cc}\theta =\arccos \left(\frac{x_{Z_1}{x}_Z+y_{Z_1}{y}_Z+z_{Z_1}{z}_Z}{\sqrt{x_{Z_1}^2+y_{Z_1}^2+z_{Z_1}^2}\times \sqrt{x_Z^2+y_Z^2+z_Z^2}}\right);& \left(5\right)\end{array}$

wherein {right arrow over (O₁Z₁)}=(x_z1 y_z1 z_z1)^T, is the unit vector of the Z axis in the public reference base coordinate system, the angle θ between the vector {right arrow over (O₁Z₁)} and the Z axis in the public reference, base coordinate system represents a swing posture of the fruit.

(c1) using the axis rotation matrix to rotate {right arrow over (O₁Z₁)} (that is, Z connected-axis) at the moment corresponding to a certain frame of image to a position that coincides with the Z axis in the public reference base coordinate system, calculating the angle between the rotated {right arrow over (O₁X₁)} and the X axis in the public reference base coordinate system, the angle represents a twisting posture of the fruit at the moment corresponding to the frame image, detail as follows:

the Z connected-axis direction vector of the frame of image is: ={right arrow over (O₁Z₁)}=(x_z1 y_z1 z_z1)^T, |{right arrow over (O₁Z₁)}|=√{square root over (x_Z1²+y_Z1²+z_Z1²)}, calculating the angle φ between the Z connected-axis and the Z axis in the public reference base coordinate system:

$\begin{array}{cc}\phi =\arccos \left(\frac{Z_{Z_1}}{\overrightarrow{O_1{Z}_1}}\right);& \left(6\right)\end{array}$

calculating the angle ψ between the projection of {right arrow over (O₁Z₁)} (that is, Z connected-axis) in the X axis and Y axis of the public reference base coordinate system and the Y axis of the public reference base coordinate system:

$\begin{array}{cc}\psi =\arccos \left(\frac{y_{Z_1}}{\sqrt{y_{Z_1}^2+x_{Z_1}^2}}\right);& \left(7\right)\end{array}$

calculating the axis rotation matrix:

$\begin{array}{cc}{T}_1=\left(\begin{array}{ccc}\cos \phi & 0& \sin \phi \\ 0& 1& 0\\ -\sin \phi & 0& \cos \phi \end{array}\right);& \left(8\right)\end{array}$

$\begin{array}{cc}{T}_2=\left(\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right);& \left(9\right)\end{array}$

the absolute coordinate of the rotated point X₁ is consistent with the coordinate of the rotated vector {right arrow over (O₁X₁)}, and the rotated vector {right arrow over (O₁X₁)} is:

$\begin{array}{cc}\left(\begin{array}{c}{x}_{X_1}^{\prime }\\ y_{X_1}^{\prime }\\ z_{X_1}^{\prime }\end{array}\right)=T_2^{-1*}{T}_1^{-1*}\left(\begin{array}{c}{x}_{X_1}\\ y_{X_1}\\ z_{X_1}\end{array}\right);& \left(10\right)\end{array}$

wherein, is the vector {right arrow over (O₁X₁)} before the {right arrow over (O₁Z₁)} of the frame of image is rotated;

the torsion angular degree is:

$\begin{array}{cc}\varphi =\arccos \left(\frac{x_{X_1^{\prime }}{x}_X+y_{X_1^{\prime }}{y}_X+z_{X_1^{\prime }}{z}_X}{\sqrt{x_{X_1^{\prime 2}}+y_{X_1^{\prime 2}}+z_{X_1^{\mathrm{\prime 2}}}}\times \sqrt{x_{X^2}+y_{X^2}+z_{X^2}}}\right);& \left(11\right)\end{array}$

the torsion angular degree is the angle between the rotated vector {right arrow over (O₁X₁)} and the X axis in the public reference base coordinate system, which represents the torsion posture of the fruit at the moment corresponding to the frame image, wherein is the unit vector of the X axis in the public reference base coordinate system.

(d1) Similarly, in step (c1), calculating the twisting posture of the fruit at each moment corresponding to each frame image.

A study method of fruit space motion, the space motion of fruit needs to use public reference base coordinates to express its absolute motion. Setting the moments corresponding to two adjacent frames of images to t+1 moment and t moment, based on the point O₁ of the connected base coordinate system in adjacent two frames of images, and calculating the displacement of point O₁ along the X axis, Y axis and Z axis of the public reference base coordinate system at t+1 moment and t moment respectively. calculating an instantaneous speed along the X axis, an instantaneous speed along the Y axis, and an instantaneous speed along the Z axis of the fruit at t moment according to the displacement, calculating an instantaneous acceleration along the X axis, an instantaneous acceleration along the Y axis and an instantaneous acceleration along the Z axis of the fruit at t moment according to the instantaneous speed, calculating a swing angular displacement of the fruit in the time interval between two adjacent frames of images, sequentially calculating a swing angular speed of the fruit at t moment and a swing angular acceleration of the fruit at t moment via the swing angular displacement, and calculating the torsion angular speed of the fruit at t moment and the torsion angular acceleration of the fruit at t moment according to the torsion angular degree from t moment to t+1 moment. The fruit space motion process is divided into translation, swing and torsion, and the motion process is shown in FIG. 6. The movement of the point O₁ along the X Y Z axis represents the translation of the fruit, the swing around the junction of the fruit stem and the fruit represents the swing of the fruit, and the twisting movement around the centerline (that is, Z connected-axis) represents the twist of the fruit. FIG. 7 shows the spatial translation trajectory of fruit, and the curve in FIG. 7 shows the movement of the point O₁ in the space coordinates of the public reference base coordinate system.

Wherein, the instantaneous speed moving along the X Y Z axis, the instantaneous acceleration moving along the X Y Z axis, the swing angular displacement, the swing angular speed, and the swing angular acceleration within the frame time interval, the torsion angular degree, the torsion angular speed, and the torsion angular acceleration within the frame time interval all indicate the tracked fruit space motion status. The specific calculation method is as follows:

(a2) setting moments corresponding to two adjacent frames of images to t+1 moment and t moment, based on the point O₁ of the connected base coordinate system in adjacent two frames of images, and calculating the displacement of the point O₁ along the X axis of the public reference base coordinate system at t+1 moment and t moment respectively:S_x=x_Ot+1−x_O1  (12);

wherein x_O1 are the coordinates x of the point O₁ under the public reference base coordinate system at t+1 moment and t moment; similarly, calculating the displacement S_y along the Y axis and the displacement S_z along the Z axis of the point O₁ of the public reference base coordinate system at t+1 moment and t moment, and the combined displacement is:S=√{square root over (S_x²+S_y²+S_z²)}  (13);

(b2) using the average speed of the point O₁ moving along the X axis of the public reference base coordinate system from t moment to t+1 moment to express the instantaneous speed v_x(t) of the fruit along the X axis at t moment:

$\begin{array}{cc}{v}_{x\left(t\right)}=\frac{S_x}{\Delta t};& \left(14\right)\end{array}$

wherein Δt is the interval time between two frames of images; similarly, the instantaneous speed v_{y (t)} of the fruit along the Y axis of the public reference base coordinate system at t moment and the instantaneous speed v_{z (t)} of the fruit along the Z axis at t moment can be obtained, then the combined speed of the fruit at t moment is:v=√{square root over (v_x(t)²+v_y(t)²+v_z(t)²)}  (15);

(c2) using the acceleration of the point O₁ moving along the X axis of the public reference base coordinate system from t moment to t+1 moment to express the instantaneous acceleration a_x(t) of the fruit along the X axis at the t moment:

$\begin{array}{cc}{a}_{x\left(t\right)}=\frac{v_{x\left(t+1\right)}-v_{x\left(t\right)}}{\Delta t};& \left(\mathrm{l6}\right)\end{array}$

wherein v_{x (t+1)} is the instantaneous speed of the fruit along the X axis at t+1 moment, and v_x(t) is the instantaneous speed of the fruit along the X axis at t moment; similarly, the instantaneous acceleration a_{y (t)} of the fruit along the Y axis of the public reference base coordinate system at t moment and the instantaneous acceleration a_{z (t)} of the fruit along the Z axis of the public reference base coordinate system at t moment can be obtained; then the combined acceleration of the fruit at t moment is:a=√{square root over (a_x(t)²+a_y(t)²+a_z(t)²)}  (17);

(d2) since there are too many solving moments, this embodiment lists the positions of the connected base coordinate system of adjacent t+1 moment and t moment corresponding to two adjacent frames of images to indicate the swing and twist of the fruit. For convenience of representation, the origins of the conjoined base coordinates of two adjacent moments are overlapped, as shown in FIG. 8, the angle Δθ in FIG. 8 is the angle from t moment Z connected-axis to t+1 moment Z connected-axis, and Δθ represents the swing angular displacement of the fruit in Δt:

$\begin{array}{cc}\Delta \theta =\arccos \left(\frac{x_{Z_{1_{t+1}}}{x}_{Z_{1_t}}+y_{Z_{1_{t+1}}}{y}_{Z_{1_t}}+z_{Z_{1_{t+1}}}{z}_{Z_{1_t}}}{\sqrt{x_{Z_{1_{t+1}}}^2+y_{Z_{z_{t+1}}}^2+z_{Z_{1_{t+1}}}^2}\times \sqrt{x_{Z_{1_t}}^2+y_{Z_{1_t}}^2+z_{Z_{1_t}}^2}}\right);& \left(18\right)\end{array}$

wherein

${\left(x_{Z_{1_t}}{y}_{Z_{1_t}}{z}_{Z_{1_t}}\right)}^T$ is the direction vector of Z connected-axis at t moment, that is t moment vector {right arrow over (O₁Z₁)};

${\left(x_{Z_{1_{t+1}}}{y}_{Z_{1_{t+1}}}{z}_{Z_{1_{t+1}}}\right)}^T$ is the direction vector of Z connected-axis at t+1 moment, that is t+1 moment vector {right arrow over (O₁Z₁)};

(e2) using the average swing angular speed from t moment to t+1 moment to express the t swing angular speed ω_θt of the fruit at t moment:

$\begin{array}{cc}{\omega }_{{\theta }_t}=\frac{\Delta \theta }{\Delta t};& \left(19\right)\end{array}$

(f2) using the amount of change of the swing angular speed from t moment to t+1 moment to express the swing angular acceleration a_θt of the fruit at t moment:

$\begin{array}{cc}{a}_{{\theta }_t}=\frac{{\omega }_{{\theta }_{t+1}}-{\omega }_{{\theta }_t}}{\Delta t};& \left(20\right)\end{array}$

(g2) FIG. 8 is a schematic diagram of the overlapping conjoined base coordinate origins of two adjacent moments. The angle Δθ between the Z connected-axis of t moment and t+1 moment in FIG. 8 represents the swing angular displacement of the fruit in Δt. However, the angle between the X connected-axis of t moment and t+1 moment, and the angle between the Y connected-axis of t moment and t+1 moment in FIG. 8 are not the torsion angular. Therefore, it is necessary to rotate the Z connected-axis of t moment (also expressed as {right arrow over (O₁Z₁)}) and the Z connected-axis of t+1 moment to the position that coincides with the Z connected-axis in the public reference base coordinate system through the axis rotation matrix. As shown in FIG. 9, the two Z connected-axes in this FIG. 9 coincide with the Z axis in the public reference base coordinate system. The angle between the two X connected-axis at this time is the torsion angular degree from t moment to t+1 moment, and the calculation method is: use the formula (6) to formula (10) in the above-mentioned step (c1) respectively calculate the rotated vector {right arrow over (O₁X₁)} at t moment and the rotated vector {right arrow over (O₁X₁)} at t+1 moment.

similarly, in formula (10) of step (c1), the vector {right arrow over (O₁X₁)} rotated at t moment is:

$\begin{array}{cc}\left(\begin{array}{c}{x}_{X_{1_t}}^{\prime }\\ y_{X_{1_t}}^{\prime }\\ z_{X_{1_t}}^{\prime }\end{array}\right)=T_{2_t}^{-1}*T_{1_t}^{-1}*\left(\begin{array}{c}{x}_{X_{1_t}}\\ y_{X_{1_t}}\\ z_{X_{1_t}}\end{array}\right);& \left(21\right)\end{array}$

wherein

${\left(x_{X_{1_t}}{y}_{X_{1_t}}{z}_{X_{1_t}}\right)}^T$ is the vector {right arrow over (O₁X₁)} before the Z connected-axis rotation at t moment;

${\left(x_{X_{1_t}}^{\prime }{y}_{X_{1_t}}^{\prime }{z}_{X_{1_t}}^{\prime }\right)}^T$ is the vector {right arrow over (O₁X₁)} after the Z connected-axis rotation at t moment; T_2t⁻¹ and T_1t⁻¹ are the axis rotation matrices under t moment;

the torsion angular speed degree of the fruit from t moment to t+1 moment is:

$\begin{array}{cc}\Delta \varphi =\arccos \left(\frac{x_{X_{1_{t+1}}^{\prime }}{x}_{X_t}+y_{X_{1_{t+1}}^{\prime }}{y}_{X_t}+z_{X_{1_{t+1}}^{\prime }}{z}_{X_t}}{\sqrt{x_{X_{1_{t+1}}^{\prime 2}}+y_{X_{1_{t+1}}^{\prime 2}}+z_{X_{1_{t+1}}^{\prime 2}}}\times \sqrt{x_{X_t^2}+y_{X_t^2}+z_{X_t^2}}}\right);& \left(22\right)\end{array}$

wherein

${\left(x_{X_{1_t}}^{\prime }{y}_{X_{1_t}}^{\prime }{z}_{X_{1_t}}^{\prime }\right)}^T$ is the rotated vector {right arrow over (O₁X₁)} at t moment,

${\left(x_{X_{1_{t+1}}}^{\prime }{y}_{X_{1_{t+1}}}^{\prime }{z}_{X_{1_{t+1}}}^{\prime }\right)}^T$ is the rotated vector {right arrow over (O₁X₁)} at t+1 moment;

the torsion angular speed of the fruit at t moment is:

$\begin{array}{cc}{\omega }_{{\varphi }_t}=\frac{\Delta \varphi }{\Delta t};& \left(23\right)\end{array}$

the torsion angular acceleration of the fruit at t moment is:

$\begin{array}{cc}{a}_{{\varphi }_t}=\frac{{\omega }_{{\varphi }_{t+1}}-{\omega }_{{\varphi }_t}}{\Delta t}.& \left(24\right)\end{array}$

When the inertial force generated by the vibration of vibrating fruit picking exceeds the separation force between the fruit and the fruit stem, the fruit is separated from the fruit branch or the fruit stem at the weakest connection point to complete the picking. The effect of fruit separation ultimately depends on the maximum inertial force generated when the fruit vibrates. Inertial force comes from acceleration, so acceleration response is one of the most important characteristics in fruit tree dynamics. This method can construct the trajectory of the fruit in the vibration process through the relationship between the conjoined base at the junction of the fruit stem and the fruit, and then solve the displacement, speed and acceleration of fruit at each moment, which is beneficial to explore the law of fruit movement. The binocular-vision-based method for tracking fruit space attitude and fruit space motion, the system and analysis software for taking fruit vibration photography belong to the prior art, such as binocular cameras and spatial position synthesis software.

The scope of protection of the present invention includes, but is not limited to, the above-mentioned embodiments. The scope of protection of the present invention is subject to the claims. Any substitutions, modifications, and improvements that can be easily conceived by those skilled in the art made to the present invention fall within the scope of protection of the present invention.

Claims

1. A computer implemented method for tracking fruit space attitude and fruit space motion, wherein the computer is configured to carry out the steps comprising: (1) marking three feature points C1, C2, C3 on a surface of a fruit;(2) establishing a connected base coordinate system by taking a junction of the fruit and a fruit stem as an origin, a X axis, a Y axis, and a Z axis of the connected base coordinate system are marked as a X connected-axis, a Y connected-axis and a Z connected-axis, respectively, and the origin of the connected base coordinate system is marked as O1, an end point of the unit vector in a positive direction of the X connected-axis is marked as X1, an end point of the unit vector in a positive direction of the Y connected-axis is marked as Y1, an end point of the unit vector in a positive direction of the Z connected-axis is marked as Z1;(3) establishing a public reference base coordinate system;(4) statically photographing the three feature points on the surface of the fruit and connected base coordinate system points c established at the junction of the fruit and the fruit stem, using software to store the photographed images, and using software to process the feature points and connected base coordinate system points O1, X1, Y1, Z1 in the images, acquiring a absolute coordinate of the feature points C1, C2, C3 and the connected base coordinate system points O1, X1, Y1, Z1, and then acquiring an inherit relationship between the feature points and the connected base coordinate system;(5) photographing the fruit dynamic motion, using software to store each frame of the image in the photographed video, using software to process the feature points in the images to acquiring the absolute coordinate of the feature points on the fruit surface, according to an inherent relationship between the feature points and the connected base coordinate system, the absolute coordinate of each frame of image corresponding to the connected base coordinate system points O1, X1, Y1, Z1 at each moment being calculated by inverse rotation transformation, and calculating an angle between the vector {right arrow over (O1Z1)} at each moment and the Z axis in the public reference base coordinate system through the space vector angle formula, and the angle representing a swing posture of the fruit;(6) rotating the {right arrow over (O1Z1)} at each moment to a position that coincides with the Z axis in the public reference base coordinate system, and calculating an angle between the rotated {right arrow over (O1Z1)} and the X axis in the public reference base coordinate system, the angle representing a twisting posture of the fruit; and(7) setting moments corresponding to two adjacent frames to t+1 moment and t moment, and calculating a displacement of the point O1 along the X axis, Y axis and Z axis of the public reference base coordinate system at t+1 moment and t moment respectively, calculating an instantaneous speed along the X axis, an instantaneous speed along the Y axis, and an instantaneous speed along the Z axis of the fruit at t moment according to the displacement, calculating an instantaneous acceleration along the X axis, an instantaneous acceleration along the Y axis and an instantaneous acceleration along the Z axis of the fruit at t moment according to the instantaneous speed, calculating a swing angular displacement of the fruit in a time interval between the two adjacent frames of images, sequentially calculating a swing angular speed of the fruit at t moment and a swing angular acceleration of the fruit at t moment via the swing angular displacement, and calculating a torsion angular speed of the fruit at t moment and a torsion angular acceleration of the fruit at t moment according to the torsion angular degree from t moment to t+1 moment.

2. The method according to claim 1, wherein the step (2) includes:establishing the connected base coordinate system by taking a junction of the fruit and the fruit stem as the origin, and taking a line between the junction of the fruit and the fruit stem and a center point of the fruit as a center line, the center line being the Z axis of the connected base coordinate system, which being marked as the Z connected-axis, taking a direction from the central point pointing to the joining point as the positive direction of the Z connected-axis, establishing the X axis and Y axis of the connected base coordinate system on a plane perpendicular to the Z axis of the connected base coordinate system, and marking them as X connected-axis and Y connected-axis respectively; marking the origin of the connected base coordinate system as O1, marking the end point of the unit vector in the positive direction of the X connected-axis as X1, marking the end point of the unit vector in the positive direction of the Y1 connected-axis as Y1, and marking the end point of the unit vector in the positive direction of the Z connected-axis as Z1.

3. The method according to claim 2, wherein the absolute coordinates are spatial coordinates in the public reference base coordinate system, and the unit vectors of the coordinate axis of the public reference base coordinate system are: X=(1 0 0)T, Y=(0 1 0)T, Z=(0 0 1)T.

4. The method according to claim 3, wherein the step (4) includes:(a) under static conditions, using two high-speed cameras to statically photograph the three feature points on the surface of the fruit and the points O1, X1, Y1, Z1 of the connected base coordinate system established at the junction of the fruit and the fruit stem, using software to store the photographed images, and using software to process the feature points C1, C2, C3 and the connected base coordinate system points O1, X1, Y1, Z1 in the images, acquiring the absolute coordinate of the feature points C1, C2, C3 and the absolute coordinate of the connected base coordinate system points O1, X1, Y1, Z1;(b) creating vectors {right arrow over (C1C2)} and {right arrow over (C1C3)} via absolute coordinate, unitizing {right arrow over (C1C2)} and {right arrow over (C1C3)} creating vector {right arrow over (C1O1)}, vector {right arrow over (C1X1)}, vector {right arrow over (C1Y1)} and vector {right arrow over (C1Z1)};(c) doing a vector product of unitized {right arrow over (C1C2)} and {right arrow over (C1C3)} to get: {right arrow over (Y)}={right arrow over (C1C2)}×{right arrow over (C1C3)}, doing a vector product of {right arrow over (X)}={right arrow over (C1C2)} and {right arrow over (Y)} to get: {right arrow over (Z)}={right arrow over (X)}×{right arrow over (Y)}, thus, establishing a coordinate system Cxyz by taking feature point C1 as an origin, the coordinate axis vector of the coordinate system Cxyz is: {right arrow over (XCxyz)}=(xCx, yCx, zCx)T, {right arrow over (YCxyz)}=(xCy, yCy, zCy)T, {right arrow over (ZCxyz)}=ZCz, yCz, zCz)T,calculating angles αx, βx, γx between the vector {right arrow over (XCxyz)} and the X axis, Y axis and Z axis of the public reference base coordinate system through the calculation formula of the space vector angle,calculating angles αy, βy, γy between the vector

5. The method according to claim 4, wherein the step (5) includes:(a) using two high-speed cameras to photograph the fruit dynamic motion, using software to store each frame of image of the photographed video, using software to process the feature points in the images, and obtaining the absolute coordinate of the feature points on the surface of the fruit;(b) calculating angles αx1, βx1, γx1 between the vector

6. The method according to claim 5, wherein the step (6) includes:(a) using the axis rotation matrix to rotate {right arrow over (O1Z1)} at the moment corresponding to a certain frame of image to a position that coincides with the Z axis in the public reference base coordinate system, calculating the angle between the rotated {right arrow over (O1X1)} and the X axis in the public reference base coordinate system, the angle represents an twisting posture of the fruit at the moment corresponding to the frame image, detail as follows:the Z connected-axis direction vector of the frame of image is:

7. The method according to claim 6, wherein the step (7) includes:(a) setting moments corresponding to two adjacent frames of images to t+1 moment and t moment, based on the point O1 of the connected base coordinate system in adjacent two frames of images, and calculating the displacement of the point O1 along the X axis of the public reference base coordinate system at t+1 moment and t moment respectively: Sx=xOt+1−xO1  (12);wherein xOt+1, xO1 are the coordinates x of the point O1 under the public reference base coordinate system at t+1 moment and t moment; similarly, calculating the displacement Sy along the Y axis and the displacement Sz along the Z axis of the point O1 of the public reference base coordinate system at t+1 moment and t moment, and the combined displacement is: S=√{square root over (Sx2+Sy2+Sz2)}  (13);(b) using the average speed of the point O1 moving along the X axis of the public reference base coordinate system from t moment to t+1 moment to express the instantaneous speed vx(t) of the fruit along the X axis at t moment: