阅读说明：本技术 一种基于视觉的果园机械导航定位方法 (Visual-based orchard machine navigation positioning method ) 是由 苑严伟 韩振浩 李佳 朱立成 赵博 靳印浩 于 2019-09-10 设计创作，主要内容包括：一种基于视觉的果园机械导航定位方法,包括如下步骤：系统初始化后判断果园机械是否接收到卫星导航信号,若卫星导航信号正常则直接使用卫星导航；若卫星导航信号丢失则采用如下视觉导航定位；所述果园机械通过深度摄像头采集贴于对应的果树树干上的果树二维码信息得到视野内每颗果树的位置坐标(x-n,y-n)；所述果园机械在果树行间行驶并通过深度摄像头采集到的图像和深度信息,采用视觉里程计实时推算所述果园机械相对于运动起始点的位姿,并推算该果园机械的相对位置；以及通过所述深度摄像头采集果树树干的三维轮廓和对应的所述果树二维码的信息,返回所述果树的树干位置和世界坐标,推算所述果园机械的绝对位置,并校正所述视觉里程计的累积误差。(A visual-based orchard machine navigation positioning method comprises the following steps: after the system is initialized, judging whether the orchard machine receives satellite navigation signals, and if the satellite navigation signals are normal, directly using satellite navigation; if the satellite navigation signal is lost, adopting the following visual navigation positioning; the orchard machine acquires the two-dimensional code information of the fruit trees attached to the corresponding fruit tree trunk through the depth camera to obtain the position coordinate (x) of each fruit tree in the visual field n ,y n ) (ii) a The orchard machine runs among fruit tree rows and calculates the position and posture of the orchard machine relative to a motion starting point in real time by adopting a visual odometer according to the image and depth information acquired by a depth camera, and calculates the position and posture of the orchard machineA relative position; and acquiring the three-dimensional outline of a fruit tree trunk and the corresponding information of the fruit tree two-dimensional code through the depth camera, returning the trunk position and the world coordinate of the fruit tree, calculating the absolute position of the orchard machine, and correcting the accumulated error of the visual odometer.)

1. The visual-based orchard machine navigation positioning method is characterized by comprising the following steps:

s100, judging whether the orchard machine receives satellite navigation signals or not after the system is initialized, and directly using satellite navigation if the satellite navigation signals are normal; if the satellite navigation signal is lost, adopting the following visual navigation positioning;

s200, the orchard machine acquires information of two-dimensional codes of fruit trees through a depth camera to obtain position coordinates (X) of each fruit tree in a visual field_n,Y_n) The fruit tree two-dimensional code is pre-generated fruit tree position coordinate information and is attached to a corresponding fruit tree trunk;

s300, driving the orchard machine among fruit tree rows, calculating the position and the attitude of the orchard machine relative to a motion starting point in real time by using a visual odometer according to the image and the depth information acquired by a depth camera, and calculating the relative position of the orchard machine; and

s400, acquiring a three-dimensional outline of a fruit tree trunk and corresponding information of the fruit tree two-dimensional code through the depth camera, returning the trunk position and the world coordinate of the fruit tree, calculating the absolute position of the orchard machine, and correcting the accumulated error of the visual odometer.

2. The vision-based orchard machine navigation and positioning method according to claim 1, wherein in step S400, the depth camera identifies the trunk position of the fruit tree through an image processing method, generates a front three-dimensional map of the orchard machine, and calculates the treeRelative distance d between center of dry position and orchard machine_n。

3. The vision-based navigation positioning method for orchard machinery, according to claim 2, is characterized in that the absolute position of the orchard machinery is calculated by referring to at least two sets of information of relative distance and position coordinates, and the accumulated error of the vision odometer is corrected.

4. The vision-based navigation and positioning method for orchard machinery according to claim 3, wherein absolute position (X) of orchard machinery₀,Y₀) Simultaneous solution estimation by the following equations:

wherein x is₁,x₂,……,x_n，y₁,y₂,……,y_nIs the corresponding position coordinate of 1-n fruit trees in the visual field, d₁,d₂,……,d_nThe relative distances between the center of the trunk position of 1-n fruit trees and the orchard machinery are respectively.

