这里以zeus为例子讲述运动正解与逆解
1. 正解
从整体速度转换为各个轮子速度即为正解，关系着如何根据既定速度控制机器人正确运行
控制结构
我们知道ROS里面驱动小车最终下发的为线速度和角速度，通过rosmsg show可以看到
对于我们的机器底盘只涉及到
linear.x linear.y和angular.z，分别表示 x，y方向线速度和一个转动的角速度z
可以看到ROS坐标系规定
下面的事就是怎么转换这个三个值到轮子的转速。前为
x方向，右为y方向，逆时针为角速度的z
解算
对于每个轮子：
前轮，左轮及后轮速度分别设为Vf, Vl，Vr(假设轮子使得底盘逆时针时的线速度为正)；轮子所在圆直接为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.逆解
从各个轮子速度(行径距离)转换为整体机器人的速度(坐标、姿态)
解算
只需从正解
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. 用途
通过正解可以转换对机器人的速度控制为对各个轮子的速度控制；通过逆解，通过积分可以根据每个轮子的行径距离求得机器人的坐标和姿态
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 差分轮解算实现
#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.实现
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;
根据配置选择加载对应模型接口解耦模型的正反解算
