113

Hi ngh C hc k thut ton quc

K nim 35 nm thnh lp Vin C hc. H Ni, 09/04/2014

Tch hp Maple v C/C++ trong tnh ton v m phng robot

Phan Bi Khi1 , L Minh Ngha2
B mn C hc ng dng, Vin C Kh, Trng i hc Bch khoa H Ni
1
khoi.phanbui@hust.edu.vn, 0913.525.160
2
minghiale.m54@gmail.com, 0904.630.796

Tm tt
Hin nay, vic tnh ton v m phng robot thng s dng cc phn mm tnh ton nh Maple, Matlab, Cc chng
trnh tnh ton ch chy c trn cc my tnh c ci t phn mm ca hng. Hn na, to ra cc chng trnh tnh ton
tng qut trong trng hp i tng thay i th s lng bin v tham s m hnh ha cho i tng s ln v lm cho
chng trnh chy tc chm. Bi bo ny nghin cu cch tch hp phn mm Maple v C/C++ to ra chng trnh tnh
ton tng qut, c tc tnh ton cao v c th chy c lp trn cc my tnh. Trong , chng trnh tnh ton trn Maple
s c chuyn sang ngn ng C/C++, v to thnh cc th vin c th tng thch vi mi loi my.
T kha: Code Generation, Maple, C/C++

1. Gii thiu
Vic tnh ton v m phng robot yu cu tc tnh ton v tnh tng qut ngy cng cao. Cc phn mm
tnh ton robot hin nay thng s dng l phn mm Maple, Matlab. Cc phn mm ny cha mt th vin ton
hc s, h tr chng ta gii cc bi ton trong thc t mt cch thun li nht. Maple l mt phn mm mnh
v tnh ton biu tng. Cc bi ton gii trn Maple thng cho ta li gii tng qut. Matlab l phn mm mnh
v tnh ton s. Hn ch ca cc chng trnh ny l ch c th chy trn cc my tnh ci t cc phn mm
ca hng, v yu cu my tnh phi c cu hnh cao ci t. Mt khc, cc chng trnh tnh ton vit bng
ngn ng C/C++ c th dch thnh m my. Do , cc chng trnh ny c tc tnh ton cao, chy c lp
trn tt c cc my tnh. Nhng cc chng trnh vit bng C/C++ li khng th thc hin cc php ton biu
tng mt cch tng qut v cc th vin ton trn C/C++ cng hn ch v i khi lp trnh chng cng rt
phc tp.
Bi bo ny nghin cu cch tch hp im mnh ca cc chng trnh phn mm trn. ng thi a ra
gii thut cho qu trnh tch hp. Trong bi bo ny, tc gi chn robot Kuka KR6 lm i tng minh ha
qu trnh tch hp v kt qu tnh ton, m phng t c.

2. M hnh ha robot
Hnh 1: Robot Kuka KR6

Hnh 1: Robot Kuka

KR6

Hnh 2: M hnh 3D robot

Kuka KR6

Robot Kuka KR6 l tay my 6 khp quay, 6 bc t do. T m hnh 3D (Hnh 2), ta gn cc h ta xc nh
mi lin h ng hc gia cc khu ca tay my.
Trong , X0Y0Z0 l h ta c s c nh. Cc h XiYiZi (i=1..n) l cc h ta khu, vi robot Kuka n=6.

114

Phan Bi Khi, L Minh Ngha

Php bin i ca cc h ta lin tip c xc nh bi cc ma trn truyn thun nht (theo [1] hoc [3]),
biu din cc php tnh tin v quay cc h ta [3] (Bng 1)
Bng 1: Quy lut bin i h ta khu trong robot Kuka
Khu

T hp php tnh tin v quay

1
2

Translate(tx1, ty1, tz1) * RotZ(q1)

Translate(tx2, ty2, tz2) * RotX(q2)

Translate(tx6, ty6, tz6) * RotY(q6) *RotX(rx6)

Trong :
Translate(xi, yi, zi): Php tnh tin h ta theo vector c ta (xi, yi, zi), c biu din bng ma trn 4x4:

