基于双目视觉追踪果实空间姿态及果实空间运动的方法与流程

本发明属于农林业果树采收领域，具体涉及一种基于双目视觉追踪果实空间姿态及果实空间运动的方法，用于林果振动采收领域中对果实运动状态的研究。

背景技术：

林果采收作业是林果生产中最耗时、最费力的一个环节，对于红枣、核桃、银杏等干果类林果，目前最有效的采收方式是机械振动收获，其振动采收机械的采收效果与多种因素有关，包括果树的生长特性和机械振动的工作参数。对于林果振动采收的研究更多的是将树木本身看作二阶系统，检测激励与响应的关系测得树木整体的刚度与阻尼比，近年来国内进行了更加细致的研究并建立了树干和树枝振动的简化力学模型，通过实验和理论计算比较认为悬臂直梁模型更为准确，为振动采收机械的研究提供了理论基础。但对于果实在振动采收过程中的运动情况的研究还存在空缺，为了更加细致深入的解释振动落果的原理，需要对果实的运动进行研究。

技术实现要素：

本发明所要解决的技术问题是针对上述现有技术的不足提供一种基于双目视觉追踪果实空间姿态及果实空间运动的方法，本基于双目视觉追踪果实空间姿态及果实空间运动的方法针对林果振动采收领域中对果实运动状态的研究，能够较好地追踪果实的运动。

为实现上述技术目的，本发明采取的技术方案为：

一种基于双目视觉追踪果实空间姿态及果实空间运动的方法，包括：

(1)在果实表面标记三个特征点：c1、c2、c3；

(2)以果实和果柄的结合处为原点建立连体基坐标系，连体基坐标系的x轴、y轴和z轴分别记为x连轴、y连轴和z连轴，将连体基坐标系的原点标记为o1，x连轴正方向上单位向量的终点标记为x1，y连轴正方向上单位向量的终点标记为y1，z连轴正方向上单位向量的终点标记为z1；

(3)建立公共参考基坐标系；

(4)对果实表面的三个特征点以及果实和果柄的结合处建立的连体基坐标系的点o1、x1、y1、z1进行静态拍摄，通过phantom软件对拍摄的图像进行存储，采用tema软件对图像中的特征点以及连体基坐标系的点o1、x1、y1、z1进行处理，获取特征点c1、c2、c3和连体基坐标系的点o1、x1、y1、z1的绝对坐标，进而获取特征点与连体基坐标系的固有关系；

(5)拍摄果实动态运动，通过phantom软件对拍摄的视频中每帧图像进行存储，采用tema软件对图像中的特征点进行处理，获取果实表面特征点的绝对坐标，根据特征点与连体基坐标系的固有关系，通过逆旋转变换计算每帧图像对应各时刻下的连体基坐标系的点o1、x1、y1、z1的绝对坐标，通过空间向量夹角公式计算各时刻下的向量与公共参考基坐标系中z轴的夹角θ，该夹角θ表示果实的摆动姿态；

(6)将各时刻下的旋转至与公共参考基坐标系中z轴重合的位置，计算旋转后的与公共参考基坐标系中x轴的夹角，该夹角表示果实的扭转姿态；

(7)设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，分别计算点o1在t+1时刻与t时刻沿公共参考基坐标系的x轴的位移、y轴的位移以及z轴的位移，根据位移计算t时刻果实沿x轴的瞬时速度、沿y轴的瞬时速度以及沿z轴的瞬时速度，根据瞬时速度计算在t时刻果实沿x轴的瞬时加速度、沿y轴的瞬时加速度以及沿z轴的瞬时加速度，计算果实在相邻两帧图像时间间隔内摆动角位移，通过摆动角位移依次计算在t时刻果实摆动角速度和在t时刻果实摆动角加速度，根据t时刻到t+1时刻的扭转角度计算t时刻果实扭转角速度和t时刻果实扭转角加速度。

作为本发明进一步改进的技术方案，所述的步骤(2)包括：

以果实和果柄的结合处为原点建立连体基坐标系，以果实和果柄的结合点与果实的中心点之间的连线为中心线，该中心线为连体基坐标系的z轴,记为z连轴,以中心点指向结合点为z连轴正方向，在垂直于连体基坐标系的z轴的平面上分别建立连体基坐标系的x轴与y轴，分别记为x连轴与y连轴；将连体基坐标系的原点标记为o1，x连轴正方向上单位向量的终点标记为x1，y连轴正方向上单位向量的终点标记为y1，z连轴正方向上单位向量的终点标记为z1。

