【六足机器人】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…...
Chapter03-Authentication vulnerabilities
文章目录 1. 身份验证简介1.1 What is authentication1.2 difference between authentication and authorization1.3 身份验证机制失效的原因1.4 身份验证机制失效的影响 2. 基于登录功能的漏洞2.1 密码爆破2.2 用户名枚举2.3 有缺陷的暴力破解防护2.3.1 如果用户登录尝试失败次…...
阿里云ACP云计算备考笔记 (5)——弹性伸缩
目录 第一章 概述 第二章 弹性伸缩简介 1、弹性伸缩 2、垂直伸缩 3、优势 4、应用场景 ① 无规律的业务量波动 ② 有规律的业务量波动 ③ 无明显业务量波动 ④ 混合型业务 ⑤ 消息通知 ⑥ 生命周期挂钩 ⑦ 自定义方式 ⑧ 滚的升级 5、使用限制 第三章 主要定义 …...
线程与协程
1. 线程与协程 1.1. “函数调用级别”的切换、上下文切换 1. 函数调用级别的切换 “函数调用级别的切换”是指：像函数调用/返回一样轻量地完成任务切换。 举例说明： 当你在程序中写一个函数调用： funcA() 然后 funcA 执行完后返回&…...
[ICLR 2022]How Much Can CLIP Benefit Vision-and-Language Tasks?
论文网址：pdf 英文是纯手打的！论文原文的summarizing and paraphrasing。可能会出现难以避免的拼写错误和语法错误，若有发现欢迎评论指正！文章偏向于笔记，谨慎食用 目录 1. 心得 2. 论文逐段精读 2.1. Abstract 2…...
12.找到字符串中所有字母异位词
🧠 题目解析 题目描述： 给定两个字符串 s 和 p，找出 s 中所有 p 的字母异位词的起始索引。 返回的答案以数组形式表示。 字母异位词定义： 若两个字符串包含的字符种类和出现次数完全相同，顺序无所谓，则互为…...
AspectJ 在 Android 中的完整使用指南
一、环境配置（Gradle 7.0 适配） 1. 项目级 build.gradle // 注意：沪江插件已停更，推荐官方兼容方案 buildscript {dependencies {classpath org.aspectj:aspectjtools:1.9.9.1 // AspectJ 工具} } 2. 模块级 build.gradle plu…...
【Redis】笔记｜第8节｜大厂高并发缓存架构实战与优化
缓存架构 代码结构 代码详情 功能点： 多级缓存，先查本地缓存，再查Redis，最后才查数据库热点数据重建逻辑使用分布式锁，二次查询更新缓存采用读写锁提升性能采用Redis的发布订阅机制通知所有实例更新本地缓存适用读多…...
客户案例 | 短视频点播企业海外视频加速与成本优化：MediaPackage+Cloudfront 技术重构实践
01技术背景与业务挑战 某短视频点播企业深耕国内用户市场，但其后台应用系统部署于东南亚印尼 IDC 机房。 随着业务规模扩大，传统架构已较难满足当前企业发展的需求，企业面临着三重挑战： ① 业务：国内用户访问海外服…...
GraphRAG优化新思路-开源的ROGRAG框架
目前的如微软开源的GraphRAG的工作流程都较为复杂，难以孤立地评估各个组件的贡献，传统的检索方法在处理复杂推理任务时可能不够有效，特别是在需要理解实体间关系或多跳知识的情况下。先说结论，看完后感觉这个框架性能上不会比Grap…...
[QMT量化交易小白入门]-六十二、ETF轮动中简单的评分算法如何获取历史年化收益32.7%
本专栏主要是介绍QMT的基础用法，常见函数，写策略的方法，也会分享一些量化交易的思路，大概会写100篇左右。 QMT的相关资料较少，在使用过程中不断的摸索，遇到了一些问题，记录下来和大家一起沟通，共同进步。 文章目录 相关阅读1. 策略概述2. 趋势评分模块3 代码解析4 木头…...