/
Unicycle.m
261 lines (228 loc) · 8.44 KB
/
Unicycle.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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
%Unicycle vehicle class
%
% This concrete class models the kinematics of a differential steer vehicle
% (unicycle model) on a plane. For given steering and velocity inputs it
% updates the true vehicle state and returns noise-corrupted odometry
% readings.
%
% Methods::
% init initialize vehicle state
% f predict next state based on odometry
% step move one time step and return noisy odometry
% control generate the control inputs for the vehicle
% update update the vehicle state
% run run for multiple time steps
% Fx Jacobian of f wrt x
% Fv Jacobian of f wrt odometry noise
% gstep like step() but displays vehicle
% plot plot/animate vehicle on current figure
% plot_xy plot the true path of the vehicle
% add_driver attach a driver object to this vehicle
% display display state/parameters in human readable form
% char convert to string
%
% Class methods::
% plotv plot/animate a pose on current figure
%
% Properties (read/write)::
% x true vehicle state: x, y, theta (3x1)
% V odometry covariance (2x2)
% odometry distance moved in the last interval (2x1)
% rdim dimension of the robot (for drawing)
% L length of the vehicle (wheelbase)
% alphalim steering wheel limit
% maxspeed maximum vehicle speed
% T sample interval
% verbose verbosity
% x_hist history of true vehicle state (Nx3)
% driver reference to the driver object
% x0 initial state, restored on init()
%
% Examples::
%
% Odometry covariance (per timstep) is
% V = diag([0.02, 0.5*pi/180].^2);
% Create a vehicle with this noisy odometry
% v = Unicycle( 'covar', diag([0.1 0.01].^2) );
% and display its initial state
% v
% now apply a speed (0.2m/s) and steer angle (0.1rad) for 1 time step
% odo = v.step(0.2, 0.1)
% where odo is the noisy odometry estimate, and the new true vehicle state
% v
%
% We can add a driver object
% v.add_driver( RandomPath(10) )
% which will move the vehicle within the region -10<x<10, -10<y<10 which we
% can see by
% v.run(1000)
% which shows an animation of the vehicle moving for 1000 time steps
% between randomly selected wayoints.
%
% Notes::
% - Subclasses the MATLAB handle class which means that pass by reference semantics
% apply.
%
% Reference::
%
% Robotics, Vision & Control, Chap 6
% Peter Corke,
% Springer 2011
%
% See also RandomPath, EKF.
% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
classdef Unicycle < Vehicle
properties
% state
W % wheel separation
accelmax
vprev
steerprev
end
methods
function veh = Unicycle(varargin)
%Unicycle.Unicycle Unicycle object constructor
%
% V = Unicycle(VA, OPTIONS) creates a Unicycle object with actual odometry
% covariance VA (2x2) matrix corresponding to the odometry vector [dx dtheta].
%
% Options::
% 'W',W Wheel separation [m] (default 1)
% 'vmax',S Maximum speed (default 5m/s)
% 'x0',x0 Initial state (default (0,0,0) )
% 'dt',T Time interval
% 'rdim',R Robot size as fraction of plot window (default 0.2)
% 'verbose' Be verbose
%
% Notes::
% - Subclasses the MATLAB handle class which means that pass by reference semantics
% apply.
veh = veh@Vehicle(varargin{:});
opt.W = 1;
opt.accelmax = Inf;
veh = tb_optparse(opt, veh.options, veh);
veh.vprev = 0;
veh.x = veh.x0;
end
function xnext = f(veh, x, odo, w)
%Unicycle.f Predict next state based on odometry
%
% XN = V.f(X, ODO) is the predicted next state XN (1x3) based on current
% state X (1x3) and odometry ODO (1x2) = [distance, heading_change].
%
% XN = V.f(X, ODO, W) as above but with odometry noise W.
%
% Notes::
% - Supports vectorized operation where X and XN (Nx3).
if nargin < 4
w = [0 0];
end
dd = odo(1) + w(1); dth = odo(2) + w(2);
% straightforward code:
% thp = x(3) + dth;
% xnext = zeros(1,3);
% xnext(1) = x(1) + (dd + w(1))*cos(thp);
% xnext(2) = x(2) + (dd + w(1))*sin(thp);
% xnext(3) = x(3) + dth + w(2);
%
% vectorized code:
thp = x(:,3) + dth;
xnext = x + [dd*cos(thp) dd*sin(thp) ones(size(x,1),1)*dth];
end
function dx = deriv(veh, t, x, u)
% to be called from a continuous time integrator such as ode45 or Simulink
% implement acceleration limit if required
if ~isinf(veh.accelmax)
if (u(1) - veh.vprev)/veh.dt > veh.accelmax
u(1) = veh.vprev + veh.accelmax * veh.dt;
elseif (u(1) - veh.vprev)/veh.dt < -veh.accelmax
u(1) = veh.vprev - veh.accelmax * veh.dt;
end
veh.vprev = u(1);
end
% implement speed and steer angle limits
u(1) = min(veh.speedmax, max(u(1), -veh.speedmax));
% compute the derivative
dx = zeros(3,1);
dx(1) = u(1)*cos(x(3));
dx(2) = u(1)*sin(x(3));
dx(3) = u(2)/veh.W;
end
function odo = update(veh, u)
%Unicycle.update Update the vehicle state
%
% ODO = V.update(U) is the true odometry value for
% motion with U=[speed,steer].
%
% Notes::
% - Appends new state to state history property x_hist.
% - Odometry is also saved as property odometry.
% update the state
dx = veh.dt * veh.deriv([], veh.x, u);
veh.x = veh.x + dx;
% compute and save the odometry
odo = [ norm(dx(1:2)) dx(3) ];
veh.odometry = odo;
veh.x_hist = [veh.x_hist; veh.x']; % maintain history
end
function J = Fx(veh, x, odo)
%Unicycle.Fx Jacobian df/dx
%
% J = V.Fx(X, ODO) is the Jacobian df/dx (3x3) at the state X, for
% odometry input ODO (1x2) = [distance, heading_change].
%
% See also Unicycle.f, Vehicle.Fv.
dd = odo(1); dth = odo(2);
thp = x(3) + dth;
J = [
1 0 -dd*sin(thp)
0 1 dd*cos(thp)
0 0 1
];
end
function J = Fv(veh, x, odo)
%Unicycle.Fv Jacobian df/dv
%
% J = V.Fv(X, ODO) is the Jacobian df/dv (3x2) at the state X, for
% odometry input ODO (1x2) = [distance, heading_change].
%
% See also Unicycle.F, Vehicle.Fx.
dd = odo(1); dth = odo(2);
thp = x(3);
J = [
cos(thp) 0 %-dd*sin(thp)
sin(thp) 0 %dd*cos(thp)
0 1
];
end
function s = char(veh)
%Unicycle.char Convert to a string
%
% s = V.char() is a string showing vehicle parameters and state in
% a compact human readable format.
%
% See also Unicycle.display.
ss = char@Vehicle(veh);
s = 'Unicycle object';
s = char(s, sprintf(' W=%g', veh.W));
s = char(s, ss);
end
end % method
end % classdef