Functions
id	v1 ()
1	figure ()
	plot (x0(1), x0(2),'rx')
	plot (x1(1), x1(2),'rx') plot(x2(1)
	x2 (2)
rx	plot (x3(1), x3(2),'rx') plot(x4(1)
rx	x4 (2)
rx rx	axis ([0 11.5 0 7]) xlabel('x')
	ylabel ('y')
	title ('Kubicky spline z Matlabu')
	plot (X2(i), Y2(i))
	plot (X3(i), Y3(i))
	plot (X4(i), Y4(i))
	x2 (1)-x1(1))/(sqrt((x2(1)-x1(1))^2+(x2(2)-x1(2))^2))
	x3 (1)-x2(1))/(sqrt((x3(1)-x2(1))^2+(x3(2)-x2(2))^2))
	x4 (1)-x3(1))/(sqrt((x4(1)-x3(1))^2+(x4(2)-x3(2))^2))] sinAlpha
	x3 (2)-x2(2))/(sqrt((x3(1)-x2(1))^2+(x3(2)-x2(2))^2))
	figure (2)
	plot (xrot2(i), yrot2(i))
	plot (xrot3(i), yrot3(i))
	plot (xrot4(i), yrot4(i))
stejne jako v prikladu	konst ()
axes, cmd, par	coordtst ()
vykresleni splinu z naseho interpolatoru Coordmv	x0 (2)
	title ('Kubicky spline z Coordmv')
	plot (pos(:, 1)/konst, pos(:, 2)/konst)
Variables
clear	all
	clc
zadani uzlovych bodu	x0 = [0,0]'
	x1 = [3,3]'
	x2 = [1.5,2.25]'
	x3 = [1,2]'
	x4 = [5,0]'
	zoom = 1
vypocet tecnych vektoru v krajnich uzlovych bodech x0 a x4 je potreba pocitat s pocatecnim natocenim robota a smerem prijezdu do cile	v0 = [0,0.1]'
	v4 = [-0.1,0.1]'
vypocet tecnych vektoru ve vnitrnich uzlovych bodech x2 a x3	M = [ 4 1 0
	Q = [ [-3x0 + 3x2 - v0]'
	v
	v2 = v(2,:)'
	v3 = v(3,:)'
hold	on
	t = 0:0.005:1
polynom	X1 = A0(1)t.^3 + B0(1)t.^2 + C0(1)*t + D0(1)
	Y1 = A0(2)t.^3 + B0(2)t.^2 + C0(2)*t + D0(2)
polynom	X2 = A1(1)t.^3 + B1(1)t.^2 + C1(1)*t + D1(1)
	Y2 = A1(2)t.^3 + B1(2)t.^2 + C1(2)*t + D1(2)
polynom	X3 = A2(1)t.^3 + B2(1)t.^2 + C2(1)*t + D2(1)
	Y3 = A2(2)t.^3 + B2(2)t.^2 + C2(2)*t + D2(2)
polynom	X4 = A3(1)t.^3 + B3(1)t.^2 + C3(1)*t + D3(1)
	Y4 = A3(2)t.^3 + B3(2)t.^2 + C3(2)*t + D3(2)
for	i
end end pokus o vypocet deviace segmentu trajektorie	devi = zeros (4,2)
	cosAlpha = [ (x1(1)-x0(1)) / (sqrt((x1(1)-x0(1))^2 + (x1(2)-x0(2))^2))
vykresleni rotaci	xrot = '(Axt^3+Bxt^2+Cxt)cosAlpha-(Ayt^3+Byt^2+Cyt)sinAlpha'
	yrot = '(Axt^3+Bxt^2+Cxt)sinAlpha+(Ayt^3+Byt^2+Cyt)cosAlpha'
	xrot1 = subs(xrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A0(1), B0(1), C0(1), A0(2), B0(2), C0(2), sinAlpha(1), cosAlpha(1)})
	yrot1 = subs(yrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A0(1), B0(1), C0(1), A0(2), B0(2), C0(2), sinAlpha(1), cosAlpha(1)})
	xrot2 = subs(xrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A1(1), B1(1), C1(1), A1(2), B1(2), C1(2), sinAlpha(2), cosAlpha(2)})
	yrot2 = subs(yrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A1(1), B1(1), C1(1), A1(2), B1(2), C1(2), sinAlpha(2), cosAlpha(2)})
	xrot3 = subs(xrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A2(1), B2(1), C2(1), A2(2), B2(2), C2(2), sinAlpha(3), cosAlpha(3)})
	yrot3 = subs(yrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A2(1), B2(1), C2(1), A2(2), B2(2), C2(2), sinAlpha(3), cosAlpha(3)})
	xrot4 = subs(xrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A3(1), B3(1), C3(1), A3(2), B3(2), C3(2), sinAlpha(4), cosAlpha(4)})
	yrot4 = subs(yrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A3(1), B3(1), C3(1), A3(2), B3(2), C3(2), sinAlpha(4), cosAlpha(4)})
