全向轮小车的数学原理.pdf

全向轮小车的数学原理全向轮小车的数学原理 对于场地坐标系的速度 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_buf7是左侧摇杆 X 方向模拟值输出，用来控制 X 方向速度。psx_buf8是左侧摇杆 Y 方向模拟值输出，用来控制 Y 方向速度。 psx_buf5是右侧摇杆 X 方向模拟值输出，用来控制小车旋转。PS2 手柄具体协 议和示例程序，参见相应文档。 joy_left_pwm = -(psx_buf7 - 0 x7F) / 2 - (0 x7F - psx_buf8) * 0.866 + (psx_buf5 - 0 x7f) * 0.4; joy_right_pwm = psx_buf7 - 0 x7F + (psx_buf5 - 0 x7f) * 0.4; joy_aux_left_pwm = -(psx