【六足机器人】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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/bicheng/63191.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

Netty面试内容整理-编码实战相关问题

Netty面试内容整理-编码实战相关问题

在 Netty 面试中，编码实战相关的问题会考察你的动手能力，具体包括 Netty 框架的使用、如何设计网络协议、如何处理一些常见的实际开发问题等。以下是一些常见的编码实战相关面试问题及要点： 如何实现一个简单的 Netty Echo 服务器？ 一个 Echo 服务器是 Netty 编码的经典示例…
阅读更多...
文化央企再一次声明

文化央企再一次声明

央企再次声明 中传国华&#xff08;北京&#xff09;科技有限公司&#xff0c;成立于2023年5月29日&#xff0c;原法定代表人曹忠喜&#xff0c;统一社会信用代码&#xff1a;91110117MACL4B9A91&#xff0c;我司中传世纪控股&#xff08;北京&#xff09;有限公司系该司的原股…
阅读更多...
Ubuntu实时流量检测

Ubuntu实时流量检测

nethogs启动 安装nethogs sudo apt install nethogs流量检测 sudo nethogs效果如下&#xff1a; 可以看到收发流量的进程PID&#xff0c;进程目录&#xff0c;发送设备&#xff0c;以及收发速率&#xff1b;但这里有个unkown TCP进程是什么呢? 可以用ps -e 列出操作前后的…
阅读更多...
大数据新视界 -- 大数据大厂之 Hive 临时表与视图：灵活数据处理的技巧（上）（29 / 30）

大数据新视界 -- 大数据大厂之 Hive 临时表与视图：灵活数据处理的技巧（上）（29 / 30）

&#x1f496;&#x1f496;&#x1f496;亲爱的朋友们&#xff0c;热烈欢迎你们来到 青云交的博客&#xff01;能与你们在此邂逅&#xff0c;我满心欢喜&#xff0c;深感无比荣幸。在这个瞬息万变的时代&#xff0c;我们每个人都在苦苦追寻一处能让心灵安然栖息的港湾。而 我的…
阅读更多...
【C语言】二维数组前缀和

【C语言】二维数组前缀和

相信你是最棒哒&#xff01;&#xff01;&#xff01; 文章目录 问题描述 题目代码&#xff1a; 总结 问题描述 输入一个 &#x1d45b;n 行 &#x1d45a;m 列的整数矩阵&#xff0c;再输入 &#x1d45e;q 个询问&#xff0c;每个询问包含四个整数 &#x1d465;1,&#x…
阅读更多...
使用脚本语言实现Lumerical官方案例——闪耀光栅(Blazed grating)（纯代码）（2）

使用脚本语言实现Lumerical官方案例——闪耀光栅(Blazed grating)（纯代码）（2）

接《使用脚本语言实现Lumerical官方案例——闪耀光栅(Blazed grating)（纯代码）（1）》 一、添加分析组 1.1 代码实现 #添加分析组 addanalysisgroup(); set("name", "grating_R"); set("x", 0); set("y", 2.5*um); addanalysisgrou…
阅读更多...
车载测试技术栈

车载测试技术栈

软件测试理论知识 了解软件测试的基本概念、流程和方法&#xff0c;包括测试需求分析、测试用例设计、测试执行、缺陷发现和修复等。 汽车行业知识 熟悉汽车行业的特点和规范&#xff0c;了解汽车的构造和工作原理&#xff0c;包括发动机、底盘、电气系统等。 通信协议知识 …
阅读更多...
61 基于单片机的小车雷达避障及阈值可调

61 基于单片机的小车雷达避障及阈值可调

所有仿真详情导航&#xff1a; PROTEUS专栏说明-CSDN博客 目录 一、主要功能 二、硬件资源 三、主程序编程 四、资源下载 一、主要功能 基于51单片机&#xff0c;采用超声波传感器检测距离&#xff0c;通过LCD1602显示屏显示&#xff0c;三个按键&#xff0c;第一个按键是…
阅读更多...
Linux基本命令---文件权限与用户管理

Linux基本命令---文件权限与用户管理

在Linux系统中&#xff0c;文件权限与用户管理是两个核心概念&#xff0c;它们共同维护着系统的安全性和稳定性。以下是如何在Linux系统中体验文件权限与用户管理的详细步骤&#xff1a; 一、用户管理 创建新用户 使用adduser命令可以创建新用户。例如&#xff0c;创建一个名为…
阅读更多...
Ubuntu——extrepo添加部分外部软件源

Ubuntu——extrepo添加部分外部软件源

extrepo 是一个用于 Ubuntu 和其他基于 Debian 的系统的工具&#xff0c;它的主要作用是简化和管理外部软件源&#xff08;repositories&#xff09;的添加和更新。通过使用 extrepo&#xff0c;用户可以方便地添加、删除和管理第三方软件源&#xff0c;而不需要手动编辑源列表…
阅读更多...
Java集合排序技术详解

Java集合排序技术详解

