仿真emulation unity3D 仿真机器人

一、概述

1.研究意义

随着人类对在复杂环境中既具备高移动能力，又具备可靠性，且易于扩展的移动平台日益迫切的需求，步行机器人作为一种拥有全方位移动能力的移动运载平台，具有非常广阔的应用前景。从仿生学角度来讲，多足机器人分为仿人、仿四足哺乳动物、仿昆虫行走机器人。在众多步行机器人中，模仿昆虫以及其他节肢动物的肢体结构和运动控制策略而创造出的六足机器人具有代表性。

　　一般来说，仿人机器人重点研究人机交互、语音识别、图像识别技术，行走重点为动态平衡；仿四足机器人多为中小型机器人，兼顾负重能力与灵活性，行走方面既有静态步行稳定性也有动态运动研究；仿昆虫机器人从尺寸上分为重载大型六足机器人和小型六足机器人，重点研究仿生腿的优化设计、行走稳定、足端精确控制、足端脚力分配、步态规划等。

　　六足机器人较二足、四足机器人，具有控制结构相对简单、行走平稳、肢体荣誉等特点，故而拥有较好的机动性，最容易实现稳定行走，可适应于复杂地形，多用于军事勘察、矿山开采、核能工业、星球探测、消防及营救、建筑业等领域。

 2.昆虫的运动原理。

　　足是昆虫的运动器官。

　　昆虫有3对足，在前胸、中胸和后胸各有一对，相应地称为前足、中足和后足。

　　每个足由基节、转节、腿节、胫节、跗节和前跗节几部分组成。基节是足最基部的一节，多粗短。转节常与腿节紧密相连而不活动。腿节是最长最粗的一节。第四节叫胫节，一般比较细长，长着成排的刺。第五节叫跗节，一般由2-5个亚节组成﹔为的是便于行走。在最末节的端部还长着两个又硬又尖的爪，可以用它们来抓住物体。 行走是以三条腿为一组进行的，即一侧的前、后足与另一侧的中足为一组。这样就形成了一个三角形支架结构，当这三条腿放在地面并向后蹬时，另外三条腿即抬起向前准备替换。 前足用爪固定物体后拉动虫体向前，中足用来支持并举起所属一侧的身体，后足则推动虫体前进，同时使虫体转向。 这种行走方式使昆虫可以随时随地停息下来，因为重心总是落在三角支架之内。并不是所有成虫都用六条腿来行走，有些昆虫由于前足发生了特化，有了其他功用或退化，行走就主要靠中、后足来完成了。 

图1  昆虫运动原理腿部示意图

3.腿部机构类型

　一般来说，腿的构造形式可分为昆虫类和哺乳动物类两种不同形式。昆虫类生物其腿的数目较多， 一般在四足以上 ;其腿分布于身体的两侧， 身体重心低，稳定性好，且运动灵活，但过低的重心不利于昆虫的越障能力 ;喃乳动物的行走腿则通常为两足或四足，且腿多分布于身体下方，重心高，便于快速奔跑和越障，但在转向等需要灵活性的场合不如昆虫类有优势。

　　通常腿部结构可以分为转动关节和平动关节。转动关节即通过电机转动，带动腿部直接旋转，关节转角即为电机转角；平动关节即通过执行缸伸缩，从而带动腿部旋转，关节转角需要通过平动距离进行转化。

 

4.六足机器人步态

　　通俗地说，步态是行走系统抬腿和放腿的顺序。

（1）相关定义

　　a.步态：是指机器人的每条腿按一定的顺序和轨迹的运动过程，正是因为这一运动过程实现了机器人的步行运动

　　b.步态周期：：步态周期是指多足机器人完成一个步态所需的时间，也就是所有腿轮番完成一次“提起 -摆动-放下”的动作共花费的时间，在此过程中机器人机体也完成过渡过程

　　c.占地系数：占地系数是指每条腿接触地面的时间和整个步态周期的比值。

　　　当占地系数等于 0.5时，机器人是用两组腿交替摆动，这种步态称为小跑步态；

　　　当占地系数小于 0.5时，机器人任何瞬间只有不足三条腿支撑地面，称为跳跃步态，

　　　当占地系数大于 0.5时，机器人轮番有三条腿以上支撑地面，这种步态俗称慢爬行步态

　　d.步幅：机器人的重心在一个步态周期中的平移为步幅。本系统中步幅为6cm。系统步幅参数也是可调的。
　　e.静态稳定性：步态的生成策略则取决于机器人的步行稳定性，即在步态生成时必须进行稳定性分析。对于多足机器人，在任何时候都要有足够多的腿立足于地面支撑机器人机体，才能确保它静态稳定地步行。通常，至少需要三条这样的腿，并且由这三条腿的立足点构成的三角形必须包围机器人的重心垂直投影，机器人步行时，虽然这个三角形区域是不停变化的，但只要机器人重心投影始终在这个交替变化的区域内，则机器人的步行就是稳定的.

（2）常见的步态

　　a.三角步态
　　三角步态也称交替三角步态，是“六足纲”昆虫最常使用的一种步态，也被誉为最快速有效的静态稳定步态。大部分六足机器人都是从仿生学的角度出发使用这一步态。昆虫三角步态的移动模式较简单，非常适合步行架构的机器人的直线行走，行进速度也比较快。
　　b.跟导步态
　　对于六足机器人来说，跟导步态的重点是选择前两足下一步的落点，而一对中足和一对后足的下一步落点由当前前足和中足的立足点决定。跟导步态每次只需要选择前两足的立足点，因而具有控制简单，稳定性较好，越沟能力强等特点，所以特别适合多足步行机在不平地面行走时采用。