作为本发明进一步改进的技术方案，所述的绝对坐标为在公共参考基坐标系中的空间坐标，所述公共参考基坐标系的坐标轴的单位向量分别为：x＝(100)^t、y＝(010)^t、z＝(001)^t。

作为本发明进一步改进的技术方案，所述的步骤(4)具体包括：

(a)在静态下通过两台高速摄像仪对果实表面的三个特征点以及果实和果柄的结合处建立的连体基坐标系的点o1、x1、y1、z1进行静态拍摄，通过phantom软件对拍摄的图像进行存储，采用tema软件对图像中的特征点c1、c2、c3以及连体基坐标系的点o1、x1、y1、z1进行处理,获取特征点c1、c2、c3的绝对坐标和连体基坐标系的点o1、x1、y1、z1的绝对坐标；

(b)通过绝对坐标建立向量与将与单位化；建立向量向量向量和向量

(c)将单位化后的与做向量积得到:以和做向量积得到：从而建立以特征点c1为原点的坐标系cxyz,则坐标系cxyz的坐标轴向量为通过空间向量夹角计算公式计算向量分别与公共参考基坐标系的x轴、y轴和z轴的夹角αx、βx、γx,计算向量分别与公共参考基坐标系的x轴、y轴和z轴的夹角αy、βy、γy,计算向量分别与公共参考基坐标系的x轴、y轴和z轴的夹角αz、βz、γz,则坐标转换矩阵为：

点o1在坐标系cxyz下的坐标通过公式(2)得到：

其中由步骤(4)中的(b)得到；

同理可得以及其中的坐标分别为点x1、y1、z1在坐标系cxyz下的坐标；

的坐标分别为点o1、x1、y1、z1在坐标系cxyz下的坐标，即表示特征点与连体基坐标系的固有关系。

作为本发明进一步改进的技术方案，所述的步骤(5)具体包括：

(a)通过两台高速摄像仪拍摄果实动态运动,通过phantom软件对拍摄的视频中每帧图像进行存储，采用tema软件对图像中的特征点进行处理，获取果实表面特征点的绝对坐标；

(b)通过空间向量夹角计算公式计算t时刻下向量分别与公共参考基坐标系的x轴、y轴和z轴的夹角αxt、βxt、γxt，计算向量分别与公共参考基坐标系的x轴、y轴和z轴的夹角计算向量分别与公共参考基坐标系的x轴、y轴和z轴的夹角αzt、βzt、γzt，其中为以t时刻对应的一帧图像中的特征点c1为原点建立的坐标系cxyz的坐标轴向量，则坐标转换矩阵为：

点o1在公共参考基坐标系中的坐标，即绝对坐标为：

其中为t时刻下点o1的绝对坐标，为t时刻下点c1的绝对坐标，为点o1在坐标系cxyz下的坐标；同理计算t时刻下点x1、y1、z1的绝对坐标；

每帧图像相互独立，且分别对应一个时刻，逐帧求解连体基坐标系的点o1、x1、y1、z1的绝对坐标；通过空间向量夹角公式计算各时刻下的向量与公共参考基坐标系中z轴的夹角θ：

其中为公共参考基坐标系中z轴的单位向量，向量与公共参考基坐标系中z轴的夹角θ表示果实的摆动姿态。

作为本发明进一步改进的技术方案，所述的步骤(6)具体包括：

(a)通过轴旋转矩阵将某帧图像对应时刻下的旋转至与公共参考基坐标系中z轴重合的位置，计算旋转后的与公共参考基坐标系中x轴的夹角，该夹角表示该帧图像对应时刻下的果实的扭转姿态，具体如下：

该帧图像的z连轴方向向量为：计算z连轴与公共参考基坐标系中z轴夹角

计算z连轴在公共参考基坐标系的x轴、y轴所在的平面上的投影与公共参考基坐标系的y轴的夹角ψ:

计算轴旋转矩阵：

旋转后的点x1的绝对坐标与旋转后的向量的坐标一致，旋转后的向量为：

其中为该帧图像的旋转前的向量

则扭转角度为：

该扭转角度为旋转后的向量与公共参考基坐标系中x轴的夹角，表示该帧图像对应时刻下的果实的扭转姿态，其中(xx、yx、zx)^t为公共参考基坐标系中x轴的单位向量；

(b)同理步骤(6)中的(a)计算每帧图像对应各时刻下的果实的扭转姿态。

作为本发明进一步改进的技术方案，所述的步骤(7)具体包括：

