自動運転の続き

自動運転の模索後

以前に、自動運転の模索において、自動運転の取り掛かりについて触れました。すでに運転手のいない自動運転タクシーが実験段階にあります。また、外国では公道走行中の自動運転車が思い通りに走行せず、事故を引き起こしたニュースが報道されています。

autodrivetaxi

前回の記述を発展させ、リサジュー曲線(五葉線)とばら曲線上を走行する車の自動制御について考察します。

制御方法

teamovingここで行われる制御は、江戸時代の茶運び人形などにも採用された初歩的な方法です。数メートル離れた客に茶菓子を出すロボットに採用されました。

客は不動であり、あらかじめ客の位置がロボットに教え込まれていました。何かの拍子に客が移動しても追従できません。

お互いに、一定の法則に従った車の動きは相手に逐一、知らされ衝突が起きそうになると、一方の車が進路をゆずる方法です。

2台の車の調整を図る今回の制御は茶菓子もてなしロボットよりは進化していると思われます。

自動運転制御コード

<html>
<head>
<input type="button" onClick=up200() value="updown " style="width:70px;height:30px;">
<input type="button" onClick=t3set() value="速 度 " style="width:70px;height:30px;">
<input type="button" onClick=R1set() value="ずれ差 " style="width:70px;height:30px;">
<input type="button" onClick=T1set() value="安全度 " style="width:70px;height:30px;">
<input type="button" onClick=start() value="開 始 " style="width:70px;height:30px;background-color:pink;">
<font size=5 color='magenta'>自動運転の模索</font><font color='black'><BR></font>
<script type="text/javascript" charset="Shift_JIS">
const HK=30;
const SIZ1=210;
const SIZ2=210;
const ZX=220;
const ZY=220;
const sc=0.0025;	//②ばら曲線
const co2 = ["blue", "yellow"];		//color
const th2 = [3, 2];					//thickness, 6-4, 4.5-3
const co1 = ["tomato", "seagreen"];	//color
const DrvMode=["手動運転", "自動運転"];
const Control=["衝突発生", "衝突回避"];

var ln=0;			//表示行
var t3=4;			//遅延処理
var R1=0.0;			//遅れ位相
var T1=8.0;			//衝突危険域 2.0 4.0 1.9418
var bx = [];		//バラ曲線の軌跡x値を格納, 801
var by = [];		//バラ曲線の軌跡y値を格納
var rx = [];		//リサジュー曲線の軌跡x値を格納, 4007
var ry = [];		//リサジュー曲線の軌跡y値を格納
var driveMode=0;	//0:なりゆき運転, 1:自動制御運転
var max2w=max2=0;	//ばら曲線のポイント数
var max1w=max1=0;	//リサジュー曲線のポイント数
var baraCount=0;	//ばら曲線の連続描画回数
var lissCount=0;	//リサジュー曲線の連続描画回数
var status=0;		//衝突なし
var updown=1;		//up/down
var ctx, wx, wy, vx, vy, first, p, q, img1;

var r=R1;			//0~1.0           |------------128:①リサジュー曲線(五葉線)実行中
var s=0.0;			//0~1.0           ||----------- 64:②ばら曲線実行中
var retry=flag=0;	//flag:98765432109876543210
                                   //||-------------256:②ばら曲線
  img1 = new Image();              //|--------------512:①リサジュー曲線(五葉線)実行終了
  img1.src = "http://aidesign.lolipop.jp/wp-content/uploads/2016/07/AutoDriving.png";//ばら曲線,早めにロードしておく

function initial(){
  var canvas1 = document.getElementById('sample');	//描画コンテキストの取得実行終了
  if (canvas1.getContext) {
	ctx = canvas1.getContext('2d');					//次から具体的な描画内容を指定する
	ctx.drawImage(img1, 5, 10, 440, 357);			//ばら曲線を描画
	ctx.font = "26px Arial";
	ctx.fillText("開始ボタンをクリックして運転開始です。", 3, ZY*2-40);
	ctx.font = "12px Arial";
	ctx.fillText("開始ボタン以外は拡張用です。(ver0.09)", 5, ZY*2-10);
  }
  else	alert("Canvas-error.");
}

