Trajectory Planning and Speed Control for a Two-Link Rigid Manipulator

Contributed by the Mechanisms Committee for publication in the JOURNAL OF MECHANICAL DESIGN. Manuscript received March 1999. Associate Editor: G. S. Chirikjian. Industrial manipulators are frequently required to perform different tasks and to carry different masses. Further, if a manipulator operates in space crowded with obstacles, it must follow a planned path very closely to avoid collisions, especially if other moving objects are present. To follow trajectory and the velocity profiles simultaneously is a difficult task. One option is to divide the problem into two parts: path planning and speed control. Usually, the desired trajectory is given as a sequence of knots (positions of the robot’s tip) in space Cartesian coordinates, where the velocity and the acceleration of the joints are subject to constraints. The control is performed at the joint level and it is desirable, therefore, to construct the trajectory at that level. The given knots are first transformed into two sets of joint displacements, and then piecewise approximation polynomials are used to fit these two sequences of joint displacements. Cubic polynomials are sufficiently smooth to provide for continuous motion 12. The function of approximation for the joint trajectory passes through the given knots and provides sufficient information (at the knots as well as at the intermediate points) for the controller. Path planning has been studied by a number of authors. A combined trajectory planning and adaptive control of a two-link rigid manipulator (TLRM) was presented in Fotouhi-C., Nikiforuk and Szyszkowski 3. An optimum path planning problem at the joint level using the cubic spline polynomial was given in Lin, Chang and Luh 4 and later in Xiangrong and Xiangfeng 5. A similar problem was repeated in Wang and Horng 6 where minimum-time path planning for robot manipulators using cubic B-spline functions was solved. The approach used in Wang and Horng 6 was similar to Lin et al. 4, but used the B-spline function rather than a spline function. A procedure to construct a robot joint trajectory using B-splines was given in Thompson and Patel 7. With the B-spline approach, it was claimed that the local modification of the path was possible for one or more joints without affecting the other joints. However, unlike the other two papers, a search for minimum-time was not given in Thompson and Patel 7. A two-part trajectory planning for a robot manipulator was proposed in Wu and Jou 8. A cubic spline function was used to plan the geometric trajectory as well as the speed. The problem was transformed then into solving an initial boundary value problem. Numerical results were given only for maintaining a constant speed along the geometric path. Two algorithms for fine-tuning B-spline motions were presented in Srinivasan and Ge 9. A path-smoothing algorithm and a speed-smoothing algorithm were used to keep the path highly smooth and to keep unintended speed variation to a minimum. The speed algorithm used a rational spline image curve to obtain a near constant kinetic energy. Robot path planning using the concept of trigonometric splines was discussed in Simon and Isik 10. It was stated that the trigonometric splines outperformed algebraic splines. In this paper, the piecewise cubic spline function is used to construct the joint trajectories with a given speed profile. The problem is divided into two parts: geometric trajectory planning and trajectory speed control which is the main contribution of this paper. The path planning is done at the joint level using cubic spline functions. The trajectory of the robot is specified by a sequence of knots (positions of the robot’s tip) in space Cartesian coordinates. These knots are then transformed into two sets of joint displacements. Linear scaling of the time variable is applied to accommodate the velocity and acceleration constraints. A new approach, which uses a nonlinear scaling of the time variable to fit the manipulator’s tip velocity to a pre-specified profile, is proposed for speed control. Unlike in Wu and Jou 8, this approach can be implemented for any speed profile, not only for constant speed. In the simulation, a semi-elliptical path was used as the desired trajectory to be followed by the tip of a TLRM. The TLRM shown in Fig. 1 can be modeled as a set of two rigid bodies connected in a serial chain with or without friction, having the equation of motion (1)Mφφ¨t+Qφ,φ˙=τtwhere the mass matrix M and the force vector Q are nonlinear functions of the degrees of freedom (2)Mφ=a11a12a12a22(3)Qφ,φ˙=−a132φ˙1+φ˙2φ˙2+a14g+a13φ˙12+a24gand aij are known functions of the physical parameters and rotations φ1,φ2 [see Fotouhi-C., Szyszkowski, Nikiforuk and Gupta 11 for details]. The vector of controls τ is represented by the torques τ1 and τ2, and φ is the vector of the degrees of freedom representing the angular position of the shoulder and elbow links of the manipulator. The objective is that the manipulator’s tip follows a desired trajectory in the space y,z specified by zbd. The positions (knots) in the y−z space can be easily converted into positions specified at the joint level, φ1−φ2. The states φ1,φ˙1 are the rotation and angular velocity of the shoulder link, φ2,φ˙2 are the rotation and angular velocity of the elbow link, and g is the gravitational acceleration. The geometric trajectory is obtained by N knots in the space coordinates system y,z given by the pairs yi,zi,i=1,…,N. These knots can be transformed into the joint rotations φ1i,φ2i of the manipulator by the following transformation (Fig. 1) (4)φ1=α−χ(5)χ=cos−1y2+z2+l12−l222l1y2+z2(6)α=tan−1zy(7)φ2=cos−1y2+z2−l12−l222l1l2Initially, these joint knots can be fitted into two sets of cubic polynomials using the cubic spline interpolation considering constant time steps, dt, between the knots, calculated as (8)dt=tf/N−1where tf is the desired traveling time. Once a trajectory has been fitted to an initial set of cubic splines, a linear scaling is used to adjust the time intervals between the points so that the maximum angular velocity and acceleration of each joint are not exceeded. By adjusting the time intervals between each pair of adjacent points, it is always possible to obtain a feasible solution to the trajectory planning problem. The time variable t is replaced by the scaled time t¯=λt, where λ is the adjustment factor. The joint velocity and joint acceleration are then (9)dφdt¯=1λdφdtd2φdt¯2=1λ2d2φdt2The velocities and accelerations are matched for each joint to pre-specified constraints. If the constraints are specified in terms of the velocity and acceleration of each joint then (10)|φ˙1t|⩽VL1|φ˙2t|⩽VL2(11)|φ¨1t|⩽AL1|φ¨2t|⩽AL2The scaling factor λ can be selected as follows (12)λ1=maxmax|φ˙1t|VL1,max|φ˙2t|VL2(13)λ2=maxmax|φ¨1t|AL1,max|φ¨2t|AL2(14)λ=max1,λ1,λ2Then, the scaled time variable t¯ and the scaled velocities and accelerations are (15)t¯=λtφ˙t¯=1λ φ˙tφ¨t¯=1λ2 φ¨tThe preceding discussion transfers the infeasible solution to a feasible one. It meets the constraints imposed in the form of the maximum allowed angular velocity and acceleration. In this section the question of fitting a specified profile to the velocity along the trajectory is addressed. This profile of the linear velocity of the tip of manipulator is specified as vbs in space coordinates. Equation (22) is an example of vbs.Nonlinear scaling is used this time to adjust the time intervals as follows. The time step variable dt is replaced by the scaled time step dt¯=λidt. The factor λi is used to adjust the time step between ti and ti+1 and is calculated from (16)λi=ciλi+1−ciλi+1where (17)ci=vbstivbsti+vbsti+1λi=|vbti||vbsti|Velocities vbti and vbsti are the actual and specified linear velocities of the tip of manipulator, respectively. Next, the time variable t is replaced by the scaled time (18)t¯=t¯i+λit−tifor ti⩽t⩽ti+1 (and for t¯i⩽t⩽¯t¯i+1). The scaled velocities and accelerations are (19)φ˙t¯i=1λi φ˙tiφ¨t¯i=1λi2 φ¨tiThe procedure of calculating the adjustment factors λi is repeated in order to either reduce the least square error norm ‖E‖ to zero or to bring the norm ‖λ‖ to one. (20)‖E‖=∑i=1NEi2/N‖λ‖=∑i=1Nλi2/NThe error between actual tip velocity, vbt¯i, and specified tip velocity, vbst¯i, for every scaled time step is defined as (21)Ei=Et¯i=|vbt¯i|−|vbst¯i|max|vbt¯i|,|vbst¯i|The norm ‖λ‖ indicates the rate of scaling at the knots for each iteration. There is no further improvement when this norm reaches one. To obtain a smooth convergence of the iterations some limits on λi can be imposed, such as, λmin⩽λi⩽λmax, where λmin<1, and λmax>1. For simulation purposes the following limits were used: λmin=0.5,λmax=1.5 for first iteration; and λmin=0.25,λmax=4.0 for other iterations. For simulation purposes a trajectory of shape shown in Fig. 2, consisting of elliptical pieces, with a particular tip velocity profile and obeying angular velocity and acceleration constraints, was considered. The initial and the desired trajectories in the y−z plane, which practically coincided, are shown in this figure. In this analysis, I1 and I2 are the mass moment of inertia of the links w.r.t. their centers of mass, m1 and m2 are the masses of the shoulder and the elbow links, respectively, ma is the mass at the elbow joint, and mb is the mass at the tip of the manipulator. The lengths of the links are l1 and l2, whereas lc1,lc2 locate the centers of mass of the links. In the simulation the physical parameters of the manipulator were set to: l1=2lc1=1.6[m],l2=2lc2=1.6[m],m1=1[kg],m2=1[kg],ma=1[kg],mb=4[kg],g=9.81[m.s−2], and I1=0=I2.For the cubic spline interpolation 61 knots (points position) were chosen along the curve. Initially, the maneuver pattern was assumed somewhat arbitrarily with the maneuver time tfin=8π seconds. Several knots with the initial time assignment assumed as t1in=0,t6in=2π/3,t26in=10π/3,t31in=4π,t36in=14π/3,t56in=22π/3,t61in=tfin=8π are also indicated in Fig. 2. The cubic spline interpolation of the angles of rotation versus time for the initial maneuver are plotted in Fig. 3 (lines φ1din,φ2din). The angular velocity of the shoulder and elbow links, φ˙1din and φ˙2din, and the linear tip velocity of the manipulator, vbin, are shown in Figs. 4, 5 and 6, respectively. For the initial and final knots in the cubic spline interpolation the following conditions were used: φ˙11=φ˙10=0,φ˙1N=φ˙1tf=0,φ˙21=φ˙20=0, and φ˙2N=φ˙2tf=0. The velocity and acceleration constraints for each joint were VL1=0.35[rad/s],VL2=0.5[rad/s],AL1=0.4[rad/s2],AL2=0.8[rad/s2]. As can be observed from Figs. 4 and 5 for the initial trajectory, the angular velocity of the shoulder and elbow links reached about −0.4 rad/s and −0.6 rad/s respectively. Clearly, it did violate the constraints imposed on the angular velocity and angular acceleration of the joints. The procedure discussed in section 3.2 (using the linear scaling) was applied to satisfy these constraints. For the case presented, the desired profile of linear tip velocity, vbs, was specified as vbs=0.3 t−t1t6−t1fort1⩽t⩽t6vbs=0.3fort6⩽t⩽t26vbs=0.3t29−t+0.15t−t26t29−t26fort26⩽t⩽t29(22)vbs=0.15fort29⩽t⩽t33vbs=0.15t36−t+0.3t−t33t36−t33fort33⩽t⩽t36vbs=0.3fort36⩽t⩽t36vbs=0.3 t61−tt61−t56fort56⩽t⩽t61where the locations of t1,t6,t26,t29,t33,t36,t56,t61 were as shown in Fig. 2. The trajectory was next modified using the procedure in section 3.3 so as to approach the desired profile of the tip velocity. In general, the motion of the manipulator had to be slowed and the maneuver time increased to tf=31.9 seconds (Fig. 6). The rotations of the links for the desired path (that satisfied all the constraints) are shown in Fig. 3 (lines φ1d,φ2d). Note that these lines are stretched in time in comparison to the lines for the initial path φ1din,φ2din. The initial and the desired trajectories in the y−z plane are shown in Fig. 2. These trajectories practically coincided although different time values were assigned to the knots, e.g. t61in=tfin=25.1,t61=tf=31.9. The angular velocity of the shoulder link for the desired path, φ˙1d, is shown in Fig. 4 where the maximum value of the angular velocity of that link was −0.3 rad/s. Figure 6 shows the absolute linear velocity of the tip of the manipulator, vb, for the desired path. Note how the linear velocity, vb, followed the given profile of tip velocity vbs defined in Eq. (22). In particular, from t6 to t26 and from t36 to t56, this velocity fluctuated slightly around 0.3 m/s. Also from t29 to t33 this velocity fluctuated slightly around its lower value of 0.15 m/s. For the rest of the time vb almost perfectly followed vbs. Figure 7 shows the convergence of the procedure for fitting this linear velocity, vb, to its desired profile, vbs. As can be seen from this figure, vb almost convergences to vbs after only four iterations which can be attributed to the limiting value of λmin and λmax.Figures 8 and 9 show the error norm and second norm for the convergence of iteration for speed planning. As can be seen from Fig. 8, for N=61 the error norm could not be reduced to less than about .013, which can be attributed to the number of knots chosen. Actually, when the error norm reached a small value, in this case .0135, the error components and the factors λi started to oscillate from iteration to iteration without further reduction in the error norm magnitude. It was found that the greater the number of knots the smaller the error (Figs. 8 and 10). For example, doubling the number of knots to N=121, decrease the error norm approximately nine times to .0015 (see Fig. 10). The second norm or λ-norm, shown in Fig. 9 for N=61, was used to determine the best solution (iteration) when the error norm could not be reduced any further. The set that causes the least fluctuations of λi is selected, that is the set for which |1−‖λ‖| is minimum. A two-phase trajectory planning for a two-link rigid manipulator was presented. The problem was solved in two stages. First a geometric trajectory planning using cubic spline function was performed to construct an initial trajectory. The motion constraints (maximum angular velocity and maximum angular acceleration) were met using a linear scaling of the time variable. Then a new approach was applied to speed control (fitting the manipulator’s tip velocity to a pre-specified profile) using a nonlinear scaling of the time variable. It was demonstrated that the specified velocity profile can be followed very closely. Simulation results were presented which show the convergence and effectiveness of the path planning approach.

Fetched live from OpenAlex and de-inverted. Abstracts are not stored in this database: the inverted indexes are 8.6 GB of the frame’s 9.3 GB of text, and the host has 13 GB free.