運動学に関するプログラム

kinematics.pyプログラムでは以下を扱う．

シリアルリンクアームの構造定義
順運動学
ヤコビ行列
逆運動学の数値解法
逆運動学の数値解法の１ステップ　＝　分解運動制御

基本的な使い方だけでなく，pythonで書かれたプログラムがどの程度の速度か，またそれはロボットの制御に使えるものかを検討する．

このテストは以下の条件で行った．

ProBook 474s
メモリ：8 GB
CPU：Core™ i5-3230M
OS: Ubuntu 20.04
jupyter notebook，python3

ここではkinematics.ipynbでの結果をベースにコメントを付け加えている．

from kinematics import *

この内部での以下の依存モジュールがインポートされている．

import numpy as np
import numpy.linalg as la
from geo import *

ここだけのもの．通常の使用では不要．

import time
 
def test(n,fn):
    i=0
    start=time.time()
    while i< n :
        fn()
        i += 1
    end = time.time()
    rslt=end-start
    return rslt

アームの構造は以下のような辞書型で記述する．

arm_conf={'dof':dof,
         'j_conf':[[[xyzabc of joint],joint_type],,],
         'wrist':[xyzabc of wrist],
         'tool':[xyzabc of tool]]

dof : 自由度数
j_conf : ジョイントの構造 [xyzabc_of_joint, joint_type]のリスト
xyzabc_of_joint : 前のリンクから見た関節座標系をxyzabcの形式で表す
joint_type : 0 - 5 で駆動軸を表す．0-2はx,y,zの並進，3-5はx,y,z周りの回転．
wrist : 最終関節から見た手首座標系をxyzabcで表す
tool : 手首に取り付けられたツール（ハンドなどのエンドエフェクタ）座標系をxyzabcで表す

関節構造の定義は，CG，物理シミュレーション，他のロボット分野（たとえばROS,OpenHRP）など種々存在していていくつかはデファクト標準になっている．

ここでの定義は，運動学に関するエッセンスを分かりやすく切り出したものになっている．

また汎用性のためジョイント構造を関節位置と駆動軸の2つに分けて定義していが，そのため計算量が増加してしまっているのはトレードオフである．

たとえば６自由度アームの順運動学で用いたアームは以下のように記述される．

bh=0.1
l1h=0.05
l2h=0.4
l3h=0.3
l4h=0.1
l5h=0.1
l6h=0.05
l_hand=0.1
 
arm_conf={
    'dof':6,
    'j_conf':[[[0,0,bh,0,0,0],5],[[0,0,l1h,0,0,0],4],
              [[0,0,l2h,0,0,0],4],[[0,0,l3h,0,0,0],5],
              [[0,0,l4h,0,0,0],4],[[0,0,l5h,0,0,0],5]],
    'wrist':[0,0,l6h,0,0,0],
    'tool':[0,0,l_hand,pi,0,0]}

順運動学計算の関数は，以下のように関節パラメタ（関節角など）とアームの２つを引数として，手先の座標系（FRAME）を返す．

def calc_fk(joints,arm_conf) :

j1=[pi/3,pi/4,pi/2,pi/4,0,pi/6]
calc_fk(j1, arm_conf)

f:(m:[[-0.9280226546839175, -0.11736248290409608, -0.35355339059327384], [0.3244692640906438, -0.72091587349737, -0.6123724356957947], [-0.1830127018922195, -0.6830127018922195, 0.7071067811865474]],v:[0.3712310601229375, 0.6429910574805843, -0.026776695296636774])

汎用関数を用いずに６自由度アームの順運動学で示したように，関節での座標変換を簡略化した関数を作成して速度を比較する．

# from geo import *
 
bh=0.1
l1h=0.05
l2h=0.4
l3h=0.3
l4h=0.1
l5h=0.1
l6h=0.05
l_hand=0.1
 
Tw=FRAME(xyzabc=[0,0,l6h,0,0,0])
Th=FRAME(xyzabc=[0,0,l_hand,pi,0,0])
def fk_6dof(th):
    T1=FRAME(xyzabc=[0,0,bh,0,0,th[0]])
    T2=FRAME(xyzabc=[0,0,l1h,0,th[1],0])
    T3=FRAME(xyzabc=[0,0,l2h,0,th[2],0])
    T4=FRAME(xyzabc=[0,0,l3h,0,0,th[3]])
    T5=FRAME(xyzabc=[0,0,l4h,0,th[4],0])
    T6=FRAME(xyzabc=[0,0,l5h,0,0,th[5]])
    return T1*T2*T3*T4*T5*T6*Tw*Th

fk_6dof(j1)

f:(m:[[-0.9280226546839175, -0.11736248290409608, -0.35355339059327384], [0.3244692640906438, -0.72091587349737, -0.6123724356957947], [-0.1830127018922195, -0.6830127018922195, 0.7071067811865474]],v:[0.3712310601229375, 0.6429910574805843, -0.026776695296636774])

test(10000, lambda : calc_fk(j1, arm_conf))

4.557099342346191

