WO2020108668A1 - 基于双目视觉追踪果实空间姿态及果实空间运动的方法 - Google Patents
基于双目视觉追踪果实空间姿态及果实空间运动的方法 Download PDFInfo
- Publication number
- WO2020108668A1 WO2020108668A1 PCT/CN2020/074017 CN2020074017W WO2020108668A1 WO 2020108668 A1 WO2020108668 A1 WO 2020108668A1 CN 2020074017 W CN2020074017 W CN 2020074017W WO 2020108668 A1 WO2020108668 A1 WO 2020108668A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- axis
- fruit
- time
- coordinate system
- vector
- Prior art date
Links
- 235000013399 edible fruits Nutrition 0.000 title claims abstract description 246
- 238000000034 method Methods 0.000 title claims abstract description 50
- 230000033001 locomotion Effects 0.000 title claims abstract description 48
- 230000001133 acceleration Effects 0.000 claims abstract description 48
- 238000006073 displacement reaction Methods 0.000 claims abstract description 36
- 239000013598 vector Substances 0.000 claims description 138
- 238000010168 coupling process Methods 0.000 claims description 21
- 238000005859 coupling reaction Methods 0.000 claims description 21
- 230000008569 process Effects 0.000 claims description 17
- 238000004364 calculation method Methods 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 16
- 230000008878 coupling Effects 0.000 claims description 15
- 235000012093 Myrtus ugni Nutrition 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 6
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- 239000000758 substrate Substances 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000003306 harvesting Methods 0.000 abstract description 12
- 238000010586 diagram Methods 0.000 description 6
- 238000013519 translation Methods 0.000 description 6
- 238000011160 research Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 238000000926 separation method Methods 0.000 description 3
- 230000005284 excitation Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 235000011201 Ginkgo Nutrition 0.000 description 1
- 235000008100 Ginkgo biloba Nutrition 0.000 description 1
- 244000194101 Ginkgo biloba Species 0.000 description 1
- 241000758791 Juglandaceae Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000013016 damping Methods 0.000 description 1
- 235000011869 dried fruits Nutrition 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000005089 fruit drop Effects 0.000 description 1
- 235000012055 fruits and vegetables Nutrition 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 235000020234 walnut Nutrition 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
- G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T3/00—Geometric image transformations in the plane of the image
- G06T3/60—Rotation of whole images or parts thereof
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/285—Analysis of motion using a sequence of stereo image pairs
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/60—Analysis of geometric attributes
- G06T7/66—Analysis of geometric attributes of image moments or centre of gravity
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/06—Power analysis or power optimisation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10016—Video; Image sequence
- G06T2207/10021—Stereoscopic video; Stereoscopic image sequence
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30181—Earth observation
- G06T2207/30188—Vegetation; Agriculture
Definitions
- the invention belongs to the field of fruit and vegetable harvesting in agriculture and forestry, and particularly relates to a method for tracking fruit spatial posture and fruit spatial movement based on binocular vision, which is used for research on fruit movement status in the field of forest fruit vibration harvesting.
- Forest fruit harvesting is the most time-consuming and laborious part of forest fruit production.
- the most effective harvesting method at present is mechanical vibration harvesting.
- the harvesting effect is related to many factors, including the growth characteristics of fruit trees and the working parameters of mechanical vibration.
- the research on the vibration harvest of forest fruit is more about treating the tree itself as a second-order system, and detecting the relationship between excitation and response to measure the overall stiffness and damping ratio of the tree.
- the cantilever straight beam model is more accurate through experiments and theoretical calculations, which provides a theoretical basis for the research of vibration harvesting machinery.
- there are still gaps in the study of the movement of the fruit during the vibration harvesting process In order to explain the principle of vibration and fruit drop in detail, it is necessary to study the movement of the fruit.
- the technical problem to be solved by the present invention is to provide a method for tracking fruit space posture and fruit space motion based on binocular vision in view of the above-mentioned shortcomings of the prior art.
- the study of fruit movement in the field of fruit vibration harvesting can better track the movement of fruit.
- a method for tracking fruit space posture and fruit space movement based on binocular vision includes:
- the angular velocity of the fruit at time t and the angular acceleration of the fruit at time t are calculated from the angular displacement in sequence.
- the torsion angle from time t to t+1 is calculated as the fruit torsion angular velocity at time t and the fruit torsion angular acceleration at time t.
- the step (2) includes:
- the center line is Z axis, referred to as a Z-axis is connected to the center point is point binding site Z-axis positive direction even establish piece X and Y axes of the coordinate system based on the Z-axis in a plane perpendicular to the base piece coordinate system, respectively, were denoted by X even axis and Y-coupling;
- the origin of the piece base coordinate system labeled O 1 the end of the X-even-axis positive direction unit vectors labeled X 1
- the end of the Y-positive direction coupling unit vectors labeled Y 1 The end point of the unit vector in the positive direction of the Z- linked axis is marked as Z 1 .
- the step (4) specifically includes:
- the coordinates of are the coordinates of points X 1 , Y 1 and Z 1 in the coordinate system C xyz ;
- the coordinates of are the coordinates of points O 1 , X 1 , Y 1 , and Z 1 in the coordinate system C xyz , which represents the inherent relationship between the feature point and the connected base coordinate system.
- the step (5) specifically includes:
- Each frame of images is independent of each other and corresponds to a time, and the absolute coordinates of the points O 1 , X 1 , Y 1 , and Z 1 of the conjoined base coordinate system are solved frame by frame; the vector at each time is calculated by the angle formula of the space vector The angle ⁇ with the Z axis in the common reference base coordinate system:
- the step (6) specifically includes:
- the twist angle is the vector after rotation
- the angle with the X axis in the common reference base coordinate system represents the torsional posture of the fruit at the corresponding moment of the frame image, where (x X , y X , z X ) T is the unit vector of the X axis in the common reference base coordinate system ;
- step (6) calculate the twisting posture of the fruit at each moment corresponding to each frame of image.
- the step (7) specifically includes:
- ⁇ t is the interval time between two frames of images; in the same way, the instantaneous velocity of the fruit along the Y-axis of the common reference base coordinate system at time t and the Z of the fruit along the common reference base coordinate system at time t The instantaneous speed of the axis v z(t) , then the combined speed of the fruit at time t is:
- v x(t+1) is the instantaneous velocity of the fruit along the X-axis at time t+1
- v x(t) is the instantaneous velocity of the fruit along the X-axis at t
- the instantaneous acceleration a y(t) of the Y axis of the base coordinate system and the instantaneous acceleration a z(t) of the fruit along the Z axis of the common reference base coordinate system at time t then the total acceleration of the fruit at time t is:
- Z direction vector t is the time of coupling, i.e. the vector at time t Is the direction vector of the Z connecting axis at time t+1, that is, the vector at time t+1
- the torsional angular velocity of the fruit at time t is:
- the torsional angular acceleration of the fruit at time t is:
- the beneficial effects of the present invention are: fruit vibration picking When the inertial force generated by the vibration exceeds the separating force of the fruit and the fruit stalk, the fruit is separated from the fruit branch or the fruit stalk 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 originates 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 during the vibration process through the relationship between the conjoined base of the fruit stalk and the fruit, and then solve the displacement, velocity and acceleration of the fruit at each time, which is conducive to the exploration of the law of fruit movement.
- the present invention can also obtain a swing posture and a twist posture during fruit vibration. This method is aimed at the research of fruit movement in the field of forest fruit vibration harvesting, and can better track the movement of fruit.
- Figure 1 is a flowchart of the method.
- Figure 2 is the relationship diagram of fruit space coordinate system.
- Fig. 3 is the characteristic point x coordinate curve.
- Figure 4 shows the y-coordinate curve of feature points.
- Figure 5 is the z coordinate curve of the feature point.
- Figure 6 is a schematic diagram of fruit spatial movement.
- Figure 7 shows the fruit's spatial translation trajectory.
- Fig. 8 is a diagram of the base coordinate positions of the conjoined two-times.
- Figure 9 is a schematic diagram of the twist angle.
- FIGS. 1 to 9 The specific implementation of the present invention will be further described below based on FIGS. 1 to 9:
- FIG. 1 is a diagram of application steps of this embodiment. The content of FIG. 1 is expanded in detail below.
- Establish a space fruit posture setting method treat the fruit as a rigid body, which does not deform in any state, and treat the fruit as a standard rotator; establish the conjoined base coordinate system with the junction of the fruit and the stem as the origin , binding to the connection point between the center point of the fruit and fruit stalk of a centerline of the Z axis base piece coordinate system, referred to as a Z-axis is connected to the center point of juncture point Z
- origin-based labeled O 1 the end of the X-even-axis positive direction unit vectors labeled X 1
- the end of the Y-coupling the positive direction unit vector is labeled Y 1
- the end of the Z-coupling the positive direction of the unit vector is label
- the absolute coordinates described in this article are all spatial coordinates in the common reference base coordinate system.
- the vectors described in this article are all vectors in the common reference base coordinate system.
- this embodiment invents a method for creating a fixed relationship between the conjoined base and the characteristic points on the surface of the fruit.
- two high-speed cameras 1-1 (M310, VEO 410) are used to determine the three feature points on the fruit surface and the points O 1 , X 1 , Y 1 of the conjoined base coordinate system established at the junction of the fruit and the fruit stalk.
- Z 1 performs static shooting, stores the captured image through Phantom software, and uses TEMA software to process the feature points in the image and the points O 1 , X 1 , Y 1 , and Z 1 of the connected base coordinate system to derive feature points
- two high-speed cameras 1-1 are used to photograph the fruit 2-1.
- the two high-speed cameras 1-1 that is, binocular visual tracking, can obtain depth information and obtain the space of the fruit 2-1 coordinates
- the reference numerals 1-1 in FIG. 2 Representative high-speed video camera, reference numerals 2-1 on behalf of the fruit
- X-2 in FIG, Y, Z group represents a common reference coordinate system
- X even, Y even, Z even Represents the Siamese base coordinate system
- X cxyz , Y cxyz , Z cxyz represent the coordinate system C xyz . It includes the following steps:
- m 1 , m 2 , n 1 , n 2 , p 1 , and p 2 correspond to the x coordinate, y coordinate, and z coordinate of the space vector, respectively;
- the corresponding coordinates are the coordinates of points X 1 , Y 1 , and Z 1 in the coordinate system C xyz ; vector vector Sum vector All are obtained from the above step (b);
- the corresponding coordinates are the coordinates of the points O 1 , X 1 , Y 1 , and Z 1 in the feature point coordinate system C xyz , which represents the inherent relationship between the feature points and the connected base coordinate system.
- the inherent relationship in this embodiment is:
- the fruit spatial motion process is divided into translation, swing and twist, and its motion process is shown in Figure 6.
- two high-speed cameras 1-1 (M310, VEO 410) are used first to capture the dynamic motion of the fruit, and the motion of the fruit under the excitation of a certain frequency.
- the Phantom software is used to store each frame of the captured video, and the TEMA software is used to process the feature points in each frame of the image to derive the absolute coordinates of the fruit surface feature points of each frame of the image in the video, as shown in Figures 3 to 5.
- FIG. 3 is a C x-coordinate of feature points of FIG. 1
- FIG 4 is C y coordinate of feature points of FIG.
- FIG 5 is a graph of the feature point C z 1 represents the video feature point motion curve C 1.
- the abscissa indicates the time corresponding to each frame of image.
- the vector angle formula calculates the vector at each moment The angle ⁇ with the Z axis in the common reference base coordinate system, the angle ⁇ represents the swinging attitude of the fruit.
- the angle with the X axis in the common reference base coordinate system the angle represents the torsional posture of the fruit. details as follows:
- step (b1) Establish the coordinate system C xyz with the feature point C 1 in the image corresponding to the frame at time t as the origin according to the methods of step (b) and step (c) above; Represents the coordinate axis vector of the coordinate system C xyz with the feature point C 1 in the image of the corresponding frame at time t as the origin, and the vector at time t is calculated by the angle calculation formula of the space vector
- the angles with the X-axis, Y-axis and Z-axis of the common reference base coordinate system are ⁇ xt , ⁇ xt , ⁇ xt respectively , calculate the vector
- the angles with the X-axis, Y-axis and Z-axis of the common reference base coordinate system are respectively Calculation vector
- the angles with the X-axis, Y-axis and Z-axis of the common reference base coordinate system are ⁇ zt , ⁇ zt and ⁇ zt respectively , then the coordinate conversion matrix at
- Each frame of images is independent of each other, and corresponds to a time, and the absolute coordinates of the points O 1 , X 1 , Y 1 , and Z 1 of the Siamese base coordinate system at each time are solved frame by frame;
- the twist angle is the vector after rotation
- the angle with the X axis in the common reference base coordinate system represents the torsional posture of the fruit at the corresponding moment of the frame image, where (x X , y X , z X ) T is the unit vector of the X axis in the common reference base coordinate system ;
- step (d1) The same step (c1) calculates the torsional posture of the fruit at each moment corresponding to each frame of image.
- a research method for the spatial motion of fruit needs to express its absolute motion with a common reference base coordinate.
- the spatial motion of the fruit needs to express its absolute motion with a common reference base coordinate.
- set the time corresponding to the adjacent two frames of images as time t+1 and time t, respectively calculate the point O 1 at time t+1 and Displacement along the X-axis, Y-axis, and Z-axis along the common reference base coordinate system at time t, and calculate the instantaneous velocity of the fruit along the X-axis, the instantaneous velocity along the Y-axis, and the instantaneous velocity along the Z-axis at time t
- Fig. 7 is the trajectory of the fruit space translation.
- the curve in Fig. 7 represents the movement of the point O 1 in the space coordinate of the common reference base coordinate system.
- the instantaneous velocity along the X axis, the instantaneous acceleration along the X axis, the angular displacement of the swing within the frame interval, the angular velocity of the swing, the acceleration of the angular swing, the torsional angle within the frame interval, the torsional angular velocity, and the torsional angular acceleration are all Represents the tracked fruit spatial movement status.
- the specific calculation method is as follows:
- ⁇ t is the interval time between two frames of images; in the same way, the instantaneous velocity of the fruit along the Y axis of the common reference base coordinate system at time t v y(t) and the instantaneous velocity of the fruit along the Z axis at time t v z (t) , the closing speed of the fruit at time t is:
- v x(t+1) is the instantaneous velocity of the fruit along the X-axis at time t+1
- v x(t) is the instantaneous velocity of the fruit along the X-axis at t
- the instantaneous acceleration a y(t) of the Y axis of the base coordinate system and the instantaneous acceleration a z(t) of the fruit along the Z axis of the common reference base coordinate system at time t then the total acceleration of the fruit at time t is:
- this embodiment enumerates the positions of the conjoined base coordinate system at time t+1 and time t corresponding to the two adjacent frames of images to indicate the fruit swing and twist.
- theta angle in FIG. 8 is a time t Z t connected to the angle between the axis Z + 1 time of coupling, represents Delta] [theta Swing angular displacement of fruit in ⁇ t:
- Z direction vector t is the time of coupling, i.e. the vector at time t Is the direction vector of the Z connecting axis at time t+1, that is, the vector at time t+1
- FIG. 8 is a schematic diagram after the origin of the conjoined base coordinates coincides at two adjacent times.
- the angle ⁇ between the Z connecting axis at time t and time t+1 in Figure 8 represents the swing angular displacement of the fruit within ⁇ t , but the angle between the Y-axis is connected, the time t with t between the X-axis of FIG. 8 even time t and time t + 1 + 1 is not the timing angle twist angle is required by the shaft rotation matrix the coupling time t Z (may be expressed as )
- the Z- linked axis at time t+1 all rotate to the position that coincides with the Z-axis in the common reference base coordinate system, as shown in FIG. 9, the two Z- connected axes in FIG.
- the calculation method is: using the formula (6) to formula (10) in the above step (c1) Method to calculate the rotated vector at time t And the rotated vector at time t+1
- step (c1) where the vector after rotation at time t for:
- Z at time t is a vector before rotation coupling At time vector Z axis even after with Is the lower axis rotation matrix T 2 -1 , T 1 -1 at time t;
- the torsional angular velocity of the fruit at time t is:
- the torsional angular acceleration of the fruit at time t is:
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Geometry (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于双目视觉追踪果实空间姿态及果实空间运动的方法，包括：以果实和果柄的结合处为原点建立连体基坐标系，对果实表面的特征点以及果实和果柄的结合处建立的连体基坐标系的点进行静态拍摄，对拍摄的图像进行存储，获取特征点与连体基坐标系的固有关系；拍摄果实动态运动，获取果实表面特征点的绝对坐标，根据特征点与连体基坐标系的固有关系，计算每帧图像对应各时刻下的连体基坐标系的点的绝对坐标，分别计算果实的位移、瞬时速度、瞬时加速度，计算果实摆动角位移、摆动角加速度，计算果实扭转角速度和t时刻果实扭转角加速度。本发明针对林果振动采收领域中对果实运动状态的研究，能够较好地追踪果实的运动。
Description
本发明属于农林业果树采收领域，具体涉及一种基于双目视觉追踪果实空间姿态及果实空间运动的方法，用于林果振动采收领域中对果实运动状态的研究。
林果采收作业是林果生产中最耗时、最费力的一个环节，对于红枣、核桃、银杏等干果类林果，目前最有效的采收方式是机械振动收获，其振动采收机械的采收效果与多种因素有关，包括果树的生长特性和机械振动的工作参数。对于林果振动采收的研究更多的是将树木本身看作二阶系统，检测激励与响应的关系测得树木整体的刚度与阻尼比，近年来国内进行了更加细致的研究并建立了树干和树枝振动的简化力学模型，通过实验和理论计算比较认为悬臂直梁模型更为准确，为振动采收机械的研究提供了理论基础。但对于果实在振动采收过程中的运动情况的研究还存在空缺，为了更加细致深入的解释振动落果的原理，需要对果实的运动进行研究。
发明内容
本发明所要解决的技术问题是针对上述现有技术的不足提供一种基于双目视觉追踪果实空间姿态及果实空间运动的方法，本基于双目视觉追踪果实空间姿态及果实空间运动的方法针对林果振动采收领域中对果实运动状态的研究，能够较好地追踪果实的运动。
为实现上述技术目的，本发明采取的技术方案为：
一种基于双目视觉追踪果实空间姿态及果实空间运动的方法，包括：
(1)在果实表面标记三个特征点：C
1、C
2、C
3；
(2)以果实和果柄的结合处为原点建立连体基坐标系，连体基坐标系的X轴、Y轴和Z轴分别记为X
连轴、Y
连轴和Z
连轴，将连体基坐标系的原点标记为O
1，X
连轴正方向上单位向量的终点标记为X
1，Y
连轴正方向上单位向量的终点标记为Y
1，Z
连轴正方向上单位向量的终点标记为Z
1；
(3)建立公共参考基坐标系；
(4)对果实表面的三个特征点以及果实和果柄的结合处建立的连体基坐标系的点O
1、X
1、Y
1、Z
1进行静态拍摄，通过Phantom软件对拍摄的图像进行存储，采用TEMA软件对图像中的特征点以及连体基坐标系的点O
1、X
1、Y
1、Z
1进行处理，获取特征点C
1、C
2、C
3和连体基坐标系的点O
1、X
1、Y
1、Z
1的绝对坐标，进而获取特征点与连体基坐标系的固有关系；
(5)拍摄果实动态运动，通过Phantom软件对拍摄的视频中每帧图像进行存储，采用TEMA 软件对图像中的特征点进行处理，获取果实表面特征点的绝对坐标，根据特征点与连体基坐标系的固有关系，通过逆旋转变换计算每帧图像对应各时刻下的连体基坐标系的点O
1、X
1、Y
1、Z
1的绝对坐标，通过空间向量夹角公式计算各时刻下的向量
与公共参考基坐标系中Z轴的夹角θ，该夹角θ表示果实的摆动姿态；
(6)将各时刻下的
旋转至与公共参考基坐标系中Z轴重合的位置，计算旋转后的
与公共参考基坐标系中X轴的夹角，该夹角表示果实的扭转姿态；
(7)设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，分别计算点O
1在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
连轴；将连体基坐标系的原点标记为O
1，X
连轴正方向上单位向量的终点标记为X
1，Y
连轴正方向上单位向量的终点标记为Y
1，Z
连轴正方向上单位向量的终点标记为Z
1。
作为本发明进一步改进的技术方案，所述的绝对坐标为在公共参考基坐标系中的空间坐标，所述公共参考基坐标系的坐标轴的单位向量分别为：X＝(1 0 0)
T、Y＝(0 1 0)
T、Z＝(0 0 1)
T。
作为本发明进一步改进的技术方案，所述的步骤(4)具体包括：
(a)在静态下通过两台高速摄像仪对果实表面的三个特征点以及果实和果柄的结合处建立的连体基坐标系的点O
1、X
1、Y
1、Z
1进行静态拍摄，通过Phantom软件对拍摄的图像进行存储，采用TEMA软件对图像中的特征点C
1、C
2、C
3以及连体基坐标系的点O
1、X
1、Y
1、Z
1进行处理,获取特征点C
1、C
2、C
3的绝对坐标和连体基坐标系的点O
1、X
1、Y
1、Z
1的绝对坐标；
(b)通过绝对坐标建立向量
与
将
与
单位化；建立向量
向量
向量
和向量
(c)将单位化后的
与
做向量积得到:
以
和
做向量积得到：
从而建立以特征点C
1为原点的坐标系C
xyz,则坐标系C
xyz的坐标轴向量为
通过空间向量夹角计算公式计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α
x、β
x、γ
x,计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α
y、β
y、γ
y,计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α
z、β
z、γ
z,则坐标转换矩阵为：
点O
1在坐标系C
xyz下的坐标通过公式(2)得到：
其中
由步骤(4)中的(b)得到；
同理可得
以及
其中
的坐标分别为点X
1、Y
1、Z
1在坐标系C
xyz下的坐标；
作为本发明进一步改进的技术方案，所述的步骤(5)具体包括：
(a)通过两台高速摄像仪拍摄果实动态运动,通过Phantom软件对拍摄的视频中每帧图像进行存储，采用TEMA软件对图像中的特征点进行处理，获取果实表面特征点的绝对坐标；
(b)通过空间向量夹角计算公式计算t时刻下向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α
xt、β
xt、γ
xt，计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角
计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α
zt、β
zt、γ
zt，其中
为以t时刻对应的一帧图像中的特征点C
1为原点建立的坐标系C
xyz的坐标轴向量，则坐标转换矩阵为：
点O
1在公共参考基坐标系中的坐标，即绝对坐标为：
其中
为t时刻下点O
1的绝对坐标，
为t时刻下点C
1的绝对坐标，
为点O
1在坐标系C
xyz下的坐标；同理计算t时刻下点X
1、Y
1、Z
1的绝对坐标；
每帧图像相互独立，且分别对应一个时刻，逐帧求解连体基坐标系的点O
1、X
1、Y
1、Z
1的绝对坐标；通过空间向量夹角公式计算各时刻下的向量
与公共参考基坐标系中Z轴的夹角θ：
其中
(x
Z、y
Z、z
Z)
T为公共参考基坐标系中Z轴的单位向量，向量
与公共参考基坐标系中Z轴的夹角θ表示果实的摆动姿态。
作为本发明进一步改进的技术方案，所述的步骤(6)具体包括：
(a)通过轴旋转矩阵将某帧图像对应时刻下的
旋转至与公共参考基坐标系中Z轴重合的位置，计算旋转后的
与公共参考基坐标系中X轴的夹角，该夹角表示该帧图像对应时刻下的果实的扭转姿态，具体如下：
该帧图像的Z
连轴方向向量为：
计算Z
连轴与公共参考基坐标系中Z轴夹角
计算Z
连轴在公共参考基坐标系的X轴、Y轴所在的平面上的投影与公共参考基坐标系的Y轴的夹角ψ:
计算轴旋转矩阵：
旋转后的点X
1的绝对坐标与旋转后的向量
的坐标一致，旋转后的向量
为：
其中
为该帧图像的
旋转前的向量
则扭转角度为：
该扭转角度为旋转后的向量
与公共参考基坐标系中X轴的夹角，表示该帧图像对应时刻下的果实的扭转姿态，其中(x
X、y
X、z
X)
T为公共参考基坐标系中X轴的单位向量；
(b)同理步骤(6)中的(a)计算每帧图像对应各时刻下的果实的扭转姿态。
作为本发明进一步改进的技术方案，所述的步骤(7)具体包括：
(a)设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，以相邻两帧图像中的连体基坐标系的点O
1为基础，分别计算点O
1在t+1时刻与t时刻沿公共参考基坐标系的X轴的位移：
其中
分别为t+1时刻与t时刻点O
1在公共参考基坐标系下x坐标；同理计算点O
1在t+1时刻与t时刻沿公共参考基坐标系的Y轴的位移S
y以及Z轴的位移S
z，则合位移为：
(b)用点O
1在t时刻到t+1时刻沿公共参考基坐标系的X轴移动的平均速度表示在t时刻果实沿X轴的瞬时速度v
x(t)：
其中Δt为两帧图像之间的间隔时间；同理可得在t时刻果实沿公共参考基坐标系的Y轴的 瞬时速度v
y(t)以及在t时刻果实沿公共参考基坐标系的Z轴的瞬时速度v
z(t)，则在t时刻果实的合速度为：
(c)用点O
1在t时刻到t+1时刻沿公共参考基坐标系的X轴移动的加速度表示在t时刻果实沿X轴的瞬时加速度a
x(t)：
其中v
x(t+1)为在t+1时刻果实沿X轴的瞬时速度，v
x(t)为在t时刻果实沿X轴的瞬时速度；同理可得在t时刻果实沿公共参考基坐标系的Y轴的瞬时加速度a
y(t)以及在t时刻果实沿公共参考基坐标系的Z轴的瞬时加速度a
z(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的内容进行详细展开。
在果实表面标记三个特征点C
1、C
2、C
3。
建立一种空间果实姿态设定方法，将果实视为刚体，其在任何状态下不发生形变，并将果 实视为标准回转体；以果实和果柄的结合处为原点建立连体基坐标系，以果实和果柄的结合点与果实的中心点之间的连线为中心线，该中心线为连体基坐标系的Z轴,记为Z
连轴,以中心点指向结合点为Z
连轴的正方向，在垂直于连体基坐标系的Z轴的平面上分别建立连体基坐标系的X轴与Y轴，分别记为X
连轴与Y
连轴；将连体基坐标系的原点标记为O
1，X
连轴正方向上单位向量的终点标记为X
1，Y
连轴正方向上单位向量的终点标记为Y
1，Z
连轴正方向上单位向量的终点标记为Z
1；应用此方式，可以将果实在空间中的运动分解为平移、绕果柄与果实结合处的摆动和绕中心线的扭转运动。
建立公共参考基坐标系，所述公共参考基坐标系的坐标轴的单位向量分别为：X＝(1 0 0)
T、Y＝(0 1 0)
T、Z＝(0 0 1)
T。本文中阐述的绝对坐标均为在公共参考基坐标系中的空间坐标。本文中阐述的向量均是在公共参考基坐标系中的向量。
由于双目相机追踪的果实表面特征点不能直接反映出果实的空间姿态，因此本实施例发明了一种将创建连体基与果实表面特征点间固定关系的方法。首先采用两台高速摄像仪1-1(M310、VEO 410)对果实表面的三个特征点以及果实和果柄的结合处建立的连体基坐标系的点O
1、X
1、Y
1、Z
1进行静态拍摄，通过Phantom软件对拍摄的图像进行存储，采用TEMA软件对图像中的特征点以及连体基坐标系的点O
1、X
1、Y
1、Z
1进行处理，导出特征点C
1、C
2、C
3和连体基坐标系的点O
1、X
1、Y
1、Z
1的空间的绝对坐标，进而建立特征点与连体基坐标系的固有关系。参见图2，本实施例采用两台高速摄像仪1-1对果实2-1进行拍摄，两台高速摄像仪1-1即双目视觉追踪方式可以得到深度信息，获取果实2-1的空间坐标，图2中的附图标记1-1代表高速摄像仪，附图标记2-1代表果实，图2中的X、Y、Z表示公共参考基坐标系，X
连、Y
连、Z
连表示连体基坐标系，X
cxyz、Y
cxyz、Z
cxyz表示坐标系C
xyz。具体包括以下步骤：
(a)在静态下通过两台高速摄像仪(M310、VEO 410)对果实表面的三个特征点C
1、C
2、C
3以及果实和果柄的结合处建立的连体基坐标系的点O
1、X
1、Y
1、Z
1进行静态拍摄，通过Phantom软件对拍摄的图像进行存储，采用TEMA软件对图像中的特征点C
1、C
2、C
3以及连体基坐标系的点O
1、X
1、Y
1、Z
1进行处理,得到特征点C
1、C
2、C
3的绝对坐标和连体基坐标系的点O
1、X
1、Y
1、Z
1的绝对坐标；特征点C
2的绝对坐标为
特征点C
1的绝对坐标为
特征点C
3的绝对坐标为
(b)通过绝对坐标建立向量
与
将
与
单位化；
与
分别为：
建立向量
为连体基坐标系的原点O
1在公共参考基坐标系中的坐标；采用同样的方法建立向量
向量
和向量
(c)将单位化后的
与
做向量积得到
以
和
做向量积得到
从而建立以特征点C
1为原点的坐标系C
xyz,坐标系C
xyz的坐标轴向量为
通过空间向量夹角计算公式计算向量
与公共参考基坐标系的X轴、Y轴和Z轴的夹角分别为α
x、β
x、γ
x,计算向量
与公共参考基坐标系的X轴、Y轴和Z轴的夹角分别为α
y、β
y、γ
y,计算向量
与公共参考基坐标系的X轴、Y轴和Z轴的夹角分别为α
z、β
z、γ
z；其中空间向量夹角计算公式为：
其中m
1、m
2、n
1、n
2、p
1、p
2分别对应空间向量的x坐标、y坐标以及z坐标；
则坐标转换矩阵为：
点O
1在坐标系C
xyz下的坐标通过公式(2)得到：
其中
由上述步骤(b)得到；
同理
以及
其中
对应的坐标分别为点X
1、Y
1、Z
1在坐标系C
xyz下的坐标；向量
向量
和向量
均由上述步骤(b)得到；
对应的坐标分别为点O
1、X
1、Y
1、Z
1在特征点坐标系C
xyz下的坐标，即表示特征点与连体基坐标系的固有关系。本实施例中的固有关系为：
果实空间运动过程分为平移、摆动与扭转，其运动过程如图6所示。基于双目视觉追踪果实空间姿态的方法，首先采用两台高速摄像仪1-1(M310、VEO 410)拍摄果实动态运动，拍摄果实在进行一定频率的激励下的运动。通过Phantom软件对拍摄的视频中每帧图像进行存储，采用TEMA软件对每帧图像中的特征点进行处理，导出视频中每帧图像的果实表面特征点的绝对坐标，如图3至图5所示，图3为特征点C
1的x坐标图，图4为特征点C
1的y坐标图，图5为特征点C
1的z坐标图，代表视频中特征点C
1运动曲线。图3至图5中横坐标表示每帧图像对应的各时刻。根据特征点与连体基坐标系的固有关系，通过逆旋转变换计算每帧图像对应各时刻下的连体基坐标系的点O
1、X
1、Y
1、Z
1的绝对坐标，通过空间向量夹角公式计算各时刻下的向量
与公共参考基坐标系中Z轴的夹角θ，该夹角θ表示果实的摆动姿态。将各时刻下的
旋转至与公共参考基坐标系中Z轴重合的位置，计算旋转后的
与公共参考基坐标系中X轴的夹角，该夹角表示果实的扭转姿态。具体如下:
(a1)通过两台高速摄像仪1-1拍摄果实动态运动,通过Phantom软件对拍摄的视频中每帧图像进行存储，采用TEMA软件对图像中的特征点进行处理，逐帧导出果实表面特征点的绝对坐标；
(b1)按照上述步骤(b)和步骤(c)的方法建立以t时刻对应的一帧图像中的特征点C
1为原点的坐标系C
xyz；
表示以t时刻对应的一帧图像中的特征点C
1为原点建立的坐标系C
xyz的坐标轴向量，通过空间向量夹角计算公式计算t时刻下向量
与公共参考基坐标系的X轴、Y轴和Z轴的夹角分别为α
xt、β
xt、γ
xt，计算向量
与公共参考基坐标系的X轴、Y轴和Z轴的夹角分别为
计算向量
与公共参考基坐标系的X轴、Y轴和Z轴的夹角分别为α
zt、β
zt、γ
zt，则t时刻下的坐标转换矩阵为：
点O
1在公共参考基坐标系中的坐标，即绝对坐标为：
其中
为t时刻下点O
1的绝对坐标，
为t时刻下点C
1的绝对坐标，
为点O
1在坐标系C
xyz下的坐标；同理计算t时刻下点X
1、Y
1、Z
1的绝对坐标；其中t时刻下点X
1的绝对坐标对应的向量等于t时刻下点C
1的绝对坐标对应的向量加上A
t
-1乘以点X
1在坐标系C
xyz下的坐标的向量；点Y
1、点Z
1的绝对坐标计算方法以此类推；
每帧图像相互独立，且分别对应一个时刻，逐帧求解各时刻下的连体基坐标系的点O
1、X
1、Y
1、Z
1的绝对坐标；通过空间向量夹角公式分别计算各时刻下的向量
与公共参考基坐标系中Z轴的夹角θ：
其中
(x
Z、y
Z、z
Z)
T为公共参考基坐标系中Z轴的单位向量，向量
与公共参考基坐标系中Z轴的夹角θ表示果实的摆动姿态；
(c1)通过轴旋转矩阵将某帧图像对应时刻下的
(即Z
连轴)旋转至与公共参考基坐标系中Z轴重合的位置，计算旋转后的
与公共参考基坐标系中X轴的夹角，该夹角表示该帧图像对应时刻下的果实的扭转姿态，具体如下：
该帧图像的Z
连轴方向向量为：
计算Z
连轴与公共参考基坐标系中Z轴夹角
计算
(即Z
连轴)在公共参考基坐标系的X轴、Y轴所在的平面上的投影与公共参考基坐标系的Y轴的夹角ψ:
计算轴旋转矩阵：
旋转后的点X
1的绝对坐标与旋转后的向量
的坐标一致，旋转后的向量
为：
其中
为该帧图像的
旋转前的向量
则扭转角度为：
该扭转角度为旋转后的向量
与公共参考基坐标系中X轴的夹角，表示该帧图像对应时刻下的果实的扭转姿态，其中(x
X、y
X、z
X)
T为公共参考基坐标系中X轴的单位向量；
(d1)同理步骤(c1)计算每帧图像对应各时刻下的果实的扭转姿态。
一种果实空间运动的研究方法，果实的空间运动需要用公共参考基坐标表示其绝对运动。以相邻两帧图像中的连体基坐标系的点O
1为基础，设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，分别计算点O
1在t+1时刻与t时刻沿公共参考基坐标系的X轴的位移、Y轴的位移以及Z轴的位移，根据位移计算t时刻果实沿X轴的瞬时速度、沿Y轴的瞬时速度以及沿Z轴的瞬时速度，根据瞬时速度计算在t时刻果实沿X轴的瞬时加速度、沿Y轴的瞬时加速度以及沿Z轴的瞬时加速度。计算果实在相邻两帧图像时间间隔内摆动角位移，通过摆动角位移依次计算在t时刻果实摆动角速度和在t时刻果实摆动角加速度。根据t时刻到t+1时刻的扭转角度计算t时刻果实扭转角速度和t时刻果实扭转角加速度。果实空间运动过程分为平移、摆动与扭转，其运动过程如图6所示。点O
1沿X Y Z轴的移动代表果实的平移，绕果柄与果实结合点的摆动代表果实的摆动，绕中心线(即Z
连轴)的扭转运动代表果实的扭转。图7为果实空间平移轨迹，图7中曲线表示点O
1在公共参考基坐标系的空间坐标中的运动。
其中沿X Y Z轴移动的瞬时速度、沿X Y Z轴移动的瞬时加速度、帧时间间隔内摆动角位移、摆动角速度、摆动角加速、帧时间间隔内扭转角度、扭转角速度、扭转角加速度均表示所追踪的果实空间运动状态。具体计算方法如下：
(a2)以相邻两帧图像中的连体基坐标系的点O
1为基础，设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，分别计算点O
1在t+1时刻与t时刻沿公共参考基坐标系的X轴的位移：
其中
分别为t+1时刻与t时刻点O
1在公共参考基坐标系下x坐标；同理计算点O
1在t+1时刻与t时刻沿公共参考基坐标系的Y轴的位移S
y以及Z轴的位移S
z，则合位移为：
(b2)用点O
1在t时刻到t+1时刻沿公共参考基坐标系的X轴移动的平均速度表示在t时刻果实沿X轴的瞬时速度v
x(t)：
其中Δt为两帧图像之间的间隔时间；同理可得在t时刻果实沿公共参考基坐标系的Y轴的瞬时速度v
y(t)以及在t时刻果实沿Z轴的瞬时速度v
z(t)，则在t时刻果实的合速度为：
(c2)用点O
1在t时刻到t+1时刻沿公共参考基坐标系的X轴移动的加速度表示在t时刻果实沿X轴的瞬时加速度a
x(t)：
其中v
x(t+1)为在t+1时刻果实沿X轴的瞬时速度，v
x(t)为在t时刻果实沿X轴的瞬时速度；同理可得在t时刻果实沿公共参考基坐标系的Y轴的瞬时加速度a
y(t)以及在t时刻果实沿公共参考基坐标系的Z轴的瞬时加速度a
z(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
连轴旋转前的向量
时刻下Z
连轴旋转后的向量
和
为t时刻下轴旋转矩阵T
2
-1、T
1
-1；
则t时刻到t+1时刻的扭转角度为：
其中
为t时刻下旋转后的向量
为t+1时刻下旋转后的向量
t时刻果实扭转角速度为：
t时刻果实扭转角加速度为：
果实振动采摘当振动产生的惯性力超过果实与果柄的分离力时，果实在最弱的连接点脱离果枝或果柄完成采摘。果实分离的效果最终取决于果实振动时产生的最大惯性力。惯性力源于加速度，因此加速度响应是果树动力学中最重要的特性之一。本方法可以通过果柄与果实结合处的连体基之间的关系，构建果实在振动过程中的运动轨迹，进而求解各时刻果实的位移、速度与加速度，有利于探究果实的运动规律。所述的基于双目视觉追踪果实空间姿态及果实空间运动的方法，进行果实振动拍摄的系统和分析软件属于现有技术，如双目相机、空间位置合成软件。
本发明的保护范围包括但不限于以上实施方式，本发明的保护范围以权利要求书为准，任何对本技术做出的本领域的技术人员容易想到的替换、变形、改进均落入本发明的保护范围。
Claims (7)
- 一种基于双目视觉追踪果实空间姿态及果实空间运动的方法，其特征在于，包括：(1)在果实表面标记三个特征点：C 1、C 2、C 3；(2)以果实和果柄的结合处为原点建立连体基坐标系，连体基坐标系的X轴、Y轴和Z轴分别记为X 连轴、Y 连轴和Z 连轴，将连体基坐标系的原点标记为O 1，X 连轴正方向上单位向量的终点标记为X 1，Y 连轴正方向上单位向量的终点标记为Y 1，Z 连轴正方向上单位向量的终点标记为Z 1；(3)建立公共参考基坐标系；(4)对果实表面的三个特征点以及果实和果柄的结合处建立的连体基坐标系的点O 1、X 1、Y 1、Z 1进行静态拍摄，通过Phantom软件对拍摄的图像进行存储，采用TEMA软件对图像中的特征点以及连体基坐标系的点O 1、X 1、Y 1、Z 1进行处理，获取特征点C 1、C 2、C 3和连体基坐标系的点O 1、X 1、Y 1、Z 1的绝对坐标，进而获取特征点与连体基坐标系的固有关系；(5)拍摄果实动态运动，通过Phantom软件对拍摄的视频中每帧图像进行存储，采用TEMA软件对图像中的特征点进行处理，获取果实表面特征点的绝对坐标，根据特征点与连体基坐标系的固有关系，通过逆旋转变换计算每帧图像对应各时刻下的连体基坐标系的点O 1、X 1、Y 1、Z 1的绝对坐标，通过空间向量夹角公式计算各时刻下的向量与公共参考基坐标系中Z轴的夹角θ，该夹角θ表示果实的摆动姿态；
(6)将各时刻下的旋转至与公共参考基坐标系中Z轴重合的位置，计算旋转后的
与公共参考基坐标系中X轴的夹角，该夹角表示果实的扭转姿态；
(7)设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，分别计算点O 1在t+1时刻与t时刻沿公共参考基坐标系的X轴的位移、Y轴的位移以及Z轴的位移，根据位移计算t时刻果实沿X轴的瞬时速度、沿Y轴的瞬时速度以及沿Z轴的瞬时速度，根据瞬时速度计算在t时刻果实沿X轴的瞬时加速度、沿Y轴的瞬时加速度以及沿Z轴的瞬时加速度，计算果实在相邻两帧图像时间间隔内摆动角位移，通过摆动角位移依次计算在t时刻果实摆动角速度和在t时刻果实摆动角加速度，根据t时刻到t+1时刻的扭转角度计算t时刻果实扭转角速度和t时刻果实扭转角加速度。 - 根据权利要求1所述的基于双目视觉追踪果实空间姿态及果实空间运动的方法，其特征在于，所述的步骤(2)包括：以果实和果柄的结合处为原点建立连体基坐标系，以果实和果柄的结合点与果实的中心点之间的连线为中心线，该中心线为连体基坐标系的Z轴,记为Z 连轴,以中心点指向结合点为Z 连 轴正方向，在垂直于连体基坐标系的Z轴的平面上分别建立连体基坐标系的X轴与Y轴，分别记为X 连轴与Y 连轴；将连体基坐标系的原点标记为O 1，X 连轴正方向上单位向量的终点标记为X 1，Y 连轴正方向上单位向量的终点标记为Y 1，Z 连轴正方向上单位向量的终点标记为Z 1。
- 根据权利要求2所述的基于双目视觉追踪果实空间姿态及果实空间运动的方法，其特征在于，所述的绝对坐标为在公共参考基坐标系中的空间坐标，所述公共参考基坐标系的坐标轴的单位向量分别为：X＝(1 0 0) T、Y＝(0 1 0) T、Z＝(0 0 1) T。
- 根据权利要求3所述的基于双目视觉追踪果实空间姿态及果实空间运动的方法，其特征在于，所述的步骤(4)具体包括：(a)在静态下通过两台高速摄像仪对果实表面的三个特征点以及果实和果柄的结合处建立的连体基坐标系的点O 1、X 1、Y 1、Z 1进行静态拍摄，通过Phantom软件对拍摄的图像进行存储，采用TEMA软件对图像中的特征点C 1、C 2、C 3以及连体基坐标系的点O 1、X 1、Y 1、Z 1进行处理,获取特征点C 1、C 2、C 3的绝对坐标和连体基坐标系的点O 1、X 1、Y 1、Z 1的绝对坐标；(b)通过绝对坐标建立向量与
将
与
单位化；建立向量
向量
向量
和向量
(c)将单位化后的与
做向量积得到:
以
和
做向量积得到：
从而建立以特征点C 1为原点的坐标系C xyz,则坐标系C xyz的坐标轴向量为
通过空间向量夹角计算公式计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α x、β x、γ x,计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α y、β y、γ y,计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α z、β z、γ z,则坐标转换矩阵为：
点O 1在坐标系C xyz下的坐标通过公式(2)得到：
其中由步骤(4)中的(b)得到；
同理可得以及
其中
的坐标分别为点X 1、Y 1、Z 1在坐标系C xyz下的坐标；
的坐标分别为点O 1、X 1、Y 1、Z 1在坐标系C xyz下的坐标，即表示特征点与连体基坐标系的固有关系。
- 根据权利要求4所述的基于双目视觉追踪果实空间姿态及果实空间运动的方法，其特征在于，所述的步骤(5)具体包括：(a)通过两台高速摄像仪拍摄果实动态运动,通过Phantom软件对拍摄的视频中每帧图像进行存储，采用TEMA软件对图像中的特征点进行处理，获取果实表面特征点的绝对坐标；(b)通过空间向量夹角计算公式计算t时刻下向量分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α xt、β xt、γ xt，计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α yt、β yt、γ yt，计算向量
分别与公共参考基坐标系的X轴、Y轴和Z轴的夹角α zt、β zt、γ zt，其中
为以t时刻对应的一帧图像中的特征点C 1为原点建立的坐标系C xyz的坐标轴向量，则坐标转换矩阵为：
点O 1在公共参考基坐标系中的坐标，即绝对坐标为：
其中为t时刻下点O 1的绝对坐标，
为t时刻下点C 1的绝对坐标，
为点O 1在坐标系C xyz下的坐标；同理计算t时刻下点X 1、Y 1、Z 1的绝对坐标；
每帧图像相互独立，且分别对应一个时刻，逐帧求解连体基坐标系的点O 1、X 1、Y 1、Z 1的绝对坐标；通过空间向量夹角公式计算各时刻下的向量与公共参考基坐标系中Z轴的夹角θ：
其中(x Z、y Z、z Z) T为公共参考基坐标系中Z轴的单位向量，向量
与公共参考基坐标系中Z轴的夹角θ表示果实的摆动姿态。
- 根据权利要求5所述的基于双目视觉追踪果实空间姿态及果实空间运动的方法，其特征在于，所述的步骤(6)具体包括：(a)通过轴旋转矩阵将某帧图像对应时刻下的旋转至与公共参考基坐标系中Z轴重合的位置，计算旋转后的
与公共参考基坐标系中X轴的夹角，该夹角表示该帧图像对应时刻下的果实的扭转姿态，具体如下：
该帧图像的Z 连轴方向向量为：计算Z 连轴与公共参考基坐标系中Z轴夹角
计算Z 连轴在公共参考基坐标系的X轴、Y轴所在的平面上的投影与公共参考基坐标系的Y轴的夹角ψ:
计算轴旋转矩阵：
旋转后的点X 1的绝对坐标与旋转后的向量的坐标一致，旋转后的向量
为：
其中为该帧图像的
旋转前的向量
则扭转角度为：
该扭转角度为旋转后的向量与公共参考基坐标系中X轴的夹角，表示该帧图像对应时刻下的果实的扭转姿态，其中(x X、y X、z X) T为公共参考基坐标系中X轴的单位向量；
(b)同理步骤(6)中的(a)计算每帧图像对应各时刻下的果实的扭转姿态。 - 根据权利要求6所述的基于双目视觉追踪果实空间姿态及果实空间运动的方法，其特征在于，所述的步骤(7)具体包括：(a)设相邻两帧图像分别对应的时刻为t+1时刻与t时刻，以相邻两帧图像中的连体基坐标系的点O 1为基础，分别计算点O 1在t+1时刻与t时刻沿公共参考基坐标系的X轴的位移：
其中分别为t+1时刻与t时刻点O 1在公共参考基坐标系下x坐标；同理计算点O 1在t+1时刻与t时刻沿公共参考基坐标系的Y轴的位移S y以及Z轴的位移S z，则合位移为：
(b)用点O 1在t时刻到t+1时刻沿公共参考基坐标系的X轴移动的平均速度表示在t时刻果实沿X轴的瞬时速度v x(t)：
其中Δt为两帧图像之间的间隔时间；同理可得在t时刻果实沿公共参考基坐标系的Y轴的瞬时速度v y(t)以及在t时刻果实沿公共参考基坐标系的Z轴的瞬时速度v z(t)，则在t时刻果实的合速度为：
(c)用点O 1在t时刻到t+1时刻沿公共参考基坐标系的X轴移动的加速度表示在t时刻果 实沿X轴的瞬时加速度a x(t)：
其中v x(t+1)为在t+1时刻果实沿X轴的瞬时速度，v x(t)为在t时刻果实沿X轴的瞬时速度；同理可得在t时刻果实沿公共参考基坐标系的Y轴的瞬时加速度a y(t)以及在t时刻果实沿公共参考基坐标系的Z轴的瞬时加速度a z(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时刻果实扭转角加速度为：
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| US17/296,624 US11694343B2 (en) | 2018-11-27 | 2020-01-23 | Binocular-vision-based method for tracking fruit space attitude and fruit space motion |
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811438358.7 | 2018-11-27 | ||
| CN201811438358.7A CN109522669B (zh) | 2018-09-16 | 2018-11-27 | 基于双目视觉追踪果实空间姿态及果实空间运动的方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2020108668A1 true WO2020108668A1 (zh) | 2020-06-04 |
Family
ID=70859714
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2020/074017 WO2020108668A1 (zh) | 2018-11-27 | 2020-01-23 | 基于双目视觉追踪果实空间姿态及果实空间运动的方法 |
Country Status (3)
| Country | Link |
|---|---|
| US (1) | US11694343B2 (zh) |
| CN (1) | CN109522669B (zh) |
| WO (1) | WO2020108668A1 (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113834278A (zh) * | 2021-09-22 | 2021-12-24 | 珠海格力电器股份有限公司 | 食材数据分析方法、装置、储藏设备及存储介质 |
| US20220005209A1 (en) * | 2018-11-27 | 2022-01-06 | Nanjing Forestry University | Binocular-vision-based method for tracking fruit space attitude and fruit space motion |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111241739B (zh) * | 2020-01-17 | 2023-06-23 | 南京林业大学 | 一种有果有叶果树振动模型的构建方法 |
| CN111260690B (zh) * | 2020-01-17 | 2023-03-21 | 南京林业大学 | 基于高速摄影双目视觉技术的果柄动态结合力获取方法 |
| CN115937314B (zh) * | 2022-12-23 | 2023-09-08 | 南京林业大学 | 一种油茶果生长姿态检测方法 |
| CN117011843B (zh) * | 2023-08-09 | 2024-05-14 | 仲恺农业工程学院 | 一种用于火龙果自动化采摘的图像识别与姿态评估方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009159787A (ja) * | 2007-12-27 | 2009-07-16 | Toshiba Corp | 回転駆動される球体の姿勢を決定する装置及び方法 |
| CN102981011A (zh) * | 2012-12-10 | 2013-03-20 | 西北工业大学 | 一种基于双目视觉的空间目标旋转状态辨识方法 |
| CN107063228A (zh) * | 2016-12-21 | 2017-08-18 | 上海交通大学 | 基于双目视觉的目标姿态解算方法 |
| CN108376411A (zh) * | 2018-01-16 | 2018-08-07 | 上海交通大学 | 一种基于双目视觉的非合作目标相对状态解算方法 |
| CN108458710A (zh) * | 2018-04-27 | 2018-08-28 | 中国计量科学研究院 | 位姿测量方法 |
| CN109522669A (zh) * | 2018-09-16 | 2019-03-26 | 南京林业大学 | 基于双目视觉追踪果实空间姿态及果实空间运动的方法 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106404653B (zh) * | 2016-06-29 | 2020-02-07 | 浙江理工大学 | 仿生智能电子果实振动检测装置 |
| CN106875444B (zh) * | 2017-01-19 | 2019-11-19 | 浙江大华技术股份有限公司 | 一种目标物定位方法及装置 |
| CN107295856A (zh) * | 2017-07-13 | 2017-10-27 | 浙江理工大学 | 一种用于林果的新型夹持振动采收装置 |
-
2018
- 2018-11-27 CN CN201811438358.7A patent/CN109522669B/zh active Active
-
2020
- 2020-01-23 US US17/296,624 patent/US11694343B2/en active Active
- 2020-01-23 WO PCT/CN2020/074017 patent/WO2020108668A1/zh active Application Filing
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009159787A (ja) * | 2007-12-27 | 2009-07-16 | Toshiba Corp | 回転駆動される球体の姿勢を決定する装置及び方法 |
| CN102981011A (zh) * | 2012-12-10 | 2013-03-20 | 西北工业大学 | 一种基于双目视觉的空间目标旋转状态辨识方法 |
| CN107063228A (zh) * | 2016-12-21 | 2017-08-18 | 上海交通大学 | 基于双目视觉的目标姿态解算方法 |
| CN108376411A (zh) * | 2018-01-16 | 2018-08-07 | 上海交通大学 | 一种基于双目视觉的非合作目标相对状态解算方法 |
| CN108458710A (zh) * | 2018-04-27 | 2018-08-28 | 中国计量科学研究院 | 位姿测量方法 |
| CN109522669A (zh) * | 2018-09-16 | 2019-03-26 | 南京林业大学 | 基于双目视觉追踪果实空间姿态及果实空间运动的方法 |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220005209A1 (en) * | 2018-11-27 | 2022-01-06 | Nanjing Forestry University | Binocular-vision-based method for tracking fruit space attitude and fruit space motion |
| US11694343B2 (en) * | 2018-11-27 | 2023-07-04 | Nanjing Forestry University | Binocular-vision-based method for tracking fruit space attitude and fruit space motion |
| CN113834278A (zh) * | 2021-09-22 | 2021-12-24 | 珠海格力电器股份有限公司 | 食材数据分析方法、装置、储藏设备及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| US11694343B2 (en) | 2023-07-04 |
| CN109522669B (zh) | 2020-11-06 |
| CN109522669A (zh) | 2019-03-26 |
| US20220005209A1 (en) | 2022-01-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2020108668A1 (zh) | 基于双目视觉追踪果实空间姿态及果实空间运动的方法 | |
| WO2019062291A1 (zh) | 一种双目视觉定位方法、装置及系统 | |
| CN109164829A (zh) | 一种基于力反馈装置和vr感知的飞行机械臂系统及控制方法 | |
| CN107160380A (zh) | 一种基于scara机械手的相机标定和坐标变换的方法 | |
| CN1455355A (zh) | 处理被动光学运动获取数据的方法 | |
| CN113327298B (zh) | 基于图像实例分割和点云pca算法的一种抓取姿态估计方法 | |
| CN107390704A (zh) | 一种基于imu姿态补偿的多旋翼无人机光流悬停方法 | |
| CN206181178U (zh) | 一种航拍相机 | |
| TW201926240A (zh) | 無人飛行機之全景拍照方法與使用其之無人飛行機 | |
| CN109146939A (zh) | 一种工件抓取模板的生成方法及系统 | |
| CN107610086A (zh) | 一种基于仿生复眼结构的工业并联机器人快速视觉检测算法 | |
| CN104457758A (zh) | 基于视频采集的Visual Map数据库建立方法及利用该数据库的室内视觉定位方法 | |
| CN105173102B (zh) | 一种基于多图像的四旋翼飞行器增稳系统与方法 | |
| CN113305830B (zh) | 一种基于人体姿态控制的仿人机器人动作系统及控制方法 | |
| CN108900775A (zh) | 一种水下机器人实时电子稳像方法 | |
| CN108170160A (zh) | 一种利用单目视觉和机载传感器旋翼无人机自主抓取方法 | |
| Song et al. | Bundledslam: An accurate visual slam system using multiple cameras | |
| CN108927807A (zh) | 一种基于点特征的机器人视觉控制方法 | |
| CN109462717A (zh) | 电子稳像方法及终端 | |
| Liu et al. | 6d object pose estimation without pnp | |
| Maeda et al. | Frequency response experiments of 3-d pose full-tracking visual servoing with eye-vergence hand-eye robot system | |
| CN115179288B (zh) | 机器人的运动学逆解方法、机器人及计算机可读存储介质 | |
| CN111260690B (zh) | 基于高速摄影双目视觉技术的果柄动态结合力获取方法 | |
| WO2020010625A1 (zh) | 机器人运动学模型优化方法、系统和存储装置 | |
| CN112161639A (zh) | 一种基于角度光流法的垂直双目惯导里程计及其计算方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| 121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 20728889 Country of ref document: EP Kind code of ref document: A1 |
|
| NENP | Non-entry into the national phase |
Ref country code: DE |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 20728889 Country of ref document: EP Kind code of ref document: A1 |