# Dual Arm Free Flying Space Robot Trajectory Planning Using Polynomial

Authors: Rishikesh Rathee; Pushparaj Mani Pathak
Journal: Journal of Robotics
 (2015)

---

## Abstract

The paper presents path planning of dual arm free flying space robot using smooth functions of time. Kinematic and dynamic modeling of dual arm free flying space robot is presented first. Using kinematic model, the Jacobian of the system has been derived, and using dynamic model, equations of motion are derived.  A path planning methodology for planar system is developed using smooth function of time such as polynomials. Due to nonholonomic behaviour of the manipulator in the zero gravity environment linear and angular momentum is conserved. The proposed method yields input trajectories that drive both the manipulator and the base to a desired configuration. Joint torque curves can be obtained by introducing this joint trajectory curves in equation of motion of the space robot.


---

## Body

## 1. Introduction

Space introduces a complicating factor to a robotic system that is not apparent on Earth; that is, the manipulator base is not fixed in space and zero or microgravity environment exists. This introduces a high degree of dynamic complexity. Moreover, in the free flying atmosphere as the space craft actuators are turned off, the robot base has six degrees of freedom and is nonholonomic in nature. In the free flying mode, the motion of manipulator will generate reaction forces, moments on its base, and its manipulator coupling points. If no compensation is made, the robot will not attain its target since the coupling has a significant effect on the kinematics, dynamics, and control of the robot.Path planning of free flying space robot considering nonholonomic nature of base can be done by bidirectional approach and checking its stability by defining Lyapunov function as in [1]. Yoshida et al. [2, 3] have developed the equation of motion for multiple arm free flying robotic system and torque optimization for its redundant arms. They also provided an overview of its dynamics and control which was verified on ETS-VII. Papadopoulos and Moosavian [4] have used barycentric vector method for studying the dynamic behavior of multiarm space robots during chase and capture operations. Papadopoulos et al. [5] also developed a path planning methodology for single arm planar free floating space manipulator systems defining joint angles as a smooth function of polynomials.In order to overcome the difficulty that the dynamic equations of dual arm space robot system cannot be linearly parameterized, Chen and Guo [6] modelled the system as underactuated and asymptotic stability of adaptive control scheme is proved with Lyapunov method. Huang et al. [7] derive the impact dynamic equations according to the dynamic model of space robot system and proposed a genetic algorithm (GA) based on approach to search the optimal configuration of space robot at capturing moment in order to minimize or avoid the impact. Sagara and Taira [8] presented cooperative manipulation of a floating object by space robots. They also discussed application of a tracking control method using the transpose of the generalized Jacobian matrix. The dynamics control of a dual arm space robot installed on a free flying spacecraft without base position and orientation control holding a single object was discussed by Zhao et al. [9] and Siciliano and Khatib [10], and Merzouki et al. [11] presented a brief of different modelling methods and control strategies for space robots.This paper proposes the path planning of dual arm planar space robot as a function of polynomial. It is motivated by the work presented in [5]. To achieve this Jacobian [12] of dual arm free flying space robot is derived by representing its link lengths as a function of inertia parameters like mass. Equation of motion of system is derived and validated by simulation results carried out in Simulink, thus validating the model. Finally, the path planning strategy is validated by simulation results using MATLAB.


## 2. Kinematic and Dynamic Modeling of Dual Arm Free Flying Space Robot

