# Optimal Control of Holding Motion by Nonprehensile Two-Cooperative-Arm Robot

Authors: Changan Jiang; Yuta Nakatomi; Satoshi Ueno
Journal: Mathematical Problems in Engineering
 (2016)

---

## Abstract

Recently, more researchers have focused on nursing-care assistant robot and placed their hope on it to solve the shortage problem of the caregivers in hospital or nursing home. In this paper, a nonprehensile two-cooperative-arm robot is considered to realize holding motion to keep a two-rigid-link object (regarded as a care-receiver) stable on the robot arms. By applying Newton-Euler equations of motion, dynamic model of the object is obtained. In this model, for describing interaction behavior between object and robot arms in the normal direction, a viscoelastic model is employed to represent the normal forces. Considering existence of friction between object and robot arms, LuGre dynamic model is applied to describe the friction. Based on the obtained model, an optimal regulator is designed to control the holding motion of two-cooperative-arm robot. In order to verify the effectiveness of the proposed method, simulation results are shown.


---

## Body

## 1. Introduction

In recent years, with the increase of the senior citizens, the shortage problem of the caregivers appears in Japan subsequently. In order to ease caregivers’ burden, many different research groups have developed some power assist devices. Among these groups, RIKEN-SRK Collaboration Center for Human-Interactive Robot Research has developed a nursing-care assistant robot named ROBEAR (see Figure1). It was designed to lift up, hold, and transfer a person from a bed to a wheelchair or to assist a patient to stand up with its 6DOF (six degrees of freedom) arms.Figure 1
ROBEAR [21].Towards the more general and difficult problem of manipulating a care-receiver, this research is concerned with a simplified one: nonprehensile manipulation of a two-rigid-link object by two cooperative arms in a two-dimensional space. That is, a care-receiver is regarded as a two-rigid-link object with a passive joint. Many research works for different nonprehensile manipulation tasks can be found in the literatures [1–4]. However, these works were concerned with one-link object to be manipulated by one or two manipulators. For multilink object manipulation, [5] captured behaviors of a human and simulated for virtual human-robot interaction. To succeed in a real manipulation of a multilink object, [6] built a dynamic model for manipulating a two-rigid-link object by applying two cooperative arms and designed controllers to realize holding and lifting-up motion without considering friction. In [7], fuzzy control method was employed to compensate the effect of friction. Further, [8] proposed a control method for two cooperative arms by utilizing static friction.Reviewing literatures related to control method for robot arms, the simplest controller is Proportional Integral Derivative (PID) feedback controller [9]. To improve the stability and tracking performance, robust control method [10–14], adaptive control method [15, 16], and sliding mode control method [17] have been applied to design appropriate controllers. In this research, considering that fast variance of movement of robot arms may lead care-receivers to feel afraid, uncomfortable, and even pained during holding and lifting-up, the designed controllers could be modified according to the care-receivers’ feeling. Since optimal regulator can be modified by changing the weights which are related to state variable and control input in cost function, it could be applied to realize the holding motion of the two cooperative arms. Based on the two-rigid-link dynamic model [6, 8], [18] discussed the feasibility of guaranteeing the stability of holding motion of two-cooperative-arm robot by using designed optimal regulator without considering friction. However, in a real-world application, the friction cannot be ignored. So, in this research, a LuGre model [19] is introduced into the dynamic model to describe friction between object and robot arms. Based on the above modified model, an approximate model around equilibrium point of the considered plant is derived. For guaranteeing the tracking performance of the system which was not considered in [18], the plant is expanded with an integrator [20] to make steady error be zero. Since the expanded plant is controllable, an optimal regulator can be designed for it according to the desired cost function. Finally, in order to verify the effectiveness of the proposed method, simulation results on holding the two-rigid-link object with two-cooperative-arm robot are shown.The rest of this paper is organized as follows. In Section2, problem statement is introduced. System dynamic model is presented in Section 3. In Section 4, according to the approximate model around equilibrium point, an optimal regulator is designed for holding the two-rigid-link object. Simulation results are shown in Section 5. Section 6 is conclusion of this paper.


## 2. Problem Statement

