0. Omni wheel robot kinematics

Omniwheel Kinematics

위와 같이 보이는 kinematics에서 아래와 같은 공식이 나온다.

이때 w1, w2, w3, w4 값에 대한 것은 어떻게 측정할까? 먼저 우리가 로봇에 명령을 주었을 때, vx, vy, w값을 줄 것이다. 그렇다면 이 값을 갖고 로봇은 pwm을 이용하여 각각에 원하는 속도 값을 주어야 한다. 그렇게 한다면 w1, w2, w3, w4값을 구할 수 있고, pwm에 대하여 비례식으로 모터는 회전할 것이다(최대 rad2sec, tick_num 필요). 그렇게 비례식으로 입력한다면, tick값을 계산하여, 실제 회전수를 구하여 이를 아래의 필요한 부분에 대입하여 사용한다.

Calculate Odometry

dx, dy, dtheta 에 대해 구해야 한다고 하자, 각각에 대한 delta 값은 우측 항의 요소들과 같다.
그렇다면 dcenter은 무엇인가?
dcenter = (dright + dleft)/2 이다. dleft와 dright는 구해진 w1, w2, w3, w4값에 r을 곱하여 만든다. 그 후, 각 방향에 대한 벡터합을 구한 뒤, dleft, dright값을 구한다.

