# A Dynamic Multiple-Query RRT Planning Algorithm for Manipulator Obstacle Avoidance
## 4 Dynamic BG-RRT Motion Replanning
### Relay Node  
In order to meet the real-time needs of manipulators working in unknown environments, the relay-node method, which generates a collision position \(q_{\text{relay}}\) according to the motion state of dynamic obstacles and the robot, is proposed. When the initial planning is blocked, \(q_{\text{relay}}\) is calculated according to the information on robot and dynamic obstacles. In the process of moving to \(q_{\text{relay}}\), a new trajectory is repaired in the updated environment. If the robot fails to repair before reaching the \(q_{\text{relay}}\), it will stop and wait until planning is complete. If the robot is affected by dynamic obstacles continuously during the motion, it will recalculate \(q_{\text{relay}}\) according to the current execution trajectory and repeat the above process to complete the task.  
The relay-node method based on path planning is described as shown in Figure 6. The red nodes and yellow nodes represent the \(V\) and \(q_{\text{relay}}\). The robot's current position is set as \(q_{\text{current}}\), thus, \(\theta\) is the angle between the direction of the obstacle and the line from the robot to the current position of obstacles. Set the velocity of robot as \(v_{\text{robot}}\) (\(v_{\text{robot}}\ \leq\ v_{\text{max}}\)), and assume that the robot always moves along with initial planning with the maximum velocity \(v_{\text{max}}\). In this way, the replanning is safe according the \(v_{\text{max}}\) than \(v_{\text{robot}}\). The velocity and motion angle of dynamic obstacles are set as \(v^{\prime}_{\text{obstacle}}=v_{\text{obstacle}}+\Delta v\) and \(\Delta\theta\), respectively, where the \(\Delta v\) and \(\Delta\theta\) are configured according to the dynamic obstacle of uncertain expression by the probability theory. Set the position where the boundary of dynamic obstacle intersects the initial planning, also near to \(q_{\text{current}}\) as \(q_{\text{obstacle}}\). Recording the sequence of path points, the \(q_{\text{relay}}\) is calculated by the binary search BinarySearch(). Therefore, the relay-node \(q_{\text{relay}}\) based on path planning in reference [19] is defined as \(n(q_{\text{relay}})=\text{BinarySearch(k)}+1\), where the \(k\) is configured as:  
\[k=\frac{d(q_{\text{obstacle}},q_{\text{current}})\cdot v_{\text{max}}}{v_{ \text{max}}-(v_{\text{obstacle}}+\Delta v)\cos(\theta+\Delta\theta)} \tag{4}\]  
Figure 5: The flowchart of the obstacle-avoidance problem.  
where \(d(q_{\text{obstacle}},q_{\text{current}})\) is the distance between \(q_{\text{obstacle}}\) and \(q_{\text{current}}\). \(q_{\text{relay}}\) in Equation (4) is much safer than \(q_{\text{relay}}\) is calculated on the line between \(q_{\text{obstacle}}\) and \(q_{\text{current}}\).  
Compared with the limited nodes in path planning, the motion trajectory is more accurate to represent the position of relay node. The dynamic obstacle avoidance problem needs to solve based on motion planning for the manipulator or other robots. As shown in Figure 7, the position of \(q_{\text{relay}}\) is a defined by summing along the execution trajectory as \(q_{\text{relay}}\gets q_{\text{current}}+k\), where the \(k\) is identical with Equation (4). Compared to the above methods, the description of \(q_{\text{relay}}\) based on motion planning is more accurate. At the same time, the calculation of the binary search method is avoided and the success rate is improved.