三轮全向移动机器人的控制方法

我看过几种三轮全方位底盘的控制方法，感觉不好理解，我在这里简单的分享一下我的一种思路。

对于场地坐标系的速度Vx,Vy推导到三个轮子的速度一共有两个矩阵公式就能解决。

第一个公式，场地坐标系速度Vx,Vy,W转换到机器人坐标系速度vx,vy,w公式：

角theta是机器人坐标系的x轴正方向与场地坐标系y轴正方向的夹角，最终theta所带值为与陀螺仪偏航角有关的一个变量。
这个公式推导一下也是很方便的，无论定位系统如何安装，这个公式不会变的！！！

第二个公式，这个公式需要你人为的推导一下。我举一个典型例子：
1，机器人坐标系与机械结构关系如图（x轴与2号轮平行）

将vx,vy分别分解到轮子方向速度，规定的轮子正方向如白色箭头所示。

计算v1:

v1 = vx’ + vy’ +wL;(L为三轮底盘中心到轮中心的距离)
v1 = -vx * cos(60) - vy*sin(60) + w*L;

明显v2 = vx + w*L;

另外将计算v3：

v3 = vx’+ vy’+ w*L;(L为三轮底盘中心到轮中心的距离)

即v3 = -vx*cos(60) + vy*sin(60) + w*L;

注意，分解速度的时候一定朝着规定的轮子正方向分解！！！

所以，第二个矩阵诞生了！基本上问题就得到解决了。

将第一个矩阵带入第二个矩阵问题得解，之后可以通过三角公式进行化简。便于我以后方便调试，我就当记笔记把后面的步骤写下来吧！

将第一个矩阵带入后得到下列方程：

计算得：

看到这些公式觉得亲切吗？
sin(A+B) = sin(A)*cos(B) + cos(A)*sin(B);
cos(A+B) = cos(A)*cos(B) - sin(A)*sin(B);
sin(A-B) = sin(A)*cos(B) - cos(A)*sin(B);
cos(A-B) = cos(A)*cos(B) + sin(A)*sin(B);
利用它们化简得：

其他情况都这样推导就行，第一个矩阵不会改变，我们只需手演算即可。

这是我对该情况封装的函数：

typedef struct
{
    float v1;
    float v2;
    float v3;
}ActThreeVell;
ActThreeVell ThreeWheelVellControl2(float Vx, float Vy, float angularVell)
{
#define AFA 60
#define L   2ActThreeVell vell;

theta = 0;

vell.v1 = (float)(-cos((AFA + theta) / 180.0f*3.1415926f) * Vx - sin((theta + AFA) / 180.0f*3.1415926f) * Vy + L * angularVell);

vell.v2 = (float)(cos(theta / 180.0f*3.1415926f) * Vx + sin(theta /180.0f*3.1415926f) * Vy      + L * angularVell);

vell.v3 = (float)(-cos((AFA - theta) / 180.0f * 3.1415926f) * Vx + sin((AFA - theta) / 180.0f*3.1415926f) * Vy + L * angularVell);

return vell;



}