カテゴリー: RaspberryPi

歩行にはどうしても一時的にでも片足立ちになることが必要なので、足郎１にもそれをさせてみた。サーボ制御言語の応用の意味もあった。

まだ、安定していないが、微調整をしていきたい。左右の動きに関する８個のサーボの角度マップは以下のようである。前後の８個は動かしていない。左右の動きの一部強い非対称性は、重心が微妙に右側にあることと、サーボの引き方のグダグダ感によるものだと思う。（この角度に向かって現在の角度から、ステップを刻みながら近づいていくように、次のスクリプト言語でプログラムされている）

以下に、上記のサーボの制御言語で書かれたスクリプトを記しておく。動画はこのスクリプトで動かしたものである。見れば、言語の文法はおよそわかっていただけると思う。最初は定義が羅列されていて、最後の方に実行コマンドが入っている

##############################
# 2017年5月18日 Toyoaki WASHIDA
# 2017年5月20日 ver.2
# 足郎1号の制御のためのプログラム
# 「足踏み」
# ファイル名: ashiro1-1.ibot
# '#'で始まる行はコメントになる
##############################
# 角度の定義、定義の名前、
# 定義の場合は、内容を {  } 内に書き込む
# デリミタは基本、空白
# %で始まる定義やコマンドの順序は関係ない
# ただし、 { }内記述の順番は意味を持つ（順番の通り実行される）
# パラメーターは、サーボ角度の値（足の角度ではない）
# （注）現状で、上のサーボとしたのサーボで方向が逆になっている
# スピード、delayの値で使うことが可能
# パラメーター使用時には'$'の接頭文字が必要
# 角度パラメーター
%param angle0 0
%param angle1 8
%param angle2 14
%param angle3 16
%param angle4 6
%param -angle1 -8
%param -angle2 -14
%param -angle3 -16
%param -angle4 -6
# スピードパラメーター
# ステップが刻まれる回数なので
# 正の整数、数字が大きくなるほど遅い
# 1ステップの最後に10msの待機を入れている
%param fast 10
%param slow 10
# 直立状態
%defangles zerophaseSide {
    #左の項のサーボ名はシステム予約語
    LeftUpperLeft:$angle0
    LeftUpperRight:$angle0
    LeftLowerRight:$angle0
    LeftLowerLeft:$angle0
    RightUpperRight:$angle0
    RightUpperLeft:$angle0
    RightLowerRight:$angle0
    RightLowerLeft:$angle0
}
# 右への傾きの第1段階、菱形に潰れる
%defangles rightphase1 {
    RightUpperRight:$angle2
    RightUpperLeft:$-angle2
    RightLowerRight:$angle2
    RightLowerLeft:$-angle2
    LeftUpperLeft:$-angle2
    LeftUpperRight:$angle2
    LeftLowerLeft:$-angle2
    LeftLowerRight:$angle2
}
# 右への傾きの第2段階、
# 右足に重心を十分移動し、左足をできる限りあげる
%defangles rightphase2 {
    RightUpperRight:$-angle4
    RightUpperLeft:$angle4
    RightLowerRight:$angle3
    RightLowerLeft:$-angle3
    LeftUpperLeft:$angle3
    LeftUpperRight:$-angle3
    LeftLowerLeft:$angle1
    LeftLowerRight:$-angle1
}
# 左への傾きの第1段階、菱形に潰れる（使わなかった、以下の改訂版を使った）
%defangles leftphase1 {
    LeftUpperLeft:$angle2
    LeftUpperRight:$-angle2
    LeftLowerLeft:$angle2
    LeftLowerRight:$-angle2
    RightUpperRight:$-angle2
    RightUpperLeft:$angle2
    RightLowerRight:$-angle2
    RightLowerLeft:$angle2
}
# （改訂S：右足をあまり開かない）左への傾きの第1段階、菱形に潰れる
# angle2 は本来14
%param angle2S 4
%param -angle2S -4
%defangles leftphase1S {
    LeftUpperLeft:$angle2
    LeftUpperRight:$-angle2
    LeftLowerLeft:$angle2
    LeftLowerRight:$-angle2
    # ここを変形
    RightUpperRight:$-angle2S
    RightUpperLeft:$angle2S
    # ここまで
    RightLowerRight:$-angle2
    RightLowerLeft:$angle2
}
# 左への傾きの第2段階、これは使わずに、次の改訂版を使った
# 左足に重心を十分移動し、右足をできる限りあげる
%defangles leftphase2 {
    LeftUpperLeft:$-angle4
    LeftUpperRight:$angle4
    LeftLowerLeft:$angle3
    LeftLowerRight:$-angle3
    RightUpperRight:$angle3
    RightUpperLeft:$-angle3
    RightLowerRight:$angle1
    RightLowerLeft:$-angle1
}
# （改訂S：）左への傾きの第2段階、
# 左足に重心を十分移動し、右足をできる限りあげる
# LeftLowerLeftをもっと引く
# この定義に関係するだけのパラメータを書いておく
%param angle3S 32
%param -angle3S -32
%param angle4S 0
%param -angle4S 0
%param angle3T 10
%param -angle3T -10
%defangles leftphase2S {
    LeftUpperLeft:$-angle4S
    LeftUpperRight:$angle4S
    # ここを変化させた
    LeftLowerLeft:$angle3S
    LeftLowerRight:$-angle3S
    # ここまで
    RightUpperRight:$angle3T
    RightUpperLeft:$-angle3T
    RightLowerRight:$angle1
    RightLowerLeft:$-angle1
}
# 前後の動き、使っていない
%defangles foreflac {
    LeftUpperFront:$angle
    LeftUpperBack:$angle
    LeftLowerFront:$angle
    LeftLowerBack:$angle
    RightUpperFront:$angle
    RightUpperBack:$angle
    RightLowerFront:$angle
    RightLowerBack:$angle
}
# 前後の動き、使っていない
%defangles backflac {
    RightUpperFront:$angle
    RightUpperBack:$angle
    RightLowerFront:$angle
    RightLowerBack:$angle
    LeftUpperFront:$angle
    LeftUpperBack:$angle
    LeftLowerFront:$angle
    LeftLowerBack:$angle
}
# 実行の定義
# 使えるコマンドは今の所
# setAngle, speed, delay, stand, relax
# メインルーチンの定義
%defexecs walk1 {
    # この実行定義はループさせないで
    # exec は、再帰的呼び出しを可能にするか
    # 無限ループを避けなければならない
    # 次のものは繰り返させる
    # カンマの前後に空白を入れないこと
    exec:walk2,3
    # 実行が終了したら
    # 立たせる
    stand:all
}
# サブルーチンの定義
%defexecs walk2 {
    # 指定されたスピードは再指定がない限り有効
    # 右に揺れ始める
    speed:$fast
    setAngle:zerophaseSide
    setAngle:rightphase1
    # 足を上げる
    speed:$slow
    setAngle:rightphase2
    setAngle:rightphase1
    # 左に揺れ始める
    speed:$fast
    setAngle:zerophaseSide
    setAngle:leftphase1S
    # 足を上げる
    speed:$slow
    setAngle:leftphase2S
    setAngle:leftphase1
    # 100ミリ秒待機
    # delay:100
}
# 実行させる
# exec 実行コマンド
# walk1 定義されている実行内容名
# メインルーチンは1回だけ実行する
%exec walk1 1
######## 以上 ###########

