Real-time local trajectory optimization of an orchard robot using improved ALTRO

Robots have been widely applied into agricultural production in an orchard. Real-time and local trajectory can be optimized for the safe and stable autonomous operation of orchard robots. However, the conventional Augmented Lagrangian TRajectory Optimizer (ALTRO) algorithm can easily fall into the numerical sickness for the difficult balance of weight, due mainly to the low efficiency of iteration. In this study, a real-time and local trajectory optimization was presented for the orchard robot using improved ALTRO. The global reference trajectory was obtained to input into the robot. The accelerated augmenting Lagrange algorithm was firstly adopted to replace the augmented Lagrange link of the original. The multiplier iteration strategy of ALTRO algorithm was improved to iteratively compute the Lagrangian multipliers with the higher convergence rate. More stability was also achieved in the accelerated convergence. Secondly, the multiplier was obtained to update each iteration within the feasible scope. The numerical stability was promoted to add the feasible domain projection of multiplier after the introduction of the Lagrange link. The updated value of multiplier was projected onto the feasible domain space, in order to ensure the rationality of the multiplier update. The numerical stability of the algorithm was improved to fully avoid the numerical ill-condition that caused by many iterations. Finally, the adaptive scaling factor was introduced using trajectory time step, in order to dynamically adjust the weight of the distance from the end point. Multiple cost weights were then balanced in the improved ALTRO algorithm. Excessive attention was removed to pose the target end point. The improved algorithm was responded better to the local obstacles during trajectory optimization.Based on the same reference path and configuration parameters, in the multi-obstacle simulation scenario, the calculation time of the proposed algorithm is reduced by 32.76% compared with the original ALTRO algorithm, while in the real experiment, the calculation time of the proposed algorithm is reduced by 67.80% compared with the original algorithm. Moreover, the trajectory obtained by the optimized algorithm in this paper is reduced by 10.59%, 2.98% and 10.17% respectively in terms of the maximum curvature, average curvature and standard deviation of curvature compared with the original algorithm. In terms of the maximum change rate, average change rate and standard deviation of change rate of the remaining state quantities and control quantities, Compared with the original algorithm, the improved algorithm decreases by 14.19%, 3.61% and 10.69% respectively, and the curvature performance and control changes of the trajectory are smoother, which can provide a good reference for the robot.