Embed Size (px)
Citation preview
Acta of Bioengineering and Biomechanics Original paperVol. 16, No. 2, 2014 DOI: 10.5277/abb140202
Motion planning for mobile surgery assistant
GRZEGORZ PAJAK*, IWONA PAJAK
Faculty of Mechanical Engineering, University of Zielona Góra, Poland.
The paper presents a method of motion planning for a mobile manipulator acting as a helper providing the necessary tools or a sur-gery assistant carrying out pre-planned procedures. Mobility of this system makes it possible to reach the position which will give opti-mal access to the operating field. The path of the end-effector, determined during operation pre-planning, is defined as a curve param-eterised by any scaling parameter, the reference trajectory of a mobile platform is not needed. The motion of the mobile manipulator isplanned in order to maximise the manipulability measure, thus to avoid manipulator singularities. The method is based on a penaltyfunction approach and a redundancy resolution at the acceleration level. Constraints connected with the existence of mechanical limitsfor a given manipulator configuration, collision avoidance conditions and control constraints are considered. A computer example in-volving a mobile manipulator consisting of a nonholonomic platform (2,0) class and a 3 DOF RPR type holonomic manipulator operat-ing in a three-dimensional task space is also presented.
Key words: mobile manipulator, path following, trajectory planning, surgery assistant
1. Introduction
Robotics is a branch of engineering that deals withdesigning and controlling mechanisms for imitatingmanipulation and locomotion features of a human be-ing. One of the most important problems in practicalapplication of robots is to convert high level specifi-cation of the task into low level motion description ofmechanisms. Motion planning is the area of roboticsthat provides techniques to solve tasks of this type.
First robots were designed for industrial applica-tions, but currently they are used in many fields, in-cluding medicine. Medical robotics has many appli-cations, such as robot assisted surgery [15], [23],prosthesis construction [24], [14] or designing devicesused for assisting people with disabilities. The grow-ing impact of robotics on the field of medicine isa consequence of the fact that robot has several abili-ties superior to human beings, such as precision, geo-metrical accuracy and repeatability. Robotic surgeryseems to be particularly interesting field. Two types of
robotic systems are currently used in surgery: tele-manipulators and navigation robots [15]. Telemanipu-lators allow the motions of surgeon to be transferredinto motions of surgical tool ensuring the hand tremorelimination and enabling operator movement scaling.Examples of such systems are Zeus, da Vinci [23] orPolish prototype surgery arm Robin Heart. Navigationrobots operate autonomously and they can be used toperform surgery procedures according to operationpre-planning [27], [10]. This type of robotic systemsis currently used in radiosurgery (CyberKnife, Ac-curay Inc.) or orthopedics (Robodoc, Curexo Tech.Corp.).
In this paper, a method of motion planning fora mobile manipulator acting as an assistant surgeon ispresented. A mobile manipulator is a robotic systemconsisting of a mobile platform and a manipulatormounted on top. By combining the mobility of theplatform with the dexterous capability of the manipu-lator, the system gains kinematic redundancy. Theredundant degrees of freedom render it possible toaccomplish complex tasks in complicated workspaces
______________________________
* Corresponding author: Grzegorz Pajak, Faculty of Mechanical Engineering, University of Zielona Góra, Prof. Zygmunta Szafrana 4,65-246 Zielona Góra, Poland. Tel: +48 683 282 273, e-mail: [email protected]
Received: February 7th, 2014Accepted for publication: February 27th, 2014
G. PAJAK, I. PAJAK12
with obstacles. This system can be used both as a helperproviding the necessary tools or surgery assistant car-rying out pre-planned procedures. In this case, themobile platform enables the position to be reachedwhich will give optimal access to the operating field.Some of the medical procedures require tracing of thepre-planned path, i.e., the robot has to move the end-effector along the curve specified by operator [22].The task of this type is, for example, hip replacement,in which the robot has to mill implant cavity movingthe tool at a particular position along the desired pathand then place the implant there. From a technicalpoint of view such tasks are parts assembly (pick andplace) and handling and stacking operations. To per-form this type of task mobile robot can use, if needed,the wheeled platform or manipulator mounted on itstop and the reference path for the platform is not re-quired. The main problem associated with the motionplanning of such a robot is to determine the controls(wheel and joint torques) which are necessary to ac-complish desired motion. The presented paper is fo-cused on the above mentioned problem.
Introduction of mobile robots has made it difficultto solve the motion planning task. The mobile plat-form has to roll without slipping, which consequentlylimits the motion of the mobile manipulator to theallowed instantaneous velocities and positions.Moreover, the platform introduces additional degreesof freedom and the whole robot becomes redundanteven if its manipulator is non-redundant. These addi-tional degrees of freedom increase the difficulty ofsolving the problem due to the ambiguity of the solu-tion. Further, from practical point of view, it is im-portant to ensure high dexterity of the robot during theexecution of its task. This can be achieved by ensuringa high manipulability of the robot’s manipulator. Thisproperty keeps the robot away from its singular con-figurations, and decreases the probability of recon-figuration of the whole mobile robot during the exe-cution of the task. Most of existing research addressesthe problem using only kinematic equations of themobile manipulator. Due to the method of solving therobot task presented in this paper these approacheswill not be discussed. The dynamic interactions be-tween the manipulator and the mobile platform havebeen studied by Yamamoto and Yun [25]. Desai andKumar [3] have solved the motion planning problemfor multiple mobile manipulators, taking their dy-namics and obstacles in the workspace into consid-eration. In work [13], an input–output feedback line-arisation technique is developed to move the mobileplatform and the manipulator. The solution at thetorque/force level was presented in [8], [9], neverthe-
less control constraints are not taken into considera-tion.
Some methods require knowledge of the mobileplatform and end-effector reference trajectories. Eger-stedt and Hu [4] proposed error-feedback control al-gorithms in which the trajectory for the mobile plat-form is planned in such a way that the end-effectortrajectory is within reach of the manipulator arm.Chung et al. [2] designed two independent controllersfor the mobile platform and the manipulator, never-theless the obtained trajectory is not optimal in anysense. Liu and Lewis [11] developed a decentralisedrobust controller by considering the mobile platformand the manipulator arm as two subsystems. Mazur[12] presented the solution to the path following con-trol problem decomposed into two separated tasksdefined for the end-effector of the manipulator and thenonholonomic platform.
This paper presents a motion planning method forapplications where only the knowledge of the end-effector path is needed. The path of the end-effector isa curve that can be parameterised by any scaling pa-rameter. The robot’s trajectory is planned in sucha manner as to maximise the manipulability measureto avoid manipulator singularities. The proposedmethod guarantees finding continuous controls whichdo not exceed their limits and ensures the fulfilmentof the constraints imposed by the robot’s mechanicallimits and collision avoidance conditions.
2. Materials and methods
2.1. Problem formulation
A mobile manipulator composed of a nonholo-nomic platform and holonomic manipulator is consid-ered to plan a trajectory. It is described by the vectorof generalised coordinates
Trp )( qqq = , (1)
where nℜ∈q – vector of the generalised coordinatesof the whole mobile manipulator; pp ℜ∈q – vector ofthe coordinates of the nonholonomic platform; rr ℜ∈q– vector of joints coordinates of the holonomic ma-nipulator; p, r – the number of coordinates describingthe nonholonomic platform and holonomic manipu-lator; n = p + r.
The platform motion is subject to nonholonomicconstraints described in the Pfaffian form
Motion planning for mobile surgery assistant 13
0qqA =pp )(~ , (2)
where )(~ pqA – )( pk × full rank matrix; k – the num-ber of independent nonholonomic constraints.
To determine the mobile manipulator motion, it isalso necessary to know the robot dynamic equations,given in the general form as
BuλAqqFqqM =++ T),()( , (3)
where )(qM – )( nn× positive definite inertia matrix,),( qqFF = – n-dimensional vector representing
Coriolis, centrifugal, viscous, Coulomb friction andgravity forces; ])(~[)( rk
p×= 0qAqA ; rk×0 – )( rk ×
zero matrix; λ – k-dimensional vector of the La-grange multipliers corresponding to nonholonomicconstraints (2); u – (n – k) -dimensional vector ofcontrols (torques/forces); B – ))(( knn −× full rankmatrix (by definition) describing which state variablesof the mobile manipulator are directly driven by theactuators.
The task of the mobile manipulator is to followa path given in a parametric form
0φqf =− )()( s , (4)
where mn ℜ→ℜ:f – m-dimensional mapping describ-ing the kinematic equations of the robot; m – dimen-sion of the workspace; )(tqq = ; ],0[ Tt∈ ; T – un-known time of performing the task; )(sφ – thefunction representing a path of the end-effector; s – thescaling parameter which depends on time t, i.e.
)(tss = ; ],0[ maxss∈ .The assumption that the manipulator at the initial
moment of motion is in the acceptable (nonsingular,collision free) configuration )0(0 qq = is taken intoaccount. If initial configuration is singular, there aretwo approaches to solve this problem. The first onerequires performing a reconfiguration process, in-volving movement of the platform without changingposition of the end-effector in the workspace. Thesecond solution is to use robust inverse method, whichallows the inverse of Jacobian matrix to be deter-mined.
At the initial moment of the motion the end-effector of the manipulator should be at the initiallocation of the path and at the final moment the entirepath should be traced
0φqf =− )0()( 0 , max)( sTs = . (5)
From the practical point of view, it is natural to as-sume that at the initial and final moments of the task
performance, the velocities of the manipulator equalzero
0q =)0( , 0)0( =s , 0q =)(T , 0)( =Ts . (6)
Constraints connected with the existence of me-chanical limits and collision avoidance conditions canbe described in the following general form
}0))(({],0[ ≥∈∀ tTt i qβ , Li :1= , (7)
where }0))(({ ≥ti qβ – the set of conditions with sca-lar functions )(iβ which involve the fulfilment of themechanical and collision avoidance constraints (seeNumerical example section for exemplary form ofthese functions); L – the number of inequality con-straints.
Additionally, the constant constraints of controlare also assumed
maxmin uuu ≤≤ , (8)
where minu ; maxu – lower and upper limits on u, re-spectively.
In practice, it is important for the configuration ofmanipulator joints to be far away from singular con-figurations. The assumption corresponds to a searchfor the trajectory for which the instantaneous per-formance index is minimized (maximising the ma-nipulability measure [26] generalised in the sense ofmobile manipulators) at each time instant ],0[ Tt∈ .
21
)det()(~ TI jjq −= , (9)
where qqfj ∂∂= )( – )( nm× dimensional Jacobianmatrix of the mobile robot.
The above manipulability measure is only an ex-ample, the method presented may be used with otherindexes of this type (e.g., measure based on the ec-centricity of the ellipsoid [1]).
Dependences (4)–(9) formulate the robotic task asan optimal control problem expressed in rather gen-eral terms. The next section will present an approachthat renders it possible to solve the above optimisationproblem.
2.2. Solution
To solve the problem (4)–(9), an approximate im-plementation of inequality constraints (7) is acceptedin the work. The solution proposed is the explicationof the trajectory planning method for stationary ma-nipulators described in [16], [18], [21] and it uses
G. PAJAK, I. PAJAK14
penalty functions approach [5], [7]. This method re-sults in the following function to be minimized
∑=
+=L
iiiII
0
))(()(~)( qqq βκ , (10)
where )(iκ – the penalty function which associatesa penalty with a violation of a constraint.
The solution of the task was divided into twoparts: the search for an optimal configuration fora given path point )(sϕ and trajectory planning fromthe initial path point to the end point. The task ofsearching for an optimal configuration for a givenpath point involves minimisation of the performanceindex (10) subject to equality constraints (2), (4).Based on the solution presented in [16], [18], [21] andusing the necessary condition for the minimum of theperformance index )(qI [6], this task can be de-scribed as
0qqA
qI1qJqJφqf
qqE =⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛−
−= −−
−
)()(]))())((([
)()(),,(~ 1
qkmnTFR
ss ,
(11)
where TTT ]))(())(([)( qAqjqJ = – ))(( nkm ×+
dimensional full rank matrix, nkm <+ )( ; )(qJR –))()(( kmkm +×+ dimensional matrix constructed
from )( km + linear independent columns of J; )(qJF –))()(( kmnkm −−×+ dimensional matrix obtained by
excluding RJ from J; kmn −−1 – )()( kmnkmn −−×−−identity matrix; qqI ∂∂= Iq )( .
Mapping ),,(~ sqqE may be interpreted as somemeasure of error between a current configuration qand acceptable configuration for a given path point s.Dependence (11) presents a system of n nonlinearequations which fully specify the configuration q ata given path point. To find the trajectory of the ma-nipulator )(tq and the path parameterisation )(tsEquation (11) has been extended by an additionalelement responsible for tracing the entire path
⎟⎟⎠
⎞⎜⎜⎝
⎛=⎟
⎟⎠
⎞⎜⎜⎝
⎛
−=
),(),(),,(~
),,(max qqE
qEqqEqqE II
I sss
ss (12)
where
[ ]⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛
−−
−= −−
−
max
1 )())())((()()(
),(ss
ss qkmn
TFRI qI1qJqJφqf
qE ,
qqAqqE )(),( =II .
Equation (12) describes the path following error andit is used to obtain the trajectory )(tq and path param-eterisation )(ts parameterised by gain coefficients I
iV ,Λ ,I
iL,Λ and IIiL,Λ . The trajectory determined using (12)
minimises the performance index (9) and fulfils kine-matic constraints (7). Then, using dynamic model of therobot, the controls necessary to accomplish trajectory
)(tq will be calculated. At this stage the control con-straints will be taken into account and if calculated val-ues of controls are close to limitations imposed the val-ues of gain coefficients I
iV ,Λ and IiL,Λ will be modified
in such a way as to fulfil these constraints.To solve the trajectory planning problem, the fol-
lowing system of equations is introduced
,
,
0EΛE
0EΛEΛE
=+
=++IIII
LII
IIL
IIV
I
(13)
where )...,,(diag 1,1,I
knVIV
IV +−ΛΛ=Λ , ,(diag 1,
IL
IL Λ=Λ
)..., 1,I
knL +−Λ – ( ))1()1( +−×+− knkn diagonal matri-
ces with positive coefficients IiV ,Λ , ,,
IiLΛ ensuring the
stability of the first equation; ...,,(diag 1,IIL
IIL Λ=Λ
),II
kLΛ – )( kk × diagonal matrix with positive coeffi-
cients ,,II
iLΛ ensuring the stability of the second equa-tion.
This is a system of homogeneous differentialequations with constant coefficients. In order to solvethese equations )22( +− kn consistent dependenciesshould be given. These dependencies can be obtained,taking into account initial conditions (6), from (12) fort = 0.
Using the Lyapunov stability theory it is possibleto show that the above form of the system of differ-ential equations (13) ensures that its solution is as-ymptotically stable for positive coefficients I
iV ,Λ ,I
iL,Λ and IIiL,Λ . Lower equation (13) is trivial as-
ymptotically stable for 0, >ΛIIiL . In order to show the
stability of the upper equation (13), let us considerthe Lyapunov function candidate of the followingform
⟩⟨+⟩⟨= IIL
IIIIIV EΛEEEEE ,21,
21),(
where >⋅< – the scalar product of vectors.
Motion planning for mobile surgery assistant 15
It is easy to see that ),( IIV EE is non-negative forpositive coefficients I
iL,Λ . After differentiating and
substituting IE determined from (13), V can bewritten as
⟩−⟨= IIV
IIIV EΛEEE ,),(
As one can see, for positive coefficients IiV ,Λ ,
0),( ≤IIV EE . It can be shown that 0),( =IIV EEfor 0E =I . Additionally, if 0E =I then 0E =I andfrom upper equation (13) it follows that 0E =I .Based on the La Salle invariant principle it can beconcluded that solution of the system of differentialequations (13) is asymptotically stable for positivecoefficients I
iV ,Λ , IiL,Λ and II
iL,Λ . This property im-plies fulfilment of conditions (5) and (6) associatedwith the final moment of the task perform ance, e.g.
0q →)(T ; max)( sTs → ; 0)( →Ts as ∞→T .The above analysis has been carried out on the as-
sumption that IiV ,Λ , I
iL,Λ and IIiL,Λ are constant coef-
ficients. However, these coefficients are used forscaling the trajectory of the mobile manipulator en-suring fulfilment of control constraints, so I
iV ,Λ , IiL,Λ ,
IIiL,Λ can vary with time. Due to the fact that rapid
changes of gain coefficients lead to large values ofcontrols it is practically reasonable to assume that
IiV ,Λ , I
iL,Λ , IIiL,Λ are slowly varying functions of time
( 0, ≅ΛIiV , 0, ≅ΛI
iL , 0, ≅ΛIIiL ), and in such a case the
above analysis holds true.In addition, if the gain coefficients I
iV ,Λ and IiL,Λ
satisfy the following inequalities
IiL
IiV ,, 2 Λ>Λ , (14)
the function ),( sI qE becomes also a strictly monotonicfunction. For the initial configuration 0q satisfyingequation (11) this property forces fulfilment of mechani-cal constraints, collision avoidance conditions and sin-gularity-free robotic motions along a given path (4).
The key moment of the aforementioned analysiswas the assumption of the full rank of matrix RJ . It isthe consequence of the following reasoning. Let usassume that the mobile manipulator consists of a plat-form of (2,0) class (as in Numerical example section).In this case matrix J can be written in the followingform
⎥⎦
⎤⎢⎣
⎡=
×rk
rp
0qAqjqjJ
)(~)()(
,
where pp qqfqj ∂∂= )()( , rr qqfqj ∂∂= )()( , rk×0– )( rk × zero matrix.
The matrix J will be full rank matrix providedthat )(qjr and )(~ qA are full rank matrices. )(~ qA isa full rank matrix by definition. The matrix )(qjr maybe determined as
r
rr
qqfRqj
∂∂
=)()()( θ ,
where θ – the platform orientation, )(θR – the rota-tion matrix which specifies orientation of the mobileplatform in the base system, )( rr qf – kinematicmodel of holonomic manipulator.
Determinant of matrix )(qjr is equal to determi-nant of matrix rr qqf ∂∂ /)( . In order to ensure the fullrank of the matrix rr qqf ∂∂ /)( the manipulabilitymeasure is maximised.
In order to obtain the trajectory of the manipu-lator )(tq and the path parameterisation )(ts , aftersimple calculations equation (13) can be rewrit-ten as
))(( maxssss IL
IV −Λ+Λ−= , (15)
where IE – the dependence obtained by excluding
)( maxss − from IE ; qqEE
∂∂
=),( sI
Iq ;
ssI
Is ∂
∂=
),(qEE ;
)...,,(diag ,1,I
knVIV
IV −ΛΛ=Λ ; )...,,(diag ,1,
IknL
IL
IL −ΛΛ=Λ ;
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
+
+++++−=
⎥⎥⎦
⎤
⎢⎢⎣
⎡
IIIIL
IIq
IIL
Is
Iq
IV
Is
Is
Iq
IIq
Iq ss
dtds
dtd
EΛqE
EΛEqEΛEEqEq
EE )()()(
, (16)
G. PAJAK, I. PAJAK16
qqqEE
∂∂
=),(II
IIq ;
qqqEE
∂∂
=),(II
IIq ; I
knVIV 1,Λ +−Λ= ;
IknL
IL 1,Λ +−Λ= .The mobile manipulator trajectory is the solution
of the system of second order differential equations(15)–(16). To determine values of controls which arerequired to realise the trajectory it is necessary totransform the dynamic equation (3). For this purposethe nonholonomic constraints are expressed by ananalytic driftless dynamic system
vpp )(~ qNq = , (17)
where )(~ pqN – ))(( kpp −× dimensional matrix;v – )( kp − dimensional vector denotes scaled angu-lar velocities of the platform driving wheels.
Introducing the full rank matrix
( ) ⎥⎥⎦
⎤
⎢⎢⎣
⎡=
×−×
×
rrkpr
rpp
1)(~
)(0
0qNqN (18)
and multiplying the dynamic equation (3) by )(qNT ,noting that 0qNqA =)(~)(~ pp , we obtain equation thatallows determination of controls for a current trajec-tory
uBqNqqFqNqqMqN )(),()()()( TTT =+ . (19)
In order to ensure fulfilment of constraints (8) anidea of trajectory scaling, presented in [17], [20], [21],is used. In this case it is suggested to modify values ofcoefficients I
VΛ , ILΛ , I
VΛ , ILΛ . It can be shown [19]
that decreasing these parameters involves reducingcontrol values which are necessary for the executionof the manipulators task. Finally, the solution ofequations (15)–(16) with suitable gain coefficientsgives an optimal trajectory satisfying path constraints(4), inequality constraints (7), control constraints (8)and boundary conditions (6).
3. Results
In the numerical example, a mobile manipulatorconsisting of a nonholonomic platform of (2, 0) classand a 3DOF RPR type holonomic manipulator work-ing in a three-dimensional task space is considered.A kinematic scheme of the mobile manipulator isshown in Fig. 1.
θ
φ 1
φ 2
q1
q2
q3
(xr, yr)
XPYP
ar
YB
XBxc
yc
l2l3
Fig. 1. Kinematic scheme of the mobile manipulator
The mobile manipulator is described by the vectorof generalised coordinates
Tcc qqqyx ),,,,,,,( 32121 φφθ=q ,
where ),( cc yx – the platform centre location; θ – theplatform orientation; 21,φφ – angles of driving wheels;
1q , 2q , 3q – angles and offset of the manipulatorjoints.
The kinematic parameters of the mobile manipu-lator are given as (all physical values are expressed inthe SI system): 3.02 =l , 2.03 =l , 3.0=a , 075.0=r .Respectively, the masses of the mobile manipulatorelements amount to: 20,20,94 32 === mmmp ( pmis the total mass of the platform, 32,mm are themasses of the manipulator arm).
The motion of the platform is subject to oneholonomic and two nonholonomic constraints, so con-straints (2) in this case take the following form
,0)sin(2
)sin(2
,0)cos(2
)cos(2
,022
21
21
21
=−−
=−−
=+−
φθφθ
φθφθ
φφθ
rry
rrx
ar
ar
c
c
where r – the radius of driving wheels; a – half-distance between the wheels.
The task of the manipulator is to trace a path de-scribed by the following equation
Tssss )86.05.0,2,64.02()( ++=φ , ]1,0[∈s .
Constraints (5), (7), (8) amount to
Motion planning for mobile surgery assistant 17
The penalty function introduced in order to takeinto account mechanical and collision avoidance con-straints (7) is taken as follows
⎩⎨⎧ ≤−
=otherwise,0
,)(for))(())((2
iiiiii
εβεβρβκ
qqq
where ρ – the positive coefficient determining thestrength of penalty; iε – the positive coefficient de-termining the threshold value which activates the i-thconstraint.
To preserve mechanical limits scalar functions)(⋅iβ (7) are expressed as
,)(,)(
max
min
jji
jji
qqqq−=
ββ
rj :1= .
For collision avoidance purposes functions )(⋅iβspecify distances between the manipulator and obsta-cles. Due to the on-line trajectory planning, in thispaper, a method of obstacle enlargement with reduc-tion of the manipulator size is used. In this case, thecollision test leads to checking a finite number of ine-qualities, so functions )(⋅iβ are expressed as
δβ −= )()( Pq ji S ,
where )(⋅jS – equation of the j-th obstacle surface;Tzyx ),,(=P – point from the set of points which
approximate the mobile manipulator; δ – small posi-tive scalar, safety margin.
For sphere with radius jr and the centre pointT
jjj cba ),,( , )(⋅jS takes the following form
},0)(
)()(:),,({)(
22
22
=−−+
−+−==
jj
jjT
j
rcz
byaxzyxS PP
and for cylinder with radius jr , height jh and the
centre point of its base placed at Tjj ba )0,,(
}.0,0)(
)(:),,({)(
22
2
jjj
jT
j
hzrby
axzyxS
≤≤=−−+
−== PP
Three cases of performing this task are considered.The first one is an end-effector motion along the pathwithout control constraints (8) and collision avoidanceconstraints. In the second case the same task as in thefirst experiment is solved, but control constraints (8)are taken into account. In the third simulation, there isan addition of obstacles in the workspace.
The values of gain coefficients have been chosenin such a way as to show their influence on the fulfil-ment of constraints imposed on controls. The valuesof coefficients I
iL,Λ , IIiL,Λ for the first simulations
have been selected experimentally and set to 1, =ΛIiL ,
1, =ΛIIiL , the values of coefficients I
iV ,Λ depend on
inequality (14) and they have been set to 1.2, =ΛIiV .
The mobile manipulator controls ( 21,uu – wheeltorques, 53,uu – joint torques and 4u – joint force)obtained in the numerical simulations are shown inFig. 2. For this solution the final time T is 9.78 [s],and it can be seen that the controls exceed the as-sumed constraints.
The second simulation presents the solution of thesame task as the first one but control constraints (8)are considered. For simplicity of numerical calcula-tions, in second and third simulations, the values ofgain coefficients have been assumed to be constant.
1.06, =ΛIL has been chosen in such a way as to ensure
fulfilling control constraints both in the second andthird simulation, 66.06, =ΛI
V results from dependence(14). Such a reduction of gain coefficients results insignificantly greater time of the task accomplishmentand in this case it equals 23.66 [s], but the controlsdetermined do not exceed the assumed constraints.Figure 3 presents the controls obtained in this simula-tion.
In the third simulation, there are two obstacleswhich may collide with the mobile manipulator. Thefirst one is represented by a cylinder with radius 0.25and height 2.0. The centre point of its base is placed at
T)0.0,05.0,0.1( − . The second obstacle is a spherewith radius 0.25 and the centre point placed at
T)7.0,0.1,5.1( .If collision avoidance conditions are not taken into
account, the platform collides with the cylinder (the
.)0.15,0.400,0.2,0.10,0.10(,)0.0,0.0,0.2,0.10,0.10(,)23,0.2,(,)2,5.0,(
,)4,0.1,0.0,0.0,0.0,0.0,0.0,0.0(
maxmin
maxmin
0
TT
TrTr
T
=−−−==−−=
=
uuqq
q
ππππ
π
G. PAJAK, I. PAJAK18
sphere is above the motion plane) and the holonomicmanipulator collides with sphere. To show these po-tential collisions, distances between the mobile ma-nipulator from the second simulation and the centresof obstacles introduced in the third simulation areshown on the left side in Fig. 4. As can be seen, therobot collides with the first obstacle for ]0.5,1.2[∈tand with the second obstacle for ]9.7,1.5[∈t .
Parameters for the third simulation are the same asin the previous one. For this experiment, the final time
T is equal to 23.68 [s] and both control constraints andcollision avoidance constraints are satisfied. Due tothe obstacles acting on the mobile manipulator thecontrols are slightly sharp. Figures 5 and 6 present themanipulator motion, and controls obtained in thissimulation. Distances between the robot and the cen-tres of the obstacles are shown on the right side in Fig. 4.As can be seen, the mobile manipulator penetratesthe safety zone of the first obstacle for ]9.5,2.1[∈t .At the time instant 4.4 robot enters the safety zone of
Fig. 2. Controls corresponding to the motion for the first case
Fig. 3. Controls corresponding to the motion for the second case
Fig. 4. Distances between the mobile manipulator and the centres of the obstacles:for the second case (left) and for the third case (right)
Motion planning for mobile surgery assistant 19
the second obstacle and remains there to the timeequal to 9.2.
Fig. 5. Collision-free mobile manipulator motion for the third case
4. Discussion
In the paper, a concept of the mobile manipulatoracting as an assistant of surgeon has been presented.The task of such a system is to move the end-effectoralong the curve specified by operator during pre-planning of the operation. This path is a curve thatcan be parameterised by any scaling parameter, theknowledge of the reference trajectory of a mobileplatform is not needed. Mounting the arm on top ofthe mobile wheeled platform extends the perform-ance capabilities of the manipulator and makes itpossible to reach the position which will give opti-mal access to the operating field. Moreover, the ma-nipulator motion is planned in a manner that maxi-mises the manipulability measure in order to avoidmanipulator singularities decreasing the probability
of reconfiguration of the whole mobile robot duringthe execution of the task. Additionally, constraintsconnected with the existence of mechanical limita-tions, collision avoidance conditions and controlconstraints have been considered.
To the best of the authors’ knowledge, no researchhas considered the problem formulated in the abovemanner. Many of the existing research addresses theproblem using only kinematic equations of the mobilemanipulator, and the dynamics of the robot is not con-sidered at all. There are some references to kinematicand dynamic model-based control methods, neverthe-less in these solutions kinematics is represented asa driftless control system and control constraints arenot taken into consideration ([8], [9]). Opposite to theabove approaches, in our method constraints resultingfrom movement without slipping are incorporatedexplicitly to the control algorithm. The solution pre-
sented allows application of scaling techniques[17], [20], [21] for the mobile manipulator trajec-tory in such away as to fulfill control constraints. Inmany works authors are mainly concerned withredundancy resolution and their solutions are notoptimal in any sense [2]. Our solution maximizesthe manipulability measure what results in avoidinga time consuming reconfiguration process of thewhole mobile robot. Opposite to similar research(e.g., [12]) in the solution proposed the referencepath for the platform is not required, which makes itpossible to apply our method in complicated work-spaces including obstacles. The method presented inthe paper ensures finding continuous controls. Theproposed approach to trajectory planning is a com-putationally efficient method. The effectiveness ofthe solution is confirmed by the results of computersimulations.
Fig. 6. Controls corresponding to the motion for the third case
G. PAJAK, I. PAJAK20
References
[1] BAYLE B., FOURQUET J.Y., RENAUD M., Manipulability ofWheeled Mobile Manipulators: Application to Motion Genera-tion, Int. Journal of Rob. Res., 2003, 22(7–8), 565–581.
[2] CHUNG J., VELINSKY S., HESS R., Interaction control of a redun-dant mobile manipulator, Int. J. Rob. Res., 1998, 17(12),1302–1309.
[3] DESAI J., KUMAR V., Nonholonomic motion planning formultiple mobile manipulators, Proc. of the IEEE Int. Conf.on Rob. and Autom., 1997, 4, 3409–3414.
[4] EGERSTEDT M., XU H., Coordinated trajectory following formobile manipulation, Proc. of the IEEE Int. Conf. on Rob.and Autom., 2000, 3479–3484.
[5] FIACCO A.V., MCCORMICK G.P., Nonlinear Programming:Sequential Unconstrained Minimization Techniques, JohnWiley & Sons, New York, 1968.
[6] GALICKI M., Optimal planning of collision-free trajectory ofredundant manipulators, The International Journal of Ro-botics Research, 1992, 11(6), 549–559.
[7] GALICKI M., The planning of robotic optimal motions in thepresence of obstacles, Int. Journal of Rob. Res., 1998, 17(3),248–259.
[8] GALICKI M., Task space control of mobile manipulators,Robotica, 2011, 29, 221–232.
[9] GALICKI M., Collision-free control of mobile manipula-tors in a task space, Mech. Syst. Sig. Proc., 2011, 25(7),2766–2784.
[10] LARYSZ D., WOLAŃSKI W., KAWLEWSKA E., MANDERA M.,GZIK M., Biomechanical aspects of preoperative planning ofskull correction in children with craniosynostosis, Acta Bio-eng. Biomech., 2012, 14(2), 19–26.
[11] LIU K., LEWIS F. L., Control of mobile robot with onboardmanipulator, Proc. of the Int. Symp. on Rob. and Manuf.,1992, 4, 539–546.
[12] MAZUR A., Path following for nonholonomic mobile manipula-tors, Rob. Motion and Control, LNCIS 360, 2007, 279–292.
[13] MAZUR A., Trajectory tracking control in workspace-definedtasks for nonholonomic mobile manipulators, Robotica,2010, 28, 57–68.
[14] MORECKI A., Manipulatory bioniczne, PWN, Warszawa,1976.
[15] NAWRAT Z., Medical robotics 2007 – trends & short review,Medical Robots, 2007, 1, 13–27.
[16] PAJAK G., GALICKI M., Collision-free trajectory planning ofthe redundant manipulators, Proc. of the Methods and Mod-els in Automation and Robotics, 2000, 2, 605–610.
[17] PAJAK G., PAJAK I., Planning of an optimal collision-freetrajectory subject to control constraints, Proc. of the 2nd In-ternational Workshop on Robot Motion and Control, 2001,141–146.
[18] PAJAK G., PAJAK I., Sub-optimal trajectory planning of theredundant manipulators, International Journal of AppliedMechanics and Engineering, 2009, 14(1), 251–260.
[19] PAJAK G., PAJAK I., Sub-optimal trajectory planning formobile manipulators, Robotica, 2014, DOI: 10.1017/S0263574714000198.
[20] PAJAK G., PAJAK I., GALICKI M., Trajectory planning ofmultiple manipulators, Proc. of the 4th International Work-shop on Robot Motion and Control, 2004, 121–126.
[21] PAJAK I., GALICKI M., The planning of suboptimal collision-free robotic motions, Proc. of the 1st International Workshopon Robot Motion and Control, 1999, 229–243.
[22] STOCCO L., Path verification for unstructured environments,ASME Design Conf., Symp. Mechan. Devices Medical Ap-plic., Pittsburgh, PA, 2001, 1103–1108.
[23] SUNG G.T., GILL I.S., Robotic laparoscopic surgery: a com-parison of the da Vinci and Zeus systems, Urology, 2001,58(6), 893–898.
[24] WOŁCZOWSKI R., BĘDZIŃSKI R., Construction and control ofhuman hand bio-prosthesis, VII National Conference on Ro-botics, 2001, 2, 247–258.
[25] YAMAMOTO Y., YUN X., Effect of the Dynamic Interaction onCoordinated Control of Mobile Manipulators, IEEE Trans.on Rob. and Autom., 1996, 12(5), 816–824.
[26] YOSHIKAWA T., Manipulability of Robotic Mechanisms, Int.Journal of Rob. Res., 1985, 4(2), 3–9.
[27] ZANETTI E.M., BIGNARDI C., Mock-up in hip arthroplastypre-operative planning, Acta Bioeng. Biomech., 2013, 15(3),123–128.