在Java编程中&#xff0c;对集合进行排序是一项常见的任务。Java提供了多种方式来对集合进行排序&#xff0c;包括使用Collections.sort()、Arrays.sort()、List接口的sort()方法以及Java 8引入的Stream API。本文将详细介绍这些排序技术&#xff0c;并探讨它们的使用场景和性能…
阅读更多...
WEB开发： Node.js路由之由浅入深（一） - 全栈工程师入门

WEB开发： Node.js路由之由浅入深（一） - 全栈工程师入门

作为一个使用Node.js多年的开发者&#xff0c;我已经习惯于用Node.js写一些web应用来为工作服务&#xff0c;因为实现快速、部署简单、自定义强。今天我们一起来学习一个全栈工程师必备技能&#xff1a;web路由。&#xff08;观看此文的前提是默认你已经装好nonde.js了&#xf…
阅读更多...
【机器学习算法】——逻辑回归

【机器学习算法】——逻辑回归

目录 逻辑回归理解损失函数代码练习1. 房屋价格与面积的关系2.基于学生特征的录取概率预测 逻辑回归理解 逻辑回归是用来二分类的&#xff01; 是在线性回归模型之后加了一个激活函数&#xff08;Sigmoid)将预测值归一化到【0~1】之间&#xff0c;变成概率值。 一般计算其中一…
阅读更多...
一个有意思pytorch的简单应用小实验

一个有意思pytorch的简单应用小实验

通过一个简单的脚本&#xff0c;来学习pytorch的基本应用&#xff0c;比如&#xff1a;前向传播、反向传播、学习率以及预测、模型的基本原理和套路。 得到结果。。。保存模型。。。输入参数。。。预测。。。像不像&#xff1f;。。。像多少&#xff1f;。。。 设计目标&#x…
阅读更多...
使用lumerical脚本语言创建定向耦合器并进行数据分析（纯代码实现）

使用lumerical脚本语言创建定向耦合器并进行数据分析（纯代码实现）

本文使用lumerical脚本语言创建定向耦合器波导、计算定向耦合器的偶数和奇数模式、分析定向耦合器的波长依赖性、分析定向耦合器的间隙依赖性（代码均有注释详解）。 一、绘制定向耦合器波导 1.1 代码实现 # 这段代码主要实现了绘制定向耦合器波导几何结构的功能。通过定义各种…
阅读更多...
Linux 35.6 + JetPack v5.1.4之RTP实时视频Python框架

Linux 35.6 + JetPack v5.1.4之RTP实时视频Python框架

Linux 35.6 JetPack v5.1.4之RTP实时视频Python框架 1. 源由2. 思路3. 方法论3.1 扩展思考 - 慎谋而后定3.2 扩展思考 - 拒绝拖延或犹豫3.3 扩展思考 - 哲学思考3.4 逻辑实操 - 方法论 4 准备5. 分析5.1 gst-launch-1.05.1.1 xvimagesink5.1.2 nv3dsink5.1.3 nv3dsink sync05…
阅读更多...
企业风险投资、融资事件数据（1921-2024)

企业风险投资、融资事件数据（1921-2024)

数据包括历年上市与非上市企业的风险投资融资数据等数据&#xff0c;包括融资时间、被投企业、投资方、退出方等数据&#xff0c;希望对大家的研究有所帮助 一、数据介绍 数据名称&#xff1a;企业风险投资、融资事件 数据范围&#xff1a;上市与非上市企业 数据年份&#x…
阅读更多...
移远5G模块移植

移远5G模块移植

移远5G模块移植 1.NCM网卡配置2.拨号工具编译3.程序运行 1.NCM网卡配置 1.1、内核配置 打开内核配置界面&#xff0c;并找到USB Network Adapters进行NCM网卡配置 > Device Drivers > Network device support > USB Network Adapters 1.2、驱动修改 打开内核源码钟的…
阅读更多...
基于单片机的智能农田灌溉节水系统设计及应用

基于单片机的智能农田灌溉节水系统设计及应用

摘 要 &#xff1a; 针对传统的灌溉方法浪费水资源节水系统设计。该系统从节水角度出发&#xff0c;对传感器和主电路进行了设计&#xff0c;主要采集灌溉地的湿度与温度数据&#xff0c;根据测量土壤中的温度与湿度作为主要参数&#xff0c;对农田灌溉节水系统进行实时控制&am…
阅读更多...
煤矿 35kV 变电站 3 套巡检机器人 “上岗”，力破供电瓶颈

煤矿 35kV 变电站 3 套巡检机器人 “上岗”，力破供电瓶颈

近日&#xff0c;杭州旗晟智能科技与甘肃某变电站配电室的三套智能巡检机器人线下测试顺利完成&#xff0c;并成功交付使用&#xff0c;这为电力运维工作注入了全新的活力与强大的技术支撑。 一、项目背景 甘肃某变电站总建筑面积1098平方米的变电站集变电、配电、监控等多功能…
阅读更多...
最新文章

Copyright @ 2022~2023