1
0
Translate xi , yi , zi
0

0 0
1 0
0 1
0 0

xi
yi

zi

(1)

RotX(qi), RotY(qi), RotZ(qi): Php quay quanh trc Ox, Oy, Oz mt gc qi. c biu din bng ma trn 4x4:
1 0
0 c
i
RotX qi
0 si

0 0

0
si
ci
0

0
ci
0
0
, RotY qi
0
si

1
0

0 si
1 0
0 ci
0

0
ci
s
0
, RotZ qi i
0
0

1
0

si
ci
0
0

0 0
0 0

1 0

0 1

(2)

ci cos qi ; si sin qi

Cc tham s (txi, tyi, tzi) (i=1..6) v rx6 l cc tham s tnh ton ng hc ca robot. Cc gi tr ny c trng cho
kch thc c th ca robot.
Cc ta khp c biu din bi:

q [ q1 ,..., q6 ]T

(3)

Ta thao tc:

p [ xE , y E , z E , , , ]T

(4)

Trong xE, yE, zE l ta im tc ng cui E, cn cc tham s , , l cc gc cardan biu din hng

ca khu thao tc ca robot.
S dng cc ma trn bin i ta nu trn ta thit lp c h phng trnh ng hc robot:

f ( q, p ) 0

(5)

y: f f1 ,f 2 ,...,f 6 , vi q, p biu din (3), (4).

3. Xc nh cc i lng cn tnh ton

i vi cc bi ton tnh ton phn tch robot ni chung, c th chia ra lm 6 loi bi ton, c trnh by
di y:

Bi ton ng hc thun v hng v v tr:

Gii h 6 phng trnh ng hc robot (5) vi 6 n s biu din bi (4), xc nh c v tr v hng khu
thao tc.

Bi ton ng hc ngc v hng v v tr:

Gii h 6 phng trnh ng hc robot (5) vi 6 n s biu din bi (3), xc nh c cc bin khp biu
din v tr ca cc khu ca robot.

Bi ton ng hc thun vi phn: o hm (5) theo thi gian:

q q ,
J p p=J

(6)

Tch hp Maple v C/C++ trong tnh ton v m phng robot

vi cc ma trn Jacobi c xc nh: J p

f
,
p

Jq

115

f
q

T (6) ta c:

-1pJ q q
p=J

(7)

pp
p=J -1p J q
q+J q q-J

(8)

Bi ton ng hc ngc vi phn: T (6) ta c

-1qJ p p
q=J

qq
q=J q1 J p
p+J p p-J

Bi ton ng lc hc thun: Phng trnh ng lc robot

M q
q+C q,q q+G
q =

(3)

Thut ton tnh ton cc ma trn M q ,C q,q ,G q c trnh by trong nhiu ti liu [1], [3],
Khi bit momen tc dng ti cc khp v cc iu kin u, gii h phng trnh vi phn (8) s dng thut ton
Runge-Kutta hoc Runge-Kutta Nystrom (theo [5]).
Cc ma trn M q ,C q,q ,G q l cc hm gm i s l cc bin khp v c cc tham s ng lc l:
+ khi lng cc khu: mi , i 1..6
+ v tr trng tm ca khu trong h ta khu:
T

rC( i ) xC( i ) , yC( i ) , zC( i ) , i 1..6

+ Ma trn qun tnh khi i vi h ta khu.

I (i i )

I xx(i )

I xy(i )
I xz(i )

I xy( i )
(i)
I yy
I (yzi )

I xz( i )

I yz( i ) , i 1..6
I zz( i )

Bi ton ng lc hc ngc:
Lc (momen) cn t ti cc khp sinh ra chuyn ng tng ng c xc nh theo (8):

=M q
q+C q,q q+G
q

(4)

Bng 2: M t cc i lng cn tnh ton ca mi loi bi ton.

Bi ton
ng hc thun v tr
ng hc ngc v tr
ng hc thun vi phn
ng hc ngc vi phn
ng lc hc thun
ng lc hc ngc

