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 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.