5. The vision-based orchard machine navigation and positioning method according to claim 1, 2, 3 or 4, wherein in step S300, the vision odometer uses the correspondence between the three-dimensional contour and the image collected by the depth camera to solve the pose of the depth camera of the image of the adjacent frame based on feature point matching or pixel gray value difference.

6. The vision-based orchard machine navigation and positioning method according to claim 5, characterized in that an extended Kalman filter or a non-linear optimization method is adopted to solve the pose of the depth camera of the adjacent frame of the image.

7. The vision-based orchard machine navigation and positioning method according to claim 5, wherein in step S300, the orchard machine takes the world coordinate position of the fruit tree as an initial position to achieve accurate positioning and navigation.

8. The vision-based orchard machine navigation and positioning method according to claim 7, wherein after the satellite navigation signal is lost, the depth camera collects image information and depth information of fruit trees in the orchard environment in real time to generate a front three-dimensional environment map of the orchard machine, meanwhile, motion of the depth camera is estimated according to image information of adjacent front three-dimensional environment maps, and the position and pose of the orchard machine relative to the initial position are obtained through the vision odometer and the relative position of the orchard machine is calculated.

9. The vision-based navigation and positioning method for orchard machinery, according to claim 8, is characterized in that the vision odometer performs absolute position correction once every interval time so as to prevent position deviation caused by accumulated errors of the vision odometer from affecting positioning accuracy.

10. The vision-based orchard machine navigation positioning method of claim 7, 8 or 9, further comprising:

s500, judging whether the satellite navigation signal is recovered or not, if the satellite navigation signal is normal, recovering the use of satellite navigation, taking the latest absolute position correction of the orchard machine as an initial point, and taking the satellite positioning information as a current point to carry out accumulated error correction of the visual odometer so as to ensure the continuity and accuracy of a navigation path.

Technical Field

The invention relates to a positioning method of an agricultural robot, in particular to a visual-based navigation and positioning method of an orchard machine.

Background

Accurate navigation is the key technology of intelligent orchard machinery equipment, and stable navigation ability can guarantee that orchard machinery automatically travels to each corner of orchard. The research of the navigation positioning method is used as the basic work of accurate navigation, and the control precision of the orchard mechanical navigation operation is directly influenced by the performance of the navigation positioning method.

When orchard machinery independently operates, need go between two lines of fruit trees, traditional satellite positioning is because the sheltering from of the dense crown of fruit tree, and the signal occasionally easily leads to the positioning effect not good, and then influences the operation precision.

Disclosure of Invention

The invention aims to solve the technical problem of the prior art and provides a visual-based orchard machine navigation and positioning method.

In order to achieve the purpose, the invention provides a visual-based orchard machine navigation positioning method, which comprises the following steps:

s100, judging whether the orchard machine receives satellite navigation signals or not after the system is initialized, and directly using satellite navigation if the satellite navigation signals are normal; if the satellite navigation signal is lost, adopting the following visual navigation positioning;

s200, the orchard machine acquires information of two-dimensional codes of fruit trees through a depth camera to obtain position coordinates (X) of each fruit tree in a visual field_n,Y_n) The fruit tree two-dimensional code is pre-generated fruit tree position coordinate information and is attached to a corresponding fruit tree trunk;

s300, driving the orchard machine among fruit tree rows, calculating the position and the attitude of the orchard machine relative to a motion starting point in real time by using a visual odometer according to the image and the depth information acquired by a depth camera, and calculating the relative position of the orchard machine; and

s400, acquiring a three-dimensional outline of a fruit tree trunk and corresponding information of the fruit tree two-dimensional code through the depth camera, returning the trunk position and the world coordinate of the fruit tree, calculating the absolute position of the orchard machine, and correcting the accumulated error of the visual odometer.

In the visual-based navigation and positioning method for the orchard machine, in step S400, the depth camera identifies the trunk position of the fruit tree through an image processing method, generates a three-dimensional map in front of the orchard machine, and calculates the relative distance d between the center of the trunk position and the orchard machine_n。

