四足机器人控制系统设计及其仿真.doc
北京理工大学珠海学院2020届本科生毕业设计四足机器人控制系统设计及其仿真学 院： 工业自动化学院 专 业： 机械工程 姓 名： 饶旭 学 号： 170409102877 指导教师： 李兵 职 称： 副教授 中国·珠海二二年五月毕业设计诚信承诺书本人承诺呈交的毕业设计四足机器人控制系统设计及其仿真是在指导教师的指导下，独立开展研究取得的成果，文中引用他人的观点和材料，均在文后按顺序列出其参考文献，设计使用的数据真实可靠。 本人签名： 日期： 年 月 日四足机器人控制系统设计及其仿真摘 要 本次设计课题为基于STM32单片机的优秀性能对四足机器人进行控制系统设计与编程，设计中STM32单片机是核心部分。目的是一个能将目的动作通过STM32单片机作为控制系统将编写代码转输出为电信号，再由这些电信号对四足机器人上的配电板连接的电机进行控制，由此达到机械腿上的电机能按照预定工作时间段进行工作，驱使连接四足机器人腿上的连轴杆正常转动。连接连轴杆上的电调驱动机械腿上的电机按照规定转动相应角度，达到机械腿按目标轨迹行走的目的。与STM32单片机相连的还有陀螺仪，陀螺仪会根据自身内部的三个定位基准轴对四足机器人自身目前与水平定位偏差进行反馈，它反馈给单片机后再由控制系统对相应电机进行调节达到修补偏差的目的。关键字：腿式机器人、STM32单片机、电调、陀螺仪； Design and Simulation of the Control System of Quadruped Robot Abstract This design project is based on the excellent performance of the STM32 microcontroller to design and program the control system of the quadruped robot. The STM32 microcontroller is the core part in the design. The purpose is to be able to use the STM32 microcontroller as a control system to convert the written code into electrical signals, and then use these electrical signals to control the motor connected to the power distribution board on the quadruped robot, thereby reaching the motor on the mechanical leg It can work according to the predetermined working time period, and drive the connecting rod on the leg of the quadruped robot to rotate normally. The ESC connected to the connecting rod drives the motor on the mechanical leg to rotate the corresponding angle according to the regulations to achieve the purpose of the mechanical leg walking according to the target trajectory. There is also a gyroscope connected to the STM32 single-chip microcomputer. The gyroscope will feedback the current positioning deviation of the quadruped robot according to its three internal positioning reference axes. After the feedback to the single-chip microcomputer, the control system will adjust the corresponding motor The purpose of patching deviations.Keywords: Leg robot, STM32 microcontroller, ESC, gyroscope；2目 录1机器人控制系统概述11.1机器人控制系统研究的背景11.1.1机器人控制系统的特点21.2基于单片机四足机器人控制系统的优点21.2.1基于单片机机器人控制系统的优点21.2.2基于单片机机器人控制系统的缺点32基于STM32单片机四足机器人控制系统研发的可行性42.1价格低廉42.2方便安装42.3市场性42.4研究分析42.4.1基本思路42.4.2技术路线42.4.3设计方案53四足机器人控制系统分析63.1单片机简介63.1.1单片机分类63.1.2STM32F407简介63.2Keil简介73.3proteus简介84基于STM32单片机的四足机器人控制系统的硬件设计94.1主要电路结构94.1.1电源94.1.2分电板94.1.3陀螺仪104.1.4电调104.1.5电机115程序部分编程125.1遥控接收处理函数135.2电机与陀螺仪数据接收发送函数165.3姿态控制185.4步行步态215.5停下215.6原地跳跃226使用PROTEUS进行系统的调试236.1仿真的注意事项236.2Keil与proteus联调246.3调试方法247结 论25参考文献26附录128附录231附录338附录453附录556致 谢58北京理工大学珠海学院2020届本科生毕业设计1 机器人控制系统概述1.1 机器人控制系统研究的背景国内外研究现状和发展趋势：随着机器人技术的发展，机器人应用领域的不断扩大，对机器人的性能提出了更高的要求，因此，如何有效地将其他领域(如图像处理、声音识别、最优控制人工智能等)的研究成果应用到机器人控制系统的实时操作中，是一项富有挑战性的研究工作。而具有开放式结构的模块化、标准化机器人，其控制系统的研究无疑对提高机器人性能和自主能力，推动机器人技术的发展具有重大意义。图1-1 四足机器人随着机器人控制技术的发展，针对结构封闭的机器人控制器的缺陷，开发“具有开放式结构的模块化、标准化机器人控制器”是当前机器人控制器的一个发展方向。近几年，日本、美国和欧洲一些国家都在开发具有开放式结构的机器人控制器，如日本安川公司基于PC开发的具有开放式结构、网络功能的机器人控制器。我国863计划智能机器人主题也已对这方面的研究立项。由于适用于机器人控制的软、硬件种类繁多和现代技术的飞速发展，开发一个结构完全开放的标准化机器人控制器存在一定困难，但应用现有技术，如工业PC良好的开放性、安全性和互联网相同性，标准的实时多任务操作系统，标准的总线结构，标准接口等，打破现有机器人控制结构封闭的局面，开发结构开放性、功能模块化的标准机器人控制器是完全可行的。 1.1.1 机器人控制系统的特点(1)机器人控制系统是一个多变量控制系统，即使简单的工业机器人也有35个自由度，比较复杂的机器人有十几个自由度，甚至几十个自由度，每个自由度一般包含一个伺服机构多个独立的伺服系统必须有机地协调起来。例如，机器人的手部运动是所有关节的合成运动，要使手部謝安照一定的轨迹运动就必须控制各关节协调运动，包括运动轨迹、动作时序等多方面的协调。 (2)运动描述复杂，机器人的控制与机构运动学及动力学密切相关。描述机器人状态和运动的数学模型是一个非线性模型，随着状态的变化，其参数也在变化，各变量之间还存在耦合。因此，仅仅考虑位置闭环是不够的，还要考虑速度闭环，甚至加速度闭环。在控制过程中，根据给定的任务，应当选择不同的基准坐标系，并做适当的坐标变换，求解机器人运动学正解和逆解。此外，还要考虑各关节之间惯性力、哥氏力等的耦合作用和重力负载的景影响，因此，系统中还经常采用一些控制策略，如重力补偿、前馈、解耦或自适应控制等。 (3)具有较高的重复定位精度，系统刚性好。除直角坐标机器人外，机器人关节上的位置检测元件不能安装在未端执行器上，而应安装在各自的驱动轴上，构成位置半闭环系统。但机器人的重复定位精度较高，一般为±0.1mm。此外，由于机器人运行时要求运动平稳，不受外力干扰，为此系统应具有较好的刚性(4)信息运算量大。机器人在动作上往往可以通过不同的方式和路径来完成，因此存在一个最优的问题，较高级的机器人可以采用人工智能的方法;用计算机建立起庞大的信息库，借助信息库进行控制、决策管理和操作。根据传感噐和模式识别的方法获得对象及环境的工况，按照给定的指标要求，自动选择最佳的控制规律。(5)需采用加或减速控制。过大的加或减速度会景影响机器人运动的平稳性，甚至使机器人发生抖动，因此在机器人起动或停止时采取加或减速控制策略。通常采用匀加或减速运动指令来实现。此外，机器人不允许有位置超调否则将可能与工件发生碰撞。因此，要求控制系统位置无超调，动态响应尽量快。1.2 基于单片机四足机器人控制系统的优点1.2.1 基于单片机机器人控制系统的优点 机器人控制系统无疑是对现代机器人在如何协调运行方面非常重要的系统，小型机器人的控制系统通常采用一些功能相对独立的单片机系统来实现，对于用单片机作为小型机器人的控制系统的优势，它具多功能、高效率、高性能、低电压、低功耗、低价格等优点。而单片机中STM32系列具有低功耗、廉价、稳定性能好、功能强大等优点。基于STM32单片机的四足机器人控制系统是一种对单片机性能要求较高，结构相对简单，实用性较强的中高端电子产品，以单片机作为核心器件，A/D转换元件为重要部分，以其体积小、成本低、相应速度快等优点，其具有很好的使用意义。1.2.2 基于单片机机器人控制系统的缺点对于使用单片机作为机器人控制系统的小型机器人而言，小型机器人运动时所受到的干扰幅度的影响相较于大型机器人受到干扰幅度的影响相对较大，但由于以单片机作为控制系统，控制命令全由单片机输出且单片机易受到干扰，所以导致机器人抗干扰能力差。在处理任务方面，单片机处理任务单一，运算速度较其他基础的控制系统慢。2 基于STM32单片机四足机器人控制系统研发的可行性2.1 价格低廉系统中采用了简单常见、安全可靠的外设，如电机和电池都是市面上常见的型号，价格稳定且并不昂贵，且STM32单片机价格有相对便宜、功能强大和简单操控等特点。 2.2 方便安装整个系统要方便安装，这样才能减少安装成本，由于该控制系统的主要控制中枢为单片机，个头小且结构简单，可以自己动手安装，一方面可以获得一些乐趣，另一方面如果出现故障问题，也易于排查。结构简单且方便安装也可以让该系统有更广阔的市场需求，低廉的成本，实用的系统，这样才能给系统的进一步完善提供条件。所以安装方面是设计的一个优点，由于控制系统与其他执行元件连接紧凑，由单片机连接各元件的线路也相对清晰，易于安装。 2.3 市场性腿式机器人系统是履带和轮式机器人的替代品，适用于崎岖的地形和复杂的环境。机械腿与不同环境中选择的接触点的自由度使他们能够克服与腿长相当的障碍。有了这样的功能，有腿的机器人有一天可以在森林和山脉中营救人员，爬楼梯以在建筑工地中携带有效载荷，检查非结构化的地下隧道并探索其它未知地域。腿式系统具有执行人类和动物能够进行的任何的运动潜力。2.4 研究分析2.4.1 基本思路研究的基本思路是以单片机为中心，单片机所在电路板为必要部分，再进行外围的设计。主要的研究方法为设计电路图，编程，进行仿真。2.4.2 技术路线先理清四足机器人控制系统的含义及意义，收集资料，分析资料，再通过学习吸取经验，进行设计与编程，检验方案的可行性，多次修改方案，最终达到最完美的效果。2.4.3 设计方案首先学习STM32单片机，掌握所以相关知识后，用Keil uVision5编写程序，并与proteus联用仿真，检验方案的可行性。3 四足机器人控制系统分析3.1 单片机简介 单片机是单片机微型计算机的简称，一种典型的嵌入式微控制器（Microcontroller Unit）。它是一种集成电路芯片，把具有数据处理能力的中央处理器CPU和一系列功能集成在一起的计算机系统。3.1.1 单片机分类单片机的分类有STM32系列单片机、51系列单片机、PIC单片机、AVR单片机。STM32单片机是专为要求高性能、低成本、低功耗的嵌入式应用设计的单片机，功能强大且实用性广，能完成相对复杂的需求。51单片机是目前最广泛应用的单片机之一，也是单片机的入门级单片机，易学且实用性广。PIC单片机指令数量比较少，编写程序较为麻烦。精简指令使其执行效率高，保密熔丝使其保密性好。AVR单片机是一种精简指令集高速8位单片机 ，其主要特点是执行速度快，可靠性高，低功耗。3.1.2 STM32F407简介其中STM32F407是STM32F4系列单片机的一种，STM32F407提供了工作频率为168 MHz的Cortex-M4内核的性能。在168 MHz频率下，从Flash存储器执行时，STM32F407能够提供210 DMIPS/566 CoreMark性能，并且利用意法半导体的ART加速器实现了FLASH零等待状态。DSP指令和浮点单元扩大了产品的应用范围。该系列产品采用意法半导体90 nm工艺和ART加速器，具有动态功耗调整功能，能够在运行模式下和从Flash存储器执行时实现低至238 µA/MHz的电流消耗。图4-1 STM32F407引脚图 3.2 Keil简介Keil软件是目前广泛运用于单片机系列的软件。Keil提供了包括C编译器、宏汇编、链接器、库管理和一个功能强大的仿真调试器等在内的完整开发方案，通过一个集成开发环境将这些部分组合在一起。Keil C51生成的目标代码效率非常之高，多数语句生成的汇编代码很紧凑，容易理解。在开发大型软件时更能体现高级语言的优势。与汇编相比，Keil可使用的C语言是较为简单易懂的，也是计算机语音入门的基础，因此该软件也易于上手。 Keil使用流程如下：第一步：在菜单栏中点击“project”选择New uVision Project”新建一个工程，在弹出对话框中，工程文件选择要放置的文件夹。弹出一个框，选择在CPU类型如STM32F407系列芯片。选择好后会弹出一个窗口提示是否添加启动代码。这样新工程的创建完毕。第二步：点击工具栏，选择File-new，新建一个空白文档。在空白处写入程序。单击左上方的Save，保存好源程序，并把文档后缀定为“.C”。如果是汇编语言，后缀为“.ASM”。把刚创建的源程序文件加入到工程项目文件中，右击右方“Source group”，单点“ADD”按钮时，选择刚保存的源程序文件，单击“Close”。第三步：单击Project,选择“options for Target （自定义）”,在“xtal（MHZ）”中设置要设置的晶振参数。单击Output栏，在“Create HEX File”前面打勾，这样做是为了输出hex文件，用于程序在仿真软件proteus上调试。第四步：单击左上方的Build，进行编译。第五步：根据编译后的反馈得知程序是否出错，通过显示错误代码位置进行修改，直至编译完全正确后，输出的hex文件可直接使用。3.3 proteus简介Proteus是一种EDA工具软件，它能仿真单片机、ARM等芯片及外围器件，它能进行原理图设计、代码的调试、PCB设计并能与KEIL联动调试，是国内外著名的EDA仿真软件。Proteus基本操作：元件的选择：按界面的左上方P按钮可弹出选择元件框。选择要使用的元件：在Pick Device弹出窗口Keywords中输入关键字。然后选中所需元件、单击OK。放置元件到绘图区：单击工作界面左边的列表元件，然后在右侧的绘图区单击选中所需放置的位置。移动元件位置：右击选中元件，然后按住左键进行拖动。 删除元件：右击选中元件使元件呈红色，选中后再一次右击则是删除，也可以在右击后选择“Deletet Object” 旋转元件：右击选中元件，在弹出窗口的旋转工具栏选择旋转方式。 元件连线：在鼠标指针处于笔状时，左击线的初始位置，再左击选择结束位置。 电源和地的绘制：单击工具栏上的Terminals Mode，左侧工具栏显示TERMINALS时，可在其中选择POWER或GROUND等符合，在放置到绘图区。 仿真开始：单击左下方的Play按键，即开始按钮进行原理图仿真测试。仿真结束：单击左下方Stop按键，结束仿真。4 基于STM32单片机的四足机器人控制系统的硬件设计在本设计中基于STM32单片机作为控制系统的设计以对各元件运行控制为基础，对硬件设计为辅，达到控制系统可以完整控制各个硬件运作。四足机器人控制系统的核心为STM32F407单片机。4.1 主要电路结构 图4-1 主要电路结构示意图4.1.1 电源电源使用DJI TB470D型号的电池作为四足机器人的主要电源，该型号的电池能提供24V的直流电，选择该直流电源能保证电路供应电流稳定。4.1.2 分电板 图4-2 分电板 主要电路结构图中PDB为分电板，可将电池的电流分流到多个需要使用的接口，供多个接口一起使用电池供应的稳定电流，接口段为塑料接口，可达到减轻机器人整体重量的目的。4.1.3 陀螺仪图4-3 陀螺仪电路结构示意图中的IMU为陀螺仪，它是一个角度传感器，即惯性测量单元，它由三个单轴的加速度计和三个单轴的陀螺仪组成，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，对这些信号进行处理之后，便可解算出物体的姿态，通过陀螺仪中标定的坐标轴yaw轴pitch轴roll轴，解算出当前所处在的运动位置的偏差，为四足机器人运动时产生的偏差进行校正提供依据。本次设计使用的为IMU ICM-20602 6-axis型号的陀螺仪，轴加速计噪声100 ug / Hz。4.1.4 电调 图4-4 电调电调，全称电子调速器，它的输入线可连接电源，输出线可连接电机。在电路结构图中为Encoder，本次设计中使用的是C620型号的电调输入线连接STM32单片机，由单片机进行控制它的电输入，达到根据控制信号调节电动机的转速的目的。4.1.5 电机在电路结构图中MOTO为电机，本设计的电机使用3508电机，在每条腿上各有两个电机分别驱动组成一条腿的两个连轴，以达到控制一条腿进行运动的目的。5 程序部分编程 图5-1总体流程图 整个程序编程部分如上总体流程图所示，首先要对四足机器人的路径规划、落脚点选择以及运动指令的生成要有初步成型的预计，为之后各执行过程中所要达到的效果做指引。在对四足机器人的运动时大致要遇到的各种复杂情况和对机械腿的落脚范围做预估后，要对控制系统可通过外部控制信号对四足机器人运动执行进行控制的方式进行设置。在设置完机械腿的运动期望和落脚点信息后，在启动时首先要根据陀螺仪对当前四足机器人整体姿态进行定位，通过对于定位时通过陀螺仪产生的坐标来判定是否偏移目标平稳坐标，若检测有偏差后便将偏差反馈到原始设定的各机械腿的运动轨迹方程与之对比后可进行修正。待修正完后，便可通过芯片对相应腿部的电机进行按照预定运动轨迹方程运行控制，达到生成指定步态的效果。在机械腿上的运动效果也可通过控制器去反馈机械能否按照预期运行的信息，从而达到对四足机器人遇到不同情况进行不同的运动轨迹反应。5.1 遥控接收处理函数图5-2 遥控接收处理函数该程序通过Usart2开启DMA传输，非阻塞接收遥控发过来的数据，接收完数据在串口空闲中断进行数据解析，不影响其它运算量大的任务，做到效率最大化。代码实例：1. #define RECEIVELEN 128 /一次DMA最大接受128字节 2. uint8_t rc_RxBufferRECEIVELEN;/DMA接收缓存数组 3. /遥控器控制变量 4. static RC_Ctrl_t RC_CtrlData = 0; 5. static uint16_t temp;/储存DMA剩余传输数据位数 6. /初始化DMA，串口2 7. void remote_control_init(void) 8. 9. RC_Init(rc_RxBuffer,RECEIVELEN); 10. 11. /返回遥控器控制变量，通过指针传递方式传递信息 12. const RC_Ctrl_t *get_remote_control_point(void) 13. 14. return &RC_CtrlData; 15. 16. void USART2_IRQHandler(void) 17. 18. if (_HAL_UART_GET_FLAG(&huart2, UART_FLAG_IDLE) && 19. _HAL_UART_GET_IT_SOURCE(&huart2, UART_IT_IDLE) 20. 21. uint16_t tmp = huart2.Instance->SR; 22. tmp = huart2.Instance->DR; 23. tmp-; 24. _HAL_UART_CLEAR_IDLEFLAG(&huart2); 25. CLEAR_BIT(huart2.Instance->SR, USART_SR_IDLE); 26. _HAL_DMA_DISABLE(huart2.hdmarx); 27. 28. temp = huart2.hdmarx->Instance->NDTR; 29. if(RECEIVELEN - temp) = 18)/判断已接受DMA数据多少位 30. 31. DetectHook(0); 32. Get_Remote_info(&RC_CtrlData ,rc_RxBuffer); 33. 34. HAL_UART_Receive_DMA(&huart2, (uint8_t *)rc_RxBuffer, RECEIVELEN); 35. SET_BIT(huart2.Instance->CR1, USART_CR1_IDLEIE); 36. DMA1->HIFCR = DMA_FLAG_DMEIF0_4 | DMA_FLAG_FEIF0_4 | DMA_FLAG_HTIF0_4 | DMA_FLAG_TCIF0_4 | DMA_FLAG_TEIF0_4; 37. _HAL_DMA_SET_COUNTER(huart2.hdmarx, RECEIVELEN); 38. _HAL_DMA_ENABLE(huart2.hdmarx); 39. 40. 41. void Get_Remote_info(RC_Ctrl_t *rc_ctrl ,volatile const uint8_t *sbus_buf) 42. 43. rc_ctrl->rc.ch0 = (sbus_buf0 | (sbus_buf1 << 8) & 0x07ff; /!< Channel 0 44. rc_ctrl->rc.ch1 = (sbus_buf1 >> 3) | (sbus_buf2 << 5) & 0x07ff; /!< Channel 1 45. rc_ctrl->rc.ch2 = (sbus_buf2 >> 6) | (sbus_buf3 << 2) | /!< Channel 2 46. (sbus_buf4 << 10) &0x07ff; 47. rc_ctrl->rc.ch3 = (sbus_buf4 >> 1) | (sbus_buf5 << 7) & 0x07ff; /!< Channel 3 48. rc_ctrl->rc.s0 = (sbus_buf5 >> 4) & 0x0003); /!< Switch left 49. rc_ctrl->rc.s1 = (sbus_buf5 >> 4) & 0x000C) >> 2; /!< Switch right 50. rc_ctrl->mouse.x = sbus_buf6 | (sbus_buf7 << 8); /!< Mouse X axis 51. rc_ctrl->mouse.y = sbus_buf8 | (sbus_buf9 << 8); /!< Mouse Y axis 52. rc_ctrl->mouse.z = sbus_buf10 | (sbus_buf11 << 8); /!< Mouse Z axis 53. rc_ctrl->mouse.press_l = sbus_buf12; /!< Mouse Left Is Press ? 54. rc_ctrl->mouse.press_r = sbus_buf13; /!< Mouse Right Is Press ? 55. rc_ctrl->key.v = sbus_buf14 | (sbus_buf15 << 8); /!< KeyBoard value 56. rc_ctrl->rc.ch4 = sbus_buf16 | (sbus_buf17 << 8); /NULL 57. 58. rc_ctrl->rc.ch0 -= RC_CH_VALUE_OFFSET; 59. rc_ctrl->rc.ch1 -= RC_CH_VALUE_OFFSET; 60. rc_ctrl->rc.ch2 -= RC_CH_VALUE_OFFSET; 61. rc_ctrl->rc.ch3 -= RC_CH_VALUE_OFFSET; 62. rc_ctrl->rc.ch4 -= RC_CH_VALUE_OFFSET; 63. 5.2 电机与陀螺仪数据接收发送函数图5-3 电机与陀螺仪接收发送函数编程电机与陀螺仪接收发送函数后，使单片机透过陀螺仪的三个定位基准轴给与的定位，对电机进行控制与调整机器人。代码实例：/*/*陀螺仪数据解析储存结构体*1. /陀螺仪数据结构体 2. typedef struct 3. 4. float v_x;/X轴速度 5. float v_z;/Z轴速度 6. double pit;/PIT轴角度 7. double yaw;/YAW轴角度 8. float acc_x;/X轴加速度 9. float acc_z;/Z轴加速度 10. float acc_y;/Y轴加速度 11. double yaw_init_offset; 12. int16_t raw_pit; 13. int16_t raw_yaw; 14. 15. Gyro_info_t; 16. /*/1. /*/2. *电机数据解析储存结构体*3. /rm电机统一数据结构体，电机通过CAN通信与主控连接，主控接收到完整一次数据进入Can中断处理，解析4. typedef struct 5. 6. uint16_t ecd;/编码值 7. uint16_t send_ecd; 8. int16_t speed_rpm;/转速 9. int16_t given_current;/输出电流 10. uint8_t temperate;/温度 11. int16_t last_ecd;/上次编码值 12. fp32 total_angle;/总角度 13. uint16_t offset_ecd;/偏移编码值 14. int32_t round_cnt;/电机圈数 15. motor_measure_t; 16. /陀螺仪数据结构体 17. /*/1. /电机数据读取解析函数 2. void get_motor_measure(motor_measure_t* ptr, uint8_t Data)3. 详细内容查看附录/电机发送函数void Send_3508_Id1_Id4(int16_t a, int16_t b, int16_t c, int16_t d) 详细内容查看附录5.3 姿态控制该程序可将机器腿上两个连轴杠的预期转动角度变化转化为对电机运行的控制，控制电机转动使得两连轴杠能转动到相应角度的位置，达到腿能运行的目的。腿通过角度环控制八个电机，实现腿的开环轨迹从而行走。主要通过改变步态的参数，如频率，占空比等，正弦的几何参数或峰值等，轨迹坐标转换为所需腿角度即菱形腿中线相对于纵轴偏移角度和对称关节夹角度一半，然后由这两个角度算出上关节所需运动的角度theta1和theta2从而根据实际电机传动结构得出电机所需转动角度，通过角度环精准到位。角度环比例项增加了腿的刚度，微分项则抵消了关节的阻尼。 图5-4 腿部角度坐标简图 图5-5 腿部摆动角度坐标简图运动轨迹解算： 图5-6 运动轨迹解算流程图主要算法如下：图5-7 主要算法图5-8 主要算法图5-9 主要算法检验步态参数设置是否合理，其主要判断最低高度，是否超过腿长，最高高度是否达到机械限位以及步态频率过低过高，还有占空比。图5-10 检验步态参数合理代码5.4 步行步态以下便是步态结构体参数（步行）：stance_height, down_AMP, up_AMP, flight_percent (proportion), step_length, FREQ0.24, 0.025f, 0.09, 0.35, 0.09, 1.5f, 0.0, / TROTStance_height：正常站立高度down_AMP：下降最大高度