function start() {				//開 始
	r=R1;			//0~1.0
	s=0.0;			//0~1.0
	baraCount = lissCount = max1w = max2w = flag=status = retry = driveMode = ln = 0;
	ctx.clearRect(0, 0, 545, 440);
//②ばら曲線の軌跡を求める
  while(s<=10.0015){
	q = s * Math.PI;
	p = 0.4 * q;
	bx[max2]   = ZX + SIZ1 * Math.sin(p) * Math.cos(q);	//x
	by[max2++] = ZY - SIZ1 * Math.sin(p) * Math.sin(q);	//y
	s += sc;											//function1
  }

//①五葉線
var th=0.0;			//0~2.0
var b=0.7;			//para-1
var c=-3;			//para-2
var d=5;			//para-3
var st=0.001;		//step
  for(max1=0; ;){
	var t = th * Math.PI;							//theta
	q = t * d;
	p = 1 + Math.sin(c * q) / 4;					//a
	r = p / 2 * (1 + b * Math.sin(q));				//r
	rx[max1] = ZX + SIZ2 * r * Math.cos(t);			//x
	ry[max1++] = ZY + 25 - SIZ2 * r * Math.sin(t);	//y
	th += st;										//linear function
	if(th > 2.0005)	break;
  }

	first = new Date().getTime();
	flag = 64 | 128;								//②ばら曲線実行中,①五葉線実行中
	head12();
	draw12();										//1:blue, 2:red

	ctx.fillStyle = 'midnightblue';					//塗りつぶしの色
	ctx.font = "14px Arial";
	ctx.fillText("\xA9Aidesign,2016", ZX+HK+70, 20);
}	// end of start()

function up200(){
}

function t3set(){
}

function R1set(){
}

function T1set(){
}

function autoDrive(){
	r=R1;			//0~1.0
	s=0.0;			//0~1.0
	baraCount=lissCount=max1w=max2w=status=retry=0;
	flag = 64 | 128;							//②ばら曲線実行中,①リサジュー曲線(五葉線)実行中
	first = new Date().getTime();
	ctx.clearRect(0, 0, 460, 440);
	++driveMode;								//自動制御運転
	++ln;head12();
	draw12();									//1:blue, 2:red
}

function head12(){
		msg(DrvMode[driveMode]);++ln;
		ctx.clearRect(5, 13, 100, 26);
		ctx.fillStyle = 'darkblue';				//塗りつぶしの色
		ctx.font = "24px Arial";
		ctx.fillText(DrvMode[driveMode], 5, 32);
}

function draw12(){								// <<<<<***** blue ②ばら曲線 64, 256 *****>>>>>
	if((flag&256)==0){	//終了済みか
	  if(max2w<max2){
		vx = bx[max2w];
		vy = by[max2w++];
		var ct2 = baraCount % 2;
		dot(vx, vy, co2[ct2], th2[ct2]);
	  }
	  else{				//終了
		baraCount++;	//ばら曲線の連続描画回数
		//alert("ばら曲線="+flag+" bCt="+baraCount+" lCt:"+lissCount+" MAX1W:"+max1w+" max1="+max1);
		if(lissCount==0){	//①リサジュー曲線(四葉線)が終わっていない
			max2w = 0;
		}
		else{
			flag -= flag % 256;							//flag &= 0xffffff00;
			flag |= 256;
			vx = vy = 999;
			//alert("ばら曲線描画終了 MAX2W:"+max2w+" max2="+max2);
		}
	  }
	}
// <<<<<***** function draw1(), blue ①リサジュー曲線(五葉線)128, 512 *****>>>>>
	if((flag&512)==0){	//終了済みか
	  if(max1w<max1){
		wx = rx[max1w];
		wy = ry[max1w];
		var wk = control2(wx, wy, vx, vy);				//衝突か
		if(wk==0)	max1w++;							//0なら衝突は起きない
		else{											//1なら衝突が起きる状態
			ring("select06");							//ピッ
			if(driveMode==0){
					pentagon(ctx, HK, wx, wy, 'darkmagenta');	//手動運転
					msg(Control[driveMode]);					//衝突発生
			}
			else{												//自動運転
				if(status==0)	retry = 0;
				ctx.fillStyle = ctx.strokeStyle = 'darkgreen';	//線のカラー設定
				ctx.font = "16px Arial";
				ctx.clearRect(wx-60, wy-10, 28, 18);
				ctx.fillText("時間調整" + (retry+1) + '回', wx-123, wy+4);
				ctx.beginPath();
				ctx.lineWidth = 2;
				ctx.arc(wx, wy, HK, 0, Math.PI*2, true);
				ctx.stroke();

				msg(Control[driveMode]+":"+retry);				//衝突回避
				++retry;
			}
			//console.log("STATUS="+status+" wk="+wk+" CT="+retry);
		}
		status = wk;
		var ct1 = lissCount % 2;
		dot(wx, wy, co1[ct1], th2[ct1]);
	  }
	  else {
		lissCount++;	//リサジュー曲線の連続描画回数
		if(baraCount==0){	//②ばら曲線が終わっていない
			max1w = 0;
		}
		else{
			flag -= flag % 256;					//flag &= 0xffffff00;
			flag |= 512;
			wx = wy = 0;
		}
	  }
	}
/////////////////////////////////////////////////////////////////////
	var time = new Date().getTime() - first;	//実行時間を計測
	ctx.fillStyle = "green";					//塗りつぶしの色
	ctx.font = "14px Arial";
	ctx.clearRect(2, 40, 78, 16);
	var tm = "     "+time+"ミリ秒";
	ctx.fillText(tm.substr(tm.length-8, 8), 5, 55);
  if(flag < 768)	setTimeout(draw12, t3);
  else{
	//alert("driveMode="+driveMode);
	if(driveMode==0)	setTimeout(autoDrive, 1000);
	else				msg("運転終了");		//すべて終了
  }
}			// end of draw12()

