第7回ROBO-剣優勝ロボット「逡巡」
山口辰久（チーム山口自動機械）

自己紹介
山口辰久
●
●
●
●
●
●
●

Twitter: @qzy13700
京都大学学生サークル 「機械研究会」OB
ROBO-剣 第１回大会から参加
ROBO-ONE 時々参加
マイクロマウス、知能ロボコン、etc.
会社員（ソフトウェア技術職）「山口自動機械」は実在する法人名・屋号ではない○

ウェブサイトの名前＆ロボコン参加チームの名前

講演の内容1.
2.

ハードウェア
制御アルゴリズム
a.
b.
c.
d.

3.

標的位置の検出
ハンドアイ校正
経路計画
試合の戦略

ソフトウェアシステム構成

name: Shunjun

Chapter 1:Hardware

height: 47cm
weight: 2.3kg

name: Shunjun

機械●height: 47cm

台座

weight: 2.3kg

○
○
○

●

MDF板12mm
松角材
ラミン丸棒

カメラマウント○○●

Softkinetic DS325
アームとともに台座に固定

５自由度アーム
KONDO ロボット用サーボモーター

○●吸盤
○
○

PROMATEバキュームリフター
耐荷重20kg

アーム部詳細
●

５自由度
○
○
○
○
○

●
●

自作ブラケット A5052 T1.5
簡易すべりスラスト軸受 (POM)○●●

肩ピッチ: KONDO KRS6003RHV
上腕ひねり: KRS4034HV
肘ピッチ: KRS4033HV
下腕(=手首)ひねり: KRS3304 ICS
手首ピッチ: KRS3304 ICS

無限回転を抑制

鍔：ABS板
柄・面の台：FDM 3Dプリント

制作風景

台座組み立て中ホームセンターの加工サービス利用

ポケットベンダーによる曲げ加工

初期バージョン
市販ブラケット使用
根本側リンクが短い
カメラ視野狭い

各部詳細

固定用吸盤

下腕のフレーム構造

カメラマウント
全体ではめ込む構造

Electronics 電装系●

制御用PC: Lenovo Thinkpad X1 Carbon○●

シリアルポートとして動作
KONDOのロボット用サーボと通信
1250kbps (KONDOの規格)

ToF距離画像センサ Softkinetic社
DepthSense DS325○○●

SoftkineticDS325

KONDO DUAL USB アダプター○○○●

CORE i7 vPro

Laptop PC(Thinkpad X1, Ubuntu)

color 640x480, depth 320x240
USB 2.0

USB DUAL Adapter
KONDO
DC-DC switching reg.
(Akizuki AE-SI8010Y)

KRS-30xx
KRS-6003HV

配線用ハブ基板○

電源と信号線の分配

Li-Po battery
11.1V 1000mAh

シンプルにしました

KRS-40xx

配線ハブ基板●●

信号線と電源を分岐
デイジーチェーン接続はあまり使わない○
根元側の電線を複数サーボが共有○
QIコネクタの電流容量○
瞬間的な大電流で電圧低下○
通信失敗が増える

12V IN

8V IN

12V
OUT

USB ICS設計方針●

自動制御前提○○○○●剛性高く○○●

軸配置もカメラも、操縦しやすさは考えず
距離画像センサなので、人間が見たとき奥行きがわかりにくいのは気にしない
視界を広くとるため少し後ろに
副作用：相手の小手が鍔に隠れていると見えない＝無駄な攻撃が抑制 (?)
ピッチ軸は両持ち、ひねり軸は簡易的すべりスラスト軸受■ 制御しやすい --- 多関節剛体として近似が十分成立する
質量をなるべく中心側に（後に妥協）

関節可動域を十分確保○○

肩: 真後ろまで肘の折りたたみ :約158度まで。逆向きは面パーツの干渉により制約

Chapter 2:
Algorithm andStrategy

タスクの範囲●

静止した標的打突動作とは●●突き: 竹刀の先端を目標物体に接触させるように動かす
その他: 竹刀の先から1/3のどこかを目標物体に接触させるように動かす

