ロボットアーム(ベクトル内積を使った)ジェスチャーコントロール *目指せ老後の食事介助アーム*
ロボットアーム(ベクトル内積を使った)ジェスチャーコントロール *目指せ老後の食事介助アーム*

ロボットアーム(ベクトル内積を使った)ジェスチャーコントロール *目指せ老後の食事介助アーム*

介護の経験から、嚥下障害と手先が不自由になった母親に食事を食べさせるのは時間がかかり根気のいる介護です。自分も、もし、大変老後介護が必要となった時、自分で楽しみの食事ができるように!食べてる実感を感じられるように! ちょっとこんなんができればいいなぁ~との発想から考えてみました。ただいま試作中(^^♪

これは、ノートに記してはいますが、私の備忘録です。 皆さんにとってご参考になれば幸いです。(^^♪

案

やり方としましては、VS CODEでpythonでMediapipeを実装し、シリアル通信でArduino IDEにシリアル値(角度)を送信し、サーボモーターを動かします。

まずは、Mediapipe poseのlandmark選定です。ここが、アームを動かすうえで重要だと思います。1つのパターンで1つのサーボモーターをうごかして、このパターンをほかのサーボモーター4つに適用して実装していきます。

pose

11,13,15のランドマークを使い角度を出していきます。どこの角度検出をするかで、アームのどのサーボモーターに当てはめるかは、少し迷いますが、まずは、この3点にしました。。

# ランドマーク15, 13, 11の間の角度を計算
        lm_15 = landmarks[mp_pose.PoseLandmark.LEFT_WRIST.value]
        lm_13 = landmarks[mp_pose.PoseLandmark.LEFT_ELBOW.value]
        lm_11 = landmarks[mp_pose.PoseLandmark.LEFT_SHOULDER.value]
        angle_15_13_11 = calculate_angle(lm_15.x, lm_15.y, lm_13.x, lm_13.y, lm_11.x, lm_11.y)
angle

いよいよプログラムに入ります。

import cv2
import mediapipe as mp
import time
import math
import serial

# シリアルポートの設定Arduinoと接続されているポートを指定
ser = serial.Serial('COM6', 9600)  # ポート名は随時変更確認

# Mediapipeの初期化
mp_pose = mp.solutions.pose
pose = mp_pose.Pose()
mp_drawing = mp.solutions.drawing_utils

# カメラのキャプチャパソコンの内蔵カメラ”0”)
cap = cv2.VideoCapture(0)


#map_value関数
def map_value(value, min_input, max_input, min_output, max_output):
    # valueをmin_inputからmax_inputの範囲からmin_outputからmax_outputの範囲にマッピング
    return min_output + (max_output - min_output) * ((value - min_input) / (max_input - min_input))
#角度計算
def calculate_angle(x1, y1, x2, y2, x3, y3):
    # ベクトルABとBCを計算
    vec_AB = (x2 - x1, y2 - y1)
    vec_BC = (x3 - x2, y3 - y2)

    # ベクトルの内積と大きさを計算
    dot_product = vec_AB[0] * vec_BC[0] + vec_AB[1] * vec_BC[1]
    magnitude_AB = math.sqrt(vec_AB[0]**2 + vec_AB[1]**2)
    magnitude_BC = math.sqrt(vec_BC[0]**2 + vec_BC[1]**2)

    # 角度を計算アークコサイン
    angle_rad = math.acos(dot_product / (magnitude_AB * magnitude_BC))
    angle_deg = math.degrees(angle_rad)
    return angle_deg

while cap.isOpened():
    success, image = cap.read()
    if not success:
        print("カメラからフレームを取得できませんでした")
        break

    # Mediapipe用に画像をRGBに変換
    image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    results = pose.process(image_rgb)

    if results.pose_landmarks:
        # ポーズランドマークを描画
        mp_drawing.draw_landmarks(image, results.pose_landmarks, mp_pose.POSE_CONNECTIONS)

        # ランドマークの位置を取得
        landmarks = results.pose_landmarks.landmark

        # ランドマーク15, 13, 11の間の角度を計算
        lm_15 = landmarks[mp_pose.PoseLandmark.LEFT_WRIST.value]
        lm_13 = landmarks[mp_pose.PoseLandmark.LEFT_ELBOW.value]
        lm_11 = landmarks[mp_pose.PoseLandmark.LEFT_SHOULDER.value]
        angle_15_13_11 = calculate_angle(lm_15.x, lm_15.y, lm_13.x, lm_13.y, lm_11.x, lm_11.y)
        
        # 角度をサーボモーターの角度に変換(30度から130度の範囲にマッピング
        servo_angle = map_value(angle_15_13_11, 0, 180, 180, 20)

        # Arduinoに角度を送信
        ser.write(f"{int(servo_angle)}\n".encode())

        # ランドマークと対応するサーボの角度を表示
        cv2.putText(image, f'Servo: {int(servo_angle)}', (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA)

    # 画像を表示
    cv2.imshow('Mediapipe Pose', image)

    # 'q'キーで終了
    if cv2.waitKey(5) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()
ser.close()

このプログラムの考え方ベクトル内積を説明していきます。

#角度計算
def calculate_angle(x1, y1, x2, y2, x3, y3):
    # ベクトルABとBCを計算
    vec_AB = (x2 - x1, y2 - y1)
    vec_BC = (x3 - x2, y3 - y2)

    # ベクトルの内積と大きさを計算 dot_productドット積は内積の意
    dot_product = vec_AB[0] * vec_BC[0] + vec_AB[1] * vec_BC[1]
ベクトル

説明ベクトル内積から角度を求める。(Mediapipe_landmark3点のなす角度)

\vec{ a }\vec{b}のなす角度を\thetaとする\vec{ a } \cdot \vec{ b } = \left| \vec{ a } \right| | \vec{ b } | \cos \theta\vec{ a }\vec{b}の内積と呼ぶ
ベクトル内積は成分を用いると
\vec{ a } =(a1, a2),\vec{b} =(b1, b2) のとき

{ \vec{ a } \cdot \vec{ b } = a_1 b_1 + a_2 b_2 } ☚内積の公式
△CAB に余弦定理を適用すると
a^2 = b^2 + c^2 - 2bc \times \cos \theta \cdots 余弦定理
AB^2 = CA^2 + CB^2 - 2CA \times CB \times \cos \theta \cdots
AB = |\vec{ b } - \vec{ a }|, \ CA = |\vec{ a }|, \ CB = \vec{ b }なので
|\vec{ b } - \vec{ a }|^2 = |\vec{ a }|^2 + |\vec{ b }|^2 - 2 |\vec{ a }| |\vec{ b }| \cos \theta
ここでなぜAB = |\vec{ b } - \vec{ a }|になるかというと、
ベクトル\vec{ a }の終点からベクトル\vec{b}の終点に向かうベクトルを考えると、\vec{b}-\vec{ a }
\vec{ a }の終点から\vec{b}の終点に向かう\vec{c}とおくと、同じ始点からのつながりで考えると、\vec{a}+\vec{ c }= \vec{ b }となるので
ゆえに、\vec{ c }=\vec{ b }-\vec{a}が分かる。ここで混乱してはならないことは、ベクトルは大きさだけでなく向きを持った量だということです。
続けると
\vec{ a } = (a_1, \ a_2), \vec{ b } = (b_1, \ b_2)とすると
( b_1 - a_1 )^2 + ( b_2 - a_2 )^2 = {a_1}^2 + {a_2}^2 + {b_1}^2 + {b_2}^2 - 2 |\vec{ a }| |\vec{ b }| \cos \theta
両辺を整理すると
-2|\vec{ a }| |\vec{ b }| \cos \theta =( b_1 - a_1 )^2 + ( b_2 - a_2 )^2 -{a_1}^2 - {a_2}^2 - {b_1}^2 - {b_2}^2

|\vec{ a }| |\vec{ b }| \cos \theta = a_1 b_1 + a_2 b_2
したがって、

|\vec{ a }| |\vec{ b }| \cos \theta = a_1 b_1 + a_2 b_2

より
{ \vec{ a } \cdot \vec{ b } = a_1 b_1 + a_2 b_2 } }

わかりやすく簡単な例題で解いてみましょう(^^♪

\vec{ a }=(1,2)と\vec{b}=(3,1)のなす角度を\thetaを求めるなら。。。。
\vec{ a }\vec{b}= 3+2 = 5
| \vec{ a }|= \sqrt{1^2 + 2^2 } = \sqrt{5}
| \vec{ b }|= \sqrt{3^2 + 1^2 } = \sqrt{10}
よって
\cos \theta = \frac{\vec{ a }・\vec{ b }}{| \vec{ a } \| | \vec{ b } |}
= \frac{5}{ \sqrt{5} \times\sqrt{10}}
= \frac{1}{ \sqrt{2}}
\theta = 45°

簡単な例で解くとわかりやすいですね~

def calculate_angle(x1, y1, x2, y2, x3, y3):
    # ベクトルABとBCを計算
    vec_AB = (x2 - x1, y2 - y1)
    vec_BC = (x3 - x2, y3 - y2)

    # ベクトルの内積と大きさを計算
    dot_product = vec_AB[0] * vec_BC[0] + vec_AB[1] * vec_BC[1]
    magnitude_AB = math.sqrt(vec_AB[0]**2 + vec_AB[1]**2)
    magnitude_BC = math.sqrt(vec_BC[0]**2 + vec_BC[1]**2)

ベクトルの大きさ(長さ)を計算しています。

ベクトルの大きさとは?
ベクトルの大きさとは、そのベクトルが持つ長さのことです。ベクトルを例に説明します

ベクトルとは?
ベクトルは、方向と大きさを持つ量です。例えば、点Aから点Bへのベクトルを考えるとします。

ベクトルABのx成分 (vec_AB[0]):点Aのx座標から点Bのx座標を引いた値
ベクトルABのy成分 (vec_AB[1]):点Aのy座標から点Bのy座標を引いた値
大きさ(長さ)の計算方法

2次元のベクトルの大きさを計算するには、次のピタゴラスの定理を使います:

大きさ = \sqrt{x^2 + y^2 }

これは、直角三角形の斜辺を求めるのと同じです。これでベクトルの大きさを求めれます。

プログラムもいよいよ大詰めです。これは、ロボット工学の本で勉強しました。ロボットに三角関数は不可欠で、ロボットアームの位置を把握するものです。ここで出てくる、苦手だったアークコサイン・タンジェントの使い方が分かりました。

   # 角度を計算アークコサイン
    angle_rad = math.acos(dot_product / (magnitude_AB * magnitude_BC))
    angle_deg = math.degrees(angle_rad)

結論から言うと、math.acos()は引数として与えた数値の逆余弦(アークコサイン)をラジアン単位で返します。

2点でラジアンを出すのは、atan(アークタンジェント)で3点から角度を出すのは、ベクトル内積を使い、acos(アークコサイン)

を使います。acos(アークコサイン)について説明していきます。

アーク

y=cosθのときθ=acos y です

acos y のyをcosθに置き換えると、θ=acos(cosθ)

そもそも角度を弧度法の単位であるラジアンで表すことは、角度θが見込む半径1の円上のアーク(弧)APの長さで表すということです。

例えばθ=360°であれば、単位円の全周だから半径r=1の円の円周は2πr=2πだからθ=2π(rad)である。

又、θ=60°であれば、アーク(弧)APの長さは、円周の1/6になるから、θ=2π/6=π/6(rad)になります。

cos\theta={x}角度θをラジアンであらわすとcos\theta={x} 単位円上のアーク(弧)APの長さがθとなるような点P(x,y(x))のx座標を与える関数が
cos\theta={x}である。
cos\theta={x}の逆関数がcos^{-1}{x}= \thetaである。
つまり単位円上の点A(1,0)からP(x,y(x))までのアーク(弧)APの長さθを与える関数はcos^{-1}{x}= \thetaである。
そしてARCをつけてcos^{-1}{x}= \theta=arcos x と表示します。

angle_deg = math.degrees(angle_rad)

アークコサインは角度で、そのコサインは数値になります。

返される角度は0(ゼロ)からPi(π)の範囲のラジアン単位になります

結果をラジアンから角度に変換するには、結果を180/Pi(π)で乗算するかDgrees関数を使用します。

例えば

Acos(-0.5)は「2.0943951」

Acos(-0.5)*180/Pi は「120」を返します。

Dgrees(Acos(-0.5))は「120」になります。

次はArduinoでアーム(サーボモーター)を動かす。Arduino IDEでのコードです。

//************Arduino IDE*************
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// PCA9685の初期化
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// サーボの最小・最大パルス幅
#define SERVOMIN  150 // パルス幅最小値
#define SERVOMAX  600 // パルス幅最大値

// サーボのチャンネル
#define SERVO_0_CHANNEL 0

void setup() {
  Serial.begin(9600);
  pwm.begin();
  pwm.setPWMFreq(60);  // サーボ周波数を60Hzに設定
  delay(10);
}

void loop() {
  if (Serial.available()) {
    int angle = Serial.parseInt();

    // サーボ角度をPWM信号に変換して設定
    int pulse_0 = map(angle, 0, 180, SERVOMIN, SERVOMAX);
    pwm.setPWM(SERVO_0_CHANNEL, 0, pulse_0);
  }
}
ホーム » 気分転換 » ロボットアーム(ベクトル内積を使った)ジェスチャーコントロール *目指せ老後の食事介助アーム*

投稿





コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です