実験2 運動学と直線補間を用いた3自由度ロボットアームの把持

実験の目的：

ロボットアームの運動学アルゴリズムを理解します。
デカルト空間における軌道計画（直線補間アルゴリズムなど）を理解します。
アルゴリズムを組み合わせて、ロボットアームの動作を制御できるようにします。
STM32 の GPIO 周辺機能ピンの設定方法を習得します。
STM32CubeMX で新規プロジェクトを作成し、MDK_ARM プロジェクトファイルを生成できるようにします。
keil IDE でユーザーコードを追加し、プロジェクトのテストを行えるようにします。

基本原理：

3自由度ロボットアームは、本章の実験1のパン・チルトに、第2軸と同じ向きのスイングモジュールを1つ追加したものと考えることができます。
3自由度ロボットアームは、腕の長さの範囲内にある3次元空間の任意の点（x, y, z）に到達する基本的な能力を備えています。
3自由度ロボットアームの運動学計算：直列3自由度ロボットアームの運動学計算.pdf

機能要件：

3自由度ロボットアームで逆運動学を呼び出して直線補間動作を行い、矩形の軌跡を描くと同時に、把持動作を模擬します。

ハードウェアの準備：

STM32 メインボード V1.3 *1、ST-Link ダウンロードケーブル *1、3S バッテリーボックス *1、リチウム電池 *3、サーボ *3、グリッパー *1

試作構成：

STP ファイル：3自由度ロボットアームSTP.rar

回路接続：

ハードウェアモジュール

ヒント：開発ボード、環境設定、電源モジュールの基本的な使い方に習熟しておくことを強くお勧めします。電子モジュールについては必要に応じて参照してください。

UNI-STM32 開発ボード【V1.3】の紹介

UNI-STM32 開発環境の設定【V2.0】

バッテリーおよび電源モジュール使用上の注意（必読）

電子モジュールの紹介

ピンの紹介

ロボットアーム	STM32 開発ボード
ターンテーブルサーボ	PA2
上腕サーボ	PA3
前腕サーボ	PH9
グリッパーサーボ	PH6

まず サーボインターフェース の位置を全体的に把握し、その後ピン表に従って配線を完了します。

実際の配線

配線のコツ：サーボを配線する際は、GND が開発ボードの GND と対応するように注意してください。

ソフトウェア設計：

設計フロー

プロジェクトのダウンロード

ソースファイル：02.ThreeDofArm_linearInterpolation.zip

プログラムコード

コアコード：main.c。プロジェクトの完全なコードは（ソースファイル）をクリックしてダウンロードしてください。

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "servo.h"
#include "three_dof_arm.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM2_Init();
  MX_USART1_UART_Init();
  MX_TIM12_Init();
  /* USER CODE BEGIN 2 */
  //机械臂各个舵机初始化
  servo_init();

  //机械臂复位（机械臂逆运动学各个舵机恢复到初始角度）
  arm_reset();
  HAL_Delay(1500);

  //机械臂回到家位置
  arm_home();
  HAL_Delay(1500);

  uint32_t action_delay1 = 200;
  uint32_t action_delay2 = 350;
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */

    //动作1： 机械臂回到家位置
    arm_home();
    HAL_Delay(1500);

    //动作2：爪子张开/闭合/张开
    claw(Arm_Claw_Open);
    HAL_Delay(350);
    claw(Arm_Claw_Off);
    HAL_Delay(350);
    claw(Arm_Claw_Open);
    HAL_Delay(350);


    //动作3：机械臂直线插补运动画矩形（高度80.0毫米）
    float arm_height1 = 80.0f;
    int numPoints1 = 80;
    linearInterpolation(  50.0,   0.0,   arm_height1,  numPoints1,  6 );
    HAL_Delay(action_delay1);
    linearInterpolation(  50.0,  -50.0,  arm_height1,  numPoints1,  4 );
    HAL_Delay(action_delay1);
    linearInterpolation( 150.0,  -50.0,  arm_height1,  numPoints1,  4 );
    HAL_Delay(action_delay1);
    linearInterpolation( 150.0,   50.0,  arm_height1,  numPoints1,  6 );
    HAL_Delay(action_delay1);
    linearInterpolation(  50.0,   50.0,  arm_height1,  numPoints1,  6 );
    HAL_Delay(action_delay1);
    linearInterpolation(  50.0,   0.0,   arm_height1,  numPoints1,  6 );
    HAL_Delay(action_delay1);

    //动作4： 机械臂回到家位置
    arm_home();
    HAL_Delay(1500);


    //动作5：机械臂直线插补运动画矩形（高度0.0毫米）
    float arm_height2 = 0.0f;
    int numPoints2 = 80;
    linearInterpolation(  50.0,   0.0,   arm_height2,  numPoints2,  6 );
    HAL_Delay(action_delay2);
    linearInterpolation(  50.0,  -50.0,  arm_height2,  numPoints2,  4 );
    HAL_Delay(action_delay2);
    linearInterpolation( 150.0,  -50.0,  arm_height2,  numPoints2,  4 );
    HAL_Delay(action_delay2);
    linearInterpolation( 150.0,   50.0,  arm_height2,  numPoints2,  6 );
    HAL_Delay(action_delay2);
    linearInterpolation(  50.0,   50.0,  arm_height2,  numPoints2,  6 );
    HAL_Delay(action_delay2);
    linearInterpolation(  50.0,   0.0,   arm_height2,  numPoints2,  6 );
    HAL_Delay(action_delay2);
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 4;
  RCC_OscInitStruct.PLL.PLLN = 84;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

