実験10 5自由度ロボットアームによる円の描画

実験の目的：

1. 5自由度直列型ロボットアームの運動学アルゴリズムを理解します。

2. デカルト空間における軌道計画（直線補間アルゴリズム、円弧補間アルゴリズムなど）を理解します。

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

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

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

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

基本原理：

1. 5自由度ロボットアームは、本章の3自由度ロボットアーム実験の構成に2自由度の手首を追加したものと考えることができ、先端をあらゆる方向に制御する能力を備えています。

2. 先端の2つの自由度の割り当ては具体的な作業シーンによって決まり、特に制限はありません。

3. 運動学アルゴリズムは必要に応じてダウンロードしてください：直列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】

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

電子モジュールの紹介

ピンの紹介

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

実際の配線

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

​

ソフトウェア設計：

設計フロー

メインフロー：

アプリケーション層からアルゴリズムを呼び出す流れ：

​

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

ソースファイル：10.FiveDofArm_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 "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);

  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_horizontal();
    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：  机械臂画两次 '圆' --保持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);

    //动作4：  机械臂画两次 '圆' --保持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);


    //动作5：  机械臂画两次 '圆' --保持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（リセットおよびクロック制御） をクリックします。

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

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

機能：

• 外部高速水晶発振器（HSE）を接続し、システムに高精度のクロック源を供給するために使用します。
• **RCC_OSC_IN**：水晶入力ピン。
• **RCC_OSC_OUT**：水晶出力ピン（帰還信号）
• デフォルト機能：
• **PH0**：**OSC_IN**（外部水晶入力）。
• **PH1**：**OSC_OUT**（外部水晶出力）。

4. デバッグインターフェースのピンを設定します。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 シリアルポートの設定

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 ターンテーブルと上腕サーボのパラメータ設定

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

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

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

サーボ S2 のパラメータ

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

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

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

3. プリスケーラ、自動リロード値、デフォルトのパルス値を設定します。TIM2 をクリックし、Parameter Settings パネルを選択します。設定結果は下図のとおりです。

3.2 前腕と手首サーボ1のパラメータ設定

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

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

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

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

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

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

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

次のようにパラメータを変更する必要があります。

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

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

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

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

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

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

次のようにパラメータを変更する必要があります。

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

​

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) ロボットアームが初期状態にあるときのサーボ値を記録し、下表に記入します。

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

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

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

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

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

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

再現時の注意点

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

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

実験結果

1. ロボットアームが逆運動学の初期位置に移動し、その後ホームポジションに移動します。

2. プログラムは次の動作を繰り返し実行します。

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

(2) ロボットアームが複数の平面上に円の軌跡を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 xx */ で終わる部分が目印です。

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

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

技術サポート

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

​