A dynamic path planning method for a grapefruit automatic picking front end

Abstract

The application provides a dynamic path planning method for a grape picking front end, and the method comprises the following steps: generating an initial trajectory according to a target grape position and a current position of the picking front end; moving along the initial trajectory and detecting path obstacles in real time; dynamically selecting a rigid obstacle avoidance strategy or a flexible penetration strategy based on physical characteristics of the obstacles; and repeatedly performing the detection and the strategy selection until a threshold range of the target position is reached. The application can greatly improve picking efficiency and save computing power.

Description

Technical Field

[0001] This invention relates to the field of agricultural mechanization technology, specifically to a dynamic path planning method for the automated harvesting front end of pomelos. Background Technology

[0002] Currently, some automated pomelo harvesting equipment already exists. This equipment first identifies the location of the pomelos, then plans an unobstructed path for the harvesting end to cut the stems and collect the pomelos. Such equipment can save a significant amount of labor. However, for lush pomelo trees, finding a suitable, completely unobstructed path among the branches and leaves is quite difficult, or rather, winding, and sometimes even impossible. Especially when the wind blows through the pomelo trees, the pre-planned path may become intermittent, even causing the automated harvesting equipment to malfunction, making path planning even more challenging. Therefore, how to relatively easily and efficiently plan a path to the pomelos is a problem that existing automated pomelo harvesting equipment urgently needs to solve. Summary of the Invention

[0003] To address the technical problems existing in the background art, this invention proposes a dynamic path planning method for an automated pomelo harvesting front end, characterized by including: generating an initial trajectory based on the target pomelo location and the current location of the harvesting front end; moving along the initial trajectory and detecting obstacles along the path in real time; dynamically selecting a rigid obstacle avoidance strategy or a flexible penetration strategy based on the physical characteristics of the obstacles; and cyclically executing the detection and strategy selection until the target location threshold range is reached.

[0004] Furthermore, the rigid obstacle avoidance strategy is as follows: generate an obstacle avoidance trajectory segment that satisfies the angular velocity constraint of the picking front end joint. After the picking front end bypasses the obstacle by executing the obstacle avoidance trajectory segment, a replacement trajectory to approach the target pomelo is regenerated.

[0005] Furthermore, the shortest distance between the obstacle avoidance trajectory segment and the obstacle is greater than the preset minimum safe distance.

[0006] Furthermore, dynamically selecting rigid obstacle avoidance strategies based on the physical characteristics of obstacles specifically includes:

[0007] Detect dynamic combinations of the primary physical properties of obstacles;

[0008] Based on the dynamic combination analysis of the first physical property, the deformation rate of the obstacle and the contact force gradient of the obstacle are analyzed.

[0009] When the deformation rate of the obstacle is lower than the preset value and the contact force gradient of the obstacle is higher than the preset value, it is determined that the obstacle constitutes a rigid obstruction to the picking front end, and a rigid obstacle avoidance strategy is selected to be executed.

[0010] Furthermore, the dynamic combination of the first physical characteristics specifically includes: ΔD1, D2, and F. (t+Δt) F (t) ;

[0011] Obstacle deformation rate δ=(ΔD1 / D2)×100%,

[0012] Specifically, ΔD1 is the average displacement of feature points after multiple consecutive frames of point cloud matching by the binocular vision module integrated on the picking front end.

[0013] D2 is the initial obstacle surface distance measured by a laser profile sensor integrated on the picking front end;

[0014] F (t) The contact force modulus value at time t for the six-dimensional force sensor integrated on the harvesting front end;

[0015] F (t+Δt) The contact force modulus at time (t+Δt) is the value of the six-dimensional force sensor integrated on the harvesting front end.

[0016] Furthermore, the flexible penetration strategy specifically involves applying a continuous contact force to the obstacle along the initial trajectory until the obstacle's displacement reaches the penetration threshold, after which the initial trajectory is continued.

[0017] Furthermore, the dynamic selection of flexible penetration strategies based on the physical characteristics of obstacles specifically includes:

[0018] Detect dynamic combinations of the second physical properties of obstacles;

[0019] The deformation rate of the obstacle and the frequency of contact force fluctuation are analyzed by dynamically combining the second physical properties.

[0020] When the deformation rate is higher than the preset value and the contact force fluctuation frequency is lower than the preset value, the flexible penetration strategy is selected.

[0021] Furthermore, the dynamic combination of the second physical characteristics specifically includes: ΔL, L0, P r P t d;

[0022] Deformation rate ε=(ΔL / L0)×100%;Contact force fluctuation frequency α=20log10(P r / P t ) / d;

[0023] Where: ΔL is the obstacle compression amount; L0 is the initial contact area diameter of the pressure-sensitive array; P r denoted as the millimeter-wave radar receiving power, Pt as the millimeter-wave radar transmitting power, and d as the depth of radar wave penetration into obstacles.