接触のために要求される精度●

目安として、正面打ちの場合
○
○
○

●

左右 ±1.5cm
前後 ±4cmぐらい (竹刀の長さに依存 )上下 上にずれると当たらない

突きの場合
○
○
○

左右 ±1.5cm
前後 手前にずれると当たらない
左右 ±1.5cm

ROBO-ONE(格闘)よりも数倍、位置の正確さが必要

オープンループ vs クローズドループ●

打突動作開始後のフィードバックの有無○●

オープンループ [今回採用]○○●

ゴルフ vs ラジコン飛行機
センサー情報を使って動作を計画する動き出したらセンサーは使わない

クローズドループ○○○○○

動作開始後、ずれを計測して修正処理速度の問題
アームの先にセンサーがほしい
画像がブレるかもロボット自身のアームが見えるので区別する必要

打突タスクの分解●

1. 標的の検出・位置の認識○●

2. 動作計画○○○○●

センサーで位置を計測できるようになる
順運動学計算■ 関節角度から腕の先端の動きがわかる
アームとセンサーのキャリブレーション■ 自分の腕がどこにどう付いているかを知る
逆運動学計算■ 接触する瞬間の姿勢を生成する
経路の生成・選択■ うまく打突できるような竹刀の振り方を考える

3. 動作の実行○角度指令を順次出力する

1. 標的の検出 : 外界センサー
Softkinetic DepthSense DS325 ToF方式距離画像センサ●●●

色画像(color): いわゆる「普通の画像」、RGB
距離画像(depth): 濃淡、各点でカメラからの遠近を表す
確信度画像(confidence): 濃淡、各点で距離がどのぐらい信頼できるかを表す↓プログラムで処理して得られる情報(ベンダー製ライブラリ等で)●ポイントクラウド: ３次元空間の点群の位置とその色○○○１つの点＝(x, y, z, (r, g, b))
距離画像の画素数に対応 (320x240)
確信度の低い点は除外

標的検出のデモ
ポイントクラウドを表示

赤・青・黄色の物体を検出確認用に球で位置を表示

1. 標的の検出 : アルゴリズム●

0. 前処理○●1. 分節化○○●

クラスタリング : 点群を塊に分ける■ 色が似ていて近い点同士を結んで徐々にまとめていく
PCLのregion growing segmentation実装を使用

2. 標的らしいクラスタを選出
○
○
○

●

ロボットから一定距離以上遠い点を除外 (背景など)

HSV色空間(色相・彩度・明度 )で色を判別
該当色が６０％以上を占めるクラスタを有効打突部位として認識
理想：形状の特徴を見る（未実装） --- 面なら丸い、胴は細長い等

3. 標的位置の計算○○

クラスタの重心を求める
理想：球体フィッティング等（未実装）

打突タスクの分解(再掲)●1. 標的の検出・位置の認識○●

2. 動作計画○○○○●

センサーで位置を計測できるようになる
順運動学計算■ 関節角度から腕の先端の動きがわかる
アームとセンサーのキャリブレーション■ 自分の腕がどこにどう付いているかを知る
逆運動学計算■ 接触する瞬間の姿勢を生成する
経路の生成・選択■ うまく打突できるような竹刀の振り方を考える

3. 動作の実行○

角度指令を順次出力する

剣先まで

5

L4

実機の関節構造と順運動学

R5関節角度{θ0,..,θ4}から竹刀先端の位置を求める。
Rn: n番目リンクの姿勢
pn: n番目のリンク遠心側頂点位置

4R4L33L2R3eZ
L22R3L11z

L00 0yR2x

R1
各リンクの座標系にてx: ピッチ軸の方向
z: アーム先端方向

逆運動学?
●

順運動学計算 (forward kinematics)○●

逆運動学計算 (inverse kinematics)○●

アーム関節角度から手先の位置を求める
手先の位置を希望の場所に持っていくためのアームの関節角度を求める

多関節（シリアルリンク）ロボットでは:○○

