四足机器人算法验证流程框架

图1.1 自足机器人软件硬件控制流程图

（1）操作员通过遥控器给机械狗下发平动速度 和转向率命令；

（2）机械狗接收到命令，质心COM生成参考轨迹并传送给身体和腿部控制器。

（3）控制器根据用户输入命令和机器人状态，如果腿处于腿在摆动中，则使用“摆动腿控制器”，如果腿处于腿在支撑中，则使用“力控支撑腿控制器”

（4）力和位置指令被发送到STM32微控制器，用于将电机指令传递给机器人的每条腿。

（5）更高的要求，比如自主导航，视觉辅助等，则可以给计算机板卡加个工控机运行SLAM或者AI算法。

（一）腿部硬件组成

腿部硬件组成以1条腿进行设计，其它腿也一样，嵌入硬件设计部分见图2.2所示：

图1.2 四足机器人单腿通信控制流程

腿部控制器基于Cortex-A8的处理器，执行腿部控制任务，如笛卡尔阻抗的控制或联合PD控制。负责完成较高的腿部频率和高速关节速度摇摆时的滤波、高带宽跟踪相位以及其他位置控制动作。最后，每条腿控制器发送扭矩指令并接收无刷电机上的编码器测量值。可以看出UP board计算机板卡的核心功能就是进行运动学和动力学运算。使用第二代酷睿i7处理器，运行UbuntuLinux和配置抢占RT补丁。UP board通过以太网与笔记本电脑通信，UP board使用轻量级接收用户命令并记录数据通信和编组（LCM）。

（二）数据转换板

其中数据转换板在四足机器人的腿部使用多个电机来实现运动和步态控制。每个腿部电机都需要根据控制指令进行旋转，以驱动腿部关节的运动。腿部电机数据转换板起到了信号转换和驱动功率放大的作用。

图1 数据转换板

使用的是GD32F150R8T6，一个GD32F4通过CAN总线负责两条腿，一个腿由3个GD32控制板通过FOC算法控制腿部三个电机，而且只运行了FOC算法。四足机器人腿部电机数据转换板是一种用于控制四足机器人腿部电机的电路板。主要功能是将来自主控制器的信号转换为适合驱动腿部电机的电流和电压信号。

在四足机器人腿部电机数据转换板上，通常包含以下主要组件：

控制信号接口：用于接收来自主控制器的控制信号，通常是PWM（脉冲宽度调制）信号或者其他数字信号。

电机驱动器：将控制信号转换为适合驱动腿部电机的电流和电压信号。电机驱动器可以采用各种技术，如MOSFET（金属氧化物半导体场效应晶体管）驱动器、H桥驱动器等。

电源接口：提供电源输入接口，连接外部电源供应，并为腿部电机提供所需的电流和电压。

保护电路：用于保护电机和电路板免受过电流、过热和电源过压等因素的损害。常见的保护电路包括过电流保护、过温保护和瞬态电压抑制器等。

反馈接口：腿部电机数据转换板还可以包含反馈接口，用于读取电机的位置、速度或电流等信息，以提供闭环控制。

腿部电机数据转换板的设计和功能会根据具体的四足机器人应用而有所不同。一些高级的转换板可能还会集成额外的功能，如通信接口（如CAN、UART等）和编码器接口等，以更好地与其他系统进行连接和协作。

（三）IMU传感器

四足机器人IMU模块的原理是基于牛顿力学原理和运动学原理，通过测量机器人的加速度和角速度来计算机器人的位置、速度和姿态等数据。这些数据可以用于控制机器人的运动和姿态调整，例如跳跃、爬行、行走和平衡等。

图2 IMU传感器

用的是串口通信，每个STM32还有独立串口通信，用于模块的设置与debug。IMU模块通常由以下主要组件组成：

加速度计：用于测量机器人的加速度，可以检测机器人的静态或动态加速度，并将其转换为电信号输出。

陀螺仪：用于测量机器人的旋转速度，可以检测机器人的角速度，并将其转换为电信号输出。

磁力计：可用于检测地球磁场方向，以提供机器人的定位信息。通常情况下，磁力计会与加速度计和陀螺仪配合使用，以提供更为准确的位置估计。

四足机器人IMU模块的用途非常广泛，主要包括以下几个方面：

姿态控制：IMU模块可以测量机器人的姿态和旋转速度，从而为机器人提供实时的姿态反馈，使机器人能够更加精确地控制的姿态和避免倾覆。

