忍者ブログ

明日の設計図

たまにロボットを考えるブログ・・・。

[PR]

×

[PR]上記の広告は3ヶ月以上新規記事投稿のないブログに表示されています。新しい記事を書く事で広告が消えます。

4自由度ヘビの軌道生成?②

ある軌道上に決まった長さのヘビロボの関節をのせて移動させる。

と、すべての関節点は関節間距離を保ったまま、軌道上を通過する必要がありますので、

全開のルールに基づき
こんな関数を

こんな風にループで回して

こんなグラフがかけます。

あとは、ヘビの全長とsinカーブの周期をどんな比にするかでしょうか?

拍手[0回]

PR

4自由度ヘビの軌道生成?①

くねくね移動がうまくいかないので、
上手くいくくねくね軌道を幾何学的に決定します。


偉い人から
「蛇行軌道にすべての車輪がのっかっていて、接線の方向を向いているべき」
とのお話を頂きましたので、それを検討します。


①sin関数にのっかる
②車輪が軌道にのるように角度を決定する
③もうひとつの車輪も同様に角度を決定する

だと思いましたが!
良く考えると・・・
平面的には2自由度、3節あります。
1個目の車輪は、のせられます。
2個目は?

1個目の車輪の位置と姿勢、例えば、
x=0,y=0,θ=45°
とかだとします。

すると、1個目の関節の位置は
x=cos45,y=sin45
です。

そこから1つめのモータの角度を変えて、
2個目の車輪を軌道に乗るように決めます。

すると、角度は接線方向になりません?!

困りました。
モータ1個では、位置&角度を決めるのは無理で、どちらか一方しか決められません。

困りました。

でもまあ、仕方ないので、接線は諦めることにしまして
各関節が、軌道に乗るような角度を決めていこうと思います。




と思いましたが、これも意外と大変。

3節あるヘビロボットの1節目のリンクだけを考えます。
このリンクの両端点がsin関数上に乗ってくれればいいのですが・・・
どうやって載せるんでしょう?
リンクを直線だと思って、x軸からの傾きを決める他ありません。

これはつまり、中心がsin関数上の円とsin関数の交点を求める問題です。

が、そのとき方については僕は収束計算以外の方法を知りません。
例えば
y=0,x=0からx=x,y=sinx
までの距離は、平方根の中に三角関数の2乗があるというこれまた難しい。

で、
その距離=リンクの長さ
という方程式を解く必要があるわけですが、
sqrt ( x*x + sinx*sinx ) = リンク長さ

うんむずかしい
。。。


プログラムを書く感じです。

・sin関数1周期を移動量、振幅を移動量にして、m単位でロボットの実現できるサイズで用意します。それを軌道関数(x)にします。
・xを入れます。→軌道関数(x)がもとまります。
・始点を決めます。
・始点からxを少し増やして、軌道関数を求めます。で距離を求めます。
・・・上を距離がいい具合になるまでやります。そのときのxをx1として覚えます。
・x1からすこしずつxを増やし、またちょうどいい距離までやってそれをx2とします。
・同様の方法でx3を決定します。
・・・これで、一番最初のヘビの形が決まりました。
・これを軌道関数1周期分計算します。
・データテーブルとして保存します。
・mbedに入れます。

これで動くんじゃね。









拍手[0回]

4自由度構成のヘビの移動

ヘビを実際に移動させるには、
ヘビのくねくね、尺取り虫、横転がりが考えられます。


前の記事の通り、yz平面における座標からサーボモータの角度を決定します。

このように、yz座標を取ります。
また、青いベクトルの長さとz軸からの角度の座標ともいえます。



上に書いた3つの移動方法は、青いベクトルを・・・

①水平方向に向け、長さを周期的に変える

②鉛直方向に向け、長さを周期的に変える

③長さを変えずに、ぐるぐる回す
ぐるぐる回すのは胴体中心に対してのものですが、
床から見てみれば、ロボット全体はタイヤの様に転がります。

の3パターンで行えます。
4軸あるので、青いベクトルはロボットに2本あります。タイミングは上手く調整します。
いろいろやってみるしかありません。



実際の動きはコチラ、完成?


とりあえず、動いたんですけど・・・
景気良く動かすのは難しいです。
たぶん景気良く動かす周期関数が存在すると思いますが・・・
そこまでやる気には、ならないのが・・・人の心っていうもの・・・。

