ドキュメントジェスチャーによるロボットアームの動作制御実験
実験の目的：
- ロボットアームの運動学アルゴリズムを理解します。2. 運動学アルゴリズムを組み合わせたプログラミング制御により、円弧補間制御を実現します。
基本的な動作原理：
-
3 軸ロボットアームの先端に旋回モジュールを追加し、エンドエフェクタのグリッパーの向きを制御できるようにしています。
-
取り付け位置は、回転軸を鉛直下向き、x 軸方向、y 軸方向などにすることができます。ただし、第 4 軸は一般に 3 軸構成では先端を平行に保てないという問題を補うために使われるため、直列 4 軸の第 4 関節は通常、第 2・第 3 関節と同じ回転方向にします。
-
運動学アルゴリズムは必要に応じてダウンロードしてください：直列4自由度ロボットアームの運動学計算.pdf
ハードウェアの準備
ディスプレイ × 1、Bluetooth キーボード・マウス × 1、メインコンソール × 1、サーボ × 4、4 自由度ロボットアーム × 1、装着型ジャイロスコープモジュール × 1
回路接続：
ハードウェアモジュール
ヒント：ハードウェアモジュールの使用説明
ハードウェアシステムの全体組立
電子モジュールの紹介
物理配線
ロボットアームのベースをメインコンソールに固定します。具体的な取り付け方法はハードウェアシステムの全体組立を参照してください。
装着型ジャイロスコープモジュールを手首に固定します。
ソフトウェアの実装
設計フローチャート
ソースコードのダウンロード
ソースコードのリンク：（HumansX サポートより提供）
プログラムコード
コードの構成：
| ファイル名 | 機能説明 |
|---|---|
| imu_control_arm.py | メインプログラム。Socket サーバーとしてジャイロスコープのデータを受信し、ロボットアームの動きを制御します |
| custom_font.py | ターミナル出力の装飾ツール |
| manager.py | 制御の中枢。すべての下位モジュールを統合管理します |
| process_gyro_data.py | ジャイロスコープのデータ処理と姿勢計算 |
| serial_section.py | シリアル通信の管理 |
| servo_section.py | サーボ制御プロトコルの生成 |
| user_arm.py | ロボットアームの逆運動学アルゴリズム。直線補間と円弧補間を実現できます |
具体的なコード：
プロジェクトの完全なコードは（ソースコードのリンク）からダウンロードしてください。
"""
--------------------------------------------------------------------------------
功能:通过socket获取手持陀螺仪模块数据,并控制机械臂运动
日期:2025-06-03
--------------------------------------------------------------------------------
"""
import socket
import json
import threading
import queue
import math
import time
import numpy as np
from src.arm_control import custom_font
from src.arm_control import manager
from src.arm_control import process_gyro_data
# from src.arm_control import draw_action
# action = draw_action.CircleDetector()
process_gyro = process_gyro_data.ProcessGyro()
# 创建线程间通信的队列
arm_command_queue = queue.Queue(maxsize=1) # 限制队列大小防止积压
# 初始化位移跟踪器
# displacement_tracker = DisplacementTracker()
roll_windows = [0.0, 0.0]
pitch_windows = [0.0, 0.0]
windows_size = 2
last_yaw_data = 0
gyro_turn_left_flag = False
gyro_turn_right_flag = False
left_numbers = 0
right_numbers = 0
loop_left = False
loop_right = False
loop_shows = False
loop_numbers = 0
def arm_worker():
global roll_windows, pitch_windows, last_yaw_data, loop_left, loop_right, loop_shows
global left_numbers,gyro_turn_left_flag,gyro_turn_right_flag, right_numbers, loop_numbers
"""机械臂控制线程"""
node = manager.RobotManager()
left = False
right = False
action_finish = False
action_loop = False
action_numbers = 0
action_numbers_left = 0
action_numbers_right = 0
action_left_start = False
action_right_start = False
action_numbers_count = 0
data_l_num = 0
data_r_num = 0
input_point = 1
while True:
command = arm_command_queue.get() # 获取包含两个参数的元组
try:
pitch, roll, sensor_data, yaw, time_delay = command
delta_yaw = yaw - last_yaw_data
last_yaw_data = yaw
# print(sensor_data)
# print(sensor_data['accel']['x'])
# print(sensor_data['gyro']['z'])
# print("yaw:", yaw)
# print("roll:", roll, "pitch:", pitch)
# 原有控制逻辑
if roll >= 55:
node.save_ang[1] -= 4
input_point = 1
elif roll <= -55:
node.save_ang[1] += 4
input_point = 1
elif pitch >= 50:
node.save_ang[0] += 4
input_point = 1
elif pitch <= -50:
node.save_ang[0] -= 4
input_point = 1
elif (sensor_data['accel']['z'] < -1.6) and (sensor_data['accel']['y'] <= 0.5) and (sensor_data['accel']['y'] >= -0.2):
node.release_sth()
elif (sensor_data['accel']['z'] > -0.6) and (sensor_data['accel']['y'] <= 0.5) and (sensor_data['accel']['y'] >= -0.2):
node.catch_sth()
elif sensor_data['gyro']['z'] < -55:
# node.save_ang[2] = node.save_ang[2] + 10
node.save_ang[2] = 100
input_point = 5
elif sensor_data['gyro']['z'] > 55:
# node.save_ang[2] = node.save_ang[2] - 10
node.save_ang[2] = 0
input_point = 5
node.arm_move(node.save_ang[0], node.save_ang[1], node.save_ang[2], input_point)
except Exception as e:
print(f"[ARM ERROR] 机械臂控制错误: {e}")
# 启动机械臂控制线程
arm_thread = threading.Thread(target=arm_worker, daemon=True)
arm_thread.start()
# 主线程处理陀螺仪数据
node = manager.RobotManager()
font = custom_font.CustomFont()
print(font.color_text(font.compony_msg, color="cyan", bold=True))
HOST = '0.0.0.0'
PORT = 9000
print(f"[INFO] 启动Socket服务端, 监听端口 {PORT} ...")
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind((HOST, PORT))
s.listen(1)
print("[INFO] 等待Arduino客户端连接...")
conn, addr = s.accept()
with conn:
print(f"[INFO] 已连接: {addr}")
buffer = b''
while True:
data = conn.recv(1024)
if not data:
print("[WARN] 客户端断开连接")
break
buffer += data
while b'\n' in buffer:
line, buffer = buffer.split(b'\n', 1)
try:
sensor_data = json.loads(line.decode('utf-8'))
# 原有角度计算
if not process_gyro.calibration_complete:
process_gyro.calibration_gyro(sensor_data['accel'])
else:
data = time.time()
roll, pitch, yaw, time_delay= process_gyro.calculate_angles_back(sensor_data['accel'], sensor_data['gyro'], data)
# action.update(sensor_data['accel'])
# 非阻塞方式发送命令到机械臂线程
if arm_command_queue.empty():
# 添加位移数据到队列
arm_command_queue.put_nowait((
pitch, roll, sensor_data, yaw, time_delay
))
except Exception as e:
print(f"[ERROR] 解码失败: {e}")
操作手順：
デバッグ手順
- ターミナルからリモート接続し、ファイルのあるシステムに入ります。ターミナルに次のコマンドを入力して Enter を押します
ssh sunrise@192.168.8.111
sunrise
パスワードを入力して Enter を押すとログインが完了します。
- ロボットアームを初期化角度に調整します。
上腕と前腕の長さを測定して記録します。例えばテスト時には、上腕の実測長は 109cm、前腕の長さは 126cm でした。
プログラムを開き、赤枠部分のコードを修正します：
- ロボットアーム初期化状態でのサーボの角度を記録します。
ロボットアームが初期状態にあるときのサーボ値を記録し、次の表に記入します。以下はテスト時のデータ値です：
| ハードウェア | ロボットアーム初期状態に対応するサーボ値 | 対応するパラメータ |
|---|---|---|
| 旋回台サーボ | 83 | alpha |
| 上腕関節 | 100.9 | theta |
| 前腕関節 | 95 | beta |
| グリッパー | 141 | gamma |
- 運動学関数 void get_coordinate(float x1,float y1,float z1) の初期化パラメータを同期して更新します。パラメータ 83,100.9,95,141 を図の赤枠部分に反映してください。
- ロボットアームの可動範囲から離れた状態でスイッチを入れ、プログラムを実行します。
注意：ロボットアームの動作が速いと先端の慣性が大きくなります。起動時は、ぶつからないようロボットアームの球面状の可動範囲に入らないようにしてください。
再現時の注意点
-
再現する際は、各関節サーボの出力軸に取り付けるサーボホーンの向きが、実物写真やデモ動画と一致していることに注意してください。これはロボットアーム各関節の回転方向に影響し、運動学式における方向と関係します。向きが間違っていると、ある関節の回転角度や動作方向が大きくずれることになります。
-
ロボットアームの初期化状態では、各関節の角度をできるだけサーボの中央付近の範囲（80～150）に収めることを推奨します。この範囲外の場合は、構造の分解・組み直しによって調整することを推奨します。これはロボットアームの可動範囲に影響します。サーボの回転範囲は 0～270 度で中央値は 135 のため、この付近にすると関節サーボの可動範囲が最大になります。中央値の範囲が不適切だとロボットアームの到達可能な点が減少し、本来到達できるはずの点が運動学式で計算した結果解なしとなり、理論上の到達点に届かなくなります。
具体的な操作
回路接続が完了したら、ターミナルで操作します：
- ターミナルからリモート接続します。ターミナルに次のコマンドを入力して Enter を押します
ssh sunrise@192.168.8.111
sunrise
パスワードを入力して Enter を押すとログインが完了します。
- 指定のディレクトリに移動してターミナルを開き、次のように入力します：
cd ~/Desktop/Arm_Experiment/module_control_servo
上記で開いたターミナルで、ジャイロスコープによるロボットアーム制御プログラムを実行します：
python imu_control_arm.py
テスト時の操作コマンドは下図のとおりです。必要に応じて参照してください。
- ハンドヘルドジャイロスコープモジュールを回し、ロボットアームが動くかを観察します。
注意： 本プログラムでデバッグしたジャイロスコープのパラメータは、実際の状況に応じて調整が必要な場合があります。本実験におけるハンドヘルドジャイロスコープモジュールの操作方法は次のとおりです：
(1) ハンドヘルドジャイロスコープモジュールを左に一定角度以上傾けるとロボットアームは左へ移動し、右に一定角度以上傾けるとロボットアームは右へ移動します。
(2) ハンドヘルドジャイロスコープモジュールを前に一定角度以上傾けるとロボットアームは前進し、後ろに一定角度以上傾けるとロボットアームは後退します。
(3) ハンドヘルドジャイロスコープモジュールを水平のまま左に一定角度以上回すとロボットアームは上へ移動し、水平のまま右に一定角度以上回すとロボットアームは下へ移動します。
(4) ハンドヘルドジャイロスコープモジュールをすばやく上に動かすとグリッパーが閉じ、すばやく下に動かすとグリッパーが開きます。
正常に実行されると、ターミナルには次の内容が表示されます：
--------------------------------------------------------------------------------------
_ _ _ _______ _
| | | | (_) |__ __| | |
| | | |_ __ _ | | ___ _ __ __ _ ___| |__ _ _ __ _ _ __ __ _
| | | | '_ \| | | |/ _ \| '_ \ / _` |/ __| '_ \| | | |/ _` | '_ \ / _` |
| |__| | | | | | | | (_) | | | | (_| | (__| | | | |_| | (_| | | | | (_| |
\____/|_| |_|_| |_|\___/|_| |_|\__, |\___|_| |_|\__,_|\__,_|_| |_|\__, |
__/ | __/ |
|___/ |___/
--------------------------------------------------------------------------------------
[INFO] 启动Socket服务端, 监听端口 9000 ...
[INFO] 等待Arduino客户端连接...
[INFO] 已连接: ('192.168.8.235', 64437)
陀螺仪模块标定中..................................................
.----------------------------.
| |
| |
| 陀螺仪校准完成 |
| |
| |
.----------------------------.
滚转角初始值:-0.41°
俯仰角初始值:3.20°
実験結果：
姿勢センサーを回転させ、ロボットアームの動きを観察します。
インターフェースドキュメント
さらに詳しくは、以下の API インターフェースドキュメントを参照して二次開発や最適化を行うことができます。
ロボット本体インターフェースの全体紹介
お問い合わせ
資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。