A schematic diagram of the system is shown in Figure2. The object is composed of two rigid links (link 1 and link 2) which are connected by a passive joint. (x,y) is the position of the passive joint; θ1, m1, and I1 and θ2, m2, and I2 are the orientation angles, masses, and inertias of link 1 and link 2, respectively. The centers of mass of two links, marked as ⊕, are located at L1 and L2 from the passive joint. The object is manipulated by two arms whose positions are denoted by (xA1,yA1) and (xA2,yA2), respectively. r1 and r2 are the radii of two arms. l1 and l2 are the distances from the contact points to the passive joint. The normal forces and friction forces are acted at the contact points, which are denoted by FN1, FN2 and FF1, FF2. In this paper, for holding or lifting up the object, the arms will be controlled to manipulate the object. That is, (xA1(t),yA1(t)) and (xA2(t),yA2(t)) should be controlled to guarantee that (x(t),y(t),θ1(t),θ2(t)) could track the desired trajectory. In order to achieve this purpose, the object dynamics model will be introduced in next section.Figure 2
Schematic diagram of the system.


## 3. System Dynamic Model

In this section the system dynamic is considered. Employing the object dynamic model from [6, 8, 18], the dynamics of the object can be described by applying Newton-Euler equations of motion as(1)Mqq¨+Cq,q˙q˙+Gq=B1FN+B2FF=τ,where si=sin⁡θi, ci=cos⁡θi, q≔(x,y,θ1,θ2)T, FN=(FN1,FN2)T, FF=(FF1,FF2)T, (2)Mq=m1+m20-m1L1s1-m2L2s20m1+m2m1L1c1m2L2c2-m1L1s1m1L1c1I1+m1L120-m2L2s2m2L2c20I2+m2L22,Cq,q˙=00-m1L1θ˙1c1-m2L2θ˙2c200-m1L1θ˙1s1-m2L2θ˙2s200000000,Gq=0m1+m2gm1gL1c1m2gL2c2,B1=s1-s2-c1c2-l100l2,B2=-c1c2-s1s20000, and g is the gravitational acceleration.In [6], a viscoelastic model was employed to describe interaction behavior between links and arms in the normal direction (see Figure 3). In this research, the same viscoelastic model in (3) is applied to the normal forces FNi.(3)FNi=kiδi+diδ˙iforδi≥00forδi<0,where ki and di are spring constants and viscosity coefficient, respectively. δi are deformation in the normal direction at contact points. According to the geometrical relationship between links and arms, li and δi can be described as follows:(4)δ1l1δ2l2=-s1-c100c1s10000s2-c200-c2-s2x-xA1y-yA1xA2-xyA2-y+r10r20.Figure 3
Viscoelastic model.In order to describe the friction forces between links and arms, LuGre model [19] which is based on the average deflection of the bristles between two surfaces is employed in this research. The sketch of LuGre model is shown in Figure 4 and the friction forces are defined as (5)FFi=σ0izi+σ1iz˙i-σ2ivisgn⁡vi,where σ0i, σ1i, and σ2i are stiffness, damping, and viscous friction coefficients, respectively. zi are average deflection of the bristles and represented as(6)z˙i=-vi-vifvizi,(7)σ0ifvi=μCi+μSi-μCie-vi/vsti1/2FNi,where μCi and μSi are coefficients of Coulomb friction and static friction, respectively. vsti are the Stribeck velocities. vi are relative velocities in the tangent direction between links and arms and represented as(8)v1=-x˙-x˙A1cos⁡θ1-y˙-y˙A1sin⁡θ1,v2=x˙-x˙A2cos⁡θ2+y˙-y˙A2sin⁡θ2.Figure 4
Sketch of LuGre model.


## 4. Optimal Regulator Design