操作手順：

全体の流れ：

(1) STM32CubeMX ソフトウェアでピン割り当てと周辺機能の初期化を設定し、設定コードを生成します。

(2) keil5 で MDK_ARM プロジェクトファイルを開き、コードの編集・コンパイル・デバッグを行います。

基本パラメータ：

本ドキュメントの（ハードウェアモジュール）に従って開発環境を設定した後の基本パラメータは次のとおりです。

ソフトウェアバージョン：STM32CubeMX V6.14.0

ハードウェアプラットフォーム：STM32F407IGT6

コンパイル環境：MDK_ARM V5.3.2

デバッグツール：ST-LINK/V2

1. 新規プロジェクトの作成と MCU の設定

1.1 MCU の選択

STM32CubeMX ソフトウェアを開き、File > New Project をクリックします。

2. MCU の絞り込み条件を設定して STM32F407IGT6 を選択し、ボード型番の列をクリックしてから、最後に Start Project をクリックしてプロジェクトの設定を開始します。

プロジェクト設定画面は次のとおりです。

1.2 クロック設定とコア設定

Pinout & Configuration の System Core で、RCC（リセットおよびクロック制御） をクリックします。

HSE クロックソースを Crystal/Ceramic Resonator（水晶またはセラミック振動子）に設定します。

設定が完了すると、対応するピン（OSC_IN と OSC_OUT）が自動的に設定されます（下図の緑色の部分を参照）。

機能：

外部高速水晶発振器（HSE）を接続し、システムに高精度のクロック源を供給するために使用します。

**RCC_OSC_IN**：水晶入力ピン。

**RCC_OSC_OUT**：水晶出力ピン（帰還信号）

デフォルト機能：

**PH0**：**OSC_IN**（外部水晶入力）。

**PH1**：**OSC_OUT**（外部水晶出力）。

デバッグインターフェースのピンを設定します。System Core で SYS をクリックし、Debug モードを Serial Wire に設定します。

設定が完了すると、対応するピン配置（pinout）が緑色になります。

緑色の部分をクリックすると、具体的な内容を確認できます。

1.3 クロックツリーの設定

クロックパラメータを順に設定します：1. input frequency の値を 8 に変更、2. HSE を選択、3. PLLCLK を選択、4. HCLK パラメータを 84 に変更。その他のパラメータはデフォルトのままで構いません。

ヒント：設定時にはエラーが自動的に検出されます。周波数設定が誤っている場合は、エラーが赤色で強調表示されます。

2. MCU 初期化パラメータの設定

2.1 USART シリアルポートの設定

PA9 と PA10 をそれぞれ USART1_TX と USART1_RX に設定します。GPIO をクリックし、ピンを設定します（この時点では USART1 が未設定のため、ピンに黄色の警告が表示されます）。

Connectivity — USART1 をクリックし、モードを Asynchronous に設定します（USART1 を設定すると関連ピンが緑色になり、ピン設定が正常であることを示します）。

2.2 プロジェクト管理の設定

デフォルトの画面は次のとおりです。

プロジェクト名を入力します（任意に設定可能）。

プロジェクトの保存先を選択します（C ドライブには保存しないでください）。

コンパイル環境として MDK_ARM を選択します。

バージョン番号 V5.3.2 を選択します。

ファームウェアパッケージ名とバージョン。