end obecny vzorecek	max1 = '(-1/27Ax(BxsinAlpha+BycosAlpha-(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))^3/(AxsinAlpha+AycosAlpha)^3+1/9Bx(BxsinAlpha+BycosAlpha-(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))^2/(AxsinAlpha+AycosAlpha)^2-1/3Cx(BxsinAlpha+BycosAlpha-(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))/(AxsinAlpha+AycosAlpha))sinAlpha+(-1/27Ay(BxsinAlpha+BycosAlpha-(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))^3/(AxsinAlpha+AycosAlpha)^3+1/9By(BxsinAlpha+BycosAlpha-(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))^2/(AxsinAlpha+AycosAlpha)^2-1/3Cy(BxsinAlpha+BycosAlpha-(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))/(AxsinAlpha+AycosAlpha))cosAlpha'
	max2 = '(-1/27Ax(BxsinAlpha+BycosAlpha+(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))^3/(AxsinAlpha+AycosAlpha)^3+1/9Bx(BxsinAlpha+BycosAlpha+(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))^2/(AxsinAlpha+AycosAlpha)^2-1/3Cx(BxsinAlpha+BycosAlpha+(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))/(AxsinAlpha+AycosAlpha))sinAlpha+(-1/27Ay(BxsinAlpha+BycosAlpha+(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))^3/(AxsinAlpha+AycosAlpha)^3+1/9By(BxsinAlpha+BycosAlpha+(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))^2/(AxsinAlpha+AycosAlpha)^2-1/3Cy(BxsinAlpha+BycosAlpha+(Bx^2sinAlpha^2+2BxsinAlphaBycosAlpha+By^2cosAlpha^2-3AxsinAlpha^2Cx-3AxsinAlphaCycosAlpha-3AycosAlphaCxsinAlpha-3AycosAlpha^2Cy)^(1/2))/(AxsinAlpha+AycosAlpha))cosAlpha'
uprava vzorecku pro konkretni segment	d
vykresleni splinu z naseho interpolatoru Coordmv	rx

Function Documentation

rx rx axis ( )

axes,cmd, par coordtst ( ) [virtual]

figure ( 2 )

1 figure ( ) [virtual]

stejne jako v prikladu konst ( ) [virtual]

plot	(	pos(:, 1)/	konst,
		pos(:, 2)/	konst
	)

plot	(	x1(1)	,
		x1(2)	,
		'rx'
	)

plot	(	xrot2(i)	,
		yrot2(i)
	)

plot	(	xrot3(i)	,
		yrot3(i)
	)

plot	(	xrot4(i)	,
		yrot4(i)
	)

plot	(	X2(i)	,
		Y2(i)
	)

plot	(	x0(1)	,
		x0(2)	,
		'rx'
	)

rx plot	(	x3(1)	,
		x3(2)	,
		'rx'
	)

plot	(	X3(i)	,
		Y3(i)
	)

plot	(	X4(i)	,
		Y4(i)
	)

title ( 'Kubicky spline z Matlabu' )

title ( 'Kubicky spline z Coordmv' )

id v1 ( ) [virtual]

vykresleni splinu z naseho interpolatoru Coordmv x0 ( 2 )

x2 ( 2 )

x2 ( 1 )

x3 ( 2 )

x3 ( 1 )

x4 ( 1 )

rx x4 ( 2 )

ylabel ( 'y' )

Variable Documentation

close all

clc