In the visual-based navigation and positioning method for the orchard machine, the absolute position of the orchard machine is calculated by referring to at least two sets of information and position coordinates of the relative distance, and the accumulated error of the visual odometer is corrected.

The visual-based navigation and positioning method for the orchard machine is characterized in that the absolute position (X) of the orchard machine₀,Y₀) Simultaneous solution estimation by the following equations:

wherein x is₁,x₂,……,x_n，y₁,y₂,……,y_nIs the corresponding position coordinate of 1-n fruit trees in the visual field, d₁,d₂,……,d_nThe relative distances between the center of the trunk position of 1-n fruit trees and the orchard machinery are respectively.

In the visual-based orchard machine navigation and positioning method, in step S300, the visual odometer solves the pose of the depth camera of the image of the adjacent frame by using the corresponding relationship between the three-dimensional profile and the image acquired by the depth camera based on feature point matching or pixel gray value difference.

According to the visual-based orchard machine navigation positioning method, the pose of the depth camera of the adjacent frame of the image is solved by adopting an extended Kalman filter or a nonlinear optimization method.

In the visual-based navigation and positioning method for the orchard machine, in step S300, the orchard machine takes the world coordinate position of the fruit tree as an initial position to achieve accurate positioning and navigation.

According to the visual-based orchard machine navigation positioning method, after the satellite navigation signal is lost, the depth camera acquires image information and depth information of fruit trees in the orchard environment in real time, a front three-dimensional environment graph of the orchard machine is generated, meanwhile, the motion of the depth camera is estimated according to the adjacent image information of the front three-dimensional environment graph, the position and the attitude of the orchard machine relative to the initial position are obtained through the visual odometer, and the relative position of the orchard machine is calculated.

In the visual-based navigation and positioning method for the orchard machine, the visual odometer performs absolute position correction once every interval time so as to prevent the position deviation caused by the accumulated error of the visual odometer from influencing the positioning precision.

The visual-based orchard machine navigation and positioning method further comprises the following steps:

s500, judging whether the satellite navigation signal is recovered or not, if the satellite navigation signal is normal, recovering the use of satellite navigation, taking the latest absolute position correction of the orchard machine as an initial point, and taking the satellite positioning information as a current point to carry out accumulated error correction of the visual odometer so as to ensure the continuity and accuracy of a navigation path.

The invention has the technical effects that:

according to the method, the positioning information and the pose estimation are obtained by utilizing the depth camera and the two-dimensional code information among the fruit tree rows, the absolute position of the orchard machinery is obtained through the distance information and the positioning information, the accumulated error of the visual odometer can be corrected without constructing dictionary loop detection, the operation amount is small, and the real-time performance is good; the orchard machine is accurate in positioning, complexity and cost input of hardware are reduced, the problem that a satellite positioning system is unreliable in positioning under the shielding of a dense tree crown is solved, positioning navigation can be achieved when the orchard machine loses satellite signals, and the continuous working requirement of the orchard machine in an orchard environment is met.

The invention is described in detail below with reference to the drawings and specific examples, but the invention is not limited thereto.

Drawings

FIG. 1 is a flowchart of a navigation positioning method according to an embodiment of the present invention;

FIG. 2 is a schematic diagram of a navigation positioning method according to an embodiment of the present invention;

fig. 3 is a schematic diagram of calculating the absolute position of an orchard machine according to an embodiment of the invention.

Wherein the reference numerals

1 orchard machinery

2 satellite positioning module

3 degree of depth camera

4 fruit tree

5 navigation path

Detailed Description

The invention will be described in detail with reference to the following drawings, which are provided for illustration purposes and the like:

referring to fig. 1, fig. 1 is a flowchart of a navigation positioning method according to an embodiment of the present invention. The vision-based orchard machine navigation positioning method starts visual navigation positioning under the condition that satellite signals are lost, and directly uses satellite navigation under the condition that the signals are normal, wherein the visual navigation positioning comprises the following steps:

