Dynamic path planning method at automatic pomelo picking front end

Abstract

The invention provides a dynamic path planning method for an automatic pomelo picking front end. The method comprises the steps that an initial track is generated according to the target pomelo position and the current position of the picking front end; moving along an initial track, and detecting a path obstacle in real time; dynamically selecting a rigid obstacle avoidance strategy or a flexible penetration strategy based on physical characteristics of an obstacle; and circularly executing detection and strategy selection until the target position threshold range is reached. The picking efficiency can be greatly improved, and the computing power is saved.

Description

Technical Field

[0001] The present invention relates to the technical field of agricultural mechanization, and in particular to a dynamic path planning method for an automated pomelo picking front end. Background Art

[0002] At present, there are some automated grapefruit picking equipment that first identifies the location of the grapefruit, then plans an unobstructed path for the front end of the picking to cut the fruit stems, and then collect the grapefruit. This type of equipment can save a lot of manpower, but for the lush grapefruit trees, it is actually quite difficult to find a suitable path that is completely unobstructed between the branches and leaves, or it is tortuous, and sometimes it is even impossible. Especially when the wind blows the grapefruit tree, it can be said that the pre-planned path may appear and disappear from time to time, and may even cause the automated grapefruit picking equipment to crash, making path planning even more difficult. Therefore, how to relatively easily plan a path that can relatively easily and efficiently reach the grapefruit is a problem that needs to be urgently solved by existing automated grapefruit picking equipment. Summary of the Invention

[0003] To solve the technical problems existing in the background technology, the present invention proposes a dynamic path planning method for an automated grapefruit picking front end, which is characterized by including: generating an initial trajectory based on the target grapefruit position and the 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 the physical characteristics of the obstacle; and cyclically executing detection and strategy selection until the target position threshold range is reached.

[0004] Furthermore, the rigid obstacle avoidance strategy is specifically as follows: an obstacle avoidance trajectory segment is generated that satisfies the joint angular velocity constraints of the picking front end. After the picking front end bypasses the obstacle by executing the obstacle avoidance trajectory segment, an alternative trajectory is regenerated to approach the target grapefruit.

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

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

[0007] Detecting a dynamic combination of the first physical characteristics of an obstacle;

[0008] Dynamically analyzing the obstacle deformation rate and the contact force gradient of the obstacle based on the first physical characteristic;

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

[0010] Furthermore, the first physical characteristic dynamic combination specifically includes: ΔD1, D2, F (t+Δt) 、F (t) ;

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

[0012] Among them, ΔD1 is specifically the displacement average of the feature points after continuous multi-frame point cloud matching of the binocular vision module integrated on the picking front end;

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

[0014] F (t) is the contact force modulus of the six-dimensional force sensor integrated on the picking front end at time t;

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

[0016] Furthermore, the flexible penetration strategy is specifically as follows: applying a continuous contact force to the obstacle along the initial trajectory until the obstacle displacement reaches the penetration threshold and then continuing the initial trajectory.

[0017] Furthermore, the flexible penetration strategy is dynamically selected based on the physical characteristics of the obstacle, specifically including:

[0018] Detecting a dynamic combination of the second physical characteristics of an obstacle;

[0019] The obstacle deformation rate and contact force fluctuation frequency are analyzed through the dynamic combination of the second physical characteristics;

