DWA動態窗口法的原理及應用

看了CSDN博客上面的各種介紹DWA的博客，就這辣雞點擊都能過萬？完全是對學術的不尊重吧，還是我來寫一下吧。

DWA算法的核心：
DWA的核心在於所謂的動態窗口，這個東西在sampling based method 中就是sampling. 對於sampling可以是去sample state，也可以sample action. 百度的lattice planner其實就是在sample state, 把未來的軌跡candidate一個個sample出來做cost minimization. 而state lattice method其實是在sample action，對於機器人來說就是sample 不同的角度速度和直線速度，預測出未來軌跡，然後做cost function minimization. 但是這兩者區別是，雖然最後都變成了軌跡，但上一個方法時直接生成軌跡，而第二種方案是用動作生成軌跡，所以第二種方法的控制器輸出會是直接角速度和直線加速度，而對於第一個方法先生成軌跡的，控制器要做的工作是軌跡跟蹤。 DWA這個方案就是第二種方法中的典型。

車輛運動學：
原文中就是這麼寫的，真實的機器人推算也可以按照這個方法做：

對於汽車，可以重新寫一下：

在代碼中我們把狀態估計寫成矩陣的形式：

%% Motion Model 根據當前狀態推算下一個控制週期（dt）的狀態
% u = [vt; wt];當前時刻的速度、角速度 x = 狀態[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
function x = f(x, u)
global dt;
F = [1 0 0 0 0
     0 1 0 0 0
     0 0 1 0 0
     0 0 0 0 0
     0 0 0 0 0];
 
B = [dt*cos(x(3)) 0
    dt*sin(x(3)) 0
    0 dt
    1 0
    0 1];
 
x= F*x+B*u;

動態窗口：
動態窗口就是在當前的狀態下，在本狀態周圍進行採樣，獲得下一時刻的合理的動作變化，這個動作僅包括角速度和線速度。在這個方法中，我們把軌跡認爲是一系列不同動作組合而產生的在delta_t內生成的軌跡。我們要做的就是設定一個評價標準，選出我們認爲的最合理的動作（也就意味着最合理的軌跡）。

對於具體的搜索，原文是這麼說的：

首先軌跡當成曲線curvature，然後選取合理的速度組合，然後生成動態窗口。

那麼什麼是合理的速度？有三個限制。第一：按這個速度行駛，最大減速度停車都不能撞到最近的障礙物：

第二個：加速度限制，當前狀態下，加減速變化都是有上下限的，因此下一時刻的狀態也是有上下限的：

最後：任何情況下都要遵守車輛本身的動力學限制，意思就是不管怎麼變化，角速度和線速度是有上下限標準的。就如圖下面框出來的Vs區域。

那麼最後動態窗口就是上面三個限制條件的交集：

代價函數（cost function）
通常來講我們用cost function來表徵不用選擇的代價，要做的事情就是cost function minimization. DWA採用了expectation maximization,其實也是一個意思。只是做法和叫法不同。
原文中採用了三個因素的組合：

很好理解：
1，我希望我的前進方向對準終點
2，我希望不發生任何碰撞
3，我希望速度儘量快

除此之外，還要加上一個：
4，保證上文提到的最短剎車距離是安全的

原文還特地解釋了說第一個因素是效率問題，如果太大可能就落到局部最優解了，如果太小，雖然可以讓你更好的在障礙物之間遊走，但是可能效率有點低。剩下兩個就是保證你行駛安全加快速。

值的注意的一點是collison checking, 原文中使用的是軟約束的方法，並不對發生碰撞做嚴格禁止，只是說這一項就沒什麼得分了。但是要注意，如果不小心設計者選擇了一個不太好的weight, 極有可能最後最優路徑還是選到了發生碰撞的路徑上，因爲另外兩項可能得分很高。直接抵消了第三項的0分。

對於cost function, 我先前就寫過一個更詳細的：
自動駕駛路徑規劃DWA算法原理解析
這個裏面直接列出了7項cost function:

這裏，我們介紹的時候還是按照最簡單的來做，代碼實現也僅包含上述的三項基礎部分。具體的計算一看代碼就知道了：

%% 計算制動距離 
%根據運動學模型計算制動距離, 也可以考慮成走一段段圓弧的累積 簡化可以當一段段小直線的累積
function stopDist = CalcBreakingDist(vel,model)
global dt;
MD_ACC   = 3;% 
stopDist=0;
while vel>0   %給定加速度的條件下 速度減到0所走的距離
    stopDist = stopDist + vel*dt;% 制動距離的計算 
    vel = vel - model(MD_ACC)*dt;% 
end

%% 障礙物距離評價函數  （機器人在當前軌跡上與最近的障礙物之間的距離，如果沒有障礙物則設定一個常數）
% 輸入參數：位姿、所有障礙物位置、障礙物半徑
% 輸出參數：當前預測的軌跡終點的位姿距離所有障礙物中最近的障礙物的距離 如果大於設定的最大值則等於最大值
% 距離障礙物距離越近分數越低
%我們用flag表示是否發生碰撞，如果發生，則這條路徑直接不考慮
function [dist,Flag] = CalcDistEval(x,ob,R)
dist=100;
for io = 1:length(ob(:,1))  
    disttmp = norm(ob(io,:)-x(1:2)')-R; %到第io個障礙物的距離 - 障礙物半徑  ！！！有可能出現負值嗎？？
    if disttmp <0
        Flag = 1;
        break;
    else
        Flag = 0;
    end
    
    if dist > disttmp   % 大於最小值 則選擇最小值
        dist = disttmp;
    end
end
 
% 障礙物距離評價限定一個最大值，如果不設定，一旦一條軌跡沒有障礙物，將太佔比重
if dist >= 3*R %最大分數限制
    dist = 3*R;
end
 
%% heading的評價函數計算
% 輸入參數：當前位置、目標位置
% 輸出參數：航向參數得分  當前車的航向和相對於目標點的航向 偏離程度越小 分數越高 最大180分
function heading = CalcHeadingEval(x,goal)
theta = toDegree(x(3));% 機器人朝向
goalTheta = toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));% 目標點相對於機器人本身的方位 
if goalTheta > theta
    targetTheta = goalTheta-theta;% [deg]