ロボットに複雑な動きをさせるためには、サーボモーターを自在に制御しなければならない。膝なしロボット足郎1号にも、すでに16個のサーボモーターがついている。次の２号では、足だけなのにその倍、32個のサーボをつける予定だ。どう制御するのか。今は、C++のプログラムを書いて実行させている。引数を色々変えれば、ここのサーボ、あるいはサーボグループを変化させることはできるし、また、一連のプログラムの実行も可能だが、結局非常に面倒になっている。
そこで、こうした多数のサーボを制御するためのコンピュータ言語を作ろうと思う。例えばこんな感じである。
（この言語の実際の応用例）

##############################
# 足郎の制御のためのプログラム
# で始まる行はコメントになる
##############################
# 角度の定義、定義の名前、
#定義の内容を {  } 内に書き込む
# デリミタは基本、空白あるいはタブ
# 定義やコマンドの順序は関係ない
# ただし、 { }内記述の順番は意味を持つ
%defangles leftflac {
    #左の項の関節名は予約語
    LeftUpperLeft:12
    LeftUpperRight:12
    LeftLowerRight:12
    LeftLowerLeft:12
    RightUpperRight:12
    RightUpperLeft:12
    RightLowerRight:12
    RightLowerLeft:12
}
%defangles rightflac {
    RightUpperRight:12
    RightUpperLeft:12
    RightLowerLeft:12
    RightLowerRight:12
    LeftUpperLeft:12
    LeftUpperRight:12
    LeftLowerLeft:12
    LeftLowerRight:12
}
%defangles foreflac {
    LeftUpperFront:12
    LeftUpperBack:12
    LeftLowerFront:12
    LeftLowerBack:12
    RightUpperFront:12
    RightUpperBack:12
    RightLowerFront:12
    RightLowerBack:12
}
%defangles backflac {
    RightUpperFront:12
    RightUpperBack:12
    RightLowerFront:12
    RightLowerBack:12
    LeftUpperFront:12
    LeftUpperBack:12
    LeftLowerFront:12
    LeftLowerBack:12
}
#といった感じで、ここでは関節グループだが、
# 個別的なサーボの制御を書いても良い
# このような関節角度の定義を書き連ねる
# 実行の定義
# 使えるコマンドは今の所
# setAngle, speed, delay, stand, relax
%defexec walk1 {
    # この実行定義はループさせないで
    # exec は、再帰的呼び出しを可能にするか
    # 無限ループを避けなければならない
    # 次のものは繰り返させる
    # カンマの前後に空白を入れないこと
    exec:walk2,8
    # 実行が終了したら
    # 立たせる
    stand:all
}
%defexec walk2 {
    speed:10
    setAngle:leftflac
    # 100ミリ秒待機
    delay:100
}
# 実行させる
# exec 実行コマンド
# walk1 定義されている実行内容名
# 1回だけ実行する
%exec walk1 1
######## 以上、サンプル ###########