[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 second physical characteristic dynamic combination 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 compression of the obstacle; L0 is the initial contact area diameter of the pressure sensitive array, P r is the millimeter-wave radar receiving power, Pt is the millimeter-wave radar transmitting power, and d is the penetration depth of the radar wave into the obstacle.

[0024] The beneficial effect of the present invention is that after identifying the target pomelo, the present invention directly uses the trajectory of the straight line to the target pomelo as the original trajectory, without the need for complex path planning or advance planning of avoidance strategies, which greatly saves computing power. At the same time, when encountering obstacles, the present invention does not always adopt an avoidance method. For example, when encountering flexible obstructions caused by leaves, there is no need to re-plan the path, but a flexible penetration strategy, that is, a squeezing strategy, is adopted. For the pomelo tree scenario, the thin branches and leaves occupy most of the obstructions. Using this method not only greatly improves the picking efficiency but also saves computing power. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 This is an overall flow chart of a dynamic path planning method for an automated grapefruit picking front end of the present invention;

[0026] Figure 2 This is a partial flow chart of a dynamic path planning method for an automated grapefruit picking front end of the present invention;

[0027] Figure 3 This is a partial flow chart of a dynamic path planning method for an automated grapefruit picking front end of the present invention;

[0028] Figure 4 This is a partial flow chart of a dynamic path planning method for an automated pomelo picking front end according to the present invention. DETAILED DESCRIPTION

[0029] Referring to the figure, the present invention proposes a dynamic path planning method for the front end of automated grapefruit picking, comprising:

[0030] S1. Generate an initial trajectory based on the target grapefruit location and the current location of the picking front end, specifically including:

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

[0032] S12. Real-time acquisition of the state of the picking front end, including reading the angle value of each joint encoder, calculating the current position of the end through forward kinematics, and detecting the real-time posture of the end effector.

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

[0034] S14. Generate a straight line trajectory in accordance with 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 ensure smooth transition of the joint speed. The generated time-position-speed 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 path obstacles in real time. The detection of path obstacles specifically adopts multimodal environmental perception, deploys a stereo vision module to collect the three-dimensional point cloud in front of the trajectory in real time, scans the spatial depth information of the path through millimeter-wave radar, and activates the flexible tactile sensor to monitor the change of the end contact force. Based on point cloud clustering analysis, it can distinguish between fixed obstacles such as branches and dynamic obstacles such as swaying branches and flying insects. It can also establish an obstacle motion trajectory prediction model to estimate the path occupancy in the next few seconds.

[0036] S3. When an obstacle is detected on the path, 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 generates an obstacle avoidance trajectory segment that satisfies the angular velocity constraints of the picking front end's joints. Once the picking front end circumvents the obstacle by executing the obstacle avoidance trajectory segment, an alternative trajectory is generated that approaches the target grapefruit. The shortest distance between the obstacle avoidance trajectory segment and the obstacle is greater than the preset minimum safety distance, meaning that rigid obstacles such as tree trunks are avoided.

[0038] The flexible penetration strategy is as follows: a continuous contact force is applied to the obstacle along the initial trajectory until the obstacle displacement reaches the penetration threshold and then the initial trajectory is continued, that is, the flexible obstacle is physically pushed away.

[0039] The rigid obstacle avoidance strategy is dynamically selected based on the physical characteristics of the obstacle, specifically including:

[0040] S311. Detect a dynamic combination of the first physical characteristics of the obstacle.

[0041] S312: Dynamically analyze the obstacle deformation rate and the contact force gradient of the obstacle based on the first physical characteristic.

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

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

[0044] The flexible penetration strategy dynamically selected based on the physical characteristics of the obstacle specifically includes:

[0045] S321. Detect a dynamic combination of the second physical characteristics of the obstacle.

[0046] The second physical characteristic dynamic combination 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 amount of obstacle compression; L0 is the diameter of the initial contact area, which is collected by the pressure-sensitive array on the picking front end; P r is the millimeter-wave radar receiving power, Pt is the millimeter-wave radar transmitting power, and d is the penetration depth of the radar wave into the obstacle.

[0047] S322. Analyze the obstacle deformation rate and contact force fluctuation frequency through dynamic combination of the second physical characteristics.

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

[0049] S4. Loop detection and strategy selection until the target position threshold range is reached.

[0050] After identifying the target pomelo, the present invention directly uses the trajectory of the straight line to the target pomelo as the original trajectory. There is no need for complex path planning or advance planning of avoidance strategies, which greatly saves computing power. At the same time, when encountering obstacles, the method of avoiding and circumventing them is not always adopted. For example, when encountering flexible obstructions caused by leaves, there is no need to re-plan the path, but a flexible penetration strategy, that is, a squeezing strategy, is adopted. For the scenario of pomelo trees, the thin branches and leaves occupy most of the obstructions. Using this method not only greatly improves the picking efficiency but also saves computing power.

[0051] The above are only preferred specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any technician familiar with this technical field, within the technical scope disclosed by the present invention, who makes equivalent replacements or changes based on the technical solutions and inventive concepts of the present invention, should be covered by the scope of protection of the present invention.

Claims

1. A dynamic path planning method for the front end of automated pomelo picking, characterized in that: include: Generate an initial trajectory based on the target grapefruit location and the current location of the picking front end; Move along the initial trajectory and detect obstacles on the path in real time; Dynamically select a rigid obstacle avoidance strategy or a flexible penetration strategy based on the physical characteristics of the obstacle; The detection and strategy selection are performed in a loop until the target position threshold range is reached.

2. The method according to claim 1, characterized in that The rigid obstacle avoidance strategy is as follows: an obstacle avoidance trajectory segment is generated that satisfies the joint angular velocity constraints of the picking front end. After the picking front end bypasses the obstacle by executing the obstacle avoidance trajectory segment, an alternative trajectory is regenerated to approach the target grapefruit.

3. The method according to claim 2, characterized in that The shortest distance between the obstacle avoidance trajectory segment and the obstacle is greater than the preset minimum safety distance.

4. The method according to claim 2, characterized in that Dynamically selecting a rigid obstacle avoidance strategy based on the physical characteristics of the obstacle specifically includes: Detecting a dynamic combination of the first physical characteristics of an obstacle; Dynamically analyzing the obstacle deformation rate and the contact force gradient of the obstacle based on the first physical characteristic; When the obstacle deformation rate is lower than the preset value and the contact force gradient of the obstacle is higher than the preset value, it is judged that the obstacle constitutes a rigid block to the picking front end, and the rigid obstacle avoidance strategy is selected.

5. The method according to claim 4, characterized in that The first physical property dynamic combination specifically includes: ΔD1, D2, F (t+Δt) 、F (t) ; Obstacle deformation rate δ = (ΔD1 / D2) × 100%, contact force gradient Among them, ΔD1 is specifically the displacement average of the feature points after continuous multi-frame point cloud matching of the binocular vision module integrated on the picking front end; D2 is the initial obstacle surface distance measured by the laser profile sensor integrated on the picking front end; F (t) is the contact force modulus of the six-dimensional force sensor integrated on the picking front end at time t; F (t+Δt) is the contact force modulus of the six-dimensional force sensor integrated on the picking front end at the moment (t+Δt).

6. The method according to claim 2, characterized in that The flexible penetration strategy is as follows: a continuous contact force is applied to the obstacle along the initial trajectory until the obstacle displacement reaches the penetration threshold and then the initial trajectory is continued.

7. The method according to claim 1, characterized in that Dynamic selection of flexible penetration strategies based on the physical characteristics of obstacles specifically includes: Detecting a dynamic combination of the second physical characteristics of an obstacle; The obstacle deformation rate and contact force fluctuation frequency are analyzed through the dynamic combination of the second physical characteristics; 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.

8. The method according to claim 7, characterized in that The second physical characteristic dynamic combination specifically includes: ΔL, L0, P r 、P t , d; deformation rate ε=(ΔL / L0)×100%; contact force fluctuation frequency α=20log10(P r / P t ) / d; where: ΔL is the amount of obstacle compression; L0 is the diameter of the initial contact area of ​​the pressure-sensitive array, P r is the millimeter-wave radar receiving power, Pt is the millimeter-wave radar transmitting power, and d is the penetration depth of the radar wave into the obstacle.

Images