6自由度双足机器人行走功能的实现

1. 运动功能说明

      6自由度双足机器人样机是一款拥有6自由度的串联型双足，相当于人形机器人的下半身。它可以实现抬腿、迈步、蹲下和起立、行走、翻跟头等等功能。

2. 结构说明

      6自由度双足机器人样机的每条腿由3个 舵机关节模组 构成。足部结构一般有两种设计方案，一种叫“狭窄足印”，比较常规，如图所示：

      还有一种叫“交叉足印”，也叫“工”形脚或“工”字脚。这种设计方案可以保证机器人行走时重心始终落在支撑零件上，能够降低控制难度，如图所示：  

3. 运动功能实现

      本文先以交叉足印样机为例进行说明。本样机的运动功能主要是依靠舵机的摆动配合实现的，我们可以参考人体结构，把两条腿上的三个舵机按从上到下的顺序，对应理解为只能前后摆动的髋关节、膝关节、踝关节（真实的生物关节类似于球铰，更加复杂）。我们需要参考人行走时三个关节的摆动角度关系，为双足机器人确定并调出对应的一系列舵机角度。

      为了比较轻松地完成前进、后退动作，需要让腿抬得高一些，以避免迈步时工形脚干涉。

3.1 电子硬件

在这个实验中，采用了以下硬件，请大家参考：

Basra主控板（兼容Arduino Uno）、Bigfish2.1扩展板、7.4V锂电池

由于舵机比较多，为了完成控制，我们需要约定一下样机的前、后方向，并对舵机进行编号：

3.2 运动控制

上位机：Controller 1.0

下位机编程环境：Arduino 1.8.19

（1）初始位置的设定：使用Controller上位机对机器人进行调试，调试目标为使机器人保持直立状态，并且保持右脚的支撑臂在前。

①将Controller下位机程序servo_bigfish.ino直接下载到主控板。这段代码供Controller上位机与主控板通信，并允许调试舵机。代码如下：

/*------------------------------------------------------------------------------------   版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.            Distributed under MIT license.See file LICENSE for detail or copy at            https://opensource.org/licenses/MIT            by 机器谱 2022-10-20 https://www.robotway.com/   ------------------------------   实验功能： 6自由度双足行走   -----------------------------------------------------   实验接线（从行走时朝前的方向看）： 左侧髋：D4；右侧髋：D3； 左侧膝：D7；右侧膝：D5； 左侧踝：D11；右侧踝：D6 ------------------------------------------------------------------------------------*/ /* * Bigfish扩展板舵机口; 4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19 * 使用软件调节舵机时请拖拽对应序号的控制块 */ #include <Servo.h> #define ANGLE_VALUE_MIN 0 #define ANGLE_VALUE_MAX 180 #define PWM_VALUE_MIN 500 #define PWM_VALUE_MAX 2500 #define SERVO_NUM 12 Servo myServo[SERVO_NUM]; int data_array[2] = {0,0};   //servo_pin: data_array[0], servo_value: data_array[1]; int servo_port[SERVO_NUM] = {4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19}; int servo_value[SERVO_NUM] = {}; String data = ""; boolean dataComplete = false; void setup() {   Serial.begin(9600); } void loop() {   while(Serial.available())   {     int B_flag, P_flag, T_flag;     data = Serial.readStringUntil('\n');     data.trim();     for(int i=0;i<data.length();i++)     {       //Serial.println(data[i]);       switch(data[i])       {         case '#':           B_flag = i;           break;         case 'P':         {           String pin = "";           P_flag = i;           for(int i=B_flag+1;i<P_flag;i++)           {             pin+=data[i];           }           data_array[0] = pin.toInt();         }         break;         case 'T':         {           String angle = "";           T_flag = i;           for(int i=P_flag+1;i<T_flag;i++)           {             angle += data[i];           }           data_array[1] = angle.toInt();           servo_value[pin2index(data_array[0])] = data_array[1];         }         break;         default: break;       }          }         /*     Serial.println(B_flag);     Serial.println(P_flag);     Serial.println(T_flag);         for(int i=0;i<2;i++)     {       Serial.println(data_array[i]);     }     */         dataComplete = true;   }   if(dataComplete)   {     for(int i=0;i<SERVO_NUM;i++)     {       ServoGo(i, servo_value[i]);       /*********************************串口查看输出***********************************/ //      Serial.print(servo_value[i]); //      Serial.print(" ");     } //    Serial.println();       /*********************************++++++++++++***********************************/     dataComplete = false;   } } void ServoStart(int which){   if(!myServo[which].attached() && (servo_value[which] != 0))myServo[which].attach(servo_port[which]);       else return;   pinMode(servo_port[which], OUTPUT); } void ServoStop(int which){   myServo[which].detach();   digitalWrite(servo_port[which],LOW); } void ServoGo(int which , int where){   ServoStart(which);   if(where >= ANGLE_VALUE_MIN && where <= ANGLE_VALUE_MAX)   {     myServo[which].write(where);   }   else if(where >= PWM_VALUE_MIN && where <= PWM_VALUE_MAX)   {     myServo[which].writeMicroseconds(where);   } } int pin2index(int _pin){   int index;   switch(_pin)   {     case 4: index = 0; break;     case 7: index = 1; break;     case 11: index = 2; break;     case 3: index = 3; break;     case 8: index = 4; break;     case 12: index = 5; break;     case 14: index = 6; break;     case 15: index = 7; break;     case 16: index = 8; break;     case 17: index = 9; break;     case 18: index = 10; break;     case 19: index = 11; break;   }   return index; }

