Time and Energy Optimal Trajectory Planning of Wheeled Mobile Dual-Arm Robot Based on Tip-Over Stability Constraint

Abstract

Trajectory planning and avoidance of tipping are the main keys to success in the mobile dual-arm manipulation, especially when the dual-arm or moving platform is running fast. The forces and moments between wheel-terrain and body-arm have been analyzed by kinematics and force analysis of a robot to define tip-over stability constraint. Then, an improved tip-over moment stability criterion for a wheeled mobile dual-arm robot is presented and defines tip-over stability constraint based on it. To improve the motion stability of the robot, this paper presents an optimal joint trajectory planning model based on time and energy. The quintic B-spline curve and an improved NSGA-II algorithm, which are time and energy, are applied to multi-objective optimization. The simulation results show that the motion stability of a robot is improved based on the tip-over stability constraint. This trajectory planning method based on the stability constraint can be applied to other mobile robots as well.

1. Introduction

The wheeled mobile dual-arm robot (WMDAR) is the most popular type of robot with the ability to move and manipulate in the recent stage. It is widely used in families, restaurants, hospitals, and other fields [1,2,3]. However, the structure of the system used by WMDAR is not sufficiently stable, and it may topple over when under the action of different dynamic factors or in the presence of external disturbances. Therefore, the trajectory planning in joint space and tip-over avoidance are the key parts to improve the motion stability of a robot.

Trajectory planning is a critical portion to improve the stability of a robot. Joint spatial trajectory planning can be used in many applications where the robot needs to change its own position in order to make adjustments. Trajectory planning interpolation algorithm needs to be introduced to meet some constraints of path points. Trajectory planning algorithms primarily include a polynomial function and B-spline curve. Sidobre and Desormeaux proposed a cubic polynomial function sequence for human–robot interaction trajectory [4]. Zhang et al. used the quintic polynomial function to realize the trajectory planning of the 6-DOF robot [5]. A B-spline curve has been applied to trajectory planning by many scholars because of its unique advantages. Meike et al. proposed a new trajectory optimization method for a series of point-to-point motions of industrial robots, which smooths the trajectory based on cubic B-spline interpolation [6]. Zou et al. proposed a new robot trajectory planning algorithm based on a non-uniform rational B-spline (NURBS) curve interpolation, and introduced the speed adaptive control method to adjust the step size of robot interpolation [7]. Gasparetto et al. applied cubic spline and quintic B-spline to trajectory planning of a 6-DOF robot [8]. Trajectory planning based on interpolation algorithm also needs to be optimized to improve its efficiency and stability of operation. Trajectory optimization is usually a multi-objective optimization problem (MOP). Many works have been done on the trajectory optimization algorithm of a robot. There are many population-based algorithms, such as the Non-dominated Sorting Genetic Algorithm (NSGA), Differential Evolution (DE), Particle Swarm Optimization (PSO), and Ant Colony Optimization (ACO) [9,10,11,12]. The NSGA-II has the advantages of fast running speed and good convergence of solution set among many optimization algorithms. It has been proved to be the most effective of MOPs [13]. Huang et al. used the 5-order B-spline to plan the trajectory in the joint space, and then used the optimal Non-dominated Sorting Genetic Algorithm (NSGA-II) to optimize the motion time and average acceleration of the whole trajectory [14]. A new hybrid interpolation algorithm “7-order polynomial + improved quartic uniform B-spline curve + 7-order polynomial” was proposed by Sun et al. Then, the NSGA-II algorithm was applied to the multi-objective optimization of time and acceleration. The effectiveness of the proposed hybrid interpolation algorithm was verified by trajectory planning experiments [15]. Therefore, the B-spline curve and NSGA-II are the optimal choices for multi-objective optimization of robot trajectory planning because of their characteristics.

In order to ensure the stability of the robot, stability constraints also need to be defined. Currently, some tipping stability criteria have been applied to mobile robots. These stability criteria primarily include the zero moment point (ZMP) [16,17,18], Force-Angle Stability (FAS) [19,20,21], and Moment-Height Stability (MHS) [22,23]. Korayem et al. proposed a simpler algorithm whose role is to determine the maximum loading capacity of the ZMP-based mobile manipulator [16]. Sugano et al. presented the concepts of the stability degree and the valid stable region based on ZMP [17]. Huang et al. proposed a coordinated motion planning method of a mobile manipulator based on ZMP [18]. However, when the center of mass (COM) of the robotic system is shifted, and thus changed, then the ZMP is insensitive to the system’s reliability. Therefore, Papadopoulos and Rey [19,20] proposed the force-angle (FA) margin criterion and described a real-time tip-over prediction based on static and dynamic force-angle measures. Talke et al. used the FA criterion to evaluate the stability of a mobile manipulator [21]. This standard neglects the vital manipulator reaction forces and moments that act on the moving platform. Moosavian and Alipour [22,23] then proposed Moment-High Stability (MHS) criterion for wheeled mobile robots by considering complex situations including dynamics and system center of gravity. In addition, stability analysis contains some other criteria that are usually applied to mobile robots as well. The Foot Rotation Indicator (FRI) criterion, proposed by Ambarish Goswami in 1999, was widely used in the stability discrimination of biped mobile robots [24]. The Normal Support Force (NSF) criterion requires that the support force can be measured by force sensors, which leads to high cost [25]. Guo et al. presented a method to analyze the tip stability of wheeled mobile manipulators based on the tipping moment (TOM) [26]. International scholars have done more research on the tipping stability criteria of mobile robots, but there is little research on the tip-over problem of wheeled mobile dual-arm robots.