（ヒント：デフォルトでは Use latest available version にチェックが入っていますが、ローカル PC に最新バージョンがない場合はエラーになります。その場合はパッケージを自分でダウンロードしてローカルにインストールする必要があります。インストール方法は下記リンクを参照してください。）

UNI-STM32 開発環境の設定【V2.0】（リンク先の「7. よくある設定 Q&A」の Q3 に解決方法があります）

上記の設定が完了すると、下図のようになります。

2.Project Manager > Code Generator ウィンドウを次のように設定します。

3.Project Manager > Advanced Settings ウィンドウで HAL ライブラリを選択します。

プロジェクトを保存します：File — Save Project をクリックします。

3. GPIO 設定

3.1 ターンテーブルと上腕サーボのパラメータ設定

ピン	タイマー	ユーザーラベル
PA2	TIM2_CH3	SERVO1

GPIO をクリックしてピン PA2 を探します。PA2 を左クリックして TIM2_CH3 を選択します（注：ピン割り当て完了後、ピンはオレンジ色になります。これはピン割り当てのみが完了し、対応する周辺機能がまだ有効化されていないことを示します）。

Timers パネルをクリックして TIM2 の設定を選択し、Channel3 のドロップダウンから PWM Generation CH3 を選択します。下の GPIO Settings を左クリックし、PA2 の行をダブルクリックして User Label を SERVO1 に設定します。

ピンの色が緑色に変われば、正常に設定が完了しています。

サーボ S2 のパラメータ

ピン	タイマー
PA3	TIM2_CH4

GPIO をクリックしてピン PA3 を探します。PA3 を左クリックして TIM2_CH4 を選択します（注：ピン割り当て完了後、ピンはオレンジ色になります。これはピン割り当てのみが完了し、対応する周辺機能がまだ有効化されていないことを示します）。

Timers パネルをクリックして TIM2 の設定を選択し、Channel4 のドロップダウンから PWM Generation CH4 を選択します。

2つのサーボのパラメータは次のように設定されていることが確認できます。

プリスケーラ、自動リロード値、デフォルトのパルス値を設定します。TIM2 をクリックし、Parameter Settings パネルを選択します。

設定結果は下図のとおりです。

パラメータ	設定値	意味
Prescaler (PSC)	`84-1`	プリスケーラ。84MHz のシステムクロックを 1MHz に分周します
Counter Period (ARR)	`19999`	自動リロード値。PWM 周期を決定します
auto-reload preload	Enable	自動リロードのプリロードを有効化
サーボのデフォルトパルス Pulse	500	500us=0.5ms。デフォルトのパルス幅を 0.5ms に設定し、サーボの初期角度は 0 度になります。

3.2 前腕とグリッパーサーボのパラメータ設定

ピン	タイマー
PH9	TIM12_CH1
PH6	TIM12_CH2

3.1 のサーボパラメータの設定方法を参考に、前腕とグリッパーのパラメータを設定します。設定後の GPIO Settings のパラメータは、下図の赤枠部分のとおりです。

Parameter Settings をクリックし、プリスケーラ、自動リロード値、デフォルトのパルス値を確認します。

プロジェクトファイルを再度保存します。

ヒント：必ずプロジェクトファイルを再度保存してください（主にパラメータを保存するため）。File — Save Project をクリックします。

4. コードの生成

1. GENERATE CODE をクリックしてコードを生成します。

コードを自動生成しています。

コード生成が成功しました。

5. ユーザーコードの追加

上記で生成したフレームワークをもとに、keil エディタでユーザーコードを追加し、コンパイルとデバッグを行います。

ヒント 1：例えば、main.c の /* USER CODE BEGIN xx */ と /* USER CODE END xx */ の間にユーザーコードを追加します。

ヒント 2：.c ファイルを独自に作成することもできます。

ヒント 3：ユーザーコードは自分で記述できます。（コード解説）に掲載されている例を参考に、真似しながら学習することもできます。

6. コードのダウンロードとテスト

(1) プロジェクトをダウンロードします。ここではデモを用意しています。本ドキュメントの「ソフトウェア設計 — プロジェクトのダウンロード」から直接ダウンロードできます。

参考リンク（UNI-STM32 開発環境の設定【V2.0】の「5. プログラムのダウンロード」）を参照してください。

(2) バッテリースイッチとメインボードのスイッチをオンにし、動作を確認します。

7. ロボットアームの操作・調整方法