順運動学は根本から順番に書いていけば式にできる
逆運動学は必ずしも陽形式に書けない

非線形最小二乗問題、数値計算による反復解法
その前に……

打突タスクの分解(再掲)●1. 標的の検出・位置の認識○●

2. 動作計画○○○○●

センサーで位置を計測できるようになる
順運動学計算■ 関節角度から腕の先端の動きがわかる
アームとセンサーのキャリブレーション■ 自分の腕がどこにどう付いているかを知る
逆運動学計算■ 接触する瞬間の姿勢を生成する
経路の生成・選択■ うまく打突できるような竹刀の振り方を考える

3. 動作の実行○

角度指令を順次出力する

座標変換の必要性●

カメラから得られる標的の座標
○
○
○

●

x

原点：カメラの光学中心
座標軸の向き：カメラの向き基準
スケール：メートル単位、ライブラリからz

運動学計算による竹刀先端の座標○○○

原点：アームの土台（便宜上）座標軸の向き：最初の回転軸基準
スケール：実測した各リンクの長さに基づく

zxy⇒どちらかに統一する変換が必要
ここでは、カメラの座標系 で表すよう統一
並進と回転、拡大縮小の形

変換パラメータはどう決める？

y座標変換パラメータのキャリブレーション
●
●
●
●
●
●
●

定義: 実測と計算によりパラメータを決定
カメラ取り付け後、運用前に竹刀と同じ長さの棒、先端にマーカーを固定
色々なポーズをとりながら観測したマーカー位置と関節角度を記録
約50点をサンプリング
最も「辻褄の合う」パラメータを計算
変換後誤差二乗和を最小化○○○

非線形最小二乗
Levenberg-Marquardt法
cminpackを利用

実測データの例
竹刀先端座標[m]x関節角度[rad]

yz

θ0

θ1

θ2

θ3

θ4

0.065294

0.018595

0.47221-2.6784-0.14019

2.7167-0.10838-0.21913

0.051924

0.14596

0.48924-2.58-0.13432.7055-0.10838

0.12134

0.036495

0.083223

0.47985-2.6289-0.094837

2.6713

1.2199-0.054782-0.055083

0.076712

0.43744-2.6289-0.18496

2.666

1.3725-0.30454

0.090883

0.068661

0.49088-2.6784-0.22443

2.63191.37250.24799

0.10284

0.11132

0.49518-2.6672-0.15669

2.63780.85059

0.26507-0.011012

0.0010318

0.44008-2.6784-0.10073

2.649

0.85059-0.29276

0.11954-0.036163

0.46815-2.4157-0.094837

2.6266-0.35402-0.63853…
…
以下、観測した姿勢の数だけ続く（約 50）キャリブレーション用データの流れΣei2 を最小化するパラメータを求める

正修
いろいろな姿勢を取る
サーボ
サーボ
サーボ
位置指令
位置指令
位置指令

関節角度

アームの幾何モデル(順運動学計算)

予測先端座標
ri

座標変換

誤差

ロボットアーム(竹刀先端)

ei=|ri - pi|

自分の手を見る

画像センサー

物体検出

観測した先端座標保存

pi更に誤差を減らす工夫●

機械系のモデルパラメータも一緒に最適化○○●

各関節の角度原点オフセット（ラジコンでいうところの「トリム」）
各リンクの長さ ×３自由度 ; 上腕、下腕、竹刀×4自由度

まとめると、以下の全ての変数を最適化
○
○
○
○
○

回転 3
並進 3
y,z軸方向への拡大縮小 ２ (１自由度分はリンク長の等倍と等価 )リンク長 3
トリム 4 (最初の軸は回転と等価 )キャリブレーション結果の例
初期値 (あまり重要でない)：
S,R=単位行列, t=零ベクトル
Lx=定規で計測したアーム長さ

結果

Levenberg-Marquardt法

t’ =(回転は、計算中は回転ベクトルで表現)

カメラから見たアームの正面向き
（Z軸)