(a)设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，以相邻两帧图像中的连体基坐标系的点o1为基础，分别计算点o1在t+1时刻与t时刻沿公共参考基坐标系的x轴的位移：

其中分别为t+1时刻与t时刻点o1在公共参考基坐标系下x坐标；同理计算点o1在t+1时刻与t时刻沿公共参考基坐标系的y轴的位移sy以及z轴的位移sz，则合位移为：

(b)用点o1在t时刻到t+1时刻沿公共参考基坐标系的x轴移动的平均速度表示在t时刻果实沿x轴的瞬时速度vx(t)：

其中δt为两帧图像之间的间隔时间；同理可得在t时刻果实沿公共参考基坐标系的y轴的瞬时速度vy(t)以及在t时刻果实沿公共参考基坐标系的z轴的瞬时速度vz(t)，则在t时刻果实的合速度为：

(c)用点o1在t时刻到t+1时刻沿公共参考基坐标系的x轴移动的加速度表示在t时刻果实沿x轴的瞬时加速度ax(t)：

其中vx(t+1)为在t+1时刻果实沿x轴的瞬时速度，vx(t)为在t时刻果实沿x轴的瞬时速度；同理可得在t时刻果实沿公共参考基坐标系的y轴的瞬时加速度ay(t)以及在t时刻果实沿公共参考基坐标系的z轴的瞬时加速度az(t)；则在t时刻果实的合加速度为：

(d)用t时刻z连轴到t+1时刻z连轴的夹角表示果实在δt内的摆动角位移：

其中为t时刻z连轴的方向向量，即t时刻向量为t+1时刻z连轴的方向向量，即t+1时刻向量

(e)用t时刻到t+1时刻平均摆动角速度表示在t时刻果实摆动角速度

(f)用t时刻到t+1时刻摆动角速度变化量表示在t时刻果实摆动角加速度

(g)通过轴旋转矩阵将t时刻的z连轴以及t+1时刻的z连轴均旋转至与公共参考基坐标系中z轴重合的位置，采用步骤(6)中步骤(a)的公式(6)至公式(10)的方法计算t时刻下旋转后的向量以及t+1时刻下旋转后的向量则t时刻到t+1时刻的扭转角度为：

其中为t时刻下旋转后的向量为t+1时刻下旋转后的向量

t时刻果实扭转角速度为：

t时刻果实扭转角加速度为：

本发明的有益效果为：果实振动采摘当振动产生的惯性力超过果实与果柄的分离力时，果实在最弱的连接点脱离果枝或果柄完成采摘。果实分离的效果最终取决于果实振动时产生的最大惯性力。惯性力源于加速度，因此加速度响应是果树动力学中最重要的特性之一。本方法可以通过果柄与果实结合处的连体基之间的关系，构建果实在振动过程中的运动轨迹，进而求解各时刻果实的位移、速度与加速度，有利于探究果实的运动规律。且本发明还可以获得果实振动过程中的摆动姿态和扭转姿态。该方法针对林果振动采收领域中对果实运动状态的研究，能够较好地追踪果实的运动。

附图说明

图1为方法流程图。

图2为果实空间坐标系关系图。

图3为特征点x坐标曲线。

图4为特征点y坐标曲线。

图5为特征点z坐标曲线。

图6为果实空间运动示意图。

图7为果实空间平移轨迹。

图8为相邻两时刻连体基坐标位置图。

图9为扭转角示意图。

具体实施方式

下面根据图1至图9对本发明的具体实施方式作出进一步说明：

本实施例提供一种基于双目视觉追踪果实空间姿态及果实空间运动的方法，图1为本实施例的应用步骤图。下面针对图1的内容进行详细展开。

在果实表面标记三个特征点c1、c2、c3。

建立一种空间果实姿态设定方法，将果实视为刚体，其在任何状态下不发生形变，并将果实视为标准回转体；以果实和果柄的结合处为原点建立连体基坐标系，以果实和果柄的结合点与果实的中心点之间的连线为中心线，该中心线为连体基坐标系的z轴,记为z连轴,以中心点指向结合点为z连轴的正方向，在垂直于连体基坐标系的z轴的平面上分别建立连体基坐标系的x轴与y轴，分别记为x连轴与y连轴；将连体基坐标系的原点标记为o1，x连轴正方向上单位向量的终点标记为x1，y连轴正方向上单位向量的终点标记为y1，z连轴正方向上单位向量的终点标记为z1；应用此方式，可以将果实在空间中的运动分解为平移、绕果柄与果实结合处的摆动和绕中心线的扭转运动。

