-
Notifications
You must be signed in to change notification settings - Fork 94
/
veloSmooth.jl
111 lines (92 loc) · 3.38 KB
/
veloSmooth.jl
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
###############
# H-OBCA: Hierarchical Optimization-based Collision Avoidance - a path planner for autonomous parking
# Copyright (C) 2018
# Alexander LINIGER [[email protected]; Automatic Control Lab, ETH Zurich]
# Xiaojing ZHANG [[email protected]; MPC Lab, UC Berkeley]
# Atsushi SAKAI [[email protected]; Komatsu Ltd / MPC Lab]
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program 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 General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
###############
# The paper describing the theory can be found here:
# X. Zhang, A. Liniger and F. Borrelli; "Optimization-Based Collision Avoidance"; Technical Report, 2017, [https://arxiv.org/abs/1711.03449]
# X. Zhang, A. Liniger, A. Sakai and F. Borrelli; "Autonomous Parking using Optimization-Based Collision Avoidance"; Technical Report, 2018 [add URL]
###############
###############
# veloSmooth: a velocity smoother
###############
function veloSmooth(v,amax,Ts)
v_ex = zeros(length(v)+40,1)
v_bar = zeros(4,length(v)+40)
v_bar2 = zeros(4,length(v)+40)
v_barMM = zeros(1,length(v))
for i = 1:length(v)
for j = 1:4
v_bar[j,i+19] = v[i];
v_ex[i+19] = v[i];
end
end
v_cut1 = 0.25*abs(v[1])
v_cut2 = 0.25*abs(v[1])+abs(v[1])
accPhase = Int(round(abs(v[1])/amax/Ts))
index1 = find((diff(v_ex).>v_cut1) & (diff(v_ex).<v_cut2))
index2 = find(diff(v_ex).>v_cut2)
index3 = find((diff(v_ex).<-v_cut1) & (diff(v_ex).>-v_cut2))
index4 = find(diff(v_ex).<-v_cut2)
if length(index1) >=1 && index1[1]==19
index1[1] = index1[1]+1
end
if length(index3) >=1 && index3[1]==19
index3[1] = index3[1]+1
end
for j = 1:length(index1)
if v_ex[index1[j]] > v_cut1 || v_ex[index1[j]+1] > v_cut1
v_bar[1,index1[j]:index1[j]+accPhase] = linspace(0,abs(v[1]),accPhase+1)''
elseif v_ex[index1[j]] < -v_cut1 || v_ex[index1[j]+1] < -v_cut1
v_bar[1,index1[j]-accPhase+1:index1[j]+1] = linspace(-abs(v[1]),0,accPhase+1)''
end
end
for j = 1:length(index3)
if v_ex[index3[j]] > v_cut1 || v_ex[index3[j]+1] > v_cut1
v_bar[2,index3[j]-accPhase+1:index3[j]+1] = linspace(abs(v[1]),0,accPhase+1)''
elseif v_ex[index3[j]] < -v_cut1 || v_ex[index3[j]+1] < -v_cut1
v_bar[2,index3[j]:index3[j]+accPhase] = linspace(0,-abs(v[1]),accPhase+1)''
end
end
for j = 1:length(index2)
v_bar[3,index2[j]-accPhase:index2[j]+accPhase] = linspace(-abs(v[1]),abs(v[1]),2*accPhase+1)''
end
for j = 1:length(index4)
v_bar[4,index4[j]-accPhase:index4[j]+accPhase] = linspace(abs(v[1]),-abs(v[1]),2*accPhase+1)''
end
for i = 20:length(v)+19
for j = 1:4
if v_bar[j,i] == 0
v_bar2[j,i] = v_bar[j,i]
elseif sign(v_ex[i]) != sign(v_bar[j,i])
v_bar2[j,i] = v_ex[i]
else
v_bar2[j,i] = v_bar[j,i]
end
end
end
for i = 20:length(v)+19
if v_ex[i] > 0
v_barMM[i-19] = minimum(v_bar2[:,i])
else
v_barMM[i-19] = maximum(v_bar2[:,i])
end
end
a = diff(v_barMM')./Ts
return v_barMM', a
end