This paper is made up of six sections. In Section 2, the forces and moments between wheel-terrain and body-arm have been analyzed by kinematics and force analysis of the robot to define the tip-over stability constraint. Section 3 has described an enhanced tipping moment stabilization criterion for the WMDAR. And then in Section 4, the stability of the robot is regarded as a constraint, and an optimal joint trajectory planning model based on time and energy is proposed. Then, the quintic B-spline curve and an improved NSGA-II algorithm are applied to multi-objective optimization. In Section 5, simulation experiment using ADAMS and MATLAB software are carried out to validate correctness of the trajectory planning method based on the tip-over stability constraint. Some conclusions are given in Section 6.

In this paper, WMDAR proposes a new concept. Due to its complete motion system, the WMDAR is designed for handling and moving over more complex terrains. The new contribution of this paper is to provide a complete description of the optimized mechanical design of a wheeled two-arm complete motion system implemented in the WMDAR for the basic control of mobile robots. This complete description is presented to facilitate replication and validation of the results. Finally, this paper empirically verifies that the control system will be used to improve the implementation control program of additional trajectories in the future.

2. Modeling and Analysis of WMDAR

2.1. The Structure of Robot

To define the tip-over stability constraint of the WMDAR, it is necessary to carry out kinematic and dynamic modeling of the robot system. The WMDAR consists of a mobile platform containing four wheels, the lower body and waist of the robot, and two arms with six degrees of freedom mounted on the shoulders, respectively, as shown in Figure 1. The four-wheeled mobile platform is a movable platform with two driving wheels and two driven wheels, which provide forward or steering power to the driving wheels through differential drive.

To describe the motion of the WMDAR, some coordinate systems are established, i.e., the world coordinates $O_W{X}_W{Y}_W{Z}_W$, the robot body coordinates $O_S{X}_S{Y}_S{Z}_S$, the left arm coordinates $O_L{X}_L{Y}_L{Z}_L$, and the end coordinates of left arm $O_{LL}{X}_{LL}{Y}_{LL}{Z}_{LL}$, the right arm $O_R{X}_R{Y}_R{Z}_R$, and the end coordinates of right arm $O_{RR}{X}_{RR}{Y}_{RR}{Z}_{RR}$, as shown in Figure 2a. The $m_b$ is the mass of the mobile platform and the body, and $O_C$ is the center of mass of the mobile platform and the body. As shown in Figure 2b, the contact mode between the mobile platform and the ground is point contact, and the four tilting axes (TOA) of the robot system are represented by four black lines in the figure. The mass and center of mass position of the robot-connecting rod are shown in

Table 1. Quality parameters of each link of the manipulator.

Connecting Rod	Quality/(kg)	Location of the Center of Mass/(mm)
1	2.002	[0, 5.955, 5.532]
2	1.268	[0, 7.494, 185.070]
3	1.246	[0, 5.864, 333.648]
4	0.710	[0, 7.180, 490.024]
5	0.698	[0, 4.821, 609.971]
6	1.461	[0, 0.001, 817.380]

.

2.2. Kinematic Model of Mobile Platform

The following assumptions are made when analyzing the kinematics of the mobile platform. (1) There is no sliding between the wheels and the ground. (2) The speed traction axis is parallel to the plane. (3) The center point of the two driving wheels is the centroid of the robot mobile platform. The mobile platform is shown in Figure 3.

The generalized coordinates and velocity of the mobile platform are defined as: $\theta =\left[\begin{array}{ccc}x& y& \varphi \end{array}\right]$, $\dot{\theta }=\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{\varphi }\end{array}\right]$. The steering angle and angular velocity of the left wheel are ${\varphi }_l,{\dot{\varphi }}_l$, and the steering angle and angular velocity of the right wheel are ${\varphi }_r,{\dot{\varphi }}_r$. Considering that the mobile platform satisfies the motion law of rigid body, there are:

$$v_l=v_c-a\dot{\varphi }\hspace{1em}{v}_r=v_c+a\dot{\varphi }$$

(1)

$$v_c=\frac{\left(v_r+v_l\right)}{2}\hspace{1em} \dot{\varphi }=\frac{\left(v_r-v_l\right)}{2a}$$

(2)

Since the wheels have no sliding, there is $v_l=r{\dot{\varphi }}_l$, $v_r=r{\dot{\varphi }}_r$ and $\dot{x}=v_{c}cos\varphi , \dot{ y}=v_{c}sin\varphi$.

$$\dot{\theta }=\left[\begin{array}{cc}\left(r/2\right)cos\varphi & \left(r/2\right)cos\varphi \\ \left(r/2\right)sin\varphi & \left(r/2\right)sin\varphi \\ r/2a& -r/2a\end{array}\right]\left(\begin{array}{c}{\dot{\varphi }}_r\\ {\dot{\varphi }}_l\end{array}\right)$$

(3)

$$\dot{\theta }=G\dot{r},\dot{r}={\left[\begin{array}{cc}{\dot{\varphi }}_r& {\dot{\varphi }}_l\end{array}\right]}^T$$

(4)

The Jacobian matrix of the differential drive wheel mobile platform is:

$$G=\left[\begin{array}{cc}\left(r/2\right)cos\varphi & \left(r/2\right)cos\varphi \\ \left(r/2\right)sin\varphi & \left(r/2\right)sin\varphi \\ r/2a& -r/2a\end{array}\right]$$

(5)

2.3. Force Model of Wheeled Mobile Dual-Arm Robot

2.3.1. Velocity and Acceleration of Joints