　　c.交替步态
　　单腿交替行走步态，也被称为五角步态。在交替步态中，各腿的运动可分为抬升和前进两个部分。当某腿的相邻各腿均已触地时，该腿开始运动，并给其相邻各腿发出信号。同样，在该腿触地时，也会给相邻各腿发出触地信号。这样，一旦整个六足系统进入行走状态，这种顺次的步态运行状态就可以一直维持下去。由于各腿等待其相邻腿触地的时间取决于其相邻腿的动作及其触地位置，因而，对于崎岖不平的地面而言，这种步态本身是不可预测的。然而，对于理想的平整地面而言，各腿的运动周期应该是一致的，故而此时的交替步态实质上等同于三角步态。

二、设计内容及实现目标

　　1.步态模拟：利用生物步态模拟“螃蟹步”和“节肢昆虫”两种步态

　　2.动作设计：前进、后退、左转、右转等动作

　　3.无线视频及实时传输、GPS模块定位

　　4.红外避障、循迹

　　5.平台搭载功能模块，如机械臂实现夹取物等功能。

三、机械结构设计

　设计内容：根据仿生学原理，建立机体和腿部结构，对机身底板的外形及腿部安放角度、单腿节段长度比例进行设计计算 ，并设计确定机器人足端形状和结构；对运动过程中的极限状态进行负载计算，校核舵机的额定转矩是否能够满足工况；

　　六足机器人行走方式与轮式、履带式车辆行进方式有明显区别，其行走轨迹为非连续的离散点，而轮式和履带式车辆行走轨迹为连续接触面。六足机器人落足点的离散性为六足机器人在不规则地形行走提供了卓越的越障能力，每个落足点之间的跨越方式和运动轨迹的变化都为六足机器人提供了不同的步态，不同的步态决定了不同的受力形式，同样制约着机械结构的设计原则。反之，不同的机械结构也决定了机器人整机应对不同工况的能力。作为六足机器人的执行机构单腿刚度对整机性能影响较大，单腿受力后足端位移变形决定着机器人足端能否完成预定的轨迹。 下面为详细设计。

1. 机器人底板布局设计

　对于摆动腿的仿昆虫式六足机器人，六条腿的布置方位对机身稳定性和运动性能有很大影响，通常有三种布局：矩形、椭圆形和圆形布局。建立图示三种布局，当每条腿摆动相同角度，即腿部扫过的扇形面积一致的情况下：

　　（1）腿部在行进时的干涉情况：矩形布局的腿部摆动范围重合度最高，圆形布局的重合度最小，即在保证相邻腿部不干涉的情况下，圆形布局腿部摆动空间最大，椭圆布局次之，矩形布局最差；

　　（2）直线行走的有效行进距离：设定单腿迈步可以完成的最大迈步距离为λ，标出每种布局的迈步距离，可见圆型布局的迈步距离最小，椭圆较大，矩形最大。

　　综合考虑上述因素，椭圆形布局的灵活性最佳，拟用椭圆布局，且自然界中六足昆虫腿部分布也均为椭圆形。

图2 三种底板布局示意图

　　六足机器人的稳定性由重心投影位置和支撑平面关系来确定，即相对固定的机器人重心位置不仅要保证时刻在变化的支撑平面内，更要预留出一定量的安全距离d。建立椭圆形机架连杆模型俯视图，其中 L 为单腿俯视投影长度，a 为机架前端和末端宽度，b 为机架中间宽度的一半，c 为机架长度的一半。

图3 稳定性分析示意图

　　稳定裕度为：d=(b+L)siny

　　其中，中间变量：

　　　　y=arctan(c/((2b-a)/2)+2L+a)

根据计算结果可见，当设定c值和 L为固定长度时，a 值越小，b 值越大可使整机在一定范围内稳定裕度 d 越高,当 a 充分小，b 充分大时整机重心将与支撑三角形最左侧边距离最近，但此种比例极不协调，通常设计时不做考虑。可见实际上圆形布局下的稳定裕度最大，考虑到机器人直线行走性能优选椭圆形布局作为本文的整机布局方案。

　底板最终设计的三维模型如下：

 

图4  三维模型和二维尺寸图

2. 机器人腿部结构设计

主流腿部结构有两种，一种是哺乳动物单腿结构，即直立腿结构，运动形式类似于在竖直平面内做钟摆运动；一种是仿昆虫单腿结构，即摆动腿结构，如图所示。

图5  两种腿部结构

在本设计中，考虑到大腿承受较大弯矩，且大腿宽度受限，因而将大腿上下腹板设计为板材形式，大腿侧向和小腿均采用连杆形式，关节处连接舵机；另外在腿板上开设减重孔，且大腿和小腿均采用矩形截面，抗弯能力强，结构加工性好，且承载力强，方便搭载功能模块。小腿采用变截面设计，无多余负重，质量小，有效减少了腿部结构末端的惯量，控制更容易，节约了驱动能量。

整体来说，腿部结构方案主要由基节连杆、大腿连杆、小腿连杆组成，大腿连杆根部与基节连杆连接舵机旋转端，大腿连杆末端与小腿连杆同样的方式连接，足端设计为
弧形结构，足端底部安装有较厚的橡胶垫，用于足端落地缓冲。

　　关于尺寸：基节连杆的存在实际上是使足端运动范围远离机器人本体，虽然足端运动扇形区域更广，但距离机体太远的足端扇形触地面会导致前后腿的接触地面干涉大，因而基节连杆加长并无明显优势。在实际布置中，基节连杆越短越有利于提高机器人的通过性，缩小宽度。因而基节的长度仅能满足舵机的安装即可。大腿与小腿的长度尺寸暂定比例为1：1.5，外形根据仿生学按照昆虫腿型进行设计。

　　三维结构如图所示。

 

图6 三维结构图

3.机器人足端结构

