引用與前言
參考鏈接
引用參考如下：
-
博客園解釋：https://www.cnblogs.com/dlutjwh/p/11158233.html 這篇博客園寫的賊棒！我當時就是一邊對着論文一邊對着他這篇來看的，所以大部分論文的文字也來源於此
-
原文論文：https://ieeexplore.ieee.org/document/580977 沒有賬號的話 就去sci-hub吧，這里是preprint版：colli.dvi (cmu.edu)
-
Python代碼：https://github.com/AtsushiSakai/PythonRobotics
ROS代碼：http://wiki.ros.org/dwa_local_planner
前言/基礎知識
其實這篇我本科畢業論文用到了 當時很仔細的看過一遍，但是因為沒有對着代碼看，僅看了一下論文的數學公式推導等，就覺得很厲害，想到這樣的方式去表示和限制。這次專門復習再來一次 結合着代碼看一次好了。這次直接使用Typro打的markdown文件，這樣在博客園的格式應該很好看了，目錄也是在左邊~
格式優美版在博客園，后續相關更新也在博客園內哦；雖然我感覺CSDN好像也挺優美的 hhh；所以最差的就是知乎了，知乎同志們可以點贊，博客園跳轉觀看吧 hhh 三個平台同時發一下好了，做個記錄
首先，我們要了解這個干什么的，在路徑規划算法 主要包括全局路徑規划和局部路徑規划。局部路徑規划主要用於動態環境下的導航和避障，對於無法預測的障礙物DWA算法可以較好地解決。DWA算法的優點是計算負復雜度較低，由於考慮到速度和加速度的限制，只有安全的軌跡會被考慮，且每次采樣的時間較短，因此軌跡空間較小。采樣的速度即形成了一個動態窗口
直接跳轉可以先看看運行的效果，感受一下
論文部分
介紹/Introduction
DWA的整體軌跡評價函數主要是三個方面：
- 與目標的接近程度
- 機器人前進的速度
- 與下一個障礙物的距離
簡而言之就是在局部規划出一條路徑，希望與目標越來越近，且速度較快，與障礙物盡可能遠。評價函數權衡以上三個部分得到一條最優路徑。
該論文相對於之前的創新點在於：
- 該方法是由一個移動機器人的運動動力學推導出來的
- 考慮到機器人的慣性（代碼中計算了剎車距離），這對於具有扭矩限制機器人在高速行駛時很重要。
- 在動態雜亂環境中速度可以較快，對於速度較快的機器人以及低電動機轉矩的機器人較為實用。
相關工作/Related Work
這部分對比的時候，因為年代的原因是隨着全局規划一起對比的
- 全局優點在於計算時可以離線進行，但是目前ROS中全局路徑也在導航過程中不斷變化。
- 全局缺點在於不能適應環境變化以及計算復雜度太高，尤其是環境不斷變化時。
- 局部缺點在於不能保證得到最優解，容易陷入局部最優（如U形障礙環境）。
- 局部優點在於計算速度快，適合環境不斷變化。
- 對比了其他的局部路徑規划算法的優缺點：勢場法計算速度很快，但是在狹窄區域會產生震盪，如果目標點在兩個很近的障礙物之間，則可能找不到路徑。
機器人運動學方程
為了使運動學方程更加接近實際，將模型的速度設為隨時間變化的分段函數，在該假設下，機器人軌跡可看做許多的圓弧積分組成，采用該方法使得障礙物碰撞檢測很方便，因為圓弧與障礙物的交點很好求。
其中關於公式參數的提前解釋：
- \(x(t)\), \(y(t)\), \(\theta(t)\) 分別表示機器人在 \(t\) 時刻的\(x\) 坐標、\(y\) 坐標以及朝向角
- \(x(t_0)\), \(x(t_n)\) 分別表示機器人在 \(t_0\) 和 \(t_1\) 時刻的\(x\)坐標
- \(v(t)\) 表示機器人的平移速度
由此得知坐標是根據速度來得到的，而這個速度不能說直接給他設置，有限制的。例如，機器人速度 \(v(t)\) 取決於初始時刻 \(t_0\) 的速度和時間 \(t_0\)，和速度一樣的， \(\theta(t)\) 也是初始轉向角 \(\theta(t_0)\) 函數
- 機器人在時間間隔 \(\hat t \in [t_0,t]\) 的平移加速度為 \(\dot v(\hat t)\)
- \(t_0\) 時刻的初始旋轉速度為 \(w(t_0)\)，\(\hat t \in [t_0,t]\) 的旋轉加速度為 \(\dot w(\hat t)\)，故(1)可以轉為：
此時機器人的軌跡由初始時刻的狀態以及加速度決定，可以認為這些狀態是可控的，同時由於機器人內部結構原因，其加速度也不是一直變化（類似於連續函數），因此可以將 \(t_0\) 到 \(t_n\) 看作是很多個時間片，積分可以轉換為求和，假設有\(n\)個時間片，在每個\([t_i,t_{i+1}]\)，機器人的加速度 \(\dot v_i\)和\(\dot w\) 保持不變，設\(\Delta^i_t=t-t_i\)，那么(3) 又可以再一步：
式(4)雖然與機器人的動力控制相關，但是不能決定機器人具體的駕駛方向，對於障礙物與機器人軌跡的交點也很難求出，可以繼續進行簡化，既然時間間隔很小，那么我們就把，那么就可以得到(5)
- \(v(t_i)+\dot v_i \cdot \Delta^i_t\) 近似為 \(v_i \in [v(t_i),v(t_{i+1})]\)
- \(\theta\left(t_{i}\right)+w\left(t_{i}\right) \cdot \Delta_{t}^{i}+\frac{1}{2} \dot{w}_{i} \cdot\left(\Delta_{t}^{i}\right)^{2}\) 近似為 \(\theta\left(t_{i}\right)+w\left(t_{i}\right) \cdot \Delta_{t}^{i}\)
最后解這個積分方程，簡化為：
其中\(F\) 展開就是：
以上都是推導\(x\) 坐標，對於\(y\) 整個過程也是一樣的，就是\(F\) 里面的一個三角函數變換了
-
當\(w_i=0\) 時，機器人行走軌跡為一條直線
-
當\(w_i \not = 0\) 時，機器人軌跡為圓弧，設：
\[M_{x}^{i}=-\frac{v_{i}}{w_{i}} \cdot \sin \theta\left(t_{i}\right) \tag{10} \]\[M_{y}^{i}=\frac{v_{i}}{w_{i}} \cdot \cos \theta(t-i) \tag{11} \]然后，奇妙的事情就發生了，我們可以得到這樣一個式子：
\[\left(F_{x}^{i}-M_{x}^{i}\right)^{2}+\left(F_{x}^{i}-M_{x}^{i}\right)^{2}=\left(\frac{v_{i}}{w_{i}}\right)^{2} \tag{12} \]這個式子就是圓在平面的公式，其中這個圓的圓心為 \((M^i_x,M^i_y)\)，半徑為\(\frac{v_i}{w_i}\)。根據上述公式可以求出機器人的軌跡，即通過一系列分段的圓弧和直線來擬合軌跡。
誤差界
將機器人軌跡進行分段會在控制點之間產生線性誤差，即\(t_{i+1}−t_i\)之間的誤差，設x坐標和y坐標的誤差分別為\(E^i_x\)和\(E^i_y\)，\(\Delta t_i=t_{i+1}−t_i\)，由於\(i∈[v(t_i),v(t_{i+1})]\)，故最大誤差\(E^i_x,E^i_y≤|v(t_{i+1})−v(t_i)|\cdot \Delta t_i\)，在\(\Delta t_i\)內是線性的。注意該上界誤差僅僅可用於機器人內部預測，而實際機器人位置一般通過里程計測量。
動態窗口法
動態窗口法在速度空間中進行速度采樣，並對隨機采樣的速度進行限制，減小采樣數目，在使用代價函數進行評價。
1. 速度搜索空間
根據以下三點進行速度空間降采樣
- 圓弧軌跡：動態窗口法僅僅考慮圓弧軌跡，該軌跡由采樣速度 \((v,w)\) 決定，這些速度構成一個速度搜索空間。
- 允許速度：如果機器人能夠在碰到最近的障礙物之前停止，則該采樣速度將被評估。
- 動態窗口：由於機器人加速度的限制，因此只有在加速時間內能達到的速度才會被保留。
2. 最優化
代價函數方程：
最大值即使最優值最大，其中\(\sigma\)使得三個部分的權重更加平滑，使得軌跡與障礙物之間保持一定的間隙。
-
Target heading: heading用於評價機器人與目標位置的夾角，當機器人朝着目標前進時，該值取最大；舉個例子：表示機器人與目標點的對齊程度，用\(180−θ\)表示，\(θ\)為機器人與目標夾角，夾角越大，代價值越小。
-
Clearance: dist 用於表示與機器人軌跡相交的最近的障礙物距離；如果障礙物與機器人軌跡不相交，則設為一個較大的值
-
Velocity: vel 表示機器人的前向移動速度，支持快速移動
安全速度
機器人能夠在撞掉障礙物之前停下，\(\text{dist}(v,w)\) 為機器人軌跡上與障礙物的最近距離，設剎車時的加速度為 \(\dot v_b, \dot w_b\) ，則 \(V_a\) 為機器人不與障礙物碰撞的速度集合：
動態窗口速度
考慮到機器人的動力加速度，搜索空間降采樣到動態窗口，只保留以當前加速度可到達的速度，設\(t\)為時間間隔，\((v_a,w_a)\)為實際速度，則動態窗口的速度集合為\(V_d\)：該集合以外的速度都不能在該時間間隔內達到。
綜上，最終的搜索空間：
最終的代價函數的限制就是在這個搜索空間內的
實驗部分
平滑處理：
評價函數的三個部分都被正則化在\([0,1]\)上，實驗中設置了\(α=2\)，\(β=0.2\)，\(γ=0.2\)，平滑處理可以使機器人與障礙物之間有一定的間隙（裕度）。
實現細節：
- 當機器人陷入局部最優時（即不存在路徑可以通過），使其原地旋轉，直到找到可行路徑。
- 安全裕度：在路徑規划時，設定一安全裕度，即在路徑和障礙物之間保留一定間隙，且該間隙隨着速度增大線性增長。
參數設定：
- \(α\) 占比重太大，機器人運動自由度大，窄的區域不容易通過，\(α\)占比重太小，機器人軌跡則不夠平滑。因此\(α\) 越大，越適合在窄區域，\(α\) 越小，越適合在寬區域。
代碼部分
這里先說一下整體的思路，主要是對着參考的Python代碼的，有空我把ROS_wiki那邊也補充完整
- 設定初始狀態：\([x,y,\theta,v,\omega]\) ；目標位置：\([x_{goal},y_{goal}]\)
- 機器人的參數：最大最小速度、最大轉角(偏航角)速度、最大加速度、最大角加速度、速度和偏航角分辨率、預測時間范圍（就是生成多大的空間）、目標，速度，障礙物的各個cost設定、各個障礙物的位置
- 計算動態窗口
- 計算控制和軌跡點
1. 計算動態窗口
主要是根據現在的狀態和默認的參數進行的設置：
def calc_dynamic_window(x, config):
"""
calculation dynamic window based on current state x
"""
# Dynamic window from robot specification
Vs = [config.min_speed, config.max_speed,
-config.max_yaw_rate, config.max_yaw_rate]
# Dynamic window from motion model
Vd = [x[3] - config.max_accel * config.dt,
x[3] + config.max_accel * config.dt,
x[4] - config.max_delta_yaw_rate * config.dt,
x[4] + config.max_delta_yaw_rate * config.dt]
# [v_min, v_max, yaw_rate_min, yaw_rate_max]
dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
return dw
可以從中看出 主要是速度的整個窗口，讀取機器人的默認速度配置，然后計算加速度時間后的速度取最小最大的，形成一個取速度的范圍
2. 采樣軌跡
根據現在的狀態和前面的速度動態窗口，配置中的速度分辨率（就是以多少間隔生成速度值）
# evaluate all trajectory with sampled input in dynamic window
# for v in np.arange(dw[0], dw[1], config.v_resolution):
# for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):
# trajectory = predict_trajectory(x_init, v, y, config)
def predict_trajectory(x_init, v, y, config):
"""
predict trajectory with an input
"""
x = np.array(x_init)
trajectory = np.array(x)
time = 0
while time <= config.predict_time:
x = motion(x, [v, y], config.dt)
trajectory = np.vstack((trajectory, x))
time += config.dt
return trajectory
其中motion就是由車輛運動的模型得出來的這時刻狀態加速度得到下一時刻的狀態，根據前面可知：v是速度，y是yaw角速度，x的各個位置為：\([x,y,\theta,v,\omega]\)
def motion(x, u, dt):
"""
motion model
"""
x[2] += u[1] * dt # v=v+a*t
x[0] += u[0] * math.cos(x[2]) * dt # x=x+v*cos(theta)
x[1] += u[0] * math.sin(x[2]) * dt # y=y+v*sin(theta)
x[3] = u[0]
x[4] = u[1]
return x
3. 計算cost
goal cost
這個cost也就是前面最優化里面提到的heading cost，朝向
def calc_to_goal_cost(trajectory, goal):
"""
calc to goal cost with angle difference
"""
dx = goal[0] - trajectory[-1, 0]
dy = goal[1] - trajectory[-1, 1]
error_angle = math.atan2(dy, dx)
cost_angle = error_angle - trajectory[-1, 2]
cost = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))
return cost
速度cost
速度cost之間是根據得出的軌跡的速度和最大速度做個差值再乘一個速度cost的系數得出來的
speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
障礙物cost
這么一看 果然還是圓形比較適合做碰撞檢測，怪不得傑哥中間也是將車膨脹成兩個圓
def calc_obstacle_cost(trajectory, ob, config):
"""
calc obstacle cost inf: collision
"""
ox = ob[:, 0]
oy = ob[:, 1]
dx = trajectory[:, 0] - ox[:, None]
dy = trajectory[:, 1] - oy[:, None]
r = np.hypot(dx, dy)
if config.robot_type == RobotType.rectangle:
yaw = trajectory[:, 2]
rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]])
rot = np.transpose(rot, [2, 0, 1])
local_ob = ob[:, None] - trajectory[:, 0:2]
local_ob = local_ob.reshape(-1, local_ob.shape[-1])
local_ob = np.array([local_ob @ x for x in rot])
local_ob = local_ob.reshape(-1, local_ob.shape[-1])
upper_check = local_ob[:, 0] <= config.robot_length / 2
right_check = local_ob[:, 1] <= config.robot_width / 2
bottom_check = local_ob[:, 0] >= -config.robot_length / 2
left_check = local_ob[:, 1] >= -config.robot_width / 2
if (np.logical_and(np.logical_and(upper_check, right_check),
np.logical_and(bottom_check, left_check))).any():
return float("Inf")
elif config.robot_type == RobotType.circle:
if np.array(r <= config.robot_radius).any():
return float("Inf")
min_r = np.min(r)
return 1.0 / min_r # OK
這一步直接計算了所有的障礙物的，也得益於python的方便，直接計算軌跡與各個障礙物的直線距離，判斷是否對於圓半徑，只要有一個障礙物碰到了 這條軌跡就被拋棄了，如果沒有的話 輸出最小的那個距離，然后分之一進行輸出以便計算cost
綜合下來就是：
# calc cost
to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)
speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(trajectory, ob, config)
final_cost = to_goal_cost + speed_cost + ob_cost
運行示意
在原基礎上，把其他速度窗口內的軌跡也畫出來了，但是因為選cost最小的也就是紅色那個哈
這么一看我好像知道我曾經用ROS DWA那邊有啥問題了：
-
刷新路徑dt太快了？走一下發現下一步的軌跡需要左移，然后右移，然后左移 emmm 這樣就S形了？
這一點可能但不是最終問題原因
-
或者是把障礙物的cost因子調低一點？好像是看到了就躲一下，進了再躲一下；
這一點是XM提醒的，我提出當時我有個問題一直沒解決，然而我分析的原因是這個，XM說：那就算你調低了 也只是暫時的 你走S是因為你朝向了那邊 進了障礙物自然要躲除非不躲，那就不會，但是你不可能把障礙物的因子調到0的。我后面思考 覺得十分有道理！
-
最近總結的時候又再搜DWA的缺點，發現了
可能還有一個原因是heading因子cost設的太高了！！[我覺得肯定是這條了！分析見后面]
參考於：動態窗口法的理解和一些細節_Azahaxia的博客-程序員寶寶_動態窗口法缺點 - 程序員寶寶 (cxybb.com)
然后再貼一下ROS那邊的（我找到了我本科畢設答辯的PPT截圖出來的 hhh）其實當時我答辯錄屏了，但是當時錄制的時候忘記點語音了 然后就是無聲的，hhh 留個留念的機會都沒有了
仔細看我走的路徑中是剛好從左下角走到右上區，所以小車總是想要往右跑，跑到了牆壁處就開始躲避一下障礙物，然后又朝着目標跑，又撞牆又躲，emm 所以這個heading我感覺不應該以最終目標點為heading，而應該以全局規划的線heading？
總結
這么一看 比frenet簡單太多了 hhhh 畢竟是1997年的了，frenet都是2010年的了：Frenet 博客園的論文閱讀與代碼實例
主要在於：
- DWA這篇的空間一直以笛卡爾坐標系，也不沒有sd坐標系 說有什么固定的線，也正是因為如此DWA通常作為小型室內無明確道路的機器人首選的局部規划器
- frenet那篇以考慮加加速度 jerk來作為cost，考慮了車輛乘坐時的舒適度就是加速度的加速度
- Frenet考慮了沿着某個道路的采樣方式，也就是說有某個固定道路線去做行駛，遇到障礙物了再避障，這也符合我們實際駕駛道路車的行為
- Frenet考慮障礙物的東西更多，縱向運動時的跟車，匯入等等 所以常常作為道路無人車局部采樣規划的首選
至此，再次“復習”完一個，這么看當初問阿冰哥的答案也能更懂了：
-
DWA是采樣評價，大部分采樣擬合再評價的都是
-
Frenet也是，OpenPlanner也是，只是說采樣空間變了而已
-
采樣-擬合-評價一般說采樣空間是暴力遍歷的，不暴力遍歷的話可以當成搜索問題