-
Notifications
You must be signed in to change notification settings - Fork 0
/
car_eight.m
133 lines (112 loc) · 2.72 KB
/
car_eight.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
%intialization
%car
% s is the parameter of curve % s_dot =constant(c)= 1 , s_doubleDot=zero
dt=0.001; % ds=c*dt => ds=dt
s_dot=1.0;
L = 2.6; % length of wheel base
delta_max=pi/6;
v_max=10.0;
Pose = [30.0; -5.0; pi/2]; % [x y theta delta]
poseDot = 0*Pose;
vel=[0.0; 0.0]; %[v w]
tspan=[0 0.001];
%controller
k=[10.0; 0.5; 1.0]; % Tuning Parameters [k_x k_y k_theta] %[150,0.5,1.5]
ref_vel= [0.0; 0.0]; %[v_ref w_ref]
ref_pose=[0.0; 0.0; 0.0]; %[x_ref y_ref theta_ref 0]
err_pose=[0.0; 0.0; 0.0]; %[x_err y_err theta_err 0]
%For plotting curve and errors
head_err=zeros;
track_err=zeros;
x=zeros;
y=zeros;
S=zeros;
pose_x=zeros;
pose_y=zeros;
v=zeros;
w=zeros;
f_x=zeros;
cross_track_error=zeros;
hold on
for s=0:s_dot*dt:2*pi
i=s*1000+1;
i=int32(i);
S(i)=s;
%path
% for lissajous x=a*cos(s) and y=b*sin(2s)
%v_ref= s_dot*sqrt(4*b^2+a^2-x^2-4*y^2)
%w_ref=(y_dot*x-4*x_dot*y)/(4*b^2+a^2-x^2-4*y^2)
x(i)= 30*cos(s);
y(i)= 50*sin(2*s);
x_dot=-30*sin(s)*s_dot;
y_dot=50*2*cos(2*s)*s_dot;
theta= atan2(y_dot,x_dot);
if theta<0
theta=theta+2*pi;
end
v_ref = s_dot*sqrt(4*50^2+30^2-x(i)^2-4*y(i)^2);
w_ref= (y_dot*x(i)-4*x_dot*y(i))/(4*50^2+30^2-x(i)^2-4*y(i)^2);
%controller
ref_pose(1,:) =x(i);
ref_pose(2,:) =y(i);
ref_pose(3,:) =theta;
ref_vel(1,:) = v_ref;
ref_vel(2,:) = w_ref;
T_e=[cos(Pose(3,:)) sin(Pose(3,:)) 0 %Transformation Matrix
-sin(Pose(3,:)) cos(Pose(3,:)) 0
0 0 1 ];
err_pose= T_e*(ref_pose-Pose);
%Control Law
vel(1,:)=ref_vel(1,:)*cos(err_pose(3,:)) + k(1,:)*err_pose(1,:);
vel(2,:)=ref_vel(2,:)+ ref_vel(1,:)*(k(2,:)*err_pose(2,:) + k(3,:)*sin(err_pose(3,:)));
% if(vel(1,:)<-v_max)
% vel(1,:)=-v_max;
% end
% if(vel(1,:)>v_max)
% vel(1,:)=v_max;
% end
if vel(2,:)>(vel(1,:)*tan(delta_max)/L)
vel(2,:)=vel(1,:)*tan(delta_max)/L;
end
if vel(2,:)<(-vel(1,:)*tan(delta_max)/L)
vel(2,:)=-vel(1,:)*tan(delta_max)/L;
end
%car
[t,m]=ode45(@(t,m) vel(1)*cos(Pose(3)), tspan,Pose(1));
Pose(1)=m(end);
[t,m]=ode45(@(t,m) vel(1)*sin(Pose(3)), tspan,Pose(2));
Pose(2)=m(end);
[t,m]=ode45(@(t,m) vel(2), tspan,Pose(3));
Pose(3)=m(end);
if Pose(3,:)>2*pi
Pose(3,:)=Pose(3,:)-2*pi;
end
%error
head_err(i)= Pose(3,:)- ref_pose(3,:);
if head_err(i) < -pi
head_err(i) = head_err(i) + 2 * pi;
else if head_err(i) > pi
head_err(i) = head_err(i) - 2 * pi;
end
end
track_err(i)= sqrt((Pose(1,:)-ref_pose(1,:))^2+(Pose(2,:)-ref_pose(2,:))^2);
pose_x1(i)=Pose(1,:);
pose_y1(i)=Pose(2,:);
v(i)=vel(1,:);
w(i)=vel(2,:);
% plot(pose_x(i),pose_y(i),'g*');
% drawnow;
% xlabel('x');
% ylabel('y');
% plot(x(i),y(i),'r*');
% drawnow;
n=0;
for j=s-0.05:0.001:s+0.05
X=30*cos(j);
Y= 50*sin(2*j);
n=n+1;
f_x(n)=sqrt((X-Pose(1,:))^2+(Y-Pose(2,:))^2);
end
cross_track_error(i)=min(f_x);
end
hold off