Then, the velocity and acceleration of arm joints will be analyzed below. The position and direction of the end effector of the arm can be expressed in the base coordinates of the robot body as follows [27]:

$$p_{n+1}={\displaystyle \sum }_{i=0}^n{R}_{0i}{b}_i^{\prime }={\displaystyle \sum }_{i=0}^n{b}_i$$

(6)

$$R_{0n}={\displaystyle \prod }_{i=1}^n{R}_{\left(i-1\right)i}$$

(7)

where $R_{on}$ represents the rotation matrix of the link n in the base coordinate system of the manipulator, which is obtained by a series of $R_{\left(i-1\right)i}$ multiplication.

The forward iterative equations of joint linear velocity and angular velocity can be obtained by deriving the above two equations, respectively:

$$v_i=v_{i-1}+{\omega }_{i-1}\times b_{i-1}+R_{oi-1}{\dot{b}}_{i-1}^{\prime }$$

(8)

$${\omega }_i={\omega }_{i-1}+{\omega }_{i-1i}$$

(9)

where $v_i$ represents the linear velocity vector of joint $i$; ${\omega }_i$ represents the angular velocity vector of joint $i$.

The forward iterative equation of linear acceleration and angular acceleration can be obtained by deriving Equations (8) and (9):

$$\begin{array}{l}{a}_i=a_{i-1}+{\alpha }_{i-1}\times b_{i-1}+{\omega }_{i-1}\times \left({\omega }_{i-1}\times b_{i-1}\right)\\ +2({\omega }_{i-1}\times R_{oi-1}{\dot{b^{\prime }}}_{i-1})+R_{oi-1}{{\ddot{b}}^{\prime }}_{i-1}\end{array}$$

(10)

$${\alpha }_i={\alpha }_{i-1}+{\alpha }_{i-1i}+{\omega }_{i-1}\times {\omega }_{i-1i}$$

(11)

where $a_i$ represents the linear velocity vector of joint $i$; ${\alpha }_i$ represents the angular velocity vector of joint $i$.

2.3.2. Force/Moment of Joints

The links and joints of the robot arms can be regarded as rigid bodies. A Newton–Euler method [28] is used for dynamics modeling to obtain the amount of force and movement of the two arms in reaction to the body. The force and moment acting on the manipulator joint are expressed as follows:

$$w_i=\left[\begin{array}{c}{f}_i{}^T\\ m_i^T\end{array}\right]=M_i{\dot{t}}_i+B_i+H_{ii+1}{w}_{i+1}$$

(12)

In Equation (12), the constraint force vector $f_i$ and constraint torque vector $m_i$ acting on the joint $i$ are included $w_i$,

$$w_i=\left[\begin{array}{c}{f}_i{}^T\\ m_i{}^T\end{array}\right]={\left[f_{iX},f_{iY},f_{iZ},m_{iX},m_{iY},m_{iZ}\right]}^T$$

(13)

$M_i$ is the generalized mass matrix of the link:

$$M_i=\left[\begin{array}{cc}{m}_{i}1& m_i{\tilde{b}}_{ic}^T\\ m_i{\tilde{b}}_{ic}^T& I_i\end{array}\right]$$

(14)

In Equation (14), $m_i$ is the mass of the link $i$; $b_{ic}$ is the vector of the center of mass (COG) of the link $i$ relative to the joint $i$; ${\tilde{b}}_{ic}^T$ is the antisymmetric matrix of $b_{ic}$; $I_i$ is the inertia matrix of the link $i$ relative to the rotation axis of the joint $i$, and $1$ is the identity matrix.

${\dot{t}}_i$ is the linear acceleration vector and angular acceleration vector of joint $i$:

$${\dot{\mathit{t}}}_i=\left[\begin{array}{c}{\mathit{a}}_i+\mathit{g}\\ {\alpha }_i\end{array}\right]$$

(15)

B_i is the cross term matrix representing centripetal force and gyro moment.

$$B_i=\left[\begin{array}{c}{m}_i{\omega }_i\times \left({\omega }_i\times b_{ic}\right)\\ {\omega }_i\times \left(I_i{\omega }_i\right)\end{array}\right]$$

(16)

$H_{ii+1}$ represents the transformation matrix of force and torque from joint $i+1$ to joint $i$.

$$H_{ii+1}=\left[\begin{array}{cc}1& 0\\ {\tilde{b}}_i& 1\end{array}\right]$$

(17)

The Formula (12) gives a backward recursion for calculating the forces and moments from n + 1 to 1. $w_n$ is the wrench of the last joint, which can be figured out from the wrench which acts on the end $w_{n+1}$. This recursive computation goes through all the intermediate joints until $i=1$ to obtain $w_1$.

3. Define Tip-Over Stability Constraint of WMDAR

In this section, a new stability criterion for the overturning moment is proposed, which is obtained by improving the WMDAR. Then, the tip-over stability margin (TOSM) is defined and converted into a tip-over stability constraint. Figure 4 shows various forces/moments on the robot system. The large ellipse represents the mobile platform, ${\left[\begin{array}{cc}{f}_1& m_1\end{array}\right]}^T$; ${\left[\begin{array}{cc}{f}_2& m_2\end{array}\right]}^T$ represents the force/torque of the body acting on the left and right arm joints; $f_{A1},f_{A2}$ are the gravity vectors of the loads on the left and right arm ends, respectively; $f_g$is the gravitational vector of the robot human body and the moving platform; $f_M$ is the inertial force acting on the center of mass of body; ${}_{}{}^{n}p{}_i{}_{}{}^{n}p{}_{i+1}$ represents the tip-over axis. The reaction force from joint 1 of the arm into the body with a wrench is indicated as $-w_{1\_i}$.