うごいたし、へびはひと段落じゃね

拍手[0回]

4自由度へビのくねくね軌道

ヘビはくねくね動くということで前に進むらしいのです。

なので、くねくね動かす方法を考えます。
くね、と言って右に振り
くね、と言って左に振ればいいのではないでしょうか?

というと、周期的な運動だと考えられるのでsin関数をそのまま入力すればいいのではないか
と考えます。

4自由度2関節のヘビなので、まず片方の関節の1自由度だけを検討します。
例えば、先ほどtheta4の逆関数として
y=f(theta4)を求めたので、
f(theta4) = sin(t)という角度を与えたとします。

ですが、この逆関数はasinなのでsinでは無くなってしまいます。
なので、切り返しが滑らかでは無くなります。
なので
=sin(sin(t))
とかにしてみます。

思惑どおりです。(グラフ上はsinになってしまっていますが)

しかし、このままではだめです。

切り返しが90度であればこのままでもいいのですが、
切り返しが90°出ないとき、もちろんヘビは90度まで曲がっていないようですから、
なので、この結果をそのまま使いません。

このグラフの単位はradですが、
degだと思いなおしたり、クネクネの振幅を命令したいと思ったり、速度0から始めたいと思えば

(この時点で2関節にした時に面倒だなと思い始めるわけですが)

1関節分のくねくねはこれでとりあえず完成です。

2関節目も同様に考えますが、

なにやら、ヘビを見ていると隣り合った関節が、
同時に同じ、あるいは同時に逆ということは無いように思われます。
なので、くねくねには位相のずれを与えるべきです。

例えば、

という感じで、ずらしておきます。
実際は、2本のグラフを書いておいて、縦線を時間軸にそって動かす。
その交点を、その時間の角度として与えます。



これで、くねくね軌道は完成です。

このくねくね軌道は、周期A1、振幅A2、位相のずれA3の3つの変数を持ちます。
実際のところ、yはmaxでd3までしかとれませんので、比として与えられます。

theta3 = arcsin(y/d3)
というところで
Y=y/d3
とします。
Y_1 = sin(%pi*sin([A1]*t)/2 * [A2]/d3)
Y_2 = sin(%pi*sin([A1]*t-[A3])/2 * [A2]/d3)

と与えることにします。




拍手[0回]

4自由度ヘビの逆運動学

ひとつ前の記事から続いています。
逆運動学は、
どんな命令でうごかしたいのかで臨機応変にいろんなことを考えればいいものですが、
今回は

中央の車輪あたりにあるy-z平面を使い、3ブロックあるものを真ん中で2つに分けていこうと思います。
クネクネする動作:
前の奴を右に
後ろの奴を左に
みたいな指示を出したいと思えば、中央のブロックからそれぞれの位置姿勢を命令したいということになります。

なので、

端の車輪の中心を、y-z平面に射影したものを見ていきます。
右に振りたければzを増やす、
上にあげたければyを増やすという命令をすればいいことになります。
もちろん「上」とか「右」とかっていう変数を置いたプログラムを作る方が便利かもしれません。

さて、順運動学をもう一回やります。

まんなかから考えるので、
同次変換行列は

この3つです。
先端までまとめると

私が命令したいのはy座標とz座標です。
最右列の上から2番目と3番目以外に興味はありません。
sin(theta4)*d3=y

-sin(theta3)*cos(theta4)*d3=z
という式です。

変数は、zとyとtheta3とtheta4です。
zとyには、どのくらい上にあげたいのか、右に曲げたいのか
ということに合わせて好きな数字を入れればいいと思います。

それでtheta3とtheta4はどうなるのかっていうところが気になります。




で、どうするのかっていうのは人の好きなのですが
theta4に関しては、変数1つの非線形方程式なのでいろんな方法が簡単に出来ます。
①arcsinをつかう
②数値計算で収束をまつ
③サインと角度のデータテーブルからそれっぽいのを選ぶ

ま、あ、すきな方法でやります。
①でやります。
何にも考えずに①でやります。
theta4 = arcsin(y/d3) です。
なぜなら、theta4の可動範囲は±90°だからです。
arcsinも同じ範囲をサポートしています。

