# Optimization Algorithm for 3D Smooth Path of Robotic Arm in Dynamic Obstacle Environments
## 3 Method Description
### Dynamic Path Planning Module
\[Q_{\text{new}}=Q_{\text{near}}+\frac{\vec{d}_{\text{rand}}}{\|\vec{d}_{\text{ rand}}\|} \tag{5}\]  
In each iteration of this algorithm, the new node is the resultant vector of the random sample vector obtained by multiplying the RRT and APF vectors by their respective weights (RRT weight and APF weight). This adjusts the characteristics to expand either in the direction of the random sample or in the direction guided by the APF. Therefore, the above equation can be expressed as Equation (6):  
\[Q_{\text{new}}=Q_{\text{near}}+\left(\alpha\frac{\vec{d}_{\text{rand}}}{\| \vec{d}_{\text{rand}}\|}+\beta\frac{\vec{F}_{\text{total}}}{\|\vec{F}_{\text{ total}}\|}\right) \tag{6}\]  
where \(\alpha\) is the weight factor for RRT, and \(\beta\) is the weight factor for APF. APF is represented by the following formula, utilizing repulsive forces against both static and dynamic obstacles and a driving force towards the goal node. It includes a representation of the attractive vector from \(Q_{\text{end}}\), while \(\vec{F}_{\text{rep,sta}}\) represents the repulsive vector from static obstacles, and \(\vec{F}_{\text{rep,dyn}}\) represents the repulsive vector from dynamic obstacles. \(\vec{F}_{\text{att}}\) can also be expressed by the following formula, where \(V_{a}\) is the maximum attractive force, \(f_{a}\) is the shaping factor, and \(\vec{d}_{\text{att}}\) is the distance vector \(\vec{d}(Q_{\text{end}},Q_{\text{near}})\) from \(Q_{\text{near}}\) to \(Q_{\text{end}}\). If \(Q_{\text{near}}\) is farther from \(Q_{\text{end}}\), the gravitational force increases, making \(\vec{F}_{\text{total}}\) tend towards \(Q_{\text{end}}\). Therefore, the repulsive force function \(\vec{F}_{\text{rep}}\) can be expressed as:  
\[\vec{F}_{\text{total}}=\vec{F}_{\text{att}}+\vec{F}_{\text{rep,sta}}+\vec{F}_{ \text{rep,dyn}} \tag{7}\]  
\[\vec{F}_{\text{att}}=V_{a}\Big{(}1-e^{-\|\vec{d}_{\text{att}}\|_{f_{a}}}\Big{)} \frac{\vec{d}_{\text{att}}}{\|\vec{d}_{\text{att}}\|} \tag{8}\]  
\[\vec{F}_{\text{rep}}=\frac{V_{r}}{\Big{(}1+e^{(\|\vec{d}_{\text{rep}}\|-d_{ \text{att}})f_{r}}\Big{)}}\frac{\vec{d}_{\text{rep}}}{\|\vec{d}_{\text{rep}}\|} \tag{9}\]  
where \(V_{r}\) is the maximum repulsive force, \(f_{r}\) is the shaping factor, and \(\vec{d}_{\text{rep}}\) is the distance vector \(\vec{d}_{\text{rep}}=\vec{d}(Q_{\text{near}},P_{\text{obs}})\) between \(P_{\text{obs}}\) and \(Q_{\text{near}}\), with \(d_{\text{safe}}\) being the safe distance. If \(Q_{\text{near}}\) is closer to \(P_{\text{obs}}\), the repulsive force increases, making \(\vec{F}_{\text{total}}\) move farther away from \(P_{\text{obs}}\). In the above formula, there are two definitions of the same repulsive force function, one for static obstacles and the other for dynamic obstacles. In APF, an exponential function is used to limit the maximum value and generate a smoother curve as it approaches the maximum. Compared to a quadratic function with an extreme value, this design allows for better control of the APF's effect during local replanning. As mentioned in the \(Q_{\text{new}}\) formula in the previous section, \(\alpha\) and \(\beta\) are parameters that control the proportion of RRT and APF, where \(\alpha\) is the weight factor for RRT and \(\beta\) is the weight factor for APF. As shown in Figure 8 below, in an environment with both static and dynamic obstacles, the motion path needs to be replanned due to the presence of dynamic obstacles. If the RRT factor and APF factor remain unchanged during the replanning process, APF will direct the search tree towards areas with more obstaclesnear \(Q_{\text{near}}\), resulting in a decrease in the convergence speed of the area and an increase in replanning time, as shown by the red dashed line in the figure. Therefore, this section explores dynamically adjusting the proportion of RRT and APF during path planning to dynamize the impact of APF.