ファイルにして一気に実行もできて、また、一旦読み込んだら、コマンドプロンプトからインタープリターのようにも実行できるようにしたらいい。

逆振り子中の加速度データが取れた。
加速度データは別スレッドで動かして取っているのだが、その間、なんだかサーボ制御の動きがおかしかった。RaspberryPiはコア4なのに、スレッド１個で動揺していては困るな。
青のX軸が前後の動きで、オレンジのYが左右、灰色のZ軸が上下である。
左右には割と安定して揺れているが、ぜん後には突然揺れが襲ってきている感じだ。
どういうタイミングで足を前後に出して行くかが問題だ。もう少し、一方の足への重心移動がはっきりと、安定して行われなければ、逆足の前への移動は難しいのかもしれない。

足郎を一旦組み上げた後、色々なハード的な問題が発生して、リードを作り直したり、足の厚みを強化したりして、時間を使った。ハード上の問題はまだあるが、さしあたって、動きを作り始めている。まず、左右の揺れを正確に刻ませたかった。
歩行まではまだまだだなぁ。

二足歩行目指して「足郎１号」（足しかないので、いや、足しか作る予定がない、笑）組み上げました。この段階ではまだ、膝関節をつけていないので、実用版ではなく、実験版です。「足郎２号」で、膝を持った普通の「足」にする予定。
足が前後していたり、微妙に開いていたりするので、まず、サーボ角度０で直立している状態をきちんと作ってから動かします。
予測していたより安定性が高い。前後に倒れる力と左右に揺れる力を利用してある貸そうと思っているので、あまり安定していてもいけないが、それはそれでいい。
サーボの反応速度が悪くなければ、二足歩行に持っていけるはず。

