# Collision-Free Motion Planning of a Six-Link Manipulator Used in a Citrus Picking Robot
## 3 Theory and Calculation
### Optimization Strategies  
There are a few shortcomings of APF, such as local minimum trap (LMT), target position not reachable (TPNR) problem, etc. In this section, some optimization strategies are proposed. This phenomenon directly leads to two problems that \(\mathbf{F}_{a}\) decreases linearly as \(d_{a}\) decreases and decreases to zero when \(d_{a}\) is equal to zero, but \(\mathbf{F}_{r}\) tends to increase with the decrease of \(d_{r}\) and increases to infinite when \(d_{r}\) is equal to zero. First, if there is an obstacle near the target position, the repulsive force will be far greater than the attractive force. When the object approaches the target position, it will be constantly bounced away by the repulsive force, which leads to path oscillation and even failure, which is the TPNR problem. Second, the torque that the joint of the manipulator can bear is limited, and the infinite repulsion force cannot be realized in the motion planning process. To solve the above two problems and the LMT, this paper optimized the models of potential fields and forces. To avoid the TPNR problem, it is necessary to still have a large gradient close to the target position, to ensure that \(\mathbf{F}_{a}\) is large enough to overcome the influence of \(\mathbf{F}_{r}\) to continuously guide the object to the target position. At the same time, to make the path planning algorithm converge, when \(d_{a}\) is equal to zero, \(\mathbf{F}_{a}\) should also trend to approach zero. For this reason, IAPF adds the Sigmoid function component based on the original attractive potential field calculation model of APF. The attractive potential field and attractive force of IAPF are calculated as  
\[U_{a}=\frac{U_{a,\max}}{1+e^{-(k_{a1}d_{a}-k_{a2})}}+\frac{1}{2}k_{a}d_{a}^{2}, \tag{10}\]  
\[\mathbf{F}_{a}=-\
abla U_{a}=\left(\frac{-k_{a1}U_{a,\max}}{\left(1+e^{-(k_{a1 }d_{a}-k_{a2})}\right)^{2}}e^{-(k_{a1}d_{a}-k_{a2})}-k_{a3}d_{a}\right)\
abla d _{a}, \tag{11}\]  
To avoid the problem of uncontrollable increase of \(\mathbf{F}_{r}\) with the decrease of \(d_{r}\), IAPF changed the original repulsive potential field model. The repulsive potential field model of APF is a piecewise function. It not only needs to be continuous and differentiable at the piecewise junction but also a gradient explosion will occur when \(d_{r}\) is equal to zero. To  
Figure 5: Schematic diagram of forces of IAPF applied on the EC63M manipulator. ensure that the repulsive force of IAPF does not increase infinitely and is still continuous and differentiable with the decrease of \(d_{r}\), the repulsive force of IAPF is changed into an anti-Sigmoid function in this paper. Thus, the function of repulsive force of IAPF not only approaches zero when it is far from the obstacle, but also increases with the decrease of \(d_{r}\), and is still continuous and differentiable. The Fr of IAPF will gradually increase and tend to the maximum repulsive force, and there will not be the problem of repulsive force explosion. The repulsive potential field and repulsive force of IAPF are calculated as  
\[U_{r}=-U_{r,max}d_{r}+\frac{U_{r,max}\log\Bigl{(}e^{k_{r1}d_{r}-k_{2}}+1 \Bigr{)}}{k_{r1}}+k_{r3}, \tag{12}\]  
\[\mathbf{F}_{r}=-\
abla U_{r}=\left\{ \begin{array}{l}\frac{U_{r,max}}{1+e^{ \mathbf{r}_{1}d_{r}-k_{r2}}}\
abla d_{r},d_{r}\leq d_{0}\\ 0,d_{r}>d_{0}\end{array} \right., \tag{13}\]  
To eliminate the influence brought by LMT, IAPF chooses to temporarily add a force \(\mathbf{F}_{t}\) to break the balance of repulsion and attraction when the gradient of the resultant potential field disappears. \(\mathbf{F}_{t}\) not only determines the quality of the result of IAPF but also determines whether IAPF will fall into LMT again. Through the simulation experiments, \(\mathbf{F}_{t}\) has a good effect when it is perpendicular to both \(\mathbf{F}_{a}\) and \(\mathbf{F}_{r}\).