概要
本記事では，求めた最短経路情報をもとに実際の軌道を生成する際に用いるアルゴリズムを紹介する．
前提
前提として以下の仮定をおく．
- ターゲットの始点と終点などを決めて，目標位置を時間の関数として生成する
- 位置入力，速度(電圧)出力の制御系
- ターゲットの位置を時間的に変化させ，位置と角度の偏差をもとに制御入力を決定する，いわゆるターゲット追尾型の位置制御を行う
- 制御入力(速度出力)から実際の機体の速度応答までは近似的に1次遅れ系となり，(モータに対して速度制御をかけているため)その時定数は十分小さいものとする
最短経路情報から座標の抽出
マイクロマウス迷路探索シミュレータの製作高速走行時には，同じ方向に続く経路は1つの直進として扱いたいので，この経路情報から曲がり角の部分だけを抽出すればよい．
基本動作
マイクロマウスの高速走行時の基本動作は，直進・スラローム旋回の2つである． 得られた経路上の曲がり角の座標から，
直進3マス→右旋回→直進2マス→左旋回→直進10マス・・・ |
のように基本動作を組み合わせて経路を生成する．
加速度の連続性と補間曲線
マイクロマウスにおいては，タイヤの滑りをいかに抑えるか/補正するかが大きな問題となる． 急な加減速を避けることによって，タイヤの滑りをある程度抑制することができる．
まず，1つの基本動作のみについて考えよう． ターゲットの位置を時間による関数 \(r(t)\) として考えると，この関数によって与えられるターゲットの加速度は， \(\frac{d^2}{dt^2}r(t)\) である． これが連続であること，すなわち微積分学の言葉で表現すると， \(r(t)\) は \(C^2\) 級であればよい． ただし，加速度が一定値をとるようなものは現実的には使いづらいため， \(C^3\) 級以上の関数を使用することになる．
さて，次はこの加速度連続な基本動作をつなぎあわせて全体として加速度連続な経路を生成したい． すなわち，基本動作の境界での加速度・速度・位置がそれぞれ連続である必要がある．
このようなときによく用いられるのが，5次関数による補間曲線である．
5次関数は，一般式
$$
g(x)=ax^5+bx^4+cx^3+dx^2+ex+f
$$
で表され，未知数が6つ存在する．
すなわち，境界条件を6つ決めれば5次関数を一意に決定することができる．
今回の要件では，用いる境界条件は，
- \(t=0\) での位置(始点の座標)
- \(t=0\) での速度(始点での速度)
- \(t=0\) での加速度(始点での加速度)
- \(t=\tau\) での位置(終点の座標)
- \(t=\tau\) での速度(終点での速度)
- \(t=\tau\) での加速度(終点での加速度)
の6つである．
これが連続となるように基本動作をつなげることで，なめらかな目標軌道の生成が可能となる．
しかし，高次の関数になればなるほど，変曲点を多く含む可能性が高くなる． 変曲点まわりでは，速度増加が一旦止まった後再度加速が始まるといった動きをするため，加速度は連続であるもののふらふらとした速度変化を生むこととなり好ましくない． したがって，なるべく低次の関数で補間したほうがよいと考えられる．
カブトガニ野郎では，4次関数を用いて軌道補間を行なっており，境界条件を1つ見捨てていることになる．
今回は終点での加速度を無視して考えた．その理由としては，
- 基本動作はすべて加速→減速のパターンであり，終点では必ず \(\left.\frac{d^2}{dt^2}r(t)\right|_{t=\tau}<0 \)
- 直進の基本動作の後には必ず旋回の基本動作が入る
- 直進による縦滑りよりも，旋回による横滑りの方がはるかに大きいため，旋回の始点(直進の終点)における加速度の連続性はさほど重要ではない
などが挙げられる．
実装
今回の実装の指針は以下の様なものであった：
- 軌道をパラメトリックに設定したい
→関数オブジェクトの利用 - 軌道だけでなく，軌道上の進み方（緩急の付け方）を関数で設定したい
→関数オブジェクトを内包する合成関数オブジェクトの作成 - ターゲット座標を取り出す際に関数ライクに扱えるようにしたい
→operator()のオーバライド - 基本単位を1つのオブジェクトとして，キューに入れて逐次処理させたい
→合成関数オブジェクトをメンバに持つ，動作の基本単位のクラスを作成．ポリモーフィズムを利用してキューに放り込む
###クラス
Position
座標と角度を示すクラス．
class Position {
private:
public:
// 本当ならprivateにしてgetterを作るべきだが怠慢
long x;
long y;
long angle;
// コンストラクタとデストラクタ
Position():x(0),y(0),angle(0){}
Position(const Position &p){
x=p.x;
y=p.y;
angle=p.angle;
}
Position(long _x,long _y,long _angle):x(_x),y(_y),angle(_angle){}
~Position(){}
// メンバ変数の一括設定
void setVars(long _x,long _y,long _angle){ x=_x; y=_y; angle=_angle; }
// operatorをオーバライド，Position同士の加減算を定義
Position operator- (const Position& rhs){ return Position(x-rhs.x,y-rhs.y,angle-rhs.angle); }
Position operator+ (const Position& rhs){ return Position(x+rhs.x,y+rhs.y,angle+rhs.angle); }
// スカラ倍，スカラ割の定義
// templateを使うとintやfloatなど複数の型のdに対して一括して定義できる
template <typename T> Position operator/ (const T d){ return Position((T)x/d,(T)y/d,(T)angle/d); }
template <typename T> Position operator* (const T d){ return Position((T)x*d,(T)y*d,(T)angle*d); }
};EasingFunctor
緩急の付け方を規定する関数オブジェクトのクラス． 実際の時刻 \(s\) に対して，変換後の時刻 \(t(s)\) を出力する．
class EasingFunctor{
protected:
float time; // 所要時間
public:
// コンストラクタとデストラクタ
EasingFunctor(){}
EasingFunctor(float _time):time(_time){}
EasingFunctor(const EasingFunctor& obj){
time=obj.time;
}
virtual ~EasingFunctor();
// operator()をオーバライド．
// EasingFunctor ef;に対してfloat t=ef(s);できるようにする．
virtual float operator()(unsigned int s){};
// 本筋ではないもの色々
virtual void initialize(){}
void setTime(float _time){ time=_time; }
float getTime() const { return time; }
};
EasingFunctor::~EasingFunctor(){}MotionFunctor
基本動作を表す関数オブジェクトのクラス． 始点と終点のPositionから，幾何学的な軌道を生成する．EasingFunctorからの出力を時刻入力として用いることによって，軌道の幾何学的構造を変えることなく緩急だけがつく．
class MotionFunctor{
protected:
// こいつら本当ならshared_ptrとか使いたい
Position* origin;
Position* dest;
float time; // 所要時間
EasingFunctor *e; // 緩急の付け方を規定する関数
public:
// コンストラクタとデストラクタ
MotionFunctor(){}
MotionFunctor(Position _origin,Position _dest,float _time,EasingFunctor *_e){
origin=new Position(_origin);
dest=new Position(_dest);
time=_time;
e=_e;
}
MotionFunctor(EasingFunctor *_e){
e=_e;
}
MotionFunctor(const MotionFunctor& obj){
origin=obj.origin;
dest=obj.dest;
time=obj.time;
e=obj.e;
}
virtual ~MotionFunctor();
// operator()をオーバライド
// MotionFunctor mf;に対してPosition p=mf(s);とできるようにする．
virtual Position operator()(unsigned int s){}
// 本筋ではないもの色々
void destroy(){ delete origin; delete dest; delete e; }
Position getOrigin() const { return *origin; }
Position getDest() const { return *dest; }
EasingFunctor *getE() const { return e; }
void setParams(Position _origin,Position _dest,float _time){
origin=new Position(_origin);
dest=new Position(_dest);
time=_time;
e->setTime(time);
initialize();
e->initialize();
}
virtual void initialize(){}
};
MotionFunctor::~MotionFunctor(){}Target
ターゲットを表すクラス．MotionFunctorを内包していて，これをキューに入れて逐次処理する．
class Target {
private:
// 始点と終点
Position origin;
Position dest;
unsigned int time; // 所要時間
MotionFunctor *mf; // 動作を規定する関数
public:
// コンストラクタとデストラクタ
Target(Position _origin,Position _dest,unsigned int _time,MotionFunctor *_mf):origin(_origin),dest(_dest),time(_time),mf(_mf){
mf->setParams(origin,dest,time);
}
Target(const Target& t){
origin=t.origin;
dest=t.dest;
time=t.time;
mf=t.mf;
mf->setParams(origin,dest,time);
}
// 本筋ではないもの色々
Position checkOrigin() { return (*mf)(0); }
Position checkDest() { return (*mf)(time); }
Position getCurrentTarget(unsigned int t){ return (*mf)(t); }
Position getDest(){ return dest; }
MotionFunctor* getMF() { return mf; }
void destroy(){ mf->destroy(); delete mf; }
unsigned int getTime() const { return time; }
};使用例
フレームワークは以上で出来上がったので，これに対して先に述べた4次関数による補間曲線を与えることを考える． まず，一番単純に直進を表す軌道について考える．
class MotionLinear : public MotionFunctor{
private:
/*
親クラスのメンバ
Position* origin;
Position* dest;
float time; // 所要時間
EasingFunctor *e; // 緩急の付け方を規定する関数
*/
Position diff; // 始点から見た終点の位置ベクトル（と角度の差分）
public:
// コンストラクタとデストラクタ
MotionLinear(EasingFunctor *_e):MotionFunctor(_e){}
MotionLinear(Position _origin,Position _dest,float _time,EasingFunctor *_e):MotionFunctor(_origin,_dest,_time,_e){
diff=*dest-*origin;
}
MotionLinear(const MotionLinear& obj){
origin=obj.origin;
dest=obj.dest;
diff=*dest-*origin;
time=obj.time;
e=obj.e;
}
~MotionLinear(){}
// 軌道の幾何学的形状を規定する関数
virtual Position operator()(unsigned int s){
float t=(*e)(s); // 実時刻sに対して仮想の時刻t=e(s)が得られる
return *origin+(diff*(t/time)); // 仮想の時刻を使ってターゲット位置を計算
}
virtual void initialize(){
diff=*dest-*origin;
}
};この直線に対して，緩急を4次関数で与える． 4次関数 \(f(t)=at^4+bt^3+ct^2+dt+e\) について， 境界条件
- \(f(0)=0\)
- \(f(\tau)=\tau\)
- \(\left.\frac{d}{dt}f(t)\right|_{t=0}=v_0\)
- \(\left.\frac{d}{dt}f(t)\right|_{t=\tau}=v_\tau\)
- \(\left.\frac{d^2}{dt^2}f(t)\right|_{t=0}=\alpha_0\)
を考えると， $$ f(0)=e=0\\ f(\tau)=\tau(a\tau^3+b\tau^2+c\tau+d)=\tau\\ a\tau^3+b\tau^2+c\tau+d=1 $$ \begin{align*} f’(0)&=d=v_0\\ f’(\tau)&=4a\tau^3+3b\tau^2+2c\tau+v_0=v_\tau\\ f’’(0)&=2c=\alpha_0 \end{align*} から， \begin{align*} a&=\frac{2v_0+\alpha_0\tau/2+v_\tau-3}{\tau^3}\\ b&=\frac{4-v_\tau-3v_0-\alpha_0\tau}{\tau^2}\\ c&=\frac{\alpha_0}{2}\\ d&=v_0\\ e&=0 \end{align*}
導出された結果を用いると，この4次関数による緩急を実現するクラスは，EasingFunctorを継承して以下のように書ける．(制御周期が5[ms]=1/200[s]の場合)
class EasingPoly4 : public EasingFunctor{
private:
/*
親クラスのメンバ変数
float time;
*/
float a,b,c,d;
public:
EasingPoly4():EasingFunctor(){}
EasingPoly4(float _time):EasingFunctor(_time){}
EasingPoly4(float _time,float v0,float vt,float a0):EasingFunctor(_time/200.f){
a=(2.f*v0+a0*0.5f*time+vt-3.f)/time/time/time;
b=(4.f-vt-3.f*v0-a0*time)/time/time;
c=a0*0.5f;
d=v0;
}
EasingPoly4(const EasingPoly4& obj){
time=obj.time;
a=obj.a;
b=obj.b;
c=obj.c;
d=obj.d;
}
~EasingPoly4(){}
virtual float func(unsigned int s){
float sf=(float)s/200.f; // sが1増えるごとに実時間では5[ms]=1/200[s]経過
return 200.f*(a*sf*sf*sf*sf+b*sf*sf*sf+c*sf*sf+d*sf); // 実時間スケールで計算した後，200かけて元のスケールに戻す
}
};制御側との結合
どこかで
std::queue<Target*> targetQueue;として，Targetのポインタを持つキューを作成する．
基本単位ごとにキューに突っ込んでいく．
Position fdest(0,0,0); // 最新の目標位置
void setDestination(Position dest,unsigned int _time,MotionFunctor *mf){
// 作って
Target *t=new Target(fdest,dest,_time,mf);
// 放り込む
targetQueue.push(t);
// 最新の目標位置を更新
fdest.setVars(dest.x,dest.y,dest.angle);
}このように関数を作っておけば，
Position p1(0,180,0);
setDestination(
p1, // 終点
65, // 所要時間
new MotionLinear( // 直線軌道で
new EasingLinear() // 一定速度
));
Position p2(0,360,0);
setDestination(
p2, // 終点
65, // 所要時間
new MotionLinear( // 直線軌道で
new EasingPoly4( // 4次関数
65, // 所要時間
300, // 始点での速度300[mm/s]
600, // 終点での速度600[mm/s]
3000) // 始点での加速度3000[mm/s/s]
));のようにして，終点座標，所要時間と軌道の形状と速度特性だけを与えてやることでどんどんマシンが動くように作れる．
位置制御をかけることを前提としているため，キューの先頭にあるTargetの位置を取得して，それを目標値とすれば，これが実現される．
// Targetのキューが空でなければ
if(!isTargetQueueEmpty()){
time++;
// ターゲット位置取得
Position p=targetQueue.front()->getCurrentTarget(time);
// 取得した値を目標に設定
tx=p.x;
ty=p.y;
tangle=p.angle;
}
/*
...ここでtx,ty,tangleに対して位置制御をかける...
*/
// キューの先頭にあるTargetが動き終えたとき
if(!isTargetQueueEmpty() && time==(targetQueue.front()->getTime())){
time=0; // timeをリセット
// 色々破壊
targetQueue.front()->destroy();
delete targetQueue.front();
// キューから取り除く
targetQueue.pop();
}