The free flying manipulator has totalkn+6 degree of freedom (DOF) with n number of links in each k number of arms where base is having six DOF [13]. Let us consider the case of dual arm (k=2) free flying manipulator mounted on a base and consists of dual links (n=2) in each arm as shown in Figure 1. Manipulator arms are assumed to be connected by revolute joints. Also the arms are attached on a base with the help of revolute joints. One of these arms is called mission arm (m) which is used to accomplish the space mission, and the other is the balance arm (b) which is used to balance the reaction motion due to the motion of the mission arm. The balance arm can also be used to accomplish the mission like the mission arm if given an appropriate trajectory to its end effector. The system center of mass (CM) remains fixed in space and the frame is considered as inertial frame. Let θ1m, θ2m, θ1b, and θ2b be the joint angles of the joints attached to mission and balance arm. The momentum constraints are imposed due to dynamical coupling between the manipulator and the base. As we know in the absence of external forces or torque, linear and angular momentum of the system is conserved. In such a situation, the initial linear and angular momentum of the system is assumed to be zero.Figure 1
Schematic diagram of dual arm free flying space robot.The symbols required for the formulation of equations and in Figure1 are defined as follows:b: superscript used to indicate parameter for balance arm,FrameI: inertia frame,Framei: ith body frame or ith link frame of manipulator for i=1⋯n and for the base frame i=0,m: superscript used to indicate parameter for mission arm,mi: mass of ith body (kg),mT: total mass of the system (kg),ri: distance from CM of ith link to the preceding joint represented in the inertia frame,si: distance from CM of ith link to the subsequently joint represented in the inertia frame,Ii: inertia matrix of ith link with respect to inertia frame,p*: position vector of CM of the end effectors with respect to inertia frame,pCM*: position vector of CM of the system with respect to inertia frame,Ri: rotation matrix of ith link with respect to the base frame,Jn*: Jacobian matrix with n number of links in the manipulator,M*: generalized inertia matrix,C*: centrifugal and Coriolis term,∅: joint variables and base variables,E3: identity matrix,pci: position vector of CM of ith link with respect to inertia frame,rc0: position vector of CM of base with respect to inertia frame,rci: distance between the CM of links,s0: position vector of the first joint between the link and the base with respect to the CM of base in the inertia frame.For free flying robotic manipulator the Denavit-Hartenberg formulation is presented in Ellery [14]. The desired end effector position for mission arm with respect to inertia frame is
(1)p*m=pCM*+R0ms0mm0n+1mmT-R0bs0bm0n+1bmT+∑i=1nRimλim-∑i=1nRibμib-mn+1mRn+1mrn+1m+mn+1bRn+1brn+1bmT,
where λim=[(m0im+m1n+1b)kinematicparameterim-mimrim]/mT, μib=[mi+1n+1bkinematicparameterib+mibrib]/mT.The desired end effector position for balance arm with respect to inertia frame is(2)p*b=pCM*+R0bs0bm0n+1bmT-R0ms0mm0n+1mmT+∑i=1nRibλib-∑i=1nRimμim-mn+1bRn+1brn+1b+mn+1mRn+1mrn+1mmT,
where λib=[(m0ib+m1n+1m)kinematicparameterib-mibrib]/mT, μim=[mi+1n+1mkinematicparameterim+mimrim]/mT:
(3)pCM*=∑i=0n+1mipci*mT.
Jacobian allows conversion of differential motions or velocities of individual joints to differential motions or velocities of point of interest in workspace. The free flyer Jacobian requires only the replacement of kinematic link parameters of the terrestrial manipulators with kinematic-dynamic parameters, that is, representing link lengths and joint offset as a function of mass.Now, consider(4)Jn*=JTiJRi6×kn.JTi is 3×kn the Jacobian matrix for linear velocity of link i. JRi is 3×kn the Jacobian matrix for angular velocity of link i.For mission and balance arm Jacobian is(5)Jn*m=∑i=1n∑j=1i∂Rim∂θjmλim-∑i=1n∑j=1i∂Rib∂θjbμibJn*b=∑i=1n∑j=1i∂Rib∂θjbλib-∑i=1n∑j=1i∂Rim∂θjmμim.The brief concept of equation of motion of a free flying space robot as a multibody system which is presented elsewhere [7] is
(6)M*∅¨+C*=τ,
where M* is generalised inertia matrix, C* is centrifugal and Coriolis term, ∅ is joint and base variables, and τ is external forces/moments on base and joint torques of the arm. The joint and base variable ∅ is given by
(7)∅=X0Y0Z0θ0xθ0yθ0zθ1mθ2mθ1bθ1bT,
where (X0Y0Z0) are the base coordinates and (θ0xθ0yθ0z) are base orientations. The generalised inertia matrix M* is given by
(8)M*=MBaseMCoupMCoupTMArms(6+kn)×(6+kn),
where MBase = R6×6 = inertia matrix of base. MArms = Rn×n = inertia matrix of manipulator arms. MCoup = R6×n = coupling inertia matrix between base and robot manipulators. n is total number of links in an arm.These matrices can be given by(9)Mbase=mTE3-mTP~0cmmTP~cmI0+∑i=1nIim+∑i=1nIib-∑i=1nmiP~ciP~0cim-∑i=1nmiP~ciP~0cib6×6MArms=∑i=1nJRiTIiJRi+miJTiTJTim+∑i=1nJRiTIiJRi+miJTiTJTibkn×knMCoup=∑i=1nmiJTim+∑i=1nmiJTib∑i=1nIiJRi+miP~ciJTim+∑i=1nIiJRi+miP~ciJTib6×kn.Ii is the inertia tensor for the ith link:(10)Ii=0.5-Ixx+Iyy+IzzIxyIxzIxy0.5Ixx-Iyy+IzzIyzIxzIyz0.5Ixx+Iyy-IzzFor dual arm free flying space robot,
(11)p0cm=[pcm*-rc0]3×1,p0ci=[pci-rc0]3×1P~cm, P~ci, P~0cm, and P~0ci are the 3 × 3 skew symmetric matrix of position vectors pcm*, pci, p0cm, and p0ci, respectively.We have assumed that there is no external force acting on the system; therefore, the centrifugal and Coriolis term is(12)C*=M˙*∅˙,
where M˙* is the derivative of generalized inertia matrix.


