2012年6月28日(木)19:55(+0900)

クロソイド曲線をC言語を用いて複数の数値積分で求めてみた

クロソイド曲線というのは、Wikipediaの記述を引用すれば、「車の速度を一定にし、ハンドルを一定の角速度で回したときに車が描く軌跡」で、道路や鉄道の緩和曲線（直線区間からカーブに入る部分など）に使われています。

クロソイド曲線は、始点からの曲線長をL、その位置での曲率半径をRとして、

RL = A² (Aは定数)

またはr=R/A、l=L/Aと無次元化して

rl = 1

で表されますが、xy平面上にこの曲線を描こうとすると、座標はlの関数では

という容易には計算し難い形になってしまいます。また、この式を実際にテイラー展開で計算しようとした場合、l²⁰程度の項まで含めても比較的短距離で発散してしまうようです（参考：「クロソイド曲線について」様）。

さて、ここでは数値積分によりクロソイド曲線を求めてみます。数値積分は最初に書いた「ハンドルを一定の角速度で回したときに。。。」という感覚に直感的に近い手法であり、また微妙に癖がありますがうまくやれば精度よく長距離でのクロソイド曲線を求められます。

まず、単位ベクトルv=(v_x, v_y)に向かって走っている車が座標r=(x, y)において一定の曲率半径Rで左に曲がった時の、車が距離dLだけ進んだ時点での座標r'と進行方向の単位ベクトルv'を求めてみます。これは、座標をr=x+iyという複素平面に置き換えると、

r' = r + Rv(1-e^idL/R)
v' = ve^idL/R

すなわちrとvの変化量は

dr = Rv(1-e^idL/R)
dv = v(e^idL/R-1)

となり、xy座標に戻すと

dx = R(v_xsin(dL/R) - v_y(1-cos(dL/R))
dy = R(v_x(1-cos(dL/R)) + v_ysin(dL/R)

dv_x = v_x(cos(dL/R)-1) - v_ysin(dL/R)
dv_y = v_xsin(dL/R) + v_y(cos(dL/R)-1)

となります。また、これらはdL/Rが十分小さいとすると

dx = v_xdL
dy = v_ydL

dv_x = v_ydL/R
dv_y = -v_xdL/R

と近似できます。

次に、数値積分の方法については、ごく単純な積分

r_i+1 = r_i + v_idL
v_i+1 = v_i + dv_i

と、速度ベルレ法による積分

r_i+1 = r_i + v_idL + dv_i/2
v_i+1 = v_i + (dv_i + dv_i+1)/2

を考えてみます。

以下に、クロソイド曲線を数値積分で計算するC言語のプログラムを載せます。

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

int main(int argc, char *argv[])
{
	double x = 0, y = 0;
	double dx = 1, dy = 0;
	double L0 = 100, R0 = 200;
	double A2 = L0*R0;
	double dL = 1.0;
	double ddx = 0.0, ddy = 0.0;
	double L, R, dtheta = 0.0, sint, cost;

	if(argc == 4) {
		L0 = atof(argv[1]);
		R0 = atof(argv[2]);
		dL = atof(argv[3]);
	}
	printf("# L0:%f R0:%f (dL:%f)\n", L0, R0, dL);

	for(L=0;L<L0*20;L+=dL)
	{
#ifdef VERLET
		x += dx*dL + ddx;
		y += dy*dL + ddy;
		dx += ddx;
		dy += ddy;
		if(L+dL>0) {
			R = A2/(L+dL);
			dtheta = dL/R;
		}
		else {
			dtheta = 0.0;
		}
	#ifdef SIMPLE
		ddx = -dy*dtheta*0.5;
		ddy = +dx*dtheta*0.5;
	#else
		sint = sin(dtheta);
		cost = cos(dtheta);
		ddx = (dx*(cost-1.0) - dy*sint)*0.5;
		ddy = (dx*sint + dy*(cost-1.0))*0.5;
	#endif
		dx += ddx;
		dy += ddy;
#else
		if(L>0) {
			R = A2/L;
			dtheta = dL/R;
		}
		else {
			dtheta = 0.0;
		}
		x += dx*dL;
		y += dy*dL;
	#ifdef SIMPLE
		dx -= dy*dtheta;
		dy += dx*dtheta;
	#else
		sint = sin(dtheta);
		cost = cos(dtheta);
//		x += R*(dx*sint - dy*(1.0-cost));
//		y += R*(dy*sint + dx*(1.0-cost));
		dx = dx*cost - dy*sint;
		dy = dx*sint + dy*cost;
	#endif
#endif

		printf(" %f %f\n", x, y);
	}

	return 0;
}

使い方は、実行する際に三つの引数を取り、始点からの曲線長が第一引数L0の点において曲率半径が第二引数R0であるようなクロソイド曲線を、始点から曲線長20×L0まで、第三引数dL刻みで描きます。

例えば

./a.out 100 200 1.0

のようにすると、距離100において曲率半径200であるようなクロソイド曲線を1.0刻みで描きます。

コンパイルする時にオプションとして-DVERLETをつけ

gcc clothoid.c -lm -DVERLET

のようにすると、数値積分として速度ベルレ法を用いたものとなり、つけない場合は上記で示したごく単純な積分になります。また、オプションとして-DSIMPLEをつけ

gcc clothoid.c -lm -DSIMPLE

のようにすると、dvの値として近似した値

dv_x = v_ydL/R
dv_y = -v_xdL/R

を用い、つけない場合は近似前の

dv_x = v_x(cos(dL/R)-1) - v_ysin(dL/R)
dv_y = v_xsin(dL/R) + v_y(cos(dL/R)-1)

を用います。

以下に、距離100において曲率半径200であるようなクロソイド曲線を1.0刻みで描いた結果を、それぞれの積分方法について示します。

まずは、最も精度が良いと思われる、速度ベルレ法を用いdvの値として近似値を用いなかった結果を示します。

曲線が徐々にカーブを強くしながらある一点に向かって収束していっており、クロソイド曲線をよく再現できていることが分かります。

次に、速度ベルレ法を用い、dvの値として近似値を用いた結果を示します。

結果は、途中で発散的振る舞いを示してしまっています。最初のほうはうまくいっていますが、途中から徐々にvの値が大きくなり、結果的に発散してしまったようです。

次に、単純な積分を用い、dvの値として近似値を用いなかった結果を示します。

結果は一見うまくいっているようですが、渦巻き部分を見ると曲線がある一点に急速に収束していることが分かります。これは、先ほどの速度ベルレ法で近似値を用いた場合とは逆で、vの値がどんどん小さくなってしまった結果のようです。

最後に、単純な積分を用い、dvの値として近似値を用いた結果を示します。

結果は、最初に示した「速度ベルレ法を用い、dvの値として近似値を用いなかった」結果とよく似ており、かなり妥当な値が出ているものと考えられます。dvの値として近似値を用いたほうが近似していない場合よりも妥当な値が出る、というのは一見奇妙ですが、恐らく単純な積分を用いたことによる誤差と、dvを近似したことによる誤差とがうまく相殺したためではないかと思われます。

なお、以上に示した差異の他に、数値積分の方法によってx軸方向に多少のズレが生じます。

芸軌社公式ブログ