[0024] The beneficial effects of this invention are as follows: After identifying the target pomelo, this invention directly uses the straight-line trajectory to the target pomelo as the original trajectory, without the need for complex path planning or advance planning of avoidance strategies, thus greatly saving computing power. At the same time, when encountering obstacles, it does not always adopt the method of avoidance and bypass. For example, when encountering flexible obstacles such as leaves, it does not need to re-plan the path, but adopts a flexible penetration strategy, that is, a squeezing strategy. In the case of pomelo trees, the thin branches and leaves occupy most of the obstacles. Using this method not only greatly improves the harvesting efficiency, but also saves computing power. Attached Figure Description

[0025] Figure 1 This is an overall flowchart of a dynamic path planning method for an automated pomelo harvesting front end according to the present invention.

[0026] Figure 2 This is a partial flowchart of a dynamic path planning method for an automated pomelo harvesting front-end according to the present invention.

[0027] Figure 3 This is a partial flowchart of a dynamic path planning method for an automated pomelo harvesting front-end according to the present invention.

[0028] Figure 4 This is a partial flowchart of a dynamic path planning method for an automated pomelo harvesting front-end according to the present invention. Detailed Implementation

[0029] Referring to the figure, this invention proposes a dynamic path planning method for the automated pomelo harvesting front end, including:

[0030] S1. Generate an initial trajectory based on the target pomelo's location and the current position of the picking end, specifically including:

[0031] S11. Obtain the three-dimensional coordinates of the target pomelo through multi-sensor fusion, such as a visual camera combined with a laser rangefinder. The visual camera is used to identify the center point of the pomelo's outline and use the center point of the outline as the target point. The laser rangefinder measures the straight-line distance to the center point of the outline. After data fusion, the coordinates of the target point are output.

[0032] S12. Real-time acquisition of the harvesting front-end status, including reading the angle values ​​of each joint encoder, calculating the current position of the end effector through forward kinematics, and detecting the real-time attitude of the end effector.

[0033] S13. Establish trajectory generation constraints: such as the range of motion of the picking tip joint and the speed limit of the picking tip end.

[0034] S14. Generate a straight trajectory according to the above constraints. Based on the principle of linear motion between two points: calculate the vector difference between the target point and the current position, and decompose it into the motion of each joint to satisfy the smooth transition of joint velocity. The generated time-position-velocity three-parameter linear interpolation trajectory is the above initial trajectory.

[0035] S2. Drive the picking front end to move along the initial trajectory and detect obstacles in the path in real time. The detection of obstacles is specifically carried out by multimodal environmental perception. A stereo vision module is deployed to collect three-dimensional point clouds in front of the trajectory in real time. The spatial depth information of the path is scanned by millimeter-wave radar. Flexible tactile sensors are activated to monitor changes in contact force at the end. Based on point cloud clustering analysis, fixed obstacles such as branches and dynamic obstacles such as swaying branches and flying insects can be distinguished. An obstacle movement trajectory prediction model can also be established to estimate the path occupancy in the next few seconds.

[0036] S3. When an obstacle is detected, the physical characteristics of the obstacle are detected and analyzed, and a rigid obstacle avoidance strategy or a flexible penetration strategy is dynamically selected based on the physical characteristics of the obstacle.

[0037] The rigid obstacle avoidance strategy specifically involves generating obstacle avoidance trajectory segments that satisfy the angular velocity constraints of the picking front-end joints. Once the picking front-end bypasses an obstacle by executing the obstacle avoidance trajectory segment, a new alternative trajectory is generated to approach the target pomelo. The shortest distance between the obstacle avoidance trajectory segment and the obstacle is greater than the preset minimum safe distance, thus avoiding rigid obstacles such as tree trunks.

[0038] The flexible penetration strategy is as follows: apply a continuous contact force to the obstacle along the initial trajectory until the displacement of the obstacle reaches the penetration threshold, then continue along the initial trajectory, that is, physically push away the flexible obstacle.

[0039] Among them, the dynamic selection of rigid obstacle avoidance strategies based on the physical characteristics of obstacles specifically includes:

[0040] S311, Detect the dynamic combination of the first physical characteristics of obstacles.

[0041] S312. Based on the first physical property, dynamically combine and analyze the deformation rate of the obstacle and the contact force gradient of the obstacle.

[0042] The first dynamic combination of physical characteristics specifically includes: ΔD1, D2, and F. (t+Δt) F (t) Obstacle deformation rate δ=(ΔD1 / D2)×100%, Specifically, ΔD1 is the average displacement of feature points after matching multiple consecutive point clouds, acquired by a binocular vision module integrated on the harvesting front end; D2 is the initial obstacle surface distance, measured by a laser contour sensor integrated on the harvesting front end; F (t)The contact force modulus at time t is detected by a six-dimensional force sensor integrated on the harvesting front end; F (t+Δt) The contact force modulus at time (t+Δt) is collected by a six-dimensional force sensor integrated on the picking front end.