建立公共参考基坐标系，所述公共参考基坐标系的坐标轴的单位向量分别为：x＝(100)^t、y＝(010)^t、z＝(001)^t。本文中阐述的绝对坐标均为在公共参考基坐标系中的空间坐标。本文中阐述的向量均是在公共参考基坐标系中的向量。

由于双目相机追踪的果实表面特征点不能直接反映出果实的空间姿态，因此本实施例发明了一种将创建连体基与果实表面特征点间固定关系的方法。首先采用两台高速摄像仪1-1(m310、veo410)对果实表面的三个特征点以及果实和果柄的结合处建立的连体基坐标系的点o1、x1、y1、z1进行静态拍摄，通过phantom软件对拍摄的图像进行存储，采用tema软件对图像中的特征点以及连体基坐标系的点o1、x1、y1、z1进行处理，导出特征点c1、c2、c3和连体基坐标系的点o1、x1、y1、z1的空间的绝对坐标，进而建立特征点与连体基坐标系的固有关系。参见图2，本实施例采用两台高速摄像仪1-1对果实2-1进行拍摄，两台高速摄像仪1-1即双目视觉追踪方式可以得到深度信息，获取果实2-1的空间坐标，图2中的附图标记1-1代表高速摄像仪，附图标记2-1代表果实，图2中的x、y、z表示公共参考基坐标系，x连、y连、z连表示连体基坐标系，xcxyz、ycxyz、zcxyz表示坐标系cxyz。具体包括以下步骤：

(a)在静态下通过两台高速摄像仪(m310、veo410)对果实表面的三个特征点c1、c2、c3以及果实和果柄的结合处建立的连体基坐标系的点o1、x1、y1、z1进行静态拍摄，通过phantom软件对拍摄的图像进行存储，采用tema软件对图像中的特征点c1、c2、c3以及连体基坐标系的点o1、x1、y1、z1进行处理,得到特征点c1、c2、c3的绝对坐标和连体基坐标系的点o1、x1、y1、z1的绝对坐标；特征点c2的绝对坐标为特征点c1的绝对坐标为特征点c3的绝对坐标为

(b)通过绝对坐标建立向量与将与单位化；与分别为：建立向量为连体基坐标系的原点o1在公共参考基坐标系中的坐标；采用同样的方法建立向量向量和向量

(c)将单位化后的与做向量积得到以和做向量积得到从而建立以特征点c1为原点的坐标系cxyz,坐标系cxyz的坐标轴向量为通过空间向量夹角计算公式计算向量与公共参考基坐标系的x轴、y轴和z轴的夹角分别为αx、βx、γx,计算向量与公共参考基坐标系的x轴、y轴和z轴的夹角分别为αy、βy、γy,计算向量与公共参考基坐标系的x轴、y轴和z轴的夹角分别为αz、βz、γz；其中空间向量夹角计算公式为：

其中m1、m2、n1、n2、p1、p2分别对应空间向量的x坐标、y坐标以及z坐标；

则坐标转换矩阵为：

点o1在坐标系cxyz下的坐标通过公式(2)得到：

其中由上述步骤(b)得到；

同理以及其中对应的坐标分别为点x1、y1、z1在坐标系cxyz下的坐标；向量向量和向量均由上述步骤(b)得到；对应的坐标分别为点o1、x1、y1、z1在特征点坐标系cxyz下的坐标，即表示特征点与连体基坐标系的固有关系。本实施例中的固有关系为：

果实空间运动过程分为平移、摆动与扭转，其运动过程如图6所示。基于双目视觉追踪果实空间姿态的方法，首先采用两台高速摄像仪1-1(m310、veo410)拍摄果实动态运动，拍摄果实在进行一定频率的激励下的运动。通过phantom软件对拍摄的视频中每帧图像进行存储，采用tema软件对每帧图像中的特征点进行处理，导出视频中每帧图像的果实表面特征点的绝对坐标，如图3至图5所示，图3为特征点c1的x坐标图，图4为特征点c1的y坐标图，图5为特征点c1的z坐标图，代表视频中特征点c1运动曲线。图3至图5中横坐标表示每帧图像对应的各时刻。根据特征点与连体基坐标系的固有关系，通过逆旋转变换计算每帧图像对应各时刻下的连体基坐标系的点o1、x1、y1、z1的绝对坐标，通过空间向量夹角公式计算各时刻下的向量与公共参考基坐标系中z轴的夹角θ，该夹角θ表示果实的摆动姿态。将各时刻下的旋转至与公共参考基坐标系中z轴重合的位置，计算旋转后的与公共参考基坐标系中x轴的夹角，该夹角表示果实的扭转姿态。具体如下:

