2.3 Modified Cost Function Calculation of OpenPlanner for Local
Path Planning
During the navigation process, the local planner of OpenPlanner rolls
out several local trajectories. The final trajectory is selected by a
cost computation function that normalizes and sums up three metrics:
priority cost, collision cost, and transition cost. To enhance
navigation robustness in complex greenhouse environments, two additional
cost metrics are incorporated into the local planner in this study:
surface cost and plant density cost. The costs of the two metrics at
timestep \(t\) are denoted as \(\text{Surf}_{t}\) and\(\text{Dens}_{t}\).
Surface cost is utilized to measure the driving cost of a trajectory
across various ground types. The smoother the ground surface, the lower
the cost for the robot to traverse it. To quantify the surface cost of a
candidate trajectory, the point clouds on the trajectory are divided
into numerous cubic grid cells of the same size. Then, a reference plane
is selected, and the vertical distances between all grid cells and this
reference plane are computed. Each distance is denoted as\(\ d_{i}\),
and after normalizing \(d_{i}\), we represent it as the ground flatness
index \(\text{gf}_{i}\), where \(i\) indicates the grid cell’s index.
Finally, the surface cost \(\text{Surf}_{t}\) of the current time can be
obtained using as follows:
\(\text{Surf}_{t}=\ \frac{\sum\text{gf}_{i}}{n}\text{ \ }\) (8)
where n denotes the number of grid cells within the path.
Plant density cost is utilized to measure the driving cost of a
trajectory in regions with dense vegetation. The denser the plants, the
higher the risk of collision with them. To quantify the plant density
cost for a candidate trajectory, the three-dimensional space above the
path is partitioned into uniformly-sized volumetric grid cells.
Subsequently, the existence of point cloud data in each grid cell is
evaluated, represented by the variable\(\ \text{pc}_{j}\),
where j corresponds to the index of grid cells. If point
cloud data exists within a grid cell,\(\ \text{pc}_{j}\) is assigned a
value of 1. Otherwise, the value is set to 0. The plant density cost\(\text{Dens}_{t}\) can be determined as follows:
\(\text{Dens}_{t}=\frac{\sum\text{pc}_{j}}{n}\) (9)
where \(n\) signifies the total number of grid cells within the path.
The total cost of a candidate trajectory, considering the five
associated cost variables, can be calculated through the following
additive function:
\(C_{t}=\ \text{Pri}_{t}\ +\ \text{Col}_{t}\ +\ \text{Trans}_{t}\ +\ \text{Surf}_{t}+\ \text{Dens}_{t}\) (10)
In this equation, \(C_{t}\) corresponds to the total cost of the
candidate trajectories. \(\text{Pri}_{t}\) signifies the priority cost,\(\text{Col}_{t}\) the collision cost, \(\text{Trans}_{t}\ \) the
transition cost, \(\text{Surf}_{t}\) the surface cost, and\(\text{Dens}_{t}\) the plant density cost. All of these costs are
normalized. By comparing the total cost of all candidate trajectories,
the optimal path can be selected as the one with the lowest cost.