## 3. Path Planning for Dual Arm Free Flying Space Robot

The angular momentum conservation cannot be integrated to analytically yield the spacecraft orientation as a function of the system’s configuration. The nonintegrability property of angular momentum introduces nonholonomic characteristics to free floating systems. For simplicity we consider the planar case of the dual arm model; that is,kn+1 DOF system in which base is having one DOF about z direction θ0. In this section the path planning for single arm as in [5] is implemented for dual arm case.The linear and angular momentum conservations for both mission and balance arms are represented by the following equations:(13)D0θ˙0+D1θ˙1m+D2θ˙2m=0D0θ˙0+D3θ˙1b+D4θ˙2b=0,
where D0, D1, D2, D3, and D4 are functions of system inertial parameters. For mission arm effect of θ1b and θ2b is neglected, hence the scleronomic constraint can be written in the form
(14)D0θ0,θ1m,θ2mdθ0+D1θ0,θ1m,θ2mdθ1m+D2θ0,θ1m,θ2mdθ2m=0.
For balance arm effect of θ1m and θ2m is neglected, hence the scleronomic constraint can be written in the form
(15)D0θ0,θ1b,θ2bdθ0+D3θ0,θ1b,θ2bdθ1b+D4θ0,θ1b,θ2bdθ2b=0.
The coefficients of the nonholonomic constraint become
(16)D0θ0,θ1m,θ2m=D0θ0,θ1b,θ2b=Δ0,D1θ0,θ1m,θ2m=Δ1m+Δ3mcos⁡⁡θ1m-θ2m,D2θ0,θ1m,θ2m=Δ2m+Δ3mcos⁡⁡θ1m-θ2m,D3θ0,θ1b,θ2b=Δ1b+Δ3bcos⁡⁡θ1b-θ2b,D4θ0,θ1b,θ2b=Δ2b+Δ3bcos⁡⁡θ1b-θ2b,
where Δ’s are the function of inertia, length, and masses of the links and base.In general form,Inertia=mass×(radiusofgyration)2(17)Δ0=I0,Δ1m=I1m+r1m2m0m1m+m1mm2ms1m2+a1m2m0m2mmT,Δ2m=I2m+m2mm0+m1mr2m2mT,Δ3m=m1mm2mr1mr2m+m0m2ma1mr2mmT,Δ1b=I1b+r1b2m0m1b+m1bm2bs1b2+a1b2m0m2bmT,Δ2b=I2b+m2bm0+m1br2b2mT,Δ3b=m1bm2br1br2b+m0m2ba1br2bmT.
Here m0 and I0 are mass and inertia of base and m1m, m2m, m1b, m2b and I1m, I2m, I1b, I2b are masses and inertia of links 1 and 2 of mission and balance arm, respectively. mT is total mass of the system. a is the link lengths of respective links, l is the distance from j-1 joint to center of mass of that link, r is the distance from center of mass to jth joint. Note that a=l+r for every link.Therefore, from the above equations we get(18)I0dθ0+Δ1m+Δ3mcos⁡⁡θ1m-θ2mdθ1m+Δ2m+Δ3mcos⁡⁡θ1m-θ2mdθ2m=0,I0dθ0+Δ1b+Δ3bcos⁡θ1b-θ2bdθ1b+Δ2b+Δ3bcos⁡θ1b-θ2bdθ2b=0.Equation (18) contains three differentials. Path planning can be done if this form is transformed to the equations consisting of two differentials. So nonintegrable equations of the form of (18) can be written as
(19)du+v·dw=0,dx+y·dz=0,
where u, v, and w are properly selected functions of θ0, θ1m, and θ2mand x, y, and z are properly selected functions of θ0, θ1b, and θ2b.For mission arm and balance arm the forward transformation is given by(20)uθ0,θ1m,θ2m=Δ0·θ0+Δ2m·θ2m-Δ3m·sin⁡(θ1m-θ2m)vθ0,θ1m,θ2m=Δ1m+2Δ3m·cos⁡⁡θ1m-θ2m,wθ0,θ1m,θ2m=θ1m.(21)xθ0,θ1b,θ2b=Δ0·θ0+Δ2b·θ2b-Δ3b·sin⁡θ1b-θ2b,yθ0,θ1b,θ2b=Δ1b+2Δ3b·cos⁡⁡θ1b-θ2b,zθ0,θ1b,θ2b=θ1b.These equations constitute a transformation(θ0,θ1m,θ2m)→(u,v,w) for mission arm and (θ0,θ1b,θ2b)→(x,y,z) for balance arm which is defined at every point of the configuration space of the system. Moreover, the orientation θ1m and θ1b are conserved through the transformation and are explicitly available for planning; that is, one can directly set a desired trajectory for θ1m and θ1b.Therefore, the planning problem reduces to choosing functionsf and g given by (22). We choose function f as a fifth-order polynomial and g can be forth-order polynomial, so that while finding coefficients of them the system initial and final configuration, velocity, and acceleration can be satisfied:
(22)w=z=ft=c5t5+c4t4+c3t3+c2t2+c1t+c0,u=x=gw=b4w4+b3w3+b2w2+b1w+b0,v=y=-g’w=-4b4w3+3b3w2+2b2w+b1.Here the coefficients (c’s) of polynomial w are computed using the initial and final values of orientation θ1m, angular velocity θ˙1m, and angular acceleration θ¨1m. The coefficients of polynomial z are computed using the initial and final values of orientation θ1b, angular velocity θ˙1b, and angular acceleration θ¨1b. But for mission and balance arm the inverse transform is defined if and only if the constraints given by (23) are satisfied:
(23)-1≤v-Δ1m2Δ3m≤1,-1≤y-Δ1b2Δ3b≤1.These constraints have been implied fromθ2m and θ2b values derived from (24) and (25). For mission arm one uses initial and final conditions of θ0, θ1m, and θ2m in (20) in order to find uint, ufin, vint, and vfin and using the polynomial Equation (22) we get unknown coefficients b3, b2, b1, and b0 for mission arm. For balance arm one uses initial and final conditions of θ0, θ1b, and θ2b in (21) to find xint, xfin, yint, and yfin and using the polynomial equation (22) we get unknown coefficients b3, b2, b1, and b0 for balance arm:
(24)θ0=1Δ0u-Δ2mw-Δ2mcos⁡-1⁡v-Δ1m2Δ3m1-v-Δ1m2Δ3m2-Δ3m1-v-Δ1m2Δ3m2,θ1m=w,θ2m=w+cos⁡-1⁡v-Δ1m2Δ3m.
Similarly, for balance arm we have
(25)θ0=1Δ0x-Δ2bz-Δ2bcos⁡-1⁡y-Δ1b2Δ3b1-y-Δ1b2Δ3b2-Δ3b1-y-Δ1b2Δ3b2,θ1b=z,θ2b=z+cos-1⁡y-Δ1b2Δ3b.The polynomialv is written as a function of b4 and the unknown spacecraft final orientation θ0fin. So b4 and θ0fin are chosen in such a way that it satisfies constraint in (23). Once f and g are found, the trajectories orθ0, θ1m, θ2m, θ1b, θ2b are found using (24) and (25), which shows the inverse transformation from u, v, and w to θ0, θ1m, θ2m and from x, y, and z to θ0, θ1b, θ2b. It is seen that both the arms have a cumulative effect on orientation of base θ0.