SR =
0.9793281 0.0655680 0.1913563-0.0664952 1.0027907 -0.0032941-0.1914953 -0.0094680 0.9832837

133.55 161.49 169.74

回転とスケール
約10度鉛直軸回り

カメラから見たアームの原点位置

初期平均誤差:205mm

上腕(L0+L1): 117 → 124.35 (+7.735)
下腕(L2+L3): 121 → 123.45 (+2.448)
手首-剣先(L4): 337 → 311.703 (-25.297)

最終平均誤差:8.24mm

軸原点オフセット [rad]0.00000, 0.03636, -0.02826, 0.02369, -0.05309

ロボットの形状も補正

最大補正 で 2.8度

打突タスクの分解(再掲)●1. 標的の検出・位置の認識○●

2. 動作計画○○○○●

センサーで位置を計測できるようになる
順運動学計算■ 関節角度から腕の先端の動きがわかる
アームとセンサーのキャリブレーション■ 自分の腕がどこにどう付いているかを知る
逆運動学計算■ 接触する瞬間の姿勢を生成する
経路の生成・選択■ うまく打突できるような竹刀の振り方を考える

3. 動作の実行○

角度指令を順次出力する

逆運動学計算(キャリブレーションと比較した図)
順運動学計算ができれば、反復計算により逆運動学計算ができる
修正
サーボ
位置指令

関節角度

アームの幾何モデル(順運動学計算)

座標変換

ロボットアーム(竹刀先端)

誤差
e=|r - p|

標的
画像センサー

予測先端座標r

物体検出

観測した標的座標 p逆運動学計算
●
●
●
●
●
●
●

打撃完了姿勢を生成するには？
接触している：竹刀と標的の距離が０
竹刀と標的の距離は関節角度(5次元)の関数
これも非線形最小二乗問題
竹刀先端と標的が近付く方向に姿勢を少しずつ変える(最急降下法等)問題：根本の軸の角度の微係数が大きいので先に動く
手首関節と標的の距離が竹刀の長さに一致するとき最小となる関数をコストに加算

関節による微係数の違い
関数
f(θ0, …, θ4)=(竹刀先端と標的のユークリッド距離)を最小化
同じ回転角なら中心側関節の方が竹刀先端を大きく動かす↓微係数の絶対値が大きい↓
そのため根本関節が先に動いてつんのめる形になりがちらしい動きにするための工夫
関数
f(θ0, …, θ4)=
(竹刀先端と標的のユークリッド距離)+ α・(手首と標的のユークリッド距離 - L4)2を最小化
手首関節と標的との距離を適切に保つ
項を導入
α: 適当な係数

打突タスクの分解(再掲)●1. 標的の検出・位置の認識○●

2. 動作計画○○○○●

センサーで位置を計測できるようになる
順運動学計算■ 関節角度から腕の先端の動きがわかる
アームとセンサーのキャリブレーション■ 自分の腕がどこにどう付いているかを知る
逆運動学計算■ 接触する瞬間の姿勢を生成する
経路の生成・選択■ うまく打突できるような竹刀の振り方を考える

3. 動作の実行○

角度指令を順次出力する

経由姿勢(waypoint)の設定●●

正面、左右４５度・９０度に振りかぶる姿勢を用意
構え→経由姿勢→打撃完了姿勢→経由姿勢→構え

何のため？
● １．打撃らしく振りかぶるため○●

２．反復逆運動学計算の都合
○
○
○

●

中段の構えから直接打撃完了姿勢に行くと「突き」に近い
関節可動域には限界がある
局所最適解に陥ってしまうケース
初期値として使用、誤差が小さく収束したものを選択

３．障害物回避のため○○

ヒューリスティックス
胴、小手の場合必ず横や斜めに振りかぶる姿勢を採用

ホームポジション (中段)

正面打ち前の経由姿勢

４５°（左右面あるいは小手）

９０°（胴）

自動打撃のデモ●「面を打つ」指示

試合の戦略 / アルゴリズム●

ROBO-剣のルール
○
○

1.
2.
3.
4.
5.

