はじめに
本ドキュメントはMIRS1404-SOFT-0012の追伸である。
A*アルゴリズムによる座標移動処理の実装例
以下に二輪駆動ロボットにおけるA*アルゴリズムによる座標移動処理の実装例を示す。
Move.cpp
/**
Copyright(c) 2015 d11120
Released under the MIT license
http ://opensource.org/licenses/mit-license.php
*/
#include <algorithm>
#include <iostream>
#include <stdlib.h>
#include <string>
#include <vector>
#include <deque>
#include <math.h>
using namespace std;
enum State {
CLOSE, //0
SEMI_OPEN, //1
OPEN, //2
SEARCHED, //3
SEMI_SELECTED,//4
SELECTED, //5
FORBIDDEN, //6
};//end enum State
struct Anode {
State state;
int ActualCost;
int HeuristicCost;
int Score;
};//end struct Anode
void ChangeStateCL(Anode* np) {
if (np->state != FORBIDDEN) np->state = CLOSE;
}//end method ChangeStateC
void ChangeStateSO(Anode* np) {
if (np->state != FORBIDDEN) np->state = SEMI_OPEN;
}//end merhod ChangeStateSO
void ChangeStateOP(Anode* np) {
if (np->state != FORBIDDEN) np->state = OPEN;
}//end method ChangeStateOP
void ChangeStateFB(Anode* np) {
if (np->state != FORBIDDEN) np->state = FORBIDDEN;
}//end method ChangeStateFB
void ChangeStateSR(Anode* np) {
if (np->state != FORBIDDEN) np->state = SEARCHED;
}//end method ChangeStateS
void ChangeStateSS(Anode* np) {
if (np->state != FORBIDDEN) np->state = SEMI_SELECTED;
}//end method ChangeStateSS
void ChangeStateSL(Anode* np) {
if (np->state != FORBIDDEN) np->state = SELECTED;
}//end method ChangeStateSL
struct Cordinate {
int x;
int y;
};//end struct Cordinate
void rotate(int det, Cordinate* pvp0, Cordinate* pvp1) {
if (0 < det) cout << "rotate : CounterClockwise" << endl;
else if (0 > det) cout << "rotate : Clockwise" << endl;
else if (0==det && 0==pvp0->x+pvp1->x && 0==pvp0->y+pvp1->y) cout << "rotate : Opposite" << endl;
else cout << "rotate : None" << endl;
}//end function rotate
void run() {
cout << "run" << endl;
}//end function rotate
void SiftNode(Cordinate* pvp, Cordinate* nwp, Cordinate* nxp) {
Cordinate Pvctr0 = { (nwp->x) - (pvp->x), (nwp->y) - (pvp->y) };
Cordinate Pvctr1 = { (nxp->x) - (nwp->x), (nxp->y) - (nwp->y) };
int det = ((Pvctr0.x)*(Pvctr1.y) - (Pvctr1.x)*(Pvctr0.y));
cout << "************(" << nwp->x << "," << nwp->y << ")*************" << endl;
rotate(det, &Pvctr0, &Pvctr1);
run();
}//end function SiftNode
int search(Anode* np, int n, int x0, int y0, int ex, int ey) {
if (np->state != FORBIDDEN) {
np->ActualCost = n;
np->HeuristicCost = (abs(ex - x0) + abs(ey - y0));
np->Score = (np->ActualCost + np->HeuristicCost);
ChangeStateSR(np);
return np->Score;
}//end if
else return -1;
}//end function search
int main() {
int X, Y;
cout << "フィールドの広さを入力:(x y) => " << flush;
cin >> X >> Y;
Anode ** anode = new Anode*[X + 2];
for (int j = 0; j<X + 2; j++) {
anode[j] = new Anode[Y + 2];
}//end for j
for (int i = 0; i<X + 2; i++) {
for (int j = 0; j<Y + 2; j++) {
ChangeStateCL(&anode[i][j]);
anode[i][j].ActualCost = 0;
anode[i][j].HeuristicCost = 0;
anode[i][j].Score = 0;
}//end for j
}//end for i
for (int n = 0; n<X + 2; n++) {
ChangeStateFB(&anode[n][0]);
ChangeStateFB(&anode[n][Y + 1]);
}//end for n
for (int n = 0; n<Y + 2; n++) {
ChangeStateFB(&anode[0][n]);
ChangeStateFB(&anode[X + 1][n]);
}//end for n
int k;
cout << "ブロックの数を入力 => " << flush;
cin >> k;
for (int n = 0; n<k; n++) {
int fx, fy;
cout << "ブロックの座標を入力:(x y) => " << flush;
cin >> fx >> fy;
if (0<fx && fx <= X && 0<fy && fy <= Y) ChangeStateFB(&anode[fx][fy]);
}//end n
int x0, y0;
cout <<"始点を入力:(x y) => " << flush;
cin >> x0 >> y0;
int ex, ey;
cout <<"終点を入力:(x y) => " << flush;
cin >> ex >> ey;
int cnt = 0;
ChangeStateOP(&anode[x0][y0]);
do {
int i, j;cnt++;
for (i = 1; i <= X; i++) {
for (j = 1; j <= Y; j++) {
if (anode[i][j].state == OPEN) {
search(&anode[i][j], cnt, i, j, ex, ey);
if (anode[i - 1][j].state != SEARCHED) ChangeStateSO(&anode[i - 1][j]);//left
if (anode[i][j + 1].state != SEARCHED) ChangeStateSO(&anode[i][j + 1]);//up
if (anode[i + 1][j].state != SEARCHED) ChangeStateSO(&anode[i + 1][j]);//right
if (anode[i][j - 1].state != SEARCHED) ChangeStateSO(&anode[i][j - 1]);//down
}//end if
}//end for j
}//end for i
for (i = 1; i <= X; i++) {
for (j = 1; j <= Y; j++) {
if (anode[i][j].state == SEMI_OPEN) {
ChangeStateOP(&anode[i][j]);
}
}//end for j
}//end for i
} while (anode[ex][ey].state != SEARCHED);
cout << endl;
cout << "**start search**" << endl;
ChangeStateSS(&anode[ex][ey]);
deque<Cordinate> DeqCordinate;
for (int n = cnt; 0<n; n--) {
int i = 0, j = 0;
for (i = 1; i <= X; i++) {
for (j = 1; j <= Y; j++) {
if (anode[i][j].state == SEMI_SELECTED) {
ChangeStateSL(&anode[i][j]);
goto flagB;
}//end if
}//end for j
}//end for i
flagB:
Cordinate a = { i, j };
DeqCordinate.push_front(a);
/*Debug
cout<<"-------("<<i<<","<<j<<")-------"<<endl;
cout<<"n-1:"<<n-1<<endl;
cout<<"anode["<<i-1<<"]["<<j<<"].ActualCost:"<<anode[i-1][j].ActualCost<<endl;
cout<<"anode["<<i-1<<"]["<<j<<"].state:"<<anode[i-1][j].state<<endl;
cout<<"anode["<<i-1<<"]["<<j<<"].score:"<<anode[i-1][j].Score<<endl;
cout<<"anode["<<i+1<<"]["<<j<<"].ActualCost:"<<anode[i+1][j].ActualCost<<endl;
cout<<"anode["<<i+1<<"]["<<j<<"].state:"<<anode[i+1][j].state<<endl;
cout<<"anode["<<i+1<<"]["<<j<<"].Score:"<<anode[i+1][j].Score<<endl;
cout<<"anode["<<i<<"]["<<j+1<<"].ActualCost:"<<anode[i][j+1].ActualCost<<endl;
cout<<"anode["<<i<<"]["<<j+1<<"].state:"<<anode[i][j+1].state<<endl;
cout<<"anode["<<i<<"]["<<j+1<<"].Score:"<<anode[i][j+1].Score<<endl;
cout<<"anode["<<i<<"]["<<j-1<<"].ActualCost:"<<anode[i][j-1].ActualCost<<endl;
cout<<"anode["<<i<<"]["<<j-1<<"].state:"<<anode[i][j-1].state<<endl;
cout<<"anode["<<i<<"]["<<j-1<<"].Score:"<<anode[i][j-1].Score<<endl;
*/
vector<int> VtrScore;
vector<int> VtrActualCost;
if (anode[i - 1][j].state == SEARCHED && anode[i - 1][j].ActualCost == n - 1) {
VtrScore.push_back(anode[i - 1][j].Score);
VtrActualCost.push_back(anode[i - 1][j].ActualCost);
}//end if
if (anode[i][j + 1].state == SEARCHED && anode[i][j + 1].ActualCost == n - 1) {
VtrScore.push_back(anode[i][j + 1].Score);
VtrActualCost.push_back(anode[i][j + 1].ActualCost);
}//end if
if (anode[i + 1][j].state == SEARCHED && anode[i + 1][j].ActualCost == n - 1) {
VtrScore.push_back(anode[i + 1][j].Score);
VtrActualCost.push_back(anode[i + 1][j].ActualCost);
}//end if
if (anode[i][j - 1].state == SEARCHED && anode[i][j - 1].ActualCost == n - 1) {
VtrScore.push_back(anode[i][j - 1].Score);
VtrActualCost.push_back(anode[i][j - 1].ActualCost);
}//end if
if (!(VtrScore.empty()) && !(VtrActualCost.empty())) {
sort(VtrScore.begin(), VtrScore.end());
int minScore = VtrScore.front();
sort(VtrActualCost.begin(), VtrActualCost.end());
int minActualCost = VtrActualCost.front();
if (anode[i - 1][j].Score == minScore && anode[i - 1][j].ActualCost == minActualCost) {
ChangeStateSS(&anode[i - 1][j]);
}//end else if
else if (anode[i][j + 1].Score == minScore && anode[i][j + 1].ActualCost == minActualCost) {
ChangeStateSS(&anode[i][j + 1]);
}//end else if
else if (anode[i + 1][j].Score == minScore && anode[i + 1][j].ActualCost == minActualCost) {
ChangeStateSS(&anode[i + 1][j]);
}//end else if
else if (anode[i][j - 1].Score == minScore && anode[i][j - 1].ActualCost == minActualCost) {
ChangeStateSS(&anode[i][j - 1]);
}//end else if
else {
cout << "unexpected situation" << endl;
}//end else
}//end if
}//end for n
cout << "***end search***" << endl;
cout << endl;
for (int j = Y; 0<j; j--) {
for (int i = 1; i<X + 1; i++) {
if (anode[i][j].state == SELECTED) {
cout << "☆" << flush;
}//end if
else if (anode[i][j].state == FORBIDDEN) {
cout << "■" << flush;
}//end else if
else {
cout << "□" << flush;
}//end else if
}//end for i
cout << endl;
}//end for j
for (int n = 0; n<X + 2; n++) {
delete[] anode[n];
}//end for n
delete[] anode;
for (auto nw=DeqCordinate.begin(); nw!=DeqCordinate.end(); ++nw) {
cout <<"("<<(*nw).x<<","<<(*nw).y<<")"<<endl;
}//end for nw
int e0, e1;
cout << "方向ベクトルの入力:(x, y) => " << flush;
cin >> e0 >> e1;
Cordinate a = {(x0-e0), (y0-e1)};
DeqCordinate.push_front(a);
auto pv = DeqCordinate.begin();
auto nw = DeqCordinate.begin();
nw++;
auto nx = DeqCordinate.begin();
nx++;
nx++;
for (int n = 0; n<DeqCordinate.size()-2; n++) {
SiftNode(&(*pv),&(*nw), &(*nx));
pv++;
nw++;
nx++;
}//end for n
return 0;
}//end function main
fig.1 実行結果
プログラムの概要
このプログラムは以下の機能から構成される。
・A*アルゴリズムによるスタートからゴールまでのノードの最短経路の探索(116行～300行)
⇒{スタートノード、ゴールノード}の座標を与え、各ノードに状態、実コスト、推定コスト及びスコアの属性を割り振りそのスコアを最小にする経路を探索する。
そして対象ノードの座標をリングバッファに格納する。
・リングバッファに格納された座標から回転処理と移動処理を単位距離のノード間に限定して行いそれを連続する。(303行～320行)
機体の{一つ前の座標、今の座標機、次の座標}を与えることにより三点の座標から二本の基底ベクトルを生成しそのベクトルの張る空間が右手系か左手系かを評価することにより、回転系の処理を実装。
この時初回の処理については例外的に機体の方向ベクトルを与える。
プログラム
以下にプログラムのリンクを示す。
Move.cpp
最後に
皆さんは悔いのないように頑張ってください。
これで私のMIRSは終わりです。