Interface編集部
2023年1月号 マイコン制御の自動運転カー製作記 第12回
●動画1
●動画2
誌面ではリストの一部が省略されていました．
リスト全体を掲載します．
●リスト1（Program1.c）
/*
ゴールまでの最短距離にある移動可能グリッドを連続して選択していく方法
①注目グリッドの周辺グリッドの内，障害物がなく移動可能なグリッドを調べる
②移動可能なグリッドの内，ゴールとのユークリッド距離が最短のグリッドを調べる
③最短グリッドへ移動し，このグリッドを注目グリッドにする
④ゴールにたどり着くまで①～③を繰り返す
*/
void Get_Saitan_Kido(void)
{
int i,total;
int x,y;
int xg,yg;
int can_go[9];
int min_idx;
float r[9];
float min_r;
int flag=0;
Zahyo ps;
Zahyo p1;
Zahyo pm[9];
Pm_CNT=0;
ROBO_POS_G.x=START_POS.x;
ROBO_POS_G.y=START_POS.y;
ps=START_POS;
Get_Grid_Index(GOAL_POS,xg,yg); //GOAL_POSに対応するグリッドのインデックス番号を呼び出す
Get_Grid_Index(START_POS,x,y); //START_POSに対応するグリッドのインデックス番号を呼び出す
flag=0;
while(flag==0){
//****通行可能なグリッドを調べる****
for(i=0;i<9;i++){
can_go[i]=0;
r[i]=0.0;
}
min_r=9999999999;
if(G_MAP_1[x-1][y-1].num==0){
can_go[0]=1;
pm[0]=Get_Grid_Zahyo(x-1,y-1); //指定したグリッドの中心座標を返す
}
if(G_MAP_1[x ][y-1].num==0){
can_go[1]=1;
pm[1]=Get_Grid_Zahyo(x ,y-1); //指定したグリッドの中心座標を返す
}
if(G_MAP_1[x+1][y-1].num==0){
can_go[2]=1;
pm[2]=Get_Grid_Zahyo(x+1,y-1); //指定したグリッドの中心座標を返す
}
if(G_MAP_1[x-1][y ].num==0){
can_go[3]=1;
pm[3]=Get_Grid_Zahyo(x-1,y ); //指定したグリッドの中心座標を返す
}
if(G_MAP_1[x ][y ].num==0){
can_go[4]=1;
pm[4]=Get_Grid_Zahyo(x ,y ); //指定したグリッドの中心座標を返す
}
if(G_MAP_1[x+1][y ].num==0){
can_go[5]=1;
pm[5]=Get_Grid_Zahyo(x+1,y ); //指定したグリッドの中心座標を返す
}
if(G_MAP_1[x-1][y+1].num==0){
can_go[6]=1;
pm[6]=Get_Grid_Zahyo(x-1,y+1); //指定したグリッドの中心座標を返す
}
if(G_MAP_1[x ][y+1].num==0){
can_go[7]=1;
pm[7]=Get_Grid_Zahyo(x ,y+1); //指定したグリッドの中心座標を返す
}
if(G_MAP_1[x+1][y+1].num==0){
can_go[8]=1;
pm[8]=Get_Grid_Zahyo(x+1,y+1); //指定したグリッドの中心座標を返す
}
//****コスト関数が最小になるグリッドを選択する****
for(i=0;i<9;i++){
if(can_go[i]==1){
r[i]=(pm[i].x-GOAL_POS.x)*(pm[i].x-GOAL_POS.x)+(pm[i].y-GOAL_POS.y)*(pm[i].y-GOAL_POS.y);
// r=sqrtf(r);
if(r[i]<min_r&&i!=4){ //i=4の場合その場から動けなくなるので除外
min_r=r[i];
min_idx=i;
}
}
}
//****コスト関数最小となるグリッドへ移動****
total=20;
for(i=0;i<total;i++){
CAR_POS_M[Pm_CNT].x=ps.x+(pm[min_idx].x-ps.x)*(float)i/(float)total;
CAR_POS_M[Pm_CNT].y=ps.y+(pm[min_idx].y-ps.y)*(float)i/(float)total;
ROBO_POS_G.x=CAR_POS_M[Pm_CNT].x;
ROBO_POS_G.y=CAR_POS_M[Pm_CNT].y;
p1.x=ROBO_POS_G.x;
p1.y=ROBO_POS_G.y;
if(Pm_CNT<MAX_Log) Pm_CNT++;
Pm_CNT_Total=Pm_CNT;
Disp_Grid_Map_Car_Pos(); //スタート位置，途中軌道，ゴール位置の表示
}
if(min_idx==0){
x=x-1;
y=y-1;
}
if(min_idx==1){
x=x;
y=y-1;
}
if(min_idx==2){
x=x+1;
y=y-1;
}
if(min_idx==3){
x=x-1;
y=y;
}
if(min_idx==4){
x=x;
y=y;
}
if(min_idx==5){
x=x+1;
y=y;
}
if(min_idx==6){
x=x-1;
y=y+1;
}
if(min_idx==7){
x=x;
y=y+1;
}
if(min_idx==8){
x=x+1;
y=y+1;
}
if(x==xg&&y==yg) flag=1;
ps=p1; //現在の座標を記録しておく
}
}
●リスト2（Program2.c）
/*
ダイクストラ法による最短経路探索
4近傍だとタテ・ヨコのみに進む
8近傍だと斜めにも進む(斜め進みの場合の移動コストがタテ・ヨコと同等)
①スタートとゴールを決める(グリッド番号で指定)
②スタートから順次重み付けを行う(同じマス目に2個以上の重みが入る場合は小さいほうを優先)
③ゴールから重みが1ずつ減るような経路を探索する
*/
//****4近傍に重みを設定****
void Set_Weighting_Factor(int x,int y)
{
int xx,yy;
int num;
num=G_MAP_1[x][y].flag; //注目グリッドの重み
xx=x-1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y;
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>num+1) //障害物がない+定義済みの重みが新しい値以下
G_MAP_1[xx][yy].flag=num+1; //重みを更新
xx=x+1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y;
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>num+1) //障害物がない+定義済みの重みが新しい値以下
G_MAP_1[xx][yy].flag=num+1; //重みを更新
xx=x;
yy=y-1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>num+1) //障害物がない+定義済みの重みが新しい値以下
G_MAP_1[xx][yy].flag=num+1; //重みを更新
xx=x;
yy=y+1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>num+1) //障害物がない+定義済みの重みが新しい値以下
G_MAP_1[xx][yy].flag=num+1; //重みを更新
}
//****8近傍に重みを設定****
void Set_Weighting_Factor_r(int x,int y)
{
int xx,yy;
int num;
int w1,w2;
num=G_MAP_1[x][y].flag; //注目グリッドの重み
w1=num+1; //縦横のウェイト
w2=num+1; //斜めのウェイト
xx=x-1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y-1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>w2) //障害物がない+定義済みの重みが新しい値以下（斜め）
G_MAP_1[xx][yy].flag=w2; //重みを更新
xx=x;
yy=y-1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>w1) //障害物がない+定義済みの重みが新しい値以下
G_MAP_1[xx][yy].flag=w1; //重みを更新
xx=x+1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y-1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>w2) //障害物がない+定義済みの重みが新しい値以下（斜め）
G_MAP_1[xx][yy].flag=w2; //重みを更新
xx=x-1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y;
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>w1) //障害物がない+定義済みの重みが新しい値以下
G_MAP_1[xx][yy].flag=w1; //重みを更新
xx=x+1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y;
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>w1) //障害物がない+定義済みの重みが新しい値以下
G_MAP_1[xx][yy].flag=w1; //重みを更新
xx=x-1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y+1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>w2) //障害物がない+定義済みの重みが新しい値以下（斜め）
G_MAP_1[xx][yy].flag=w2; //重みを更新
xx=x;
yy=y+1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>w1) //障害物がない+定義済みの重みが新しい値以下
G_MAP_1[xx][yy].flag=w1; //重みを更新
xx=x+1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y+1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].num==0&&G_MAP_1[xx][yy].flag>w2) //障害物がない+定義済みの重みが新しい値以下（斜め）
G_MAP_1[xx][yy].flag=w2; //重みを更新
}
//****指定した重み係数のグリッドを抽出****
int Serch_Weighting_Factor(int w,Zahyo pm[])
{
int num,x,y;
num=0;
for(x=0;x<Grid_NUM_X;x++){
for(y=0;y<Grid_NUM_Y;y++){
if(G_MAP_1[x][y].flag==w){ //指定した重み係数のあるグリッド
pm[num].x=x; //指定した重み係数のあるグリッドの座標
pm[num].y=y; //指定した重み係数のあるグリッドの座標
num++; //該当グリッドの数
}
}
}
return num;
}
//****4近傍グリッドで重みが1減っているグリッドを返す****
int Serch_Weighting_Factor_2(int x,int y,Zahyo pm[])
{
int xx,yy;
int num,num2;
int w;
w=9999;
num2=0;
num=G_MAP_1[x][y].flag; //注目グリッドの重み
xx=x-1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y;
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x+1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y;
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x;
yy=y-1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x;
yy=y+1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
return num2;
}
//****8近傍グリッドで重みが1減っているグリッドを返す****
int Serch_Weighting_Factor_2r(int x,int y,Zahyo pm[])
{
int xx,yy;
int num,num2;
int w;
w=9999;
num2=0;
num=G_MAP_1[x][y].flag; //注目グリッドの重み
xx=x;
yy=y-1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x-1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y;
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x+1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y;
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x;
yy=y+1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x-1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y-1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき(斜め)
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x+1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y-1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき（斜め）
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x-1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y+1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき（斜め）
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
xx=x+1;
xx=Limit_Int(xx,Grid_NUM_X-1,0);
yy=y+1;
yy=Limit_Int(yy,Grid_NUM_Y-1,0);
if(G_MAP_1[xx][yy].flag==num-1){ //近傍グリッドの重みが注目グリッドの重み-1であるとき（斜め）
pm[num2].x=xx;
pm[num2].y=yy;
num2++;
}
return num2;
}
/*
ダイクストラ法でパスを求める
grid_sp_num:1グリッド(100mm)進む時間(100にセットすると100msecで100mm移動=1000mm/sの速度)
*/
void Get_Dijkstra_Pass(int grid_sp_num)
{
int i,j,w,num,num_max,num_old,d;
int x,y,x0,y0;
int xg,yg,xg0,yg0;
int flag=0;
int total;
Zahyo pm[1000];
Zahyo ps;
//****前処理****
num=0;
for(x=0;x<Grid_NUM_X;x++){
for(y=0;y<Grid_NUM_Y;y++){
G_MAP_1[x][y].flag=999; //全フラグを999に塗りつぶす
if(G_MAP_1[x][y].num!=0) num++; //障害物があるグリッドの数を数える
}
}
num_max=num;
ps=START_POS;
ROBO_POS_G.x=START_POS.x;
ROBO_POS_G.y=START_POS.y;
Get_Grid_Index(GOAL_POS,xg,yg); //GOAL_POSに対応するグリッドのインデックス番号を呼び出す
Get_Grid_Index(START_POS,x,y); //START_POSに対応するグリッドのインデックス番号を呼び出す
G_MAP_1[x][y].flag=0; //スタート地点の重みをゼロ
w=0; //重みの初期値
Pi();
//****スタート地点から全領域へ重みを付ける****
x0=x; //スタート地点を記録する
y0=y; //スタート地点を記録する
flag=0;
num_old=999; //num_oldの初期値
while(flag==0){
LED_GREEN_Brink(); //緑LEDを明滅
LED_RED_Brink(); //赤LEDを明滅
num=Serch_Weighting_Factor(w,BUF_Zahyo);
for(i=0;i<num;i++){
x=BUF_Zahyo[i].x; //注目グリッドのインデックス
y=BUF_Zahyo[i].y; //注目グリッドのインデックス
Set_Weighting_Factor_r(x,y); //注目グリッドの8近傍グリッドの重みを確定する
}
// Disp_Grid_Map5(); //ロードした地図データを表示(flagを反映した白黒表示)
// Sleep(1000); //
//****ウェイトのアップデート，未探索グリッドの数を調べる****
w++;
num=Serch_Weighting_Factor(999,BUF_Zahyo); //未探索グリッドの数
if(num<=num_max) flag=1; //全部探索が終わった
d=num-num_old;
num_old=num;
if(d==0) flag=1; //numが変化しなくなった
}
Pi();
//****重みゴールから逆にたどり経路を求める****
xg0=xg;
yg0=yg;
num_max=0;
flag=0;
pm[0]=Get_Grid_Zahyo(xg0,yg0);
num_max++;
while(flag==0){
num=Serch_Weighting_Factor_2r(xg,yg,BUF_Zahyo);
if(num>=1){
xg=BUF_Zahyo[0].x;
yg=BUF_Zahyo[0].y;
pm[num_max]=Get_Grid_Zahyo(xg,yg);
num_max++;
}
else{
G_MAP_1[xg][yg].flag=9999; //探索終了
xg=xg0;
yg=yg0;
}
G_MAP_1[xg][yg].gx=9999; //探索終了サイン
// Disp_Grid_Map5(); //ロードした地図データを表示(flagを反映した白黒表示)
// Sleep(1000);
if(xg==x0&&yg==y0) flag=1;
}
Pi();
//****求まった経路をスタートからたどる****
total=grid_sp_num;
Pm_CNT=0;
for(j=0;j<num_max-1;j++){
for(i=0;i<total;i++){
CAR_POS_M[Pm_CNT].gx=ps.x+(pm[num_max-j-2].x-ps.x)*(float)i/(float)total; //スタート位置を1個飛ばす
CAR_POS_M[Pm_CNT].gy=ps.y+(pm[num_max-j-2].y-ps.y)*(float)i/(float)total; //
if(Pm_CNT<MAX_Log) Pm_CNT++;
Pm_CNT_Total=Pm_CNT;
}
ps=pm[num_max-j-2];
ROBO_POS_2[j].x=CAR_POS_M[Pm_CNT-1].gx;
ROBO_POS_2[j].y=CAR_POS_M[Pm_CNT-1].gy;
ROBO_POS_2[j].c=90.0;
}
SCAN_NUM_ALL=num_max-1;
}