运动控制：IMU模块可以测量机器人的加速度和角速度，从而为机器人提供实时的运动状态反馈，以便控制机器人的移动、速度和方向等。

环境感知：IMU模块可以检测机器人所处的环境信息，例如地面坡度、震动等，这些信息可以为机器人提供更准确的运动预测和控制。

（四）电机控制器与磁编码器

四足机器人电机控制器与磁编码器是电机控制器接收数据转换板转发来的信息（力矩、位置、速度及位置和速度的增益），并且上传反馈信息（位置、速度以及基于电流环测量的扭矩）最大通信频率：4kHz/驱动器数目 （可通过增加CAN总线数目扩大通信带宽）

图3 磁编码器

四足机器人电机控制板中的磁编码器是一种可以测量电机旋转角度和速度的传感器。可以通过检测磁场变化，计算出电机旋转的精确位置和角度，并将这些数据反馈给电机控制板，以实现更加精准的电机控制。

磁编码器通常由以下主要组件组成：

磁铁：固定在电机轴上，作为参考点，以提供电机旋转的基准点。

磁编码器感应器：通常使用霍尔效应传感器或光电传感器，用于检测磁铁的磁场变化，并将其转换为电信号输出。

信号处理器：用于处理磁编码器产生的电信号，以确定电机旋转的位置和角度。

反馈接口：将磁编码器的位置和角度数据反馈给电机控制板，以实现闭环控制。

磁编码器的原理是基于磁场感应原理，利用磁铁和磁编码器感应器之间的磁场变化来确定电机旋转的位置和角度。具体地说，当电机旋转时，磁铁会改变周围的磁场分布，磁编码器感应器会检测到这种变化，并将其转换为电信号输出。信号处理器会将这些输出信号处理成电机旋转的位置和角度数据，并将其反馈给电机控制板。

在四足机器人中，通过使用磁编码器可以实现对腿部电机的精确控制和运动驱动。磁编码器可以用于检测电机的位置、速度和加速度等参数，从而提高机器人的定位精度和运动控制能力。同时，磁编码器还可以用于检测动态负载和防止电机过载，以提高机器人的安全性和稳定性。

二、算法设计简介

2.1 控制算法简介

基础的有电机上运行的FOC，是一种对无刷电机的驱动控制方法，通过精确地控制磁场大小与方向，使得电机的运动转矩平稳、噪声小、效率高，并且具有高速的动态响应。可以让对无刷电机进行“像素级”控制。

图2.1 FOC控制框图

机器狗腿部无刷电机FOC控制算法程序，FOC算法的优劣直接影响机械狗的运动性能，可以说是底层最重要的一环。

图2.2 ZMP原理构型

经典控制算法ZMP算法：ZMP (Zero Moment Point)是一个用于机器人平衡的方法，通过确保经过重心垂直的力矩线在支撑面之上，来保证机器人的稳定性。

图2.3 MPC原理构型

主流控制算法MPC算法：模型预测控制（MPC）是四足机器人中一种普遍采用的控制策略。MPC利用机器人的动力学模型，通过预测和优化目标函数，实时规划最优的步行和运动策略，有效处理多步稳定性和系统约束问题。

2.2  控制算法开发过程

(一)四足机器人建模

图2.4 机器狗仿真模型

图2.5 simulink仿真模块

首先通过Matlab建利机器狗物理模、 simulink模块拖拽、 绑定、 设定参数、附加功能、正弦波输入23腿、反向输入14腿 控制机器测试模型

图2.6 正逆运动学理论与simulink仿真模块

其中利用正逆运动学解算来编写四足机器人Matlab的simulink控制器与规划器：

（1）正运动学解算：

正运动学解算用于将关节角度映射到机器人的末端执行器位置。可以通过机器人的几何模型来实现。在Simulink中，可以使用机器人工具箱（Robotics Toolbox）来进行正运动学计算。根据机器人的几何模型和关节角度输入，输出机器人的末端执行器位置。

（2）逆运动学解算：

逆运动学解算用于根据期望的末端执行器位置来计算关节角度。这对于控制器和规划器来说非常重要，因为可以将期望的末端执行器轨迹转化为关节角度轨迹。逆运动学解算通常涉及求解非线性方程组，可以使用数值方法（例如牛顿-拉夫逊法）或符号计算方法来实现。

