実験7 4自由度ロボットアームの運動学による8の字と円の描画

実験の目的：

4自由度直列ロボットアームの運動学アルゴリズムを理解します。
デカルト空間における軌道計画（直線補間アルゴリズム、円弧補間アルゴリズムなど）を理解します。

3.アルゴリズムと軌道計画を組み合わせて、ロボットアームの動作を制御できるようにします。

4.STM32 の GPIO 周辺機能ピンの設定方法を習得します。

5.STM32CubeMX をもとに新規プロジェクトを作成し、MDK_ARM プロジェクトファイルを生成できるようにします。

6.keil IDE でユーザーコードを追加し、プロジェクトのテストを行えるようにします。

基本原理：

1.3軸ロボットアームの先端に揺動モジュールを1つ追加することで、エンドエフェクタであるグリッパーの向きを制御できます。

2.取り付け位置は、回転軸が鉛直下向き、X軸方向、Y軸方向など、さまざまな向きにできます。ただし一般に第4軸は、3軸では先端を平行に保てないという問題を補うために使われるため、直列4軸の第4関節は通常、第2・第3関節と同じ回転方向になります。

3.運動学アルゴリズムは必要に応じてダウンロードしてください：直列4自由度ロボットアームの運動学計算.pdf

機能要件:

4自由度ロボットアームで逆運動学を呼び出して補間運動を行い、8の字と円の軌跡を描きます。同時にグリッパーの開閉を制御して把持を模擬します。

ハードウェアの準備：

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

試作機の構成：

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

回路接続：

ハードウェアモジュール

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

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

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

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

電子モジュールの紹介

ピンの説明

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

まず サーボインターフェース の位置を全体的に把握し、ピン表に従って配線してください。

実機の配線

配線のコツ：サーボを配線する際は、GND を開発ボード上の GND に合わせてください。

ソフトウェア設計：

設計フロー

メインフロー：

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

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

ソースファイル：07.FourDofArm_other.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 "four_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();
  /* USER CODE BEGIN 2 */

  servo_init();//机械臂各个舵机初始化

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

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

  uint32_t action_delay = 300;
  /* 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：  机械臂画两次 '8' --保持x坐标不变, 在y-z平面画8
    for(int i=0;i<2;i++){
        draw_eight_x_fixed(80.0, 0.0, 50.0, 40.0, 40.0, 240);
    }
    HAL_Delay(action_delay);


    //动作4：  机械臂画两次 '8' --保持y坐标不变, 在x-z平面画8
    for(int i=0;i<2;i++){
        draw_eight_y_fixed(100.0, 0.0, 50.0, 40.0, 40.0, 240);
    }
    HAL_Delay(action_delay);


    //动作5： 机械臂画两次 '8' --保持z坐标不变, 在x-y平面画8
    for(int i=0;i<2;i++){
        draw_eight_z_fixed(100.0, 0.0, 0.0, 40.0, 40.0, 280);
    }
    HAL_Delay(action_delay);



    //动作6：  机械臂画两次 '圆' --保持x坐标不变, 在y-z平面画圆
    for(int i=0;i<2;i++){
        draw_circle_x_fixed(100.0, 0.0, 50.0, 80.0, 240);
    }
    HAL_Delay(action_delay);

    //动作7：  机械臂画两次 '圆' --保持y坐标不变, 在x-z平面画圆
    for(int i=0;i<2;i++){
        draw_circle_y_fixed(100.0, 0.0, 50.0, 60.0, 240);
    }
    HAL_Delay(action_delay);


    //动作六8：  机械臂画两次 '圆' --保持z坐标不变, 在x-y平面画圆
    for(int i=0;i<2;i++){
        draw_circle_z_fixed(100.0, 0.0, 50.0, 60.0, 240);
    }
    HAL_Delay(action_delay);

  }
  /* 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の選択

1.STM32CubeMX を起動し、File > New Project をクリックします。

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

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

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

1.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**（外部水晶発振子出力）。

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

設定が完了すると、対応するピン出力が緑色になります。

緑色の部分をクリックすると、詳細を確認できます。

1.3 クロックツリーの設定

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

ヒント：設定中はエラーが自動的に検出されます。周波数の設定が誤っている場合は、赤くハイライトされてエラーが表示されます。

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

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

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

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

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

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

1.プロジェクト名を入力します（任意に定義して構いません）。

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

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

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

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

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

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

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

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

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

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

3.GPIOの設定

3.1 旋回台サーボと上腕サーボのパラメータ設定

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

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

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

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

サーボS2のパラメータ

ピン	タイマー
PA3	TIM2_CH4

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

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

2つのサーボの設定後のパラメータは次のとおりです。

3.プリスケーラ、自動リロード値、デフォルトのパルス値を設定します。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_CH2
PH6	TIM12_CH1

1.3.1 のサーボパラメータの設定方法を参考に、前腕と手首のパラメータを設定してください。設定後の GPIO Settings のパラメータは、下図の赤枠部分のとおりです。

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

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

ピン	タイマー
PF6	TIM10_CH1

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

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

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

3.プリスケーラ、自動リロード値、デフォルトのパルス値を設定します。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 度です。

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

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

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

4. コードの生成

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

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

3.コードの生成に成功しました。

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.ロボットアームの操作・調整方法

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

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

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

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

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

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

ハードウェア	ロボットアーム初期状態のサーボ値
旋回台サーボ
上腕サーボ
前腕サーボ
手首サーボ

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

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

ハードウェア	ロボットアーム初期状態のサーボ値
旋回台サーボ	135
上腕サーボ	123
前腕サーボ	138
手首サーボ	134

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

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

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

逆運動学でロボットアームの各サーボの角度を計算し、補間を行うことでロボットアームの動作を制御します。

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

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

再現のポイント

1.再現する際は、各関節サーボの出力軸に取り付けるサーボホーンの向きを、実機の例示写真やデモ動画と一致させてください。これはロボットアームの各関節の回転方向に影響し、運動学の式における方向と関係します。向きが正しくないと、ある関節の回転角度や動作方向が大きくずれることがあります。

2.ロボットアームの初期化状態では、各関節の角度をできるだけサーボの中間値付近（80～150）にすることを推奨します。この範囲にない場合は、構造を組み直して調整することを推奨します。これはロボットアームの可動範囲に影響します。サーボの回転範囲は 0～270 度、中間値は 135 で、これにより各関節サーボが最大の可動範囲を得られます。中間値の設定が不適切だとロボットアームの到達可能点が減り、本来到達できる点でも運動学の式で計算すると解なしとなって、理論上の到達可能点に到達できなくなります。

実験結果

ロボットアームは逆運動学の初期位置に移動し、その後ホームポジションに移動します。
プログラムは次の内容をループ実行します。

(1)ロボットアームがホームポジションに戻り、グリッパーの開閉を制御します。

(2)ロボットアームが複数の平面上に 8の字 の軌跡を2回ずつ描きます（Y-Z平面、X-Z平面、X-Y平面上に描画）。

(3)ロボットアームが複数の平面上に円の軌跡を2回ずつ描きます（Y-Z平面、X-Z平面、X-Y平面上に描画）。

コード解説

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

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

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

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

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

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

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

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

（2）上記の赤枠のコードを自分のプロジェクトの main.c にコピーし、コンパイル・書き込みを行えば動作を確認できます。

交流・サポート

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