３輪オムニの制御プログラム

前回の記事で三輪オムニの計算を行ったので、それをもとに制御プログラムを作成した。
https://github.com/hayate2718/omni_3wheel_control

前提条件

前提条件として、前回記事の図のような３輪オムニを想定して、プログラムを作成している。したがって、ホイールの位置関係等の前提条件が異なる場合は、それに応じて修正が必要となる。一般化したプログラムを作ろうと思ったがいいアイデアが思い浮かばなかった。
32bitsMCU程度の演算装置を想定して、プログラムを作成している。

コード

omni_3wheel_control.cpp

#include "omni_3wheel_control.h"
#include "math.h"


omni_3wheel_control::omni_3wheel_control() : 
	wheel_1(0),
	wheel_2(0),
	wheel_3(0),
	wheel_1_velocity(0),
	wheel_2_velocity(0),
	wheel_3_velocity(0),
	global_velocity_x(0),
	global_velocity_y(0),
	local_velocity_x(0),
	local_velocity_y(0),
	robot_angle(0),
	robot_range(0.1),
	robot_angular_velocity(0),
	robot_angular_wheel(0)
{

	//ここでオムニホイールの角度を定義
	cos_theta_1 = 1 / 2;
	sin_theta_1 = sqrt_3 / 2;

	cos_theta_2 = 1 / 2;
	sin_theta_2 = -(sqrt_3) / 2;

	cos_theta_3 = -1;
	sin_theta_3 = 0;


	return;
}

void omni_3wheel_control::local_control(float local_velocity_x, float local_velocity_y){

	this->local_velocity_x = local_velocity_x;
	this->local_velocity_y = local_velocity_y;

	wheel_1 = this->local_velocity_x * cos_theta_1 + this->local_velocity_y * sin_theta_1;
	wheel_2 = this->local_velocity_x * cos_theta_2 + this->local_velocity_y * sin_theta_2;
	wheel_3 = this->local_velocity_x * cos_theta_3 + this->local_velocity_y * sin_theta_3;

	return;
}

void omni_3wheel_control::global_control(float global_velocity_x, float global_velocity_y) {
	
	this->global_velocity_x = global_velocity_x;
	this->global_velocity_y = global_velocity_y;

	float angle = 0;
	angle = this->get_robot_angle();

	this->local_control(
		this->global_velocity_x * cosf(angle)+this->global_velocity_y * sinf(angle),
		this->global_velocity_y * cosf(angle)-this->global_velocity_x * sinf(angle)
	);

	return;
}


void omni_3wheel_control::angular_velocity_control(float angular_velocity) {
	
	this->robot_angular_velocity = angular_velocity;

	this->robot_angular_wheel = this->robot_range * this->robot_angular_velocity;
	
	return;
}

float omni_3wheel_control::get_wheel_velocity_1() {
	this->wheel_1_velocity = this->wheel_1 + this->robot_angular_wheel;
	return this->wheel_1_velocity;
}

float omni_3wheel_control::get_wheel_velocity_2() {
	this->wheel_2_velocity = this->wheel_2 + this->robot_angular_wheel;
	return this->wheel_2_velocity;
}

float omni_3wheel_control::get_wheel_velocity_3() {
	this->wheel_3_velocity = this->wheel_3 + this->robot_angular_wheel;
	return this->wheel_3_velocity;
}


void omni_3wheel_control::set_robot_angle(float angle) {
	this->robot_angle = angle;
	return;
}

float omni_3wheel_control::get_robot_angle() {
	return this->robot_angle;
}


void omni_3wheel_control::set_sin_theta_1(float theta){
	this->sin_theta_1 = theta;
	return;
}

void omni_3wheel_control::set_cos_theta_1(float theta) {
	this->cos_theta_1 = theta;
	return;
}

void omni_3wheel_control::set_sin_theta_2(float theta) {
	this->sin_theta_2 = theta;
	return;
}

void omni_3wheel_control::set_cos_theta_2(float theta) {
	this->cos_theta_2 = theta;
	return;
}

void omni_3wheel_control::set_sin_theta_3(float theta) {
	this->sin_theta_3 = theta;
	return;
}

void omni_3wheel_control::set_cos_theta_3(float theta) {
	this->cos_theta_3 = theta;
	return;
}

void omni_3wheel_control::set_robot_range(float range) {
	this->robot_range = range;
	return;
}

omni_3wheel_control.h

#pragma once

/*
前提としてオムニホイールの位置関係及び計算は下記URL参照
"https://qiita.com/hayate2718/items/9f5db2bfb46d39a7b330"
*/

/*
MCU等の計算資源の少ない演算装置を想定して無理数の定数を先に定義する。
32bitsMCUが計算資源少ないかは、議論の余地があるか...
*/

#define PI 3.141592f //円周率を定義
#define sqrt_3 1.732051f //3^(0.5)を定義

class omni_3wheel_control
{
private:

	float sin_theta_1; //各ホイールの回転方向がローカル座標系のx軸となす角によるx,y成分を定義する
	float cos_theta_1;

	float sin_theta_2;
	float cos_theta_2;

	float sin_theta_3;
	float cos_theta_3;
	
	float wheel_1; //バッファ
	float wheel_2;
	float wheel_3;

	float wheel_1_velocity; //各オムニホイールの目標速度(回転数ではない)[m/s]
	float wheel_2_velocity;
	float wheel_3_velocity;


	float local_velocity_x; //ローカル座標系の目標速度[m/s]
	float local_velocity_y;

	float global_velocity_x; //グローバル座標系の目標速度[m/s]
	float global_velocity_y;

	float robot_angle; //ローカル座標系とグローバル座標系がなす角

	float robot_range; //オムニホイールと重心までの距離[m]
	float robot_angular_velocity; //ロボットの目標角速度[rad/s]
	float robot_angular_wheel; //ロボットの回転方向のホイール速度[m/s]

public:

	omni_3wheel_control();

	void local_control(float local_velocity_x, float local_velocity_y); //ローカル座標系の制御関数

	void global_control(float global_velocity_x,float global_velocity_y); //グローバル座標系の制御関数

	void angular_velocity_control(float robot_angular_velocity);

	float get_wheel_velocity_1(); //ホイールの目標速度を取得する

	float get_wheel_velocity_2();

	float get_wheel_velocity_3();

	void set_sin_theta_1(float theta);
	void set_cos_theta_1(float theta);

	void set_sin_theta_2(float theta);
	void set_cos_theta_2(float theta);
	
	void set_sin_theta_3(float theta);
	void set_cos_theta_3(float theta);

	void set_robot_range(float range);

	void set_robot_angle(float angle); //ローカル座標系とグローバル座標系がなす角を入力する[rad]

	float get_robot_angle(); //ローカル座標系とグローバル座標系がなす角を取得する[rad]

};

解説

解説といってもある程度コメントアウトを入れてるからほとんどないのだが、

global_control(float global_velocity_x, float global_velocity_y);
set_robot_angle(float angle);

の2つの関数と、

    cos_theta_1 = 1 / 2;
	sin_theta_1 = sqrt_3 / 2;

	cos_theta_2 = 1 / 2;
	sin_theta_2 = -(sqrt_3) / 2;

	cos_theta_3 = -1;
	sin_theta_3 = 0;

の計算について解説する。
この二点については、組み込みっぽいプログラムかなぁと思うので。
先に、cos_theta_1とかの変数の計算について解説する。これらの変数はホイールの回転方向の角度の三角比なのだが、３輪オムニにおいてこれらは定数なので先に計算して、利用する際は結果を参照する形に置いている。利用時に/当の演算子を使うとそこで無駄な計算が入るので計算速度向上のため計算結果を参照して使う方式にしている。
次に、global_control(float global_velocity_x, float global_velocity_y); の関数について解説する。この関数はグローバル座標系におけるホイールの速度を計算する関数である。ローカル座標系の制御関数の引数にグローバルからローカルへの座標変換を行う計算を与えている。ローカル座標系の制御関数を利用することでプログラムのメモリ使用量を減らしている。
次に、set_robot_angle(float angle);の関数について解説する。角度についてはマイコン等でロボットを制御するとき、目標速度とはことなり、センサから取得したパラメータをセットするため独立した関数を与えている。

プログラムの使い方

このプラグラムをライブラリとして用いる場合は、

#include "omni_3wheel_control.h"
int main(void){
    
    float wheel_velocity_tar[3]={};
    
    omni_3wheel_control use_control;
    use_control.set_robot_angle(PI/2); //角度をセット
    
    /*以下グローバル座標系の場合*/
    use_control.global_control(50,20);
    wheel_velcity_tar[0] = use_control.get_wheel_velocity_1();
    wheel_velcity_tar[1] = use_control.get_wheel_velocity_2();
    wheel_velcity_tar[2] = use_control.get_wheel_velocity_3();
    
    /*以下ローカル座標系の場合*/
    use_control.angular_velocity_control(PI); //目標角速度をセット
    use_control.local_control(50,20);
    wheel_velcity_tar[0] = use_control.get_wheel_velocity_1();
    wheel_velcity_tar[1] = use_control.get_wheel_velocity_2();
    wheel_velcity_tar[2] = use_control.get_wheel_velocity_3();

    
return 0;
}

グローバル座標系、ローカル座標系における制御関数の計算結果は、どちらも同じ変数に格納されるため、任意の制御関数を実行し、計算結果を呼び出す関数を実行すれば目標速度が得られる。グローバル座標系の場合については、ロボットの状態に応じて、角度をセットしてあげる必要がある。ローカル座標系で角度をセットしても計算結果は変わらない。
set_robot_angle();関数が本来センサからの信号を周期的に取得して、セットすることが前提の関数であるため、テストプログラム上だと、angular_velocity_control();関数と共存できない。