在Frenet座標系下的動態障礙物避障

本文介紹如何實現基於Frenet座標系的動態障礙物避障。其中包括：

cubic spline generation
Frenet transformation to gloval coordnates
sampling-based search method
Bezier curve
cost function formalization
lazy collision checking
pure pursuit trajectory tracking

cubic soline generation
對於給定的一連串waypoints我們需要對其進行平滑線處理，這裏介紹一種三階spline的平滑方法：

function [Px,Py,YAW] = cubic_spline(x,y,MAX_ROAD_WIDTH)
   figure
%     plot(x,y,'ro');
    hold on
    
    N = length(x);
   A = zeros(N,N);
   B = zeros(N,1);

   for i = 1:N-1
   h(i) = x(i+1) - x(i);
   end
   
   A(1,1) = 1; 
   A(N,N) = 1;
   for i = 2:N-1
       A(i,i) = 2*(h(i-1) + h(i));
   end
   
   for i  =2 : N-1
   A(i, i+1) = h(i);
   end
   
   for i  = 2: N-1
       A(i,i-1) = h(i-1);
   end
   
   for i = 2:N-1
       B(i) = 6* (y(i+1)-y(i))/h(i) - 6* (y(i)-y(i-1))/h(i-1);
   end
   m= A\B
   
   for i = 1:N
       a(i) = y(i);
   end
   
     for i = 1:N
       c(i) = m(i)/2;
     end
   
      for i = 1:N-1
          d(i) =( c(i+1)-c(i) )/(3*h(i));
      end
      
       for i = 1:N-1
           b(i)  = (a(i+1)-a(i))/h(i)- h(i)/3*(c(i+1)+ 2*c(i));
       end
       Px= [];
       Py = [];
       for  i= 1:N-1
           X = x(i):0.1:x(i+1);
           Y = a(i)+ b(i)*(X-x(i)) + c(i) * (X- x(i)).^2 + d(i) * (X - x(i)).^3;
           Px = [Px,X];
           Py = [Py,Y];
       plot(X, Y,'g-','LineWidth',3)   
       plot(X, Y+MAX_ROAD_WIDTH,'r-','LineWidth',2)   
       plot(X, Y-MAX_ROAD_WIDTH,'r-','LineWidth',2)   
       end
       
       for i = 1: length(Px)-1
           yaw(i) = atan((Py(i+1)-Py(i))/(Px(i+1)- Px(i)));
       end
       yaw(end+1) = yaw(end);
        YAW =   yaw;