ではtheta3をやります。
theta4が計算してからtheta3を計算すれば
cos(theta4)は定数です。

で、再びarcsinをやります。
theta3 = -arcsin(z/d3cos(theta4)) です。

theta3とtheta4が決まりました。

●theta3 = -arcsin(z/d3cos(theta4))
●theta4 = arcsin(y/d3)

この角度をサーボモータに指示すればいいわけですね。







拍手[0回]

4自由度ヘビの順運動学

割と一般的な方法で運動学をすすめるにあたり、簡単な自由度でまとめておく記事です。

題材は4自由度のヘビ
2軸ごとに直交
左から始まるとして
d1-θ1θ2-d2-θ3θ4-d3
という様に、リンク長さと、回転軸があります。

さて、座標系は写真の姿勢で→向きをx軸、鉛直↑向きをy軸、手前に伸びる奴をz軸として、
θ1-y軸
θ2-z軸
θ3-y軸
θ4-z軸
という回転軸を持ちます。

左側のリンク端をロボットの基準座標系
5つの同次変換行列をつかって位置姿勢を表現します。
 
maximaを使います。
同次変換行列は回転行列の右に位置ベクトルをかくという方法です。
なので、下の行は便宜上存在しているだけなので0,0,0,1です。
どんな時の0,0,0,1です。
d1は左端からθ1θ2までの距離、d2はそこからθ3θ4までの距離、d3はそこから右端までです。
同次変換行列は、進んでから姿勢を変換させていると捉えていいので、T12はd1とθ1を表現します。
T23はθ1からθ2までの距離を持つことができますが、直交軸なので最右列は0,0,0,1となってます。



単純なマニピュレータとして
基準座標系から右端の、位置姿勢は
 
という同次変換行列にまとめられます。
4つだけのモータにこれだけ紙面を必要とするというのは、
やる気が無くなります。
手で書いていたら人生というのは短すぎるでしょう。
この式の表し方にはまだまだ改善の余地があると将来をきたいします。

これが、教科書に書いてある順運動学ですね。





拍手[0回]

mbed備忘録6:BlueUSBのプログラムって、どうなって…Wiiリモコンは...

BlueUSBの続編です。

BlueUSBのUSB通信のところは難解なので、必要な部分はブラックボックスのままで引き抜く方針で、その結果をインターバル割込みで読みます。

+アウトライン+
①BlueUSB+Wiiリモコンの動きを確認します。
②必要な部分だけを見つけます。
③割り込みでデータを見ます。
④自分用にまとめてみる。

①いつもの手法で行きます。
プログラムのいろんなところにprintfをばらまいて動きを見ます。
深さ優先探索です。

BlueUSBの中にmain.cppがあり、
main関数は
TestShell();で終わっています。

なので、その先に行きます。

TestShell.cppの中にTestShell();があります。
その中には
    USBInit();

    gApp.Run();
があります。


でまず、
    USBInit();
 printf("A\n");
    gApp.Run();
   printf("B\n");

見たいにすると、Bが出力されないことが分かります。
なのでgApp.Run();に肝があると思います。

で、TestShell.cppの中に

