麦克纳姆轮系运动学模型解算与分析
麦克纳姆轮系运动学模型解算与分析
麦克纳姆轮（Mecanum Wheel）是一种全向移动轮，广泛应用于各类机器人底盘。通过独特的轮毂和辊子设计，麦克纳姆轮能够实现全方位的移动，包括前后、左右平移以及原地旋转。本文将深入探讨麦克纳姆轮的运动学模型，从基本原理到实际应用，为读者提供全面的理论基础和实践指导。
麦克纳姆轮的定义与安装方法
麦克纳姆轮由轮毂和围绕轮毂的辊子组成，辊子轴线与轮毂轴线成45°角。这种设计使得麦克纳姆轮能够在不改变轮子方向的情况下，实现全方位的移动。
麦克纳姆轮分为两种类型：左旋轮（A型）和右旋轮（B型），它们互为镜像关系。在实际应用中，通常使用四个麦克纳姆轮，两个左旋轮和两个右旋轮，以实现全方位的移动。
常见的安装方式有四种：
- X-正方形：四个轮子的接触点形成一个正方形，但这种安装方式无法实现主动旋转。
- X-长方形：轮子转动可以产生yaw轴转动力矩，但力臂较短。
- O-正方形：四个轮子位于正方形的四个顶点，平移和旋转都无问题，但受限于机器人底盘的形状和尺寸。
- O-长方形：最常见的方式，轮子转动可以产生较长的yaw轴转动力矩。
麦克纳姆轮的运动学模型
正运动学模型与逆运动学模型
以O-长方形的安装方式为例，四个轮子的着地点形成一个矩形。正运动学模型描述了如何通过四个轮子的速度计算底盘的运动状态，而逆运动学模型则根据底盘的运动状态解算出四个轮子的速度。
底盘运动的分解
刚体在平面内的运动可以分解为三个独立分量：X轴平动、Y轴平动和yaw轴自转。设：
- (v_x) 为X轴方向的速度（向右为正）
- (v_y) 为Y轴方向的速度（向前为正）
- (\omega_z) 为yaw轴的角速度（逆时针为正）
轮子轴心位置的速度计算
定义：
- (\vec{r}_i) 为从几何中心指向轮子轴心的矢量
- (\vec{v}_i) 为轮子轴心的运动速度矢量
- (v_{i,t}) 为轮子轴心沿垂直于(\vec{r}_i)方向的速度分量
可以计算出：
[ \vec{v}_i = v_x \hat{i} + v_y \hat{j} + \omega_z \times \vec{r}_i ]
同理可以算出其他三个轮子轴心的速度。
辊子的速度计算
根据轮子轴心的速度，可以分解出沿辊子方向的速度(v_{roller})和垂直于辊子方向的速度。其中垂直方向的速度可以忽略，而沿辊子方向的速度为：
[ v_{roller} = \vec{v}i \cdot \hat{u}{roller} ]
其中(\hat{u}_{roller})是沿辊子方向的单位矢量。
轮子的真实转速计算
从辊子速度到轮子转速的计算较为简单：
[ \omega_{wheel} = \frac{v_{roller}}{r_{wheel}} ]
其中(r_{wheel})是轮子的半径。
结合以上四个步骤，可以根据底盘运动状态解算出四个轮子的转速。逆运动学模型的方程组为：
[ \begin{cases}
\omega_1 = \frac{1}{r_{wheel}}(v_x - v_y - (a+b)\omega_z) \
\omega_2 = \frac{1}{r_{wheel}}(v_x + v_y + (a+b)\omega_z) \
\omega_3 = \frac{1}{r_{wheel}}(v_x + v_y - (a+b)\omega_z) \
\omega_4 = \frac{1}{r_{wheel}}(v_x - v_y + (a+b)\omega_z)
\end{cases} ]
简化计算方法
另一种更简单的逆运动学计算方式是基于线性叠加原理。只需要计算出麦轮底盘在「沿X轴平移」、「沿Y轴平移」、「绕几何中心自转」时，四个轮子的速度，就可以通过简单的加法，计算出这三种简单运动所合成的「平动+旋转」运动时所需要的四个轮子的转速。
麦克纳姆轮的运动分析
两种类型麦轮的运动分析
对于A轮（左旋轮）：
- 前进时斜向右前方运动
- 后退时斜向左后方运动
对于B轮（右旋轮）：
- 前进时斜向左前方运动
- 后退时斜向右后方运动
麦克纳姆轮车整体运动分析
以O-长方形安装为例（最常见的安装方法）：
- 前进：所有轮子正转
- 倒退：所有轮子反转
- 左移：A轮反转，B轮正转
- 右移：A轮正转，B轮反转
- 原地左转：同侧A轮反转，B轮正转
- 原地右转：同侧A轮正转，B轮反转
- 斜向运动：对角线上的轮子以不同方向转动
- 绕轴旋转：前轴或后轴的轮子以相反方向转动
麦克纳姆轮的控制实现
简单控制
简单控制仅关注方向控制，通过设置x轴、y轴、z轴的PWM值来控制小车的方向。但这种方式速度不稳定，方向控制也不精确。
void Set_Pwm_Vel(int linear_Pwm_x, int linear_Pwm_y, int angular_Pwm_z)
{
Car_AHL_Pwm = linear_Pwm_y - linear_Pwm_x + angular_Pwm_z*(a+b);
Car_AHR_Pwm = linear_Pwm_y + linear_Pwm_x - angular_Pwm_z*(a+b);
Car_AFL_Pwm = linear_Pwm_y - linear_Pwm_x - angular_Pwm_z*(a+b);
Car_AFL_Pwm = linear_Pwm_y + linear_Pwm_x + angular_Pwm_z*(a+b);
TIM1->CCR1 = Car_AHL_Pwm;
TIM1->CCR2 = Car_AHR_Pwm;
TIM1->CCR3 = Car_AFL_Pwm;
TIM1->CCR4 = Car_AFL_Pwm;
}
精确控制
精确控制同时关注方向和速度，通过PID控制算法来控制每个轮子的速度，从而实现对小车整体速度的精确控制。
void Set_Vel(float linear_x, float linear_y, float angular_z)
{
kinematics.exp_vel.linear_x = linear_x;
kinematics.exp_vel.linear_y = linear_y;
kinematics.exp_vel.angular_z = angular_z;
}
void Exp_Speed_Cal(u32 dT_us)
{
float linear_vel_x_mins;
float linear_vel_y_mins;
float angular_vel_z_mins;
float tangential_vel;
float x_rpm;
float y_rpm;
float tan_rpm;
linear_vel_x_mins = kinematics.exp_vel.linear_x * 60.0f;
linear_vel_y_mins = kinematics.exp_vel.linear_y * 60.0f;
angular_vel_z_mins = kinematics.exp_vel.angular_z * 60.0f;
tangential_vel = angular_vel_z_mins * ((kinematics.wheels_x_distance_ / 2) + (kinematics.wheels_y_distance_ / 2));
x_rpm = linear_vel_x_mins / kinematics.wheel_circumference_;
y_rpm = linear_vel_y_mins / kinematics.wheel_circumference_;
if((kinematics.exp_vel.linear_x == 0
&& kinematics.exp_vel.linear_y == 0
&& kinematics.exp_vel.angular_z == 0)
|| my_abs(sensor.gyro_rps[Z] - kinematics.fb_vel.angular_z) > 0.5)
{
pid_yaw.out = tangential_vel / kinematics.wheel_circumference_;
}
else
{
PID_Controller(dT_us, kinematics.exp_vel.angular_z, sensor.gyro_rps[Z], &pid_yaw, 0, 0);
pid_yaw.out = Get_MiMx(pid_yaw.out, -kinematics.max_rpm_, kinematics.max_rpm_);
}
tan_rpm = pid_yaw.out;
kinematics.exp_wheel_rpm.motor_1 = x_rpm - y_rpm - tan_rpm;
kinematics.exp_wheel_rpm.motor_2 = x_rpm + y_rpm + tan_rpm;
kinematics.exp_wheel_rpm.motor_3 = x_rpm + y_rpm - tan_rpm;
kinematics.exp_wheel_rpm.motor_4 = x_rpm - y_rpm + tan_rpm;
}
void Encoder_Task(u32 dT_us)
{
curr_encoder[0] = -(short)TIM2->CNT;
curr_encoder[1] = (short)TIM3->CNT;
curr_encoder[2] = -(short)TIM4->CNT;
curr_encoder[3] = (short)TIM20->CNT;
for(int i = 0; i < 4; i++){
encoder_incre[i] = curr_encoder[i] - last_encoder[i];
if(encoder_incre[i] > 10000){
encoder_incre[i] -= 65535;
}
else if(encoder_incre[i] < -10000){
encoder_incre[i] += 65535;
}
last_encoder[i] = curr_encoder[i];
}
float dT_s = dT_us * 1e-6;
kinematics.fb_wheel_rpm.motor_1 = encoder_incre[0] / 30000.0f / dT_s * 60.0f;
kinematics.fb_wheel_rpm.motor_2 = encoder_incre[1] / 30000.0f / dT_s * 60.0f;
kinematics.fb_wheel_rpm.motor_3 = encoder_incre[2] / 30000.0f / dT_s * 60.0f;
kinematics.fb_wheel_rpm.motor_4 = encoder_incre[3] / 30000.0f / dT_s * 60.0f;
}
void Motor_Task(u32 dT_us)
{
Encoder_Task(dT_us);
Exp_Speed_Cal(dT_us);
Fb_Speed_Cal(dT_us);
PID_Controller(dT_us, kinematics.exp_wheel_rpm.motor_1, kinematics.fb_wheel_rpm.motor_1, &pid[FL], 0, 0);
pid[FL].out = Get_MiMx(pid[FL].out, -1.0, 1.0);
PID_Controller(dT_us, kinematics.exp_wheel_rpm.motor_2, kinematics.fb_wheel_rpm.motor_2, &pid[FR], 0, 0);
pid[FR].out = Get_MiMx(pid[FR].out, -1.0, 1.0);
PID_Controller(dT_us, kinematics.exp_wheel_rpm.motor_3, kinematics.fb_wheel_rpm.motor_3, &pid[BL], 0, 0);
pid[BL].out = Get_MiMx(pid[BL].out, -1.0, 1.0);
PID_Controller(dT_us, kinematics.exp_wheel_rpm.motor_4, kinematics.fb_wheel_rpm.motor_4, &pid[BR], 0, 0);
pid[BR].out = Get_MiMx(pid[BR].out, -1.0, 1.0);
kinematics.pwm.motor_1 = -pid[FL].out;
kinematics.pwm.motor_2 = -pid[FR].out;
kinematics.pwm.motor_3 = -pid[BL].out;
kinematics.pwm.motor_4 = -pid[BR].out;
Set_PWM();
}
通过PID控制算法，可以精确控制小车在各个方向的速度，实现对小车的精确控制。
本文原文来自csdn