自分でロボットアームを組み立てた場合、どのパラメータを設定する必要があるでしょうか。また、どのように調整すれば、ロボットアームを実験動画のとおりに動作させられるでしょうか。デモプログラムと合わせて一緒に見ていきましょう。

ロボットアームを初期化角度に調整します。初期状態は下図のとおりです。

上腕と前腕の長さを測定して記録します。例えばテスト時の実測値は、上腕の長さが 109mm、前腕の長さが 125mm でした。

プログラムを開き、赤枠部分のコードを次のように変更します。

ロボットアームの初期化状態におけるサーボの角度を記録します。

(1) ロボットアームが初期状態にあるときのサーボ値を記録し、下表に記入します。

ハードウェア	ロボットアーム初期状態のサーボ値
ターンテーブルサーボ
上腕サーボ
前腕サーボ
グリッパーサーボ

状態	対応するサーボ値
グリッパー開
グリッパー閉

以下はテスト時のデータ値です。

ハードウェア	ロボットアーム初期状態のサーボ値
ターンテーブルサーボ	135
上腕サーボ	123
前腕サーボ	138

状態	対応するサーボ値
グリッパー開	65
グリッパー閉	142

初期化角度を three_dof_arm.h ファイルに書き込みます。プログラムを開き、パラメータ値 135、123、138、65、142 を対応する位置にコピーします（図の赤枠部分を参照）。

これにより、逆運動学関数 void get_coordinate(float x, float y, float z) を呼び出す際に、3つの定数値（Arm_Alpha_Angle、Arm_Theta_Angle、Arm_Beta_Angle）が使用されます。

これにより、ロボットアームが直線補間を行う際に、逆運動学で各サーボの角度を計算し、ロボットアームの動作を制御します。

ロボットアームの可動範囲から離れた位置でスイッチを入れ、プログラムを実行します。

注意：ロボットアームは動作速度が速く先端の慣性が大きいため、電源を入れる際は必ずロボットアームの球状の可動範囲から離れ、ぶつからないようにしてください。

再現時の注意点

再現する際は、各関節サーボの出力側サーボホーンの取り付け方向を、実物の例示写真およびデモ動画と一致させてください。これはロボットアームの各関節の回転方向に影響し、運動学の式における方向とも関係します。方向が正しくない場合、ある関節の回転角度や運動方向が大きくずれることがあります。
ロボットアームの初期化状態では、各関節の角度をできるだけサーボの中間値付近（80～150）にすることをお勧めします。この範囲外の場合は、構造を分解・再組立して調整してください。これはロボットアームの可動範囲に影響します。サーボの回転範囲は 0～270 度、中間値は 135 で、こうすることで各関節サーボが最大の可動範囲を得られます。中間値の範囲が適切でないと、ロボットアームの到達可能点が減少し、本来到達できるはずの点でも運動学の式で計算すると解が得られず、理論上の到達可能点に到達できなくなります。

実験結果

ロボットアームが逆運動学の初期位置に移動し、その後ホームポジションに移動します。
プログラムは次の動作グループを繰り返し実行し、矩形の軌跡を描きます。

(1) ロボットアームがホームポジションに戻ります。(2) グリッパーが開く／閉じる／開くを行います。(3) ロボットアームが直線補間動作で矩形を描きます（高さ 80.0mm）。

(4) ロボットアームがホームポジションに戻ります。(5) ロボットアームが直線補間動作で矩形を描きます（高さ 0.0mm）。

コード解説

1. ユーザーコードの解説

下図は、プロジェクトファイルに追加したユーザーコードを示しています。

2. ユーザーコードの取り込み方法

1つ目は、ユーザー定義ファイル（.c や .h など）を自分のプロジェクトファイルに取り込み、keil5 IDE で .c ファイルを追加してコンパイルする方法です。

既存のユーザーファイルの取り込み：keil で既存のユーザーファイルを取り込む方法を参照してください。

2つ目は、main.c ファイルにユーザーコードを追加する方法です。

ユーザーコード：/* USER CODE BEGIN xx */ で始まり、/* USER CODE END xx */ で終わる部分が目印です。

(1) まず、参照するプロジェクトファイルのコードを開きます。main.c ファイルは主に stm32cubemx によって自動生成されたものです。このフレームワークをもとに実現したい機能を補完・改良します。具体的には /* USER xx で始まり終わる部分のコードを確認してください。下図の赤枠部分が記入したユーザーコードです。

(2) 上記の赤枠のコードを自分のプロジェクトファイル main.c にコピーし、コンパイルしてダウンロードすると、動作を確認できます。

技術サポート

資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。