In the above section, the dynamic model of the considered system was introduced. According to the obtained nonlinear model, for specifiedFNi and FFi, if there exists q which can satisfy (1) when q¨=q˙=0, we call the state q an equilibrium point of the system. Before designing optimal regulator, a linear approximation to the obtained nonlinear model around an equilibrium point will be firstly found.We assume that friction forcesFFi=0 and positions of arms as (xAi0,yAi0) when the system is at the equilibrium point q=q0=(x0,y0,θ10,θ20)T, Δq=(Δx,Δy,Δθ1,Δθ2)T is minor change from the equilibrium point, and (9)sin⁡Δθi=Δθi,cos⁡Δθi=1,sin⁡θi0+Δθi=sin⁡θi0+cos⁡θi0·Δθi,cos⁡θi0+Δθi=cos⁡θi0-sin⁡θi0·Δθi.Then, substituteq=q0+Δq into (1);(10)τ0+Δτ=Mq0+ΔqΔq¨+Cq0+Δq,Δq˙Δq˙+Gq0+Δqis obtained, where M(q0+Δq)=M(q0)+ΔM, G(q0+Δq)=G(q0)+ΔG, and (11)ΔM=00-m1L1c10Δθ1-m2L2c20Δθ200-m1L1s10Δθ1-m2L2s20Δθ2-m1L1c10Δθ1-m1L1s10Δθ100-m2L2c20Δθ2-m2L2s20Δθ200,Cq0+Δq,Δq˙=00-m1L1Δθ1˙c10-s10Δθ1-m2L2Δθ2˙c20-s20Δθ200-m1L1Δθ1˙s10+c10Δθ1-m2L2Δθ2˙s20+c20Δθ200000000,ΔG=00-m1gL1s10Δθ1-m2gL2s20Δθ2. Let Δ·Δ=0, Δ·Δ˙=0, Δ·Δ¨=0, and then (10) becomes(12)Mq0Δq¨+Gq0+ΔG=τ0+Δτ.Since G(q0)=τ0 when the system is stationary at equilibrium point, (12) can be simplified as(13)Mq0Δq¨+ΔG=Δτ.According to (1), we can get(14)Δτ=c10Δθ1-c20Δθ200s10Δθ1-s20Δθ20000000000FN1FN2FN1l1FN2l2+-s10Δθ1-s20Δθ2-c10Δθ1c20Δθ20000FF1FF2+s10-s2000-c10c200000-100001ΔFN1ΔFN2ΔFN1l1ΔFN2l2+-c10c20-s10s200000ΔFF1ΔFF2.In this research, we assume that diδ˙i≈0, so we get (15)ΔFN1ΔFN2ΔFN1l1ΔFN2l2=k100000k20k1l1k1δ10000k2l2k2δ2Δδ1Δl1Δδ2Δl2. From (4), (16)Δδ1Δl1Δδ2Δl2=--s10c1000-c10-s100000s20-c2000-c20-s20ΔxA1ΔyA1ΔxA2ΔyA2-s10-c10x0-xA10c10+y0-yA10s100c10s10-x0-xA10s10+y0-yA10c100-s20c200xA20-x0c20+yA20-y0s20c20s200xA20-x0s20-yA10-y0c20ΔxΔyΔθ1Δθ2 can be obtained. Linearize (6) around zi=0 and vi=0; we can get z˙i=-vi and(17)ΔFF1ΔFF2=σ01c10σ01s10a0-σ02c20-σ02s200bΔxΔyΔθ1Δθ2+αc10αs1000-βc20-βs2000Δx˙Δy˙Δθ1˙Δθ2˙,where a, b, α, and β are shown as follows:(18)a=-σ01x0-xA10s10-σ11+σ21x˙-x˙A1s10+σ01y0-yA10c10+σ11+σ21y˙-y˙A1c10sgn⁡Δv1>0-σ01x0-xA10s10-σ11-σ21x˙-x˙A1s10+σ01y0-yA10c10+σ11-σ21y˙-y˙A1c10sgn⁡Δv1<00sgn⁡Δv1=0,b=σ02x0-xA20s20+σ12+σ22x˙-x˙A2s20-σ02y0-yA20c20-σ12+σ22y˙-y˙A2c20sgn⁡Δv2>0σ02x0-xA20s20+σ12-σ22x˙-x˙A2s20-σ02y0-yA20c20-σ12-σ22y˙-y˙A2c20sgn⁡Δv2<00sgn⁡Δv2=0,α=σ11+σ21sgn⁡Δv1>0σ11-σ21sgn⁡Δv1<00sgn⁡Δv1=0,β=σ12+σ22sgn⁡Δv2>0σ12-σ22sgn⁡Δv2<00sgn⁡Δv2=0.Substitute the above equations into (13); we can get approximate model as follows:(19)Mq0Δq¨+H7Δq˙+H4-H1-H2-H5-H6Δq=H3ΔxA1ΔyA1ΔxA2ΔyA2,where (20)H1=00FN1c10-FN2c2000FN1s10-FN2s2000000000,H2=-k1s120-k2s220k1s10c10+k2s20c20-k1x0-xA10s10c10-k1y0-yA10s120k2xA20-x0s20c20+k2yA20-y0s220k1s10c10+k2s20c20-k1c120-k2c220k1x0-xA10c120+k1y0-yA10s10c10-k2xA20-x0c220-k2yA20-y0s20c20k1l1s10+k1δ1c10-k1l1c10+k1δ1s10A0k2l2s20-k2δ2c20-k2l2c20-k2δ2s200B,H3=k1s120-k1s10c10k2s220-k2s20c20-k1s10c10k1c120-k2s20c20k2c220-k1l1s10-k1δ1c10k1l1c10-k1δ1s100000-k2l2s20+k2δ2c20k2l2c20+k2δ2s20,H4=0000000000-m1gL1s100000-m2gL2s20,H5=00-FF1s10-FF2s2000-FF1c10FF2c2000000000,H6=-σ01c120-σ02c220-σ01s10c10-σ02s20c20-ac10bc20-σ01s10c10-σ02s20c20-σ01s120-σ02s220-as10bs2000000000,H7=-αc120-βc220-αs10c10-βs20c2000-αs10c10-βs20c20-αs120-βs2200000000000,where A denotes k1l1x0-xA10c10 + k1l1y0-yA10s10 − k1δ1x0-xA10s10 + k1δ1y0-yA10c10 and B denotes -k2l2xA20-x0c20 − k2l2yA20-y0s20 − k2δ2xA20-x0s20 + k2δ2yA20-y0c20.We setFFi=0 at equilibrium point, so H5 is 04×4 and (19) can be represented as the following state equation:(21)x˙=Ax+Bu,y-r=Cx,where (22)x=ΔqΔq˙T,u=ΔXA1ΔYA1ΔXA2ΔYA2T,A=04×4I4×4M-1q0H1+H2+H6-H4M-1q0H7,B=04×4M-1q0H3,C=I4×404×4.In order to guarantee the stability and tracking performance of the above plant, the control system is designed as shown in Figure5. The above plant in (21) will be expanded to the following state equation:(23)x˙e=Aexe+Bevy-r=C04×4xe,where (24)xe=xuAe=AB04×804×4Be=08×4I4×4.Figure 5
Block diagram of control system.Sincerank⁡BeAeBe=12, we know that plant (23) is controllable, and an optimal regulator can be designed for it. The cost function J is employed as (25)J=∫0∞xeTQxe+vTRvdt,where Q>0 and R>0. Then, optimal regulator which minimizes the cost function J is given as(26)v=-Kxe=-R-1BeTPxe,where P is the solution of the following Riccati equation:(27)PAe+AeTP-PBeR-1BeTP+Q=0. So, K1 and K2 can be derived as(28)K1K2=KABC04×4-1.According to (18), we can see that A will vary along with the variations of H6 and H7. That means parameters K1 and K2 of the designed optimal regulator will also vary simultaneously.