void Run()があって、
ForとかIfとかがあるので↓の様にします。

   void Run()
    {
        printf("C\n");
        for(;;)
        {
            printf("D\n");
            const char* cmd = ReadLine();
            if (strcmp(cmd,"scan") == 0 || strcmp(cmd,"inquiry") == 0)
            {    printf("E\n");
                Inquiry();}
            else if (strcmp(cmd,"ls") == 0)
             {   printf("F\n");
  ・・・・・・

こうすると、Dまで出力されることが分かります。
なので、肝はReadLine();の中にあると思います。

同様にReadLine();は↓のようにします。
    const char* ReadLine()
    {
        printf("L\n");
        int i;
        for (i = 0; i < 255; )
        {
            printf("M%2d\n",i);
            USBLoop();
            printf("O\n");
            int c = GetConsoleChar();
            if (c == -1)
                continue;
            if (c == '\n' || c == 13)
                break;
            _line[i++] = c;
        }
        _line[i] = 0;
        return _line;
    }

USBLoopが怪しいです。しかもMに続く数字はずっと'0'でした。
つまりUSBLoopになんどもなんども入っています。
で、USBHost.cppの中にVoid Loop()があります。
同様に下のようにします。
    void Loop()
    {
        printf("P\n");
        u16 elapsed = CommunicationArea.FrameNumber - (u16)_frameNumber;    // extend to 32 bits
        _frameNumber += elapsed;

        // Do callbacks, if any
        while (_callbacksPending)
        {
            printf("Q\n");
            for (int i = 0; i < MAX_ENDPOINTS_TOTAL; i++)
            {
                printf("R”);
                Endpoint* endpoint = Endpoints + i;
                if (endpoint->CurrentState == Endpoint::CallbackPending)
・・・・・

してみると、初期設定的なWiiリモコンとペアリングっぽいことをやっている間はPとQとRが出てきます。
なんか知りませんがPの次がRだったりして良くわかりません。(すべてをTeraterm出来ているわけではない?)

で、Wiiとの通信が確立するとRがいっぱいとQになります。
たぶんwhileから抜け出していません。










②以上のことを踏まえると、

    USBInit();のあとに、いろいろすっとばして
    USBLoop();が回っていればいいと思います。

ですので、
void TestShell()
{
    USBInit();
    while(1){
    printf("AA\n");
    USBLoop();
    printf("AB\n");
    }
・・・・・
としてみました。
いまのところ問題は見つかっていません。








③割り込みと同時にいろいろ起きて大丈夫か?にチャレンジします。
main.cppに書き加えます
Lチカと割り込み時に時間をカウントしてみましょう。
void TestShell();
/***/
Ticker intvTimer;       // インターバルタイマ
DigitalOut Led2(LED2);  // LED2
extern int32_t TimeMs=0;
void isrInterval(void) {
    Led2=(TimeMs/50)%2;
    TimeMs+=5;
}
/***/

int main()
{
            // インターバルタイマ 割込み設定、タイマスタート       
    intvTimer.attach_us(isrInterval, 5000);  // 500ms
    pc.baud(9600);
    printf("BlueUSB\nNow get a bunch of usb or bluetooth things and plug them in\n");
    TestShell();
}
で、それに加えLoop内で時間を確認します。
    void Loop()
    {
        printf("P\n");
        u16 elapsed = CommunicationArea.FrameNumber - (u16)_frameNumber;    // extend to 32 bits
        _frameNumber += elapsed;

        // Do callbacks, if any
        while (_callbacksPending)
        {
            printf("Q\n");
            for (int i = 0; i < MAX_ENDPOINTS_TOTAL; i++)
            {
                printf("R%d\n",TimeMs);
                Endpoint* endpoint = Endpoints + i;
                if (endpoint->CurrentState == Endpoint::CallbackPending)
                {
注意)extern int32_t TimeMs;をUSBHost.cppの上の方に書き加えてます。
複数のファイルにまたがるので、(あまりexternの使い方わかってない)
で出力は以下の用になりました。
BlueUSB
Now get a bunch of usb or bluetooth things and plug them in
USB INIT (Controller is 1396 bytes)
AA
P
HubStatusChange Hub:0 Port:1 00010101
ResetPort Hub:0 Port:1
AB
AA
P
AB
AA
P
HubStatusChange Hub:0 Port:1 00100103
AB
AA
P
AB
がはじめのほう。
で通信がはじまると
================wii====================
P
P
0000 A1 30 00 03                                     .0..
R16110
R16120
R16125
R16135
R16140
R16150
R16155
R16165
R16170
R16175
R16185
R16190
Q
R16200
R16210
R16215
R16225
OnHidControl
0000 00                                              .
R16300
R16310
R16315
R16325
R16330
R16340
R16345
R16350
R16360
R16365
R16375
R16380
Q
R16390
R16400
R16405
R16415
OnHidControl
0000 00                                              .
R16490
R16500
R16505
R16515
R16520
R16525
R16535
R16540
R16550
R16555
R16565
R16570
Q
R16580
R16590
R16595
R16605
WII 0300 529 546 650
R16630
R16640
R16645
R16655
R16660
R16670
R16675
R16685
R16690
R16695
R16705
R16710
Q
R16720
R16730
R16735
R16745
WII 0300 529 546 650

という感じになります。
通信が始まるのに16秒ぐらいかかってますね。
Teratermにはいてる時間が一番ながいんでしょうけど。




④自分用にまとめるとは?
この後サーボ動かしたり、加速度を読んだりしたいのでBluetoothの通信は簡素にしてしまいたいし、通信と同じループ内でサーボを動かしたりしたいので、汎用ループを作ってしまいたい所存です。

Try1.
割り込み内で、通信以外を全部やる。
→結果無理。
無理だった作戦。
・Wiiリモコン値をグローバル変数に格納
・インターバル割り込みでprintfしてteratermする。
結果:
teratermするとBluetoothとの通信が終わっちゃう。
割り込みはしてもいいけどシリアル通信は無理っぽい。
時間的になのか、リソースの使い方的なものなのかは不明。(今回はどうでもいい)
もしかすると確率的に、軽い割り込みであったとしても、Bluetoothが止まるかも知れない。
(遊びなのでどうでもいい。)

Try2.
USBLoopから抜けているので、汎用ループ内にUSBLoopを置く。
結果:
今のところOk。

USBHost.cppは何もしない
TestShell.cppは↓
                case 0x37: // Accelerometer http://wiki.wiimoteproject.com/Reports
                {
                    int pad = (d[2] & 0x9F) | ((d[3] & 0x9F) << 8);
                    int x = (d[2] & 0x60) >> 5 | d[4] << 2;
                    int y = (d[3] & 0x20) >> 4 | d[5] << 2;
                    int z = (d[3] & 0x40) >> 5 | d[6] << 2;
                    //printf("WII %04X %d %d %d\n",pad,x,y,z);
                    m_pad=pad;
                    m_x = x;
                    m_y = y;
                    m_z = z;
                    wii_f=1;
                }
                break;
各値をグローバルに入れる&Wiiリモコンとの通信出来てますフラグを立てる。
main.cppは↓
int main()
{
            // インターバルタイマ 割込み設定、タイマスタート       
    intvTimer.attach_us(isrInterval, 5000);  // 500ms
    pc.baud(9600);
    printf("BlueUSB\nNow get a bunch of usb or bluetooth things and plug them in\n");
    //TestShell();
    USBInit();
    while(1){
    USBLoop();
        if(wii_f)
        {
        printf("main %04X %d %d %d %d\n",m_pad,m_x,m_y,m_z,TimeMs);
        }
    }
}

で出力は、
BlueUSB
Now get a bunch of usb or bluetooth things and plug them in
USB INIT (Controller is 1396 bytes)
HubStatusChange Hub:0 Port:1 00010101
ResetPort Hub:0 Port:1
HubStatusChange Hub:0 Port:1 00100103
Connect Hub:0 Port:1 full
と始まって、
Wiiリモコンと通信すると
================wii====================
0000 A1 30 00 03                                     .0..
OnHidControl
0000 00                                              .
OnHidControl
0000 00                                              .
main 0300 529 546 650 14760
main 0300 528 546 650 14785
main 0300 528 546 650 14815
main 0300 528 546 650 14845
こんな感じになります。

さしあてってWii_fのif内でいろいろはできるんじゃないでしょうか。
終わり。

拍手[0回]

mbed備忘録5:Bluetoothはじめました

最後にmbedの記事を書いたのは半年以上前になってますから、何もしてなかったんですねきっと。

この備忘録は、mbed、Wiiリモコン、Bluetoothドングルを買って遊び始めるときのお話です。

いろいろなHPを見て下の回路でいんじゃねとなりました。
数多のHPを書いてくれた人ありがとう。

レギュレータで5Vとります。
USBのバスパワーとレギュレータがぶつかるかもしれないので、ドングルのところはDが入ってます。mbedのボードにはそもそも入ってます。

ちな、加速度センサつきです。

PWMサーボ4つピンを付けます。

USBのドングルは秋月の基板がついてるメスAでつけます。
ドングルはグリーンハウスのGH-BHDA42です。

Wiiリモコンは黒いです。

以上、ハードの下ごしらえ終わり。



ソフトは、
mbedの開発環境にログインして、
BlueUSBをプログラムでインポートします。
自分のところでコンパイルします。
mbedをつなぎます。
mbed内に保存します。
terateamを受信LFで408000みたいなそういう雰囲気のボーレートにします。
mbedをリセットします。なんか文字が出てきます。
Wiiリモコンを準備します。
電源を入れて1と2を推しっぱなしにします。
すかさずmbedをリセットします。
文字が出てきます。
待ちます。
文字が出てきます。
待ちます。
Wii 0000 0000  みたいなのが流れ始めます。

終わり。

拍手[0回]

アクリルの加工依頼

ブログの更新が無いのは、ロボロボしてないからです。

ロボロボしたわけではないですが、人柱的な備忘録的なものです。

本日紹介するのはこれ

アクリルの円筒です。
Φ14.8h8のRa12.5を指示して加工依頼をだしました。
50個頼んで¥15,000弱でした。

個人で削りの加工依頼をするのはなかなか難しいものです。

図面送る→見積もらう→加工方法など相談→再見積→注文
という流れが一般的かと思います。

しかし、加工屋さんによってはそもそも個人相手をしてくれなかったり、対応が遅かったり、まー思った通りにはいきません。

そんなわけで今回、思い通りのものをササッとやってくれたお店を紹介です。

アクリ屋ドットコムさんです。
http://www.acry-ya.com/


もともと、定尺の板だけでなく寸法を指定してWeb上で見積も出してくれる便利なところです。
凄いのは穴加工とかもWeb上で指示できるし、見積もでる。

という凄いところですが、今回はまず、
「このくらいのサイズのものが欲しいんですけど、図面とか送ったらやってくれますか?」
っていう趣旨のメールを出しました。

そしたら、やってくれました。図面を送って見積書をもらいました。
しかもその見積書はWebページと連動してまして・・・カートに入れるとか、そういう感じで注文が出来たし、クレジットで払いました。



いやはや、アパート暮らしになってお家で旋盤なんてちょっとと思いましたが、
値段は高く付きますが、これはありだなと思います。




拍手[0回]

wxMaximaにて列ベクトルの外積:備忘録

いろいろとあって、逆運動学するソフトがひつようになりました。
そこでフリーソフトのmaximaをチョイス!

いろいろをまず解説すると、
6軸マニの逆運動学を行う、、、には
ヤコビアンで数値計算がひつようぽい、、、ので
同次変換行列を計算して、、、
6軸分のヤコビアンを求める、、、となり
関節ごとの同次変換行列から、回転軸の単位ベクトルと関節位置を使いヤコビアンを求める、、、つまり
回転軸が分かって角速度を与えれば、後は腕のながさで次の関節の位置の速度と角速度が出ると、、、そういうわけで


腕の長さベクトルと回転軸周りの角速度ベクトルを外積しないといけません!

つまり同次変換行列から、列ベクトルを抜き出して外積してその結果から新たにヤコビアンの行列を生成します。

で、maximaには標準で外積の機能がりません←どうしてこうなってる?

調べるとべクトル用のパッケージがあってそれを使えばできるとかなんとか・・・

load(vect);

u:[a,b,c];

v:[x1,x2,x3];

express(u~v);

って書くとこうなる↓
[b*x3-c*x2,c*x1-a*x3,a*x2-b*x1]

いいじゃんねー。

だがしかし、これは行ベクトルというより、単なるリストなのです。

ちなみに1行しかないmatrixで宣言すると

load(vect);

u:matrix([a,b,c]);

v:matrix([x1,x2,x3]);

express(u~v);

って書くとこうなる↓
~ used with improper arguments: matrix([a,b,c])matrix([x1,x2,x3])
#0: express1(expn=matrix([a,b,c]) ~ matrix([x1,x2,x3]))(vect.mac line 166)
 -- an error. To debug this try: debugmode(true);

くそだ、なんだこれ


そんなわけで、関数を定義します。

cros(a,x):=matrix(a[2]*x[3]-a[3]*x[2],a[3]*x[1]-a[1]*x[3],a[1]*x[2]-a[2]*x[1]);
コレ、テストに出ル。

結果

あ、間違ってる、画像治すのめんどいけどプログラムはなおす。
u1v3-u3v2×
u2v3-u3v2○


はい、できました。

寝ましょう。

拍手[1回]

プロフィール

HN:
Adel
年齢:
34
性別:
男性
誕生日:
1989/09/17
職業:
会社員
趣味:
モチベーション探し
自己紹介:
ロボットつくるのが夢

ブログ内検索

アクセス解析