Trajectory planning and control methodology 117 The other end of the cable connects to the end-effector mounting support by passing through each spring. For eliminating the effects of radial play in the pulley shaft, flange couplings are employed to minimize these effects. The cables are assembled in such a way that at the home-pose, the tension along the cables is high. The experiments are carried out for two orientations;
vertical and horizontal under no-load conditions. The entire experimental setup and it’s digital model are represented below in Figures 4.4a to 4.4d.
(a) (b)
(c) (d)
Motor-1 Motor-3
Motor-2
Figure 4.4 – Representation of the (a) Experimental setup of the tensegrity mechanism in vertical orientation , (b) Experimental setup of the tensegrity mechanism in horizontal orientation , (c) Digital model of the experimental setup and (d) one of the three ESCON 36/2 servo-controllers
provide the solutions to the IKP were studied in Chapter-3 as well as in [VC20] with respect to the Euler angles of the universal joint. The simpler form of the IKP from Figure 3.4a is given by:
(bix−cix)2+ (biy−ciy)2+ (biz−ciz)2 =l2i , withi= 1,2,3 (4.1) The length of each spring for a given tilt angle can be estimated using Eq. (4.1), which is a simpler form of Eqs. (3.18a) to (3.18c). The IKP for each spring is converted into angular displacements of the pulley, which will be the target position for each DC-motor. The equation for the desired angular position of the pulley with respect to the IKP is given by:
θdi= (li−lhome)
r , withi= 1,2,3 (4.2)
In Eq. (4.2), θdi is the desired angular position calculated from IKP for given input tilt angles η and ϕ. The parameterr indicates the pulley radius, which is 20 mm. At the home position, the value of lhome is 68 mm for rf = 56.7 mm and h= 0.6. For every angular displacement of the DC-motor, encoder data is passed to the BB black ports. The angular displacement of the pulley from the encoder channel information can be calculated by:
θmi= Eiπ
2CG , withi= 1,2,3 (4.3)
In Eq. (4.3), Ei indicates the output data of the encoder channels of Motor-i. The angle θmi is the measured angular displacement at the output shaft of the gearbox. C indicates the counts per revolution of the encoder, which is 500 and Gis the reduction ratio of the gearbox, which is taken from Table 4.2. At a given tilt angle, the mechanism tries to attain the position θdi. In order to minimize the errors between the desired and measured angular positions, the PID control algorithm is employed.
4.4.1 Trajectory generation
The trajectory planning approach proposed in [KD04], which was studied in Chapter-2 for the dynamic force model is employed for the control of the tensegrity mechanism. For every angular displacement of the DC-Motor, it is necessary to determine the joint velocities and accelerations for the mechanism. During the change of tilt angles from home-pose to the desired position, the cartesian velocities of the prismatic joints can be calculated using the equation [Car+15] :
l˙1
l˙2 l˙3
=Jc
"
˙ η ϕ˙
#
, withJc=
∂l1
∂η
∂l1
∂ϕ
∂l2
∂η
∂l2
∂ϕ
∂l3
∂η
∂l3
∂ϕ
(4.4)
Trajectory planning and control methodology 119 In Eq. (4.4),Jcrepresents the direct kinematics matrix (A) of the 3-SPS-U tensegrity mech-anism. Using Eq. (4.4), the joint velocities are estimated, followed by which the desired angular velocities of the output shaft can be estimated. For given input tilt angles, the trajectory gen-eration is carried out using a fifth-degree polynomial equation [KD04]. This equation is used to define the positions(t), velocity ˙s(t) and acceleration ¨s(t) for the mechanism and they are given by:
s(t) = 10 t tf
!3
−15 t tf
!4
+ 6 t tf
!5
(4.5)
˙
s(t) = 30 t2 t3f
!
−60 t3 t4f
!
+ 30 t4 t5f
!
(4.6)
¨
s(t) = 60 t t3f
!
−180 t2 t4f
!
+ 120 t3 t5f
!
(4.7) For the experiments, a linear trajectory and a circular trajectory are performed on the tensegrity mechanism for both orientations. The position, velocity and acceleration equations, as well as the maximum simulation time, varies according to the type of trajectory.
Linear trajectory
In the case of linear trajectory, the minimum travelling time tf is determined when the velocities and accelerations reach the saturation point in the trajectory. For the fifth degree polynomial, the value oftf for a linear trajectory is given by [KD04] :
tf = max 15|D|
8kv ,
s10|D|
√3ka
!
(4.8) In Eq. (4.8), |D| represents the norm of the position, kv represents the maximum velocity and ka represents the maximum acceleration. At home-position, let D1 be the vector that contains the initial tilt anglesηi andϕi. The final tilt positions are given by the vectorD2. The intermediate pose D is given by:
D(t) =D1+ (D2−D1)s(t) (4.9)
The corresponding velocity and acceleration vectors can be found using the first and second order derivative of Eq. (4.9) with respect to time. The equations are given by [Car+15] :
V(t) = (D2−D1) ˙s(t) (4.10)
A(t) = (D2−D1)¨s(t) (4.11)
The joint coordinate vector can be obtained by the equation:
q(t) =f(D(t)) (4.12) In Eq. (4.12), f = [l1, l2, l3]T is the vector that contains the solution of the IKP for given tilt angles. The joint velocities can be computed with the help of the Jacobian matrix Jcusing the equation:
˙
q(t) =JcV(t) (4.13)
The accelerations in the joints can be computed using the Jacobian matrix and it’s time deriva-tive. The equation is given by:
¨
q(t) =JcA(t) + ˙JcV(t) (4.14) The desired position, velocity and acceleration vectors of the joints from Eqs. (4.12) to (4.14) will be used as inputs for the PID algorithm to perform a linear trajectory.
Circular trajectory
Compared to the linear trajectory, the travelling time required by the mechanism is higher as the circular path goes from 0 to 2π radians. The value oftf is given by:
tf = max (γV,√
γA) (4.15)
In Eq. (4.15), γV is the ratio of maximum joint velocity to speed of motor and γA is the ratio of maximum joint acceleration to the acceleration of motor [Car+15]. Before the creation of circular trajectory, an initial linear trajectory is performed where one of the three springs is tilted to a certain angle. The values of the input tilt angles to perform a circular trajectory is given by:
D(t) =
"
η ϕ
#
=
"
Rsin(r0+ (r1−r0)s(t)) Rcos(r0+ (r1−r0)s(t))
#
(4.16) In Eq. (4.16), R corresponds to the radius of the circular trajectory which is equal to the tilt angle chosen within the singularity free workspace. The values r0 and r1 maps the initial and final points of circle and it goes from 0 to -2π to perform a counterclockwise circular trajectory.
The corresponding angular velocities and accelerations are given by:
V(t) =
"
R(r1−r0) ˙s(t) cos(r0+ (r1−r0)s(t))
−R(r1−r0) ˙s(t) sin(r0+ (r1−r0)s(t))
#
(4.17)
A(t) =
"
R(r1−r0)¨s(t) cos(r0+ (r1−r0)s(t))−R(r1−r0)2s˙(t)2sin(r0+ (r1−r0)s(t))
−R(r1−r0)¨s(t) sin(r0+ (r1−r0)s(t))−R(r1−r0)2s(t)˙ 2cos(r0+ (r1−r0)s(t))
#
(4.18)
Trajectory planning and control methodology 121 The IKP, the joint velocities and accelerations can then be computed by substituting Eqs. (4.16) to (4.18) in Eqs. (4.12) to (4.14). At the end of the circular trajectory, once again a linear tra-jectory is performed to bring the mechanism back to the home position.
4.4.2 Force control algorithm
In Chapter-2, two types of algorithm were classified namely: Force control and position control. The former was employed for the bio-inspired robot in order to accomplish static phases through motor torque control by setting threshold limits for moving inside unknown environments. Similarly, for the control of the tensegrity mechanism, the force control algorithm is employed to reach the desired positions for input tilt angles. By applying motor torques and current, the mechanism tilts to attain the desired position. Once the desired position is reached, the current and torque are cut off for attaining a static phase. For each angular position of DC-Motor, encoder position data is transmitted to the user by the BB black. However, the output data from encoders are not directly used to calculate the solutions to the IKP as there exists a significant difference between the desired and measured positions. These differences are caused by factors such as motor inertia, frictional effects and inertial forces. In order to compensate these factors, a closed loop feedback system is employed which tries to minimize the errors between the desired and measured positions. The classical relation for the PID control scheme which provides the motor torque is given by the equation [Car+15;Jha+16;Lem19] :
Γ =J(¨θi+KP(θdi−θmi) +KD( ˙θdi−θ˙mi) +KI
Z t
0 (θdi−θmi)) , with i= 1,2,3 (4.19) In Eq. (4.19), Γ is the regulated torque obtained at the gearbox output shaft after PID correction.
For computing the torques induced on each motor, Eq. (4.19) is multiplied by the gear reduction ratio G from Table 4.2. The inertia of motor-gearbox assembly J is taken as 4.1e−7 kg.m2 [Max]. In Eq. (4.19),KP is the proportionality term which examines the error term and responds proportionally. The integral term KI tries to reduce the error at each time instant. The overshoot of value from the desired position is compensated by the difference term KD. Thus, the PID in general takes into account the past step, the present step and the future step such that the error is reduced [MLN18;O’D09]. The values for the PID terms of Eq. (4.19) are given by:
ω= ktke
RJ = 14rad/s
KP = 3ω2 = 588, KD = 3ω = 42, KI =ω3 = 2744 (4.20) In Eq. (4.20), ω is a parameter which is determined from the torque constant (kt), speed con-stant (ke), resistance (R) and inertial parameters (J) of the DC-Motor [Car+15;Lem19]. The closed loop system which is incorporated for the control of the 3-SPS-U tensegrity mechanism is represented in Figure 4.5. In Figure 4.5, e and ˙e indicates the error and its derivative which
BeagleBone black
KI ∫ KP
KD θdi
θdi θdi
+ +
+
Motor with ESCON controller
Γ
θmi
θmi Feedback
Feedback e
e
Figure 4.5 – Closed loop feedback control scheme employed for the tensegrity mechanism [Car+15;Lem19]
are computed based on the difference between desired and measured data. Before the start of experiments, the ESCON servo controllers are calibrated in a computer. The saturation current is used as reference for controlling the direction of rotation of the DC-Motors. For safer opera-tions, 90% and 10% duty cycles are set as limits for the rotation of motors on either direction.
The PWM calibration of each ESCON servo-controller is done as per Table 4.4.
PWM Duty Nominal current (A) Motor torque(mN.m) Motor speed (rpm)
10% -0.177 -6.05 3720 (Counter-clockwise)
50% (idle) 0 0 0
90% 0.177 6.05 3720 (Clockwise)
Table 4.4 – ESCON controller calibration for the force control algorithm
The force control algorithm is executed in C language using the BB black. A simple form of the control loop that is employed to perform experiments on the 3-SPS-U tensegrity mechanism for linear and circular input trajectories is provided in Algorithm 2.