else
    targetTheta = theta-goalTheta;% [deg]
end
 
heading = 180 - targetTheta;

其實仿真中我發現，這個第一個weight的選擇還是有說法的。不同的選擇結果不太一樣：
weight_headiing = 2weight_headiing =0.02

正則化
對於這幾個單位不同的因素，我們必須進行正則化，把這些量變成無量綱量(百分比)，這樣纔會讓比較變得有意義：

%% 歸一化處理 
% 每一條軌跡的單項得分除以本項所有分數和
function EvalDB=NormalizeEval(EvalDB)
% 評價函數正則化
if sum(EvalDB(:,3))~= 0
    EvalDB(:,3) = EvalDB(:,3)/sum(EvalDB(:,3));  %矩陣的數除  單列矩陣的每元素分別除以本列所有數據的和
end
if sum(EvalDB(:,4))~= 0
    EvalDB(:,4) = EvalDB(:,4)/sum(EvalDB(:,4));
end
if sum(EvalDB(:,5))~= 0
    EvalDB(:,5) = EvalDB(:,5)/sum(EvalDB(:,5));
end

值的注意的幾個細節：
1，當前速度的影響：

在這種情況下，我們需要穿過door，但如果當前速度過高，最後的最優軌跡將會是向前方直行，而不是右轉，因爲角速度最高限制的原因，無法規劃出一條右轉通過door的曲線。由下圖的expectation function 就可直觀看出：

所以，爲了應對這種情況，比較好的一種辦法就是修正最高的速度限制，保證在這附近機器人速度不會太高。

同樣的情況，對於機器人的加速度也是一樣的：

由於小加速度，導致我們右轉的行爲expectation不高，結果車子也不會向右轉。而是選擇expectation更高的直行行爲。

最後貼出我修改過的matlab 代碼：

% 直接命令行運行
dwa_V_1_0

以下爲main function

% -------------------------------------------------------------------------
%
% File : DWA
%
% Discription : Mobile Robot Motion Planning with Dynamic Window Approach
%
% Environment : Matlab
%
% Author :Yuncheng Jiang
%
%
% License : Modified BSD Software License Agreement
% log: collision checking 修改爲硬約束
% -------------------------------------------------------------------------
 
