実験9：運動学と円弧補間を用いた5自由度ロボットアームの円弧軌道計画

実験の目的：

5自由度直列型マニピュレータの運動学アルゴリズムを理解します。
円弧補間アルゴリズムなど、デカルト空間における軌道計画を理解します。
アルゴリズムと軌道計画を組み合わせて、ロボットアームの動作を制御できるようになります。
STM32 の GPIO 周辺機能のピン設定方法を習得します。
STM32CubeMX で新規プロジェクトを作成し、MDK_ARM プロジェクトファイルを生成できるようになります。
Keil IDE でユーザーコードを追加し、プロジェクトのテストができるようになります。

基本原理：

5自由度マニピュレータは、本章の3自由度マニピュレータに2自由度の手首を追加したものとみなすことができ、先端をあらゆる方向に制御する能力を持ちます。
先端の2自由度の割り当ては具体的な作業シーンによって決まり、特に制限はありません。
運動学アルゴリズムは必要に応じてダウンロードしてください：直列5自由度ロボットアーム運動学計算.pdf

機能要件：

5自由度マニピュレータが逆運動学を呼び出して円弧補間動作を実現し、円弧軌道を描くとともに、グリッパーの開閉を制御します。

ハードウェアの準備：

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

試作機の構成：

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

回路接続：

ハードウェアモジュール

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

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

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

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

電子モジュールの紹介

ピンの説明

5軸ロボットアーム	STM32 開発ボード
旋回台サーボ	PA2
上腕サーボ	PA3
前腕サーボ	PH9
手首サーボ1	PH6
手首サーボ2	PF6
グリッパーサーボ	PF7

まずサーボインターフェースの全体を理解してから、ピン表に従って配線を行ってください。

実際の配線

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

ソフトウェア設計：

設計フロー

メインフロー：

アプリケーション層からのアルゴリズム呼び出しフロー：

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

ソースファイル：09.FiveDofArm_arcInterpolation.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 "five_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();
  MX_TIM10_Init();
  MX_TIM11_Init();
  /* USER CODE BEGIN 2 */
  servo_init();           //机械臂各个舵机初始化

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

  arm_home_horizontal();  //机械臂手腕水平方向回到 家 位置
  HAL_Delay(1500);

  /* USER CODE END 2 */

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

    /* USER CODE BEGIN 3 */

    //机械臂动作：机械臂圆弧插补运动
    arcInterpolation(50, 0, 180, 0, 90, 180, 1);   //圆弧-向上
    claw(Arm_Claw_Open);
    HAL_Delay(50);

    arcInterpolation(180, 0, 50, 0, 90, 180, -1);  //圆弧-向下
    claw(Arm_Claw_Off);
    HAL_Delay(1250);

  }
  /* 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 に設定します。

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

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

1.3 クロックツリーの設定

クロックパラメータを順に設定します：1. 入力周波数の値を 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 前腕サーボと手首サーボ1のパラメータ設定

ピン	タイマー
PH9	TIM12_CH2
PH6	TIM12_CH1

3.1 のサーボパラメータの設定方法を参照して、前腕と手首のパラメータを設定してください。設定後の GPIO Settings のパラメータは以下のとおりです。

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

3.3 手首サーボ2のパラメータ設定

ピン	タイマー
PF6	TIM10_CH1

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

Timers パネルをクリックして TIM10 を選択し、Activated にチェックを入れ、PWM Generation CH1 を選択します。設定後の状態は以下のとおりです。

ピンの色が緑色に変わったことを確認します。

プリスケーラ値、オートリロード値、デフォルトのパルス値を設定します。TIM10 の Parameter Settings パネルをクリックします。デフォルトは次のとおりです。

以下のパラメータを変更する必要があります。

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

設定後の状態は下図のとおりです。

3.4 グリッパーサーボのパラメータ設定

ピン	タイマー
PF7	TIM11_CH1

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

Timers パネルをクリックして TIM11 を選択し、Activated にチェックを入れ、PWM Generation CH1 を選択します。設定後の状態は以下のとおりです。

ピンの色が緑色に変わったことを確認します。

プリスケーラ値、オートリロード値、デフォルトのパルス値を設定します。TIM11 の Parameter Settings パネルをクリックします。デフォルトは次のとおりです。

以下のパラメータを変更する必要があります。

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

設定後の状態は下図のとおりです。

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

ヒント：必ずプロジェクトファイルを再度保存してください（主にパラメータを保存するためです）。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、前腕の長さが 126mm でした。

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

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

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

ハードウェア	マニピュレータ初期状態に対応するサーボ値
旋回台サーボ
上腕サーボ
前腕サーボ
手首サーボ1
手首サーボ2

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

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

ハードウェア	マニピュレータ初期状態に対応するサーボ値
旋回台サーボ	134
上腕サーボ	123
前腕サーボ	135
手首サーボ1	147（水平方向の初期角度値）
	52（垂直方向の初期角度値）
手首サーボ2	138

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

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

水平方向の逆運動学関数 void get_coordinate_horizontal(float x, float y, float z) を呼び出す際には、定数値（Arm_Alpha_Angle、Arm_Theta_Angle、Arm_Beta_Angle、Arm_Oga_Angle）が使用されます。

逆運動学でロボットアームの各アクチュエータの角度を計算し、補間によってロボットアームの動作を制御します。

垂直方向の逆運動学関数 void get_coordinate_vertical(float x, float y, float z) を呼び出す際には、定数値（Arm_Alpha_Angle、Arm_Theta_Angle、Arm_Beta_Angle、Arm_Gamma_Angle）が使用されます。

逆運動学でロボットアームの各アクチュエータの角度を計算し、補間によってロボットアームの動作を制御します。

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

注意：ロボットアームの動作が速いと先端の慣性が大きくなります。起動時は、ぶつからないようにロボットアームの球状の可動範囲から必ず離れてください。

再現時の注意点

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

実験結果

ロボットアームが逆運動学の初期位置に移動し、その後ホームポジションに移動します。
プログラムが繰り返し実行されます。

(1) ロボットアームが上方向に円弧軌道を描き、グリッパーが開きます。

(2) ロボットアームが下方向に円弧軌道を描き、グリッパーが閉じます。

コード解説

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株式会社までお問い合わせください。