SyntaxHighlighter

---SyntaxHighlighter ウィジェット---

2016年9月1日木曜日

ポテンシャル法のC言語での実装例

ロボットの障害物回避などに使われる、ポテンシャル法のC言語での実装例
(あっているかは不明なので、参考程度に)


なんで調べても数式ばっかりで実装例が出てこないんだろう?


*ポテンシャル場の確認
#include <stdio.h>
#include <math.h>

double get_pot(double x, double y)
{
const double Enemy1_x = 5.;
const double Enemy1_y = 10.;

const double Enemy2_x = -10.;
const double Enemy2_y = -20.;

const double Goal_x = 17.;
const double Goal_y = 0.;

const double Weight_Enemy1 = 1.0;
const double Weight_Enemy2 = 1.0;
const double Weight_Goal = 5.0;

//スカラー場なのでx,y成分共に同じ
double Enemy1_pot = 1.0 / sqrt((x - Enemy1_x)*(x - Enemy1_x) + (y - Enemy1_y)*(y - Enemy1_y));
double Enemy2_pot = 1.0 / sqrt((x - Enemy2_x)*(x - Enemy2_x) + (y - Enemy2_y)*(y - Enemy2_y));
double Goal_pot = -1.0 / sqrt((x - Goal_x)*(x - Goal_x) + (y - Goal_y)*(y - Goal_y));

double pot = Enemy1_pot * Weight_Enemy1 + Enemy2_pot * Weight_Enemy2 + Goal_pot * Weight_Goal;
return pot;
}

int main(void)
{
FILE *fp = fopen("output.txt", "w");
if (fp == NULL)
{
printf("Err\n");
return -1;
}

printf("wait...\n");

const double delta = 0.1;
const double max = 1.0; //上限(発散部分除外)
// const double zoom = 10.;
for (double x = -30; x < 30; x += delta)
{
for (double y = -30; y < 30; y += delta)
{
double pot = get_pot(x, y);
if (pot > max)
pot = max;
if (pot < -max)
pot = -max;


fprintf(fp, "%lf,%lf,%lf\n", x, y, pot);
}
fprintf(fp, "\n");//pm3dは空行がないと補完しない
}
fclose(fp);
printf("Done\n");

return 0;
}


*GNUPLOTスクリプト
set pm3d corners2color max
set pm3d depthorder
set datafile separator ","
set xrange[-30:30]
set yrange[-30:30]
#set view map
splot "output.txt" with pm3d


*結果


*勾配ベクトル場の確認
#include <stdio.h>
#include <math.h>

double get_pot(double x,double y)
{
const double Enemy1_x = 5.;
const double Enemy1_y = 10.;

const double Enemy2_x = -10.;
const double Enemy2_y = -20.;

const double Goal_x = 17.;
const double Goal_y = 0.;

const double Weight_Enemy1 = 1.0;
const double Weight_Enemy2 = 1.0;
const double Weight_Goal = 5.0;

//スカラー場なのでx,y成分共に同じ
double Enemy1_pot = 1.0 / sqrt((x - Enemy1_x)*(x - Enemy1_x) + (y - Enemy1_y)*(y - Enemy1_y));
double Enemy2_pot = 1.0 / sqrt((x - Enemy2_x)*(x - Enemy2_x) + (y - Enemy2_y)*(y - Enemy2_y));
double Goal_pot = -1.0 / sqrt((x - Goal_x)*(x - Goal_x) + (y - Goal_y)*(y - Goal_y));

double pot = Enemy1_pot * Weight_Enemy1 + Enemy2_pot * Weight_Enemy2 + Goal_pot * Weight_Goal;
return pot;
}