step S100, judging whether the orchard machine 1 receives satellite navigation signals after system initialization, and directly using satellite navigation if the satellite navigation signals are normal; if the satellite navigation signal is lost, adopting the following visual navigation positioning;

step S200, the orchard machine 1 acquires information of two-dimensional codes of fruit trees through the depth camera 3 to obtain position coordinates (X) of each fruit tree 4 in a visual field_n,Y_n) The fruit tree two-dimensional code is position coordinate information of a pre-generated fruit tree 4 and is attached to a corresponding fruit tree 4 trunk, namely before navigation positioning is started, a two-dimensional code generated by satellite (GPS/Beidou) position coordinate information and longitude and latitude information of the fruit tree 4 is attached to the corresponding fruit tree 4 trunk in advance, so that a depth camera 3 can conveniently acquire the position coordinate information in real time during navigation;

step S300, driving the orchard machine 1 among 4 rows of fruit trees, calculating the position and posture of the orchard machine 1 relative to a motion starting point in real time by using a visual odometer through the image and depth information acquired by the depth camera 3, and calculating the relative position of the orchard machine 1; and

and S400, acquiring a three-dimensional trunk outline of a fruit tree 4 and information of the corresponding two-dimensional fruit tree code through the depth camera 3, returning the trunk position and the world coordinate of the fruit tree 4, calculating the absolute position of the orchard machine 1, and correcting the accumulated error of the visual odometer.

The present embodiment may further include:

and S500, judging whether the satellite navigation signal is recovered, if the satellite navigation signal is normal, recovering the use of satellite navigation, taking the latest absolute position correction of the orchard machine 1 as an initial point, and taking the satellite positioning information as a current point to carry out accumulated error correction of the visual odometer so as to ensure the continuity and accuracy of the navigation path 5.

In step S400, the depth camera 3 identifies a trunk position of the fruit tree 4 by an image processing method, the depth camera 3 generates a three-dimensional map in front of the orchard machine 1, and calculates a relative distance d between a center of the trunk position and the orchard machine 1_n. The absolute position of the orchard machine 1 can be calculated by referring to at least two groups of information of the relative distance and the position coordinates, and the accumulated error of the visual odometer can be correctedAnd (4) poor. Wherein the absolute position (X) of the orchard machine 1₀,Y₀) Simultaneous solution estimation by the following equations:

wherein, X₁,X₂,……,X_n，Y₁,Y₂,……,Y_nIs the position coordinate corresponding to 1-n fruit trees 4 in the visual field, d₁,d₂,……,d_nThe relative distances between the center of the trunk position of 1-n fruit trees 4 and the orchard machine 1 are respectively.

In step S300, the visual odometer solves the pose of the depth camera 3 of the image of the adjacent frame by using the correspondence between the three-dimensional contour and the image acquired by the depth camera 3 based on feature point matching or pixel gray value difference. And solving the pose of the depth camera 3 of the image of the adjacent frame by adopting an extended Kalman filter or a nonlinear optimization method. And the orchard machine 1 takes the world coordinate position of the fruit tree 4 as an initial position to realize accurate positioning navigation.

When the satellite navigation signal is lost, the depth camera 3 collects image information and depth information of a fruit tree 4 in the orchard environment in real time to generate a front three-dimensional environment map of the orchard machine 1, meanwhile, the motion of the depth camera 3 is estimated according to the adjacent image information of the front three-dimensional environment map, the position and the posture of the orchard machine 1 relative to the initial position are obtained through the visual odometer, and the relative position of the orchard machine 1 is calculated. The visual odometer performs an absolute position correction once every interval time to prevent a position offset from affecting positioning accuracy due to accumulated errors of the visual odometer.

Referring to fig. 2, fig. 2 is a schematic diagram of a navigation positioning method according to an embodiment of the present invention. As shown in the figure, when the orchard machine 1 runs along the navigation path 5 between 4 rows of fruit trees, the visual odometer is adopted to calculate the pose estimation of the current orchard machine 1 relative to the motion starting point in real time through the image and the depth information acquired by the depth camera 3, and calculate the relative position of the orchard machine 1. Generating a two-dimensional code according to position coordinate information of a fruit tree 4 in an orchard environment in advance, attaching the two-dimensional code to a trunk of the fruit tree 4, collecting a three-dimensional contour and two-dimensional code information of the trunk through a depth camera 3, returning a trunk distance and a world coordinate of the fruit tree 4, calculating an absolute position of an orchard machine 1, and correcting an accumulated error of a visual odometer.