(a1)通过两台高速摄像仪1-1拍摄果实动态运动,通过phantom软件对拍摄的视频中每帧图像进行存储，采用tema软件对图像中的特征点进行处理，逐帧导出果实表面特征点的绝对坐标；

(b1)按照上述步骤(b)和步骤(c)的方法建立以t时刻对应的一帧图像中的特征点c1为原点的坐标系表示以t时刻对应的一帧图像中的特征点c1为原点建立的坐标系cxyz的坐标轴向量，通过空间向量夹角计算公式计算t时刻下向量与公共参考基坐标系的x轴、y轴和z轴的夹角分别为αxt、βxt、γxt，计算向量与公共参考基坐标系的x轴、y轴和z轴的夹角分别为计算向量与公共参考基坐标系的x轴、y轴和z轴的夹角分别为αzt、βzt、γzt，则t时刻下的坐标转换矩阵为：

点o1在公共参考基坐标系中的坐标，即绝对坐标为：

其中为t时刻下点o1的绝对坐标，为t时刻下点c1的绝对坐标，为点o1在坐标系cxyz下的坐标；同理计算t时刻下点x1、y1、z1的绝对坐标；其中t时刻下点x1的绝对坐标对应的向量等于t时刻下点c1的绝对坐标对应的向量加上at^-1乘以点x1在坐标系cxyz下的坐标的向量；点y1、点z1的绝对坐标计算方法以此类推；

每帧图像相互独立，且分别对应一个时刻，逐帧求解各时刻下的连体基坐标系的点o1、x1、y1、z1的绝对坐标；通过空间向量夹角公式分别计算各时刻下的向量与公共参考基坐标系中z轴的夹角θ：

其中为公共参考基坐标系中z轴的单位向量，向量与公共参考基坐标系中z轴的夹角θ表示果实的摆动姿态；

(c1)通过轴旋转矩阵将某帧图像对应时刻下的(即z连轴)旋转至与公共参考基坐标系中z轴重合的位置，计算旋转后的与公共参考基坐标系中x轴的夹角，该夹角表示该帧图像对应时刻下的果实的扭转姿态，具体如下：

该帧图像的z连轴方向向量为：计算z连轴与公共参考基坐标系中z轴夹角

计算(即z连轴)在公共参考基坐标系的x轴、y轴所在的平面上的投影与公共参考基坐标系的y轴的夹角ψ:

计算轴旋转矩阵：

旋转后的点x1的绝对坐标与旋转后的向量的坐标一致，旋转后的向量为：

其中为该帧图像的旋转前的向量

则扭转角度为：

该扭转角度为旋转后的向量与公共参考基坐标系中x轴的夹角，表示该帧图像对应时刻下的果实的扭转姿态，其中(xx、yx、zx)^t为公共参考基坐标系中x轴的单位向量；

(d1)同理步骤(c1)计算每帧图像对应各时刻下的果实的扭转姿态。

一种果实空间运动的研究方法，果实的空间运动需要用公共参考基坐标表示其绝对运动。以相邻两帧图像中的连体基坐标系的点o1为基础，设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，分别计算点o1在t+1时刻与t时刻沿公共参考基坐标系的x轴的位移、y轴的位移以及z轴的位移，根据位移计算t时刻果实沿x轴的瞬时速度、沿y轴的瞬时速度以及沿z轴的瞬时速度，根据瞬时速度计算在t时刻果实沿x轴的瞬时加速度、沿y轴的瞬时加速度以及沿z轴的瞬时加速度。计算果实在相邻两帧图像时间间隔内摆动角位移，通过摆动角位移依次计算在t时刻果实摆动角速度和在t时刻果实摆动角加速度。根据t时刻到t+1时刻的扭转角度计算t时刻果实扭转角速度和t时刻果实扭转角加速度。果实空间运动过程分为平移、摆动与扭转，其运动过程如图6所示。点o1沿xyz轴的移动代表果实的平移，绕果柄与果实结合点的摆动代表果实的摆动，绕中心线(即z连轴)的扭转运动代表果实的扭转。图7为果实空间平移轨迹，图7中曲线表示点o1在公共参考基坐标系的空间坐标中的运动。

