使用XR806联调控制四足马术机器人

1项目介绍

四足马术机器人

本项目使用XR806串口协议与大疆A板的stm32通讯，实现并联四足机器人的单腿运动学逆解与整体步态规划，本文将讲解项目所涉及的算法以及代码实现步骤。

四足马术机器人实物

2单腿运动学逆解

相关算法

控制2个无刷电机（红色箭头各代表一个电机控制）并联成单足，经过角度闭环解算出足端轨迹，由足端做摆线轨迹（下图中绿色部分）形成类似于动物猫狗等单腿的运动

%摆线方程（matlab）
sigma=2*pi*t/(Ts);
xep=(xf-xs)*((sigma-sin(sigma))/(2*pi))+xs;
zep=h*(1-cos(sigma))/2+zs;
x=[x,xep];
z=[z,zep];

3整体步态规划

相关算法

Walk步态是一种静态步态，即在运动过程中始终有三条腿处于支撑相，至多只有一条腿处于摆动相，四足动物在walk步态中四条腿最常见的轮换顺序为1→3→4→2→1。

4无刷电机角度速度

pid闭环

以P比例、I积分、D微分通过增量式PID使无刷电机能稳定的控制速度，角度使用pd控制函数如下

int Balance(float Angle,float Gyro,int Middle,float Balance_Kp,float Balance_Kd)
{ 
  float Angle_bias,Gyro_bias;
 int balance;
 Angle_bias=Middle-Angle;            
 Gyro_bias=0-Gyro;               
 balance=Balance_Kp*Angle_bias+Gyro_bias*Balance_Kd; 
 return balance;
}

5关键功能实现

代码展示

XR806初始化串口配置：

 if(HAL_UART_Init(UARTID, &param) != HAL_OK)
 return -1;
 /*使能DMA*/
 if (HAL_UART_EnableTxDMA(UARTID) != HAL_OK)
 return -2;
 if (HAL_UART_EnableRxDMA(UARTID) != HAL_OK)
 return -3;

在main函数中while循环用下列函数发送对应数据：

HAL_UART_Transmit_DMA(UARTID, (uint8_t *)buffer,sizeof(buffer));

串口3接收回调，执行难对应的前进后退：

 switch(RxBuffer_control)
 {
 case 'W': Motor_Control( 1, 1, 1, 1); break;
 case 'S': Motor_Control(-1, -1, -1, -1); break;
 case 'A': Motor_Control(-1, -1, 1, 1); break;
 case 'D': Motor_Control( 1, 1, -1, -1); break;
 case 'P': Motor_Control( 0, 0, 0, 0); break;
 }