用圖搜索方法求解car-like機器人的最優路徑

目的

　　本文將傳統的圖搜索方法應用於具有最小轉向半徑的機器人上（例如無人車），求其任意兩個位姿之間的最短路徑，如下圖所示。所用的軟件是Mathematica。

1　步驟

　　要使用圖搜索方法，當然得有“圖”才行。我們將機器人的位姿空間離散化得到圖，爲此首先對$xy$平面離散化，按照一定的間隔dx得到均勻二維網格，後面我們就在這個網格的基礎上建立“圖”。

{dx,d\[Theta]}={0.2,Pi/12.};
gridpts=Tuples[Range[-3,3,dx],2];

　　如果你想添加一些障礙物，可以像下面這麼做。

obpts={{1,1},{-1,1},{-1,-1},{1,-1},{1,-0.5},{-0.5,-0.5},{-0.5,0.5},{0.5,0.5},{0.5,0},{1,0}}*1.8;
removePtsInObstacle[pts_]:=Module[{check},check=RegionMember[Polygon@obpts];Select[gridpts,!(check[#])&]]
gridptsOK=removePtsInObstacle[gridpts];

　　car-like機器人的構型空間$xy\theta$是個三維空間，下面把機器人的角度$\theta$也離散化，然後與二維網格合在一起建立一個$xy\theta$空間中的“三維網格”。網格有了，網格的節點就是圖的節點，一個節點實際代表機器人的某個位姿，現在唯一缺少的就是圖的邊了。NearestNeighborGraph函數用於生成最近鄰圖，即節點之間的距離小於一定閾值的就給它們用邊連起來，大於閾值的就沒有邊。這裏我取的閾值是$3$倍的間隔dx，如果太大則離散的太粗，效果太差，太小則要處理的邊太多，計算非常慢。

gridptsOK3D=Flatten[Table[{pt[[1]],pt[[2]],\[Theta]},{pt,gridptsOK},{\[Theta],-Pi,Pi,d\[Theta]}],1];
graph=NearestNeighborGraph[gridptsOK3D,{All,dx*3}];
nodePairs=Transpose@{EdgeList[graph][[;;,1]],EdgeList[graph][[;;,2]]};

　　下面給圖的邊設置權重，這一步是最重要的，前面的只是對狀態空間離散化而已。cross2函數用來計算兩個二維向量的差積，calEdgeWeight函數用來計算邊的權重，這裏要考慮幾種情況（或者說機器人的約束），其中第一個If描述的是非完整約束，第二個是計算弧長，第三個是car-like機器人最小轉向半徑的約束，具體可以參考論文《Kinodynamic Trajectory Optimization and Control for Car-Like Robots》。

Rmin =0.5;
cross2[a_,b_]:=Module[{x1,y1,x2,y2},  {x1,y1}=a;  {x2,y2}=b;  x1*y2-y1*x2]
calEdgeWeight[nodePairs_] := Module[{n1, n2, vec},
   {n1, n2} = nodePairs;
   vec = (n2 - n1)[[1 ;; 2]];
   If[Abs@cross2[AngleVector[n1[[3]]] + AngleVector[n2[[3]]], vec] > 0.1, Return[100000]];
   If[n1[[3]] == n2[[3]], Return[Norm[vec]]];
   If[Norm[vec]/Abs[n1[[3]] - n2[[3]]] < Rmin, Return[100000],Return[Norm[vec]]];
];
edgeWeights=calEdgeWeight/@nodePairs;
elist=EdgeList[graph];
graph=Graph[elist,EdgeWeight->edgeWeights];

　　最後檢驗搜索的效果。FindShortestPath函數用於從我們建立的圖中搜索最短的路徑，這個函數使用的圖搜索算法應該是Dijkstra算法。

drawPoseArrow[pose_]:={Arrowheads[0.015],Thickness[0.002],Arrow[{pose[[1;;2]],pose[[1;;2]]+0.2AngleVector[pose[[3]]]}]};
Manipulate[
startpose=Flatten[{p,\[Theta]}];
startpose0=First@Nearest[gridptsOK3D,startpose,1];
targetpose={0,0,0};
targetpose0=First@Nearest[gridptsOK3D,targetpose,1];
path=FindShortestPath[graph,startpose0,targetpose0];
pathpairs=Partition[path,2,1];
Graphics[{Polygon@obpts,Line/@pathpairs[[;;,;;,1;;2]],Red,PointSize[0.01],drawPoseArrow/@path},ImageSize->500,Axes->True,PlotRange->{{-1,1},{-1,1}}*3],{{p,{2,1}},Locator},{{\[Theta],0},-Pi,Pi,0.1},TrackedSymbols:>True]

2　優缺點

　　這種方法的缺點很明顯，離散化導致節點太多，上面這個簡單的例子一共有$17525$個節點、$608834$個邊，這顯然太多了，計算起來非常慢，難以用於實時性要求高的場合。上面最耗時的就是計算邊權重的calEdgeWeight函數，這裏我提供一個編譯後的版本calEdgeWeightCompile，如下，比原來的快多了。直接編譯成了C語言再調用，比Mathematica語言快上百倍。

calEdgeWeightCompile = Compile[{{nodePairs, _Real, 2}},
   Module[{n1x, n1y, n1t, n2x, n2y, n2t, nv, x1, y1, x2, y2},
    {{n1x, n1y, n1t}, {n2x, n2y, n2t}} = nodePairs;
    nv = Norm[{n2x, n2y} - {n1x, n1y}];
    {x1, y1} = {Cos[n1t], Sin[n1t]} + {Cos[n2t], Sin[n2t]};
    {x2, y2} = {n2x, n2y} - {n1x, n1y};
    If[Abs[x1*y2 - y1*x2] > 0.1, Return[100000.0]];
    If[n1t == n2t, Return[nv]];
    If[nv/Abs[n1t - n2t] < Rmin, Return[100000.0], Return[nv]] ], CompilationTarget -> "C"];

　　這種方法的優點也很明顯，只需要計算一遍，就能得到任何兩個位姿之間的最短路徑。當然，如果環境（也就是障礙物）變了就要重新計算。

3　與其它方法的關係

　　這種方法既然能求得最短的曲線，那麼自然會問它和另一種最短曲線：Reeds-Shepp 有什麼關係呢？實際上它們倆是一樣的，從下圖中就可以看出來。圖中綠色的曲線是用Reeds-Shepp方法求得的，可見它們基本是重合的。Reeds-Shepp方法的缺點是沒有考慮障礙物，所以只能用在空曠的環境中，優點是計算速度很快，它不需要離散化，所以求得是精確的解。

　　還有一種求最短曲線的思路，解Hamilton-Jacobi方程所代表的最優控制問題，但是這個方法比較複雜，你得懂一點偏微分方程的粘性解。該方法的缺點是你要限定一個目標點，所以目標點變化了，你就得重新解。而且這種方法也需要離散化，所以也不快，當然解出來的曲線與Reeds-Shepp曲線和圖搜索的解仍然是一樣的。