python - 将度数转化为三电机运动

Question

我有一个带有三个电机(红色)的机器人，每个电机都有一个全向(蓝色)( see example of omniwheel here )。如果我想让机器人向前移动，我可以同时激活电机 1 和 3，然后 2 处的轮子将自由旋转，垂直于电机旋转的方向。

我如何编写一个函数，将度数作为输入并输出 0-1 范围内的 3 个值(无电机速度，全电机速度)，以便机器人在朝向以度数指定的真实方位移动时面向相同的方向？

例如输入 45 度，机器人相对于图中的北方向东北方向移动，同时保持恒定的旋转。

谢谢。

Answer 1

一些想法:

电机旋转速度 V1..V3 应在 -1..1 范围内，其中 -1 是完全顺时针旋转，1 是完全逆时针旋转。

电机产生旋转力矩。要排除机器人的旋转，力矩之和应为零。对于平等的腿

  V1 + V2 + V3 = 0

补偿力矩后，每个电机都会沿 OX 或 OY 轴产生力，这与其速度在轴上的投影相对应。提供以速度 S 向 Fi 方向移动:

- V1 * Sqrt(3)/2 + V2  - V3 * Sqrt(3)/2 = S * Cos (Fi)   //OX axis
 -V1 / 2 + V3 / 2  = S * Sin (Fi)                         //OY axis   

检查向上移动

   Fi  = Pi/2
   V1, V2, V3 = -1, 0, 1
   V1 + V2 + V3 = 0  //moment is OK
   Sqrt(3)/2 - Sqrt(3)/2 = 0  //zero speed or OX
   1/2 + 1/2 = S  //S speed for OY

一般来说:求解三个线性方程组，得到V1、V2、V3

  V1 + V2 + V3 = 0
- V1 * Sqrt(3)/2 + V2  - V3 * Sqrt(3)/2 = S * Cos (Fi)  
 -V1 / 2 + V3 / 2  = S * Sin (Fi)        

求解 45 度给出

   > solve({v1+v2+v3=0,-0.87*v1+v2-0.87*v3=0.71,-0.5*v1+0.5*v3=0.71},{v1,v2,v3});
   {v1 = -.90, v2 = .38, v3 = .52}

一些转换以获得封闭形式的解决方案:

  V2 = -V1 - V3
- V1 * Sqrt(3)/2 -V1 - V3  - V3 * Sqrt(3)/2 = S * Cos (Fi)  
  V1 + V3 =  - 2 * S * Cos (Fi) / (2 + Sqrt(3))
  V3 - V1 =  2 * S * Sin(Fi)
  and finally
  V3 = S * (Sin(Fi) - Cos (Fi) / (2 + Sqrt(3)))
  V1 = - S * (Sin(Fi) + Cos (Fi) / (2 + Sqrt(3))) 
  V2 = -V1 - V3

如果计算后某些速度的绝对值 V 超过 1.0，则将所有速度除以该值以确保它们在范围内

(当然，现实生活中的动态更复杂)