function dot(x1, y1, col, thickness){
	ctx.fillStyle = col;						//塗りつぶしの色は赤
	ctx.fillRect(x1, y1, thickness, thickness);
}

function control2(gx, gy, hx, hy){				//衝突チェック-自動制御処理
		var w = 0;
		var ux = gx - T1;
		var px = gx + T1;
		var uy = gy - T1;
		var py = gy + T1;
		//console.log("VX=%d VY=%d  U1=%d V1=%d  U2=%d V2=%d", hx, hy, ux, px, ux, py);
		if(hx>=ux){
		  if(hx<=px){
			if(hy>=uy){							//衝突が起きる
			  if(hy<=py){						//衝突発生
				w = 1;							//増分なし、衝突回避に時間調整
			  }
			}
		  }
		}
	return w;
}

function pentagon(ctx, hk, px, py, color){
const DT=36;							//5角形, DT=360/(5*2)
const R=0.382;
var x=[];
var y=[];
	for(var i=0; i<10; ++i){
		var t=(i*DT+90)*Math.PI/180.0;
		if(i % 2){
			x[i]=R * Math.cos(t);
			y[i]=R * Math.sin(t);
		}
		else{
			x[i]=Math.cos(t);
			y[i]=Math.sin(t);
		}
		x[i] = px + x[i] * hk;
		y[i] = py - y[i] * hk;
	}
	ctx.beginPath();					//パスの描画を始める
	ctx.moveTo(x[0], y[0]);				//線の開始位置 (xの座標値 , yの座標値)
	for(var j=1; j<10; ++j)	ctx.lineTo(x[j], y[j]);
	ctx.lineTo(x[0], y[0]);
	ctx.closePath();
	ctx.fillStyle = color;				//線のカラー設定
	ctx.fill();							//線の終了
}

function ring(tune){
	document.getElementById(tune).play();
}
function msg(tx){
		ctx.fillStyle = 'darkgreen';				//塗りつぶしの色
		ctx.font = "14px Arial";
		ctx.fillText(tx, 460, (ln++)*14+14);
}

window.onload = initial;
</script>
</head>
<body>
<canvas id="sample" width="550" height="440" style="background-color:snow;border:1px solid green;float:left;"></canvas>
</body>
<audio id="select06" preload="auto"> <source src="http://aidesign.lolipop.jp/wp-content/uploads/2014/11/lock3.wav"> </audio>
</html>

自動運転例

自動運転の模索

コメントを残す

下記項目は必須ですがメールアドレスは公開されません。名前はニックネームをお使いください。

CAPTCHA