i lng cn tnh ton

V tr v hng ca khu thao tc l hm ca cc bin khp
Rng buc gia v tr khu thao tc v cc bin khp.
Ma trn Jacobi ca rng buc .
V tr, vn tc v gia tc im thao tc l hm ca cc bin khp
Rng buc gia v tr, vn tc, gia tc im thao tc v cc bin khp.
Ma trn Jacobi, o hm ca ma trn Jacobi theo thi gian
Phng trnh ng lc ca robot (xc nh cc ma trn M, C, G).
Phng trnh ng lc ca robot (xc nh cc ma trn M, C, G).

116

Phan Bi Khi, L Minh Ngha

4. Gi cng c chuyn code ca Maple sang code C/C++

Phn mm Maple c 2 gi (package) h tr vic chuyn code Maple sang code ca cc ngn ng khc nh
(C, Csharp, Fotran, Matlab, Java) l gi codegen v gi CodeGeneration (trong gi CodeGeneration l
phin bn mi ca codegen).
V d 1: Chuyn mt biu thc trong Maple thnh 1 biu thc trong C/C++
>
>
cg = x + y + z*z + sin(x*y + sqrt(z));

V d 2: Chuyn mt th tc trong Maple thnh mt hm trong C/C++

>
>
double ham_1 (double x, double y)
{
return(x + y);
}

Gi cng c codegen cha mt tp hp cc th tc c chc nng bin i cc biu thc v th tc trong

Maple thnh cc biu thc v hm trong C cng nh cc ngn ng khc. Cc th tc bao gm:
C(deprecated), cost, declare, dontreturn, eqn, fortran (deprecated), GRADIENT, HESSIAN, horner, JACOBIAN,
joinprocs, makeglobal, makeparam, makeproc, makevoid, MathML, optimize, packargs, packlocals,
packparams, pre2trans, renamevar, split, swapargs.
Gi cng c CodeGeneration l phin bn nng cp ca gi cng c codegen, n c th chuyn biu
thc Maple sang cc ngn ng khc mt cch ti u nht. Danh sch cc th tc ca gi cng c ny l:
C, CSharp, Fortran, Java, Matlab, VisualBasic

5. Tnh ton cc phng thc trn Maple v chuyn code ca chng sang code C/C++
i vi bi ton tnh ton trn phn mm Maple ni chung th ta c nhiu cch lp trnh v tnh ton.
Nhng do gi cng c chuyn code ca Maple l codegen v CodeGeneration ch c th chuyn cc hm v
php ton c trong th vin math.h ca C m thi. V vy, phn ny tc gi trnh by phng thc lp trnh tnh
ton chuyn code xc nh trong mc 2 trn Maple v dng m Maple c th chuyn code c sang code C,
v ti u ha cc phng thc .
V d:
>
>

Cng nh cc phng thc cn chuyn code l vn tc v gia tc im thao tc xc nh trong mc 2, biu

thc fx v dfx nh trn cng l cc hm cha bin v o hm ca chng. R rng, Maple khng th chuyn
c 2 biu thc fx v dfx thnh code C c.
>
Warning, the function names {x} are not recognized in the target language
cg1 = cos(x(t));
>
Warning, the function names {x, diff} are not recognized in the target language
cg2 = -sin(x(t)) * diff(x(t), t);

C 2 gii php cho vn ny:

Gii php 1: Gii php thun - Ta lp trnh trn Maple ngay t u v ngm hiu cc o hm ca cc bin l cc
biu tng, khi o hm ta thay th cc o hm thnh cc biu tng .
Gii php 2: Gii php ngc - Ta lp trnh tnh ton ton b chng trnh bng Maple cho n khi ra biu thc
cui cng cn tnh ton. Khi , biu thc cn tnh ton s cha cc hm v tham s m trong C khng c v
Maple cha th chuyn ngay biu thc sang C c. Vy chuyn biu thc sang C ta s phi thay th