其中沿xyz轴移动的瞬时速度、沿xyz轴移动的瞬时加速度、帧时间间隔内摆动角位移、摆动角速度、摆动角加速、帧时间间隔内扭转角度、扭转角速度、扭转角加速度均表示所追踪的果实空间运动状态。具体计算方法如下：

(a2)以相邻两帧图像中的连体基坐标系的点o1为基础，设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，分别计算点o1在t+1时刻与t时刻沿公共参考基坐标系的x轴的位移：

其中分别为t+1时刻与t时刻点o1在公共参考基坐标系下x坐标；同理计算点o1在t+1时刻与t时刻沿公共参考基坐标系的y轴的位移sy以及z轴的位移sz，则合位移为：

(b2)用点o1在t时刻到t+1时刻沿公共参考基坐标系的x轴移动的平均速度表示在t时刻果实沿x轴的瞬时速度vx(t)：

其中δt为两帧图像之间的间隔时间；同理可得在t时刻果实沿公共参考基坐标系的y轴的瞬时速度vy(t)以及在t时刻果实沿z轴的瞬时速度vz(t)，则在t时刻果实的合速度为：

(c2)用点o1在t时刻到t+1时刻沿公共参考基坐标系的x轴移动的加速度表示在t时刻果实沿x轴的瞬时加速度ax(t)：

其中vx(t+1)为在t+1时刻果实沿x轴的瞬时速度，vx(t)为在t时刻果实沿x轴的瞬时速度；同理可得在t时刻果实沿公共参考基坐标系的y轴的瞬时加速度ay(t)以及在t时刻果实沿公共参考基坐标系的z轴的瞬时加速度az(t)；则在t时刻果实的合加速度为：

(d2)由于求解时刻太多，因此本实施例列举相邻两帧图像对应的相邻t+1时刻与t时刻的连体基坐标系的位置示意果实的摆动与扭转。为方便表示，将相邻两时刻的连体基坐标原点重合，如图8所示，图8中的夹角δθ为t时刻z连轴到t+1时刻z连轴的夹角，δθ表示果实在δt内的摆动角位移：

其中为t时刻z连轴的方向向量，即t时刻向量为t+1时刻z连轴的方向向量，即t+1时刻向量

(e2)用t时刻到t+1时刻平均摆动角速度表示在t时刻果实摆动角速度

(f2)用t时刻到t+1时刻摆动角速度变化量表示在t时刻果实摆动角加速度

(g2)图8为相邻两时刻的连体基坐标原点重合后的示意图，图8中t时刻与t+1时刻的z连轴之间的夹角δθ表示果实在δt内的摆动角位移，但是该图8中t时刻与t+1时刻的x连轴之间的夹角、t时刻与t+1时刻的y连轴之间的夹角并不是扭转角，因此需要通过轴旋转矩阵将t时刻的z连轴(也可表述为)以及t+1时刻的z连轴均旋转至与公共参考基坐标系中z轴重合的位置，如图9所示，此图9中的两个z连轴与公共参考基坐标系中z轴重合，此时的两个x连轴之间的夹角即为t时刻到t+1时刻的扭转角度，计算方法为：采用上述步骤(c1)中公式(6)至公式(10)的方法分别计算t时刻下旋转后的向量以及t+1时刻下旋转后的向量

同理步骤(c1)中公式(10)，其中t时刻下旋转后的向量为：

其中为t时刻下z连轴旋转前的向量t时刻下z连轴旋转后的向量和为t时刻下轴旋转矩阵

则t时刻到t+1时刻的扭转角度为：

其中为t时刻下旋转后的向量为t+1时刻下旋转后的向量

t时刻果实扭转角速度为：

t时刻果实扭转角加速度为：

果实振动采摘当振动产生的惯性力超过果实与果柄的分离力时，果实在最弱的连接点脱离果枝或果柄完成采摘。果实分离的效果最终取决于果实振动时产生的最大惯性力。惯性力源于加速度，因此加速度响应是果树动力学中最重要的特性之一。本方法可以通过果柄与果实结合处的连体基之间的关系，构建果实在振动过程中的运动轨迹，进而求解各时刻果实的位移、速度与加速度，有利于探究果实的运动规律。所述的基于双目视觉追踪果实空间姿态及果实空间运动的方法，进行果实振动拍摄的系统和分析软件属于现有技术，如双目相机、空间位置合成软件。

本发明的保护范围包括但不限于以上实施方式，本发明的保护范围以权利要求书为准，任何对本技术做出的本领域的技术人员容易想到的替换、变形、改进均落入本发明的保护范围。