With the continuous growth in the number of electric vehicles, the speed of charging facility construction has not kept pace with user requirements, especially in public parking lots and highway service areas. The scarcity of charging piles often leads to long queues and unbalanced charging resource allocation. In order to alleviate this pressure and improve charging convenience, an automatic charging manipulator that supports multi-station charging from a single pile has become an effective solution. In real parking environments, however, open spaces are easily occupied by dynamic obstacles such as improperly parked electric vehicles, stacked goods, or nearby pedestrians. Such obstacles impose serious constraints on the motion of the automatic charging manipulator. Consequently, obstacle avoidance and trajectory planning constitute one of the most critical issues that must be solved for full-scenario automatic charging of electric vehicles. This work focuses on a six-degree-of-freedom serial manipulator used for automatic charging of electric vehicles. We propose an improved Rapidly-exploring Random Tree (RRT) algorithm with global adaptive step size and hybrid sampling. We then combine fifth-order B-spline curves and genetic algorithms to achieve time-optimal trajectory planning. Finally, a prototype experimental platform is built to validate the obstacle avoidance trajectory planning in a simulated automatic charging scenario.
The widespread adoption of electric vehicles has brought significant attention to charging infrastructure. Public charging stations in many dense urban areas are overwhelmed by the fast-growing number of electric vehicles. The charging process itself is still dominated by manual plug-in operations, and the charging cable of direct-current piles is heavy and stiff. This creates usability problems for many drivers. An automated charging manipulator can eliminate human intervention, improve the convenience of charging, and increase the utilization rate of charging facilities for electric vehicles. Accurate motion control and intelligent obstacle avoidance are therefore essential prerequisites for reliable operation of such systems.
The trajectory planning algorithm for a manipulator determines whether the charging gun can reach the charging port of the electric vehicle safely and smoothly. The challenging operating conditions of public parking spaces demand a path planner that can handle narrow passages, unexpected obstacles, and stringent time constraints. We first design an automatic charging manipulator that fits typical parking layouts and vehicle dimensions. Then we analyze its kinematics to establish the mapping between joint motion and the end-effector pose. Based on the workspace analysis, we build an obstacle avoidance path planner using a modified RRT algorithm. The planner produces a sequence of collision-free waypoints in joint space. These waypoints are subsequently used to generate a five-order B-spline trajectory with continuous velocity and acceleration. A genetic algorithm is applied to minimize the total travel time while respecting joint velocity, acceleration, and jerk limits. Finally, we implement the complete planning framework on a prototype collaborative robot to demonstrate its effectiveness in a simulated charging station scenario.
1. Introduction
The electrification of transportation is accelerating worldwide because of carbon-neutral targets, strict environmental regulations, and the technological refinement of electric vehicles. Many governments have announced ambitious plans to phase out internal combustion engine vehicles, and automobile manufacturers have followed with corresponding strategies. The global population of electric vehicles is increasing at a remarkable pace, especially in the Chinese market where the fleet of new-energy vehicles has exceeded twelve million units. Although this growth is encouraging, charging infrastructure lags far behind. Statistics indicate that the total number of public charging piles is insufficient for the current number of electric vehicles. Fast charging piles are often concentrated in a few regions, while many parking lots remain unserved. Such a mismatch greatly limits the convenience of owning an electric vehicle.
Automatic charging manipulators offer a promising prospect because they allow one charging station to serve multiple parking spaces. A mobile or rail-mounted robotic arm can move to the designated parking spot, locate the charging port, and plug in the charging connector without human assistance. Early prototypes developed by several companies have already demonstrated the feasibility of this approach. A snake-like charging robot designed by an American electric vehicle manufacturer uses visual feedback to align with the vehicle charging socket. Later systems based on an industrial lightweight arm adopted a vision-guided method to identify the charging port position. In parallel, several research groups have explored rail-guided robotic arms that enlarge the workspace along the parking row. These systems successfully reduce the physical effort required from the user and shorten the waiting time for electric vehicle charging.
However, the open parking environment prevents a robotic charger from using a simple preplanned motion. A car may be parked slightly to the left or right, the parking lot may contain temporary goods, and other electric vehicles may be passing through the aisle. Consequently, the manipulator must plan a safe trajectory around obstacles while maintaining sufficient accuracy to insert the charging plug into the port. This task can be divided into two subproblems: obstacle-avoidance path planning and time-optimized trajectory planning. Path planning seeks a geometrically feasible and collision-free route in the configuration space. Trajectory planning assigns a temporal law to that geometric route so that the motion is smooth, stable, and efficient. The research reported here addresses both aspects in sequence, focusing on an electric vehicle charging manipulator with six rotary joints.
The literature reports many algorithms for robot motion planning. Rapidly-exploring Random Tree (RRT) is widely used because of its probabilistic completeness and easy implementation in high-dimensional configuration spaces. The basic RRT algorithm randomly samples the configuration space, searches for the nearest node in the existing tree, and extends a fixed step toward the sample. This process repeats until the goal configuration is reached. Although simple, the standard RRT has three main drawbacks. First, the uniform random sampling lacks goal bias, causing a large number of unnecessary expansions in regions far from the goal. Second, a fixed extension step size cannot adapt to the local density of obstacles: a large step often produces collisions in narrow passages, while a small step wastes time in open areas. Third, the final path is typically jagged and not optimized. Several improved variants have been proposed to mitigate these problems. The goal-biased RRT (GB-RRT) biases the sampling towards the target with a certain probability, which significantly accelerates convergence in simple environments. The bidirectional RRT (Bi-RRT) grows two trees from both the start and the goal, reducing the search time through concurrent exploration. The RRT* algorithm introduces a rewiring mechanism to asymptotically minimize the path cost, but it usually needs more computation. Many further enhancements combine heuristic steering, lazy collision checks, or machine-learning-based sampling. For obstacle avoidance of a charging manipulator, we need an approach that achieves both low path cost and short planning time in cluttered charging environments.
Trajectory planning research has also developed various interpolation and optimization methods. Polynomial interpolation is a classic approach. Cubic polynomials ensure continuity of position and velocity, but the acceleration profiles may contain discontinuous corners. Higher-order polynomials provide smoother jerk profiles but are more computationally demanding. The 5-3-5 hybrid spline combines the advantages of cubic and quintic polynomials, yet the transition points may still produce unwanted oscillations in complex paths. Another popular method is B-spline interpolation, which has local support and favorable continuity properties. A fifth-order B-spline guarantees continuous position, velocity, and acceleration, making it suitable for producing a smooth joint trajectory. To optimize the execution time while satisfying joint limits, one can formulate an objective function representing the total duration and solve it using evolutionary algorithms. Genetic algorithms are natural candidates because they handle non-linear, non-convex optimization and do not require derivative information. In the present study, fifth-order B-spline interpolation is used to construct the trajectory through the waypoints generated by the improved RRT planner, and a genetic algorithm is used to minimize the total charging manipulation time under the kinematic constraints of each joint.
The remainder of this paper is organized according to the design and validation flow. Section 2 describes the mechanical design of an automatic charging manipulator for electric vehicles, including the structural dimensions, modular joints, and actuator selection. Section 3 establishes the kinematics model of the six-degree-of-freedom arm and analyzes its workspace. Section 4 details the proposed improved RRT algorithm with hybrid sampling and adaptive step size, and compares it with existing planners in two- and three-dimensional spaces. Section 5 presents the fifth-order B-spline trajectory planning, the genetic-algorithm-based time optimization, and the prototype experiment. Section 6 summarizes the main conclusions and future work.
2. Structural Design of the Automatic Charging Manipulator for Electric Vehicles
The design of the manipulator begins with a detailed analysis of the automatic charging scenario. In a typical charging lot, the mechanical arm is mounted on a sliding rail along the side of the parking spaces. When an electric vehicle stops in a reserved slot, the arm travels to the vicinity of the vehicle, locates the charging port, and inserts the connector. The parking slot dimensions, vehicle dimensions, charging port height, and the required reachable area all influence the choice of the arm length and joint configuration. Survey results on mainstream electric vehicles show that the average distance from the charging port to the near end of the vehicle is 667 mm, and the average height of the charging port above the ground is 890 mm. The distance between the base of the charger and the parking stall boundary is typically about 365 mm, while the average gap between the parking boundary and the vehicle bumper is 462 mm. Based on these data, the total length of the manipulator is set to 2300 mm. Since most charging ports of electric vehicles are located at a height close to 800 mm, the base of the manipulator is placed at 800 mm above the ground to match the port height with a comfortable posture.
To provide enough flexibility for obstacle avoidance and to adapt to varying port orientations, the manipulator adopts a six-degree-of-freedom articulated configuration, usually referred to as a 6R serial arm. The first three rotary joints are responsible for positioning the end-effector, whereas the last three joints determine the orientation of the charging gun. This configuration resembles most industrial robot arms and is sufficiently dexterous to approach the charging socket from various directions. The structural parameters and joint limits are selected by considering typical parking layouts and safety constraints. Table 1 lists the joint motion ranges used for the charging manipulator studied in this work.
| Joint | Range (deg) |
|---|---|
| Joint 1 | -165 to 180 |
| Joint 2 | -135 to 80 |
| Joint 3 | -125 to 125 |
| Joint 4 | -170 to 170 |
| Joint 5 | -120 to 120 |
| Joint 6 | -180 to 180 |
The modular joint design is adopted to simplify assembly and maintenance. Each joint module consists of a motor, a harmonic reducer, an encoder, a brake, and a structural housing. The output shaft of the harmonic reducer is connected with a standardized flange so that the modules can be quickly assembled in different orientations. The links connecting consecutive joints are also standardized. A complete assembly of the designed arm includes six rotary joint modules, connecting links, and a mounting flange for the charging gun at the end-effector.
The gravitational load at each joint is calculated under the worst-case horizontal posture. The weight of each link and joint module, the payload at the end-effector, and the length of each link determine the required torque. The torque requirement for joint i is expressed by
$$M_i = \frac{G_i g L_i}{2} + T_i g L_i,$$
where \(G_i\) is the total weight of the arm components after joint i, \(T_i\) is the payload mass, \(L_i\) is the effective length, and \(g\) is the gravitational acceleration. The calculated required torques and motor powers are given in Table 2.
| Joint | Payload (kg) | Motor weight (kg) | Link weight (kg) | Link length (m) | Required torque (N·m) | Required power (W) |
|---|---|---|---|---|---|---|
| 1 | 1.0 | 7.0 | 1.29 | 0.39 | 14.92 | 94 |
| 2 | 1.0 | 3.5 | 0.57 | 0.80 | 182.52 | 1147 |
| 3 | 1.0 | 3.0 | 0.18 | 0.20 | 9.02 | 57 |
| 4 | 1.0 | 2.0 | 0.53 | 0.75 | 55.98 | 352 |
| 5 | 1.0 | 2.0 | 0.14 | 0.20 | 0.29 | 2 |
| 6 | 1.0 | 1.0 | 0.21 | 0.30 | 6.19 | 39 |
Based on the calculated torques, servo motors and harmonic reducers are selected. Servo motors provide smooth speed control, high accuracy, and low noise, which are preferred for precise charging operations. The selected drive units are summarized in Table 3. The maximum motor power is 1500 W in joint 2, and the minimum is 50 W in joints 5 and 6. All reducers use a 50:1 ratio.
| Joint | Motor | Rated power (W) | Reducer | Reduction ratio |
|---|---|---|---|---|
| 1 | Servo motor | 100 | Harmonic LSS-20 | 50 |
| 2 | Servo motor | 1500 | Harmonic LSS-25 | 50 |
| 3 | Servo motor | 100 | Harmonic LSS-20 | 50 |
| 4 | Servo motor | 400 | Harmonic LSS-25 | 50 |
| 5 | Servo motor | 50 | Harmonic LSS-14 | 50 |
| 6 | Servo motor | 50 | Harmonic LSS-14 | 50 |
3. Kinematic Analysis of the Automatic Charging Manipulator
The kinematic analysis of the automatic charging manipulator establishes the relationship between the joint angles and the position/orientation of the charging gun. To describe the pose of a rigid body, we use position vectors and rotation matrices. The modified Denavit-Hartenberg (D-H) convention is adopted to construct the link coordinate frames. According to the modified D-H convention, the transformation matrix from coordinate frame i-1 to frame i is given by
$$T_i^{i-1} = \mathrm{Rot}(x, \alpha_{i-1}) \, \mathrm{Trans}(x, a_{i-1}) \, \mathrm{Rot}(z, \theta_i) \, \mathrm{Trans}(z, d_i),$$
which can be expanded as
$$T_i^{i-1} = \begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 & a_{i-1} \\ \sin\theta_i\cos\alpha_{i-1} & \cos\theta_i\cos\alpha_{i-1} & -\sin\alpha_{i-1} & -d_i\sin\alpha_{i-1} \\ \sin\theta_i\sin\alpha_{i-1} & \cos\theta_i\sin\alpha_{i-1} & \cos\alpha_{i-1} & d_i\cos\alpha_{i-1} \\ 0 & 0 & 0 & 1 \end{bmatrix}.$$
The D-H parameters of the designed arm are listed in Table 4. These parameters follow the coordinate assignment shown in the established kinematic model.
| Frame | \(\theta_i\) (deg) | \(d_i\) (mm) | \(a_{i-1}\) (mm) | \(\alpha_{i-1}\) (deg) |
|---|---|---|---|---|
| 1 | \(\theta_1\) | 0 | 0 | 0 |
| 2 | \(\theta_2\) | \(d_2\) | 0 | 90 |
| 3 | \(\theta_3\) | 0 | \(a_2\) | 0 |
| 4 | \(\theta_4\) | \(d_4\) | \(a_3\) | 0 |
| 5 | \(\theta_5\) | 0 | 0 | -90 |
| 6 | \(\theta_6\) | 0 | 0 | -90 |
The forward kinematic matrix is obtained by multiplying the individual link transformation matrices:
$$T_6^0 = T_1^0 T_2^1 T_3^2 T_4^3 T_5^4 T_6^5 = \begin{bmatrix} n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix}.$$
The position vector \(\mathbf{p}\) of the charging gun can be expressed in terms of joint variables \(\theta_1, \theta_2, \ldots, \theta_6\). Some components of the rotation matrix and the position vector are summarized in Eq. (1) for brevity:
$$
\begin{aligned}
p_x &= s_1(d_2 + d_4 c_{23} + a_3 c_{23} + a_2 c_2) + a_3 c_1 c_3, \\
p_y &= s_1 a_3 c_2 + a_2 c_1 s_2 + d_4 c_1 c_{23} – d_2 c_1, \\
p_z &= a_2 s_2 + a_3 s_{23} – d_4 c_{23}, \\
n_x &= c_6(c_1 s_{23}s_5 + c_5(c_1 c_{23}c_4 – s_1 s_4)) + s_6(c_1 s_{23}s_4 + s_1 c_4),
\end{aligned}
$$
where the shorthand notation \(s_i = \sin\theta_i\), \(c_i = \cos\theta_i\), \(s_{23} = \sin(\theta_2+\theta_3)\), and \(c_{23}=\cos(\theta_2+\theta_3)\) is used.
For the inverse kinematics, analytical closed-form solutions are preferred to guarantee deterministic and real-time control. Since the designed arm has a spherical wrist, the first three joints determine the wrist position and the last three joints determine the wrist orientation. By multiplying the inverse of the first three transformation matrices on both sides of Eq. (1), the joint angles can be solved sequentially. The derivation is standard and yields
$$\theta_1 = \arctan\!\left(\frac{p_y}{p_x}\right),$$
and the remaining angles can be recovered by algebraic manipulation using equations such as
$$a_3 \cos\theta_3 + d_4 \sin\theta_3 = K,$$
where \(K\) is a constant computed from the known link lengths and the wrist position. The existence of multiple solutions inherent to the inverse kinematics is resolved by selecting the configuration that minimizes joint displacements and ensures the charging gun approaches the electric vehicle charging socket from a safe direction. The inverse kinematics is implemented in MATLAB and tested for several representative charging port positions to verify the correctness.
The reachable workspace of the designed manipulator is computed using the Monte Carlo method. A large number of random joint configurations are generated within the joint limits, and the forward kinematic equation is used to compute the corresponding Cartesian coordinates of the end-effector. A total of 10,000 random configurations are sampled to construct a point cloud of the workspace. The result indicates that the workspace is symmetric with respect to the x-z plane and is large enough to cover the charging ports of mainstream electric vehicles parked in the adjacent slot. The Monte Carlo workspace representation provides a reference boundary for the obstacle avoidance planner and for the kinematic feasibility check during trajectory generation.
4. Obstacle Avoidance Path Planning Using a Hybrid-Sampling Adaptive RRT Algorithm
The path planning module determines a collision-free geometric path for the automatic charging manipulator between the home configuration and the charging port configuration. We first revisit the classical RRT algorithm and its widely used variants, and then introduce our improvements.
4.1 Basic RRT and Its Limitations
The basic RRT algorithm starts from a random tree initialized at the starting configuration \(q_{\text{start}}\). In each iteration, a random configuration \(q_{\text{rand}}\) is uniformly sampled from the configuration space \(C\). The algorithm finds the nearest vertex \(q_{\text{near}}\) in the tree and extends it over a fixed distance \(\Delta q\) toward \(q_{\text{rand}}\). If the new configuration \(q_{\text{new}}\) is collision-free, it is added to the tree. The process is repeated until the tree reaches the goal configuration or a maximum number of iterations is exceeded. The basic RRT is shown in Algorithm 1.
Although RRT ensures probabilistic completeness, it suffers from large randomness, poor goal orientation, and slow convergence in obstacle-rich charging environments. Many variants exist. The goal-biased RRT (GB-RRT) chooses the goal as the sampling target with a probability \(p_0\), namely
$$
q_{\text{rand}} =
\begin{cases}
M \cdot \mathrm{rand}(1,2), & p \ge p_0,\\
q_{\text{goal}}, & p < p_0,
\end{cases}
$$
where \(p\) is a uniform random value in \([0,1]\). The bidirectional RRT (Bi-RRT) expands two trees simultaneously from the start and the goal. In contrast, RRT* introduces a rewiring process that enables asymptotic optimality. However, these methods still have fixed step lengths and do not exploit local environment information.
4.2 Hybrid Sampling Method Based on Environmental Features and Probability Models
To improve sampling efficiency, we design a hybrid sampling method that integrates an adaptive Gaussian mixture model (GMM) with environmental feature perception. The sampling probability density function is a linear combination of a global uniform distribution and several local Gaussian components:
$$p(x) = \lambda U(C) + (1-\lambda)\sum_{k=1}^{K} \omega_k \mathcal{N}(x \mid \mu_k, \Sigma_k),$$
where \(\lambda \in [0,1]\) is the global exploration factor, \(U(C)\) is the uniform distribution over the configuration space, and \(\omega_k\) are the mixture weights that satisfy \(\sum \omega_k = 1\). The centers \(\mu_k\) and covariance matrices \(\Sigma_k\) are updated adaptively based on obstacle density and successful expansion history.
An obstacle sensitivity function is defined to estimate the local obstacle density:
$$\rho(x) = \frac{1}{N_b} \sum_{i=1}^{N_b} \exp\left(-\frac{\|x – o_i\|^2}{2\sigma_o^2}\right),$$
where \(o_i\) are points on obstacle surfaces and \(\sigma_o\) is the perception radius. A lower value of \(\rho(x)\) implies clearer space. We further compute a traversability coefficient \(\alpha_k\) for the k-th Gaussian component by integrating the free-space fraction over its effective region:
$$\alpha_k = 1 – \frac{1}{V_k}\int_{V_k} \rho(x) dx,$$
where \(V_k\) is the volume of the effective region of the k-th Gaussian. The path growth benefit factor \(\beta_k\) measures the success rate of previous expansions in that region:
$$\beta_k = \frac{N_{\text{success}}^{(k)}}{N_{\text{attempt}}^{(k)} + \epsilon},$$
where \(N_{\text{success}}^{(k)}\) and \(N_{\text{attempt}}^{(k)}\) are the numbers of successful and attempted expansions for the k-th region, respectively, and \(\epsilon\) is a small constant that avoids division by zero. The mixture weights are then updated with a dual exponential smoothing rule:
$$\omega_k^{(t+1)} = \gamma \frac{\alpha_k \beta_k}{\sum_{i=1}^{K}\alpha_i \beta_i} + (1-\gamma)\omega_k^{(t)},$$
where \(\gamma \in (0,1)\) is the learning rate. This mechanism raises the sampling probability in free, growth-promoting areas while suppressing ineffective sampling near dense obstacles.
The covariance matrix for each Gaussian component is also updated according to the obstacle density gradient. The density gradient \(\nabla \rho(\mu_k)\) determines the direction in which obstacle concentration increases. To make the sampling robust, the covariance is made anisotropic:
$$\Sigma_k = \sigma_{\min}^2 I + (\sigma_{\max}^2-\sigma_{\min}^2)\frac{\nabla \rho(\mu_k)\nabla \rho(\mu_k)^T}{\|\nabla \rho(\mu_k)\|^2},$$
where \(\sigma_{\min}\) and \(\sigma_{\max}\) are the lower and upper bounds of the sampling standard deviation. In this way, the Gaussian component compresses its sampling in the obstacle direction and expands along the tangential clearance direction. The centers \(\mu_k\) are moved toward regions that have produced successful samples using a mean-shift update formula:
$$\mu_k^{(t+1)} = \frac{\sum_{i=1}^{M}x_i G(\|x_i-\mu_k^{(t)}\|/\sigma_k)}{\sum_{i=1}^{M}G(\|x_i-\mu_k^{(t)}\|/\sigma_k)},$$
where \(G\) is the Epanechnikov kernel and \(x_i\) are the most recent \(M\) successful expansion configurations. Through these coupled updates, the proposed hybrid sampling method maintains an appropriate balance between exploration and exploitation in the automatic charging environment.
4.3 Global Adaptive Step Size
Another key improvement is the global adaptive step size. Instead of using a fixed step length throughout the tree expansion, the proposed algorithm estimates the local complexity of the environment and adjusts the step size continuously. The local environment complexity is modeled as the obstacle density around the candidate new node:
$$D(q) = \sum_{i=1}^{N_{\text{obs}}} \frac{1}{N_{\text{obs}}} \exp\left(-\frac{\|q – o_i\|^2}{2r^2}\right),$$
where \(N_{\text{obs}}\) is the number of obstacle points within a sensing radius and \(r\) is the safety radius of the manipulator end-effector. The step size \(\Delta s\) is computed by a nonlinear mapping using the hyperbolic tangent function:
$$\Delta s(q) = s_{\max} – (s_{\max}-s_{\min})\tanh(D(q)/D_0),$$
where \(s_{\max}\) and \(s_{\min}\) are the maximum and minimum allowed step lengths, and \(D_0\) is a scaling factor. In an open region, \(D(q)\) tends to zero and \(\Delta s\) approaches \(s_{\max}\), enabling fast exploration. In a cluttered region, \(D(q)\) becomes large and \(\Delta s\) decreases to \(s_{\min}\), allowing fine collision-free movement. This scheme eliminates the conflicting requirements of coarse and fine steps and improves the robustness of the planner in diverse charging parking lots.
The minimum step size also respects the joint velocity limit. Given the maximum angular velocity \(\dot{\theta}_{\max}^{(j)}\) of each joint, the feasible Cartesian step must satisfy
$$\Delta s_{\min} \le \min_j \dot{\theta}_{\max}^{(j)} \Delta t \cdot \kappa,$$
where \(\Delta t\) is the control cycle and \(\kappa\) is a safety factor. The final step length is then smoothed by a first-order filter:
$$\Delta s_{\text{new}} = \lambda_s \Delta s_{\text{calc}} + (1-\lambda_s)\Delta s_{\text{prev}},$$
with \(\lambda_s \in [0,1]\) to avoid abrupt switching. This adaptive step size accelerates convergence in open spaces while preserving the ability to pass through narrow gaps needed by electric vehicle charging installations.
4.4 Heuristic Convergence Strategy
The third improvement is a heuristic line-detection strategy that accelerates the termination condition. When the random tree has expanded sufficiently close to the goal \(q_{\text{goal}}\), the algorithm checks whether any existing tree node \(x\) can connect to \(q_{\text{goal}}\) by a straight-line path without collision. If the line segment between \(x\) and \(q_{\text{goal}}\) lies entirely in the free space, the goal can be immediately added to the tree and the search terminates. To avoid frequent full-length collision checks, a dynamically sized detection radius is defined as
$$r_{\text{detect}}(t) = \eta \| x_{\text{tree}}^{\text{centroid}}(t) – q_{\text{goal}} \|,$$
where \(x_{\text{tree}}^{\text{centroid}}(t)\) is the geometric centroid of the current tree and \(\eta \in (0,1)\) is a coefficient. As the tree centroid approaches the goal, the detection radius shrinks and the number of candidate nodes to check decreases. A spatial hashing grid is used to access the nodes inside a neighborhood of \(q_{\text{goal}}\) in constant time. The heuristic significantly reduces the number of RRT iterations required in unobstructed or partially open spaces, making the algorithm efficient for the final approach to the electric vehicle charging port.
4.5 Simulation Results and Comparisons
We compare the proposed hybrid-sampling adaptive RRT algorithm with the basic RRT, GB-RRT, Bi-RRT, and RRT* in two test environments. The first environment is a two-dimensional map with dimensions \(100 \times 100\). The start point is at \((10,90)\) and the goal is at \((80,20)\). The parameters for the proposed algorithm are set to \(\lambda = 0.4\), \(k = 100\), \(w = 0.15\), and the base step size is 5. Each algorithm is run 50 times and the results are averaged. The quantitative results of the two-dimensional benchmark are shown in Table 5.
| Algorithm | Time (s) | Path cost (m) |
|---|---|---|
| RRT | 10.58 | 169.36 |
| GB-RRT | 4.45 | 147.24 |
| Bi-RRT | 3.27 | 164.56 |
| RRT* | 11.78 | 133.12 |
| Hybrid-sampling adaptive RRT | 1.49 | 132.24 |
The results indicate that the proposed method yields the shortest average planning time and the lowest path cost among all methods. Relative to basic RRT, the path cost is reduced by 21.9% and the planning time is shortened by 85.9%. Relative to RRT*, the path cost is nearly identical, but the planning time is about 87.4% shorter. The advantage arises because the multi-Gaussian sampling focuses the generation of candidates near feasible clearance channels, while the adaptive step size reduces the number of expansion steps in open areas.
In the second validation, a three-dimensional cubic map with dimensions \(600 \times 600 \times 600\) is used, with the start at \((50,50,50)\) and the goal at \((500,500,500)\). The base step length is set to 30. The results are listed in Table 6.
| Algorithm | Time (s) | Path cost (m) |
|---|---|---|
| RRT | 2.46 | 1217.26 |
| GB-RRT | 0.58 | 986.12 |
| Bi-RRT | 0.56 | 1150.45 |
| RRT* | 3.57 | 889.87 |
| Hybrid-sampling adaptive RRT | 0.48 | 903.56 |
In the three-dimensional environment, the proposed algorithm maintains a balanced performance: the path cost is close to the shortest value obtained by RRT*, while the planning time is more than seven times smaller. The explicit integration of obstacle density and expansion success probability prevents the tree from getting trapped in dead ends, which is particularly useful when the charging manipulator must navigate around the rear mirrors or pillars of electric vehicles. The simulation study demonstrates that the improved planner is both efficient and reliable for automatic charging tasks.
5. Trajectory Planning Based on Fifth-Order B-Splines and Genetic Algorithm
The path planner generates a sequence of discrete waypoints in the joint space. To drive the manipulator smoothly, these waypoints must be converted into continuous functions of time. In this section, we construct joint trajectories with fifth-order B-spline interpolation and then optimize the time allocation using a genetic algorithm.
5.1 Fifth-Order B-Spline Trajectory Construction
A fifth-order B-spline curve is expressed as
$$p(u) = \sum_{i=0}^{n} d_i N_{i,5}(u),$$
where \(d_i\) are the control points and \(N_{i,5}(u)\) are the fifth-order B-spline basis functions defined recursively:
$$N_{i,0}(u) = \begin{cases}1, & u_i \le u < u_{i+1},\\ 0, & \text{otherwise},\end{cases}$$
$$N_{i,k}(u) = \frac{u-u_i}{u_{i+k}-u_i} N_{i,k-1}(u) + \frac{u_{i+k+1}-u}{u_{i+k+1}-u_{i+1}} N_{i+1,k-1}(u).$$
The knot vector is selected using the cumulative chord length method to match the distribution of the waypoints. For a sequence of \(m+1\) joint waypoints, we require \(m+4\) control points. Since the interpolation conditions supply \(m+1\) equations, six additional boundary conditions are imposed by the starting and ending velocity and acceleration. In this work, the charging manipulator starts and stops with zero velocity and zero acceleration. Thus, we have
$$p(0) = \theta_0,\quad p(1) = \theta_f,$$
$$\dot{p}(0) = v_0,\quad \dot{p}(1) = v_f,$$
$$\ddot{p}(0) = a_0,\quad \ddot{p}(1) = a_f.$$
Solving the linear system gives all control points. Afterwards, the joint position, velocity, acceleration, and jerk profiles can be evaluated at any time instant using the B-spline derivatives.
For the trajectory planning experiments, a representative collision-free path around an obstacle in the charging environment is selected. The joint waypoints used for the six joints are listed in Table 7.
| Waypoint | Joint 1 | Joint 2 | Joint 3 | Joint 4 | Joint 5 | Joint 6 |
|---|---|---|---|---|---|---|
| Start | 50.3 | -68.8 | 4.7 | 10.2 | 5.3 | 10.2 |
| WP1 | 32.9 | -65.9 | 14.2 | 11.5 | 3.5 | 18.6 |
| WP2 | 14.6 | -61.6 | 24.8 | 13.7 | 2.6 | 25.2 |
| WP3 | -5.5 | -58.5 | 34.6 | 14.7 | 1.2 | 33.7 |
| WP4 | -30.6 | -61.6 | 35.4 | 13.4 | 2.4 | 35.4 |
| WP5 | -35.9 | -68.3 | 23.5 | 12.7 | 5.5 | 26.5 |
| WP6 | -42.8 | -72.6 | 12.4 | 9.5 | 8.3 | 18.4 |
| Goal | -48.5 | -81.2 | 3.5 | 6.4 | 11.7 | 9.6 |
If equal time intervals of 4 seconds are initially assigned to each segment, the joint position, velocity, and acceleration curves are smooth. The fifth-order B-spline yields analytical expressions whose first and second derivatives are continuous. Hence, the charging gun moves without abrupt force changes during the charging operation.
5.2 Time-Optimal Trajectory Optimization Using Genetic Algorithm
The total time required to execute the trajectory between the starting pose and the charging port pose is a key performance indicator. Given \(n\) waypoints, the motion between consecutive waypoints \(i\) and \(i+1\) takes time \(T_i\). The total time \(T_{\text{total}}\) is
$$T_{\text{total}} = \sum_{i=1}^{n-1} T_i.$$
The optimization problem is to minimize \(T_{\text{total}}\) while respecting the maximum angular velocity \(\dot{\theta}_{\max}\), maximum angular acceleration \(\ddot{\theta}_{\max}\), and maximum jerk \(\dddot{\theta}_{\max}\) for each joint:
$$\text{Minimize} \quad T_{\text{total}} = \sum_{i=1}^{n-1} T_i,$$
$$\text{subject to} \quad |\dot{\theta}_j(t)| \le \dot{\theta}_{\max,j}, \quad |\ddot{\theta}_j(t)| \le \ddot{\theta}_{\max,j}, \quad |\dddot{\theta}_j(t)| \le \dddot{\theta}_{\max,j}, \quad j=1,\dots,6.$$
In this work, the upper bounds for the six joints are given in Table 8.
| Joint | Motion range (deg) | Max velocity (deg/s) | Max acceleration (deg/s²) |
|---|---|---|---|
| 1 | -165 to 180 | 150 | 90 |
| 2 | -135 to 80 | 100 | 70 |
| 3 | -125 to 125 | 120 | 95 |
| 4 | -170 to 170 | 95 | 70 |
| 5 | -120 to 120 | 100 | 80 |
| 6 | -180 to 180 | 95 | 85 |
The genetic algorithm encodes the time intervals \(T_1, T_2, \ldots, T_{n-1}\) as chromosome genes. Real-number encoding is adopted to maintain solution precision. The initial population is generated uniformly within 10% to 50% of the reference segment time. The fitness function is defined as the reciprocal of the penalized total time:
$$F_{\text{fit}} = \frac{1}{T_{\text{total}} + \xi P(t)},$$
where \(\xi\) is a penalty factor and \(P(t)\) is a penalty function that adds a term whenever a joint exceeds its kinematic limit. The penalty function is formulated as
$$P(t) = \sum_{j=1}^{6}\sum_{k=1}^{3} \max\left(0, \frac{\|q_j^{(k)}(t)\|}{q_{\max,j}^{(k)}} – 1\right),$$
where the superscript \((k)\) indicates velocity, acceleration, or jerk. If no constraint is violated, the penalty is zero; otherwise, the severity is proportional to the violation extent.
The genetic algorithm uses a population size of 100 and a maximum of 150 generations. The crossover probability varies from 0.8 to 0.4 while the mutation probability varies from 0.1 to 0.01. Three independent experiments are carried out to test the stochastic behavior of the optimization. The results are summarized in Table 9.
| Time interval | Initial (s) | Experiment 1 (s) | Experiment 2 (s) | Experiment 3 (s) |
|---|---|---|---|---|
| T1 | 4 | 2.642 | 2.595 | 2.846 |
| T2 | 4 | 2.784 | 2.869 | 2.546 |
| T3 | 4 | 3.155 | 2.654 | 2.536 |
| T4 | 4 | 2.553 | 2.431 | 2.735 |
| T5 | 4 | 2.613 | 2.531 | 3.244 |
| Total | 20 | 13.747 | 13.080 | 13.907 |
Although the total time could be further reduced, the achieved results satisfy the imposed kinematic constraints. Selecting Experiment 2 as the representative solution, the optimized joint trajectories still possess continuous angle, velocity, and acceleration profiles. The maximal angular velocity during the entire trajectory is about 58 deg/s, which is below the limit of all joints. The acceleration curves remain continuous and bounded, ensuring that the charging manipulator does not induce excessive shock to the electric vehicle charging port.
The optimized B-spline trajectory is computed by performing a cumulative time summation of the optimized intervals. Since B-spline basis functions are parameterized by a normalized knot vector, a strictly increasing mapping between the physical time and the parameter \(u\) is established. The joint angles are then interpolated at each control cycle. The resulting joint motions are smooth and efficient, allowing the charging gun to follow the obstacle-free path in approximately 13 seconds instead of the non-optimized 20 seconds. This nearly 35% reduction in execution time is meaningful in a fast-charging station where many electric vehicles are waiting.
5.3 Prototype Validation
To validate the effectiveness of the complete motion-planning framework, we build a prototype experimental platform using a JAKA MiniCobo collaborative robot. The robot has six degrees of freedom and a positioning accuracy of 0.1 mm, which is sufficient to mimic the automatic charging process. The experimental platform consists of a PC host, the robot controller, the robot arm, and an electric-vehicle-like charging target. The target is a planar board with a black circular pattern representing the charging socket. A white rectangular box is placed between the start pose and the socket to simulate an obstacle in the parking lot. The manipulator is equipped with a small camera at its end-effector, although in the planned experiment the socket position is predefined to focus on trajectory planning.
The motion planning algorithm is implemented in the Robot Operating System (ROS). The robot model is visualized in RViz. The improved RRT planner first generates a collision-free joint-space path around the white obstacle. The generated waypoints are then processed by the fifth-order B-spline interpolation, and the genetic algorithm optimizes the time allocation. Finally, the optimized joint trajectory is sent to the robot controller. The spatial path of the prototype arm demonstrates that the end-effector successfully avoids the obstacle and reaches the target socket area. Three schemes are tested to compare the improvement of the proposed algorithm: Scheme 1 uses basic RRT for path planning and B-spline plus genetic algorithm for trajectory planning; Scheme 2 uses Bi-RRT; Scheme 3 uses the proposed hybrid-sampling adaptive RRT. The trajectory planning stages in all three schemes use the same kinematic bounds and optimization method. The resulting motion durations are listed in Table 10.
| Scheme | Path planner | Motion duration experiments (s) |
|---|---|---|
| 1 | Basic RRT | 5.874, 5.237, 5.356 |
| 2 | Bi-RRT | 5.487, 5.587, 5.213 |
| 3 | Proposed hybrid-sampling adaptive RRT | 5.196, 4.431, 4.618 |
The prototype experiments show that Scheme 3 yields shorter motion durations in all three attempts. This result is consistent with the reduced path cost and higher planning efficiency of the proposed algorithm. In addition, the optimized joint profiles are within all prescribed kinematic limits, and the charging gun approaches the socket with no collision. The feasibility of the complete trajectory planning and optimization method is therefore verified experimentally.
6. Conclusion and Outlook
This paper systematically studies the trajectory planning and optimization of an automatic charging manipulator for electric vehicles. A six-degree-of-freedom articulated arm is designed based on the geometric requirements of typical charging parking spaces and the distribution of charging ports on mainstream electric vehicles. Kinematic modeling is carried out using the modified D-H convention, and the workspace is estimated by the Monte Carlo method. For obstacle avoidance, a hybrid-sampling adaptive RRT algorithm is proposed that combines an environment-aware Gaussian mixture sampling model, a global adaptive step size adjustment, and a heuristic line-check termination acceleration strategy. Simulation results in both two- and three-dimensional maps show that the proposed algorithm can reduce path cost and planning time significantly compared with standard RRT, GB-RRT, Bi-RRT, and RRT*. The generated path is then transformed into smooth joint motions using fifth-order B-spline interpolation. A genetic algorithm optimizes each time segment between waypoints under joint velocity, acceleration, and jerk bounds. The optimized trajectories reduce the total charging manipulation time by about 35% in the studied case. Prototype experiments with a JAKA MiniCobo robot confirm that the complete framework successfully drives the manipulator around an obstacle to the charging socket.
There are several possible directions for future research. First, the current environment model is constructed offline; in real parking lots, the scene around an electric vehicle may change dynamically. Adding real-time three-dimensional perception using depth cameras and point-cloud processing would improve the robustness of the obstacle avoidance algorithm. Second, the path planner could be integrated with an online local replanning layer to respond to moving obstacles such as pedestrians or other electric vehicles. Third, the optimization criterion currently minimizes time; future work may introduce multi-objective functions that balance time, energy consumption, and mechanical stress. Finally, the prototype experiments could be extended to actual charging sockets with a force-controlled insertion stage to guarantee gentle contact with the electric vehicle charging port. These enhancements will improve the maturity of automatic charging systems and accelerate their deployment in commercial electric vehicle charging stations.