在足端设计方面，足式机器人足端结构可分为两类：一类是有足端被动关节的机构。另一类是固定式足端，即足端无被动关节而仅有缓冲材料组成的固定形状的足端。无被动关节的足端在触地和运动时有极小的惯量，能量消耗低，活动更为灵活。但足端在触地过程中随着机体结构的移动，小腿相对于地面的夹角发生变化，因而在运动控制上如果仅仅控制足端上的某一点的轨迹会导致足端与地面打滑，在大型足式机器人结构中这种打滑会产生极大的作用力。此种无被动关节的足端多应用在小型足式机器人上，且机器人多可跑步前进或以小碎步前进，通过这种行进方式可避免固定式足端的弊端。

　　相比之下，有被动关节的足端在支撑移动的过程中，小腿与地面的夹角虽有变化，但可通过被动关节抵消这一影响。此外，在控制中只需控制此被动关节中心的轨迹即可。在大型足式机器人中，为节约能量消耗，通常单步步长都较长，足端在一次触地过程中相对机体运动较远，被动关节可抵消这一影响，因而在大型足式机器人足端设计中均采用被动关节。而对于小型六足机器人，六条腿的末端只要大致运动形式正确机器人便可行进，腿末端与支撑面产生滑动甚至个别腿末端未与支撑面接触也不影响机器人工作，因而小型六足机器人在设计中各个驱动关节只要按照预定转角运行即可，对地面的高低起伏可不做具体调整或进行姿态运算，也不必要设计为被动关节式，故本设计中采用固定式足端。下图为两种固定式足端结构的机器人。

图7 六足机器人

 

 

四、运动学分析

　　进行某条摆动腿的运动学分析时，运动学正解求解就是已知机身的位置姿态 和三个转动关节的大小，求解足端末点的位置坐标。用 D-H 方法在某条腿建立坐标系

图8 运动学分析

　相邻关节的 D-H 变换矩阵为：

 

D-H参数表： 

　　得变换矩阵：

　　运动学逆解的求解就是已知机体的位置姿态和某条腿足端点的位置坐标，求 解这个腿上各个旋转关节的大小。采用左乘逆变换矩阵的方法，将关节变量分离 出来，从而求得关节转角

　　前面求解运动学正解时，得到了腿部足端点与各个关节角的关系式。对两边 进行求导，就得到了足端点的速度与关节角速度的关系

　　六足机器人每条腿有 3 个关节，其雅可比矩阵是 6*3 阶矩阵，前 3 行代表足 端线速度的传送比，后 3 行代表足端角速度的传送比。对于六足机器人的足端， 只需要求出其线速度的传送比雅克比矩阵

 

 

五、控制系统搭建

1.Arduino控制板

图9 控制板实物图

 

2. 24路舵机控制板

 

 

图10 24路舵机控制板实物图片

（1）接线端说明

图11 控制板接线示意图

　　拔掉上图所示跳线帽是6.4V低压报警；插上跳线帽是5V低压报警。低压报警的时候，电压越低，滴滴声的频率越高，此时请关闭电源开关，给锂电池充电。跳线帽只会改变蜂鸣器产生低压报警声的电压阈值，不会对控制板有其他功能上的影响。 简单的理解就是接降压芯片就要插上跳线帽，不接降压芯片就不接跳线帽。如果使用的是LDX-218舵机或者LDX-227舵机，那么将下图所示的跳线帽拔掉；控制板正极可以直接接上7.4V锂电池供电。

图12 上位机界面

（2）上位机软件及操作教程见网盘：

　　链接:https://pan.baidu.com/s/1p4YcNMlsagYNA9zjLTiWbg    提取码:r63r

3. OpenMV

(1)实物图

图13 Openmv 模块图

（2）使用教程

　　查看网址：https://singtown.com/openmv/

 

 

 

4. 超声波

 

图14 超声波模块

 

5.语音识别

　　语音识别模块图：

图16 语音识别模块

 

 图17 原理图

 6.程序

（1）OpenmV程序

30 2019

import sensor, image, time,lcd, math

sensor.reset()


sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)


# Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max)
# The below thresholds track in general red/green/blue things. You may wish to tune them...
thresholds = [(32, 78, 27, 5, 57, -15), # generic_red_thresholds
              (30, 100, -64, -8, -32, 32), # generic_green_thresholds
              (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds

lcd.init() # Initialize the lcd screen.

sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking

face_cascade = image.HaarCascade("frontalface", stages=25)
eyes_cascade = image.HaarCascade("eye", stages=24)
print(face_cascade, eyes_cascade)

clock = time.clock()

while(True):
    clock.tick()
    img = sensor.snapshot()
    print(clock.fps())
    lcd.display(sensor.snapshot()) # Take a picture and display the image.

    objects = img.find_features(face_cascade, threshold=0.5, scale_factor=1.5)

# Draw faces
    for face in objects:
        img.draw_rectangle(face)
    # Now find eyes within each face.
    # Note: Use a higher threshold here (more detections) and lower scale (to find small objects)
        eyes = img.find_features(eyes_cascade, threshold=0.5, scale_factor=1.2, roi=face)
        for e in eyes:
            img.draw_rectangle(e)
    for blob in img.find_blobs(thresholds, pixels_threshold=200, area_threshold=200):
                # These values depend on the blob not being circular - otherwise they will be shaky.
         if blob.elongation() > 0.5:
               img.draw_edges(blob.min_corners(), color=(255,0,0))
               img.draw_line(blob.major_axis_line(), color=(0,255,0))
               img.draw_line(blob.minor_axis_line(), color=(0,0,255))
                # These values are stable all the time.
         img.draw_rectangle(blob.rect())
         img.draw_cross(blob.cx(), blob.cy())
                # Note - the blob rotation is unique to 0-180 only.
         img.draw_keypoints([(blob.cx(), blob.cy(), int(math.degrees(blob.rotation())))], size=20)

 

（2）蓝牙控制六足机器人运动

#include <LobotServoController.h>
LobotServoController myse;
void setup() {
  Serial.begin(9600);
  Serial1.begin(9600);
}

void loop() {

 while(Serial1.available())
 {
      char c = Serial1.read();
      Serial1.println(c);
      if (c=='6')
      {
          while (1)
          {
              ActionGroup(100);
          }
      }
      if (c=='7')
      {
          while(1)
          {
              ActionGroup(150);
          }
      }
      if (c=='8')
      {
          while (1)
          {
                ActionGroup(200);
          }
       }
 }
 }
void ActionGroup(int t)
{
   char c = Serial1.read();
    if(c=='1')
      {
        Serial1.println("forward");
        myse.stopActionGroup();
        delay(200);
        myse.setActionGroupSpeed(21,t);
        delay(200);
        myse.runActionGroup(21,1);
        delay(200);
      } 
    if(c=='2')
      {
        Serial1.println("backward");
        myse.stopActionGroup();
        delay(200);
        myse.setActionGroupSpeed(22,t);
        delay(200);
        myse.runActionGroup(22,1);
        delay(200);
      }
    if(c=='3')
      {
        Serial1.println("shiftleft");
        myse.stopActionGroup();
        delay(200);
        myse.setActionGroupSpeed(23,t);
        delay(200);
        myse.runActionGroup(23,1);
        delay(200);
      }
    if(c=='4')
      {
        Serial1.println("shiftright");
        myse.stopActionGroup();
        delay(200);
        myse.setActionGroupSpeed(24,t);
        delay(200);
        myse.runActionGroup(24,1);
        delay(200);
      }
    if(c=='7')
      {
        Serial1.println("turnleft");
        myse.stopActionGroup();
        delay(200);
        myse.setActionGroupSpeed(27,t);
        delay(200);
        myse.runActionGroup(27,1);
        delay(200);
       }
    if(c=='8')
       {
          Serial1.println("turnright");
          myse.stopActionGroup();
          delay(200);
          myse.setActionGroupSpeed(28,t);
          delay(200);
          myse.runActionGroup(28,1);
          delay(200);
        }
    if(c=='0')
        {
          Serial1.println("stop");
          myse.stopActionGroup();
          delay(200);
          myse.setActionGroupSpeed(20,t);
          delay(200);
          myse.runActionGroup(20,1);
          delay(200);
        }
}

 （3）六足机器人语音控制程序

/***************************Sonny 开发****************************
**  工程名称：语音控制机器人前行后退
** CPU: STC11L08XE
** 晶振：22.1184MHZ
** 波特率：9600 bit/S
**  作者：Sonny
**  修改时间：2019.07.29
/***************************Sonny 开发******************************/
#include "delay.h"
#include "uart.h"
#include "LobotServoController.h"
#include "bool.h"
#include "config.h"
/************************************************************************************/
// nAsrStatus ÓÃÀ´ÔÚmainÖ÷³ÌÐòÖбíʾ³ÌÐòÔËÐеÄ״̬£¬²»ÊÇLD3320оƬÄÚ²¿µÄ״̬¼Ä´æÆ÷
// LD_ASR_NONE:  ±íʾûÓÐÔÚ×÷ASRʶ±ð
// LD_ASR_RUNING£º  ±íʾLD3320ÕýÔÚ×÷ASRʶ±ðÖÐ
// LD_ASR_FOUNDOK:  ±íʾһ´Îʶ±ðÁ÷³Ì½áÊøºó£¬ÓÐÒ»¸öʶ±ð½á¹û
// LD_ASR_FOUNDZERO: ±íʾһ´Îʶ±ðÁ÷³Ì½áÊøºó£¬Ã»ÓÐʶ±ð½á¹û
// LD_ASR_ERROR:  ±íʾһ´Îʶ±ðÁ÷³ÌÖÐLD3320оƬÄÚ²¿³öÏÖ²»ÕýÈ·µÄ״̬
/***********************************************************************************/
uint8 idata nAsrStatus=0;
uint8_t G0_flag=DISABLE;//ÔËÐбêÖ¾£¬ENABLE:ÔËÐС£DISABLE:½ûÖ¹ÔËÐÐ
void MCU_init();
void ProcessInt0(); //ʶ±ð´¦Àíº¯Êý
void  delay(unsigned long uldata);
void  User_handle(uint8 dat);//Óû§Ö´ÐвÙ×÷º¯Êý
void Led_test(void);//µ¥Æ¬»ú¹¤×÷ָʾ
void Delay200ms();
sbit LED=P4^2;//ÐźÅָʾµÆ
//Ó¦ÓÃIO¿Ú¶¨Òå £¨Ä£¿é±ê×¢ P2£©
sbit PA1=P1^0; //¶ÔÓ¦°åÉϱêºÅ P1.0
sbit PA2=P1^1;  //¶ÔÓ¦°åÉϱêºÅ P1.1
sbit PA3=P1^2;  //¶ÔÓ¦°åÉϱêºÅ P1.2
sbit PA4=P1^3;  //¶ÔÓ¦°åÉϱêºÅ P1.3
sbit PA5=P1^4;  //¶ÔÓ¦°åÉϱêºÅ P1.4
sbit PA6=P1^5;  //¶ÔÓ¦°åÉϱêºÅ P1.5
sbit PA7=P1^6;  //¶ÔÓ¦°åÉϱêºÅ P1.6
sbit PA8=P1^7;  //¶ÔÓ¦°åÉϱêºÅ P1.7

/***********************************************************
 int main(void)
 {
 int i = 0;
  SystemInit();//ϵͳʱÖӵȳõʼ»¯
 delay_init(72);      //ÑÓʱ³õʼ»¯
 NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//ÉèÖÃNVICÖжϷÖ×é2:2λÇÀÕ¼ÓÅÏȼ¶£¬2λÏìÓ¦ÓÅÏȼ¶
  uartInit(9600);//´®¿Ú³õʼ»¯Îª9600
  uint8 idata nAsrRes;
 uint8 i=0;
 Led_test();
 MCU_init();
 LD_Reset();
 UartIni(); /*´®¿Ú³õʼ»¯*/
 nAsrStatus = LD_ASR_NONE;  // ³õʼ״̬£ºÃ»ÓÐÔÚ×÷ASR
 #ifdef TEST 
 PrintCom("Ò»¼¶¿ÚÁСÄþ\r\n"); /*text.....*/
 PrintCom("¶þ¼¶¿ÚÁ1.Ç°½ø\r\n"); /*text.....*/
 PrintCom(" 2.ºóÍˤ\r\n"); /*text.....*/
 PrintCom(" 3.×óÒÆ\r\n"); /*text.....*/
 PrintCom(" 4.ÓÒÒÆ\r\n"); /*text.....*/
 PrintCom("  5.ÕÅ×ì\r\n"); /*text.....*/
 PrintCom(" 6.ҧס\r\n"); /*text.....*/
 PrintCom(" 7.×óת\r\n"); /*text.....*/
 PrintCom(" 8.ÓÒת\r\n"); /*text.....*/
 PrintCom(" 0.ÔÝÍ£\r\n"); /*text.....*/
 #endif
 while(1){
  
  receiveHandle(); //½ÓÊÕ´¦Àí
  delay_ms(1);
  i++;
  if(i ==2000)
   getBatteryVoltage(); //·¢ËÍ»ñÈ¡µç³ØµçѹָÁî
  if(i == 3000) { 
   printf("Bvolt:%d mv\r\n",batteryVolt); //´òÓ¡µç³Øµçѹ
   i = 0;
 }
  switch(nAsrStatus)
  {
   case LD_ASR_RUNING:
   case LD_ASR_ERROR:  
    break;
   case LD_ASR_NONE:
   {
    nAsrStatus=LD_ASR_RUNING;
    if (RunASR()==0) /* Æô¶¯Ò»´ÎASRʶ±ðÁ÷³Ì£ºASR³õʼ»¯£¬ASRÌí¼Ó¹Ø¼ü´ÊÓÆô¶¯ASRÔËËã*/
    {
     nAsrStatus = LD_ASR_ERROR;
    }
    break;
   }
   case LD_ASR_FOUNDOK: /* Ò»´ÎASRʶ±ðÁ÷³Ì½áÊø£¬È¥È¡ASRʶ±ð½á¹û*/
   {    
    nAsrRes = LD_GetResult();  /*»ñÈ¡½á¹û*/
    User_handle(nAsrRes);//Óû§Ö´Ðк¯Êý
    nAsrStatus = LD_ASR_NONE;
    break;
   }
   case LD_ASR_FOUNDZERO:
   default:
   {
    nAsrStatus = LD_ASR_NONE;
    break;
   }
  }// switch     
 }// while
}
/***********************************************************
* Ãû    ³Æ£º   LEDµÆ²âÊÔ
* ¹¦    ÄÜ£º µ¥Æ¬»úÊÇ·ñ¹¤×÷ָʾ
* Èë¿Ú²ÎÊý£º ÎÞ
* ³ö¿Ú²ÎÊý£ºÎÞ
* ˵    Ã÷£º      
**********************************************************/
void Led_test(void)
{
 LED=~ LED;
 Delay200ms();
 LED=~ LED;
 Delay200ms();
 LED=~ LED;
 Delay200ms();
 LED=~ LED;
 Delay200ms();
 LED=~ LED;
 Delay200ms();
 LED=~ LED;
}
/***********************************************************
* Ãû    ³Æ£º void MCU_init()
* ¹¦    ÄÜ£º µ¥Æ¬»ú³õʼ»¯
* Èë¿Ú²ÎÊý£º 
* ³ö¿Ú²ÎÊý£º
* ˵    Ã÷£º      
* µ÷Ó÷½·¨£º
**********************************************************/
void MCU_init()
{
 P0 = 0xff;
 P1 = 0x00;
 P2 = 0xff;
 P3 = 0xff;
 P4 = 0xff;
 P1M1=0;
 P1M0=0xff;
 LD_MODE = 0;  // ÉèÖÃMD¹Ü½ÅΪµÍ£¬²¢ÐÐģʽ¶Áд
 IE0=1;
 EX0=1;
 EA=1;
}
/***********************************************************
* Ãû    ³Æ£º ÑÓʱº¯Êý
* ¹¦    ÄÜ£º
* Èë¿Ú²ÎÊý£º 
* ³ö¿Ú²ÎÊý£º
* ˵    Ã÷£º      
* µ÷Ó÷½·¨£º
**********************************************************/
void Delay200us()  //@22.1184MHz
{
 unsigned char i, j;
 _nop_();
 _nop_();
 i = 5;
 j = 73;
 do
 {
  while (--j);
 } while (--i);
}

void  delay(unsigned long uldata)
{
 unsigned int j  =  0;
 unsigned int g  =  0;
 while(uldata--)
 Delay200us();
}

void Delay200ms()  //@22.1184MHz
{
 unsigned char i, j, k;

 i = 17;
 j = 208;
 k = 27;
 do
 {
  do
  {
   while (--k);
  } while (--j);
 } while (--i);
}

/***********************************************************
* Ãû    ³Æ£º Öжϴ¦Àíº¯Êý
* ¹¦    ÄÜ£º
* Èë¿Ú²ÎÊý£º 
* ³ö¿Ú²ÎÊý£º
* ˵    Ã÷£º      
* µ÷Ó÷½·¨£º
**********************************************************/
void ExtInt0Handler(void) interrupt 0 
{  
 ProcessInt0();    /* LD3320 ËͳöÖжÏÐźţ¬°üÀ¨ASRºÍ²¥·ÅMP3µÄÖжϣ¬ÐèÒªÔÚÖжϴ¦Àíº¯ÊýÖзֱð´¦Àí*/
}
/***********************************************************
* Ãû    ³Æ£ºÓû§Ö´Ðк¯Êý
* ¹¦    ÄÜ£ºÊ¶±ð³É¹¦ºó£¬Ö´Ðж¯×÷¿ÉÔڴ˽øÐÐÐÞ¸Ä
* Èë¿Ú²ÎÊý£º ÎÞ
* ³ö¿Ú²ÎÊý£ºÎÞ
* ˵    Ã÷£º ͨ¹ý¿ØÖÆPAx¶Ë¿ÚµÄ¸ßµÍµçƽ£¬´Ó¶ø¿ØÖÆÍⲿ¼ÌµçÆ÷µÄͨ¶Ï     
**********************************************************/
void  User_handle(uint8 dat)
{
     //UARTSendByte(dat);//´®¿Úʶ±ðÂ루ʮÁù½øÖÆ£©
   if(0==dat)
   {
     G0_flag=ENABLE;
    LED=0;
   }
   else if(ENABLE==G0_flag)
   { 
     G0_flag=DISABLE;
    LED=1;
    switch(dat)     /*¶Ô½á¹ûÖ´ÐÐÏà¹Ø²Ù×÷,¿Í»§ÐÞ¸Ä*/
     {
      case CODE_QJ:   /*ÃüÁî¡°²âÊÔ¡±*/
      PrintCom("¡°Ç°½ø¡±ÃüÁîʶ±ð³É¹¦\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ¢
       runActionGroup(1, 1); //ÔËÐÐ1ºÅ¶¯×÷×é1´Î
       delay_ms(1500);
       stopActionGroup();  //Í£Ö¹¶¯×÷×éÔËÐÐ
       runActionGroup(1, 1); //ÔËÐÐ1ºÅ¶¯×÷×é1´Î
              break;
     case CODE_HT:  /*ÃüÁî¡°È«¿ª¡±*/
      PrintCom("¡°ºóÍË¡±ÃüÁîʶ±ð³É¹¦\r\n");//´®¿ÚÊä³öÌáʾÐÅÏ¢
      runActionGroup(2, 1); //ÔËÐÐ2ºÅ¶¯×÷×é1´Î
       delay_ms(1500);
       stopActionGroup();  //Í£Ö¹¶¯×÷×éÔËÐÐ
              break;
     case CODE_ZY:  /*ÃüÁî¡°¸´Î»¡±*/    
      PrintCom("¡°×óÒÆ¡±ÃüÁîʶ±ð³É¹¦\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ¢
      runActionGroup(3, 1); //ÔËÐÐ3ºÅ¶¯×÷×é1´Î
       delay_ms(1500);
       stopActionGroup();  //Í£Ö¹¶¯×÷×éÔËÐÐ
       setActionGroupSpeed(1, 200);//½«1ºÅ¶¯×÷×éÔËÐÐËÙ¶ÈÉèÖÃΪ200%
       runActionGroup(1, 1); //ÔËÐÐ1ºÅ¶¯×÷×é1´Î
             break;
     case CODE_YY:  /*ÃüÁî¡°¸´Î»¡±*/    
      PrintCom("¡°ÓÒÒÆ¡±ÃüÁîʶ±ð³É¹¦\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ¢
      runActionGroup(4, 1); //ÔËÐÐ4ºÅ¶¯×÷×é1´Î
       delay_ms(1500);
       stopActionGroup();  //Í£Ö¹¶¯×÷×éÔËÐÐ
             break;
     case CODE_ZZ:  /*ÃüÁî¡°¸´Î»¡±*/    
      PrintCom("¡°×óת¡±ÃüÁîʶ±ð³É¹¦\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ¢
      runActionGroup(7, 1); //ÔËÐÐ7ºÅ¶¯×÷×é1´Î
       delay_ms(1500);
       stopActionGroup();  //Í£Ö¹¶¯×÷×éÔËÐÐ
             break;
     case CODE_YZ:  /*ÃüÁî¡°¸´Î»¡±*/    
      PrintCom("¡°ÓÒת¡±ÃüÁîʶ±ð³É¹¦\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ¢
      runActionGroup(8, 1); //ÔËÐÐ8ºÅ¶¯×÷×é1´Î
       delay_ms(1500);
       stopActionGroup();  //Í£Ö¹¶¯×÷×éÔËÐÐ
             break;
     case CODE_ZT:  /*ÃüÁî¡°¸´Î»¡±*/    
      PrintCom("¡°ÔÝÍ£¡°ÃüÁîʶ±ð³É¹¦\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ¢
      runActionGroup(0, 1); //ÔËÐÐ0ºÅ¶¯×÷×é1´Î
       delay_ms(1500);
       stopActionGroup();  //Í£Ö¹¶¯×÷×éÔËÐÐ
             break;                           
     case CODE_ZK:  /*ÃüÁî¡°¸´Î»¡±*/    
      PrintCom("¡°ÕÅ¿ª¡°ÃüÁîʶ±ð³É¹¦\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ¢
      runActionGroup(5, 1); //ÔËÐÐ5ºÅ¶¯×÷×é1´Î
       delay_ms(1500);
       stopActionGroup();  //Í£Ö¹¶¯×÷×éÔËÐÐ
             break;    
     case CODE_YH:  /*ÃüÁî¡°¸´Î»¡±*/    
      PrintCom("¡°Ò§ºÏ¡°ÃüÁîʶ±ð³É¹¦\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ¢
      runActionGroup(6, 1); //ÔËÐÐ6ºÅ¶¯×÷×é1´Î
       delay_ms(1500);
       stopActionGroup();  //Í£Ö¹¶¯×÷×éÔËÐÐ
             break;    
     default:PrintCom("ÇëÖØÐÂʶ±ð·¢¿ÚÁî\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ
             break;
    } 
   } 
   else  
   {
    PrintCom("Çë˵³öÒ»¼¶¿ÚÁî\r\n"); //´®¿ÚÊä³öÌáʾÐÅÏ¢£¨¿Éɾ³ý£© 
   }
} 

 

六、实物制作

 1.加工件明细表

零件名称	数量	厚度
大腿版前侧	6	3mm
大腿版外侧	6	3mm
底板	1	3mm
底板下	1	3mm
小腿版两侧	12	3mm
支撑	6	3mm
夹爪固定端	1	3mm

 2.外购件清单

名称	数量	价格	淘宝链接
舵机	18	1761	https://item.taobao.com/item.htm?spm=a1z09.2.0.0.158a2e8dfZR5nf&id=23334420583&_u=ue2uapoac1a
24路舵机控制板	1	138	https://item.taobao.com/item.htm?spm=a1z09.2.0.0.158a2e8dfZR5nf&id=521728033704&_u=ue2uapo81b2
机械夹爪	1	140	https://m.tb.cn/h.e6vxWHd?sm=db9dee
铝板加工费	附表	430	https://item.taobao.com/item.htm?spm=a1z09.2.0.0.6d6d2e8dz2okDx&id=570777216949&_u=ae2uapo0ef4
铜柱	24	19.98	略
OPENmv	1	247.88	https://item.taobao.com/item.htm?spm=a1z09.2.0.0.386e2e8d5j93fe&id=557245458884&_u=uai92kkcb0d
语音识别模块	1	83	https://m.tb.cn/h.ehCi7R3?sm=8d8524
16路舵机控制板	2	204	https://item.taobao.com/item.htm?spm=a1z09.2.0.0.158a2e8dfZR5nf&id=521728033704&_u=ue2uapo81b2

 3.实物拼装图

 

图18 实物图

4.参数表

参数	数值	含义
a	142.18	基体中心距中间腿关节距离
b	159.12	基体中心距前腿关节水平距离
c	113.27	基体中心距前腿关节竖直距离
d	60.74	大腿基节长度
e	121.97	大腿长度
f	165.53	小腿长度

 

七、步态算法分析

1.六足机器人常见步态

（1）二步态

　　二步态也叫三足步态、三角步态。支撑状态为 L1、L3、L5 和 L2、L4、L6腿 交替支撑。行走示意图和步态相图如图所示，示意图中红色连杆表示支撑腿，步态相图中黑色实心部分表示支撑相。

图19  三足步态示意图

 

（2）三步态

　　三步态情况下始终有四条腿作为支撑相。通常将 L1、 L5 分为一组，L2、 L4 分为一组，L3、 L6 分为一组，也可按照其它方式两两分组。移动时每组腿依次抬起摆动。因支撑平面为四边形，因而又称之为四足步态或四角步态。其行走示意图如下：

图20 四足步态示意图

（3）六步态

　　六步态情况下每条腿各自为一个分组，依次抬起进行摆动。其抬起顺序变化相比于三步态更多，可以为 L1 -L6 -L2 -L5 -L3 -L4 也可以是 L1 -L4 -L2 -L6 -L3 -L5不同的迈步方式使得机器人在整个运动周期内的支撑平面以不同的方式变化，以便适应不同的越障或爬坡需求。其行走示意图如下：

图21 六步态示意图

 

2.六足机器人工况分析

　　平路二步态工况指的是六足机器人在水平地面上以二步态方式行走的工况。此种工况处于支撑相的腿数目最少，因而单腿和整机受垂向力最大。对于机架来说支撑点最少，最不稳定。对于单腿来说，尤其是单独在一侧支撑的单腿，其一条腿就支撑了整机约为一半的重量，加上冲击等因素此条腿承受的竖直力还要更多。暂设其受力系数为 1.5 倍,考虑到整机质量约为 3 t，得到单侧支撑相足端冲击约为 22500 N。

 

 图22 平路二步态

　　纵坡二步态足端受沿机体方向的最大纵向力。受纵向力数值与坡面角度有关。当纵向力最大时机体与坡面平行且足端支撑形式与平路二步态相同，即不采取姿态补偿措施。按照设计要求，最大纵坡角度为 30°。整机全重 3 t，单侧腿受力约为 1.5 t，分解成沿坡面力 7500 N，垂直坡面力 12990 N。

图23 纵坡二步态 

 3.MATLAB步态算法

MATLAB机器人工具箱下载地址：http://petercorke.com/wordpress/toolboxes/robotics-toolbox

 （1）运动分析

第一步：建立坐标系，做模型　

 

第二步：经过变换，则M点相对于坐标

 

 

 变换矩阵：

 

六足机器人基体的尺寸及各部分腿的编号：

 

MATLAB代码：

clear
clc
L1=60.74;%大腿与身体的长
L2=121.97;%大腿长
L3=165.53;%小腿长
a=305; %六边形边长
theta=[-120 -60 0 60 120 -180].*pi./180; %关节与基体的转角
origin=[340;0;0;1];% 初始位置，即足部移动340的距离
t=[a/2;-sqrt(3)*a/2;0];%水平方向偏移量，即腿关节距形体中心的位移

R=cell(1,6);% R为6个元素的元胞组
T=cell(1,6);% R为6个元素的元胞组
for i=1:6;
    R{i}=[cos(theta(i)),-sin(theta(i)),0; sin(theta(i)),cos(theta(i)),0;0,0,1];%旋转矩阵
    T{i}=[R{i},t;0 0 0 1];%变换矩阵
end

Trans=cell(1,6);
for i=1:6
    Trans{i}=T{i}*origin;
end

alpha=[0 0 0 0 0 0];
beta=[0 0 0 0 0 0];
gamma=[0 0 0 0 0 0];
phi=[0 0 0 0 0 0];
K1=[0 0 0 0 0 0];
K2=[0 0 0 0 0 0];
for i =1:6
    alpha(i)=(atan(Trans{i}(2)/Trans{i}(1)));%α值，基节转角
    if(alpha(i)>0)
        alpha(i)=alpha(i);
    else
        alpha(i)=alpha(i)+pi;
    end
    K1(i)=Trans{i}(1)/cos(alpha(i))-L1;
    K2(i)=Trans{i}(3);
    gamma(i)=(acos((K1(i)^2+K2(i)^2-L2^2-L3^2)/(2*L2*L3)));% γ值，小腿外转角
    phi(i)=atan(L3*sin(gamma(i))/(L2+L3*cos(gamma(i))));%φ值
    beta(i)=asin(K2(i)/(sqrt(L2+L3*cos(gamma(i)))^2+(L3*sin(gamma(i)))^2))-phi(i);%β值，大腿转角
  
    alphi(i)=alpha(i)*180/pi;%变换为角度
    beta(i)=beta(i)*180/pi;%变换为角度
    gamma(i)=gamma(i)*180/pi;%变换为角度
end

（2）simScape仿真动画：

1.支持包和安装插件下载

下载地址：https://ww2.mathworks.cn/campaigns/offers/download_smlink_confirmation.html?elqsid=1564535708654&potential_use=Student

2.安装教程

https://ww2.mathworks.cn/help/physmod/smlink/ug/installing-and-linking-simmechanics-link-software.html

（1）以管理员身份打开MATLAB，选择当前文件夹为下载的安装包路径；

（2）控制台输入：install_addon('smlink.r2017b.win64.zip')

（3）在MATLAB命令行窗口输入 regmatlabserver

（4）启动服务器后，激活Solidwork的MATLAB物理建模插件

a.在MATLAB命令行窗口输入：smlink_linksw

 

b.启动SW

c.找到工具菜单选择添加插件，选择Simscape Multibody Link

 仿真动画程序：

　　仿真示意图：

 

 

 

 

八、六足机器人的步行效率实验

1.不同足部结构对过障能力的实验探究

（1）实验问题：

　　本实验设定六足机器人所处环境分别为草洼地和斜沙坡两种环境，并在两种环境下分别测试评价不同足部结构的过障能力

 

（2）实验参数：

 

　　　　自变量：足部结构i，i=1表示足部结构与地面为点接触；i=2 表示足部结构与地面为线接触、i=3表示足部结构与地面为面接触。

　　　　因变量：六足机器人的过障能力 W

　　　　不变量：六足机器人的自重M、除脚步结构外的其他结构L、控制方式C、行走姿态Z、供电功率P

　　　　忽略因素：除地面环境变化外的其他环境变量，如环境温度、风力等；不考虑电池和机械结构在屡次实验中的耗损和折旧；

（3）实验方式：

 

　　　　足部结构的设计方式：对于点接触，机构初始状态即为点接触，脚步结构为弧形形状，与地面接触面为点接触；

　　　　　　　　　　　　　　对于线接触，机构足部每只脚由两块铝板构成，每块铝板底部为点接触，用胶带或细长物连接即可实现

　　　　　　　　　　　　　　对于面接触，机构足部用海绵、泡沫块等大面积物质固定，保证接触地面为面接触即可

　　　　过障能力的评价指标：a.是否能够完全通过障碍 Y

　　　　　　　　　　　　　　b.越过障碍耗电量的大小 V

　　　　　　　　　　　　　　c.越过障碍的耗时多少 T

（4）科学性分析：

　　六足机器人较其他移动机器人而言，最大的优势在于其多足结构可以适应多种复杂地形，故而越障能力对于六足机器人而言显得至关重要。对于越障而言，优先考虑的是能够通过障碍；其次是通过障碍所消耗能源的多少；最后是通过障碍花费的时间的多少。这三个评价指标涵盖了移动平台在越障中的关键因素，从功能的完整性、耗能、效率等三方面进行考虑，符合现实场景中六足机器人的应用痛点，研究性十分具有价值和意义。

　　其次，在设计中发现足部结构的改变对越障能力有着很大的影响，在同样的基体结构、能源供给、环境条件下，面接触的足部结构可能更便于越障，故此处将足部结构作为变量进行试验论证，并对比不同环境下的数据结果，研究论证“面接触式的足部结构可更好适应全部环境”或是“某种环境条件面接触式足部结构更适应全部环境”等观点。

（5）实验数据、结果（待续）

2.不同步态对移动速度的实验探究

（1）实验问题：

　　本实验设定六足机器人存在三角步态、四角步态、六角步态三种典型步态，并在满足步态稳定的前提下对其在平地的移动速度进行分析比较，并得出最高效的行走步态方案。

（2）实验参数

　　自变量：六足机器人的步态模式

　　因变量：六足机器人的移动速度

　　无关变量：六足机器人的自重、地面环境、机械结构

　　忽略因素：除地面环境变化外的其他环境变量，如环境温度、风力等；不考虑电池和机械结构在屡次实验中的耗损和折旧；

（3）实验方式

  　　步态模式由24路舵机控制板通过上位机界面编程更换不同模式，并在不同模式下进行试验。

　　　a.三角步态为：

 

　　　b.四角步态为：

 

　　　c.六角步态为：

 

 

　　其次，六足机器人移动速度的评价指标

　　　a.以固定距离内，六足机器人从起点出发至到达终点所用时间作为速度指标

　　　b.在同样距离内，不同步态所消耗的电量

（4）科学性分析

　　六足机器人优势在于可适应性强，但其劣势在于移动速度较慢，故而在满足可适应的条件下使其移动速度最大化，是六足机器人发展的必要研究方向，对六足的野外搜救、侦察等应用中发挥很大的作用。

　　六足机器人对移动速度的影响因素包括很多，如供电功率、行走机构、步态模式等，此处选取步态模式作为研究对象，仿照自然界中的昆虫、哺乳两大类典型的爬行步态进行试验，测试选取移动速度最佳的方案，仿生学的基础上保证了一定的科学性。