Tch hp Maple v C/C++ trong tnh ton v m phng robot

117

cc hm v tham s m trong C khng c thnh cc biu tng mi. Lnh subs trong Maple cho php ta lm
vic ny.
Gii thut cho qu trnh tch hp Maple v C/C++ (Hnh 3)
Xy dng m hnh, i
tng robot trong C/C++

Xc nh cc phng thc
ca i tng cn chuyn
code

Tnh ton tng qut trn

Maple
Chuyn cc th tc trong
Maple sang C/C++

Lin kt vi cc
th vin C/C++
Chng trnh kt hp
gia Maple v C/C++

Th vin lin kt
ng (*.dll, *lib)

Application
(*.exe)
Hnh 3: Gii thut qu trnh tch hp Maple v C/C++

6. Chng trnh m phng s dng code chuyn t Maple sang C/C++

Chng trnh m phng c lp trnh s dng lp trnh giao din MFC ca C/C++ v s dng th vin
ha OpenGL, v th vin m ngun m Eigen ([6]) l th vin tnh ton ma trn v vector.

Bi ton ng hc thun vi phn vi u vo l cc bin khp c cho nh sau:

t
sin ; q3 cos 2t
4
6
7
2

t
t
q4 sin t cos(2t ); q5 sin ; q6 cos 2t sin
10
6
7
6
2
2
q1

sin t ; q2

Kt qu l m phng chuyn ng 3D v th biu din bin khp v vn tc, gia tc im thao tc (hnh 4,5)

Hnh 4: M phng chuyn ng 3D

Hnh 5: th bin khp (tri) v gia tc

im tc ng cui (phi)

Bi ton ng hc ngc: V tr im thao tc chuyn ng trn 1 ng cong trn c xc nh (l giao

ca mt mt cu v mt mt phng), hng ca khu thao tc c xc nh bng 3 gc Cardan ([3]):

118

Phan Bi Khi, L Minh Ngha

x 2 y 1 2 z 2 1

x y 1
0 0

Hnh 6: M phng ng hc ngc

Hnh 7: Hp thoi thit lp cc tham s

Ta c th thay i cc tham s ng hc v ng lc hc ca robot Kuka thng qua hp thoi thit lp cc tham

s (hnh 7) bng cch click vo nt bm Configuration.

Kt lun
Bi bo trnh by gii thut quy trnh tch hp Maple v C/C++ cho robot Kuka KR6. Kt qu minh ha
chng trnh tnh ton v m phng cho robot Kuka v cho cc robot c cng cu hnh v cc tham s vi robot
Kuka.
Gii thut trnh by c th p dng m rng cho mi robot c cc cu trc phc tp khc, cc chng
trnh trn C/C++ c th xy dng nn cc chng trnh m phng hoc c th dch thnh cc th vin lin kt
dng cho cc chng trnh khc. Cc robot c cng cu hnh v cc tham s ng hc v ng lc c th dng
cng mt module tch hp.

Ti liu tham kho

[1]
[2]

[3]
[4]
[5]
[6]
[7]

Phan Bui Khoi (2004). Dynamical investigation of relation manipulation mechanisms in mechanical processing.
Proceedings of National Conference on Mechanics. Vol. 1, pp. 181-190.
Phan Bi Khi, Anh Tun, H Huy Hng (2009). iu khin robot hn im chuyn ng thao tc cng ngh theo
chng trnh. Tuyn tp cng trnh Hi ngh c hc ton quc k nim 30 nm Vin C hc v Tp ch c hc. Tp 2,
tr. 331-338.
Nguyn Vn Khang (2007). ng lc hc h nhiu vt. NXB Khoa hc v k thut.
Nguyn Vn Khang, Chu Anh M (2011). C s robot cng nghip. NXB Gio dc Vit Nam.
inh Vn Phong (2010). M phng s v iu khin cc h c hc. NXB Gio dc Vit Nam.
http://eigen.tuxfamily.org
Maple Programming Guide, Copyright Maplesoft, a division of Waterloo Maple Inc. 2011