【六足机器人】03步态算法
温馨提示：此部分内容需要较强的数学能力，包括但不限于矩阵运算、坐标变换、数学几何。
一、数学知识
1.1 正逆运动学（几何法）
逆运动学解算函数
// 逆运动学-->计算出三个角度
void inverse_caculate(double x, double y, double z)
{double L1 = 0.054; // 单位mdouble L2 = 0.061;double L3 = 0.155;double R = sqrt(x * x + y * y);double aerfaR = atan2(-z, R - L1); // 使用atan2以获得正确的象限double Lr = sqrt(z * z + (R - L1) * (R - L1));double aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));theta1_new = atan2(y, x); // atan2自动处理y=0的情况theta2_new = aerfa1 - aerfaR;double aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));theta3_new = -(aerfa1 + aerfa2);
}正运动学解算函数
// 正运动学-->计算出坐标(以COXA为原点)
void forward_kinematics(double theta1, double theta2, double theta3)
{double L1 = 0.054; // 单位mdouble L2 = 0.061;double L3 = 0.155;double Lr = L2 * L2 + L3 * L3 - 2 * L2 * L3 * cos(PI - theta3);double aerfa1 = acos((Lr * Lr + L2 * L2 - L3 * L3) / (2 * Lr * L2));double aerfaR = aerfa1 - theta2;my_z = -Lr * cos(aerfaR);double R = L1 + Lr * sin(aerfaR);my_x = R * cos(theta1);my_y = R * sin(theta1);
}封装后的代码
// 正运动学解算(输入关节角度计算足端坐标)
void Forward_kinematics(double theta1, double theta2, double theta3, uint8_t leg)
{ Myaxis_Init(&Pi3_axis[leg]);Pi3_axis[leg].x = cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));Pi3_axis[leg].y = sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));Pi3_axis[leg].x = L3 * sin(theta2 + theta3) + L2 * sin(theta2);
}
// 逆运动学解算(根据足端坐标计算出三个角度rad)
void Inverse_Kinematics(double x, double y, double z, uint8_t leg)
{Hexapod_thetas_Init(&Hexapod_leg[leg]);double R = sqrt(x * x + y * y);double aerfaR = atan2(-z, R - L1); // 使用atan2以获得正确的象限double Lr = sqrt(z * z + (R - L1) * (R - L1));double aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));Hexapod_leg[leg].Theta[0] = atan2(y, x); // atan2自动处理y=0的情况Hexapod_leg[leg].Theta[1] = aerfa1 - aerfaR;double aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));Hexapod_leg[leg].Theta[2] = -(aerfa1 + aerfa2);
}1.2 DH参数
1.2.1 旋转矩阵变换
1.2.2 坐标变换
1.2.3 DH参数（标准版/改进版）
1.2.4 MATLAB仿真代码
D-H参数单位:mm关节转角 关节距离 连杆长度 转角Theta(n) d(n) a(n-1) Alpha(n-1) theta1 0 0 0theta2 0 54 pi/2theta3 0 61 00 0 155 0// 正运动解算
x = cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
y = sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
z = L3 * sin(theta2 + theta3) + L2 * sin(theta2);// 逆运动解算
L1 = 0.054; %单位m
L2 = 0.061;
L3 = 0.155;
R = sqrt(x * x + y * y);
aerfaR = atan2(-z, R - L1); %使用atan2以获得正确的象限
Lr = sqrt(z * z + (R - L1) * (R - L1));
aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));
theta1_new = atan2(y, x); %atan2自动处理y=0的情况
theta2_new = aerfa1 - aerfaR;
aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));
theta3_new = -(aerfa1 + aerfa2);二、步态算法
2.1 总线舵机通信协议
servo.c
/**************************************************************************************************************/
/******************************************** 算法控制 2024.9.10 *********************************************/
/******************************************** 六足机器人：GL *********************************************/
/*************************************************************************************************************/#include "servo.h"
#include "usart.h"
#include <stdio.h>
#include "stdarg.h"
#include "ControlServo.h"
#include "arm_math.h"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+5HexapodTxBuf[3] = CMD_SERVO_MOVE; // 填充舵机移动指令HexapodTxBuf[4] = 1; // 要控制的舵机个数HexapodTxBuf[5] = GET_LOW_BYTE(Time); // 取得时间的低八位1HexapodTxBuf[6] = GET_HIGH_BYTE(Time); // 取得时间的高八位HexapodTxBuf[7] = servoID; // 舵机IDHexapodTxBuf[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 + 5HexapodTxBuf[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); // 可参数中取得舵机IDHexapodTxBuf[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 + 5HexapodTxBuf[3] = CMD_SERVO_MOVE; // 舵机移动指令HexapodTxBuf[4] = Num; // 要控制舵机数HexapodTxBuf[5] = GET_LOW_BYTE(Time); // 取得时间的低八位HexapodTxBuf[6] = GET_HIGH_BYTE(Time); // 取得时间的高八位HexapodTxBuf[index++] = ID1; // 填充舵机IDHexapodTxBuf[index++] = GET_LOW_BYTE(PT1); // 填充目标位置低八位HexapodTxBuf[index++] = GET_HIGH_BYTE(PT1); // 填充目标位置高八位HexapodTxBuf[index++] = ID2; // 填充舵机IDHexapodTxBuf[index++] = GET_LOW_BYTE(PT2); // 填充目标位置低八位HexapodTxBuf[index++] = GET_HIGH_BYTE(PT2); // 填充目标位置高八位HexapodTxBuf[index++] = ID3; // 填充舵机IDHexapodTxBuf[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; // 数据长度，数据帧除帧头部分数据字节数，此命令固定为5HexapodTxBuf[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; // 数据长度，数据帧除帧头部分数据字节数，此命令固定为2HexapodTxBuf[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; // 数据长度，数据帧除帧头部分数据字节数，此命令固定为5HexapodTxBuf[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; // 数据长度，数据帧除帧头部分数据字节数，此命令固定为2HexapodTxBuf[3] = CMD_GET_BATTERY_VOLTAGE; // 填充获取电池电压命令HAL_UART_Transmit_DMA(&huart6, (uint8_t *)HexapodTxBuf, HexapodTxBuf[2] + 2); // 发送
}servo.h
#ifndef __SERVO_H_
#define __SERVO_H_#include "main.h"
#include "usart.h"#define FRAME_HEADER 0x55 //帧头
#define CMD_SERVO_MOVE 0x03 //舵机移动指令
#define CMD_ACTION_GROUP_RUN 0x06 //运行动作组指令
#define CMD_ACTION_GROUP_STOP 0x07 //停止动作组指令
#define CMD_ACTION_GROUP_SPEED 0x0B //设置动作组运行速度
#define CMD_GET_BATTERY_VOLTAGE 0x0F //获取电池电压指令#define GET_LOW_BYTE(A) ((uint8_t)(A)) // 获得A的低八位
#define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8)) // 获得A的高八位extern uint16_t batteryVolt;void moveServo(uint8_t servoID, uint16_t Position, uint16_t Time);
void moveServos(uint8_t Num, uint16_t Time, ...);
void runActionGroup(uint8_t numOfAction, uint16_t Times);
void stopActionGroup(void);
void setActionGroupSpeed(uint8_t numOfAction, uint16_t Speed);
void setAllActionGroupSpeed(uint16_t Speed);
void getBatteryVoltage(void);// 发送指令名
#define SERVO_MOVE_TIME_WRITE 1
#define SERVO_MOVE_TIME_READ 2
#define SERVO_MOVE_TIME_WAIT_WRITE 7
#define SERVO_MOVE_TIME_WAIT_READ 8
#define SERVO_MOVE_START 11
#define SERVO_MOVE_STOP 12
#define SERVO_ID_WRITE 13
#define SERVO_ID_READ 14
#define SERVO_ANGLE_OFFSET_ADJUST 17
#define SERVO_ANGLE_OFFSET_WRITE 18
#define SERVO_ANGLE_OFFSET_READ 19
#define SERVO_ANGLE_LIMIT_WRITE 20
#define SERVO_ANGLE_LIMIT_READ 21
#define SERVO_VIN_LIMIT_WRITE 22
#define SERVO_VIN_LIMIT_READ 23
#define SERVO_TEMP_MAX_LIMIT_WRITE 24
#define SERVO_TEMP_MAX_LIMIT_READ 25
#define SERVO_TEMP_READ 26
#define SERVO_VIN_READ 27
#define SERVO_POS_READ 28
#define SERVO_OR_MOTOR_MODE_WRITE 29
#define SERVO_OR_MOTOR_MODE_READ 30
#define SERVO_LOAD_OR_UNLOAD_WRITE 31
#define SERVO_LOAD_OR_UNLOAD_READ 32
#define SERVO_LED_CTRL_WRITE 33
#define SERVO_LED_CTRL_READ 34
#define SERVO_LED_ERROR_WRITE 35
#define SERVO_LED_ERROR_READ 36// 指令长度
#define SERVO_MOVE_TIME_WRITE_LEN 7
#define SERVO_MOVE_TIME_READ_LEN 3
#define SERVO_MOVE_TIME_WAIT_WRITE_LEN 7
#define SERVO_MOVE_TIME_WAIT_READ_LEN 3
#define SERVO_MOVE_START_LEN 3
#define SERVO_MOVE_STOP_LEN 3
#define SERVO_ID_WRITE_LEN 4
#define SERVO_ID_READ_LEN 3
#define SERVO_ANGLE_OFFSET_ADJUST_LEN 4
#define SERVO_ANGLE_OFFSET_WRITE_LEN 3
#define SERVO_ANGLE_OFFSET_READ_LEN 3
#define SERVO_ANGLE_LIMIT_WRITE_LEN 7
#define SERVO_ANGLE_LIMIT_READ_LEN 3
#define SERVO_VIN_LIMIT_WRITE_LEN 7
#define SERVO_VIN_LIMIT_READ_LEN 3
#define SERVO_TEMP_MAX_LIMIT_WRITE_LEN 4
#define SERVO_TEMP_MAX_LIMIT_READ_LEN 3
#define SERVO_TEMP_READ_LEN 3
#define SERVO_VIN_READ_LEN 3
#define SERVO_POS_READ_LEN 3
#define SERVO_OR_MOTOR_MODE_WRITE_LEN 7
#define SERVO_OR_MOTOR_MODE_READ_LEN 3
#define SERVO_LOAD_OR_UNLOAD_WRITE_LEN 4
#define SERVO_LOAD_OR_UNLOAD_READ_LEN 3
#define SERVO_LED_CTRL_WRITE_LEN 4
#define SERVO_LED_CTRL_READ_LEN 3
#define SERVO_LED_ERROR_WRITE_LEN 4
#define SERVO_LED_ERROR_READ_LEN 3//接收指令长度
#define RECV_SERVO_MOVE_TIME_READ_LEN 7
#define RECV_SERVO_MOVE_TIME_WAIT_READ_LEN 7
#define RECV_SERVO_ID_READ_LEN 4
#define RECV_SERVO_ANGLE_OFFSET_READ_LEN 4
#define RECV_SERVO_ANGLE_LIMIT_READ_LEN 7
#define RECV_SERVO_VIN_LIMIT_READ_LEN 7
#define RECV_SERVO_TEMP_MAX_LIMIT_READ_LEN 4
#define RECV_SERVO_TEMP_READ_LEN 4
#define RECV_SERVO_VIN_READ_LEN 5
#define RECV_SERVO_POS_READ_LEN 5
#define RECV_SERVO_OR_MOTOR_MODE_READ_LEN 7
#define RECV_SERVO_LOAD_OR_UNLOAD_READ_LEN 4
#define RECV_SERVO_LED_CTRL_READ_LEN 4
#define RECV_SERVO_LED_ERROR_READ_LEN 4#define SERVO_BROADCAST_ID 0xFE // 广播id#endif
2.2 控制机器人腿
ControlServo.c
#include "ControlServo.h"
#include <math.h>
#include <string.h>
#include <usart.h>
#include <stdio.h>
#include <servo.h>
#include "arm_math.h"Myaxis Pi3_axis[6]; // 以髋关节为基准坐标系下的Pi3坐标,i为腿部编号(0-5:A-F)
HexapodLeg Hexapod_leg[6]; // 定义机器人的腿部结构体,i为腿部编号(0-5:A-F)
Myaxis adjhjk;// 初始化坐标系
void Myaxis_Init(Myaxis *axis)
{axis->x = 0;axis->y = 0;axis->z = 0;
}// 初始化腿部关节角度值
void Hexapod_thetas_Init(HexapodLeg *Hexapod_leg)
{Hexapod_leg->Theta[0] = 0;Hexapod_leg->Theta[1] = 0;Hexapod_leg->Theta[2] = 0;
}// 正运动学解算(输入关节角度计算足端坐标)
void Forward_kinematics(double theta1, double theta2, double theta3, uint8_t leg)
{ Myaxis_Init(&Pi3_axis[leg]);Pi3_axis[leg].x = cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));Pi3_axis[leg].y = sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));Pi3_axis[leg].x = L3 * sin(theta2 + theta3) + L2 * sin(theta2);
}
// 逆运动学解算(根据足端坐标计算出三个角度rad)
void Inverse_Kinematics(double x, double y, double z, uint8_t leg)
{Hexapod_thetas_Init(&Hexapod_leg[leg]);double R = sqrt(x * x + y * y);double aerfaR = atan2(-z, R - L1); // 使用atan2以获得正确的象限double Lr = sqrt(z * z + (R - L1) * (R - L1));double aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));Hexapod_leg[leg].Theta[0] = atan2(y, x); // atan2自动处理y=0的情况Hexapod_leg[leg].Theta[1] = aerfa1 - aerfaR;double aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));Hexapod_leg[leg].Theta[2] = -(aerfa1 + aerfa2);
}/********************************************************************************************************LX224总线舵机调节范围：0 —— 1000 500----------------10° --> 10 * (1000/240.0) = temp实际发送：500 ± (int)temp0 —— 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);}}}
}/*** @brief 控制一条腿运动* @param leg: leg为腿部编号(0-5:A-F)* @param HexapodLeg: 腿部结构体,用于存储关节的角度* @param Time: 运动时间* @retval 无*/
void Servo_Control_Leg(uint8_t leg, HexapodLeg Hexapod_leg, uint16_t Time)
{int base_index = 3 * leg + 1; // 计算当前腿的舵机IDfor (int seg = 0; seg < 3; seg++){int servo_ID = base_index + seg; // 根据关节位置计算舵机IDswitch (seg){case 0: // COXAServo_Set_Position(servo_ID, Hexapod_leg.Theta[0], Time);break;case 1: // FEMURServo_Set_Position(servo_ID, Hexapod_leg.Theta[1], Time);break;case 2: // TIBIAServo_Set_Position(servo_ID, Hexapod_leg.Theta[2], Time);break;default:break;}}
}
ControlServo.h
/****************************/
//舵机控制板实现六足机器人
/****************************/
#ifndef __CONTROLSERVO_H_
#define __CONTROLSERVO_H_#include "main.h"#define PI 3.14159265358979323846/************************************************************************ leg4 FL 12 11 10 ** ** 1 2 3 FR leg1* leg5 ML 15 14 13 ** ** 4 5 6 MR-----leg2--------逆时针 ↑↑* leg6 HL 18 17 16 ** ** 7 8 9 HR leg3
***********************************************************************/
#define MR 2 //leg2
#define FR 1 //leg1
#define FL 4 //leg4
#define ML 5 //leg5
#define HL 6 //leg6
#define HR 3 //leg3/**********************************************************************/
/*************************Hexapod3.0基本信息***************************/
/**********************************************************************/
#define L1 0.054 // coxa 关节长度 单位m
#define L2 0.061 // fumer 关节长度 单位m
#define L3 0.155 // tibia 关节长度 单位m//坐标轴
typedef struct { double x;double y;double z;
}Myaxis;//机器人腿部关节成员
typedef struct{//double Theta_Write[3];//用于输出舵机控制信号(反向运动学)//double Theta_Read[18];//用于读取舵机角度(正向运动学)double Theta[3]; //每条腿对应的3个关节角度//uint8_t servo_low[18],servo_high[18];//高低位数据接收(测试用)}HexapodLeg;/**********************************************************************/
/**********************************************************************///自己写的函数
void Forward_kinematics(double theta1, double theta2, double theta3, uint8_t leg); // 正运动学解算(输入关节角度计算足端坐标)
void Inverse_Kinematics(double x,double y,double z, uint8_t leg); // 逆运动学解算(根据足端坐标计算出三个角度rad)void Myaxis_Init(Myaxis *axis); //初始化坐标系
void Hexapod_init(void);
int Angle_convert(double angle); // 角度转换 (弧度-->度数-->舵机角度)
void Servo_Control_Leg(uint8_t leg, HexapodLeg Hexapod_leg, uint16_t Time); // 设置舵机角度(弧度)
void Servo_Set_Position(u8 Servo_ID, double angle, uint16_t Time); // 控制一条腿运动#endif
2.3 三角步态
Gait.c
/**************************************************************************************************************/
/******************************************** 步态规划 2024.9.10 *********************************************/
/******************************************** 六足机器人：GL *********************************************/
/*************************************************************************************************************/#include "Gait.h"
#include "servo.h"
#include "ControlServo.h"// 定义六个coxa关节相对于中央点坐标系下的位置
double body_contact_points[6][4] = {{0.07600, 0, 0, 1}, // MR 中右{0.07600, 0.08485, 0, 1}, // FR 前右{-0.07600, 0.08485, 0, 1}, // FL 前左{-0.07600, 0, 0, 1}, // ML 中左{-0.07600, -0.08485, 0, 1}, // HL 后左{0.07600, -0.08485, 0, 1} // HR 后右
};// 定义六个coxa对应中央坐标系的朝向转角(rad)
double body_contact_points_rotation[] = {0, // MR56 * PI / 180, // FR(56 + 68) * PI / 180, // FLPI, // ML-(56 + 68) * PI / 180, // HL-56 * PI / 180, // HR
};/**********************************************************************/
/*************************Hexapod3.0基本信息***************************/
/*********************************************************************** leg4 FL 12 11 10 ** ** 1 2 3 FR leg1* leg5 ML 15 14 13 ** ** 4 5 6 MR-----leg2--------逆时针 ↑↑* leg6 HL 18 17 16 ** ** 7 8 9 HR leg3***********************************************************************/
/**********************************************************************/
// a:coxa b:fumer c:tibia
// 初始姿态的三个关节角度设置
typedef struct
{double a;double b;double c;
} hexapod_Position;hexapod_Position Hexapod_Init_Position;
// Hexapod初始化位置: 在这里修改机器人的初始位置
void init_hexapod_position(hexapod_Position *pos)
{pos->a = 0.0;// pos->b = 0.4182;// pos->c = 4 * 0.4182;pos->b = 100.0 * (240.0 / 1000.0) * (PI / 180.0);pos->c = -400.0 * (240.0 / 1000.0) * (PI / 180.0);
}void Hexapod_init(void)
{int index = 1;int leg = 0;int seg = 0;init_hexapod_position(&Hexapod_Init_Position);for (leg = 0; leg < 6; leg++){for (seg = 0; seg < 3; seg++){switch (seg){case 0: // COXAServo_Set_Position(index, Hexapod_Init_Position.a, 200);break;case 1: // FEMURServo_Set_Position(index, Hexapod_Init_Position.b, 200);break;case 2: // TIBIAServo_Set_Position(index, Hexapod_Init_Position.c, 200);break;}index++;}// delay_ms(1000);}
}/*****************************************************************************************************************************/
/*步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划==-==步态规划*/
/*****************************************************************************************************************************///**********************(步态规划)************************//
/** leg4 FL 12 11 10 D** **A 1 2 3 FR leg1* leg5 ML 15 14 13 E** **B 4 5 6 MR-----leg2--------逆时针 ↑↑* leg6 HL 18 17 16 F** **C 7 8 9 HR leg3*/
//**********************(步态规划)************************//Myaxis CPi3_axis[6]; // 定义以机身几何中心为绝对坐标系参考下的Pi3坐标,i为腿部编号(0-5:A-F)
Myaxis Pi0_Pi3_axis[6]; // 定义以机身几何中心为绝对坐标系参考下的Pi0Pi3向量
extern Myaxis Pi3_axis[6];
// HexapodLeg Hexapod_Moveleg[6]; // 用于存储移动时的腿部信息
extern HexapodLeg Hexapod_leg[6]; // 定义机器人的腿部结构体,i为腿部编号(0-5:A-F)
// double P[3] = {0.1577, 0, -0.1227}; // 每个coxa坐标下，足末端的位置/*** @brief 初始化机器人中心坐标系下向量Pi0_Pi3的坐标向量坐标* @param Y_Tran: 机器人中心坐标系下coxa的纵坐标* @param R_Angle: 机器人中心坐标系下coxa的偏转角度* @param leg: 腿部编号(0-5:A-F)* @retval 无*/
void Hexapod_center_Position03_Init(double X, double Y, double R_Angle, uint8_t leg)
{double theta1 = Hexapod_Init_Position.a;double theta2 = Hexapod_Init_Position.b;double theta3 = Hexapod_Init_Position.c;Myaxis_Init(&CPi3_axis[leg]); // 初始化坐标系Myaxis_Init(&Pi0_Pi3_axis[leg]); // 初始化坐标系CPi3_axis[leg].x = X + cos(R_Angle) * cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2)) - sin(R_Angle) * (sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2)));CPi3_axis[leg].y = Y + sin(R_Angle) * (cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2))) + cos(R_Angle) * (sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2)));CPi3_axis[leg].z = L3 * sin(theta2 + theta3) + L2 * sin(theta2);// CPi3_axis[leg].x = X + 132.9736 * sin(R_Angle);// CPi3_axis[leg].y = Y + 132.9736 * cos(R_Angle); //132.9736为初始站立姿态关节P0到关节P3的直线距离// CPi3_axis[leg].z = - 77.3670; //77.3670为初始站立姿态高度/*机身几何中心坐标系中Pi0Pi3向量*/Pi0_Pi3_axis[leg].x = CPi3_axis[leg].x - X;Pi0_Pi3_axis[leg].y = CPi3_axis[leg].y - Y;Pi0_Pi3_axis[leg].z = CPi3_axis[leg].z - 0;
}/*** @brief 初始站立状态的位姿和Pi3坐标初始化* @retval 无*/
void Hexapod_Gait_Init(void)
{// 计算初始站立姿态的Pi3相对于腿部基准坐标的坐标位置Forward_kinematics(0, -(PI / 4), 2 * PI / 3, 0); // 0号——A_LegForward_kinematics(0, -(PI / 4), 2 * PI / 3, 1); // 1号——B_LegForward_kinematics(0, -(PI / 4), 2 * PI / 3, 2); // 2号——C_LegForward_kinematics(0, -(PI / 4), 2 * PI / 3, 3); // 3号——D_LegForward_kinematics(0, -(PI / 4), 2 * PI / 3, 4); // 4号——E_LegForward_kinematics(0, -(PI / 4), 2 * PI / 3, 5); // 5号——F_Leg// 输出初始站立姿态的角度且固定腿部基准坐标系Inverse_Kinematics(Pi3_axis[0].x, Pi3_axis[0].y, Pi3_axis[0].z, 0); // 0号——A_LegInverse_Kinematics(Pi3_axis[1].x, Pi3_axis[1].y, Pi3_axis[1].z, 1); // 1号——B_LegInverse_Kinematics(Pi3_axis[2].x, Pi3_axis[2].y, Pi3_axis[2].z, 2); // 2号——C_LegInverse_Kinematics(Pi3_axis[3].x, Pi3_axis[3].y, Pi3_axis[3].z, 3); // 3号——D_LegInverse_Kinematics(Pi3_axis[4].x, Pi3_axis[4].y, Pi3_axis[4].z, 4); // 4号——E_LegInverse_Kinematics(Pi3_axis[5].x, Pi3_axis[5].y, Pi3_axis[5].z, 5); // 5号——F_Leg// 得到不同状态下的Pi0Pi3向量相对于几何中心的位置描述Hexapod_center_Position03_Init(0.07600, 0.08485, 56 * PI / 180, 0); // 0号——A_LegHexapod_center_Position03_Init(0.07600, 0, 0, 1); // 1号——B_LegHexapod_center_Position03_Init(0.07600, -0.08485, -56 * PI / 180, 2); // 2号——C_LegHexapod_center_Position03_Init(-0.07600, 0.08485, (56 + 68) * PI / 180, 3); // 3号——D_LegHexapod_center_Position03_Init(-0.07600, 0, PI, 4); // 4号——E_LegHexapod_center_Position03_Init(-0.07600, -0.08485, -(56 + 68) * PI / 180, 5); // 5号——F_Leg
}//**********************(纵向三角步态规划)************************//
//*************************ACE——BDF交替**************************//static Myaxis Move_Position03; // 重新缓存Pi3坐标
/*ACE转角-前进*/
void ACE_Advance_Gait_LegUp(void)
{Move_Position03 = Pi3_axis[0]; // 依照每条腿的机械结构和关节转动特点设立的坐标系,所以腿部基准坐标系都一样，这里随便取一个Pi3坐标// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x - SL_half_edge * 0.25, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 0); // 0号——A_LegInverse_Kinematics(Move_Position03.x + SL_half_edge * 0.25, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 2); // 2号——C_LegInverse_Kinematics(Move_Position03.x, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 4); // 4号——E_Leg// 发送控制指令Servo_Control_Leg(A, Hexapod_leg[A], pace_time);Servo_Control_Leg(C, Hexapod_leg[C], pace_time);Servo_Control_Leg(E, Hexapod_leg[E], pace_time);delay_ms(pace_time);
}/*ACE转角-落脚*/
void ACE_Advance_Gait_LegMove_1(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y + SL_half_edge, Move_Position03.z, 0); // 0号——A_LegInverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y + SL_half_edge, Move_Position03.z, 2); // 2号——C_LegInverse_Kinematics(Move_Position03.x, Move_Position03.y + SL_half_edge, Move_Position03.z, 4); // 4号——E_Leg// 发送控制指令Servo_Control_Leg(A, Hexapod_leg[A], pace_time);Servo_Control_Leg(C, Hexapod_leg[C], pace_time);Servo_Control_Leg(E, Hexapod_leg[E], pace_time);delay_ms(pace_time);
}/*ACE移动*/
void ACE_Advance_Gait_LegMove_2(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 0); // 0号——A_LegInverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 2); // 2号——C_LegInverse_Kinematics(Move_Position03.x, Move_Position03.y - SL_half_edge, Move_Position03.z, 4); // 4号——E_Leg// 发送控制指令Servo_Control_Leg(A, Hexapod_leg[A], pace_time);Servo_Control_Leg(C, Hexapod_leg[C], pace_time);Servo_Control_Leg(E, Hexapod_leg[E], pace_time);delay_ms(pace_time);
}/*BDF抬脚-转角*/
void BDF_Advance_Gait_LegUp(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 1); // 1号——B_LegInverse_Kinematics(Move_Position03.x + SL_half_edge * 0.25, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 3); // 3号——D_LegInverse_Kinematics(Move_Position03.x - SL_half_edge * 0.25, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z + Hip_edge, 5); // 5号——F_Leg// 发送控制指令Servo_Control_Leg(B, Hexapod_leg[B], pace_time);Servo_Control_Leg(D, Hexapod_leg[D], pace_time);Servo_Control_Leg(F, Hexapod_leg[F], pace_time);delay_ms(pace_time);
}/*BDF转角-落脚*/
void BDF_Advance_Gait_LegMove_1(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x, Move_Position03.y + SL_half_edge, Move_Position03.z, 1); // 1号——B_LegInverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y + SL_half_edge, Move_Position03.z, 3); // 3号——D_LegInverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y + SL_half_edge, Move_Position03.z, 5); // 5号——F_Leg// 发送控制指令Servo_Control_Leg(B, Hexapod_leg[B], pace_time);Servo_Control_Leg(D, Hexapod_leg[D], pace_time);Servo_Control_Leg(F, Hexapod_leg[F], pace_time);delay_ms(pace_time);
}/*BDF前进*/
void BDF_Advance_Gait_LegMove_2(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x, Move_Position03.y - SL_half_edge, Move_Position03.z, 1); // 1号——B_LegInverse_Kinematics(Move_Position03.x - SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 3); // 3号——D_LegInverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y - SL_half_edge, Move_Position03.z, 5); // 5号——F_Leg// 发送控制指令Servo_Control_Leg(B, Hexapod_leg[B], pace_time);Servo_Control_Leg(D, Hexapod_leg[D], pace_time);Servo_Control_Leg(F, Hexapod_leg[F], pace_time);delay_ms(pace_time);
}/*Hexapod前进步态*/
void Hexapod_Advance_Gait(void)
{BDF_Advance_Gait_LegUp();ACE_Advance_Gait_LegMove_2();BDF_Advance_Gait_LegMove_1();ACE_Advance_Gait_LegUp();BDF_Advance_Gait_LegMove_2();ACE_Advance_Gait_LegMove_1();
}/*
*********************(横向三角步态规划)************************
*************************ACE——BDF交替*************************
*/void ACE_RightTrans_Gait_LegUp(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y - SL_half_edge * 0.25, Move_Position03.z + Hip_edge, 0); // 0号——A_LegInverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y + SL_half_edge * 0.25, Move_Position03.z + Hip_edge, 2); // 2号——C_Leg Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y, Move_Position03.z + Hip_Redge, 4); // 4号——E_Leg// 发送控制指令Servo_Control_Leg(A, Hexapod_leg[A], pace_time);Servo_Control_Leg(C, Hexapod_leg[C], pace_time);Servo_Control_Leg(E, Hexapod_leg[E], pace_time);delay_ms(pace_time);
}void ACE_RightTrans_Gait_LegMove_1(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y - SL_half_edge * 0.5, Move_Position03.z, 0); // 0号——A_LegInverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z, 2); // 2号——C_Leg Inverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y, Move_Position03.z, 4); // 4号——E_Leg// 发送控制指令Servo_Control_Leg(A, Hexapod_leg[A], pace_time);Servo_Control_Leg(C, Hexapod_leg[C], pace_time);Servo_Control_Leg(E, Hexapod_leg[E], pace_time);delay_ms(pace_time);
}void ACE_RightTrans_Gait_LegMove_2(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 2); // 2号——C_LegInverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 4); // 4号——E_LegInverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 0); // 0号——A_Leg
}void BDF_RightTrans_Gait_LegUp(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度 Inverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y, Move_Position03.z + Hip_Redge, 1); // 1号——B_LegInverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y + SL_half_edge * 0.25, Move_Position03.z + Hip_edge, 3); // 3号——D_LegInverse_Kinematics(Move_Position03.x + SL_half_edge * 0.5, Move_Position03.y - SL_half_edge * 0.25, Move_Position03.z + Hip_edge, 5); // 5号——F_Leg// 发送控制指令Servo_Control_Leg(B, Hexapod_leg[B], pace_time);Servo_Control_Leg(D, Hexapod_leg[D], pace_time);Servo_Control_Leg(F, Hexapod_leg[F], pace_time);delay_ms(pace_time);
}void BDF_RightTrans_Gait_LegMove_1(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度 Inverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y, Move_Position03.z, 1); // 1号——B_LegInverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y + SL_half_edge * 0.5, Move_Position03.z, 3); // 3号——D_LegInverse_Kinematics(Move_Position03.x + SL_half_edge, Move_Position03.y - SL_half_edge * 0.5, Move_Position03.z, 5); // 5号——F_Leg// 发送控制指令Servo_Control_Leg(B, Hexapod_leg[B], pace_time);Servo_Control_Leg(D, Hexapod_leg[D], pace_time);Servo_Control_Leg(F, Hexapod_leg[F], pace_time);delay_ms(pace_time);
}void BDF_RightTrans_Gait_LegMove_2(void)
{// 前、中、后腿的姿态规划不一样,分开用逆运动学取角度Inverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 5); // 2号——C_LegInverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 1); // 4号——E_LegInverse_Kinematics(Move_Position03.x, Move_Position03.y, Move_Position03.z, 3); // 0号——A_Leg
}/*Hexapod右转*/
void Hexapod_RightTrans_Gait(void)
{ACE_RightTrans_Gait_LegUp();BDF_RightTrans_Gait_LegMove_2();ACE_RightTrans_Gait_LegMove_1();BDF_RightTrans_Gait_LegUp();ACE_RightTrans_Gait_LegMove_2();BDF_RightTrans_Gait_LegMove_1();
}/*
*********************(右转三角步态规划)************************
*************************ACE——BDF交替*************************
*/void ACE_TurnRight_Gait_LegUp(void)
{ // //前、中、后腿的姿态规划不一样,分开用逆运动学取角度// Inverse_Kinematics(Move_Position3_X + SL_half * 1/2,Move_Position3_Y ,Move_Position3_Z + Hip_Redge,2);//2号——C_Leg// Inverse_Kinematics(Move_Position3_X - SL_half * 1/2,Move_Position3_Y,Move_Position3_Z + Hip_Redge,4);//4号——E_Leg // Inverse_Kinematics(Move_Position3_X + SL_half * 1/2,Move_Position3_Y ,Move_Position3_Z + Hip_Redge,0);//0号——A_Leg
}void ACE_TurnRight_Gait_LegMove_1(void)
{}void ACE_TurnRight_Gait_LegMove_2(void)
{}void BDF_TurnRight_Gait_LegUp(void)
{ // //前、中、后腿的姿态规划不一样,分开用逆运动学取角度// Inverse_Kinematics(Move_Position3_X - SL_half * 1/2,Move_Position3_Y ,Move_Position3_Z + Hip_Redge,2);//2号——C_Leg// Inverse_Kinematics(Move_Position3_X + SL_half * 1/2,Move_Position3_Y,Move_Position3_Z + Hip_Redge,4);//4号——E_Leg // Inverse_Kinematics(Move_Position3_X - SL_half * 1/2,Move_Position3_Y ,Move_Position3_Z + Hip_Redge,0);//0号——A_Leg
}void BDF_TurnRight_Gait_LegMove_1(void)
{}void BDF_TurnRight_Gait_LegMove_2(void)
{}void Hexapod_Turnight_Gait(void)
{ACE_TurnRight_Gait_LegUp();BDF_TurnRight_Gait_LegMove_2();ACE_TurnRight_Gait_LegMove_1();BDF_TurnRight_Gait_LegUp();ACE_TurnRight_Gait_LegMove_2();BDF_TurnRight_Gait_LegMove_1();
}/************************************************************************************************************************************************/
/************************************************************* 算法控制 2024.7.30 **************************************************************/
/***********************************************************************************************************************************************/
Gait,h
#ifndef __GAIT_H_
#define __GAIT_H_
#include "main.h"#define Hip_edge 0.01 // 新足端执行器抬升高度
#define Hip_Redge 0.01 // 足端执行器抬升高度
// #define SL_half_edge 0.10342409f/2 // 中心舵机1,7,10,16在x轴方向的步长(新步长)———髋关节转动15度确立的步长
#define SL_half_edge 0.0517204
// #define SL_half_edge 45.7388 // 中心舵机1,7,10,16在x轴方向的步长(新步长)———髋关节转动20度确立的步长
#define SL_half 66.4868 // 中心舵机1,7,10,16在x轴方向的步长(老步长)———髋关节转动30度确立的步长
#define pace_time 500 // 步伐时间(ms)// 定义机器人的腿部序号
#define A 0
#define B 1
#define C 2
#define D 3
#define E 4
#define F 5/*自己写的函数*/void Hexapod_center_Position03_Init(double X_Tran, double Y_Tran, double R_Angle, uint8_t leg); // 初始化机器人中心坐标系下向量Pi0_Pi3的坐标向量坐标
void Hexapod_Gait_Init(void); // 初始站立状态的位姿和坐标初始化
void Hexapod_Advance_Gait(void); // Hexapod前进
void Hexapod_Back_Gait(void); // Hexapod后退
void Hexapod_LeftTrans_Gait(void); // Hexapod左平移
void Hexapod_RightTrans_Gait(void); // Hexapod右平移
void Hexapod_TurnLeft_Gait(void); // Hexapod左转
void Hexapod_Turnight_Gait(void); // Hexapod右转#endif
相关文章：
【六足机器人】03步态算法
温馨提示：此部分内容需要较强的数学能力，包括但不限于矩阵运算、坐标变换、数学几何。 一、数学知识 1.1 正逆运动学（几何法） 逆运动学解算函数 // 逆运动学-->计算出三个角度 void inverse_caculate(double x, double y, …...
路由VueRouter的基本使用
1.下载VueRouter到当前工程。 vue2：VueRouter3.x Vuex3.x。 vue3：VueRouter4.x Vuex4.x。 在终端使用命令： year add vue-router3.6.5 2.引入。 import VueRouter from vue-router 3,安装注册。 Vue.use(VueRouter) 4…...
Guiding a Diffusion Model with a Bad Version of Itself
Guiding a Diffusion Model with a Bad Version of Itself Abstract1. Introduction2. Background3. Why does CFG improve image quality?Score matching leads to outliers.CFG 消除异常值Discussion 4 Our method Abstract 在图像生成扩散模型中，主要关注的轴心…...
快速上手！低功耗Air724UG模组软件指南：FTP示例
Air724UG模组集成了高性能处理器和丰富的外设接口，支持多种通信协议，包括FTP（文件传输协议）。通过Air724UG模组，开发者可以轻松实现设备的远程文件管理功能。一起接着看下去吧！ 一、简介 FTP（…...
GAMES101 完结篇(笔记和作业）
写在前面 我已经把笔记和作业代码放在了GitHub上，欢迎访问GAMES101笔记及作业 (github.com)，如果对你有帮助，欢迎fork or star 下面我想简单介绍一下这里面的东西 Homework Homework文件夹里有0~8的作业框架，参考的其他大佬的代…...
3D Slicer与MONAI人工智能三维影像处理
如何又快又高效的做三维影像？勾画roi？ 案例1 患者腹腔占位半月余，完善CT增强扫描，使用Slicer 对肿瘤，胰腺，动脉，静脉进行三维重建。重建时间1-5分钟。 案例2 胸部CT平扫，使用 slic…...
NC65客开单据自定义项处理以及自定义项相关介绍（超级详细带图以及代码NC65自定义项大全）
自定义项教程 自定义项和物料辅助属性简介 自定义档案的概念： NC系统中有大量的档案，这些档案中有相当一部分为系统预置的，鉴于用户对系统应用的个性化需求，系统支持用户自定用户自己的档案，并对其进行维护管理&…...
责任链模式的理解和实践
责任链模式（Chain of Responsibility）是行为型设计模式之一，它通过将多个对象连成一条链，并沿着这条链传递请求，直到有对象处理它为止。这个模式的主要目的是将请求的发送者和接收者解耦，使请求沿着处理链传…...
【大模型-向量库】详解向量库管理：连接管理、集合管理、向量管理
在向量数据库（Vector Database）中，向量库管理的概念是非常重要的，因为它涉及到如何高效地存储、索引和检索大规模的向量数据。向量库管理通常包括三个主要方面：连接管理、集合管理和向量管理。以下是对这三者的详细解释…...
MySQL书籍推荐
《高性能MySQL（第4版）》-西尔维亚博特罗斯 系统层次 Mysql性能优化和高可用架构实践 2020 系统基础 MySQL性能调优与架构设计 系统基础 Mysql技术大全 2021 综合 MySQL数据库应用案例教程 综合实战 从入门到项目实践 综合实战 丰富 超值 MySQ…...
常见的数据结构：
数据结构是计算机科学中的一个核心概念，它涉及到组织、管理和存储数据的方式，以便可以有效地访问和修改数据。数据结构的形式有很多，每种结构都有其特定的用途、优势和局限性。以下是一些常见的数据结构： 1. **数组（A…...
快速、高效的数据处理：深入了解 Polars 库
快速、高效的数据处理：深入了解 Polars 库 在数据科学和分析领域，Pandas 一直是 Python 数据处理的标杆。然而，随着数据量的增加，Pandas 在性能上的局限性逐渐显现。为了解决这一问题，越来越多的开发者开始寻找替代方…...
【LINUX】Linux 下打包与部署 Java 程序的全流程指南
文章目录 一、Java 程序打包1. 使用 Maven 打包2. 使用 Gradle 打包 二、运行 JAR 文件1. 前台运行2. 后台运行方法 1：使用 & 符号方法 2：使用 nohup 三、关闭运行中的程序1. 查找程序 PID2. 关闭程序 四、使用 Shell 脚本管理程序1. 创建 Shell 脚本…...
Spark 计算总销量
Spark 计算总销量 题目： 某电商平台存储了所有商品的销售数据，平台希望能够找到销量最好的前 N 个商品。通过分析销售记录，帮助平台决策哪些商品需要更多的推广资源。 假设你得到了一个商品销售记录的文本文件 product_id, product_name,…...
矩阵置零
矩阵置零 给定一个 m x n 的矩阵，如果一个元素为 0 ，则将其所在行和列的所有元素都设为 0 。请使用 原地算法。 示例 1： 输入：matrix [[1,1,1],[1,0,1],[1,1,1]] 输出：[[1,0,1],[0,0,0],[1,0,1]]示例 2ÿ…...
Ai编程cursor + sealos + devBox实现登录以及用户管理增删改查（十三）
一、什么是 Sealos？ Sealos 是一款以 Kubernetes 为内核的云操作系统发行版。它以云原生的方式，抛弃了传统的云计算架构，转向以 Kubernetes 为云内核的新架构，使企业能够像使用个人电脑一样简单地使用云。 二、适用场景 业务运…...
深度解读：生产环境中的日志优化与大数据处理实践20241116
🌟 深度解读：生产环境中的日志优化与大数据处理实践 在现代软件开发中，日志是系统调试与问题排查的重要工具。然而，随着应用的复杂化和数据量的增长，传统日志模块在应对复杂嵌套对象、大数据类型时可能面临性能问题和安…...
docker 搭建gitlab，亲测可用
1、Gitlab镜像 查找Gitlab镜像 docker search gitlab 拉取Gitlab镜像 docker pull gitlab/gitlab-ce:latest 2、启动Gitlab容器 # 启动容器 docker run \-itd \-p 9980:80 \-p 9922:22 \-v /home/gitlab/etc:/etc/gitlab \-v /home/gitlab/log:/var/log/gitlab \-v /ho…...
SpringBoot 分层解耦
从没有分层思想到传统 Web 分层，再到 Spring Boot 分层架构 1. 没有分层思想 在最初的项目开发中，很多开发者并没有明确的分层思想，所有逻辑都堆砌在一个类或一个方法中。这样的开发方式通常会导致以下问题： 代码混乱࿱…...
opencv复习
目录 1.core 1.图像变换 1.1 affine仿射变换 1.2 透视变换 2.四元数（旋转） 2.1 轴角转四元数 2.2 旋转矩阵转四元数 2.3 欧拉角转旋转矩阵 2.4 四元数转旋转矩阵 2.5 四元数用eigen用的比较多 2. imgproc. Image Processing 2.1 bilateralF…...
flask-socketio相关总结
flask-socketio是一个为flask应用程序添加的实时双向通信功能的扩展库，有了这个库，就可以在flask应用中应用websocket协议，帮助flask实现低延迟、双向的客户端、服务端通信。客户端通过任何SocketIO官方库，都能与服务器建立长连接…...
2024-12-03OpenCV图片处理基础
OpenCV图片处理基础 OpenCV的视频教学：https://www.bilibili.com/video/BV14P411D7MH 1-OpenCV摄像头读取 OpenCV使用摄像头读取图片帧，点击S保存当前帧到指定文件夹，点击Q关闭窗口，点击其他按钮打印按钮的值 要实现这个功能&…...
本地部署开源趣味艺术画板Paint Board结合内网穿透跨网络多设备在线绘画
文章目录 前言1.关于Paint Board2.本地部署paint-board3.使用Paint Board4.cpolar内网穿透工具安装5.创建远程连接公网地址6.固定Paint Board公网地址 前言 大家好，是不是每次想要在电脑上画画时，都被那些笨重的专业绘图软件搞得头大如斗呢？…...
iOS、android的app备案超简单的公钥、md5获取方法
很多云商的备案平台，推荐下载一些工具来获取公钥和MD5，但是这些工具的跨平台性不是很好，安装也十分麻烦，安装的时候还需要设置国内源等等。 这里，其实有在线工具可以获取APP的公钥和MD5、SHA1值这些信息的。不需要安装…...
SpringCloud 与 SpringBoot版本对应关系，以及maven,jdk
目录 SpringCloud 与 SpringBoot各版本的对应关系 方式一 Learn 方式二 OverView SpringBoot与JDK、maven 容器等对应关系 SpringCloud 与 SpringBoot各版本的对应关系 SpringCloudSpringBootFinchley2.0.xFinchley.SR1Spring Boot >=2.0.3.RELEASE and <=2.0.9RELEAS…...
23种设计模式之装饰模式
目录 1. 简介2. 代码2.1 ABatterCake （抽象组件）2.2 BatterCake （具体组件）2.3 ADecorator （抽象装饰者）2.4 EggDecorator （具体装饰者）2.5 SausageDecorator（具体装饰者…...
HTMLHTML5革命：构建现代网页的终极指南 - 2. HTMLHTML5H5的区别
HTML&HTML5革命：构建现代网页的终极指南 2. HTML&HTML5&H5的区别 大家好，我是莫离老师 在上一节课，我们了解了HTML的重要性和前端开发的核心概念。 今天，我们将深入探讨 HTML、HTML5 和 H5 的区别，并重点…...
Django之ORM表操作
ORM表操作 1.ORM单表操作 首先想操作表的增删改查，需要先导入这个表,以之前创建的UserInfo表为例,在app下的views.py中导入 from app import modelsdef query(request):new_obj models.UserInfo(id1,name北北,bday2019-09-27,checked1,)new_obj.save()return Htt…...
python下几个淘宝、天猫、京东爬虫实例
以下是使用Python编写的针对淘宝、天猫、京东详情页的爬虫实例。请注意，这些实例仅供参考，实际使用时可能需要根据网站结构的变化进行调整，并且需要遵守各平台的爬虫协议和法律法规。 淘宝详情页爬虫实例 环境准备： Python 3.xSe…...
级联树结构TreeSelect和上级反查
接口返回结构 前端展示格式 前端组件 <template><div ><el-scrollbar height"70vh"><el-tree :data"deptOptions" :props"{ label: label, children: children }" :expand-on-click-node"false":filter-node-me…...