$$-w_{1\_i}={[f_i{}^T, m_i^T]}^T\left(i=1,2\right)$$

(18)

The tip-over moment of the mobile platform around the tip-over axis can be obtained by the following formula.

$$\begin{gathered} TO{M}_{ii+1}=\left(f_g\times l_{ii+1}\right)\cdot a_{ii+1}+\left(f_M\times l_{ii+1}\right)\cdot a_{ii+1}+{\displaystyle \sum }_{i=1}^k[\left(m_i\cdot a_{ii+1}\right) \\ +\left(f_i\times d_{ii+1}\right)\cdot a_{ii+1}+\left(f_{Ai}\times s_{ii+1}\right)\cdot a_{ii+1}]\left(k=1,2\right) \end{gathered}$$

(19)

In Equation (19), the first item is the weight torque of the human body and the mobile platform. The next term is the force/torque at the left and right arm joint 1 and the torque of the end load acting on the tip-over axis. $l_{ii+1}$ represents an orthogonal vector from the point $O_S$ to the tip-over axis $a_{ii+1}$. $d_{ii+1}$ represents an orthogonal vector from the point $O_{L1} \mathrm{or} O_{R1}$ to the tip-over axis $a_{ii+1}$. $s_{ii+1}$ represents an orthogonal vector from the point $O_{LL} \mathrm{or} O_{RR}$ to the tip-over axis $a_{ii+1}$.

In case of tipping, the robot will tip out along the tipping axis. The tipping axis is formed by two adjacent wheels, and the unit vector can be acquired by two neighboring coordinate wheel topography contact points.

$$a_{ii+1}=\frac{{}_{}{}^{n}p{}_{i+1}-{}_{}{}^{n}p{}_i}{\left|{}_{}{}^{n}p{}_{i+1}-{}_{}{}^{n}p{}_i\right|}$$

(20)

For the WMDAR system, the minimum moment acting on each tip-over axis of the mobile platform is:

$$TOM=min\{TO{M}_{12}, TO{M}_{23}, \cdots , TO{M}_{n1}\}$$

(21)

It can be seen from the above formula that when the minimum value of TOM of the robot human system is lower than 0. That is, the TOM of the robot human system along tip-over axis is outside, and the robot human system will overturn. Therefore, the tip-over stability margin (TOSM) is defined as:

$${\psi }_{ii+1}=\frac{TO{M}_{ii+1}}{TO{M}_{norm}}$$

(22)

$$\psi =min\{{\psi }_{12}, {\psi }_{23}, \cdots , {\psi }_{n1}\}$$

(23)

$TO{M}_{norm}$ in Equation (22) is a constant value, which is the minimum TOM exerting on the tip-over axis when the WMDAR is in a stable condition. In other words, $TO{M}_{norm}>0$. Thus, the minimum TOSM $\psi$ can be used to evaluate the stability of the robot when $\psi$ > 0. In other words, the minimum TOM on the tip-over axis will exceed 0, and the WMDAR system will become more reliable and stable. Whereas, when $\psi$ < 0, it shows that there is a moment on the flip axis, which is directed outward, and this situation represents that the system might flip.

This improved tip-over stability criterion takes into account the effects of joint force/torque of the robot arm and speed/acceleration of the mobile platform. In addition, the tip-over stability margin value can be used to evaluate the stability of the robot. Therefore, the tip-over stability margin can be regarded as the stability constraint, and then the trajectory of the robot can be planned.

4. Trajectory Planning Model

4.1. Joint Trajectory Parameterization

The trajectory of the robot can be regarded as a set of discrete configuration sequences with time as the independent variable. To ensure the stability and reliability of the robot human system during operation, the discrete sequence should be as continuous and smooth as possible. In this paper, the motion state of the WMDAR system only discusses 14 degrees of freedom, since the waist and head degrees of freedom are not considered. If a group of interpolation nodes are found between the start and end of the robot, and are connected with each other by smooth curves, the continuous smooth trajectory planning of the robot under constraints can be realized.

It is assumed that the trajectory of the robot joint $i$ can be described by a sequence composed of a group of $m+1$ nodes, and the sequence is $\left\{q_0,q_1,\cdots ,q_m\right\}$. The time interval between adjacent nodes is $\left\{T_1,T_2,\cdots ,T_m\right\}$. The total time of motion is $T={{\displaystyle \sum }}_{i=1}^m{T}_i$. The five-degree B-spline curve is used to parametrically describe the joint trajectory to keep the acceleration curve continuous and smooth. The B-spline equation is:

$$p\left(\mu \right)={\displaystyle \sum }_{j=0}^n{d}_j{N}_{j,k}\left(\mu \right)$$

(24)

where $n$ represents the number of splines, $j$ is the control point serial number, and there are $n+k$ control points in total. $d_j$ are the control points, and $N_{j,k}\left(t\right)$ is a k-degree canonical B-spline basis function. Where $\mu \in \left[{\mu }_i,{\mu }_{i+1}\right]$ is a normalized time vector ($0\le \mu \le 1$), $p\left(\mu \right)$ is the position of the joint at time $t$, and $k$ represents the number of B-splines. In this paper, $k=5$.

To better define the basis function of the spline curve, a node vector is introduced, $t=\left[t_0,t_1,\cdots ,t_{n-1},t_n\right]$. Each curve is divided into n segments in joint space. The position $q_i$ of the end point of each curve is known. The continuity conditions are used for intermediate nodes.

