挑战全网思路最清晰的六足机器人步态设计——三角步态（逆运动学），跟着思路做就没有多大问题。注意：需要提前具备机器人学的知识和高数的内容，以及单片机的掌握能力。（代码注释十分清晰，跟着1-6慢慢理解）
1.逆运动学代码
double theta1_new, theta2_new, theta3_new; // 逆运动学解算后的角度值
// 逆运动学 计算出三个角度
void Inverse_Kinematics(double x, double y, double z)
{
double L1 = 0.054; // 单位m
double L2 = 0.061;
double L3 = 0.155;
double R = sqrt(x * x + y * y);
double aerfaR = atan2(-z, R - L1); // 关节点2与足点的连线与x轴的夹角
double Lr = sqrt(z * z + (R - L1) * (R - L1)); // 关节点2到足点的长度
double aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2)); // 关节点2与足点的连线与L2的夹角
theta1_new = atan2(y, x);
theta2_new = aerfa1 - aerfaR;
double aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3)); // 关节点2与足点的连线与L3的夹角
theta3_new = -(aerfa1 + aerfa2);
}2.计算齐次变换矩阵
// 定义六个coxa关节相对于中央点坐标系下的位置
double body_contact_points[6][4] = {
{0, 0.076, 0, 1}, // MR 中右
{0.0848, 0.076, 0, 1}, // FR 前右
{0.0848, -0.076, 0, 1}, // FL 前左
{0, -0.076, 0, 1}, // ML 中左
{-0.0848, -0.076, 0, 1}, // HL 后左
{-0.0848, 0.076, 0, 1} // HR 后右
};
// 定义六个coxa对应中央坐标系的朝向转角（弧度制）
double body_contact_points_rotation[] = {
0, // MR
56 * PI / 180, // FR
(56 + 68) * PI / 180, // FL
PI, // ML
-(56 + 68) * PI / 180, // HL
-56 * PI / 180, // HR
};
double Homo[4][4]; // 创建一个总的4x4矩阵存储 center-to-coxa齐次变换矩阵
void get_transformation_homo(double rx, double ry, double rz, double x, double y, double z)
{
double rx_d = rx * PI / 180.0;
double ry_d = ry * PI / 180.0;
double rz_d = rz * PI / 180.0; // 将角度转换为弧度
Homo[0][0] = cos(rx_d) * cos(rz_d);
Homo[0][1] = -cos(ry_d) * sin(rz_d);
Homo[0][2] = sin(ry_d);
Homo[0][3] = x;
Homo[1][0] = -sin(rx_d) * sin(ry_d) * cos(rz_d) + cos(rx_d) * sin(rz_d);
Homo[1][1] = sin(rx_d) * sin(ry_d) * cos(rz_d) + cos(rx_d) * cos(rz_d);
Homo[1][2] = sin(rx_d) * cos(ry_d);
Homo[1][3] = y;
Homo[2][0] = -cos(rx_d) * sin(ry_d) * cos(rz_d) + sin(rx_d) * sin(rz_d);
Homo[2][1] = cos(rx_d) * sin(ry_d) * sin(rz_d) + sin(rx_d) * cos(rz_d);
Homo[2][2] = cos(rx_d) * cos(ry_d);
Homo[2][3] = z;
Homo[3][0] = 0;
Homo[3][1] = 0;
Homo[3][2] = 0;
Homo[3][3] = 1;
}3.计算目标位置坐标
double body_contact_points_renew[6][4]; // 存储coxa的转换位置
void get_coxa_trans_to_tibia_position(void)
{
int i;
for (i = 0; i < 6; i++)
{
body_contact_points_renew[i][0] = Homo[0][0] * body_contact_points[i][0] + Homo[0][1] * body_contact_points[i][1] + Homo[0][2] * body_contact_points[i][2] + Homo[0][3];
body_contact_points_renew[i][1] = Homo[1][0] * body_contact_points[i][0] + Homo[1][1] * body_contact_points[i][1] + Homo[1][2] * body_contact_points[i][2] + Homo[1][3];
body_contact_points_renew[i][2] = Homo[2][0] * body_contact_points[i][0] + Homo[2][1] * body_contact_points[i][1] + Homo[2][2] * body_contact_points[i][2] + Homo[2][3];
body_contact_points_renew[i][3] = 1;
} // 算出每个coxa的偏移坐标
double offset[6][3]; // coxa关节点-老位置到新位置的偏移
for (i = 0; i < 6; i++)
{
offset[i][0] = body_contact_points[i][0] - body_contact_points_renew[i][0];
offset[i][1] = body_contact_points[i][1] - body_contact_points_renew[i][1];
offset[i][2] = body_contact_points[i][2] - body_contact_points_renew[i][2];
}
double P[3] = {0.1577, 0, -0.1227}; // 每个coxa坐标下，足末端的位置
for (i = 0; i < 6; i++)
{
target_position[i][0] = offset[i][0] * cos(body_contact_points_rotation[i]) + offset[i][1] * sin(body_contact_points_rotation[i]) + P[0];
target_position[i][1] = -offset[i][0] * sin(body_contact_points_rotation[i]) + offset[i][1] * cos(body_contact_points_rotation[i]) + P[1];
target_position[i][2] = offset[i][2] + P[2];
}
}4.步态规划（设置角度序列）
#define step_T 20
double swing[step_T]; // 摆动-动作序列-y方向
double stance[step_T]; // 固定-动作序列-y方向
double hip[step_T]; // 摆动-动作序列-z方向
double swing_x[step_T];
double stance_x[step_T];
double length = 0.05; // 自定义前进的距离
double height = 0.04; // 自定义运动脚抬起的最大高度
// 初始化步态序列函数
void linspace_init()
{
int i = 0;
for (i = 0; i < step_T; i++)
{
stance[i] = 0 - (((double)i / (step_T)) * (length)); // 0~-0.04
stance_x[i] = 0 - (((double)i / (step_T)) * (length));
}
for (i = 0; i < step_T; i++)
{
swing[i] = (((double)i / (step_T)) * (length)); // 0~0.04
swing_x[i] = (((double)i / (step_T)) * (length));
}
for (i = 0; i < (step_T); i++)
{
hip[i] = 0 - (((double)i / (step_T)) * (height)); // 0~-0.06
}
for (i = (step_T * 0.5); i < (step_T); i++)
{
hip[i] = -height + (((double)i / (step_T)) * (height)); //-0.06~0
}
}
// 更新步态序列函数
void linspace()
{
int i = 0;
for (i = 0; i < step_T; i++)
{
stance[i] = -length + (((double)i / (step_T)) * (length * 2)); //-0.04~0.04
stance_x[i] = -length + (((double)i / (step_T)) * (length * 2));
}
for (i = 0; i < step_T; i++)
{
swing[i] = length - (((double)i / (step_T)) * (length * 2)); // 0.04~-0.04
swing_x[i] = length - (((double)i / (step_T)) * (length * 2));
}
for (i = 0; i < (step_T * 0.5); i++)
{
hip[i] = 0 - (((double)i / (step_T)) * (height)); // 0~-0.06
}
for (i = (step_T * 0.5); i < (step_T); i++)
{
hip[i] = -height + (((double)i / (step_T)) * (height)); //-0.06~0
}
}
// 定义数组，来存储角度序列
double alpha_W[6][step_T];
double gma_W[6][step_T];
double beta_W[6][step_T]; // 走路的时候，每条腿前进的角度运动序列
double alpha_S[6][step_T];
double gma_S[6][step_T];
double beta_S[6][step_T]; // 支撑的时候，每条腿前进的角度运动序列
extern double theta1_new, theta2_new, theta3_new;
// 逆运动学角度序列填充 (double PID_roll,double PID_pitch)
void inverse_kinematics_angle_filling(char direct)
{
int i, j;
if (direct)
{
for (i = 0; i < step_T; i++)
{
for (j = 0; j < 6; j++)
{
//Servo_IK(0, 0, 0, swing_x[i], swing[i], hip[i], j);
Servo_IK(0, 0, 0, 0, swing[i], hip[i], j);
alpha_W[j][i] = theta1_new;
gma_W[j][i] = theta2_new;
beta_W[j][i] = theta3_new;
}
}
for (i = 0; i < step_T; i++)
{
for (j = 0; j < 6; j++)
{
//Servo_IK(0, 0, 0, stance_x[i], stance[i], 0, j);
Servo_IK(0, 0, 0, 0, stance[i], 0, j);
alpha_S[j][i] = theta1_new;
gma_S[j][i] = theta2_new;
beta_S[j][i] = theta3_new;
}
}
}
else if (!direct)
{
for (i = 0; i < step_T; i++)
{
for (j = 0; j < 6; j++)
{
//Servo_IK(0, 0, 0, -swing_x[i], -swing[i], hip[i], j);
Servo_IK(0, 0, 0, 0, -swing[i], hip[i], j);
alpha_W[j][i] = theta1_new;
gma_W[j][i] = theta2_new;
beta_W[j][i] = theta3_new;
}
}
for (i = 0; i < step_T; i++)
{
for (j = 0; j < 6; j++)
{
//Servo_IK(0, 0, 0, -stance_x[i], -stance[i], 0, j);
Servo_IK(0, 0, 0, 0, -stance[i], 0, j);
alpha_S[j][i] = theta1_new;
gma_S[j][i] = theta2_new;
beta_S[j][i] = theta3_new;
}
}
}
}5.舵机控制
/********************************************************************************************************
LX224总线舵机调节范围：
0 —— 1000 500
----------------10° --> 10 * (1000/240.0) = temp
实际发送：500 ± (int)temp
0 —— 240°
弧度-->舵机角度：int degrees = (int)(angle * (180.0 / PI) * (1000 / 240.0));
舵机角度-->弧度：temp = 400*(240/1000)(PI/180)
********************************************************************************************************/
// 角度转换 (弧度-->度数-->舵机角度)
int Angle_convert(double angle)
{
double degrees = angle * (180.0 / PI);
int servoAngle = (int)(degrees * (1000 / 240.0));
return servoAngle;
}
/*************************************************************************************/
/*************************************************************************************
左边：
coxa 关节舵机 角度增大：向前 角度减小：向后
fumer 关节舵机 角度增大：向上 角度减小：向下
tibia 关节舵机 角度增大：向下 角度减小：向上
左边：
coxa 关节舵机 角度增大：向后 角度减小：向前
fumer 关节舵机 角度增大：向下 角度减小：向上
tibia 关节舵机 角度增大：向上 角度减小：向下
// 10 200-800
// 11 100-900
// 12 0-1000
// 13 250-750
// 14 100-900
// 2 100-900
*************************************************************************************/
/*************************************************************************************/
// 设置总线舵机角度(参数为弧度值)
void Servo_Set_Position(u8 Servo_ID, double angle, uint16_t Time)
{
int Servo_angle;
Servo_angle = Angle_convert(angle);
// 左边三条腿
if (Servo_ID > 9)
{
if (Servo_ID == 10 || Servo_ID == 13 || Servo_ID == 16) // 如果是coxa关节
{
Servo_angle = 500 - Servo_angle;
if (100 <= Servo_angle && Servo_angle <= 900)
{
printf("舵机调试信息coxa : Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
moveServo(Servo_ID, Servo_angle, Time);
}
else
{
printf("left_coxa_angle_error!\r\n"); // 打印错误信息
}
}
if (Servo_ID == 11 || Servo_ID == 14 || Servo_ID == 17) // 如果是femur关节
{
Servo_angle = 500 - Servo_angle;
if (50 <= Servo_angle && Servo_angle <= 950)
{
printf("舵机调试信息femur: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
moveServo(Servo_ID, Servo_angle, Time);
}
else
{
printf("left_femur_angle_error!\r\n"); // 打印错误信息
}
}
if (Servo_ID == 12 || Servo_ID == 15 || Servo_ID == 18) // 如果是tibia关节
{
Servo_angle = 500 + Servo_angle;
if (0 <= Servo_angle && Servo_angle <= 1000)
{
printf("舵机调试信息tibia: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
moveServo(Servo_ID, Servo_angle, Time);
}
else
{
printf("left_tibia_angle_error! "); // 打印错误信息
printf("错误信息: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
}
}
}
// 右边三条腿
else
{
if (Servo_ID == 1 || Servo_ID == 4 || Servo_ID == 7) // 如果是coxa关节
{
Servo_angle = 500 + Servo_angle;
if (100 <= Servo_angle && Servo_angle <= 900)
{
printf("舵机调试信息coxa : Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
moveServo(Servo_ID, Servo_angle, Time);
}
else
{
printf("right_coxa_angle_error!\r\n"); // 打印错误信息
}
}
if (Servo_ID == 2 || Servo_ID == 5 || Servo_ID == 8) // 如果是femur关节
{
Servo_angle = 500 + Servo_angle;
if (50 <= Servo_angle && Servo_angle <= 950)
{
printf("舵机调试信息femur: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
moveServo(Servo_ID, Servo_angle, Time);
}
else
{
printf("right_femur_angle_error!\r\n"); // 打印错误信息
}
}
if (Servo_ID == 3 || Servo_ID == 6 || Servo_ID == 9) // 如果是tibia关节
{
Servo_angle = 500 - Servo_angle;
if (0 <= Servo_angle && Servo_angle <= 1000)
{
printf("舵机调试信息tibia: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
moveServo(Servo_ID, Servo_angle, Time);
}
else
{
printf("right_tibia_angle_error! "); // 打印错误信息
printf("错误信息: Servo_ID= %d, Servo_angle = %d\r\n", Servo_ID, Servo_angle);
}
}
}
}
// 控制一条腿运动
void Servo_Control_Leg(unsigned char leg, double alpha1, double beta1, double gamma1, uint16_t Time)
{
int base_index = 3 * leg - 2; // 计算当前腿的舵机ID
for (int seg = 0; seg < 3; seg++)
{
int servo_ID = base_index + seg; // 根据关节位置计算舵机ID
switch (seg)
{
case 0: // COXA
Servo_Set_Position(servo_ID, alpha1, Time);
break;
case 1: // FEMUR
Servo_Set_Position(servo_ID, beta1, Time);
break;
case 2: // TIBIA
Servo_Set_Position(servo_ID, gamma1, Time);
break;
default:
break;
}
}
}6.控制板与舵机通信
/****************************************************************************************************************
舵机控制板实现六足机器人 舵机控制板实现六足机器人 舵机控制板实现六足机器人 舵机控制板实现六足机器人
****************************************************************************************************************/
uint16_t batteryVolt; // 电压
// 功能：控制单个舵机转动
void moveServo(uint8_t servoID, uint16_t Position, uint16_t Time)
{
HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
HexapodTxBuf[2] = 8; // 数据长度=要控制舵机数*3+5，此处=1*3+5
HexapodTxBuf[3] = CMD_SERVO_MOVE; // 填充舵机移动指令
HexapodTxBuf[4] = 1; // 要控制的舵机个数
HexapodTxBuf[5] = GET_LOW_BYTE(Time); // 取得时间的低八位1
HexapodTxBuf[6] = GET_HIGH_BYTE(Time); // 取得时间的高八位
HexapodTxBuf[7] = servoID; // 舵机ID
HexapodTxBuf[8] = GET_LOW_BYTE(Position); // 取得目标位置的低八位
HexapodTxBuf[9] = GET_HIGH_BYTE(Position); // 取得目标位置的高八位
// HAL_UART_Transmit_DMA(&huart1, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送给串口打印
// printf("\r\n发送给舵机的指令:");
// printf("\r\n");
HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送给控制板
}
// 控制多个舵机转动
// Num:舵机个数,Time:转动时间,...:舵机ID,转动角，舵机ID,转动角度 如此类推
void moveServos(uint8_t Num, uint16_t Time, ...)
{
uint8_t index = 7;
uint8_t i = 0;
uint16_t temp;
va_list arg_ptr; //
va_start(arg_ptr, Time); // 取得可变参数首地址
if (Num < 1 || Num > 32)
{
return; // 舵机数不能为零和大与32，时间不能小于0
}
HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
HexapodTxBuf[2] = Num * 3 + 5; // 数据长度 = 要控制舵机数 * 3 + 5
HexapodTxBuf[3] = CMD_SERVO_MOVE; // 舵机移动指令
HexapodTxBuf[4] = Num; // 要控制舵机数
HexapodTxBuf[5] = GET_LOW_BYTE(Time); // 取得时间的低八位
HexapodTxBuf[6] = GET_HIGH_BYTE(Time); // 取得时间的高八位
for (i = 0; i < Num; i++)
{ // 从可变参数中取得并循环填充舵机ID和对应目标位置
temp = va_arg(arg_ptr, int); // 可参数中取得舵机ID
HexapodTxBuf[index++] = GET_LOW_BYTE(((uint16_t)temp));
temp = va_arg(arg_ptr, int); // 可变参数中取得对应目标位置
HexapodTxBuf[index++] = GET_LOW_BYTE(((uint16_t)temp)); // 填充目标位置低八位
HexapodTxBuf[index++] = GET_HIGH_BYTE(temp); // 填充目标位置高八位
}
va_end(arg_ptr); // 置空arg_ptr
// printf("动作组指令：");
HAL_UART_Transmit_DMA(&huart1, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送给串口打印
// printf("%s", HexapodTxBuf);
HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送给控制板
}
// 控制一条腿 --> 三个舵机
// ID：舵机ID，PT：舵机位置
void Move_Hexapod_Leg(uint8_t Num, uint16_t Time, uint16_t ID1, uint16_t PT1, uint16_t ID2, uint16_t PT2, uint16_t ID3, uint16_t PT3)
{
uint8_t index = 7;
HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
HexapodTxBuf[2] = Num * 3 + 5; // 数据长度 = 要控制舵机数 * 3 + 5
HexapodTxBuf[3] = CMD_SERVO_MOVE; // 舵机移动指令
HexapodTxBuf[4] = Num; // 要控制舵机数
HexapodTxBuf[5] = GET_LOW_BYTE(Time); // 取得时间的低八位
HexapodTxBuf[6] = GET_HIGH_BYTE(Time); // 取得时间的高八位
HexapodTxBuf[index++] = ID1; // 填充舵机ID
HexapodTxBuf[index++] = GET_LOW_BYTE(PT1); // 填充目标位置低八位
HexapodTxBuf[index++] = GET_HIGH_BYTE(PT1); // 填充目标位置高八位
HexapodTxBuf[index++] = ID2; // 填充舵机ID
HexapodTxBuf[index++] = GET_LOW_BYTE(PT2); // 填充目标位置低八位
HexapodTxBuf[index++] = GET_HIGH_BYTE(PT2); // 填充目标位置高八位
HexapodTxBuf[index++] = ID3; // 填充舵机ID
HexapodTxBuf[index++] = GET_LOW_BYTE(PT3); // 填充目标位置低八位
HexapodTxBuf[index++] = GET_HIGH_BYTE(PT3); // 填充目标位置高八位
HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送
}
// 运行指定动作组
// Times:执行次数
void runActionGroup(uint8_t numOfAction, uint16_t Times)
{
HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
HexapodTxBuf[2] = 5; // 数据长度，数据帧除帧头部分数据字节数，此命令固定为5
HexapodTxBuf[3] = CMD_ACTION_GROUP_RUN; // 填充运行动作组命令
HexapodTxBuf[4] = numOfAction; // 填充要运行的动作组号
HexapodTxBuf[5] = GET_LOW_BYTE(Times); // 取得要运行次数的低八位
HexapodTxBuf[6] = GET_HIGH_BYTE(Times); // 取得要运行次数的高八位
HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, 7); // 发送
// HAL_UART_Transmit_DMA(&huart1, (uint8_t *)HexapodTxBuf, 7); //发送
}
// 停止动作组运行
void stopActionGroup(void)
{
HexapodTxBuf[0] = FRAME_HEADER; // 填充帧头
HexapodTxBuf[1] = FRAME_HEADER;
HexapodTxBuf[2] = 2; // 数据长度，数据帧除帧头部分数据字节数，此命令固定为2
HexapodTxBuf[3] = CMD_ACTION_GROUP_STOP; // 填充停止运行动作组命令
HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送
}
// 设定指定动作组的运行速度
void setActionGroupSpeed(uint8_t numOfAction, uint16_t Speed)
{
HexapodTxBuf[0] = HexapodTxBuf[1] = FRAME_HEADER; // 填充帧头
HexapodTxBuf[2] = 5; // 数据长度，数据帧除帧头部分数据字节数，此命令固定为5
HexapodTxBuf[3] = CMD_ACTION_GROUP_SPEED; // 填充设置动作组速度命令
HexapodTxBuf[4] = numOfAction; // 填充要设置的动作组号
HexapodTxBuf[5] = GET_LOW_BYTE(Speed); // 获得目标速度的低八位
HexapodTxBuf[6] = GET_HIGH_BYTE(Speed); // 获得目标熟读的高八位
HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送
}
// 设置所有动作组的运行速度
void setAllActionGroupSpeed(uint16_t Speed)
{
setActionGroupSpeed(0xFF, Speed); // 调用动作组速度设定，组号为0xFF时设置所有组的速度
}
// 发送获取电池电压命令
void getBatteryVoltage(void)
{
// uint16_t Voltage = 0;
HexapodTxBuf[0] = FRAME_HEADER; // 填充帧头
HexapodTxBuf[1] = FRAME_HEADER;
HexapodTxBuf[2] = 2; // 数据长度，数据帧除帧头部分数据字节数，此命令固定为2
HexapodTxBuf[3] = CMD_GET_BATTERY_VOLTAGE; // 填充获取电池电压命令
HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送
}PS. 有问题私信或者评论，视情况决定是否继续更新。