test(10000, lambda : fk_6dof(j1))

2.1974411010742188

簡略化したものと比べて汎用のものはおよそ倍の時間がかかっている．座標変換の演算回数が倍になっていることが大きく効いている．またアームの構造をたどりながら計算していることも多少のオーバヘッドになっていることも予想される．

ヤコビ行列の計算も関節パラメタとアームの構造を引数として与えることで計算できる．

ヤコビ行列はnumpyの配列として返す．

またヤコビ行列を計算する過程で順運動学計算も行われているのでその結果も併せて返す．

逆運動学の数値解法ではヤコビ行列とともに順運動学も必要なので都合が良い．

def calc_fk_jacob(joints,arm_conf) :

calc_fk_jacob(j1,arm_conf)

(f:(m:[[-0.9280226546839175, -0.1173624829040961, -0.3535533905932739], [0.3244692640906438, -0.7209158734973699, -0.6123724356957946], [-0.18301270189221944, -0.6830127018922194, 0.7071067811865474]],v:[0.37123106012293755, 0.6429910574805843, -0.026776695296636704]),
 array([[-6.42991057e-01, -8.83883476e-02, -2.29809704e-01,
          2.77555756e-17, -2.15593109e-01,  0.00000000e+00],
        [ 3.71231060e-01, -1.53093109e-01, -3.98042083e-01,
         -2.77555756e-17, -1.98648278e-02,  0.00000000e+00],
        [ 0.00000000e+00, -7.42462120e-01, -4.59619408e-01,
          0.00000000e+00, -1.25000000e-01, -6.93889390e-18],
        [ 0.00000000e+00, -8.66025404e-01, -8.66025404e-01,
          3.53553391e-01, -3.62372436e-01,  3.53553391e-01],
        [ 0.00000000e+00,  5.00000000e-01,  5.00000000e-01,
          6.12372436e-01,  7.86566092e-01,  6.12372436e-01],
        [ 1.00000000e+00,  0.00000000e+00,  0.00000000e+00,
         -7.07106781e-01,  5.00000000e-01, -7.07106781e-01]]))

test(10000, lambda : calc_fk_jacob(j1,arm_conf))

8.52026104927063

必須引数

trans: 目標FRAME
s_th: 初期関節パラメタ
arm_conf: アームの構造定義

オプション引数

step: 収束計算に入るまでのステップ刻み
lim: 許容誤差 = 終了条件
th_lim: 関節パラメタの最大変更幅（大きく動くのは特異点近傍）
rlim: 特異値の切り捨てしきい値（特異点近傍で暴れるのを防ぐ）
rt_rate = 並進／回転の比率．回転への重み付け

def ik(trans, s_th, arm_conf,
   step=0.01,lim=0.0001,th_lim=0.05,rlim=0.001,
   rt_rate = 1.0) :

j1 : 関節角初期著，先の関節角
j2 : 目標の関節角（ikを解いたときの真値）
t2 : 目標FRAME（j2のときの手先座標系）

t1 = calc_fk(j1, arm_conf)
j2 = [-pi/4, pi/3, pi/4, -pi/6, pi/4, -pi/3]
t2 = calc_fk(j2, arm_conf)

tmp=ik(t2, j1, arm_conf)
tmp

[-0.7853992380411264,
 1.0471823145659105,
 0.7854289266268336,
 -0.5236083304262702,
 0.785385704890891,
 -1.047186685374375]

関節角真値との比較

j2

[-0.7853981633974483,
 1.0471975511965976,
 0.7853981633974483,
 -0.5235987755982988,
 0.7853981633974483,
 -1.0471975511965976]

目標FRAMEとの比較

calc_fk(tmp, arm_conf)

f:(m:[[-0.8736006964397756, 0.4713891354566923, -0.12088881732125217], [-0.43705845826583545, -0.6507483775451598, 0.6208916597769961], [0.21401338097939093, 0.5952468665148388, 0.7745188446164727]],v:[0.5483724110655246, -0.6733742353343456, 0.0528419499007637])

t2

f:(m:[[-0.8736006849457358, 0.4713886023536162, -0.12089097912352764], [-0.4370594868340854, -0.6507483361571917, 0.6208909791235274], [0.21401132734195855, 0.5952473339375414, 0.7745190528383291]],v:[0.5483767998160876, -0.6733767998160873, 0.05284261874940961])

test(1000, lambda : ik(t2, j1, arm_conf))

214.67796325683594

約215msec.

たとえば軌道を10分割して経由点ごとにikを求めると約2秒．これが遅いか？速いか？

tmp=ik(t2, j1, arm_conf, step=0.5)
tmp

[-0.7854053421388558,
 1.0470834009055887,
 0.7856294026312589,
 -0.5236667018858575,
 0.785300634569454,
 -1.0471188119222254]

calc_fk(tmp, arm_conf)

f:(m:[[-0.8735997102232438, 0.4713932758501981, -0.12087979889577782], [-0.4370554078831119, -0.6507470712205464, 0.6208951761271848], [0.21402363593302334, 0.5952450157549413, 0.7745174332969585]],v:[0.5483455669661724, -0.6733568503070729, 0.05283732616862849])

