Smooth and Time-Optimal Trajectory Planning for Robots Using Improved Carnivorous Plant Algorithm

Abstract

To improve the safety and reliability of robotic manipulators during high-speed precision movements, this paper proposes a method for smooth and time-optimal trajectory planning incorporating kinodynamic constraints. The primary objective is to use an evolutionary algorithm to determine a trajectory by considering time and jerk within the feasible path-pseudo-velocity phase plane region. Firstly, the path parameterization theory extracted the maximum pseudo-velocity projection curve from the kinodynamic constraints. Subsequently, the feasible region in the phase plane was defined through reachability analysis of discrete linear systems. Thereafter, we constructed the trajectory function using a cubic B-spline curve, optimizing its control points with an improved carnivorous plant optimization algorithm. Finally, the effectiveness and practicality of this method were verified through simulations on a 6-DOF manipulator.

1. Introduction

Introducing dynamic constraints is crucial for improving the safety and reliability of mobile robots, especially during high-speed manipulator operations. Limiting jerk helps reduce mechanical strain, avoid resonance, and prevent actuator damage, ensuring smoother, more precise trajectories. While third-order constraints (jerk-based) enhance trajectory smoothness and component durability, they come with higher computational costs. Second-order constraints (acceleration-based) are sufficient for speed-focused tasks, but third-order constraints are essential for applications prioritizing smooth motion and longevity. In summary, smooth, time-optimal trajectory planning with third-order dynamic constraints improves system performance and ensures accurate, efficient operation.

One solution of trajectory planning is to interpolate the position and time series of joints with smooth curves at several intermediate points of the geometric path, and then optimize the specific coefficient of the interpolation function according to the relevant constraints and performance indexes. In earlier studies, Lin et al. used cubic interpolation from splines to approximate the path and optimized the spline node position by the local iterative polyhedral search method [1]. Gasparetto and Zanotto [2] proposed quintic B-splines in joint trajectory interpolation and optimized the trajectory using sequential quadratic programming to further ensure the continuity of the trajectory. In 2008, they proposed an optimal trajectory planning technology for a robot manipulator. In order to obtain the optimal trajectory, Gasparetto and Zanotto minimized the objective function, consisting of the total execution time and the square integral of the jerk (the derivative of acceleration) [3]. Wang et al. employed a quintic B-spline function to interpolate the trajectory and combined it with a hybrid WOA-GA algorithm to realize the time–jerk optimal trajectory under kinematic constraints [4]. Machmudah et al. successfully planned the joint motion trajectory with the optimal time and torque by using the high-order polynomial curve method, combined with the genetic algorithm and particle swarm optimization algorithm. The kinematic and dynamic constraints occur at the same time in the environment where the manipulators are facing obstacles [5]. However, trajectory planning directly in joint space is a complex, high-dimensional problem. While existing methods are capable of generating trajectories that meet optimal performance metrics, they rely on a large number of parameters and high-order interpolation operations. This dependence increases the calculation’s time complexity and may reduce motion accuracy due to fluctuations in the interpolation function’s coefficient.

Another solution involves transforming the many-body differential equations and system constraints into one-dimensional motion along a specific geometric path, reducing the multi-dimensional state space of a robotic system to a low-dimensional state space [6]. In this context, Bobrow et al. [7], along with Shin and McKay [8], pioneered the idea of characterizing the equations of motion using scalar path parameters and their time derivatives. They parameterized the joint torque constraints to path-curve coordinates, making it possible to find a time-optimal curve in the path-pseudo-velocity phase plane. Building on this, Shiller and Lu [9] extended this work by transforming the original problem into an equivalent reduction problem, replacing the torque constraint with a constraint on maximum pseudo-acceleration and pseudo-velocity along the path. Verscheure et al. converted the time-optimal trajectory planning (TOTP) problem into a convex optimization problem [10]. They expressed the original problem as a large sparse optimization problem after discretization, then adopted second-order cone programming to improve solving efficiency. To avoid abrupt changes in torque, Oberherber et al. used spline curves to approximate the planning results in the phase plane, optimizing the control points of the spline curves [11]. Pham et al. proposed a Reachability Analysis-based TOPP method (TOPP-RA) [12], focusing on solving the time optimization problem under second-order constraints. By carefully controlling the bidirectional numerical integration process and using the dichotomous algorithm to address constraint violations during integration, E. Barnett et al. proposed the widely applicable bisection algorithm for TOTP (TOTP-BA) method [13]. Lin presented a unified methodology to find the near optimal solution of a minimum-jerk trajectory based on PSO [14]. Kucuk [15] combined cubic spline with the seventh order polynomial to construct minimum-time smooth motion trajectories beginning and ending with zero jerk using a PSO algorithm for serial and parallel manipulators. Huang et al. [16] also described a methodology adopting the quintic B-spline interpolation in joint space but applied a non-dominated sorting genetic algorithm (NSGA-II) as a multi-objective optimization algorithm to obtain a time–jerk optimal trajectory.

Despite significant progress in these parameterization solutions, satisfying third-order constraints and finding optimal trajectories in multi-objective environments remains challenging. Faced with these challenges, we draw inspiration from previous discussions by analyzing the convex polygonal region formed by the first to third order constraints of discrete path points within the path-pseudo-velocity phase plane, allowing us to extract the maximum pseudo-velocity curve. Additionally, we further define the feasible region by assessing the reachability of adjacent path points. To reduce the dimensionality of the variables during the optimization process, we selected the control points of the cubic B-spline curve as our optimization variables and employed an improved carnivorous plant optimization algorithm for solving. Throughout the iterative resolution process, we took into account both efficiency and smoothness to ensure an optimal outcome.

The rest of this paper is organized as follows. The constraints in trajectory planning and the parameterization of the smooth and time-optimal path are discussed in the upcoming section. Then, the reconstruction of feasible regions and the mathematical model for applying cubic B-spline interpolation in the phase plane are presented in Section 3. In Section 4, the Carnivorous Plant Algorithm and its enhancements are introduced. Section 5 provides an analysis of the numerical simulation results. Finally, Section 6 summarizes the key findings and conclusions of this study.

2. Modeling Optimization Problems

2.1. Path Parameterization

Consider a smooth path represented in the joint space coordinates, denoted as θ(s), where s represents a scalar path parameter. The parameter $s$ describes the geometric shape of the path, and its mapping relationship with time $t$, $s\left(t\right):\mathbb{R}\to \left[0,\mathsf{\Lambda }\right]$, reveals the law of trajectory change over time. Furthermore, to ensure continuous advancement of the robot along the path of total length $\mathsf{\Lambda }$, it must satisfy $\dot{s}\left(t\right)\ge 0$.

The joint velocity, acceleration, and jerk of the robot can be determined by the chain derivation rule as follows:

$$\begin{array}{c}\begin{array}{c}\left\{\begin{array}{c}\dot{q}=\dot{s}q\prime \\ \ddot{q}=\ddot{s}q\prime +{\dot{s}}^{2}q″\\ \stackrel{⃛}{q}=\stackrel{⃛}{s}q\prime +3\dot{s}\ddot{s}q″+{\dot{s}}^3{q}^{‴}\end{array}\right.\end{array}\end{array}$$

(1)

In this equation, $q\prime =\partial q\left(s\right)/\partial s,q^{″}={\partial }^{2}q\left(s\right)/\partial s^2,{\mathrm{a}\mathrm{n}\mathrm{d}q}^{‴}={\partial }^{3}q\left(s\right)/\partial s^3$ denote the first, second, and third derivatives of the joint position with respect to s. Similarly, $\dot{s}=ds/dt,\ddot{s}=d^{2}s/d{t}^2$ and $\stackrel{⃛}{s}=d^{3}s/d{t}^3$ represent the pseudo-velocity, pseudo-acceleration, and pseudo-jerk, respectively.

For an n-axis robot with rigid components, the complete dynamic equation under a nonlinear friction model can be expressed as follows:

$$\begin{array}{c}\tau \left(q,\dot{q},\ddot{q}\right)=M\left(q\right)\ddot{q}+C\left(q\right)\dot{q}+G\left(q\right)\end{array}$$

(2)

where $\tau \in {\mathbb{R}}^{\mathbf{n}}$ is the joint force vector; $M\left(q\right)\in {\mathbb{R}}^{n\times n}$ is the inertia matrix; $C\left(q\right)\in {\mathbb{R}}^{n\times n}$ is the Coriolis and centripetal force matrix; and $G\left(\mathrm{q}\right)\in {\mathbb{R}}^{\mathbf{n}}$ is the gravity term vector.

We substitute Equation (1) into Equation (2) to achieve the mapping of joint torques to $s$ space:

$$\begin{array}{c}\begin{array}{c}\left\{\begin{array}{c}\begin{array}{c}\tau \left(s,\dot{s},\ddot{s}\right)=\mathrm{m}\left(s\right)\ddot{s}+\mathrm{c}\left(s\right){\dot{s}}^2+\mathrm{g}\left(s\right)\\ \mathrm{m}\left(s\right)=M\left(q\right)q\prime \end{array}\\ \begin{array}{c}\mathrm{c}\left(s\right)=M\left(q\right)q″+{q\prime }^{T}C\left(q\right)q\prime \\ \mathrm{g}\left(s\right)=G\left(q\right)\end{array}\end{array}\right.\end{array}\end{array}$$

(3)

To ensure smooth and precise movement of the robot during trajectory movement, it is necessary to control the rate of torque. Calculate the time derivative from Equation (3):

$$\begin{array}{c}\begin{array}{c}\left\{\begin{array}{c}\dot{\tau }\left(s,\dot{s},\ddot{s},\stackrel{⃛}{s}\right)=\mathrm{m}\left(s\right)\stackrel{⃛}{s}+\mathrm{p}\left(s\right)\dot{s}\ddot{s}+\mathrm{c}\prime \left(s\right){\dot{s}}^3+\mathrm{g}\prime \left(s\right)\dot{s}\\ \mathrm{p}\left(s\right)=\mathrm{m}\left(s\right)+2\mathrm{c}\left(s\right)\end{array}\right.\end{array}\end{array}$$

(4)

2.2. Objective Function Parameterization

The objective function combines task completion time and normalized jerk (the derivative of acceleration). For a robot moving along a predetermined path, the total travel time is defined as follows:

$$\begin{array}{c}\begin{array}{c}{\int }_0^{T}1dt={\int }_0^{\mathsf{\Lambda }}{\left({\displaystyle \frac{ds}{dt}}\right)}^{-1}ds={\int }_0^{\mathsf{\Lambda }}{\displaystyle \frac{1}{\dot{s}}}ds\end{array}\end{array}$$

(5)

The smoothness of the trajectory is assessed by integrating the squared normalized jerk of the joints:

$$\begin{array}{c}{\int }_0^T\lambda {\displaystyle \sum _k^n}{\displaystyle \frac{{\left({\stackrel{⃛}{q}}_k\right)}^2}{\left({\overline{\stackrel{⃛}{q}}}_k\right)}}dt={\int }_0^{\mathsf{\Lambda }}\lambda {\displaystyle \sum _k^n}{\displaystyle \frac{{\left({\stackrel{⃛}{q}}_k\right)}^2}{\left({\overline{\stackrel{⃛}{q}}}_k\right)}}{\left({\displaystyle \frac{ds}{dt}}\right)}^{-1}ds={\int }_0^{\mathsf{\Lambda }}\lambda {\displaystyle \sum _k^n}{\displaystyle \frac{{\left({\stackrel{⃛}{q}}_k\right)}^2}{\left({\overline{\stackrel{⃛}{q}}}_k\right)}}{\displaystyle \frac{1}{\dot{s}}}ds \end{array}$$

(6)

where $\lambda$ is a weighting factor used to balance different factors, and for $k=1\dots n, {\overline{\stackrel{⃛}{q}}}_k=\underset{s\in \left[0,\mathsf{\Lambda }\right]}{max}{\overline{\stackrel{⃛}{q}}}_k\left(s\right).$

Based on this, the smooth time-optimal trajectory planning problem can be formulated as the following constrained optimization problem:

$$\begin{array}{c}\begin{array}{c}\underset{s\left(t\right)}{\mathit{min}}J={\int }_0^{\mathsf{\Lambda }}{\displaystyle \frac{1}{\dot{s}}}+\lambda {\displaystyle \sum _k^n}{\displaystyle \frac{{\left({\stackrel{⃛}{q}}_k\right)}^2}{\left({\overline{\stackrel{⃛}{q}}}_k\right)}}{\displaystyle \frac{1}{\dot{s}}}ds\\ \begin{array}{c}\mathrm{s}.\mathrm{t}\hfill \\ \left\{\begin{array}{c}{\tau }_{min}\le m\left(s\right)\ddot{s}+c\left(\mathrm{s}\right){\dot{s}}^2+g\left(s\right)\le {\tau }_{max}\\ \begin{array}{c}{\dot{\tau }}_{min}\le \mathrm{m}\left(s\right)\stackrel{⃛}{s}+\mathrm{p}\left(s\right)\dot{s}\ddot{s}+\mathrm{c}\prime \left(s\right){\dot{s}}^3+\mathrm{g}\prime \left(s\right)\dot{s}\le {\dot{\tau }}_{max}\\ {\dot{q}}_{min}\le q\prime \left(s\right)\dot{s}\le {\dot{q}}_{max},\\ {\ddot{q}}_{min}\le q\prime \left(s\right)\ddot{s}+q^{″}\left(s\right){\dot{s}}^2\le {\ddot{q}}_{max}\end{array}\\ \begin{array}{c}{\stackrel{⃛}{q}}_{min}\le q\prime \left(s\right)\stackrel{⃛}{s}+3q^{″}\left(s\right)\ddot{s}\dot{s}+q^{‴}\left(s\right){\dot{s}}^3\le {\stackrel{⃛}{q}}_{max}\\ s\left(0\right)=0,s\left(T\right)=\mathsf{\Lambda },\dot{s}\left(0\right)={\dot{s}}_0,\dot{s}\left(T\right)={\dot{s}}_T,\\ \dot{s}\left(t\right)\ge 0,t\in \left[0,T\right]\end{array}\end{array}\right.\end{array}\end{array}\end{array}$$

(7)

3. Optimization Problem Solving

3.1. Feasibility Region Analysis in the Phase Plane

The optimization variable $s\left(t\right)$ constitutes a typical functional optimization problem. Directly solving this problem is challenging; hence, we simplify it into a finite discrete form.

Taking Δs as the step size, we sample along the path length, dividing the interval $[0,\mathsf{\Lambda }]$ into N segments and N + 1 grid points $0=:s_0,s_1\dots s_{N-1},s_N:=\mathsf{\Lambda }$. For each grid point $s$, we identify a series of surface regions with varying shapes. We arrange these surface regions in order to form a four-dimensional state space $(s,\dot{s},\ddot{s},\stackrel{⃛}{s})$ defined by the boundaries of these surfaces. Within this high-dimensional space, motion is constrained independently of the initial conditions. When mapping from the subspace of $(\dot{s},\ddot{s},\stackrel{⃛}{s})$ to the subspace of $({\dot{s}}^2,\ddot{s})$, we observe that the set of linear inequalities formed by second-order constraints delineates a convex polygonal region, as shown in Figure 1.

These constraints can be expressed by the following inequalities:

$$\begin{array}{c}{\mathsf{\Gamma }}_{i,min}\le A_i\ddot{s}+B_i{\dot{s}}^2+C_i\le {\mathsf{\Gamma }}_{i,max},i=1,\dots ,2n,\end{array}$$

(8)

where $A_i,B_i,C_i,{\mathsf{\Gamma }}_{i,min}$ and ${\mathsf{\Gamma }}_{i,max}$represent the elements in the vectors of physical quantities under the kinematic constraints $\ddot{q}$ and dynamic constraints τ in model (7), respectively.

For each value of s, the pseudo-velocity and pseudo-acceleration are limited by the following:

$$\begin{array}{c}\begin{array}{c}0\le \dot{s}\le {\dot{s}}_{max,\mathsf{\Gamma }}\left(s\right)\\ {\ddot{s}}_{min,\mathsf{\Gamma }}\left(s,\dot{s}\right)\le \ddot{s}\le {\ddot{s}}_{max,\mathsf{\Gamma }}\left(s,\dot{s}\right)\end{array}\end{array}$$

(9)

where:

$$\begin{array}{c} \begin{array}{c}{\dot{s}}_{max,\mathsf{\Gamma }}\left(s\right)=\underset{i,j}{min}\left\{\underset{{\mathsf{\Gamma }}_i,{\mathsf{\Gamma }}_j}{max}\left({\displaystyle \frac{A_j\left({\mathsf{\Gamma }}_i-C_i\right)-A_i\left({\mathsf{\Gamma }}_j-C_j\right)}{A_j{B}_i-A_i{B}_j}}\right)\right\},i,j=1,\dots ,2n,\\ {\ddot{s}}_{min,\mathsf{\Gamma }}\left(s,\dot{s}\right)=\underset{i}{max}\left\{\underset{{\mathsf{\Gamma }}_i}{min}\left({\displaystyle \frac{{\mathsf{\Gamma }}_i-C_i-B_i{\dot{s}}^2}{A_i}}\right)\right\},i=1,\dots ,2n,\\ {\ddot{s}}_{max,\mathsf{\Gamma }}\left(s,\dot{s}\right)=\underset{i}{min}\left\{\underset{{\mathsf{\Gamma }}_i}{max}\left({\displaystyle \frac{{\mathsf{\Gamma }}_i-C_i-B_i{\dot{s}}^2}{A_i}}\right)\right\},i=1,\dots ,2n,\end{array}\end{array}$$

(10)

Similarly, when mapping to the $(\ddot{s},\stackrel{⃛}{s})$ subspace, a set of linear inequalities formed by third-order constraints also defines another convex polygonal region, as shown in Figure 2. These third-order constraints are expressed as follows:

$$\begin{array}{c}{\Psi }_{i,min}\le a_i\stackrel{⃛}{s}+b_i^{\mathrm{*}}\ddot{s}+c_i^{\mathrm{*}}\le {\Psi }_{i,max},i=1,\dots ,2n,\end{array}$$

(11)

where $b_i^{*}=b_i\dot{s},c_i^{*}=c_i{\dot{s}}^3+h_i\dot{s};a_i,b_i,c_i,{\Psi }_{i,min}$ and ${\Psi }_{i,max}$ represent the elements in the vectors of physical quantities under the kinematic constraints $\stackrel{⃛}{q}$ and dynamic constraints $\dot{\mathit{\tau }}$ in model (7), respectively.

For each set of values for s and $\dot{s}$, the boundaries for the pseudo-acceleration $\ddot{s}$ and the pseudo-jerk $\stackrel{⃛}{s}$ induced by the third-order constraints are as follows:

$$\begin{array}{c}\begin{array}{c}{\ddot{s}}_{min,\Psi }\left(s,\dot{s}\right)\le \ddot{s}\le {\ddot{s}}_{max,\Psi }\left(s,\dot{s}\right) \\ {\stackrel{⃛}{s}}_{min}\left(s,\dot{s},\ddot{s}\right)\le \stackrel{⃛}{s}\le {\stackrel{⃛}{s}}_{max}\left(s,\dot{s},\ddot{s}\right)\end{array}\end{array}$$

(12)

where:

$$\begin{array}{c}{\ddot{s}}_{min,\Psi }\left(s,\dot{s}\right)=\underset{i,j}{max}\left\{\underset{{\Psi }_i,{\Psi }_j}{min}\left({\displaystyle \frac{a_j\left({\Psi }_i-c_i^{\mathrm{*}}\right)-a_i\left({\Psi }_j-c_j^{\mathrm{*}}\right)}{a_j{b}_i^{\mathrm{*}}-a_i{b}_j^{\mathrm{*}}}}\right)\right\},i,j=1,\dots ,2n\\ {\ddot{s}}_{max,\Psi }\left(s,\dot{s}\right)=\underset{i,j}{min}\left\{\underset{{\Psi }_i,{\Psi }_j}{max}\left({\displaystyle \frac{a_j\left({\Psi }_i-c_i^{\mathrm{*}}\right)-a_i\left({\Psi }_j-c_j^{\mathrm{*}}\right)}{a_j{b}_i^{\mathrm{*}}-a_i{b}_j^{\mathrm{*}}}}\right)\right\},i,j=1,\dots ,2n\\ \begin{array}{c}{\stackrel{⃛}{s}}_{min}\left(s,\dot{s},\ddot{s}\right)=\underset{i}{max}\left\{\underset{{\Psi }_i}{min}\left({\displaystyle \frac{{\Psi }_i-c_i^{\mathrm{*}}-b_i^{\mathrm{*}}\ddot{s}}{a_i}}\right)\right\},i=1,\dots ,2n\\ {\stackrel{⃛}{s}}_{max}\left(s,\dot{s},\ddot{s}\right)=\underset{i}{min}\left\{\underset{{\Psi }_i}{max}\left({\displaystyle \frac{{\Psi }_i-c_i^{\mathrm{*}}-b_i^{\mathrm{*}}\ddot{s}}{a_i}}\right)\right\},i=1,\dots ,2n\end{array}\end{array}$$

(13)

We can redefine the first-order constraint $\dot{q}$ in model (7) as $|{\dot{s}}_{max,q}q\prime |\le \dot{q}$. For a fixed grid point $s$, set $\dot{s}$ as $\mathit{min}\left({\dot{s}}_{max,q}\left(s\right),{\dot{s}}_{max,\mathsf{\Gamma }}\left(s\right)\right)$. Then, using a vector positioning algorithm, quickly determine whether the space enclosed by the third-order constraints within the $\ddot{s}-\stackrel{⃛}{s}$ plane is non-empty.

If not, calculate the pseudo-acceleration range $\left[{\ddot{s}}_{min,\Psi }\left(s,\dot{s}\right),{\ddot{s}}_{max,\Psi }\left(s,\dot{s}\right)\right]$ according to Equation (13) and verify whether this range intersects with the reference range $\left[{\ddot{s}}_{min,\mathsf{\Gamma }}\left(s,\dot{s}\right),{\ddot{s}}_{max,\mathsf{\Gamma }}\left(s,\dot{s}\right)\right]$ defined by Equation (11). If the initial space is empty or there is no intersection in the pseudo-acceleration range, gradually reduce $\dot{s}$ until an intersection is found.

By implementing the above method at each grid point on the path, it becomes possible to construct a set $S_{\mathrm{s}\mathrm{d}}$ of $\left(s,\dot{s}\right)$ that meets all the constraints, obtaining the projection of the high-dimensional state space boundary onto the $s-\dot{s}$ plane.

Since calculations in high-dimensional space are complex and time-consuming, simplifying the problem to the $s-\dot{s}$ plane can improve algorithm efficiency and speed. This “maximum pseudo-velocity projection” reveals the limited relationship between the path and joint speeds and narrows down the feasible region on the $s-\dot{s}$ plane.

Furthermore, by employing Model Predictive Control reachability theory, we can analyze the constraint relationships between path points, and we can further refine the feasible region range on the $s-\dot{s}$ plane. Given that $\begin{array}{c}{\displaystyle \frac{d{\dot{s}}^2}{ds}}=2\ddot{s}\end{array}$, the velocity relationship between adjacent path points can be discretized as follows:

$$\begin{array}{c}\begin{array}{c}{x}_{i+1}=x_i+2\mathsf{\Delta }{u}_i,i=0\dots N-1\end{array}\\ x_i\in {\mathcal{X}}_i, u_i\in {\mathcal{U}}_i\end{array}$$

(14)

where $x_i$ is the state variable at stage $i$, representing the squared pseudo-velocity ${\dot{s}}_i^2$ at point $s_i$; $u_i$ is the control variable at stage $i$, representing a constant path pseudo-acceleration ${\ddot{s}}_i$ (covering the interval $[s_i,s_{i+1}]$); and ${\mathcal{X}}_i$ and ${\mathcal{U}}_i$ are the explicit constraints for the state variable and control variable at stage i, respectively.

The maximum pseudo-velocity projection curve obtained is defined as an explicit constraint for the state variable, and the second-order kinodynamic constraints are defined as an explicit constraint for the control variable. The control-state set ${\mathsf{\Omega }}_i$, defined jointly by these constraints, still maintains a convex polygonal shape:

$$\begin{array}{c}{\mathsf{\Omega }}_i≔\left\{\left(u,x\right)|{A_{i}u}_i+B_i{x}_i+C_i\in {\mathsf{\Gamma }}_i,\sqrt{x_i}\in S_{\mathrm{s}\mathrm{d},i}\right\}\end{array}$$

(15)

Equations (14) and (15) describe a discrete-time linear system constrained by linear control-state inequality conditions. Based on this system, the optimal parameterization is computed in two passes, as depicted in Figure 3. In the first pass, starting from the initial pseudo-velocity state along the predetermined path, the boundaries of the reachable set ${\mathcal{L}}_i$ are calculated stage by stage using a forward (fw) recursive method. The calculation process is as follows:

$$\begin{array}{c}{x}^{+}≔\underset{\left(u,\tilde{x}\right)\in {\mathsf{\Omega }}_i,x^{+}\in {\mathcal{X}}_{i+1}}{max}\left(\tilde{x}+2\mathsf{\Delta }u\right)\\ x^{-}≔\underset{\left(u,\tilde{x}\right)\in {\mathsf{\Omega }}_i,x^{-}\in {\mathcal{X}}_{i+1}}{min}\left(\tilde{x}+2\mathsf{\Delta }u\right)\\ {\mathcal{L}}_i≔\left\{x∣x^{-}\le x\le x^{+}\right\}\end{array}$$

(16)

In the second pass, starting from the path’s end state and using a backward (bw) recursive minimum control strategy to ensure each state falls within the reachable set of its previous stage, the specific calculation formula is the following:

$$x_i^{\mathrm{*}}≔\mathit{min}\left\{x_{i+1}^{\mathrm{*}}-2\mathsf{\Delta }min\left(u_i\right),x_i^{+}\right\}$$

(17)

Through this method, the backward phase trajectory x* updates the upper bound of the feasible region.

3.2. Trajectory Construction

When constructing a discrete model to ensure that the pseudo-velocity curve meets the constraint conditions throughout the entire motion process, it is typically necessary to use densely sampled points. However, this approach increases the dimensionality of the optimization variables, thereby reducing the efficiency of model solving. To address this issue, we used cubic B-spline curves within the feasible region of the phase plane to construct third-order continuous ($C^3$) [17] pseudo-velocity curves, as shown in Figure 4.

In the process of defining the B-spline curves, we selected a set of path nodes, denoted as $\mathit{S}=[S_0,S_1,\dots ,S_{l-1}]$, where each pair of adjacent nodes encompasses mmm original path segments. Based on these nodes, we defined the corresponding pseudo-velocity vector $\mathit{f}=[f_0,f_1,\dots ,f_{l-1}]$, where each node $S_i$ corresponds to a pseudo-velocity vector component $f_i$. These components $f_i$, serving as control points, have a decisive impact on the geometric shape of the B-spline curve.

By substituting the optimization variable $s\left(t\right)$ in model (7) with the pseudo-velocity vector $\mathit{f}$, the discrete model for the optimal trajectory planning problem can be reformulated as follows:

$$\begin{array}{c}\underset{f}{\mathit{min}}J={\sum }_{i=1}^N{\displaystyle \frac{1}{\dot{s}\left(s_i\right)}}\left(1+\lambda {\displaystyle \sum _k^n}{\displaystyle \frac{{\left({\stackrel{⃛}{q}}_{k,i}\right)}^2}{\left({\overline{\stackrel{⃛}{q}}}_{k,i}\right)}}\right)∆s\\ \begin{array}{c}\mathrm{s}.\mathrm{t}\hfill \\ \left\{\begin{array}{c}{\tau }_{min}\le \tau \left(s_i,\dot{s}\left(s_i\right),\ddot{s}\left(s_i\right)\right)\le {\tau }_{max}\\ \begin{array}{c}{\dot{\tau }}_{min}\le \dot{\tau }\left(s_i,\dot{s}\left(s_i\right),\ddot{s}\left(s_i\right),\stackrel{⃛}{s}\left(s_i\right)\right)\le {\dot{\tau }}_{max}\\ {\dot{q}}_{min}\le \dot{q}\left(s_i,\dot{s}\left(s_i\right)\right)\le {\dot{q}}_{max}\\ {\ddot{q}}_{min}\le \ddot{q}\left(s_i,\dot{s}\left(s_i\right),\ddot{s}\left(s_i\right)\right)\le {\ddot{q}}_{max}\end{array}\\ \begin{array}{c}{\stackrel{⃛}{q}}_{min}\le \stackrel{⃛}{q}\left(s_i,\dot{s}\left(s_i\right),\ddot{s}\left(s_i\right),\stackrel{⃛}{s}\left(s_i\right)\right)\le {\stackrel{⃛}{q}}_{max}\\ 0\le \dot{s}\left(s_i\right)\le \sqrt{x_i^{*}}\end{array}\end{array}\right.\end{array}\end{array}$$

(18)

Since the number of control points is far less than the number of sampling points, the dimensionality of the optimization variables is accordingly reduced, thereby speeding up the model solving process.

Despite the control points being relatively sparse, B-splines demonstrate a robust ability for shape expression, allowing the pseudo-velocity curve to be finely adjusted to complex shapes to fulfill model requirements. Furthermore, B-splines exhibit local support characteristics, meaning the interaction between distant control points is minimal. This feature significantly facilitates the rapid convergence of the pseudo-velocity curve during the optimization process. The pseudo-velocity curve is represented in cubic B-spline form as follows:

$$\dot{s}\left(u\right)=\sum _{i=0}^{n-1}{N}_{i,3}\left(u\right)f_i$$

(19)

Here, $u={\displaystyle \frac{s}{\mathsf{\Lambda }}}$ is a normalized path parameter, which maps the path length$s$ to intervals $\left[0, 1\right]$.

In Equation (19), the basis function $N_{i,3}\left(u\right)$ in the cubic B-spline is calculated according to the following recursive relation:

$$\begin{array}{c}\left. \begin{array}{cc} & N_{i,0}\left(u\right)=\left\{\begin{array}{cc}1,& \mathrm{if}{u}_i\le u\le u_{i+1},\\ 0,& \mathrm{otherwise}.\end{array}\right.\\ & N_{i,k}\left(u\right)={\displaystyle \frac{u-u_i}{u_{i+k}-u_i}}{N}_{i,k-1}\left(u\right)+{\displaystyle \frac{u_{i+k+1}-u}{u_{i+k+1}-u_{i+1}}}{N}_{i+1,k-1}\left(u\right)\end{array}\right\},k=\mathrm{1,2},3.\end{array}$$

(20)

The first and second derivatives of B-spline with respect to the normalized path parameter $u$ are

$$\begin{array}{c}{\displaystyle \frac{d\dot{\mathit{s}}\left(u\right)}{du}}={\displaystyle \frac{d{N}_{\mathrm{0,3}}\left(u\right)}{du}}{f}_0+\cdots +{\displaystyle \frac{d{N}_{n-\mathrm{1,3}}\left(u\right)}{du}}{f}_{n-1}={\displaystyle \sum _{i=0}^{n-1}}\left({\displaystyle \frac{d{N}_{i,3}\left(u\right)}{du}}{f}_i\right)\\ {\displaystyle \frac{d^2\dot{\mathit{s}}\left(u\right)}{d{u}^2}}={\displaystyle \frac{d^2{N}_{\mathrm{0,3}}\left(u\right)}{d{u}^2}}{f}_0+\cdots +{\displaystyle \frac{d^2{N}_{n-\mathrm{1,3}}\left(u\right)}{d{u}^2}}{f}_{n-1}={\displaystyle \sum _{i=0}^{n-1}}\left({\displaystyle \frac{d^2{N}_{i,3}\left(u\right)}{d{u}^2}}{f}_i\right)\end{array}$$

(21)

where

$${\displaystyle \frac{d^k{N}_{i,3}\left(u\right)}{d{u}^k}}=3\left({\displaystyle \frac{d^{k-1}{N}_{i,2}\left(u\right)}{d{u}^{k-1}}}{\displaystyle \frac{1}{u_{i+3}-u_i}}-{\displaystyle \frac{d^{k-1}{N}_{i+\mathrm{1,2}}\left(u\right)}{d{u}^{k-1}}}{\displaystyle \frac{1}{u_{i+4}-u_{i+1}}}\right),k=\mathrm{1,2}$$

The parameter vector $\mathrm{u}$ used in the B-spline contains $n-2$ uniformly distributed knots, calculated as follows:

$$\mathrm{u}=\left[\begin{array}{ccccc}0& u_1& u_2& \cdots & 1\end{array}\right]=\underset{n-2}{\underbrace{\left[\begin{array}{ccccc}0& \mathsf{\Delta }s& 2\mathsf{\Delta }s& \cdots & \mathsf{\Lambda }\end{array}\right]}}\times {\displaystyle \frac{1}{\mathsf{\Lambda }}}$$

(22)

where $\mathsf{\Delta }s={\displaystyle \frac{\mathsf{\Lambda }}{n-3}}$.

To ensure that the B-spline passes through the starting and ending points, i.e., $s\left(0\right)=0,f_0={\dot{s}}_0$ and $s\left(T\right)=\mathsf{\Lambda },f_{n-1}={\dot{s}}_{\mathsf{\Lambda }}$, the first and last elements of the parameter vector $\mathrm{u}$ are each repeated four times:

$$\mathrm{u}=\left[\begin{array}{ccccc}\underset{4}{\underbrace{0\dots 0}}& u_i& u_{i+1}& \dots & \underset{4}{\underbrace{1\dots 1}}\end{array}\right].$$

(23)

Therefore, the calculations for the pseudo-acceleration $\ddot{s}$ and the pseudo-jerk $\stackrel{⃛}{s}$ along the path $s$ are as follows:

$$\begin{array}{ll} & \ddot{s}\left(s\right)=\left({\displaystyle \frac{1}{\mathsf{\Lambda }}}\right){\left.\left(\dot{s}\left(u\right){\displaystyle \frac{d\dot{s}\left(u\right)}{du}}\right)\right|}_{u={\displaystyle \frac{s}{\mathsf{\Lambda }}}}\\ & \stackrel{⃛}{s}\left(s\right)={\left({\displaystyle \frac{1}{\mathsf{\Lambda }}}\right)}^2{\left.\left(\dot{s}(u{)}^2{\displaystyle \frac{d^2\dot{s}\left(u\right)}{d{u}^2}}\right)\right|}_{u={\displaystyle \frac{s}{\mathsf{\Lambda }}}}+{\left({\left.\left({\displaystyle \frac{1}{\mathsf{\Lambda }}}\dot{s}\left(u\right){\displaystyle \frac{d\dot{s}\left(u\right)}{du}}\right)\right|}_{u={\displaystyle \frac{s}{\mathsf{\Lambda }}}}\right)}^2\end{array}$$

(24)

4. Improved Carnivorous Plant Algorithm

To address complex kinematic and dynamic constraints and optimize robot manipulation paths in multi-objective environments, previous studies have utilized heuristic algorithms to identify optimal trajectories that satisfy third-order constraints [14,15,16,17]. Building on these findings, we conducted an in-depth analysis and developed further enhancements for the cubic B-spline curve. Specifically, this paper introduces the Improved Carnivorous Plant Algorithm (ICPA) to optimize the distribution of control points in the cubic B-spline, meeting third-order constraint requirements while enhancing path smoothness and efficiency.

4.1. Carnivorous Plant Algorithm

The Carnivorous Plant Algorithm (CPA) [18], introduced by Ong in 2021, is a bio-inspired meta-heuristic designed to mimic the survival strategies and adaptation mechanisms of carnivorous plants in challenging environments. The algorithm demonstrates high performance in tackling complex search space problems characterized by high-dimensional design variables, multiple constraints, and numerous local optima. The CPA operates through four main stages: initialization, classification and grouping, growth, and reproduction.

During the initialization stage, CPA constructs an initial population of N individuals consisting of carnivorous plants ($n_{\mathit{CPlant}}$) and prey ($n_{\mathit{Prey}}$) through random allocation. Importantly, $n_{\mathit{Prey}}$ should be k times of $n_{\mathit{CPlant}}$. The spatial distribution of these individuals is represented in matrix form.

$$Pop=\left[\begin{array}{cccc}{X}_{\mathrm{1,1}}& X_{\mathrm{1,2}}& \cdots & X_{1,D}\\ X_{\mathrm{2,1}}& X_{\mathrm{2,2}}& \cdots & X_{2,D}\\ ⋮& ⋮& \ddots & ⋮\\ X_{N,1}& X_{N,2}& \cdots & X_{N,D}\end{array}\right]$$

(25)

where $D$ represents the spatial dimension, and the sum of $n_{\mathit{CPlant}}$ and $n_{\mathit{Prey}}$ equals $N$. The random initialization of the population follows the equation:

$$X_{i,j}=L_{b_j}+\left(U_{b_j}-L_{b_j}\right)\times \mathrm{rand}$$

(26)

Let $i=\mathrm{1,2},\dots ,N$ and $j=\mathrm{1,2},\dots ,D$, where $L_b$ and $U_b$ represent the lower and upper bounds of the search space, respectively. The term “$rand$” refers to a random number within the interval [0, 1].

Entering the classification and grouping stage, the algorithm sorts each individual in descending order according to the fitness value, and then it labels the top $n_{\mathit{CPlant}}$ individuals as carnivorous plants and the remaining $n_{\mathit{Prey}}$ individuals as prey. The most suitable prey is assigned to the top carnivorous plant. Immediately afterward, the second and third most suitable prey are paired with the corresponding carnivorous plants. This process repeats until each carnivorous plant is paired with its corresponding prey. After that, the extra prey is paired again starting with the highest-ranking carnivorous plant, and the pattern continues. Take Figure 5 as an example: it shows the visualization of carnivorous plants and prey, in which the population size n is set to 6 and the coefficient k is 2.

During the growth stage, which is part of the exploration phase, carnivorous plants release scents to attract prey. Captured prey provide nutrients that foster plant growth; however, not all preys are successfully captured. To account for this, an attraction rate ($ar$) is introduced, determining prey capture by comparing the $ar$ value with a random number between 0 and 1. The equations used in this phase are as follows:

$$\begin{array}{c}\begin{array}{c}{NewCP}_{i,j}=growth\times {CP}_{i,j}+\left(1-growth\right)\times {Prey}_{v,j}\end{array}\\ growth={growth}_{rate}\times {rand}_{i,j}\end{array}$$

(27)

$$\begin{array}{c}{NewPrey}_{i,j}=growth\times {Prey}_{u,j}+\left(1-growth\right)\times {Prey}_{v,j}, u\ne v\\ growth=\left\{\begin{array}{ll}growth\_rate\times {rand}_{i,j}& iff\left({Prey}_v\right)>f\left({Prey}_u\right)\\ 1-growth\_rate\times {rand}_{i,j}& iff\left({Prey}_v\right)<f\left({Prey}_u\right)\end{array}\right.\end{array}$$

(28)

where ${CP}_{i,j}$ represents the carnivorous plant of group $i$, ${Prey}_{v,j}$ and ${Prey}_{u,j}$ respectively represent two randomly selected preys in group $i$, $growth\_rate$ is a predefined value, and ${rand}_{i,j}$ is a random value chosen from the range [0, 1].

The reproduction stage marks the beginning of the exploitation process and is repeated based on the value of $n_{\mathit{CPlant}}$. In this stage, carnivorous plants use the nutrients obtained to grow and reproduce, thereby ensuring that the optimal individual in the population, that is, the top-ranked individuals, can reproduce. This allows the CPA algorithm to focus on pursuing the optimal solution.

The mathematical expression of this reproduction stage is as follows:

$$\begin{array}{c}{NewCP}_{i,j}={CP}_{1,j}+Reproduction\_rate\times {rand}_{i,j}\times {mate}_{i,j}\\ {mate}_{i,j}=\left\{\begin{array}{ll}{CP}_{v,j}-{CP}_{i,j},& iff\left({CP}_i\right)>f\left({CP}_v\right)\\ {CP}_{i,j}-{CP}_{v,j},& iff\left({CP}_i\right)<f\left({CP}_v\right)\end{array}\right., i\ne v\ne 1\end{array}$$

(29)

where ${CP}_{1,j}$ is the best solution, ${CP}_{v,j}$ is the randomly selected carnivorous plant, and $Reproduction\_rate$ is a predefined value.

The pseudocode of CPA is as follows (Algorithm 1):

Algorithm 1 Carnivorous Plant Algorithm

Input: Objective function $f\left(x\right)$, where $x=\left(x1, x2, . . . , xd\right)$; 1: Number of iterations, population size, attraction rate, growth rate, and other parameters. Output: Best solution found by the algorithm. 2: Initialize population with $n$ individuals in $d$ dimensions; 3: Evaluate the fitness of each individual; 4: Rank individuals according to their fitness values; 5: Determine the best individual $g^{*}$ as the first carnivorous plant; 6: while stopping criterion not met do 7:     Classify the top $n_{\mathit{CPlant}}$ individuals as carnivorous plants; 8:     Classify the remaining $n_{\mathit{Prey}}$ individuals as prey; 9:      Group the carnivorous plants and prey; 10:     /* Growth of carnivorous plants and prey */ 11:     for $i=1$ to $n_{\mathit{CPlant}}$ do 12:          for $Group cycle$ = 1 to $group\_iter$ do 13:               if $attraction\_rate$ > random number then 14:                    Prey is captured and digested; 15:                     Generate a new carnivorous plant using Equation (27); 16:               else 17:                    Prey escapes; 18:                    Generate new prey using Equation (28); 19:               end if 20:          end for 21:     end for 22:     /* Reproduction of the first carnivorous plant */ 23:     for $i=1$ to $n_{\mathit{CPlant}}$ do 24:      Generate a new carnivorous plant based on the first carnivorous plant using Equation (29); 25:     end for 26:     Evaluate the fitness of each new carnivorous plant and prey; 27:     Merge old and newly formed carnivorous plants and prey; 28:     Rank all individuals by fitness; 29:     Determine the current best individual $g^{*}$ as the first carnivorous plant; 30: end while

4.2. Improved Carnivorous Plant Algorithm

As the number of control points increases, the search efficiency of the Carnivorous Plant Algorithm (CPA) tends to decrease. To address this issue, this paper introduces the following improvements to the CPA: first, a chaotic search strategy is employed to optimize the algorithm’s initial solution, enhancing search diversity. Second, a growth stage strategy based on the normal distribution is implemented, allowing the algorithm to dynamically adjust the growth direction during the search process according to the quality of the solution. These enhancements improve both the efficiency and accuracy of the search. The improved algorithm is referred to as the Improved Carnivorous Plant Algorithm (ICPA).

Tent map has become an ideal choice for initializing the population due to its simplicity and efficiency in generating chaotic sequences [19], whose equation is the following:

$$x_{n+1}=\left\{\begin{array}{ll}{\displaystyle \frac{x_n}{0.7}},& 0<x_n\le 0.7\\ {\displaystyle \frac{x_n\left(1-x_n\right)}{0.3}},& 0.7<x_n\le 1\end{array}\right.$$

(30)

The specific method of using Tent map to initialize the population is as follows:

$$\begin{array}{ll}{X}_i^j\left(0\right)& =x_{\mathit{min}}+\mathit{Chaos}\times \left(x_{\mathit{max}}-x_{\mathit{min}}\right),\\ & \left\{\begin{array}{l}i=\mathrm{1,2},\dots ,n\\ j=\mathrm{1,2},\dots ,d\end{array}\right.\end{array}$$

(31)

where $X_i^j\left(0\right)$ represents the initial value of the $i$-th individual on the $j$-th decision variable; $x_{\mathit{min}}$ and $x_{\mathit{max}}$ represent the lower and upper bounds of the search space for the independent variables, respectively; $M$ denotes the number of individuals in the population; $d$ is the dimension of the problem considered; and $\mathit{Chaos}$ is the chaos factor generated through the tent map.

In the optimization problem of solving the minimum value, individuals with smaller objective function values are more likely to be close to the optimal solution. To use this characteristic to guide the search direction, we propose a new strategy in which the growth of offspring depends on the normal distribution $N(\mu ,{\sigma }^2)$. Here, $\mu$ is the preferred parent, and $\sigma$ is an adjustable standard deviation that controls the exploration range of new individuals.

According to the $6\sigma$ principle of the normal distribution, the interval $(\mu -3\sigma ,\mu +3\sigma )$ encompasses the majority of possible values. By adjusting σ, we can ensure that the generated offspring are concentrated around μ, thereby enhancing the algorithm’s ability to focus on the current best solution. Simultaneously, this approach allows for the exploration of new solutions within a certain range.

To implement this strategy, we replace the previous Equations (27) and (28) with new Equations (32) and (33) during the offspring generation phase:

$${NewCP}_{i,j}=N\left({CP}_{i,j},\hspace{0.33em}\sqrt{\epsilon +{\left[{\displaystyle \frac{{CP}_{i,j}-{Prey}_{v,j}}{6}}\right]}^2}\right)$$

(32)

$${New\mathit{Prey}}_{i,j}=\left\{\begin{array}{ll}N\left({Prey}_{v,j},\hspace{0.33em}\sqrt{\epsilon +{\left[{\displaystyle \frac{{Prey}_{v,j}-{Prey}_{u,j}}{6}}\right]}^2}\right)& iff\left({Prey}_v\right)>f\left({Prey}_u\right)\\ N\left({Prey}_{u,j},\hspace{0.33em}\sqrt{\epsilon +{\left[{\displaystyle \frac{{Prey}_{u,j}-{Prey}_{v,j}}{6}}\right]}^2}\right)& iff\left({Prey}_v\right)<f\left({Prey}_u\right)\end{array}\right.$$

(33)

where $\epsilon$ is a small constant to ensure stability and maintain a certain degree of randomness.

4.3. Smooth and Time-Optimal Trajectory Planning Algorithm

During trajectory optimization, the fitness function evaluates the performance of the solution and guides it to meet objectives and constraints. To handle constraints that may be violated by the solution, we construct a fitness function based on a static penalty approach [20]. Specifically, at each node of the trajectory, we apply functions $P_{constrain}$ to compute various constraints:

$$P_{constrain}\left(x,x_{min},x_{max},C\right)=C\cdot \left[\mathbb{I}\left(x<x_{min}\right)+\mathbb{I}\left(x>x_{max}\right)\right]$$

(34)

Here, $x$ represents a certain constrained variable; $x_{min}$ and $x_{max}$ are the minimum and maximum limits of this variable, respectively. $C$ is a fixed positive constant for this type of constraint, and $\mathbb{I}\left(condition\right)$ is an indicator function used to check if the condition $condition$ is satisfied. $P_{constrain}$ produces a positive value when any constraint violation exists; if there are no violations, it returns 0.

The specific penalty terms constructed from the constraints in the discrete model (17) are as follows:

$$\left\{\begin{array}{c}{P}_{\tau }\left(s_j\right)={\displaystyle \sum _{i=1}^6}{P}_{constrain}\left({\tau }_i\left(s_j,{\dot{\mathit{s}}}_j,{\ddot{s}}_j\right),{\tau }_{\mathit{min},i},{\tau }_{\mathit{max},i},C_{\tau }\right),\\ \begin{array}{c}{P}_{\dot{\mathit{\tau }}}\left(s_j\right)={\displaystyle \sum _{i=1}^6}{P}_{constrain}\left({\dot{\mathit{\tau }}}_i\left(s_j,{\dot{\mathit{s}}}_j,{\ddot{s}}_j,{\stackrel{⃛}{\mathit{s}}}_j\right),{\dot{\mathit{\tau }}}_{\mathit{min},i},{\dot{\mathit{\tau }}}_{\mathit{max},i},C_{\dot{\mathit{\tau }}}\right),\\ P_{\dot{\mathit{q}}}\left(s_j\right)={\displaystyle \sum _{i=1}^6}{P}_{constrain}\left({\dot{\mathit{q}}}_i\left(s_j,{\dot{\mathit{s}}}_j,{\ddot{s}}_j\right),{\dot{\mathit{q}}}_{\mathit{min},i},{\dot{\mathit{q}}}_{\mathit{max},i},C_{\dot{\mathit{q}}}\right),\end{array}\\ \begin{array}{c}{P}_{\ddot{\mathit{q}}}\left(s_j\right)={\displaystyle \sum _{i=1}^6}{P}_{constrain}\left({\ddot{\mathit{q}}}_i\left(s_j,{\dot{\mathit{s}}}_j,{\ddot{s}}_j\right),{\ddot{\mathit{q}}}_{\mathit{min},i},{\ddot{\mathit{q}}}_{\mathit{max},i},C_{\ddot{\mathit{q}}}\right),\\ P_{\stackrel{⃛}{\mathit{q}}}\left(s_j\right)={\displaystyle \sum _{i=1}^6}{P}_{constrain}\left({\stackrel{⃛}{\mathit{q}}}_i\left(s_j,{\dot{\mathit{s}}}_j,{\ddot{s}}_j\right),{\stackrel{⃛}{\mathit{q}}}_{\mathit{min},i},{\stackrel{⃛}{\mathit{q}}}_{\mathit{max},i},C_{\stackrel{⃛}{\mathit{q}}}\right),\\ P_{{\dot{\mathit{s}}}^{\mathrm{*}}}\left(s_j\right)={\displaystyle \sum _{i=1}^6}{P}_{constrain}\left(\dot{\mathit{s}}\left(s_j\right),0,{\dot{\mathit{s}}}_j^{\mathrm{*}},C_{{\dot{\mathit{s}}}^{\mathrm{*}}}\right),\end{array}\end{array}\right.$$

(35)

By summing up the penalty terms for joint constraints at every point along the trajectory, the total penalty value can be determined. Utilizing this value, the fitness function is defined as follows:

$$itness=J+\sum _{j=1}^n\left(P_{\tau }\left(s_j\right)+P_{\dot{\mathit{\tau }}}\left(s_j\right)+P_{\dot{\mathit{q}}}\left(s_j\right)+P_{\ddot{\mathit{q}}}\left(s_j\right)+P_{\stackrel{⃛}{\mathit{q}}}\left(s_j\right)+P_{{\dot{\mathit{s}}}^{\mathrm{*}}}\left(s_j\right)\right)$$

(36)

In summary, the complete algorithm flow for achieving the smooth and time-optimal trajectory planning algorithm (TS-ICPA) of the robot is shown in Figure 6 based on the above discussion.

5. Simulation and Result Analysis

5.1. Analysis of Performance Improvement of Carnivorous Plant Algorithm

To assess the optimization performance of the Improved Carnivorous Plant Algorithm (ICPA), this study used six commonly referenced benchmark test functions from the literature [21]. Additionally, we conducted a detailed comparison of the ICPA with several other algorithms, including the original Carnivorous Plant Algorithm (CPA), Genetic Algorithm (GA) [22], Particle Swarm Optimization (PSO) [23], and Artificial Bee Colony (ABC) [24].

This comparison mainly focuses on the performance of each algorithm on the following characteristics: the ability to converge quickly, the effect of identifying a large number of local minima points, the ability to escape from local optimal solutions, and the avoidance of premature convergence.

The detailed information of each test function is listed in

Table 1. Set of benchmark functions.

Function	Dim	Range	Optimal
$F_1\left(x\right)={\displaystyle \sum _{i=1}^n}{\left({\displaystyle \sum _{j-1}^i}{x}_j\right)}^2$	10	[−100, 100]	0
$F_2\left(x\right)={\mathbf{max}}_{\mathit{i}}\left\{\left|x_i\right|,1\le i\le n\right\}$	10	[−100, 100]	0
$F_3\left(x\right)={\displaystyle \sum _{i=1}^n}{\left(\left[x_i+0.5\right]\right)}^2$	10	[−100, 100]	0
$F_4\left(x\right)={\displaystyle \sum _{i=1}^n}\left[x_i^2-10\cos \left(2\pi x_i\right)+10\right]$	10	[−5.12, 5.12]	0
$F_5\left(x\right)=-20\mathrm{e}\mathrm{x}\mathrm{p}\left(-0.2\sqrt{{\displaystyle \frac{1}{n}}{\sum }_{i=1}^n{x}_i^2}\right)$ $-\mathrm{e}\mathrm{x}\mathrm{p}\left({\displaystyle \frac{1}{n}}{\sum }_{i=1}^n\cos \left(2\pi x_i\right)\right)+20+e$	10	[−32, 32]	0
$F_6\left(x\right)={\displaystyle \sum _{i=1}^{11}}{\left[a_i-{\displaystyle \frac{x_1\left(b_i^2+b_i{x}_2\right)}{b_i^2+b_i{x}_3+x_4}}\right]}^2$	4	[−5, 5]	0.00030

, where F1, F2, and F3 are unimodal benchmark functions, and F4, F5, and F6 are multimodal benchmark functions. For the parameters of the comparison algorithms, we adopted the fine-tuning values recommended in previous work. The parameters of all algorithms are summarized as follows:

(1)

CPA: group iteration = 2; attraction rate = 0.8; growth rate = 2; reproduction rate = 1.8; n_CPlant = 20; n_Prey = 80;

(2)

ICAP: group iteration = 2; attraction rate = 0.8; growth rate = 2; reproduction rate = 1.8; n_CPlant = 20; n_Prey = 80; Epsilon = 1 × 10⁻⁶;

(3)

GA: population size = 100; crossover probability, Pc = 0.8; mutation probability, Pm = 0.2;

(4)

PSO: population size = 100; learning factor, c = 1.5; learning factor, c2 = 1.5; inertia weight, w_max = 0.8; inertia weight, w_min = 0.4;

(5)

ABC: population size = 100.

The statistical significance of the results is ensured and errors are reduced by conducting 30 Monte Carlo runs for each function, with the mean and standard deviation of CPA, ICPA, GA, PSO, and ABC being recorded.

The corresponding test results are presented in

Table 2. Results of benchmark functions.

Function	Type	CPA	ICPA	GA	PSO	ABC
F1	Mean	$1.59\times 10^0$	$1.60\times 10^{-1}$	$3.85\times 10^3$	$4.41\times 10^{-1}$	$3.57\times 10^2$
	Std.	$9.48\times {10}^{-1}$	$3.61\times {10}^{-2}$	$1.98\times {10}^2$	$6.06\times {10}^{-1}$	$1.63\times {10}^2$
F2	Mean	$3.46\times {10}^{-2}$	$1.77\times {10}^{-3}$	$2.45\times {10}^1$	$3.04\times {10}^{-2}$	$4.54\times {10}^0$
	Std.	$1.98\times {10}^{-3}$	$5.34\times {10}^{-4}$	$2.14\times {10}^0$	$1.65\times {10}^{-2}$	$7.61\times {10}^{-1}$
F3	Mean	$1.38\times {10}^{-5}$	$2.29\times {10}^{-8}$	$1.19\times {10}^2$	$5.39\times {10}^{-6}$	$3.38\times {10}^{-2}$
	Std.	$3.67\times {10}^{-6}$	$5.44\times {10}^{-9}$	$4.15\times {10}^1$	$7.30\times {10}^{-6}$	$5.31\times {10}^{-4}$
F4	Mean	$2.24\times {10}^1$	$4.64\times {10}^0$	$2.94\times {10}^1$	$1.53\times {10}^1$	$3.22\times {10}^1$
	Std.	$7.55\times {10}^0$	$6.74\times {10}^{-1}$	$9.43\times {10}^0$	$9.04\times {10}^0$	$5.10\times {10}^0$
F5	Mean	$1.11\times {10}^{-3}$	$3.98\times {10}^{-5}$	$9.30\times {10}^0$	$8.23\times {10}^{-1}$	$1.61\times {10}^{-1}$
	Std.	$3.41\times {10}^{-5}$	$1.28\times {10}^{-5}$	$5.50\times {10}^0$	$1.16\times {10}^0$	$1.67\times {10}^{-2}$
F6	Mean	$3.09\times {10}^{-4}$	$3.07\times {10}^{-4}$	$6.65\times {10}^{-3}$	$3.08\times {10}^{-4}$	$7.31\times {10}^{-4}$
	Std.	$1.51\times {10}^{-6}$	$8.06\times {10}^{-8}$	$5.54\times {10}^{-3}$	$9.59\times {10}^{-16}$	$5.27\times {10}^{-6}$

. The optimal iterative convergence curve of each test function is illustrated in Figure 7. It can be observed that ICPA outperforms its comparison algorithm across all test functions.

5.2. Simulation Experiments and Analysis

A trajectory optimization simulation experiment was conducted on the UR5 collaborative robot along a predetermined path. Figure 8 shows the UR5 collaborative robot used in the simulation experiment. The DH parameters are used to calculate the kinematics and dynamics of the UR5 robot, with its coordinate system and associated parameters illustrated in Figure 9 and

Table 3. The DH parameters used to calculate the UR5 kinematics and dynamics.

Kinematics	alpha [rad]	a [m]	d [m]	theta [rad]
Joint 1	π/2	0	0.089159	0
Joint 2	0	−0.425	0	π/2
Joint 3	0	−0.39225	0	0
Joint 4	π/2	0	0.10915	−π/2
Joint 5	−π/2	0	0.09465	0
Joint 6	0	0	0.0823	0

.

In the experiments of this paper, Matlab and CoppeliaSim (formerly V-REP) were mainly used as simulation platforms. The control code was written in C++. The experiments were run in the ROS environment, and the hardware platform was a computer equipped with an Intel(R) Xeon(R) i5-7500 3.4 GHz processor and 16 GB memory. With this hardware and software configuration, the experiment was able to achieve efficient simulation and data analysis of robot motion control, verifying the effectiveness and practical applicability of the algorithm.

Table 4. UR5 experimental boundary conditions.

Joint i	1	2	3	4	5	6
Velocity (deg/s)	160	160	160	160	160	160
Acceleration (deg/s2)	573	573	573	1146	1146	1146
Jerk(deg/s3)	10,000	10,000	10,000	10,000	10,000	10,000
Torque (Nm)	100	100	100	150	150	150
Torque rate(Nm/s)	500	500	500	500	500	500

presents the required configurations and motion limits for each robot joint, including constraints on velocity, acceleration, and other key parameters. These limits define the safe operating range for each joint, ensuring that the robot’s motion stays within feasible boundaries. Based on the physical and mechanical specifications of the robot’s joints, these constraints are crucial for maintaining operational stability and ensuring smooth performance.

The generation process for the test path is as follows: firstly, five nodes are randomly selected within the range of [−80°, 80]⁶, and from these nodes, 200 equidistant nodes are generated. Then, cubic spline interpolation is applied to these equidistant nodes to form the joint space path $\mathit{q}$(s).

The positions of the five initial nodes used in the experiment are shown in

Table 5. Five initial node positions of the test path.

Position No.	Joint No. (°)
	1	2	3	4	5	6
1	−27.16	−1.13	−66.75	6.31	17.22	−32.13
2	33.52	−11.10	−70.92	−44.91	−52.19	−13.37
3	57.52	32.53	−34.89	−75.99	55.31	−28.93
4	−6.75	−54.11	−10.69	−57.08	69.62	58.80
5	−23.62	68.46	−50.35	65.07	−78.60	56.47

. The s path is defined by ds = ‖dq‖. To ensure high accuracy of the trajectory solution, the test path was resampled before the experiment with an s resolution of 0.293, thereby transforming it into a dense s path containing 2400 nodes.

In this paper, we explore an innovative trajectory planning method based on phase plane theory. This method adopts the comprehensive optimization of total jerk and time as the core goal while considering complex kinodynamic constraints up to the third order.

Currently, there are no similar studies that consider these factors comprehensively, which makes comparative analysis of our approach with existing studies challenging. To demonstrate the effectiveness and advantages of our proposed method, this study compares two approaches through time-optimal simulation experiments. The first is our newly developed method, TS-ICPA (where the time-jerk optimization weight λ is set to 0), and the second is the Advanced TOPP-BA, which employs second-order kinodynamic constraints.

The number of B-spline control points of our trajectory optimization algorithm is 120, the upper limit of ICPA iterations is 200, and the penalty coefficient is set to 50. Apart from this, other experimental parameters remain consistent with those in the performance test.

Figure 10 shows the time parameterization results generated by the TOPP-BA using second-order kinodynamic constraints and our method (λ = 0) on the test path. Preliminary observations show that the two curves behave fairly closely in most cases, and the relative errors with each other remain within a small range. The results show a high correlation of 0.986. This numerical analysis, along with our observations, suggests that while there are some minor differences between the two algorithms, they generally produce very similar outcomes overall.

For further analysis, Figure 10 and Figure 11 present the normalized trajectory results for each joint. The constant-step fifth-order Dormand–Prince method, an explicit Runge–Kutta ODE solver, is applied to process the time-parameterized curve. To ensure accuracy, an integration step size of 0.008 s was used during the processing.

As shown in Figure 11, the joint velocity, acceleration, and torque trajectories of the two methods are highly similar in terms of overall shape, peak positions, and proximity to the set boundaries, which further supports the validity of our approach. In theory, both methods should result in the same motion time; however, our method exhibited a slight increase of 0.31 s. Despite this, the trajectories generated by our algorithm demonstrate greater smoothness and consistency compared to the reference TOPP-RA.

Two key factors contribute to this outcome. First, our method is heuristic, meaning it asymptotically approaches the optimal solution rather than directly finding the global optimum. Second, unlike TOPP-RA, our method incorporates additional constraints, specifically on jerk and torque rate, which enhances the smoothness and control of the generated trajectories.

The application of the TS-ICPA method results in improved joint motion stability, as illustrated in Figure 12. A comparative analysis of jerk and torque rate variations reveals that with the TOPP-BA approach (Figure 12a,c), the robot’s normalized joint jerk falls within the range of (−14.8, 6.0), while the normalized torque rate ranges from (−6.1, 17.4). In contrast, when the TS-ICPA method is applied (Figure 12b,d), both the normalized squared jerk and torque rate are significantly reduced and confined within the narrower range of (−1, 1), demonstrating the method’s effectiveness in enhancing motion smoothness and stability.

In the absence of additional constraints, the TOPP-BA method results in significant variations in jerk and torque rates along certain path segments. For example, Figure 12a demonstrates substantial fluctuations in the jerk of joint 1 between t = 0.59 s and t = 0.88 s, while Figure 12c shows marked variations in the torque rate of joint 2 between t = 0.68 s and t = 0.78 s. In contrast, our method introduces third-order motion dynamics constraints and employs cubic spline interpolation to smooth these abrupt changes. This approach effectively reduces the peak values of the parameter trajectories and enhances the smoothness of motion, as evidenced in Figure 12b,d. Although this method results in a slight increase in motion time, it significantly improves stability.

Figure 13 illustrates the relationship between the trajectory planning objectives—time efficiency and motion smoothness—as the trade-off coefficient λ takes on different values. This demonstrates how varying λ impacts the balance between minimizing the trajectory duration and enhancing the smoothness of the motion.

For an intuitive comparison with the case where λ = 0, both the trajectory duration and the total squared jerk have been normalized relative to their values at λ = 0. When the trade-off coefficient λ is set to 0.5, 1, and 1.5, the corresponding normalized jerk results for the trajectories generated by the proposed method are presented in Figure 14. This comparison highlights how adjusting λ influences the balance between trajectory duration and smoothness.

The results in Figure 13 indicate that as the trade-off coefficient λ increases from 0 to 0.5, the trajectory duration rises by 3.5% (reaching approximately 1.035), while the normalized total squared jerk decreases by about 38% (to approximately 0.62). As λ increases further to 1, the trajectory duration increases by an additional 9.8% compared to the baseline, with the normalized total squared jerk decreasing by around 54%. When λ reaches 1.5, the trajectory duration increases by 17.9%, accompanied by a significant reduction in normalized total squared jerk by about 65%.

From Figure 14, it can be inferred that as λ increases, the acceleration range for each joint contracts. For example, joint 1’s minimum acceleration decreases from −0.465 at λ = 0.5 to −0.351 at λ = 1.5, while its maximum acceleration reduces from 0.247 to 0.171. This suggests that increasing λ leads to smoother motion at the cost of longer trajectory duration, as reflected by the reduced acceleration extremes.

The numerical results align with our engineering intuition: achieving time efficiency often requires making compromises in motion smoothness. These findings highlight the adaptability and flexibility of our proposed trajectory optimization model, which can dynamically adjust the trade-off coefficient to meet specific requirements. This allows the model to strike an optimal balance between high-speed motion and smoothness. The demonstrated optimization strategy is highly valuable in scenarios where both motion efficiency and stability are critical considerations.

6. Conclusions

This paper introduces a novel approach for smooth and time-optimal trajectory planning of robots. The method centers on constructing an integrated objective function in the phase plane, which accounts for both the total execution time and the squared integral of acceleration. By optimizing these factors, the approach ensures that the robot’s motion is both efficient and smooth throughout the trajectory.

After formulating the objective function, this paper performs a comprehensive analysis of the kinematic and dynamic constraints up to the third order in the phase plane to define the feasible trajectory region. To further enhance trajectory smoothness and improve computational efficiency, a third-order B-spline interpolation method was utilized for trajectory generation.

Additionally, an enhanced Carnivorous Plant Algorithm was employed to optimize the trajectories, demonstrating notable efficiency in the optimization process. The conducted simulation experiments validate the effectiveness and superiority of the proposed method, showing its ability to generate smooth, time-optimal trajectories under constrained conditions.