（3）控制器设计：

控制器的设计是根据机器人的动力学模型和控制目标来确定的。可以包括位置控制、力/力矩控制、姿态控制等。在Simulink中，使用建模和仿真工具来设计和验证控制器的性能。通过将正逆运动学解算与控制器结合起来，实现机器人的闭环控制。

（4）规划器设计：

规划器用于生成机器人的轨迹，使其能够完成各种任务，例如行走、躲避障碍物等。规划器可以基于运动学或动力学模型进行设计，同时考虑机器人的约束条件。在Simulink中，使用路径规划工具箱（Path Planning Toolbox）来设计和生成机器人的轨迹。规划器通常与逆运动学解算相结合，以生成关节角度轨迹。

理解机器人的几何和动力学模型是至关重要的，这有助于进行正逆运动学解算和控制器设计。

使用合适的数值方法或符号计算方法实现逆运动学解算，并对求解过程进行优化，以提高计算效率和准确性。在设计控制器时，考虑机器人的动力学特性和控制目标，并进行模拟和实验验证，以确保控制器的性能和稳定性。规划器的设计需要考虑机器人的约束条件和任务要求，同时结合逆运动学解算生成关节角度轨迹。采用模块化的设计方法，将正逆运动学解算、控制器和规划器分开实现，并进行集成测试，以确保整个系统的一致性和正确性。

图3.7 Linux环境下Mujoco四足机器人建模

同样在后期在Linux系统Mujoco仿真软件中建立机器狗模型测试算法：

（1）建立机器狗模型：

在Mujoco仿真软件中，使用MJCF文件格式来定义机器狗的模型。指定机器狗的各个部分（例如身体、腿、关节等）的参数，以及之间的连接方式和约束条件。手动编写MJCF文件，也可以使用Mujoco官方提供的模型编辑器（Model Editor）来进行可视化编辑。

（2）编写C算法代码：

可以使用Python语言编写算法代码，并调用Mujoco Python包中的API来实现与Mujoco仿真软件的交互。在VSMC算法中，需要定义神经元网络的结构和参数，并将其与机器狗模型进行耦合，以实现运动控制。

（3）进行仿真测试：

在完成机器狗模型和算法的编写后，使用Mujoco仿真软件进行仿真测试。在Python代码中调用Mujoco Python包中的API来加载机器狗模型、运行算法，并获取仿真结果（例如机器狗的位置、姿态、速度等）。根据仿真结果，评估算法的性能，并进行调试和优化。在Mujoco仿真软件中建立机器狗模型并测试VSMC算法需要一定的编程和仿真经验，但是可以帮助快速验证算法的有效性，并且大大减少了实际物理实验的成本和时间。

（二）四足机器人控制算法开发

建模都是从复杂到简单:RBD->SRDB/CD->ZMP/CoP->LIP，而实际应用中控制器是从简单到复杂:阻抗(无模型)->VMC/Balance QP/MPC(质心动力学模型)->WBC(浮动基座动力学模型+优先级的任务空间控制)

（1）研读四足机器人论文

在编写控制代码前，先研究相关的四足机器人论文，了解其结构、运动学和动力学特性，以及控制的基本思想和实现方法。这将帮助更好地理解控制算法的原理和优缺点，并为后续的代码编写和调试提供指导。

（2）编写控制代码

根据控制的思想和实现方法，使用编程语言（例如MATLAB或Python）编写控制代码。在编写代码时，考虑机器人的动力学模型和控制目标，并设计合适的滑模面和控制律。使用数值方法（例如Euler法或Runge-Kutta法）来进行离散化和数值求解。

（3）进行仿真验证

在完成控制代码编写后，使用仿真软件（MATLAB Simulink或Mujoco）对四足机器人进行仿真验证。需要将机器人的动力学模型和控制代码结合起来，并进行集成测试。根据仿真结果，评估控制算法的性能和稳定性，并进行调试和优化。

（4）实验测试

在完成仿真验证后，将控制代码上传到实际的四足机器人系统中进行实验测试。需要将控制代码与机器人的硬件系统结合起来，并进行实时控制。根据实验结果，可以评估控制算法的实际应用效果，并进行调试和优化。在编写和调试四足机器人控制控制代码时，需要具备一定的机器人学、控制理论和编程经验，同时需要进行充分的仿真验证和实验测试，以确保控制算法的有效性和可靠性。