## 5. Simulation Results

In this section, by using the above proposed optimal regulator, simulation for holding two-rigid-link object at the same place was done. The initial state of the object isq0=-0.029m-0.234m150.4∘29.8∘T and the desired state is qr=-0.03m-0.235m150∘30∘T. In this simulation, qr is regarded as equilibrium point and initial positions of two arms are obtained as u0=-0.3033[m]-0.1357[m]0.2441[m]-0.1353[m]T. The parameters of the system are shown in Table 1. The total time of simulation is T=7 [s], and Fd=0[Kgf]-0.1[Kgf]T is added as disturbance to the joint of the two-rigid-link object during t=2~2.1[s]. For optimal regulator, we chose Q1=diag⁡40,20,2,4,0.4,0.2,0.02,0.04,2,2,2,2, R1=diag{7.5,7.5,7.5,7.5} and Q2=diag⁡200,100,10,20,2,1,0.1,0.2,10,10,10,10, R2=diag{5,5,5,5} for comparison. The simulation results are shown as in Table 1.Table 1
Parameters of the system.
i = 1 i = 2 m i [Kg] 2.71 2.5 L i [m] 0.367 0.399 I i [Kg⋅m2] 0.17 0.136 r i [m] 0.051 0.051 k i [N/m] 1 0 5 1 0 5 d i [N⋅s/m] 1 0 3 1 0 3 σ 0 i [N/m] 10 10 σ 1 i [N⋅s/m] 0.1 0.1 σ 2 i [N⋅s/m] 0.001 0.001 μ C i 0.08 0.08 μ S i 0.161 0.161 v s t i [m/s] 0.01 0.01In Figure6, dashed lines show the position (x,y) of the object’s joint and orientation angles (θ1,θ2) of the object with Q1 and R1, solid lines show the ones with Q2 and R2, and dotted lines show the desired position (xref,yref) and orientation angles (θ1ref,θ2ref). From Figure 6, we can see the position of the object’s joint and the orientation angles of the object are controlled to be stable on the desired state; even a disturbance Fd was added during t=2~2.1[s]. Figure 7 shows the positions of two arms with different Q and R during stabilizing the object. Since we set the initial positions of two arms as the ones which can make the system be equilibrium on the desired state, we can see two arms were moved to try to keep the object at the beginning. After being disturbed by Fd, two arms began to move again to stabilize the object. Finally the object was stabilized and kept on the desired state. In Figures 8 and 9, the variations of parameters K1T and K2T during t=1~4[s] by using Q2 and R2 are shown. We can see that the parameters of the designed optimal regulator varied during holding motion of two cooperative arms for guaranteeing the stability of the whole system.Figure 6
State of the two-rigid-link object.Figure 7
Positions of two arms.Figure 8
ParameterK1T.Figure 9
ParameterK2T.From the above simulation results, the effectiveness of the proposed method is verified. Also, we know that makingQ bigger and R smaller can improve response of this system. That means in the future work, for avoiding leading care-receiver to feel afraid, uncomfortable, and even pained, it is feasible to modify the movement of two-cooperative-arm robot according to the feeling of the care-receiver by changing Q and R.Further, by using the same parameters as above, we compared the simulation results between our proposed method and the one of [18] which did not consider friction in dynamic model of the system. In Figure 10, the states of the two-rigid-link object with and without considering friction are shown. Since the friction is small, two results are nearly the same. Then, we changed the parameters of friction to σ0i=101, σ1i=1, and σ2i=0.01 to make the friction bigger and obtained Figure 11. From Figure 11, we can see that the tracking performance of the position (solid lines) of the object with our proposed method is better than the one (dashed lines) with the method of [18], and the speeds of convergence of the orientation angles (solid lines) of the object with our proposed method are a little later than the one (dashed lines) with the method of [18]. In practice, since the effect of the friction could not be small enough to be ignored, the friction should be considered in the dynamic model of the system. The effectiveness of our proposed method to control the holding motion of two-cooperative-arm robot with considering friction is verified by the above simulation results.Figure 10
State of the two-rigid-link object.Figure 11
State of the two-rigid-link object with different friction.


## 6. Conclusion

In this paper, an approximate model of a two-rigid-link object which is regarded as a care-receiver was obtained based on its dynamic model including viscoelastic model and LuGre model for interaction behavior between object and robot arms. For guaranteeing the stability and tracking performance of the considered system, an optimal regulator was designed to realize the holding motion of two cooperative arms. In order to show the effectiveness of the designed optimal regulator, simulation results were given. The feasibility of modifying the movement of two-cooperative-arm robot by changingQ and R of the designed optimal regulator was also shown.



---

*Source: 5921871-2016-09-14.xml*