test(100, lambda : ik(t2, j1, arm_conf, step=0.5))

5.4649951457977295

test(100, lambda : ik(t2, j1, arm_conf, step=0.2))

5.5400636196136475

test(100, lambda : ik(t2, j1, arm_conf, step=0.1))

5.442981719970703

test(100, lambda : ik(t2, j1, arm_conf, step=0.05))

5.426947355270386

test(100, lambda : ik(t2, j1, arm_conf, step=0.02))

11.032327651977539

刻み幅を小さくすると途中の計算が多くなるのは分かるが stepが0.05以上でほぼ一定になっているのが何故かわからないのでループカウンタを入れて調べてみた．

Table 1: ステップ幅とループ回数

ステップ幅	全ループ回数	ステップ刻み回数	収束演算回数
0.01	215	214	1
0.02	109	108	1
0.05	54	51	3
0.1	53	48	5
0.2	53	45	8
0.5	53	37	16

ステップが小さい場合

ステップ幅が0.02以下の場合は収束演算は1回で終了している．
それ故，全体でかかる時間は分割回数に比例，すなわちステップ幅に逆比例することになる．

ステップが大きい場合

ステップ幅が0.05以上では収束演算回数はステップ幅にほぼ比例しているように見える．
ステップ刻み回数がステップ幅に逆比例していないのは，目標にまっすぐ向かっていないためと思われる．
全体のループ回数がほぼ一定なのは偶然と考えられる．

計算速度とのトレードオフで適切なステップ幅を予想すると，今回のケースでは0.05ではないだろうか．その理由は，分割回数がステップ幅に逆比例しているギリギリのところ，すなわち目標にまっすぐ安定して向かっていると考えられるからである．

適切なステップ幅はアームの姿勢に依存する．しかし，0.05，今回のアームのサイズを1mのオーダで考えると5cm，というのは妥当なところである．またrt_ratio=1.0としているので，アームのサイズにかかわらず0.05rad=2.86°というのも十分妥当性がある．

これはikの1ステップを切り出した関数である．

ikの数値解法の結果の分析から分かるように，step=0.02以下からなら一回の逆行列演算でlim=0.0001の精度で目標座標系になるように解が求められる．

逆に言えば，次の目標値が0.02以内ならヤコビ行列の線型方程式を一回解くだけで0.0001の精度で目標に到達するということである．

これを1mオーダのアームでいえば，次の目標が2cm以内なら0.1mmの精度で簡単に動作させることができるということになる．

この1ステップの動作を連続させる方法を分解運動制御という． 1ステップを単位時間で除して速度として制御することが多く，これを分解運動速度制御（resolved motion rate control, rmrc）と呼ぶ．

def ik_one_step(trans, s_th, arm_conf,
   step=0.01,lim=0.0001,th_lim=0.05,rlim=0.001,
   rt_rate = 1.0) :

ik_one_step(t2, j1, arm_conf)

[1.0408382827198988,
 0.7759559830674535,
 1.582579132467006,
 0.7811428070265566,
 0.006602255357083237,
 0.5193434192274071]

この例では目標を遠方に置いているが，これで目標に回転も含めて0.01近づいたことになる．

任意の空間軌道を十分小さく(lim以下に)分割して現在関節パラメタに対してその都度この関数を呼び出してやればその軌道に沿った動作をさせることが可能となる．

test(1000, lambda : ik_one_step(t2, j1, arm_conf))

1.088411808013916

一回の計算は約1msecである．その内，ヤコビ行列の生成に0.85msecかかっている．

また1cmで軌道を分割した場合で，10m/secの速度まで対応できることが分かる．これは軌道生成としては十分な性能である．

これをC言語などで実装すればサーボループに組み込んでも十分な性能を得ることができる．この方式を用い手先座標系で直接サーボを実行することで，手先でのインピーダンス制御やハイブリッド制御が実現されている．

運動学に関するプログラム

モジュールのインポート

時間計測の関数

シリアルリンクアームの構造定義方法

６自由度アームの例

順運動学計算

順運動関数

使用例

比較のための関数定

ヤコビ行列

ヤコビ行列計算関数

使用例

逆運動学の数値解法

使用例

時間計測

刻み幅（step）を変えて時間計測

逆運動学の数値解法の１ステップ　＝　分解運動制御

使用例

時間計測

RoboLab.Suehiro

運動学に関するプログラム

モジュールのインポート

時間計測の関数

シリアルリンクアームの構造定義方法

６自由度アームの例

順運動学計算

順運動関数

使用例

比較のための関数定

ヤコビ行列

ヤコビ行列計算関数

使用例

逆運動学の数値解法

使用例

時間計測

刻み幅（step）を変えて時間計測

逆運動学の数値解法の１ステップ ＝ 分解運動制御

使用例

時間計測

逆運動学の数値解法の１ステップ　＝　分解運動制御