function [] = dwa_V_1_0()

close all;
clear all;

disp('Dynamic Window Approach sample program start!!')

%% 機器人的初期狀態[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
% x=[0 0 pi/2 0 0]'; % 5x1矩陣 列矩陣  位置 0，0 航向 pi/2 ,速度、角速度均爲0
x = [0 0 pi/10 0 0]'; 

% 下標宏定義 狀態[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
POSE_X      = 1;  %座標 X
POSE_Y      = 2;  %座標 Y
YAW_ANGLE   = 3;  %機器人航向角
V_SPD       = 4;  %機器人速度
W_ANGLE_SPD = 5;  %機器人角速度 

goal = [10,10];   % 目標點位置 [x(m),y(m)]

% 障礙物位置列表 [x(m) y(m)]

obstacle=[0 2;
          2 4;
          2 5;      
          4 2;
%           4 4;
          5 4;
%            5 5;
          5 6;
          5 9
          8 8
          8 9
          7 9];
% obstacle=[0 2;
%           4 2;
%           4 4;
%           5 4;
%           5 5;
%           5 6;
%           5 9
%           8 8
%           8 9
%           7 9
%           6 5
%           6 3
%           6 8
%           6 7
%           7 4
%           9 8
%           9 11
%           9 6];

 for i =-1
    for j = -1:12
        obstacle = [obstacle; [i,j]];
    end
 end     
for i =12
    for j = -1:12
        obstacle = [obstacle; [i,j]];
    end
end 
for j =-2
    for i = -1:12
        obstacle = [obstacle; [i,j]];
    end
end 
for j=13
    for i= -1:12
        obstacle = [obstacle; [i,j]];
    end
end 
 
obstacleR = 0.5;% 衝突判定用的障礙物半徑
global dt; 
dt = 0.1;% 時間[s]

% 機器人運動學模型參數
% 最高速度m/s],最高旋轉速度[rad/s],加速度[m/ss],旋轉加速度[rad/ss],
% 速度分辨率[m/s],轉速分辨率[rad/s]]
Kinematic = [1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];
%定義Kinematic的下標含義
MD_MAX_V    = 1;%   最高速度m/s]
MD_MAX_W    = 2;%   最高旋轉速度[rad/s]
MD_ACC      = 3;%   加速度[m/ss]
MD_VW       = 4;%   旋轉加速度[rad/ss]
MD_V_RESOLUTION  = 5;%  速度分辨率[m/s]
MD_W_RESOLUTION  = 6;%  轉速分辨率[rad/s]]


% 評價函數參數 [heading,dist,velocity,predictDT]
% 航向得分的比重、距離得分的比重、速度得分的比重、向前模擬軌跡的時間
evalParam = [0.08, 0.1 ,0.1, 3.0];
% evalParam = [2, 0.2 ,0.2, 3.0];
area      = [-3 14 -3 14];% 模擬區域範圍 [xmin xmax ymin ymax]

% 模擬實驗的結果
result.x=[];   %累積存儲走過的軌跡點的狀態值 
tic; % 估算程序運行時間開始