In an embodiment of the invention, the specific working process of the navigation and positioning method for the orchard machine is as follows:

after the system is initialized, judging whether the satellite signal is lost, if the signal is lost, taking the world coordinate position at the moment as an initial position, and acquiring image data by the depth camera 3;

the depth camera 3 can acquire image information and a depth map in real time to generate a three-dimensional environment in front of the orchard machine 1, the pose estimation of the orchard machine 1 relative to the initial position is obtained through a visual odometer, and the relative position is calculated; the depth camera 3 acquires image data, identifies the trunk position of the fruit tree 4 by an image processing method, and determines the distance between the orchard machine 1 and the central point of the fruit tree 4 by combining a depth map. The position coordinate information of the fruit tree 4 is generated into a two-dimensional code to be attached to the trunk of the fruit tree 4, and the depth camera 3 acquires the two-dimensional code information to obtain the position coordinate of the fruit tree 4. And calculating the absolute position of the orchard machine 1 by referring to the distance information and the position coordinates of a plurality of groups (at least 2 groups) of fruit trees 4, and correcting the accumulated error of the visual odometer. The absolute position correction is carried out once every a period of time, so that the situation that the accumulated error of pose estimation is too large is prevented. When the satellite signal is recovered, the last absolute position correction is used as an initial point, the satellite positioning information is used as a current point to perform the first visual odometer correction, and therefore the continuity and the accuracy of the navigation path 5 are guaranteed.

The visual odometer is a self-positioning method of the mobile robot system, and can be divided into a feature point method and a direct method according to the requirement of extracting features. When the visual odometer works, based on feature point matching or pixel gray value difference, the camera pose estimation of the adjacent frame images is solved by utilizing the corresponding relation between the known three-dimensional structure and the images, the solving method is more, and the method is preferably solved by expanding a Kalman filter and a nonlinear optimization method.

The solution process can be summarized as two basic equations:

assuming that the orchard machine 1 is carrying some kind of sensor and camera motion in an unknown environment, the motion of the camera for a continuous period of time while navigating is considered to be a discrete time t as what happens in 1, … …, k, since the camera usually collects data at some time. At these times, the position of the orchard machine 1 itself is represented by x, and the position at each time is denoted as x₁，……，x_kWhich constitute the trajectory of the movement of the orchard machine 1. Assuming that the map is composed of a plurality of road signs, and each time, the sensor measures a part of road points, and n road points are set, and y is used₁，……，y_nThey are represented.

First equation x_k＝f(x_k-1,u_k,w_k) For the equation of motion, typically the orchard machine 1 will carry a sensor, such as an encoder or inertial sensor, which measures its own motion. This sensor may measure readings about motion, but not necessarily directly as a difference in position, and possibly acceleration, angular velocity, etc. However, whatever the sensor, a generic, abstract mathematical model can be used to represent, namely:

x_k＝f(x_k-1,u_k,w_k)

in the formula, u_kIs the reading of the motion sensor, w_kIs noise. The process is described by a general function f, but the action mode of f is not specified, so that the whole function can refer to any motion sensor and becomes a general equation.

Second equation z_k,j＝h(y_j,x_k,v_k,j) For the observation equation, the observation equation is described when the orchard machine 1 is at x_kPosition to see aEach road sign point y_jAn observation data z is generated_k,j，v_k,jThe noise in the observation is the noise, the sensors used in the observation are in various forms, such as a depth camera 3, a laser radar and the like, and the relationship is also described by an abstract function h.

z_k,j＝h(y_j,x_k,v_k,j)

Knowing the motion measurement reading u, and the sensor reading z, the positioning problem (estimate x) and mapping problem (estimate y) are solved.