夕方からの時間のほとんどを使って、サーボから伸びるリード線を16本作成した。
サーボのゼロ位置の調整、そこから足と上板への距離を測って、それに見合うものを作成したが、おそらく全て調整し直す必要がある。１、２ミリ長くなるので、その分切って、半田付けで繋げるという作業が必要になると思う。
その調整が滑って終わったら、サーボの初期位置でこのロボットを立たせるつもりだ。サーボに支える力がなかったら、最初から全て考え直さなければならなくなる。そうならないことを願いたい。
16個のサーボを、うまく制御するためのプログラム上の工夫が必要だとは思っている。

さしあたって「膝なし版」の二足歩行ロボットに向けて、アルミの枠組みを作成した。
昔見た、ブリキの廃材で作ったロボットの雰囲気。関節に駆動力がかからないので、遊びが大きく、ネジ穴が多少合わなくても問題なし。
今日中に、このフレームに、サーボモータ16個を取り付けて、ハード的なところを完成させたい。16個も（！）大丈夫かな（笑）

先の記事で書いたこと（こちら）を証明するために、まず、１足ロボット アイコメ01でやって見た。ただ、左右に体を揺らせているだけである。前後のバランスの違いで、動画のように前に進んでいる。
この時の設定ファイル controler.init の内容を記録しておく。一足歩行と無関係なものもある。

################################
# 2017年5月1日作成
# controler.init
# controler 初期化ファイル
# 実行時にパスを設定できる
# ./controler -e -init /home/pi/controler.init
######################################
# で始まる行はコメント
# サーボ0の初期角度（度で指定）
initialangle:0:-2.0
# サーボ1の初期角度
initialangle:1:-6.0
# 以下の各種角度は、上記初期角度に対する相対角度である
# 実行時間（秒単位で指定）加速度チェックの時間でもある
duration:3
# 臨界加速度：関節が固定化される加速度
acceleration:0.1
# 関節固定化に際して、追加的な角度（傾いただけ、逆側は伸びる）
#diffAngle:-1
diffAngle:2
# 完全に弛緩させる角度
slackAngle:-6.0
# 倒す時の吊り上げ角度
fallAngle:2.0
# 最大リアクション回数
# 転倒に対するリアクションがこの回数に到達したら反応しなくなる
# 負の数字を入れると無限に反応する
# 0を入れると反応しない
maxReaction:0
# 最初に緩和する角度
initialSlakAngle:-8.0
# 逆振り子の実行回数
# pendIteration:10
# 不活性期間（ミリ秒）
# 一度転倒に反応すると、この期間は、反応しなくなる
inertiaPeriod:800

 
 

逆振り子ロボットに、いろいろな転倒をさせてみた。あるいは、転倒の回避を試みた。数え切れないほどやったが、その一部を書いておきたい。

逆振り子ロボットアイコメ01の雄姿（笑）は左のようなものだ。写真では、左右方向にだけ倒れるように作ってある。足先に向けて、2台のサーボのワイヤーが張ってあるので、これを張ったり緩めたりして転倒させたり、転倒を制御させたりしている。
写真は、サーボ角度をほぼニュートラルにちかい位置にして直立させている。
サーボを緩めて、自然転倒させた時の３軸加速度センサーが捉えるデータを示しておこう。

