全向轮小车的数学原理

全向轮小车的数学原理

对于场地坐标系的速度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);

利用它们化简得：

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

在我们的算法实现中，不考虑小车车身坐标系与地面坐标系之间的夹角θ，因此只需把θ看作0即可。

下边三行代码即为以上算法在项目中的具体实现，见ps2.c文件。其中joy_left_pwm，joy_right_pwm和joy_aux_left_pwm分别代表三个电机接口的PWM电压输出。

joy_left_pwm是底层控制板左侧电机接口输出，joy_right_pwm是底层控制板右侧电机接口输出，joy_aux_left_pwm是顶层左侧电机接口输出。

Ps2手柄在红灯模式下，psx_buf[7]是左侧摇杆X方向模拟值输出，用来控制X 方向速度。psx_buf[8]是左侧摇杆Y方向模拟值输出，用来控制Y方向速度。psx_buf[5]是右侧摇杆X方向模拟值输出，用来控制小车旋转。PS2手柄具体协议和示例程序，参见相应文档。

joy_left_pwm=-(psx_buf[7]-0x7F)/2-(0x7F-psx_buf[8])*0.866+(psx_buf[5] -0x7f)*0.4;

joy_right_pwm=psx_buf[7]-0x7F+(psx_buf[5]-0x7f)*0.4;

joy_aux_left_pwm=-(psx_buf[7]-0x7F)/2+(0x7F-psx_buf[8])*0.866+ (psx_buf[5]-0x7f)*0.4;