int main(void)
{
FILE *fp = fopen("output.txt", "w");
if (fp == NULL)
{
printf("Err\n");
return -1;
}

printf("wait...\n");

const double delta = 1.2;
// const double zoom = 10.;
for (double x = -30; x < 30; x += delta)
{
for (double y = -30; y < 30; y += delta)
{
//勾配ベクトル算出 -grad(P)
double vx = -(get_pot(x + delta, y) - get_pot(x, y))/delta;
double vy = -(get_pot(x, y + delta) - get_pot(x, y))/delta;
double v = sqrt(vx*vx + vy*vy);

// vx *= zoom;
// vy *= zoom;
//見やすさのためにベクトルを正規化
vx /= v;
vy /= v;

// printf("%lf,%lf,%lf\n", x, y);
if(!isnan(vx) && !isnan(vy)) //無効な値は除外
fprintf(fp, "%lf,%lf,%lf,%lf,%lf\n", x, y, vx, vy,v);
}
// fprintf(fp, "\n");//pm3dは空行がないと補完しない
}
fclose(fp);
printf("Done\n");

return 0;
}


*GNUPLOTスクリプト
set datafile separator ","
set xrange[-30:30]
set yrange[-30:30]
set size ratio 1
set palette rgb 33,13,10
#plot "output.txt" u 1:2:3:4:(sqrt($3*$3+$4*$4)) w vector lc palette ti ""
plot "output.txt" u 1:2:3:4:5 w vector lc palette ti ""


*結果

*ポテンシャル法による障害物回避
#include <stdio.h>
#include <math.h>

double get_pot(double x, double y)
{
const double Enemy1_x = 5.;
const double Enemy1_y = 10.;

const double Enemy2_x = -10.;
const double Enemy2_y = -20.;

const double Goal_x = 17.;
const double Goal_y = 0.;

const double Weight_Enemy1 = 1.0;
const double Weight_Enemy2 = 1.0;
const double Weight_Goal = 5.0;

//スカラー場なのでx,y成分共に同じ
double Enemy1_pot = 1.0 / sqrt((x - Enemy1_x)*(x - Enemy1_x) + (y - Enemy1_y)*(y - Enemy1_y));
double Enemy2_pot = 1.0 / sqrt((x - Enemy2_x)*(x - Enemy2_x) + (y - Enemy2_y)*(y - Enemy2_y));
double Goal_pot = -1.0 / sqrt((x - Goal_x)*(x - Goal_x) + (y - Goal_y)*(y - Goal_y));

double pot = Enemy1_pot * Weight_Enemy1 + Enemy2_pot * Weight_Enemy2 + Goal_pot * Weight_Goal;
return pot;
}

int main(void)
{
FILE *fp = fopen("output.txt", "w");
if (fp == NULL)
{
printf("Err\n");
return -1;
}

printf("wait...\n");

//出発地点
double x = -10.;
double y = 20.;

const double delta = 0.1;
const double speed = 0.1;
for (int i = 0; i < 1000;i++)
{
//勾配ベクトル算出 -grad(P)
double vx = -(get_pot(x + delta, y) - get_pot(x, y)) / delta;
double vy = -(get_pot(x, y + delta) - get_pot(x, y)) / delta;
double v = sqrt(vx*vx + vy*vy);


//ベクトルを正規化
vx /= v / speed;
vy /= v / speed;

//移動反映
x += vx;
y += vy;

fprintf(fp, "%lf,%lf,%lf,%lf,%lf\n", x, y, vx, vy, v);
}
fclose(fp);
printf("Done\n");

return 0;
}


*GNUPLOTスクリプト
set datafile separator ","
set xrange[-30:30]
set yrange[-30:30]
set size ratio 1
set palette rgb 33,13,10
#plot "output.txt" u 1:2:3:4:(sqrt($3*$3+$4*$4)) w vector lc palette ti ""
plot "output.txt" u 1:2:3:4:5 w vector lc palette ti ""


*結果

追記: コメントにスカラー場なので」と書いてしまっているが、x,yによらず同じ形状なので、が正しいか。

0 件のコメント:

コメントを投稿