ロボトレースのショートカットコースをMATLABのPRMパスプランナーで生成する話
はじめに
この記事はMicro Mouse Advent Calendar 2020 の19日目の記事です。 前日の18日目はebi-chanさんによる一年の振り返りでした。カメムシが大量にいたら嫌ですよねぇ。カメムシが家に出たら間違っても叩き潰さないよう気を付けようと思います。
さて、本記事の内容は、MATLABのRobotics System Toolboxで提供されている PRMパスプランナー を使って、ロボトレースのショートカットコースを生成する方法です。
まずは結果
以下の動画でおおよその動作が分かると思います。このコースは2019年の全国大会の課題コースです。動画の左下のFigure1に元コースの (x,y) データの散布図が表示されており、 左上のFigure2にショートカットパスを計算しているアニメーションが表示されています。ショートカットコースを計算した結果が右のFigure3に表示されており、 元のコースとショートカットコースの比較をしています。
元のコースとショートカットコースの比較を拡大して見てみます。図の青い線が元のコースデータで、赤い線がショートカットコースです。 大きな円弧とスラロームが合わさったような青い線が、ほぼ大きな円弧の赤い線になっているところが特に分かりやすいかと思います。 また、コース終盤の長い直線が少し角度を変えて小さな円弧でつながったような箇所も、赤い線が真っすぐになっているので分かりやすいかと思います。
MATLABスクリプト
上記ような動作をするMATLABスクリプトを以下に掲載します。 自分のMATLABで動かしてみる際はコースデータが無ければ動かないので、以下のGitHub のリポジトリをクローンして動かしてください。
また、使用しているコースデータですが、 kyamashita5 さんが以下のGitHubで公開されていたのでありがたく使わせていただきました。ありがとうございます。
スクリプト
以下スクリプトです。冒頭のFileNameをコメントアウトされている中から選べば、色々なコースのショートカットコースを生成できます。
%% コースデータを読み込む
clear
clf
%-----------ファイル名を選択-----------%
FileName = 'course_data/2019AllJapan.txt';
% FileName = 'course_data/2019Student.txt';
% FileName = 'course_data/2019East.txt';
% FileName = 'course_data/2018student_points.txt';
% FileName = 'course_data/2018alljapan_points.txt';
% FileName = 'course_data/2017alljapan_points.txt';
% FileName = 'course_data/2016alljapan_points.txt';
% FileName = 'course_data/2015alljapan_points.txt';
Position = readmatrix(FileName); %[m}
OneGridScale = 1e2; %[cm]
Position = Position * OneGridScale;
figure(1)
scatter(Position(:, 1), Position(:, 2));
axis equal
%% ショートカットパス生成
% --------------交差フラグテーブルを生成--------------%
FlagMargin = 0.3 * OneGridScale;
SearchRange = 0.03 * OneGridScale;
Flag = GetCrossFlagTable(Position, FlagMargin, SearchRange);
% --------------ショートカットパスを生成----------------------%
ExpansionLength = 0.1; %拡張する大きさ[m]
ExpansionLength = ExpansionLength * OneGridScale;
NumNodes = 100;
ConnectionDistance = 2;
[ShortcutPath, SaveBineryMap] = GetShortcutPath(Position, Flag, ExpansionLength, OneGridScale, NumNodes, ConnectionDistance);
%% Functions
%--------------コースが交差している箇所を検知する関数-------------%
function flagTable = GetCrossFlagTable(position, flagMargin, searchRange)
positionNum = numel(position(:, 1));
storePosition = zeros(positionNum, 2);
Sum = zeros(1, positionNum);
Flag = zeros(1, positionNum);
for i = 1 : positionNum
storePosition(i, :) = position(i, :);
Idx = my_rangesearch(storePosition, storePosition(i, :), searchRange); % 現在の点の周囲の点を探索
Outlier = isoutlier(Idx); % 外れ値検知
Sum(i) = sum(Outlier, 'all');
Flag(i) = ge(Sum(i), 1); % 交差しているかフラグ
end
% 白線を拡大した分、交差フラグをある程度前後に発生させる
for i = 1 : positionNum
if Flag(i) == 1
for j = 1 : flagMargin
if i - flagMargin >= 1
Flag(i - flagMargin) = 1;
end
end
end
end
flagTable = Flag;
end
%--------------ショートカットパスを計算する関数-------------%
function [path, saveBinaryMap] = GetShortcutPath(position, flag, expansionLength, oneGridScale, numNodes, connectionDistance)
positionNum = numel(position(:, 1));
%----------グリッドマップ初期化-----------%
MaxPositions = max(position);
MinPositions = min(position);
MaxPosition = ceil(MaxPositions); % 正に丸める
MinPosition = floor(MinPositions); % 負に丸める
MapElements_x = (MaxPosition(1) - MinPosition(1)); % [cm]
MapElements_y = (MaxPosition(2) - MinPosition(2)); % [cm]
GridMap = ones(MapElements_y, MapElements_x); % 1マス1[cm]のグリッドマップ
saveGridMap = ones(MapElements_y, MapElements_x); % figureで表示するためだけのマップ。実際は必要ない
GridMapOrigine = [position(1, 1) + abs(MinPosition(1)), position(1, 2) + abs(MinPosition(2))];
%xとyを丸めてマップ外になってしまったときに修正
if GridMapOrigine(1, 1) <= 1
GridMapOrigine(1, 1) = 1;
elseif GridMapOrigine(1, 1) >= MapElements_x
GridMapOrigine(1, 1) = MapElements_x;
end
if GridMapOrigine(1, 2) <= 1
GridMapOrigine(1, 2) = 1;
elseif GridMapOrigine(1, 2) >= MapElements_y
GridMapOrigine(1, 2) = MapElements_y;
end
ExpansionHalf = ceil(expansionLength / 2);
startLocation = GridMapOrigine;
shortcutPath = NaN(positionNum, 2);
shortcutPath(1, :) = startLocation / oneGridScale;
ignoreFlag = 0;
PathNum = 1;
%------生の位置データをグリッドマップに変換-----------%
for i = 1 : positionNum
% 各位置を四捨五入して整数にする
x = ceil(position(i, 1)) + abs(MinPosition(1)); % 生データをグリッド座標に写すためにオフセットする
y = ceil(position(i, 2)) + abs(MinPosition(2));
%xとyを丸めてマップ外になってしまったときに修正
if x <= 1
x = 1;
elseif x >= MapElements_x
x = MapElements_x;
end
if y <= 1
y = 1;
elseif y >= MapElements_y
y = MapElements_y;
end
% 白線を膨張させる
for j = 1 : expansionLength + 1
Yref = MapElements_y - (y - (ExpansionHalf + 1) + j); %行列は左上から数えられてしまうので左下からに変換
if Yref >= 1 && Yref <= MapElements_y
for k = 1 : expansionLength + 1
Xref = x - (ExpansionHalf + 1) + k;
if Xref >= 1 && Xref <= MapElements_x
GridMap(Yref, Xref) = 0;
saveGridMap(Yref, Xref) = 0;
end
end
end
end
% 最短経路を探す
if (ignoreFlag == 1 && flag(i) == 0) || i >= positionNum % 一回交差したら交差しなくなるか終わるまで処理を無視
ignoreFlag = 0;
end
if ignoreFlag == 0 && flag(i) == 1 || i >= positionNum % 交差したか終わったら最短経路を探す
endLocation = [position(i, 1) + abs(MinPosition(1)), position(i, 2) + abs(MinPosition(2))]; % 丸めていない生のポジション
BinaryMap = binaryOccupancyMap(GridMap, oneGridScale);
[tempPath, prm] = GetPath(BinaryMap, startLocation/oneGridScale, endLocation/oneGridScale, numNodes, connectionDistance);
for n = 1 : numel(tempPath(:, 1))
shortcutPath(PathNum, :) = tempPath(n, :);
PathNum = PathNum + 1;
end
startLocation = endLocation;
ignoreFlag = 1;
figure(2)
show(BinaryMap)
show(prm)
GridMap = ones(MapElements_y, MapElements_x); % 1マス10[mm]のグリッドマップ
end
end
shortcutPath = rmmissing(shortcutPath); %NuNを削除
path = shortcutPath;
figure(3)
hold on
saveBinaryMap = binaryOccupancyMap(saveGridMap, oneGridScale);
show(saveBinaryMap)
animatedline(shortcutPath(:, 1), shortcutPath(:, 2), 'Color', 'red', 'LineWidth', 1);
animatedline((position(:, 1) + abs(MinPosition(1))) / oneGridScale, (position(:, 2) + abs(MinPosition(2))) / oneGridScale, 'Color', 'blue', 'LineWidth', 1);
end
%--------------バイナリマップからパスを計算する関数-------------%
function [path, prm] = GetPath(BinaryMap, startLocation, endLocation, numNodes, connectionDistance)
%----------PRM設定-----------%
prm = robotics.PRM(BinaryMap);
prm.NumNodes = numNodes;
prm.ConnectionDistance = connectionDistance;
%---------パス生成-----------%
TF = 0;
counter = 0;
while 1
path = findpath(prm, startLocation, endLocation);
counter = counter + 1;
if TF == isempty(path)
break;
end
if counter >= 100
break;
end
prm.NumNodes = prm.NumNodes + 100;
end
end
簡単な解説
簡単にやっていることを解説します。アルゴリズムはProbabilistic Roadmap (PRM) を使用しています。これはMATLABのRobotics System Toolboxから提供されています。 マイクロマウス参加者なら（多分）無償で取得できるライセンス に Robotics System Toolboxが含まれているので、ライセンスの問題は無いはずです。
PRMはMATLABの公式ドキュメントの複雑度の異なる環境でのパス計画
を読んでもらえれば使用方法はわかるかと思います。アルゴリズムを簡単に説明すると、ロボットが移動できる箇所とできない箇所を記したバイナリマップを作成し、
移動できる箇所にランダムな点を打って点と点をつなぐ一番短い経路を探すというものです（多分...）。
ここでいうバイナリマップは、ロボットが移動できる箇所として元コースからロボトレーサの横幅分を膨張させたマップです（ルール上、トレーサ本体がコースに被っていれば良いので）。
しかし、ロボトレースのコースは交差する箇所があるので、ドキュメントの内容そのままを適用することはできませんでした。
具体的には、交差する箇所で進行方向を指定することができず、道順に沿った経路を生成できないという問題が起こりました。
そこで、スクリプト中のGetCrossFlagTable関数でコースが交差している箇所を探して、交差している点と点で分断したマップごとで最短経路を探し、逐次合成するといった手法をとりました。
賢い方法かどうかは知りません。
問題点
なかなかいい感じにショートカットコースを生成することはできましたが、今の段階ではあくまでPCの中でのお話です。 実際にはロボトレーサ自身がショートカットコースを生成できなければ、ロボトレースのルール上意味がありません。 ロボトレース実機に今回紹介したアルゴリズムを適用するにあたっていくつか問題点があるので、説明します。
まず、MATLABスクリプトをC/C++コードに書き直す段階で問題があります。
多くのロボトレーサにはおそらくマイコンが搭載されており、C/C++で書かれたプログラムで動いているかと思います（もちろん例外はあると思います）。
MATLABにはありがたいことに、MATLABスクリプトをC/C++コードで吐き出してくれる機能（MATLAB Coder, Simulink Coder, Embedded Coder
など）が存在するので、
今回紹介したスクリプトをC/C++コードに変換してやればよいのですが、残念ながらC/C++コード生成に対応していない関数が含まれていました。
たしか、findpath関数が対応していなかったと思います（他にもあるかもしれません）。
今後のアップデートでC/C++コード生成に対応すれば、上記アルゴリズムを簡単にロボトレーサに適用できるかもしれません。
もちろん、自分でC/C++コードに書き直せば問題はないのですが、面倒くさいバグが混入するかもしれないのでやりたくありません。
さらに、PRMは比較的計算コストが高いアルゴリズムなので、マイコンで計算したときの処理時間が気がかりです。 そこそこの性能があるPCでもある程度計算に時間がかかるのに、貧弱なマイコンで動かしたらどの程度時間がかかるか分かりません。 ロボトレースは競技時間内に3回走行できますが、計算に時間がかかりすぎると3回走行できないかもしれません。
タコ壺（曲率の小さなカーブが連続している箇所）などのショートカットコースがギザギザしてしまっている点も問題があります。 クリティカルに問題が発生するのは多分、ショートカットコースをパス追従する際ですが、パスがあまりギザギザしていると曲がり切れず、大回りに旋回してしまう可能性があります。 MATLAB公式ドキュメントの差動駆動型ロボットのパス追従 にある例を使って尖ったパスを追従させてみるとわかるかと思いますが、大回りに旋回してしまいます。
まとめ
MATLABを使ってロボトレースのショートカットコースを生成する方法を紹介しました。 生成はできましたが、あくまでPC上での話で、ロボトレーサ実機に適用するにはまだ解決しなければならない問題がある方法でした。 改善案として、A*アルゴリズムをC/C++コード生成に対応しているMATLAB関数のみで書き、C/C++コードに変換してロボトレーサ実機に適用できればなぁと考えています。
終わりに
初めてのアドベントカレンダー参加なので、至らない点がありましたらコメントしていただけるとうれしいです。 また、だいぶ前に書いたスクリプトについての記事なのでちょっと違ったことを書いているかもしれませんが、ご愛嬌ということでお願いします。
話は少しずれますが、去年も以下のようなロボトレースについての記事を書きましたので、ついでに見ていただけたらうれしいです。 2019年のロボトレース大会に出場した機体の紹介と、その機体でボイスコイルモータ（VCM)を使ってライントレースをした話の記事です。
明日の Micro Mouse Advent Calendar 2020 20日目は もすさん によるEAGLEのulpについての記事です。お楽しみに！
コメント
コメントを投稿