이렇게 구한 델타 값을 이용하여 odometry에 계속하여 더한다. (단 theta는 360' 가 한계)

위와 같은 방식으로도 계산이 가능하며, 이는 터틀봇에서 채택한 방식이기도 하다.

예시 코드

1. 모터 code 작성

#ifndef _ONBOARD_MAT01_H_
#define _ONBOARD_MAT01_H_

#include <Encoder.h>

//Motor param config
	#define MotorType					DC_Motor

// define your robot' specs here
	#define MOTOR_MAX_PWM 255 // motor's maximum RPM
	#define MAX_RPM 158 // motor's maximum RPM
	#define COUNTS_PER_REV 2300 // wheel encoder's no of ticks per rev
	#define WHEEL_DIAMETER 0.068 // wheel's diameter in meters
	#define PWM_BITS 8 // PWM Resolution of the microcontroller
	#define BASE_WIDTH 0.26 // width of the plate you are using

//PID parmeter
    #define K_P 3.0 // P constant
    #define K_I 0.2 // I constant
    #define K_D 0.2 // D constant

struct port_t
{
   int Dir_A;
   int Dir_B;
   int PWM_Pin;
   int Encoder_A;
   int Encoder_B;
};

//===================
// Define motor port
#define PORT_LF   0
#define PORT_RF   1
#define PORT_LR   2
#define PORT_RR   3

port_t port[4] =
{
// A    B   PWM  EN_A  EN_B
  {35,  34,  12,  18,  31},   // A-- Right Rear (RR)
  {36,  37,   8,  19,  38},   // B-- Left  Rear (LR)
  {42,  43,   9,   3,  49},   // C-- Right Front(RF)
  {A5,  A4,   5,   2,  A1},   // D-- Left  Front(LF)
};

class DC_Motor
{
private:
    int Dir_A, Dir_B;
    int PWM_Pin;
    int Encoder_A, Encoder_B;
    Encoder *_encoder;
    int dir;
    float kp, ki, kd;
    int Port;
    long prev_encoder_ticks_;
    int Last_tar, Bias, Last_bias, Pwm, Last_Pwm, Out_Pwm;
    int rpm;

public:
    DC_Motor(int Motor_Port)    //
    {
        Port = Motor_Port;
        Dir_A = port[Port].Dir_A;
        Dir_B = port[Port].Dir_B;
        PWM_Pin = port[Port].PWM_Pin;
        Encoder_A = port[Port].Encoder_A;
        Encoder_B = port[Port].Encoder_B;
    }
    int Init(int _dir)
    {
        pinMode(Dir_A, OUTPUT);
        pinMode(Dir_B, OUTPUT);
        pinMode(PWM_Pin, OUTPUT);
        _encoder = new Encoder(Encoder_A, Encoder_B);
        kp = K_P;
        ki = K_I;
        kd = K_D;
        dir = _dir;
    }
	
	void PrintPortConfig()
	{
        Serial.print("Motor Prot : ");
        Serial.print(Port);   //
        Serial.print(", Dir : ");
        Serial.print(Dir_A);
        Serial.print(", ");
        Serial.print(Dir_B);
        Serial.print("\t, PWM : ");
        Serial.print(PWM_Pin);
        Serial.print(", Encoder : ");
        Serial.print(Encoder_A);
        Serial.print(", ");
        Serial.println(Encoder_B);
	}
	
    int setSpd(int spd)
    {
        spd = dir?(-spd):(spd);
        if(spd > 0)
        {
            digitalWrite(Dir_A, HIGH);
            digitalWrite(Dir_B, LOW);
            analogWrite(PWM_Pin, abs(spd));
        }
        else if(spd < 0)
        {
            digitalWrite(Dir_A, LOW);
            digitalWrite(Dir_B, HIGH);
            analogWrite(PWM_Pin, abs(spd));  
        }
        else
        {
            digitalWrite(Dir_A, LOW);
            digitalWrite(Dir_B, LOW);
            analogWrite(PWM_Pin, 0); 
        }
    }

    long getEncoderPosition()
    {
        long position = _encoder->read();
        return dir ? -position : position;
    }
	
    void clrEncoderPosition()
    {
        _encoder->write(0);
    }
	
	int getMotorRPM()
	{
		return dir ? -rpm : rpm;
	}

    void updateSpd()
    {
//      this function calculates the motor's RPM based on encoder ticks and delta time
//      convert the time from milliseconds to minutes
//      unsigned long current_time = millis();
//      unsigned long dt = current_time - prev_update_time_;
//      double dtm = (double)dt / 60000;
        double dtm = 0.000167;
        double delta_ticks = getEncoderPosition() - prev_encoder_ticks_;

        rpm = (delta_ticks / COUNTS_PER_REV) / dtm;    //calculate wheel's speed (RPM)
//      prev_update_time_ = current_time;
        prev_encoder_ticks_ = getEncoderPosition();
    }

    void Incremental_PID(int target)
    { 
        updateSpd();
        if(Last_tar > target)       
          Last_tar-=2;
        else if(Last_tar < target)
          Last_tar+=2;
        Bias = rpm - Last_tar;          
        Pwm += kp * (Bias - Last_bias) + ki * Bias;      
        if(Pwm > MOTOR_MAX_PWM)  Pwm = MOTOR_MAX_PWM;   
        if(Pwm < -MOTOR_MAX_PWM) Pwm = -MOTOR_MAX_PWM;
        Last_bias=Bias;                             
        Out_Pwm *= 0.7;                             
        Out_Pwm += Last_Pwm * 0.3;
        Last_Pwm = Pwm;
        if(Out_Pwm < 6 && Out_Pwm > -6) Out_Pwm = 0;
        setSpd(Out_Pwm);
    }

    void Update_PID(float _kp, float _ki, float _kd)
    {
        kp = _kp;
        ki = _ki;
        kd = _kd;
    }

};

#endif

간단한 모터 코드이다. 모터에 필요한 핀 번호는 port에 저장되어 있다. 만일 자신의 핀번호와 다르다면 수정하도록 하자.

2. omniwheel 4 motor 제어 코드

#include "onboard_MAT01.h"

typedef DC_Motor Motor_Class;

class Mecanum_Car
{
public:
    int Now_spd;
    Mecanum_Car(Motor_Class *_LF_Wheel, Motor_Class *_RF_Wheel, Motor_Class *_LR_Wheel, Motor_Class *_RR_Wheel)
    {
        this->LF_Wheel = _LF_Wheel;
        this->RF_Wheel = _RF_Wheel;
        this->LR_Wheel = _LR_Wheel;
        this->RR_Wheel = _RR_Wheel;
    }
    void Init(void)
    {
        LF_Wheel->Init(0); 
        RF_Wheel->Init(1); 
        LR_Wheel->Init(0); 
        RR_Wheel->Init(1); //
    }
    void SetSpd(int Spd)
    {
        LF_Wheel_Spd = RF_Wheel_Spd = LR_Wheel_Spd = RR_Wheel_Spd = constrain(Spd, -MAX_RPM, MAX_RPM);  
    }
    
    void ROS_MoveBase(float Line_vel, float Pan_vel, float Angle_vel)
    {
        Line_vel *= cos(45);
        Pan_vel *= sin(45);
        LF_Wheel_Spd = constrain((Line_vel - Pan_vel + Angle_vel * 0.25) * 30, -MAX_RPM, MAX_RPM);
        RF_Wheel_Spd = constrain((Line_vel + Pan_vel + Angle_vel * 0.25) * 30, -MAX_RPM, MAX_RPM);
        LR_Wheel_Spd = constrain((Line_vel - Pan_vel - Angle_vel * 0.25) * 30, -MAX_RPM, MAX_RPM);
        RR_Wheel_Spd = constrain((Line_vel + Pan_vel - Angle_vel * 0.25) * 30, -MAX_RPM, MAX_RPM);
    }

    void ClearOdom()
    {
         LF_Wheel->clrEncoderPosition();
         RF_Wheel->clrEncoderPosition();
         LR_Wheel->clrEncoderPosition();
         RR_Wheel->clrEncoderPosition();
    }
    
    void ReadOdom()
    {
        Serial.print(RR_Wheel->getEncoderPosition());
        Serial.print(", ");
        Serial.print(RR_Wheel->getMotorRPM());
        Serial.println(", ");
    }
    
    void Increment_PID(void)
    {
        LF_Wheel->Incremental_PID(LF_Wheel_Spd);
        RF_Wheel->Incremental_PID(RF_Wheel_Spd);
        LR_Wheel->Incremental_PID(LR_Wheel_Spd);
        RR_Wheel->Incremental_PID(RR_Wheel_Spd);
                
    }
    void Update_PID(float _kp, float _ki, float _kd)
    {
          LF_Wheel->Update_PID(_kp, _ki, _kd);
          RF_Wheel->Update_PID(_kp, _ki, _kd);
          LR_Wheel->Update_PID(_kp, _ki, _kd);
          RR_Wheel->Update_PID(_kp, _ki, _kd);
    }

private:
    Motor_Class *LF_Wheel;
    Motor_Class *RF_Wheel;
    Motor_Class *LR_Wheel;
    Motor_Class *RR_Wheel;
    
    int LR_Wheel_Spd;
    int RR_Wheel_Spd;
    int RF_Wheel_Spd;
    int LF_Wheel_Spd;
};

모터 4개를 제어하는 코드이다. 여기서 movebase에 위에서 공부한 내용을 넣었다.
단위는 수정해줄 필요가 있다. RPM을 rad/s으로 스스로 바꿔주면 되겠다.

[ref]

• http://faculty.salina.k-state.edu/tim/robotics_sg/Control/kinematics/odometry.html
• https://modernrobotics.northwestern.edu/nu-gm-book-resource/13-4-odometry/
• https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_burger/turtlebot3_core/turtlebot3_core.ino (528번째 줄)