将度数转化为三电机运动
Translate degrees into 3-motor movement
我有一个带有三个电机(红色)的机器人,每个电机都有一个全向(蓝色)(see example of omniwheel here)。如果我想让机器人向前移动,我可以同时启动电机 1 和 3,2 处的轮子将自由旋转,垂直于电机旋转的方向。
我如何编写一个函数,将度数作为输入并输出 0-1 范围内的 3 个值(无电机速度,全电机速度),以便机器人在朝向指定的真实方位移动时面向相同的方向度数?
例如输入 45 度,机器人相对于图中的北方向东北移动,同时保持恒定旋转。
谢谢。
一些想法:
电机旋转速度 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,则将所有速度除以该值以确保它们在范围内
(当然,现实生活动态更复杂)
有趣的问题!
这行得通吗:
import math
def three_motors(degree):
theta1, theta2, theta3 = 150.0, 270.0, 30.0
motor1 = math.sin((degree-theta1)*(math.pi/180.0))
motor2 = math.sin((degree-theta2)*(math.pi/180.0))
motor3 = math.sin((degree-theta3)*(math.pi/180.0))
return motor1, motor2, motor3
three_motors(0)
Out[40]: (-0.49999999999999994, 1.0, -0.49999999999999994)
符号约定是如果你的眼睛在中心并且你正在看纺车,那么正数对应逆时针旋转,负数对应顺时针旋转。如果需要,您可以除以标准数。
我不确定你是在要求实际的代码编写还是数学来编写代码,但我可以在数学方面提供帮助。
我首先假设该函数具有输入所需的轴承 Phi,并为电机输出 v1、v2 和 v3。
v1、v2 和 v3,所有元素都在 [-1,1] 范围内,其中 -1 将顺时针旋转机器人,输出 +1 将逆时针旋转。
方位角输入 Phi 决定了机器人移动的方向。您可以将其转换为 x_dot 和 y_dot。 Theta 点表示 angular 旋转(它将为 0)。
x_dot = -sin(phi)
y_dot = cos(phi)
theta_dot = 0
x_dot = v1_x + v2_x + v3_x
y_dot = v1_y + v2_y + v3_y
theta_dot = v1 + v2 + v3 = 0
(对于下面的方程式,我将它们构造成直线向上为 0 度,逆时针方向增加,以便您可以更轻松地推广到其他车轮位置)
v1 = v1_x + v1_y = - v1*sin(135)i + v1*cos(135)j
v2 = v2_x + v2_y = - v2*sin(270)i + v2*cos(270)j
v3 = v3_x + v3_y = - v3*sin(45)i + v3*cos(45)j
现在您需要设置一个方程组。
x_dot = -sin(phi) = v1_x + v2_x + v3_x
y_dot = cos(phi) = v1_y + v2_y + v3_y
theta_dot = 0 = v1 + v2 + v3
这是一个包含 3 个方程和 3 个未知数的系统,您的未知数是 v1、v2 和 v3。方程的矩阵形式类似于 A*v = x,解为 v = A^-1 * x.
这将为您提供 v 向量(v1、v2、v3)的值。在输出这些值之前,我会通过除以 max(abs(v1,v2,v3)).
对它们进行归一化(以防万一)
如果您想进一步编辑代码以允许机器人在移动的同时改变其面向的方向,只需使 Theta_dot 非零(正为逆时针,负为顺时针)。在这种情况下,您还必须考虑旋转时参考系的变化。
我有一个带有三个电机(红色)的机器人,每个电机都有一个全向(蓝色)(see example of omniwheel here)。如果我想让机器人向前移动,我可以同时启动电机 1 和 3,2 处的轮子将自由旋转,垂直于电机旋转的方向。
我如何编写一个函数,将度数作为输入并输出 0-1 范围内的 3 个值(无电机速度,全电机速度),以便机器人在朝向指定的真实方位移动时面向相同的方向度数?
例如输入 45 度,机器人相对于图中的北方向东北移动,同时保持恒定旋转。
谢谢。
一些想法:
电机旋转速度 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,则将所有速度除以该值以确保它们在范围内
(当然,现实生活动态更复杂)
有趣的问题! 这行得通吗:
import math
def three_motors(degree):
theta1, theta2, theta3 = 150.0, 270.0, 30.0
motor1 = math.sin((degree-theta1)*(math.pi/180.0))
motor2 = math.sin((degree-theta2)*(math.pi/180.0))
motor3 = math.sin((degree-theta3)*(math.pi/180.0))
return motor1, motor2, motor3
three_motors(0)
Out[40]: (-0.49999999999999994, 1.0, -0.49999999999999994)
符号约定是如果你的眼睛在中心并且你正在看纺车,那么正数对应逆时针旋转,负数对应顺时针旋转。如果需要,您可以除以标准数。
我不确定你是在要求实际的代码编写还是数学来编写代码,但我可以在数学方面提供帮助。
我首先假设该函数具有输入所需的轴承 Phi,并为电机输出 v1、v2 和 v3。
v1、v2 和 v3,所有元素都在 [-1,1] 范围内,其中 -1 将顺时针旋转机器人,输出 +1 将逆时针旋转。
方位角输入 Phi 决定了机器人移动的方向。您可以将其转换为 x_dot 和 y_dot。 Theta 点表示 angular 旋转(它将为 0)。
x_dot = -sin(phi)
y_dot = cos(phi)
theta_dot = 0
x_dot = v1_x + v2_x + v3_x
y_dot = v1_y + v2_y + v3_y
theta_dot = v1 + v2 + v3 = 0
(对于下面的方程式,我将它们构造成直线向上为 0 度,逆时针方向增加,以便您可以更轻松地推广到其他车轮位置)
v1 = v1_x + v1_y = - v1*sin(135)i + v1*cos(135)j
v2 = v2_x + v2_y = - v2*sin(270)i + v2*cos(270)j
v3 = v3_x + v3_y = - v3*sin(45)i + v3*cos(45)j
现在您需要设置一个方程组。
x_dot = -sin(phi) = v1_x + v2_x + v3_x
y_dot = cos(phi) = v1_y + v2_y + v3_y
theta_dot = 0 = v1 + v2 + v3
这是一个包含 3 个方程和 3 个未知数的系统,您的未知数是 v1、v2 和 v3。方程的矩阵形式类似于 A*v = x,解为 v = A^-1 * x.
这将为您提供 v 向量(v1、v2、v3)的值。在输出这些值之前,我会通过除以 max(abs(v1,v2,v3)).
对它们进行归一化(以防万一)如果您想进一步编辑代码以允许机器人在移动的同时改变其面向的方向,只需使 Theta_dot 非零(正为逆时针,负为顺时针)。在这种情况下,您还必须考虑旋转时参考系的变化。