## 4. Simulation and Results

Dynamic model derived in (6) of space robot is simulated using MATLAB and Simulink software. Simulation of equation of motion is obtained by providing step input to four DC motors located at the four joints of the dual arm robot. Modelling of DC motor is obtained using the following equations:
(26)Jd2θdt2=T-bdθdt,Ldidt=-Ri+V-e,T=Kti,e=Kedθdt.
In the above equation (26), J is moment of inertia of rotor, θ is motor rotation, T is motor torque, b is damping ratio, L is electric inductance of motors, i is armature current, R is armature resistance, V is voltage supplied, e is back electromotive force (emf), and Kt and Ke are electromotive force constants. Table 1 shows the parameters used in simulation of DC motor. Table 2 shows the parameters used for simulation of space robot.Table 1
Parameters used in simulation of DC motor.
Moment of inertia of rotors (J)
0.01 kgm2
Damping ratio of motors shafts (b)
0.0001 Nms
Electromotive force constant (Kt=Ke=K)
0.01 Nm/Amp
Electric resistance of the two motors (R)
0.1 ohm
Electric inductance of motors (L)
0.5 HTable 2
Parameters used in simulating of space robot.
Mass of base (m0)
4 kg
Mass of first link of mission and balance armm1m=m1b
0.2942 kg
Mass of second link of mission and balance arm including masses of their grippersm2m=m2b
0.2942 kg
Length of first link of mission and balance arma1m=a1b
0.4 m
Length of second link of mission and balance arma2m=a2b
0.3 m
Distance of CM of base to first joint of both the armss0m=s0b
0.5 m
Inertia of first link of mission and balance armI1m=I1b
0.03 kg m2
Inertia of second link of mission and balance arm including end effectors inertiaI2m=I2b
0.02 kg m2
Inertia of base  (I0)
0.4 kg m2End effectors trajectory is obtained by first simulating joint motions of the robot and then transferring it to its tip using Jacobian given by (5). To validate equations of motion of the proposed model, base disturbance caused due to the effect of mass and inertia of links should be negligible, hence increasing base mass to 400 kg and inertia to 300 kg m2. The two links of each arm are made straight and joint between them is locked by increasing the motor damping B2 by 100 times. As expected the two end effectors of mission arm and balance arm plot a circular trajectory as shown in Figure 2, with radius equal to the summation of their dynamic link lengths.Plot ofXtip v/s Ytip of dual arm space robot for both the arms.
(a)
(b)For simulation of path planning parameters used are same as given in Table2 and the duration of motion is chosen equal to 10 s. The free-floater has to move its manipulator endpoint to a new location and at the same time change its spacecraft attitude to a desired one. Only manipulator actuators are to be used. Let initial system configuration of mission arm be (θ1m,θ2m)in ≡ (0°, 30°) and the final be (θ1m,θ2m)fin ≡ (30°, 60°). Let initial system configuration of balance arm be (θ1b,θ2b)in ≡ (180°, 150°) and the final be (θ1b,θ2b)fin ≡ (140°, 100°). The initial system configuration of base is (θ0)in ≡ 0° and the final is (θ0)fin ≡ 5°.This requirement may result in a range of possibleb4. Of these, b4 is chosen so that the range of allowable final spacecraft attitudes is maximized. For this case, for both the arms b4=20. Joint torques curves can be obtained by simplifying (6) to the planar case.As shown in Figure3, the desired configuration is reached in the specified time. Also, all trajectories are smooth throughout the motion, and the system starts and stops smoothly at zero velocities, as expected and shown in Figures 4, 5, and 6. This is an important characteristic of the method employed and is due to the use of smooth functions, such as polynomials. As shown in Figure 7, the required torques can be easily applied by the joint actuators to reach the final configuration. From Figure 8, total base orientation changes from 53.27° to −6.85° which contains the desired 5°, but the total base disturbance is around 60° which is undesirable. Increasing or decreasing simulation time and base parameters keeping in mind the singularities has no effect on the path taken but increases or decreases the torque requirements and the magnitude of velocities or accelerations. Hence, increasing the base mass to 10 kg and its inertia to 1 kg-m2 for feasible solution we can minimize total base disturbance to 10° as shown in Figure 9.Path followed by all four joints of both the arms using polynomial approach for (θ1m,θ2m)in ≡ (0°, 30°), (θ1m,θ2m)fin ≡ (30°, 60°), (θ1b,θ2b)in ≡ (180°, 150°), and (θ1b,θ2b)fin ≡ (140°, 100°).
(a)
(b)
(c)
(d)Rate of change of joint angles of both the arms.
(a)
(b)
(c)
(d)End effectors velocity of mission arm.
(a)
(b)End effectors velocity of balance arm.
(a)
(b)Manipulator torques of both the arms.
(a)
(b)
(c)
(d)Base disturbance and base velocity variation with time.
(a)
(b)Base disturbance and base velocity variation with time for increased mass and inertia.
(a)
(b)


## 5. Conclusions

In this paper dynamic modeling and path planning of dual arm free flying space robot are presented. Equation of motion for the case of dual arm free flying is derived and simulated in Simulink. Results show that if the base mass and inertia are increased to a certain limit keeping in mind the singularities, then base disturbance can be decreased and the end effectors follow the required circular path. A path planning methodology was implemented for dual arm free flying space manipulators using smooth and continuous functions such as polynomials. The desired configuration is reached in the specified time. Also, all joint trajectories are smooth throughout the motion, and the system starts and stops smoothly at zero velocities, as expected. This is an important feature of the method used and is due to the use of functions, such as polynomials. The required torques is small and torque variation is smooth and can be easily be applied by the joint actuators to reach the final configuration. From the practical point of view, one should investigate the applicability of the method to more than two arms and three-dimensional systems. One can also see the effect of increasing the degree of polynomial considering uncertainties in inertial parameters during path planning.



---

*Source: 324831-2015-02-26.xml*