0 引言
随着現代技術的發展，飛行器種類不斷變多，應用也日趨專一化、完善化，如專門用作植保的大疆PS-X625無人機，用作街景拍攝與監控巡察的寶雞行翼航空科技的X8無人機，以及用作水下救援的白鲨MIX水下無人機等，決定飛行器性能主要是内部的飛控系統和外部的路徑規劃問題。就路徑問題而言，在具體實施任務時僅靠操作員手中的遙控器控制無人飛行器執行相應的工作，可能會對操作員心理以及技術提出極高的要求，為了避免個人操作失誤，進而造成飛行器損壞的危險，一種解決問題的方法就是對飛行器進行航迹規劃。
飛行器的測量精度，航迹路徑的合理規劃，飛行器工作時的穩定性、安全性等這些變化對飛行器的綜合控制系統要求越來越高。無人機航路規劃是為了保證無人機完成特定的飛行任務，并且能夠在完成任務的過程中躲避各種障礙、威脅區域而設計出最優航迹路線的問題。常見的航迹規劃算法如圖1所示。
圖1 常見路徑規劃算法
文中主要對無人機巡航階段的航迹規劃進行研究，假設無人機在飛行中維持高度與速度不變，那麼航迹規劃成為一個二維平面的規劃問題。在航迹規劃算法中,A算法計算簡單，容易實作。在改進A算法基礎上，提出一種新的、易于了解的改進A算法的無人機航迹規劃方法。傳統A算法将規劃區域栅格化，節點擴充隻限于栅格線的交叉點，在栅格線的交叉點與交叉點之間往往存在一定角度的兩個運動方向。将存在角度的兩段路徑無限放大、細化，然後分别用兩段上的相應路徑規劃點作為切點，找到相對應的組成内切圓的圓心，然後作弧，并求出相對應的兩切點之間的弧所對應的圓心角，根據下式計算出弧線的長度
式中：R———内切圓的半徑；
α———切點之間弧線對應的圓心角。
1 RRT定義
RRT（Rapidly-Exploring Random Tree）算法是一種基于采樣的路徑規劃算法，常用于移動機器人路徑規劃，适合解決高維空間和複雜限制下的路徑規劃問題。基本思想是以産生随機點的方式通過一個步長向目标點搜尋前進，有效躲避障礙物，避免路徑陷入局部極小值，收斂速度快。本文通過matlab實作RRT算法，解決二維平面的路徑規劃問題。
2 地圖
為了友善算法的實作，使用離散量來表達環境地圖。其中，數值0表示無障礙物的空區域，數值1表示該區域有障礙物。
RRT算法中搜尋到的頂點坐标為連續點，在地圖中産生随機點，算法将通過連續的點建構樹。此過程中，對樹枝和頂點進行檢測，檢測頂點所處位置是否是空區域。下載下傳附錄中.dat檔案，繪制地圖。
note:資料中的列為x軸，行為y軸
3 RRT算法原理
通過matlab程式建構從起始位置到目标位置的樹，并生成連接配接兩個點的路徑。使用一顆中心點在起始點的樹，而不是兩顆樹（一個中心點在起始位置，一個中心點在目标位置）。
編寫一個matlab函數，輸入和輸出有相同的形式。
其中：
map：.mat檔案中的地圖矩陣
q_start：起點的x和y坐标
q_goal：目标點的x和y坐标
k: 在目标點無法找到是，控制産生搜尋樹的最大疊代次數為k次
delta_q : q_new 和 q_near之間的距離
p: 将q_goal 作為q_rand 的機率，當随機産生的随機數小于p，将目标點作為随機點q_rand，當随機産生的數大于p時，産生一個随機點作為q_rand
vertices：頂點的x和y坐标，生成随機樹的過程中産生的所有的點的坐标都存儲在這個矩陣中，第一個點為起點，最後一個點為目标點。是一個2行n列的矩陣
deges：生成随機樹的所有樹枝，一共有n-1個樹枝，是以該矩陣有n-1行，每一行的兩列分别表示兩個點的索引号。一旦搜尋到目标點，最後一行将表示目标點，沿着目标點回溯，即可找到路徑
path: 從起始點到目标點的索引，是一個行向量
下面用一個圖來表示上面提到的算法裡的一些變量：
4 障礙物檢測
檢測樹枝（即q_near和q_new之間的edge）是否處于自由空間，可以使用增量法或者等分法，示意圖如下(假設兩點之間有10個點，左圖為為增量檢測法，右圖為等分法，從示意圖中可以看出使用等分法檢測次數更少)：
在本文中，使用k=10000，delta_q=50,p=0.3, 我們将獲得如下結果：
5 路徑平滑處理
完成基本的RRT算法之後，我們獲得了一條從起點到終點的路徑，現在對這條路徑進行平滑和降噪處理，處理完成之後，我們将得到一條更短的路徑。
采用貪心算法：
連接配接q_start和q_goal,如果檢測到兩個點之間有障礙物，則将q_goal替換為前一個點，直到兩個點能連接配接上（中間無障礙物）為止。一旦q_goal被連接配接上，
在matlab中定義平滑函數：
path: 從起始點到目标位置的路徑索引号
vertices：樹中所有的頂點坐标
delta：增量距離，用來檢測路徑頂點之間的直接連接配接是否在自由空間之内，每個edge都被delta分割成幾段
path_smooth：經過平滑處理之後，路徑點将會減少，用path_smooth記錄平滑之後的路徑,仍然是一個行向量，記錄路徑的索引号
平滑處理之後的路徑為：
6 總結
RRT算法是一種增量式的搜尋算法，基于機率的思想，它是一種機率完備的路徑優化算法，具有求解速度上的優勢。RRT基本算法有其自身缺陷，求解得到的路徑通常品質不好，帶有棱角，不夠光滑。是以需要對路徑進行平滑處理，才能得到适合機器人路徑跟蹤的路徑曲線。
1 matlab版本
2014a
2 參考文獻
[1] 包子陽,餘繼周,楊杉.智能優化算法及其MATLAB執行個體（第2版）[M].電子工業出版社，2016.
[2]張岩,吳水根.MATLAB優化算法源代碼[M].清華大學出版社，2017.
[3]RRT路徑規劃算法