The positioning problem is to obtain known motion measurement readings u and sensor readings z, solve the relative motion of the orchard machine 1 between adjacent images (between each frame of video), and estimate the position x of the orchard machine 1 relative to the starting point (the position calculated according to each two frames of images is recorded as x)₁，……，x_k). The problem of building the picture refers to the situation that the orchard machine 1 is at the time k and x_kWhere a certain landmark y is detected_jConsider how to describe y in mathematical language_jI.e. estimate y.

Note that X and Y related to the above formula are different from X and Y in the absolute position of the fruit tree two-dimensional code and the orchard machine 1, X and Y in the fruit tree two-dimensional code are position coordinate information obtained by satellite positioning, and the absolute position of the orchard machine 1 is the absolute position coordinate of the orchard machine 1 calculated by a simultaneous equation based on the position coordinate of the fruit tree two-dimensional code, and the case is different here to show the difference between the two.

Referring to fig. 3, fig. 3 is a schematic diagram of calculating an absolute position of an orchard machine according to an embodiment of the present invention, and the principle of calculating an absolute position of an orchard machine 1 according to the present invention is shown in fig. 3. Reading u of the movement measurement_kThe satellite positioning module 2 or sensors such as an encoder and an IMU (inertial measurement Unit) can be used for reading, wherein the encoder is connected with the motor shaft and is used for measuring information such as the position, the angle and the number of turns of the motor shaft; the satellite positioning module 2 or the IMU is arranged on the frame, is close to the gravity center of the body as much as possible, and is used for measuring the three-axis attitude angle and the acceleration of the body during working. The encoder and IMU are for example only and the sensor type is not limited to these two types as long as the sensor can measure the motion of the machine itself. Conveying applianceSensor reading z_k,jAccessible sensor such as degree of depth camera 3, lidar reads, and this embodiment adopts degree of depth camera 3 to gather z_k,jAnd is arranged at the front position of the top of the orchard machine 1. The position coordinate information of the fruit tree 4 is generated in advance to be attached to the trunk of the fruit tree 4, the orchard machine 1 can acquire the position coordinate (X) of each fruit tree 4 in the visual field by acquiring the two-dimensional code information through the depth camera 3_n,Y_n). The trunk position of the fruit tree 4 is identified through an image processing method, the depth camera 3 can generate a three-dimensional map in front of the orchard machine 1, and then the relative distance d between the center of the fruit tree 4 and the orchard machine 1 can be calculated_n。

By referring to the distance information and position coordinates of several groups (at least 2 groups) of fruit trees 4, the absolute position (X) of the orchard machine 1 can be calculated in a plane geometric mode₀,Y₀) And further correcting the accumulated error of the visual odometer.

The solution process can be summarized as a basic equation:

the absolute position (X) of the orchard machine 1 can be calculated by simultaneous solution of equations₀,Y₀)。

The navigation and positioning of the orchard machine in the orchard can be divided into two parts: when the orchard machine runs among the fruit tree rows, calculating the pose estimation of the current orchard machine relative to the motion starting point in real time by adopting a visual odometer through the image and depth information acquired by the depth camera; generating a two-dimensional code from the position coordinate information of the fruit tree in the orchard environment, attaching the two-dimensional code to a fruit tree trunk, collecting the three-dimensional contour and the two-dimensional code information of the trunk through a depth camera, returning the trunk distance and the world coordinate of the fruit tree, and calculating the absolute position of the orchard machine. The method can calculate the estimation of the relative pose of the continuous motion of the orchard machine in real time, can also obtain the world coordinate of the orchard machine, correct the accumulated error of the pose of the visual odometer and realize the accurate positioning when the satellite signal is lost by the orchard machine. The orchard machine can realize positioning navigation when the satellite signals are lost, and the problem of poor positioning navigation effect caused by the shielding of the dense crown of the orchard on the satellite signals is solved.

The present invention may be embodied in other specific forms without departing from the spirit or essential attributes thereof, and it should be understood that various changes and modifications can be effected therein by one skilled in the art without departing from the spirit and scope of the invention as defined in the appended claims.