[0043] S313. When the deformation rate of the obstacle is lower than the preset value and the contact force gradient of the obstacle is higher than the preset value, it is determined that the obstacle constitutes a rigid obstruction to the picking front end, and the rigid obstacle avoidance strategy is selected to be executed.

[0044] Specifically, the flexible penetration strategy dynamically selected based on the physical characteristics of obstacles includes:

[0045] S321, Detect the dynamic combination of the second physical properties of obstacles.

[0046] The dynamic combination of the second physical characteristics specifically includes: ΔL, L0, P r P t d; Deformation rate ε=(ΔL / L0)×100%; Contact force fluctuation frequency α=20log10(P r / P t ) / d; ΔL is the obstacle compression amount; L0 is the initial contact area diameter, obtained by the pressure-sensitive array on the picking front end; P r denoted as the millimeter-wave radar receiving power, Pt as the millimeter-wave radar transmitting power, and d as the depth of radar wave penetration into obstacles.

[0047] S322. The deformation rate of the obstacle and the frequency of contact force fluctuation are analyzed by dynamic combination of the second physical characteristics.

[0048] S323. When the deformation rate is higher than the preset value and the contact force fluctuation frequency is lower than the preset value, select the flexible penetration strategy.

[0049] S4. Execute the detection and strategy selection repeatedly until the target location threshold range is reached.

[0050] After identifying the target pomelo, this invention directly uses the straight-line trajectory to the target pomelo as the original trajectory, without the need for complex path planning or pre-planned avoidance strategies, thus greatly saving computing power. At the same time, when encountering obstacles, it does not always use the avoidance method. For example, when encountering flexible obstacles such as leaves, it does not need to replan the path, but instead uses a flexible penetration strategy, i.e., a squeezing strategy. In the case of pomelo trees, the thin branches and leaves account for most of the obstacles. Using this method not only greatly improves harvesting efficiency, but also saves computing power.

[0051] The above are merely preferred embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A dynamic path planning method for the automated pomelo harvesting front end, characterized in that, include: An initial trajectory is generated based on the location of the target pomelo and the current position of the picking end. It moves along the initial trajectory and detects obstacles in the path in real time; Dynamically select rigid obstacle avoidance strategy or flexible penetration strategy based on the physical characteristics of the obstacle; The detection and strategy selection are performed repeatedly until the target location threshold range is reached; Generate an obstacle avoidance trajectory segment that satisfies the angular velocity constraints of the picking front end joint. After the picking front end bypasses the obstacle by executing the obstacle avoidance trajectory segment, a new alternative trajectory to approach the target pomelo is generated. Dynamically selecting rigid obstacle avoidance strategies based on the physical characteristics of obstacles specifically includes: Detect the dynamic combination of the primary physical properties of obstacles; Based on the dynamic combination analysis of the first physical property, the deformation rate of the obstacle and the contact force gradient of the obstacle are analyzed. When the deformation rate of the obstacle is lower than the preset value and the contact force gradient of the obstacle is higher than the preset value, it is determined that the obstacle constitutes a rigid obstruction to the picking front end, and a rigid obstacle avoidance strategy is selected to be executed. The flexible penetration strategy is as follows: apply a continuous contact force to the obstacle along the initial trajectory until the displacement of the obstacle reaches the penetration threshold, and then continue along the initial trajectory; The dynamic selection of flexible penetration strategies based on the physical characteristics of obstacles specifically includes: Detect dynamic combinations of the second physical properties of obstacles; The deformation rate of the obstacle and the frequency of contact force fluctuation are analyzed by dynamically combining the second physical properties. When the deformation rate is higher than the preset value and the contact force fluctuation frequency is lower than the preset value, the flexible penetration strategy is selected.

2. The method according to claim 1, characterized in that, The first dynamic combination of physical properties specifically includes: ΔD1, D2, F (t+Δt) F (t) ; Obstacle deformation rate δ=(ΔD1 / D2)×100%, contact force gradient F=(F(t+Δt)-F (t) ) / Δt; Wherein, ΔD1 is the average displacement of feature points after multiple consecutive frames of point cloud matching by the binocular vision module integrated on the harvesting front end; D2 is the initial obstacle surface distance measured by the laser contour sensor integrated on the harvesting front end; F (t) F(t+Δt) represents the contact force modulus of the six-dimensional force sensor integrated on the harvesting front end at time t; F(t+Δt) represents the contact force modulus of the six-dimensional force sensor integrated on the harvesting front end at time (t+Δt).

3. The method according to claim 1, characterized in that, The second dynamic combination of physical properties specifically includes: ΔL, P r P t d; deformation rate Contact force fluctuation frequency α = 20log10(P) r / P t ) / d; where: ΔL is the amount of obstacle compression; P is the diameter of the initial contact area of ​​the pressure-sensitive array. r For millimeter-wave radar receiving power, P t d represents the transmission power of the millimeter-wave radar, and d represents the penetration depth of the radar wave into obstacles.

Images