当前位置：首页 > news >正文

18002.机器人电机姿态控制

news 2025/10/1 7:47:57

文章目录

1 背景
2 实现过程
2 双线性插值算法
结论

1 背景

通过3个电机实现对机器人脖子的控制，包括俯仰, 翻滚，偏航。其中俯仰和翻滚由2个电机共同控制，偏航由单独电机控制。

2 实现过程

通过对脖子进行极限测试，分别俯仰, 翻滚的极值数据，并进行记录。

在这里插入图片描述

人的脖子目视正前方定义为pitch 为0，roll为0, yaw 为0.
角度值电机1 电机2
俯（负）向前25度 D1 = -0.42 ，D2 = 0.42
仰（正）向后32度 C1 = 0.66 ， C2 = -0.66
滚（正）向左25度 B1 = -0.57, B2 = -0.57
滚 (负) 向右25度 A1 = 0.55 , A2 = 0.55

在这里插入图片描述

2 双线性插值算法

#include <vector>
#include <cmath>
#include <algorithm>/*** @brief 将脖子的俯仰和翻滚角度映射到两个电机的控制值，基于精确的四个关键点对应关系* @param pitch_rad 俯仰角（弧度）。向前俯为负，向后仰为正。* @param roll_rad 翻滚角（弧度）。向左翻滚为正，向右翻滚为负。* @param yaw_rad 偏航角（弧度）。当前映射关系未定义，保留参数。* @return std::vector<float> 包含两个电机控制值的向量：[motor1, motor2, 0]。*/
std::vector<float> parallelMapping(float pitch_rad, float roll_rad, float yaw_rad = 0.0f) {// 1. 将弧度转换为角度float pitch_deg = pitch_rad * 180.0f / M_PI;float roll_deg = roll_rad * 180.0f / M_PI;// 2. 定义关键点坐标（基于您提供的精确数据）// 俯仰运动关键点const float PITCH_FORWARD_DEG = -25.0f;  // D点：前俯25°const float PITCH_FORWARD_M1 = -0.42f;   // D点电机1值const float PITCH_FORWARD_M2 = 0.42f;    // D点电机2值const float PITCH_BACKWARD_DEG = 32.0f;  // C点：后仰32°const float PITCH_BACKWARD_M1 = 0.66f;   // C点电机1值const float PITCH_BACKWARD_M2 = -0.66f;  // C点电机2值// 翻滚运动关键点const float ROLL_LEFT_DEG = 25.0f;       // B点：左翻滚25°const float ROLL_LEFT_M1 = -0.57f;       // B点电机1值const float ROLL_LEFT_M2 = -0.57f;       // B点电机2值const float ROLL_RIGHT_DEG = -25.0f;     // A点：右翻滚25°const float ROLL_RIGHT_M1 = 0.55f;       // A点电机1值const float ROLL_RIGHT_M2 = 0.55f;        // A点电机2值// 3. 对输入角度进行安全限幅pitch_deg = std::clamp(pitch_deg, PITCH_FORWARD_DEG, PITCH_BACKWARD_DEG);roll_deg = std::clamp(roll_deg, ROLL_RIGHT_DEG, ROLL_LEFT_DEG);// 4. 初始化电机输出值float motor1 = 0.0f;float motor2 = 0.0f;// 5. 计算俯仰（pitch）分量对电机的贡献if (pitch_deg >= 0) {// 后仰运动：在C点(32°, 0.66, -0.66)和原点(0°, 0, 0)之间线性插值float t_pitch = pitch_deg / PITCH_BACKWARD_DEG; // 归一化参数 [0, 1]motor1 += PITCH_BACKWARD_M1 * t_pitch;motor2 += PITCH_BACKWARD_M2 * t_pitch;} else {// 前俯运动：在D点(-25°, -0.42, 0.42)和原点(0°, 0, 0)之间线性插值float t_pitch = -pitch_deg / PITCH_FORWARD_DEG; // 取绝对值，归一化参数 [0, 1]motor1 += PITCH_FORWARD_M1 * t_pitch;motor2 += PITCH_FORWARD_M2 * t_pitch;}// 6. 计算翻滚（roll）分量对电机的贡献if (roll_deg >= 0) {// 左翻滚运动：在B点(25°, -0.57, -0.57)和原点(0°, 0, 0)之间线性插值float t_roll = roll_deg / ROLL_LEFT_DEG; // 归一化参数 [0, 1]motor1 += ROLL_LEFT_M1 * t_roll;motor2 += ROLL_LEFT_M2 * t_roll;} else {// 右翻滚运动：在A点(-25°, 0.55, 0.55)和原点(0°, 0, 0)之间线性插值float t_roll = -roll_deg / ROLL_RIGHT_DEG; // 取绝对值，归一化参数 [0, 1]motor1 += ROLL_RIGHT_M1 * t_roll;motor2 += ROLL_RIGHT_M2 * t_roll;}// 7. 对最终的电机输出值进行安全限幅motor1 = std::clamp(motor1, -1.0f, 1.0f);motor2 = std::clamp(motor2, -1.0f, 1.0f);return {motor1, motor2, 0.0f}; // 第三个值保留给yaw，当前未使用
}

// 测试几个典型角度组合
int main() {// 中立位置（所有角度为0）auto result1 = parallelMapping(0.0f, 0.0f, 0.0f);printf("中立: motor1=%.2f, motor2=%.2f\n", result1[0], result1[1]);// 最大后仰（pitch = 32° ≈ 0.56弧度）auto result2 = parallelMapping(0.56f, 0.0f, 0.0f);printf("后仰: motor1=%.2f, motor2=%.2f\n", result2[0], result2[1]);// 最大前俯（pitch = -25° ≈ -0.44弧度）auto result3 = parallelMapping(-0.44f, 0.0f, 0.0f);printf("前俯: motor1=%.2f, motor2=%.2f\n", result3[0], result3[1]);// 组合运动：中等后仰+左翻滚auto result4 = parallelMapping(0.3f, 0.2f, 0.0f);printf("组合: motor1=%.2f, motor2=%.2f\n", result4[0], result4[1]);return 0;
}