%        s = zeros(length(Px),1);
%        s(1) = 0;
%        for i = 2: length(Px)
%            s(i) = sqrt((Px(i)-Px(i-1)^2+ Py(i)-Py(i-1)^2);
%            s(i) =s(i-1) + 
%        end        
end

對於一串原始的reference line 參考線點集，我們可以通過三階曲線擬合將這些點擬合成連續光滑的三階曲線，這個曲線至少是二階C1連續的。

Frenet transformation to gloval coordnates
Frenet 座標系也稱道路座標系，或SL座標系。之所以使用這樣的座標系，是因爲我們在實施規劃曲線的時候是以道路爲前提的，也就是說，生成的曲線最後都是可以被表達成從當前車輛位置所在的參考線索引位置指向參考線前進方向的新的曲線，而這些新的曲線的參考線前進方向可以用參參考線積分所得的弧線長度表示，也就是S, 而法向，則可以用距離參考線的橫向距離來表示也就是L。一旦我們規劃的曲線能夠在道路座標系下表示出來，我們就可以通過向量計算獲得這個曲線在全局座標系下的表示。

可以看到，從Frenet座標系轉向全局座標系的向量表示由橫縱向兩部分組成。一個是參考線前進方向，r的方向。另一個就是橫向偏移的方向。也就是n法向。

function [waypoint] = frenet_to_global (cx, cy,YAW, location_ind,P) % waypoint generate a 2 by n matrix
waypoint = [];
for i  = 1 : length(location_ind)
    theta = YAW(location_ind(i)) + pi/2; 
    waypoint(1, i) = cx(location_ind(i)) + cos(theta) * P(i,2);
    waypoint(2, i) = cy(location_ind(i)) + sin(theta) * P(i,2);
end
% for i = 1: length(location_ind)-1
%     T = [cx(location_ind(i+1))-cx(location_ind(i)) ; cy(location_ind(i+1))-cy(location_ind(i))];
%     theta = atan(cy(location_ind(i+1))-cy(location_ind(i))/cx(location_ind(i+1))-cx(location_ind(i)));
%     R = [cos(theta),-sin(theta); sin(theta), cos(theta)];
%     trans = [R T; 0 0 1];
%     waypoint(:,i) = trans * [P(i,1); P(i,2);1];
%     
% end
end

函數frenet_to_global簡單的介紹了座標轉化的過程。轉化的原理很簡單，首先找到frenet座標系下面的點對應於reference line上面的點，再從這個點向n法向偏移對應的d的距離。theta就是計算出法向的偏移方向。而函數中的location_ind是一個座標序列號儲存矩陣，裏面存放着我們規劃出的曲線在對應的參考線上最近點的索引號。事實上，我們在規劃出連續的貝塞爾曲線後，使用參考線的採樣頻率對曲線離散化，獲得S方向的索引號，以及對應這些點的法向偏移量L。索引位置時，這些信息被存放在P中，P的第二列就是橫向偏移量。

    while  s<= Ti
    p(i,:)= Bezierfrenet(D0, Ti, Di,s);
    target_speed =  target_speed;
    location_ind(i) = S;
    s = s + sqrt((cx(S+1)-cx(S))^2 +  (cy(S+1)-cy(S))^2);
    S = S+1;
    i = i+1;
    end

另外一個問題，就是需要對規劃出來的曲線進行參考線方向的積分，這個也直接影響到我們在參考線方向積分的距離，這些存儲下來的點將被存放在P矩陣中。

sampling-based search method
samling-based search method結構上將位置和車速規劃進行了分離。位置上使用了基於貝塞爾曲線的採樣撒點，生成曲線，計算cost function, 進行碰撞檢測，最後搜索出最優安全曲線。車速上，使用了ACC控制器，在縱向發佈一個期望車速計算出期望加速度。

貝塞爾曲線
貝塞爾曲線的相關知識可以參考我以前的文章：
Bezier(貝塞爾)曲線的軌跡規劃在自動駕駛中的應用(一)
Bezier(貝塞爾)曲線的軌跡規劃在自動駕駛中的應用(二)
Bezier(貝塞爾)曲線的軌跡規劃在自動駕駛中的應用（三）
Bezier(貝塞爾)曲線(三階)的軌跡規劃在自動駕駛中的應用(四)
Bezier(貝塞爾)曲線(五階)的軌跡規劃在自動駕駛中的應用(五)
Bezier(貝塞爾)曲線(五階)的軌跡規劃在自動駕駛中的應用(六)
貝塞爾曲線的選擇，我們進行了對比：
自動駕駛使用貝塞爾曲線進行動態障礙物避障測試
 自動駕駛實測：貝塞爾曲線靜態障礙物避障

我們通過一個簡單的測試函數可以瞭解貝塞爾曲線：

clc
 clear all
 
 
  Ti = 10;
 Di = 4;
 figure
 i = 1;
 for t = 0:0.1: Ti
p(i,:) = Bezierfrenet(Ti, Di,t);

i = i+1;
 end
 
 figure
 
plot(p(:,1),p(:,2))
testa = Bezierfrenet(Ti, Di,10)
 function [p] = Bezierfrenet(Ti, Di,t)
 p0 = [ 0, 0];
 p1 = [Ti/2, 0];
 p2= [Ti/2, Di];
 p3 = [Ti, Di];
 %設置控制點
 
 p= (1-(t)/Ti)^3*p0 + 3*(1-(t)/Ti)^2*(t)/Ti*p1 + 3*(1-(t)/Ti)*((t)/Ti)^2*p2 + ((t)/Ti)^3*p3;

 end

2. 曲線採樣（sampling）
我們在Frenet 座標系下向S方向進行採樣。採樣方向的距離由短到長採樣：

MAXT = 10;% 最大預測 [s]
MINT = 4; % 最小預測 [s]

L方向也是一樣，從負最大到正最大：

MAX_ROAD_WIDTH =1.5 ; % 最大道路寬度 [m]

效果如圖：

cost function formalization
cost function 選擇有很多，具體看我論文：
A Dynamic Motion Planning Framework for Autonomous Driving in Urban Environments

而在本文中，我們只提供簡單的cost function fomalization:

% 損失函數權重
KJ = 0.1;
KT = 0.1;
KD = 10;
KLAT = 1.0;
KLON = 1.0;

function [frenet_paths] = calc_frenet_paths(c_d, c_d_d, c_d_dd,MAX_ROAD_WIDTH,...
    D_ROAD_W, MINT,DT,MAXT,KJ,KD,KT)
%    xs = c_d;
%    axs = c_d_dd;
%    axe = 0;
%    vxe = 0;
%    vxs = c_d_d;
% frenet_paths = [];
% tfp = [];
% T = [];
% D = [];
j = 1;
for di = -MAX_ROAD_WIDTH: D_ROAD_W: MAX_ROAD_WIDTH
    for Ti =  MINT: DT: MAXT
           t = 0: DT: Ti; t = t';
%         for i = 1: length(t)
%         p(i,:)= Bezierfrenet(Ti, Di,t(i));
%         end
  
%         for i = 1: length(t)
%             d_d(i) = calc_first_derivative(a1,a2,a3,a4,a5,t(i));
%         end
%         
%         for i = 1: length(t)
%             d_dd(i) = calc_second_derivative(a2,a3,a4,a5,t(i));
%         end
%         
%         for i = 1: length(t)
%             d_ddd(i) = calc_third_derivative(a3,a4,a5,t(i));
%         end
        
%         JP= sum(d_ddd.^2);
        
        tfp(j) = KT / Ti + KD * abs(di);
        T(j) = Ti;
        D(j) = di;
        j = j+1; 
    end
end

frenet_paths = [tfp',T',D'];
frenet_paths = sortrows(frenet_paths);
end

我們在函數中，對曲線進行了離散化，因爲cost function需要進行離散化來計算。舉個例子，這裏我只使用了曲線長度和曲線終點對參考線的偏移量兩個指標作爲cost function。第一個指標意思是我們更傾向於使用更長的預測距離，這是因爲更長的預測距離對遠方的障礙物更加敏感，同時穩定性也更高。第二個指標意思是我們傾向於規劃出來的軌跡儘量接近於參考線，也就是軌跡儘量是循參考線的跡。代碼塊中comment掉的一個另一個指標是Jerk,也就是加加速度，用於表徵車輛的燃油經濟性和駕駛舒適度，這個從曲線角度表示就是對曲線求三階導數，看三階導數的連續性。看得出來，計算出三階導數表達式後，對錶達式離散化取值，然後積分獲得這一項的cost function.

lazy collision checking
lazy collison checking 是延遲碰撞檢測，用於加速碰撞檢測，原理上就是碰撞檢測獨立於cost function, 在計算完cost function後對cost function 進行一次低到高的排序，從頭開始進行碰撞檢測，一直找到第一個滿足碰撞檢測的軌跡參數。具體的參考我的論文：A Dynamic Motion Planning Framework for Autonomous Driving in Urban Environments以及自動駕駛中的滯後碰撞檢測(lazy-collision-checking)

pure pursuit trajectory tracking
純跟蹤算法，用於解決軌跡跟蹤的問題，具體的原理參考Pure Pursuit trajectory tracking and Stanley trajectory tracking總結與比較，相關的測試已經給出：Pure Pursuit純跟蹤算法Python/Matlab算法實現，在本文的測試中，選取了以下部分進行純跟蹤的實現：

function [waypoint] = frenet_to_global (cx, cy,YAW, location_ind,P) % waypoint generate a 2 by n matrix
waypoint = [];
for i  = 1 : length(location_ind)
    theta = YAW(location_ind(i)) + pi/2; 
    waypoint(1, i) = cx(location_ind(i)) + cos(theta) * P(i,2);
    waypoint(2, i) = cy(location_ind(i)) + sin(theta) * P(i,2);
end
% for i = 1: length(location_ind)-1
%     T = [cx(location_ind(i+1))-cx(location_ind(i)) ; cy(location_ind(i+1))-cy(location_ind(i))];
%     theta = atan(cy(location_ind(i+1))-cy(location_ind(i))/cx(location_ind(i+1))-cx(location_ind(i)));
%     R = [cos(theta),-sin(theta); sin(theta), cos(theta)];
%     trans = [R T; 0 0 1];
%     waypoint(:,i) = trans * [P(i,1); P(i,2);1];
%     
% end
end

function [x, y, yaw, v] = update(x, y, yaw, v, a, delta,dt,L)
    x = x + v * cos(yaw) * dt;
    y = y + v * sin(yaw) * dt;
    yaw = yaw + v / L * tan(delta) * dt;
   v = v + a * dt;
end

function [a] = PIDcontrol(target_v, current_v, Kp,MAX_ACCEL)
a = Kp * (target_v - current_v);
a = min(max(a, -10), MAX_ACCEL);
end

function [ trans] =  BackTransfer(x,y,heading_current)

theta =  heading_current;
R = [cos(theta),-sin(theta); sin(theta), cos(theta)];
xt = x;
yt = y;
T = [xt; yt ] ;
trans = [[R,T];[0,0,1]];
end

function [delta] = pure_pursuit_control(x,y,yaw,v,path_x,path_y,k,Lfc,L,predict)
if predict >length(path_x)
    predict = length(path_x);
end

tx =path_x(predict);
    ty =path_y(predict);
    denom = tx-x;
    if denom == 0
        denom = 0.0001
    end
   alpha =atan(ty-y)/((denom))-yaw;
    
 Lf = k * v + Lfc;
 delta = atan(2*L * sin(alpha)/Lf)  ;
end

function [lat,current_ind]= calc_current_index(x,y, cx,cy)
N =  length(cx);
Distance = zeros(N,1);
for i = 1:N
Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
end
[value, location]= min(Distance);
current_ind = location;
lat = value;
 if cy(current_ind)>y
        lat = - lat;
    else
        lat = lat;
    end
end

function [angle] = pipi(angle)

if (angle > pi)
    angle =  angle - 2*pi;
elseif (angle < -pi)
    angle = angle + 2*pi;
else
    angle = angle;
end
end

最後通過主函數即可調用這些函數對車輛的狀態進行實時更新，同時根據實時更新的車輛狀態來規劃實時路徑。
主函數僞代碼入下：

while not reach end
	calc_frenet_paths
	calculate lateral_offset, current_ind
	optimal_trajectory = collision_checking_test(candidate_trajectory(frenet_paths))
	calculate acceleration % steering wheel angle
	update vehicle states
end while

最後效果：

在Frenet座標系下的動態障礙物避障

在Frenet座標系下的動態障礙物避障

Python的類示例

python實現牛頓法(newton's method)

Python-cvxopt庫的使用(2)(解決QP問題)

二次規劃問題轉換爲半正定問題(QPtoSDP)

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結