下载完成后，保持主控板和电脑的USB连接，以便利用上位机进行调试。

②双击打开Controller 1.0b.exe：

③界面左上角选择：设置-面板设置，弹出需要显示的调试块，可通过勾选隐藏不需要调试的舵机块：联机-选择主控板对应端口号以及波特率。

④拖动进度条，可以观察相应的舵机角度转动，直到样机姿态达到我们的预想目标。然后勾选左下角添加-转化，获得舵机调试的数组：

⑤该数组可直接复制到后面的行走程序中“各个舵机的初始位置”部分进行使用。

（2）编写并下载行走程序的代码（Biped_Robot.ino）到主控板：

/*------------------------------------------------------------------------------------   版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.            Distributed under MIT license.See file LICENSE for detail or copy at            https://opensource.org/licenses/MIT            by 机器谱 2022-10-20 https://www.robotway.com/   ------------------------------   实验功能： 6自由度双足行走   -----------------------------------------------------   实验接线（从行走时朝前的方向看）： 左侧髋：D4；右侧髋：D3； 左侧膝：D7；右侧膝：D5； 左侧踝：D11；右侧踝：D6 ------------------------------------------------------------------------------------*/ #include <Servo.h> Servo myServo[6]; int servo_port[6]={4,7,11,3,8,12}; float servo_value[6] = {65, 60, 105, 60, 100, 80};//各个舵机的初始位置 void setup() { for(int i=0; i<5; i++){     ServoGo(i, (int)servo_value[i]); } delay(2000); } void loop() { left_go(); right_go(); } void left_go() { for(int i=0; i<10; i++){ servo_value[4] -= 10; servo_value[5] -= 6; for(int j=0; j<6; j++){     ServoGo(j, (int)servo_value[j]); }     delay(25); } for(int i=0; i<10; i++){ servo_value[0] += 2.5; servo_value[2] -= 1; servo_value[3] += 2.5; servo_value[4] += 10; servo_value[5] += 4.5; for(int j=0; j<6; j++){     ServoGo(j, (int)servo_value[j]); }     delay(25); } } void right_go() { for(int i=0; i<10; i++){ servo_value[1] += 10; servo_value[2] += 7.5; servo_value[5] += 0.5; for(int j=0; j<6; j++){     ServoGo(j, (int)servo_value[j]); }     delay(25); } for(int i=0; i<10; i++){ servo_value[0] -= 2.5; servo_value[1] -= 10; servo_value[2] -= 6.5; servo_value[3] -= 2.5; servo_value[5] += 1; for(int j=0; j<6; j++){     ServoGo(j, (int)servo_value[j]); }     delay(25); } } void ServoStart(int which) {   if(!myServo[which].attached())myServo[which].attach(servo_port[which]);   pinMode(servo_port[which], OUTPUT); } void ServoStop(int which) {   myServo[which].detach();   digitalWrite(servo_port[which],LOW); } void ServoGo(int which , int where) {   if(where!=200)   {     if(where==201) ServoStop(which);     else     {       ServoStart(which);       myServo[which].write(where);     }   }   } void ServoMove(int which, int start, int finish, int t) { int a; if((start-finish)>0) a=-1; else a=1; for(int i=0;i<abs(start-finish);i++)   {ServoGo(which,start+i*a);delay(t/(abs(start-finish)));} }

      将上个步骤记录的角度数据依次填入相应舵机的初始值，比如：float servo_value[6] = {90, 44, 46, 82, 115, 100}; //各个舵机的初始位置。为了减少重力的干扰，一般先把机器人调整成直立状态采值。采值后，需要注意的是舵机0、3的初始值应在上个步骤结果值基础上减去10，使其进入行走姿态。修改完成后便可以直接将程序下载到主控板，然后将主控板、电池安装到机器人最上面的平台（应以重心较低为标准），然后连接好舵机线，顺利的话现在机器人应该就可以正常行走了。但是由于舵机的内部结构问题，相应的角度输出可能会存在误差，这样的表现便是机器人的支撑臂会出现“打架”现象，这个问题的解决方法是修改程序中对应的参数调节系数，例如：servo_value[4] -= 11；

      可以修改参数“11”的大小，需要注意的是程序的整个循环特定的位置对应特定的值，因此在修改的时候程序后面相应舵机的输出系数也需要相应的改变，由此达到一个完满的循环。

资料内容：6自由度双足-行走-例程源代码、6自由度双足-样机3D文件、Controller1.0b资料包【资料下载详见 6自由度双足-行走】