【xr806开发板试用】XR806联调stm32控制并联四足马术机器人单腿运动学逆解与整体步态规划

一、项目介绍

使用RX806串口协议与大疆A板的stm32通讯，实现并联四足机器人的单腿运动学逆解与整体步态规划。

                                实物图

二、涉及相关算法

1.单腿运动学逆解

控制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];

2.步态规划（目前主要采用wark步态）

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

3.无刷电机角度速度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;
}

三、代码实现

XR806：
初始化串口配置：

static int uart_init(void)
{
 UART_InitParam param;

 param.baudRate = 115200;
 param.dataBits = UART_DATA_BITS_8;
 param.stopBits = UART_STOP_BITS_1;
 param.parity = UART_PARITY_NONE;
 param.isAutoHwFlowCtrl = 0;

 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;

 return 0;
}

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

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

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

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
     if(huart->Instance == USART3)
 {
  HAL_UART_Receive_IT(&huart3,&RxBuffer_control,1);

 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;
 }
}

四、实物展示

1.原地

https://www.youku.com/video/X...

2.全方位运动

https://www.youku.com/video/X...