% movcount=0;
%% Main loop   循環運行 5000次 指導達到目的地 或者 5000次運行結束
for i = 1:5000  
    % DWA參數輸入 返回控制量 u = [v(m/s),w(rad/s)] 和 軌跡
    [u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
    x = f(x,u);% 機器人移動到下一個時刻的狀態量 根據當前速度和角速度推導 下一刻的位置和角度
    
    % 歷史軌跡的保存
    result.x = [result.x; x'];  %最新結果 以列的形式 添加到result.x
    
    % 是否到達目的地
    if norm(x(POSE_X:POSE_Y)-goal')<0.25   % norm函數來求得座標上的兩個點之間的距離
        disp('==========Arrive Goal!!==========');break;
    end
    
    %====Animation====
    hold off;               % 關閉圖形保持功能。 新圖出現時，取消原圖的顯示。
    ArrowLength = 0.5;      % 箭頭長度
    
    % 機器人
    % quiver(x,y,u,v) 在 x 和 y 中每個對應元素對組所指定的座標處將向量繪製爲箭頭
    quiver(x(POSE_X), x(POSE_Y), ArrowLength*cos(x(YAW_ANGLE)), ArrowLength*sin(x(YAW_ANGLE)), 'ok'); 
    % 繪製機器人當前位置的航向箭頭
    hold on;                                                     
    %啓動圖形保持功能，當前座標軸和圖形都將保持，從此繪製的圖形都將添加在這個圖形的基礎上，並自動調整座標軸的範圍
    
    plot(result.x(:,POSE_X),result.x(:,POSE_Y),'-b');hold on;    % 繪製走過的所有位置 所有歷史數據的 X、Y座標
    plot(goal(1),goal(2),'*r');hold on;                          % 繪製目標位置
    
    %plot(obstacle(:,1),obstacle(:,2),'*k');hold on;              % 繪製所有障礙物位置
    DrawObstacle_plot(obstacle,obstacleR);
    
    % 探索軌跡 畫出待評價的軌跡
    if ~isempty(traj) %軌跡非空
        for it=1:length(traj(:,1))/5    %計算所有軌跡數  traj 每5行數據 表示一條軌跡點
            ind = 1+(it-1)*5; %第 it 條軌跡對應在traj中的下標 
            plot(traj(ind,:),traj(ind+1,:),'-g');hold on;  %根據一條軌跡的點串畫出軌跡   traj(ind,:) 表示第ind條軌跡的所有x座標值  traj(ind+1,:)表示第ind條軌跡的所有y座標值
        end
    end
    
    axis(area); %根據area設置當前圖形的座標範圍，分別爲x軸的最小、最大值，y軸的最小最大值
    grid on;
    drawnow limitrate;  %刷新屏幕. 當代碼執行時間長，需要反覆執行plot時，Matlab程序不會馬上把圖像畫到figure上，這時，要想實時看到圖像的每一步變化情況，需要使用這個語句。
    %movcount = movcount+1;
    %mov(movcount) = getframe(gcf);%  記錄動畫幀
end
toc  %輸出程序運行時間  形式：時間已過 ** 秒。
%movie2avi(mov,'movie.avi');  %錄製過程動畫 保存爲 movie.avi 文件

%% 繪製所有障礙物位置
% 輸入參數：obstacle 所有障礙物的座標   obstacleR 障礙物的半徑
function [] = DrawObstacle_plot(obstacle,obstacleR)
r = obstacleR; 
theta = 0:pi/20:2*pi;
for id=1:length(obstacle(:,1))
 x = r * cos(theta) + obstacle(id,1); 
 y = r  *sin(theta) + obstacle(id,2);
 plot(x,y,'-m');hold on; 
end
% plot(obstacle(:,1),obstacle(:,2),'*m');hold on;              % 繪製所有障礙物位置

%% DWA算法實現 
% model  機器人運動學模型  最高速度m/s],最高旋轉速度[rad/s],加速度[m/ss],旋轉加速度[rad/ss], 速度分辨率[m/s],轉速分辨率[rad/s]]
% 輸入參數：當前狀態、模型參數、目標點、評價函數的參數、障礙物位置、障礙物半徑
% 返回參數：控制量 u = [v(m/s),w(rad/s)] 和 軌跡集合 N * 31  （N：可用的軌跡數）
% 選取最優參數的物理意義：在局部導航過程中，使得機器人避開障礙物，朝着目標以較快的速度行駛。
function [u,trajDB] = DynamicWindowApproach(x,model,goal,evalParam,ob,R)
% Dynamic Window [vmin,vmax,wmin,wmax] 最小速度 最大速度 最小角速度 最大角速度速度
Vr = CalcDynamicWindow(x,model);  % 根據當前狀態 和 運動模型 計算當前的參數允許範圍

% 評價函數的計算 evalDB N*5  每行一組可用參數 分別爲 速度、角速度、航向得分、距離得分、速度得分
%               trajDB      每5行一條軌跡 每條軌跡都有狀態x點串組成
[evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam);  %evalParam 評價函數參數 [heading,dist,velocity,predictDT]

if isempty(evalDB)
    disp('no path to goal!!');
    u=[0;0];return;
end

% 各評價函數正則化
evalDB = NormalizeEval(evalDB);

% 最終評價函數的計算
feval=[];
for id=1:length(evalDB(:,1))
    feval = [feval;evalParam(1:3)*evalDB(id,3:5)']; %根據評價函數參數 前三個參數分配的權重 計算每一組可用的路徑參數信息的得分
end
evalDB = [evalDB feval]; % 最後一組
 
[maxv,ind] = max(feval);% 選取評分最高的參數 對應分數返回給 maxv  對應下標返回給 ind
u = evalDB(ind,1:2)';% 返回最優參數的速度、角速度  

%% 評價函數 內部負責產生可用軌跡
% 輸入參數 ：當前狀態、參數允許範圍（窗口）、目標點、障礙物位置、障礙物半徑、評價函數的參數
% 返回參數：
%           evalDB N*5  每行一組可用參數 分別爲 速度、角速度、航向得分、距離得分、速度得分
%           trajDB      每5行一條軌跡 每條軌跡包含 前向預測時間/dt + 1 = 31 個軌跡點（見生成軌跡函數）
function [evalDB,trajDB] = Evaluation(x,Vr,goal,ob,R,model,evalParam)
evalDB = [];
trajDB = [];
for vt = Vr(1):model(5):Vr(2)       %根據速度分辨率遍歷所有可用速度： 最小速度和最大速度 之間 速度分辨率 遞增 
    for ot=Vr(3):model(6):Vr(4)     %根據角度分辨率遍歷所有可用角速度： 最小角速度和最大角速度 之間 角度分辨率 遞增  
        % 軌跡推測; 得到 xt: 機器人向前運動後的預測位姿; traj: 當前時刻 到 預測時刻之間的軌跡（由軌跡點組成）
        [xt,traj] = GenerateTrajectory(x,vt,ot,evalParam(4));  %evalParam(4),前向模擬時間;
        % 各評價函數的計算
        heading = CalcHeadingEval(xt,goal); % 前項預測終點的航向得分  偏差越小分數越高
        [dist,Flag] = CalcDistEval(xt,ob,R);    % 前項預測終點 距離最近障礙物的間隙得分 距離越遠分數越高
        vel     = abs(vt);                  % 速度得分 速度越快分越高
        stopDist = CalcBreakingDist(vel,model); % 制動距離的計算
        if dist > stopDist && Flag == 0 % 如果可能撞到最近的障礙物 則捨棄此路徑 （到最近障礙物的距離 大於 剎車距離 才取用）
            evalDB = [evalDB;[vt ot heading dist vel]];
            trajDB = [trajDB;traj];   % 每5行 一條軌跡  
        end
    end
end

%% 歸一化處理 
% 每一條軌跡的單項得分除以本項所有分數和
function EvalDB=NormalizeEval(EvalDB)
% 評價函數正則化
if sum(EvalDB(:,3))~= 0
    EvalDB(:,3) = EvalDB(:,3)/sum(EvalDB(:,3));  %矩陣的數除  單列矩陣的每元素分別除以本列所有數據的和
end
if sum(EvalDB(:,4))~= 0
    EvalDB(:,4) = EvalDB(:,4)/sum(EvalDB(:,4));
end
if sum(EvalDB(:,5))~= 0
    EvalDB(:,5) = EvalDB(:,5)/sum(EvalDB(:,5));
end

%% 單條軌跡生成、軌跡推演函數
% 輸入參數： 當前狀態、vt當前速度、ot角速度、evaldt 前向模擬時間、機器人模型參數（沒用到）
% 返回參數; 
%           x   : 機器人模擬時間內向前運動 預測的終點位姿(狀態); 
%           traj: 當前時刻 到 預測時刻之間 過程中的位姿記錄（狀態記錄） 當前模擬的軌跡  
%                  軌跡點的個數爲 evaldt / dt + 1 = 3.0 / 0.1 + 1 = 31
%           
function [x,traj] = GenerateTrajectory(x,vt,ot,evaldt)
global dt;
time = 0;
u = [vt;ot];% 輸入值
traj = x;   % 機器人軌跡
while time <= evaldt   
    time = time+dt; % 時間更新
    x = f(x,u);     % 運動更新 前項模擬時間內 速度、角速度恆定
    traj = [traj x]; % 每一列代表一個軌跡點 一列一列的添加
end

%% 計算制動距離 
%根據運動學模型計算制動距離, 也可以考慮成走一段段圓弧的累積 簡化可以當一段段小直線的累積
function stopDist = CalcBreakingDist(vel,model)
global dt;
MD_ACC   = 3;% 
stopDist=0;
while vel>0   %給定加速度的條件下 速度減到0所走的距離
    stopDist = stopDist + vel*dt;% 制動距離的計算 
    vel = vel - model(MD_ACC)*dt;% 
end

%% 障礙物距離評價函數  （機器人在當前軌跡上與最近的障礙物之間的距離，如果沒有障礙物則設定一個常數）
% 輸入參數：位姿、所有障礙物位置、障礙物半徑
% 輸出參數：當前預測的軌跡終點的位姿距離所有障礙物中最近的障礙物的距離 如果大於設定的最大值則等於最大值
% 距離障礙物距離越近分數越低
function [dist,Flag] = CalcDistEval(x,ob,R)
dist=100;
for io = 1:length(ob(:,1))  
    disttmp = norm(ob(io,:)-x(1:2)')-R; %到第io個障礙物的距離 - 障礙物半徑  ！！！有可能出現負值嗎？？
    if disttmp <0
        Flag = 1;
        break;
    else
        Flag = 0;
    end
    
    if dist > disttmp   % 大於最小值 則選擇最小值
        dist = disttmp;
    end
end
 
% 障礙物距離評價限定一個最大值，如果不設定，一旦一條軌跡沒有障礙物，將太佔比重
if dist >= 3*R %最大分數限制
    dist = 3*R;
end
 
%% heading的評價函數計算
% 輸入參數：當前位置、目標位置
% 輸出參數：航向參數得分  當前車的航向和相對於目標點的航向 偏離程度越小 分數越高 最大180分
function heading = CalcHeadingEval(x,goal)
theta = toDegree(x(3));% 機器人朝向
goalTheta = toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));% 目標點相對於機器人本身的方位 
if goalTheta > theta
    targetTheta = goalTheta-theta;% [deg]
else
    targetTheta = theta-goalTheta;% [deg]
end
 
heading = 180 - targetTheta;  

%% 計算動態窗口
% 返回 最小速度 最大速度 最小角速度 最大角速度速度
function Vr = CalcDynamicWindow(x,model)

V_SPD       = 4;%機器人速度
W_ANGLE_SPD = 5;%機器人角速度 

MD_MAX_V = 1;% 
MD_MAX_W = 2;% 
MD_ACC   = 3;% 
MD_VW    = 4;% 

global dt;
% 車子速度的最大最小範圍 依次爲：最小速度 最大速度 最小角速度 最大角速度速度
Vs=[0 model(MD_MAX_V) -model(MD_MAX_W) model(MD_MAX_W)];
 
% 根據當前速度以及加速度限制計算的動態窗口  依次爲：最小速度 最大速度 最小角速度 最大角速度速度
Vd = [x(V_SPD)-model(MD_ACC)*dt x(V_SPD)+model(MD_ACC)*dt ...
    x(W_ANGLE_SPD)-model(MD_VW)*dt x(W_ANGLE_SPD)+model(MD_VW)*dt];
 
% 最終的Dynamic Window
Vtmp = [Vs;Vd];  %2 X 4  每一列依次爲：最小速度 最大速度 最小角速度 最大角速度速度
Vr = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
 
%% Motion Model 根據當前狀態推算下一個控制週期（dt）的狀態
% u = [vt; wt];當前時刻的速度、角速度 x = 狀態[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
function x = f(x, u)
global dt;
F = [1 0 0 0 0
     0 1 0 0 0
     0 0 1 0 0
     0 0 0 0 0
     0 0 0 0 0];
 
B = [dt*cos(x(3)) 0
    dt*sin(x(3)) 0
    0 dt
    1 0
    0 1];
 
x= F*x+B*u;  

%% degree to radian
function radian = toRadian(degree)
radian = degree/180*pi;

%% radian to degree
function degree = toDegree(radian)
degree = radian/pi*180;
%% END

DWA動態窗口法的原理及應用

《Python進階》學習筆記

Leetcode 3161. 物塊放置查詢

leetcode 60 排列序列

一個docker容器暴露多個端口

微服務實踐之使用 Visual Studio 2022 調試Dapr 應用程序

wpf附加屬性理解 WPF附加屬性

在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以太網驅動總結