cosAlpha = [ (x1(1)-x0(1)) / (sqrt((x1(1)-x0(1))^2 + (x1(2)-x0(2))^2))

d

Initial value:

 subs(max1, {'Ax', 'Bx', 'Cx', 'Dx', 'Ay', 'By', 'Cy', 'Dy', 'sinAlpha', 'cosAlpha'}, {A0(1), B0(1), C0(1), D0(1), A0(2), B0(2), C0(2), D0(2), sinAlpha(1), cosAlpha(1)})
devi(1,1) = subs(d)

end end pokus o vypocet deviace segmentu trajektorie devi = zeros (4,2)

for i

Initial value:

1:size(t,2)-1
   plot(X1(i),Y1(i))

vypocet tecnych vektoru ve vnitrnich uzlovych bodech x2 a x3 M = [ 4 1 0

end obecny vzorecek max1 = '(-1/27*Ax*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*Bx*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cx*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*sinAlpha+(-1/27*Ay*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*By*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cy*(Bx*sinAlpha+By*cosAlpha-(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*cosAlpha'

max2 = '(-1/27*Ax*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*Bx*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cx*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*sinAlpha+(-1/27*Ay*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^3/(Ax*sinAlpha+Ay*cosAlpha)^3+1/9*By*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))^2/(Ax*sinAlpha+Ay*cosAlpha)^2-1/3*Cy*(Bx*sinAlpha+By*cosAlpha+(Bx^2*sinAlpha^2+2*Bx*sinAlpha*By*cosAlpha+By^2*cosAlpha^2-3*Ax*sinAlpha^2*Cx-3*Ax*sinAlpha*Cy*cosAlpha-3*Ay*cosAlpha*Cx*sinAlpha-3*Ay*cosAlpha^2*Cy)^(1/2))/(Ax*sinAlpha+Ay*cosAlpha))*cosAlpha'

hold on

Q = [ [-3*x0 + 3*x2 - v0]'

vykresleni splinu z naseho interpolatoru Coordmv rx

t = 0:0.005:1

v

Initial value:

 inv(M) * Q  


v1 = v(1,:)'

v0 = [0,0.1]'

v2 = v(2,:)'

v3 = v(3,:)'

v4 = [-0.1,0.1]'

zvetseni rastru x0 = [0,0]'

polynom X1 = A0(1)*t.^3 + B0(1)*t.^2 + C0(1)*t + D0(1)

vypocet tecnych vektoru ve vnitrnich uzlovych bodech x1 = [3,3]'

polynom X2 = A1(1)*t.^3 + B1(1)*t.^2 + C1(1)*t + D1(1)

x2 = [1.5,2.25]'

polynom X3 = A2(1)*t.^3 + B2(1)*t.^2 + C2(1)*t + D2(1)

x3 = [1,2]'

polynom X4 = A3(1)*t.^3 + B3(1)*t.^2 + C3(1)*t + D3(1)

x4 = [5,0]'

vykresleni rotaci xrot = '(Ax*t^3+Bx*t^2+Cx*t)*cosAlpha-(Ay*t^3+By*t^2+Cy*t)*sinAlpha'

xrot1 = subs(xrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A0(1), B0(1), C0(1), A0(2), B0(2), C0(2), sinAlpha(1), cosAlpha(1)})

xrot2 = subs(xrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A1(1), B1(1), C1(1), A1(2), B1(2), C1(2), sinAlpha(2), cosAlpha(2)})

xrot3 = subs(xrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A2(1), B2(1), C2(1), A2(2), B2(2), C2(2), sinAlpha(3), cosAlpha(3)})

xrot4 = subs(xrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A3(1), B3(1), C3(1), A3(2), B3(2), C3(2), sinAlpha(4), cosAlpha(4)})

Y1 = A0(2)*t.^3 + B0(2)*t.^2 + C0(2)*t + D0(2)

Y2 = A1(2)*t.^3 + B1(2)*t.^2 + C1(2)*t + D1(2)

Y3 = A2(2)*t.^3 + B2(2)*t.^2 + C2(2)*t + D2(2)

Y4 = A3(2)*t.^3 + B3(2)*t.^2 + C3(2)*t + D3(2)

yrot = '(Ax*t^3+Bx*t^2+Cx*t)*sinAlpha+(Ay*t^3+By*t^2+Cy*t)*cosAlpha'

yrot1 = subs(yrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A0(1), B0(1), C0(1), A0(2), B0(2), C0(2), sinAlpha(1), cosAlpha(1)})

yrot2 = subs(yrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A1(1), B1(1), C1(1), A1(2), B1(2), C1(2), sinAlpha(2), cosAlpha(2)})

yrot3 = subs(yrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A2(1), B2(1), C2(1), A2(2), B2(2), C2(2), sinAlpha(3), cosAlpha(3)})

yrot4 = subs(yrot, {'Ax', 'Bx', 'Cx', 'Ay', 'By', 'Cy', 'sinAlpha', 'cosAlpha'}, {A3(1), B3(1), C3(1), A3(2), B3(2), C3(2), sinAlpha(4), cosAlpha(4)})

zoom = 1

Priklad2.m File Reference

Functions

Variables

Function Documentation

Variable Documentation