# Position Selection and Collision-Free Path Planning for Fruit Picking Robots
## 3 Collision-Free Path Planning
### Model Predictive Artificial Potential Field  
This paper introduces model predictive artificial potential field (MPAPF), a method that formulates a nonlinear optimization problem to plan paths in joint space using APF. This MPAPF path planning method has the advantage of avoiding the problem of generated paths exiting the manipulator workspace or not accurately considering joint limits. Additionally, collision checks are performed for joint positions as well, ensuring that all points of the manipulator do not collide with the obstacle. Using the APF values of states for multiple steps reduces the risk of local minima compared to APF methods. In the case of APF, the path may get stuck in a local minimum if there is no nearby position with a lower potential value than the current one. However, MPAPF finds a path by minimizing the cumulative potential value over multiple steps, allowing it to move to a higher-potential position if that helps it to escape from a local minimum. Figure 7: Inverse Reachability map without assigning negative values: (**a**–**e**) for each target and (**f**) for the aggregate target. The states at any discrete time instant within the prediction horizon \(n\) with sampling time \(T_{s}\) and the angular velocity at time instant \(k\), \(u_{k}\), are expressed as follows:  
\[ \begin{split}\theta_{k+1}&=\theta_{k}+T_{s}\mathbf{u}_{k} \\ \theta_{k+2}&=\theta_{k+1}+T_{s}\mathbf{u}_{k+1}=\theta_{ k}+T_{s}(\mathbf{u}_{k}+\mathbf{u}_{k+1})\\ &\vdots\\ \theta_{k+n}&=\theta_{k}+T_{s}(\mathbf{u}_{k}+\mathbf{u}_{k+1}+ \cdots+\mathbf{u}_{k+n-1})\end{split}  \tag{3}\]  
Equation (3) can be simply expressed as follows:  
\[\hat{\theta}_{k+1}=\mathbf{A}\theta_{k}+\mathbf{B}\hat{\mathbf{u}}_{k} \tag{4}\]  
where  
\[ \begin{split}\hat{\mathbf{\theta}}_{k+1}&=\left[ \begin{array}{cccc}\theta_{k+1}&\theta_{k+2}&\cdots&\theta_{k+n}\end{array} \right]^{\text{T}}\\ \hat{\mathbf{u}}_{k}&=\left[\begin{array}{cccc}\mathbf{u}_{k}&\mathbf{u}_{k+1}&\cdots&\bm {u}_{k+n-1}\end{array}\right]^{\text{T}}\\ A&=\left[\begin{array}{cccc}\mathbf{I}_{m}&0&\cdots&0\\ 0&\mathbf{I}_{m}&\cdots&0\\ \vdots&\vdots&\vdots&\vdots\\ 0&0&\cdots&\mathbf{I}_{m}\end{array}\right]\\ B&=T_{s}\left[\begin{array}{cccc}\mathbf{I}_{m}&0&\cdots&0\\ \mathbf{I}_{m}&\mathbf{I}_{m}&\cdots&0\\ \vdots&\vdots&\vdots&\vdots\\ \mathbf{I}_{m}&\mathbf{I}_{m}&\cdots&\mathbf{I}_{m}\end{array}\right]\end{split}  \tag{5}\]  
where \(m\) is the number of joints, \(\theta_{k}\in\mathbb{R}^{m}\) is the joint angle at time instant \(k\) and \(\mathbf{I}_{m}\in\mathbb{R}^{m\times m}\) is an identity matrix. If \(\hat{\mathbf{\theta}}_{k+1}\) are known, the positions of the joints, \(\mathbf{P}_{j}(\hat{\theta}_{k+1})\), can be determined by the forward kinematics. A nonlinear minimization problem can be formulated as follows using the joint positions and angles, along with the positions of the obstacles:  
\[\min_{\mathbf{b}_{k}}\ \alpha f(\hat{\mathbf{\theta}}_{k+1})+\beta\varsigma(\hat{\mathbf{ \theta}}_{k+1}) \tag{6}\]  
where  
\[ \begin{split} f(\hat{\mathbf{\theta}}_{k+1})&=\sum_{i=1}^{ \alpha}\bar{\Omega}_{rep,i}(\mathbf{P}_{j}(\hat{\mathbf{\theta}}_{k+1}))\\ \bar{\Omega}_{rep,i}(\mathbf{P}_{j}(\hat{\mathbf{\theta}}_{k+1}))& =\sum_{j=1}^{m}\mathrm{U}_{rep,i}(\mathbf{P}_{j}(\hat{\mathbf{\theta}}_{k+1}))\\ g(\hat{\theta}_{k+1})&=|\hat{\mathbf{\theta}}_{k+1}-\mathbf{C} \cdot\mathbf{\theta}_{target}|\\ \mathbf{C}=\left[\begin{array}{cccc}\mathbf{I}_{m}&\mathbf{I}_{m}&\cdots&\mathbf{I}_{m} \end{array}\right]^{\text{T}}\in\mathbb{R}^{m\times m}\end{split} \]  
where \(\alpha\) and \(\beta\) are constants representing the gains of each function, \(f\) is the obstacle avoidance function, \(g\) is the target attraction function, and \(o\) is the number of obstacles. \(\theta_{target}\in\mathbb{R}^{m}\) denotes joint angles when reaching the target from the selected position. Function \(f\) uses a repulsive potential function, Equation (2), to avoid obstacles and \(g\) acts as an attractive potential to move all joints to the target angle.