同じ技の連続攻撃禁止 (3秒以内)
異なる技で連続攻撃最大 3回まで、その後 3秒攻撃禁止

打っても良い箇所の集合を求める
a ⊆ {x ∈ {小手、面、胴}}センサーで発見した標的の集合bを求める
a∩bからランダムに選んで打つ → 経路計画へ打った箇所と時間を記録、連続技回数を加算「中段」の構えに戻る

その他、気づいたこと●

対象位置検出精度は重要○○●止まっている相手にしか当たらない○●●

昨年より精度・信頼性向上
キャリブレーションが自動で精度よくできるようになった相手のモーション開始後射程に入る →１秒ぐらいかかって到達 →もうその場所にはいない

隙のないホームポジションによる防御は有効
相手が構えていると遠くてどこも打てないことがある今後の拡張●動く標的への対応○○●

標的の移動を多項式で近似
自分が振り下ろすのにかかる時間を考慮、未来の位置を狙う

より賢い障害物回避○○○

標的以外の障害物（相手の竹刀、フレーム、鍔など）も見えているので …
１．振り下ろす軌道上にないことを確認してから打つ
２．迷路探索のように障害物を回避する軌道を生成する

Chapter 3:
SoftwareSystemIntegration

ソフトウェアシステム構成●●●

Linux (Ubuntu 16.04 LTS)
ROS LunarC++ROSノード構成
既成パッケージのノード

自作ノード

vertex_rgb_data
target_detector

ros_depthsense_camera_node

標的座標kondo_server
モーション再生
サーボ現在位置取得
動作完了通知soundplaymain・動作生成
・対戦戦略
・座標変換・運動学計算等をすべて含む
本当は分離したい

keydown

keyboard

利用したライブラリ類●

ROSツール○●ROSノード
○
○
○

●

RViz
ros_depthsense_camera: DS325のドライバ・データ取得
sound_play: 音声ファイル再生
keyboard: 手動操作用 （開始・終了・脱力・デバッグ用動作）

ライブラリ
○
○
○
○

pcl: ポイントクラウド操作
cminpack: Levenberg-Marquardt法による非線形最適化
gflags: コマンドラインオプションの処理
protobuf: Protocol Buffer、構造化データクラス生成とシリアライズ・デシリアライズ■ （ディスクへの保存に）

補足●

作業時間：およそ４日
○○
○
○

●

使わなかったコードや実験を除く
昨年までのシステムを ROSへ移植
ポイントクラウドからの物体検出を新規実装
大会当日早朝に初めて全機能が揃った

MoveIt! は未使用○○○○

多関節アームの動作プランニングを行える ROSのライブラリ
既に昨年までに作っていたプログラムを移植したため使用せず
アームの寸法等をキャリブレーションするのは入門書の範囲を超えると予想
一応、control_msgs/FollowJointTrajectgoryGoalをメッセージ型に採用

ソフトウェアの一部公開
●
●
●
●
●
●
●
●

GitHubにてLinux用のKONDO ICS通信ライブラリを公開
https://github.com/yamaguchi-am/kondo-ics
PCから直接KONDOのロボット用サーボを制御する(これをもとにROSノード作成した)三種類のボーレートに対応(115200bps, 625000bps, 1.25Mbps)
ICS 3.5に対応
ID以外のEEPROM変更操作には未対応(開発中)
ICS 3.6で増えたコマンドには未対応

Appendix

大会以外でのロボットの活用

柄内部にパワー LED
透明樹脂の竹刀を光らせる

一発芸の書道
時系列の直接教示を再生

ハロウィンの仮装
時系列の直接教示を再生名前の由来逡巡(しゅんじゅん)
ためらい。
また、10-14等の小さい数を表す語としても使われる。(塵劫記)
類似の小数を表す語で「瞬息」「弾指」「刹那」は極めて短い時間の意味がある。
c.f. 阿伽羅（あから）：10の224乗「あから2010」 情報処理学会のコンピュータ将棋プログラムの名に採用された

別のアプローチで手先のカメラフィードバック制御のみ