横軸の目盛りは10ミリ秒単位で、センサーから撮ってきたデータの番号である。縦軸は上から順に、Z、X、Y軸の加速度センサーのデータである。
Z軸は、垂直方向であるが、サーボの脱力によって上下の微妙な揺れが生じるが、倒れるに従ってそれは減衰している。青がX軸だが、倒れないようになっている方向のため、微妙な揺れだけにとどまっている。一番下のY軸のデータが、倒れる方向のものだ。ただ、ゼロから、一挙に（回転方向である負の方向に）上昇するが、倒れるに従ってまたゼロに戻っていっている。回転に従ってこの方向の動きがなくなるからである。倒れた瞬間に、加速度センサーはパニックになる。
このパニック状態は、ロボットが一番不快な状態になっていると考えれば良い。これは、のちに、ロボットに運動を学習させるときに、不快感のシグナルとして使いたいと思っている。
衝撃が治ると、Y軸が垂直になっているので、1g（gは重力加速度）になって、他はほぼゼロになっている。わずかの傾きがあるくらいだ。
次に、転倒をロボット自身んが検知した瞬間（0.1gの変化）、転倒する反対側のサーボを、角度を縮める形で固定した。すなわち、逆方向に揺り戻そうと動かした時の反応は、次のようになる。
まず、Z軸が、脱力と片側固定化の、一瞬の大きな揺れの後、1gを維持している。先の完全転倒の場合と比べて、水平状態がほぼ維持されていることを意味している。それでもわずかに左に傾いていることは、Y軸の動きによって知ることができる。それが一見安定しているようだが、脱力から0.8秒後に反対側に向かっての回転が発生して、矢印のところで臨界点を超える。この臨界点を超えるというのは、データからの判断ではなく、ロボットを見ていた私が、これは、ほっておけば倒れると判断したところで、私がその時点でロボットを支えた。
最初の転倒方向への完全転倒は避けれたのだが、逆のサーボが引き戻したことによって、逆側に倒れてしまう事態になったというわけである。

（以下の記事の元になっているのは、この論文）
1関節ロボット、アイコメ0.1（外形はこちら）上におかれている３軸加速度センサーが転倒に対してどのように反応するかを調べてみた。
正直驚いた。ロボットに載せずに、机の上で動かしたりのシミュレーションはすでに行っていて、その結果も示しているが、その結果とは大きく違っている。
垂直に立てた状態から、手を離して、5度か10度自由に傾かせて、手で止めてまた垂直に戻すというのを3回やったデータである。止めたところは、およそ、ここで関節を固定させたらいいなと、私が判断した場所である。
（１）重力の誤差が小さい。Z（縦）方向のg（重力加速度）は1で、他はゼロが理論的に予想されるところだ。ほぼ、そんな感じだ。Y軸は、私がほぼ水平（すなわち棒が垂直に立っている）と思われるように調整しているが、前後の軸（X軸）は、丁寧に調整していないのでわずかにずれている。
（２）一度の揺れを感度良く捉え、たくさんのデータを輩出している。前の結果と比べてもらえば一目瞭然だ。サーボによる調整のトリガを出す上で、とてもわかりやすい。
ここで、理論的に予測される、臨界角度を調べてみよう。アイコメ01は10センチの横棒の中央から33センチの高さ（コンピュータが置かれているアルミ板までの高さ使う）だから、その比は6.6である。この場合の臨界角βは、ラディアンで0.075869、角度で4.346973501度であることがわかっている（この論文参照）。

すると、左の図からわかるように、Y軸方向の加速度は、sinθcosθであるから、重力加速度のsinθcosθになったときが垂直からの角度がθになったときであることがわかる。このθに0.075869を入れると、上記の値は、0.001324162となる。つまり、Y軸方向の加速度が絶対値で、0.001324162ときである。
もし、この理論値が正しければ、極めて初期の段階で、関節の固定が必要となることを意味している。
サーボモータに、こんなに微妙なコントロールは不可能であるから、実際上は、センサーが倒れ始めているデータを出し始めた途端、関節を固定化するようにサーボの角度を与えることが必要になる。