## 4 Improved Algorithms
### I-RRT Algorithm
#### 4.3.4 Adaptive Step Size  
The classic algorithm has a fixed step length for expansion; approaching obstacles cannot be avoided well, causing collisions and occupying a large amount of memory, and the generated step length grows towards a random point without directionality. Both of these issues lead to a reduction in the efficiency of the algorithm, so an adaptive step size strategy is adopted [26]. When the minimum distance between \(\mathrm{q_{nearest}}\) and the obstacle is greater than the step size, the obstacle is marked as \(\mathrm{s}\), the idea of gravity in the APF method is introduced into the adaptive step size of the RRT algorithm [27], and the random tree is guided to grow towards the target. On the basis of the original RRT algorithm growing towards random points, the step component G(n) towards the target is added so that the new node has a tendency to deviate towards the target point. The formula of step F(n) is defined as follows:  
\[\text{F}(n)\ =\ R(n)\ +\ G(n) \tag{10}\]  
where F(n) represents the traction step length of the nth node, R(n) represents the traction step length of the nth node by a random point, and G(n) represents the traction step length of the nth node by the target object. The gravitational potential energy \(U\) of the target point to the nearest node is known by the APF method and is defined as follows:  
\[U=\frac{1}{2}K_{p}*\left\|x_{goal}-\ x_{near}\right\|^{2} \tag{11}\]  
where \(k_{p}\) represents the gravitational coefficient and \(\left\|x_{goal}-x_{near}\right\|\) represents the absolute value of the Euclidean distance between the position vector \(x_{goal}\) of the target point and the position vector \(x_{near}\) of the nearest node. Then, the gravitational force \(G\) is the derivative of the gravitational potential energy \(U\), namely,  
\[G=K_{p}*\left\|x_{goal}-x_{near}\right\| \tag{12}\]  
Then  
\[G(n)=\rho*k_{p}\frac{x_{goal}-x_{near}}{\left\|x_{goal}-\ x_{near}\right\|} \tag{13}\]  
where \(\rho\) represents the step length of random growth; then, \(R(n)\) can also be deduced as  
\[R(n)=\rho*\frac{x_{rand}-x_{near}}{\left\|x_{rand}-x_{near}\right\|} \tag{14}\]  
where \(\left\|x_{rand}-x_{near}\right\|\) represents the absolute value of the Euclidean distance between the position vector \(x_{rand}\) of the random point and the position vector \(x_{near}\) of the nearest node. By inserting Equations (13) and (14) into Equation (10) to obtain the traction step length of the nearest node as Equation (15), defined as \(F_{1}(n)\), the following formula is obtained:  
\[F_{1}(n)=\rho*\frac{x_{rand}-x_{near}}{\left\|x_{rand}-x_{near}\right\|}+\rho* k_{p}\frac{x_{goal}-x_{near}}{\left\|x_{goal}-\ x_{near}\right\|} \tag{15}\]  
When the minimum distance to the obstacle is less than the step length, two situations are possible. Case 1: First, a virtual new node is generated according to the above steps, and whether the distance between the virtual new node and obstacle s is less than the distance between the nearest node and the obstacle is evaluated. If the distance is less than the distance between the nearest node and the obstacle, then the virtual new node has a tendency to approach the obstacle is proven. Then, the virtual new node is removed, the step length of the new node growth is changed, and the step length is reduced on the basis of the step length \(F_{1}(n)\), which is defined as the step length \(F_{2}(n)\). The formula is as follows  
\[F_{2}(n)=\frac{\text{dist}_{2}}{\text{dist}_{1}}(\rho*\frac{x_{rand}-x_{near} }{\left\|x_{rand}-x_{near}\right\|}+\rho*k_{p}\frac{x_{goal}-x_{near}}{ \left\|x_{goal}-x_{near}\right\|}) \tag{16}\]  
where dist\({}_{1}\) represents the distance between the nearest node and obstacles and dist\({}_{2}\) represents the distance between the virtual new node and obstacles. Whether the node collides with an obstacle is evaluated. If it collides, a random node is regenerated; if there is no collision, a new node is added to the random tree.