這里以zeus為例子講述運(yùn)動正解與逆解
1. 正解
從整體速度轉(zhuǎn)換為各個輪子速度即為正解晤斩，關(guān)系著如何根據(jù)既定速度控制機(jī)器人正確運(yùn)行
控制結(jié)構(gòu)
我們知道ROS里面驅(qū)動小車最終下發(fā)的為線速度和角速度，通過rosmsg show可以看到
對于我們的機(jī)器底盤只涉及到
linear.x linear.y和angular.z姆坚，分別表示 x澳泵，y方向線速度和一個轉(zhuǎn)動的角速度z
可以看到ROS坐標(biāo)系規(guī)定
下面的事就是怎么轉(zhuǎn)換這個三個值到輪子的轉(zhuǎn)速。前為
x方向兼呵，右為y方向兔辅，逆時針為角速度的z
解算
對于每個輪子：
前輪，左輪及后輪速度分別設(shè)為Vf, Vl击喂，Vr(假設(shè)輪子使得底盤逆時針時的線速度為正)维苔；輪子所在圓直接為L
- 前輪只受線速度y和角速度控制影響，可得到
Vf = y+z*L/2 - 左輪收到x懂昂、y和角速度控制影響
Left Wheel
Vl = -x*cos(30°)-y*sin(30°)+z*L/2 - 右輪同樣ru左輪可以得到：
Vr = x*cos(30°)-y*sin(30°)+z*L/2
2.逆解
從各個輪子速度(行徑距離)轉(zhuǎn)換為整體機(jī)器人的速度(坐標(biāo)介时、姿態(tài))
解算
只需從正解
Vf = y + z*L/2
Vl = -x*cos(30°) - y*sin(30°) + z*L/2
Vr = x*cos(30°) - y*sin(30°) + z*L/2
反推即可得到
x = (-Vl + Vr) / (2 * cos(30°))
y = (2Vf - Vl - Vr) / 3
z = (Vf + Vl + Vr) * 2 / (3L)
3. 用途
通過正解可以轉(zhuǎn)換對機(jī)器人的速度控制為對各個輪子的速度控制；通過逆解凌彬，通過積分可以根據(jù)每個輪子的行徑距離求得機(jī)器人的坐標(biāo)和姿態(tài)
4.代碼
model.h 模型接口類
#ifndef PIBOT_MODLE_H_
#define PIBOT_MODLE_H_
struct Odom{
float x;
float y;
float z;
float vel_x;
float vel_y;
float vel_z;
};
struct Model{
Model(){}
Model(float _wheel_radius, float _body_radius): wheel_radius(_wheel_radius), body_radius(_body_radius){}
void set(float _wheel_radius, float _body_radius){
wheel_radius = _wheel_radius;
body_radius = _body_radius;
}
~Model(){}
//robot speed ------------> motor speed
virtual void motion_solver(float* robot_speed, float* motor_speed) = 0;
//motor speed-------------> robot speed
//interval ms
virtual void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval) = 0;
protected:
float wheel_radius;
float body_radius;
};
#endif
differential.h 差分輪解算實(shí)現(xiàn)
#ifndef PIBOT_DIFFERENTIAL_H_
#define PIBOT_DIFFERENTIAL_H_
#include "model.h"
#include "math.h"
#define MOTOR_COUNT 2
class Differential : public Model{
public:
Differential() {}
Differential(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
void motion_solver(float* robot_speed, float* motor_speed){
// robot_speed[0] x robot_speed[1] y robot_speed[2] z
motor_speed[0] = (-robot_speed[0] + robot_speed[2] * body_radius)/ wheel_radius;
motor_speed[1] = (robot_speed[0] + robot_speed[2] * body_radius) / wheel_radius;
}
void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval)
{
float dxy_ave = (-motor_dis[0] + motor_dis[1]) / 2.0;
float dth = (motor_dis[0] + motor_dis[1]) / (2* body_radius);
float vxy = 1000 * dxy_ave / interval;
float vth = 1000 * dth / interval;
odom->vel_x = vxy;
odom->vel_y = 0;
odom->vel_z = vth;
float dx = 0, dy = 0;
if (motor_dis[0] != motor_dis[1])
{
dx = cos(dth) * dxy_ave;
dy = -sin(dth) * dxy_ave;
odom->x += (cos(odom->z) * dx - sin(odom->z) * dy);
odom->y += (sin(odom->z) * dx + cos(odom->z) * dy);;
}
if (motor_dis[0] + motor_dis[1] != 0)
odom->z += dth;
}
};
#endif
omni3.h三輪全向輪解算
#ifndef PIBOT_OMNI3_H_
#define PIBOT_OMNI3_H_
#include "model.h"
#include "math.h"
#define MOTOR_COUNT 3
#define _sqrt_of_3 1.73205f
class Omni3:public Model{
public:
Omni3() {}
Omni3(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
void motion_solver(float* robot_speed, float* motor_speed){
// robot_speed[0] x robot_speed[1] y robot_speed[2] z
motor_speed[0] = (robot_speed[1] + robot_speed[2] * body_radius)/ wheel_radius;
motor_speed[1] = (-robot_speed[0]*_sqrt_of_3 - robot_speed[1]*0.5 + robot_speed[2] * body_radius) / wheel_radius;
motor_speed[2] = (robot_speed[0]*_sqrt_of_3 - robot_speed[1]*0.5 + robot_speed[2] * body_radius) / wheel_radius;
}
void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval){
if (motor_dis[0]!=0 || motor_dis[1]!=0 || motor_dis[2]!=0){
//speed
double dvx = (-motor_dis[1]+motor_dis[2])*_sqrt_of_3/3.0f;
double dvy = (motor_dis[0]*2-motor_dis[1]-motor_dis[2])/3.0f;
double dvth = (motor_dis[0]+motor_dis[1]+motor_dis[2])/ (3 * body_radius);
odom->vel_x = dvx / dt;
odom->vel_y = dvy / dt;
odom->vel_z = dvth / dt;
//odometry
double dx = (-sin(th)*motor_dis[0]*2+(-_sqrt_of_3*cos(th)+sin(th))*motor_dis[1]+(_sqrt_of_3*cos(th)+sin(th))*motor_dis[2])/3.0f;
double dy = (cos(th)*motor_dis[0]*2+(-_sqrt_of_3*sin(th)-cos(th))*motor_dis[1]+(_sqrt_of_3*sin(th)-cos(th))*motor_dis[2])/3.0f;
double dth = (motor_dis[0]+motor_dis[1]+motor_dis[2])/ (3 * body_radius);
odom->x += dx;
odom->y += dy;
odom->z += dth;
}
}
};
#endif
4.實(shí)現(xiàn)
static Differential diff(wheel_diameter*0.5, wheel_track*0.5);
static Omni3 omni3(wheel_diameter*0.5, wheel_track*0.5);
Model* model = &omni3;
根據(jù)配置選擇加載對應(yīng)模型接口解耦模型的正反解算