$$p_i\left(1\right)=p_{i+1}\left(0\right)$$

(25)

The $n-1$ equations can be obtained by substituting Equation (25) into Equation (24). Six constraints are introduced to keep the curve continuous and smooth.

$$\left\{\begin{array}{c}{p}_1\left(0\right)={\theta }_o,p_n\left(1\right)={\theta }_f\\ p_1{}^{\left(1\right)}\left(0\right)=v_o,p_n{}^{\left(1\right)}\left(1\right)=v_f\\ p_1{}^{\left(2\right)}\left(0\right)=a_o,p_n{}^{\left(2\right)}\left(1\right)=a_f\end{array}\right.$$

(26)

The derivative vectors of each order can be obtained according to the De Boor recursive formula.

$$p^r\left(\mu \right)={\displaystyle \sum }_{j=i-k+r}^i{d}_j^r{N}_{j,k-r}\left(\mu \right)$$

(27)

where, $d_j^l=\left\{\begin{array}{c}{d}_j, l=0\\ \left(k+1-l\right)\frac{d_j^{l-1}-d_{j-1}^{l-1}}{t_{j+k+1-l}-u_j},l=1,2,\cdots ,r ; j=i-k+l,\cdots i\end{array}\right.$.

The $n+5$ equations can be obtained by synthesizing Formulas (24)–(26), and then $n+5$ control points can be obtained. Finally, the B-spline equation is introduced to obtain the curve with a time interval as the parameter.

4.2. Trajectory Optimization Model

(1)

Optimization objective function

The objective function is to minimize the time and energy consumption of the robot human system.

$$\min \left\{\begin{array}{c}T={\displaystyle \sum }_{i=1}^n{T}_i\\ E={{\displaystyle \int }}_0^T\frac{1}{2}{\dot{q}}^{T}D\left(q\right)\dot{q}dt\end{array}\right.$$

(28)

where, $\dot{q}$ is the angular velocity of joint, $D\left(q\right)$ is the inertia matrix of robot, $D\left(q\right)={{\displaystyle \sum }}_{i=1}^n{\left(J_i\left(q\right)\right)}^T{M}_i{J}_i\left(q\right)$, $J_i\left(q\right)$is object Jacobian matrix, and $T$ is the whole motion period.

(2)

Optimization constraints

The margin to control the flip stability of the robot has the following constraint during the whole motion period.

$$\psi =min\{{\psi }_{12}, {\psi }_{23}, \cdots , {\psi }_{n1}\}>const$$

(29)

where $const$ is constant, in this paper, $const>0$.

The maximum joint angle, maximum acceleration, maximum speed, and maximum torque of each joint are also considered. The joint space constraints are defined as follows.

$$\left\{\begin{array}{c}\left|q_i\left(t\right)\right|\le q_i^{max}\\ \left|{\dot{q}}_i\left(t\right)\right|\le {\dot{q}}_i^{max}\end{array},\begin{array}{c}\left|{\ddot{q}}_i\left(t\right)\right|\le {\ddot{q}}_i^{max}\\ \left|{\tau }_i\left(t\right)\right|\le {\tau }_i^{max}\end{array}\right.$$

(30)

The initial and end positions, speed, and acceleration of the robot also have some constraints.

Positional boundary constraints:

$$\left\{\begin{array}{c}\Omega \left(t=0\right)={\Omega }^{start}\\ \Omega \left(t=T\right)={\Omega }^{goal}\end{array}\right.$$

(31)

And the speed/acceleration boundary constraint:

$$\left\{\begin{array}{c}\dot{q}\left(t=0\right)=0,\ddot{q}\left(t=0\right)=0\\ \dot{q}\left(t=T\right)=0,\ddot{q}\left(t=T\right)=0\end{array}\right.$$

(32)

4.3. NSGA-II Algorithm Based on the Distance of Reference Points

Kalyanmoy Deb [29] improved the basic genetic algorithm and proposed the adaptive Non-dominated Sorting Genetic Algorithm II (NSGA-II). Although the NSGA-II algorithm has high computational efficiency, it is difficult to choose among many solutions in the Pareto front. Therefore, in order to find the Pareto optimal solution of interest to decision makers, a dominance principle based on the distance of reference points is proposed.

The dominance principle is introduced into the sorting part of the NSGA-II algorithm. The main feature of the dominating sorting algorithm is to retain the solution closest to the reference point as the optimal solution to the next generation; that is, the solution dominates other solutions in Pareto, as shown in Figure 5.

In order to determine the optimal solution near the reference point, it is necessary to use an achievement scalar function (ASF) to measure the near–far relationship between the Pareto solution and the reference point [30,31]. The weighted Euclidean distance is used in this paper.

$$Dist\left(x,g\right)=\sqrt{{\displaystyle \sum }_{i=1}^M{w}_i{\left(\frac{f_i\left(x\right)-f_i\left(g\right)}{f_i^{max}-f_i^{min}}\right)}^2}$$

(33)

where $x$ is the solution, $g$ is the reference point, $f_i^{ma{x}_i^{min}}$ are the maximum and minimum values of the ith objective function, respectively, and $w_i$ is the weight of the i-th target.

Suppose solution $x_1$ dominates solution $x_2$, if and only if the following conditions are met:

$$D\left(x_1,x_2,g\right)=\frac{Dist\left(x_1,g\right)-Dist\left(x_2,g\right)}{Distmi{n}_{max}<-\delta }$$

(34)

where $\delta$ is the threshold of the dominant relationship, which is mainly used to control the distribution range of the solution, $\delta \in \left[0,1\right]$.

The NSGA-II algorithm process is analyzed as follows in Figure 6, and the parameters of the NSGA-II algorithm are shown in

Table 2. The parameters of the NSGA-II algorithm.

Parameters	Value
Population (Pop)	100
The number of termination iterations (Gen)	50
Probability of crossover (Pc) Probability of mutation (Pm)	0.9 0.1

.

5. Robot Motion Simulation Experiment

5.1. Determine the Motion Scheme

The motion scheme is when the robot rotates an arc to reach the specified position, and the arm also moves to the corresponding posture to prepare for the next object grasping. It is assumed that the global reference coordinate system $O_W{X}_W{Y}_W{Z}_W$ and the base coordinate system $O_S{X}_S{Y}_S{Z}_S$ coincide at the beginning of the simulation.

The coordinates of the initial state of the platform center are ${\theta }_{OS}{}^{start}={\left[0,0\right]}^T$. The initial joint angle of the left arm is ${\theta }_l{}^{start}={\left[0,0,0,0,0,0\right]}^T$. The initial joint angle of the right arm is ${\theta }_r{}^{start}={\left[0,0,0,0,0,0\right]}^T$. When the robot simulation ends, the center coordinate of the mobile platform (relative to the global reference coordinate system $O_W{X}_W{Y}_W{Z}_W$) is ${\theta }_{OS}{}^{end}={\left[2,0.4\right]}^T$. The joint angle of the left arm is ${\theta }_l{}^{end}={\left[{24}^{\circ },-{35}^{\circ }, {50}^{\circ },-{75}^{\circ },-{78}^{\circ },{95}^{\circ }\right]}^T.$ The joint angle of the right arm is ${\theta }_r{}^{end}={\left[{75}^{\circ },{90}^{\circ },-{42}^{\circ },{80}^{\circ },{85}^{\circ },{45}^{\circ }\right]}^T$. The robot model is imported into ADAMS software, and the motion scheme is shown in Figure 7.

5.2. Trajectory Optimization Model Solving

The variation range of the optimization time interval is determined according to the motion scheme, T_min = [0.5, 1.5, 1.0, 1.0, 1.5, 0.5]^T, T_max = [1.5, 2.5, 2.0, 2.0, 2.5, 1.5]^T. The stability constraint $\psi >const=0.35$. Therefore, comparisons are made in two cases. Case 1: stability constraints are not considered. Case 2: stability constraints are considered ($\psi >0.35$). Then, the improved NSGA-II algorithm is run, and the two situations are compared and analyzed, as shown in the Figure 8.

The Pareto-optimal solutions based on time and energy are solved by the improved NSGA-II optimization algorithm in Figure 8. The selection principle of the reference points is the average of the maximum and minimum values of the Pareto-optimal solutions. It can be found that the reference points in two cases are basically near points (8.1, 1634.9) and (9.5, 1320.9), according to the many optimization results. We set the weight vector as ${[0.4, 0.5]}^T$ in the Euclidean distance between the Pareto solution and the reference point. The threshold in Euclidean distance-dominated ranking is $\delta =0.001$. The solutions of interest to the decision-maker are shown in the Figure 9. Then, 10 groups of optimal solutions are selected according to the population size, as shown in

Table 3. Optimal solutions of case 1.

Serial Number	${\mathit{T}}_1\left(\mathit{s}\right)$	${\mathit{T}}_2\left(\mathit{s}\right)$	${\mathit{T}}_3\left(\mathit{s}\right)$	${\mathit{T}}_4\left(\mathit{s}\right)$	${\mathit{T}}_5\left(\mathit{s}\right)$	${\mathit{T}}_6\left(\mathit{s}\right)$	${\mathit{T}}_{\mathit{a}\mathit{l}\mathit{l}}\left(\mathit{s}\right)$	$\mathit{E}\left(\mathit{J}\right)$
1	1.1140	1.6535	1.3018	1.2634	1.4712	1.2246	8.0315	1500.9
2	1.0942	1.7238	1.2179	1.3475	1.4469	1.2304	8.0606	1481.9
3	1.1027	1.7351	1.2888	1.2526	1.4841	1.2101	8.0732	1472.6
4	1.1197	1.6483	1.3144	1.3328	1.4763	1.2126	8.1041	1471.2
5	1.0915	1.6574	1.1741	1.2395	1.5494	1.2216	8.1267	1464.1
6	1.1175	1.6546	1.2893	1.3455	1.5585	1.2161	8.1381	1462.1
7	1.1026	1.7281	1.2716	1.3435	1.4725	1.2332	8.1793	1454.3
8	1.1234	1.7106	1.2653	1.3204	1.5439	1.2347	8.1983	1436.6
9	1.0960	1.7435	1.3040	1.2535	1.5232	1.2162	8.2096	1436.4
10	1.1043	1.6145	1.2931	1.2561	1.5723	1.2275	8.2571	1423.5

and

Table 4. Optimal solutions of case 2.

Serial Number	${\mathit{T}}_1\left(\mathit{s}\right)$	${\mathit{T}}_2\left(\mathit{s}\right)$	${\mathit{T}}_3\left(\mathit{s}\right)$	${\mathit{T}}_4\left(\mathit{s}\right)$	${\mathit{T}}_5\left(\mathit{s}\right)$	${\mathit{T}}_6\left(\mathit{s}\right)$	${\mathit{T}}_{\mathit{a}\mathit{l}\mathit{l}}\left(\mathit{s}\right)$	$\mathit{E}\left(\mathit{J}\right)$
1	1.2780	1.9981	1.4066	1.1603	1.6607	1.3595	8.8632	1233.8
2	1.2460	1.9310	1.3428	1.2604	1.7834	1.3281	8.8917	1233.3
3	1.2637	2.0259	1.4131	1.1594	1.7355	1.3137	8.9113	1232.9
4	1.2951	2.0069	1.4217	1.1844	1.6918	1.3372	8.9371	1216.5
5	1.2380	2.0180	1.4084	1.2698	1.7691	1.3068	9.0101	1199.1
6	1.2898	2.0062	1.3376	1.1794	1.8908	1.3522	9.0560	1186.9
7	1.2999	1.9775	1.3980	1.2375	1.8474	1.3625	9.1228	1174.6
8	1.2925	1.9080	1.3222	1.4640	1.7820	1.3618	9.1305	1171.7
9	1.2660	1.9884	1.3046	1.3985	1.8742	1.3101	9.1418	1164.5
10	1.2420	1.8977	1.4467	1.3512	1.8872	1.3200	9.1448	1162.0

.

The optimal solution is selected according to the principle that the accelerations at the starting point and the end point are as small as possible; that is, the range of T₁ and T₆ is as large as possible. Therefore, the data of group 8 in Table 3 and group 7 in Table 4 are selected for analysis. The analysis results are shown in

Table 5. Analysis of the two cases.

Optimal Solution	Optimization Case 1	Optimization Case 2	Variation	Percentage Change
Energy-optimal solution $E\left(J\right)$	1436.6	1174.6	Down $\downarrow$	18.20%
Time-optimal solution $T_{all}\left(s\right)$	8.1983	9.1228	Up ↑	11.28%

.

It can be seen from Table 5 that the optimal solution in optimization case 2 increases by 11.28% compared with that in optimization case 1; however, the optimal solution in energy reduces by 18.20%. The decrease of the energy-optimal solution is greater than the increase of the time-optimal solution in two cases considering different stability constraints from Table 5. To sum up, the different stability constraints have a great impact on robot trajectory planning. The greater the stability constraint, that is, the larger $\psi$ (tip-over stability margin) is, the much greater the decrease of robot trajectory planning energy than the increase of time, which is of great practical significance for mobile robots that need battery drive.

5.3. Analysis of Optimization Results

The main purpose of this optimization is to improve the stability of the robot human system. Therefore, the stability criterion and TOSM proposed above are used to qualitatively and quantitatively analyze the stability and reliability of the robot. At the same time, tip-over moment is closely related to joint motion according to the definition. Therefore, it is necessary to analyze the angular acceleration and angular velocity of each joint to further verify whether the robot moves smoothly. The variation curve of the minimum TOSM of the robot under the two optimization conditions is shown in Figure 10.

It can be seen from Figure 10 that the stability of the robot human system is different under different stability constraints. In case 1, the minimum TOSM $\psi$ is in the range of 0.2~0.6 due to less running time. The motion oscillation of the robot is relatively large from the simulation experiment in ADAMS software during this time period, resulting in smaller TOSM and lower robot stability. In case 2, the minimum TOSM $\psi$ is greater than 0.35. The minimum TOSM $\psi$ is greater and the stability of the robot is better than that in case 1. In summary, the minimum TOSM index can be used to quantitatively evaluate the stability of the robot human system. It can also be used to analyze the stability of other mobile robots. Next, the displacement, velocity, and acceleration curves of each joint under the two optimization cases of the robot are also shown in Figure 11 and Figure 12.

It can be seen from Figure 11 and Figure 12 that the angular velocity and angular acceleration of the robot joint decrease significantly when considering stability constraint. Meanwhile, the robot has better stability when considering the stability constraint. In order to quantitatively analyze the joint angular velocity and angular acceleration, the following tables are listed for data analysis, as shown in

Table 6. The effects of different stability constraints on joint velocity.

	Joint Serial Number	$\left|{\mathit{v}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}\_\mathit{c}\mathit{a}\mathit{s}\mathit{e}1}\right|$	$\left|{\mathit{v}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}\_\mathit{c}\mathit{a}\mathit{s}\mathit{e}2}\right|$	Percentage Change	$\left|{\mathit{v}}_{\mathit{m}\mathit{a}\mathit{x}\_\mathit{c}\mathit{a}\mathit{s}\mathit{e}1}\right|$	$\left|{\mathit{v}}_{\mathit{m}\mathit{a}\mathit{x}\_\mathit{c}\mathit{a}\mathit{s}\mathit{e}2}\right|$	Percentage Change
Left arm	Joint 1	2.8910	2.6190	$\downarrow$9.41%	6.4963	5.7032	$\downarrow$12.21%
	Joint 2	4.2417	3.8249	$\downarrow$9.83%	9.4964	8.9211	$\downarrow$6.06%
	Joint 3	6.0687	5.4717	$\downarrow$9.84%	11.9745	11.3003	$\downarrow$5.63%
	Joint 4	9.0835	8.2043	$\downarrow$9.68%	19.3534	17.9745	$\downarrow$7.12%
	Joint 5	9.4287	8.5083	$\downarrow$9.76%	21.5095	19.8789	$\downarrow$7.58%
	Joint 6	11.5040	10.4060	$\downarrow$9.54%	21.6774	19.2030	$\downarrow$11.41%
Right arm	Joint 1	9.0771	8.1354	$\downarrow$10.37%	14.3601	12.9474	$\downarrow$9.84%
	Joint 2	10.8972	8.8202	$\downarrow$19.06%	21.7574	17.4806	$\downarrow$19.66%
	Joint 3	5.0876	4.5726	$\downarrow$10.12%	10.1924	8.2633	$\downarrow$18.93%
	Joint 4	9.7424	8.7512	$\downarrow$10.17%	22.2041	16.3423	$\downarrow$26.4%
	Joint 5	10.3016	9.2776	$\downarrow$9.94%	17.6119	14.4317	$\downarrow$18.06%
	Joint 6	5.4250	4.8701	$\downarrow$10.23%	11.0025	9.0673	$\downarrow$17.59%
Drive where	Left wheel	191.7866	173.2072	$\downarrow$9.69%	361.3142	351.4745	$\downarrow$2.72%
	Right wheel	320.1776	288.9103	$\downarrow$9.77%	608.9494	598.4260	$\downarrow$1.73%

Note: The unit of angular velocity is ($°/s$), “$\downarrow$” Represents the percentage decline in case 2 over case 1.

and

Table 7. The effects of different stability constraints on joint acceleration.

	Joint Serial Number	$\left|{\mathit{a}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}\_\mathit{c}\mathit{a}\mathit{s}\mathit{e}1}\right|$	$\left|{\mathit{a}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}\_\mathit{c}\mathit{a}\mathit{s}\mathit{e}2}\right|$	Percentage Change	$\left|{\mathit{a}}_{\mathit{m}\mathit{a}\mathit{x}\_\mathit{c}\mathit{a}\mathit{s}\mathit{e}1}\right|$	$\left|{\mathit{a}}_{\mathit{m}\mathit{a}\mathit{x}\_\mathit{c}\mathit{a}\mathit{s}\mathit{e}2}\right|$	Percentage Change
Left arm	Joint 1	0.0393	0.0081	$\downarrow$79.39%	6.6119	4.6136	$\downarrow$30.22%
	Joint 2	0.0538	0.0083	$\downarrow$84.57%	7.0834	5.6161	$\downarrow$20.71%
	Joint 3	0.0494	0.0331	$\downarrow$33.00%	8.5732	7.0212	$\downarrow$18.1%
	Joint 4	0.1672	0.0218	$\downarrow$86.96%	15.8264	13.0212	$\downarrow$17.72%
	Joint 5	0.1538	0.0123	$\downarrow$92.00%	17.8643	16.2621	$\downarrow$8.97%
	Joint 6	0.1897	0.0458	$\downarrow$75.00%	16.8167	11.3236	$\downarrow$32.66%
Right arm	Joint 1	0.0130	0.0068	$\downarrow$47.69%	15.5193	12.8832	$\downarrow$16.99%
	Joint 2	0.1771	0.1027	$\downarrow$42.01%	16.9412	14.2035	$\downarrow$16.16%
	Joint 3	0.0755	0.0143	$\downarrow$81.06%	7.9407	6.0935	$\downarrow$23.26%
	Joint 4	0.1496	0.0104	$\downarrow$93.05%	19.2833	8.4394	$\downarrow$56.23%
	Joint 5	0.1039	0.0242	$\downarrow$76.71%	15.5865	12.4621	$\downarrow$20.05%
	Joint 6	0.0850	0.0602	$\downarrow$29.18%	9.6239	7.1614	$\downarrow$25.59%
Drive where	Left wheel	1.7202	1.2085	$\downarrow$29.75%	255.8227	199.1425	$\downarrow$22.16%
	Right wheel	2.8519	2.0278	$\downarrow$28.9%	423.9443	350.3461	$\downarrow$17.36%

Note: The unit of angular acceleration is ($°/s^2$), “$\downarrow$” Represents the percentage decline in case 2 over case 1.

.

It can be seen from Table 6 and Table 7 that the maximum and average values of joint velocities in Case 2 mostly decrease by about 10% when the tipping stability constraint is considered. The better the stability of the robot, the larger the reduction in joint velocity. At the same time, the maximum and average values of joint acceleration in Case 2 decreased more than those in Case 1. According to the analysis of ADAMS simulation, the overall oscillation phenomenon is more obvious in Case 1, and the stability of the robot decreases; in Case 2, the oscillation phenomenon is significantly reduced, and the dynamic stability of the robot is better. In the case of the overturning stability constraint, the time is slightly increased, but the robot joint speed and acceleration are significantly reduced and the stability is improved. Therefore, joint trajectory planning based on stability constraints is an important guide for smooth operation of the robot and can make the robot’s operation more reliable.

6. Conclusions

To define the tip-over stability constraint of WMDAR, the forces and moments between wheel-terrain and body-arm have been analyzed by kinematics and force analysis of robot. A new stability criterion for the overturning moment is proposed, which is obtained by improving the WMDAR. A new stability constraint for calculating TOSM is derived based on the improved stability criterion. Then, an optimal joint trajectory planning model based on time and energy is proposed. The optimization model is solved according to the quintic B-spline interpolation algorithm and an improved NSGA-II optimization algorithm, and the robot motion simulation experiment is conducted. The trajectory planning and optimization can improve the efficiency and stability of the WMDAR system through the analysis of two optimization cases. Some main conclusions are drawn through graphical and theoretical analysis, as follows:

• When the joint angular velocity and angular acceleration are reduced, the energy reduction is much greater than the time increase, and the stability of the robot is clearly improved, which has obvious practical significance for the battery-powered mobile robot.
• This trajectory planning method can be applied to other mobile robots, and can also provide guidance for other mobile robots’ trajectory path planning.