CN117944055A - 一种人形机器人四肢协同平衡控制方法及装置 - Google Patents

Abstract

本发明涉及机器人自动控制技术领域，尤其涉及一种人形机器人四肢协同平衡控制方法及装置，方法包括采用多源状态信息融合方法，获取人形机器人四肢和躯干的三维运动状态信息；根据三维运动状态信息，建立人形机器人四肢和躯干之间的运动协调模型；根据运动协调模型，建立基于四肢协同的平衡控制策略协调规划四肢运动，所述平衡控制策略包括后掠控制策略、跨越控制策略、虚拟模型控制策略；输出控制指令，将控制指令转换为驱动信号，驱动机器人四肢运动，完成平衡控制，本发明解决了现有方法在建模、反馈和控制策略方面的问题，有效解决现有四肢协同平衡控制存在的问题,实现精确、鲁棒的控制效果。

Description

一种人形机器人四肢协同平衡控制方法及装置

技术领域

本发明涉及机器人自动控制技术领域，尤其涉及一种人形机器人四肢协同平衡控制方法及装置。

背景技术

随着科学技术的发展，机器人在各个领域得到广泛应用，实现了自动化生产、服务工作、家庭助理、医疗护理等多种功能,大大提高了人类的工作效率。其中，人形机器人因其模拟人体结构的特点，具有更强的环境适应性和工作灵活性,是机器人发展的重要方向。但是与人体相比,当前的人形机器人在自主感知、运动控制等方面还存在明显的不足，一个核心的技术难题是如何实现其四肢的协调控制和身体的动态平衡,使其可以稳定自主地执行各种日常动作和运动。

在当前的人形机器人四肢协同平衡控制研究中,已有一些典型的方法。例如基于线性反馈控制的方法,这类方法建立了简化的四肢联合动力学模型,采用比例-积分-微分(PID)等线性控制器,可以实现一定程度的平衡控制。但是由于过于简化的模型忽略了四肢之间的约束关系,使得这种方法很难针对复杂动作实现高精度控制。另外一类方法应用机器学习技术,采用深度神经网络等直接将传感器信息映射到四肢驱动控制,实现端到端的控制策略。这类方法依赖大量数据训练控制模型,但鲁棒性较差。总体来说,现有的控制方法理论基础不够深入,很难对人形机器人四肢协同平衡实现精确、鲁棒的控制效果。

综上所述,现有的人形机器人四肢协同平衡控制方法,存在一些共性的问题,如控制效果不佳、鲁棒性差等。这些问题的根本原因在于：1、现有方法没有深入考虑人体四肢之间的解剖约束关系,建立的动力学模型过于简化,难以描述四肢间的相互影响；2、没有设计出有效的多源传感信息融合方法,依靠单一传感器,使得状态反馈效果较差；3、控制策略较为单一,大多采用传统的PID等线性控制,难以应对机器人在复杂环境中的各种不确定性。因此,现有方法很难实现对人形机器人四肢精确协调控制和身体动态平衡。

发明内容

为了解决现有方法在建模、反馈和控制策略方面的问题,本发明提出了一种新的四肢协同平衡控制方法，该方法在建模上考虑四肢多约束耦合关系,采用多源传感器融合提高反馈质量，并设计混合控制策略即本方法建立了基于四肢关节约束的动力学模型,同时结合视觉、力觉等多源反馈进行状态估计。在控制策略上,融合基于模型的谐波控制、基于学习的稳定控制和基于样本的参考控制等多种控制策略,形成一个层次化、混合的控制框架。从理论分析和仿真实验可见,本方法可以有效解决现有四肢协同平衡控制存在的问题,实现精确、鲁棒的控制效果。

本发明通过以下技术方案实现：

一种人形机器人四肢协同平衡控制方法，包括以下步骤：

S1、采用多源状态信息融合方法，获取人形机器人四肢和躯干的三维运动状态信息；

S2、根据三维运动状态信息，建立人形机器人四肢和躯干之间的运动协调模型；

S3、根据运动协调模型，建立基于四肢协同的平衡控制策略协调规划四肢运动，所述平衡控制策略包括后掠控制策略、跨越控制策略、虚拟模型控制策略；

S4、输出控制指令，将控制指令转换为驱动信号，驱动机器人四肢运动，完成平衡控制。

进一步的，步骤S1中采用视觉传感，通过图像处理算法分析摄像头获取的视频流，检测出四肢和躯干关键部位在三维空间中的位置、运动轨迹和速度变化信息；

采用力觉传感，通过压阻式和电容式传感器相结合，获得各维度的力数据。

进一步的，步骤S2 中采用四肢运动学模型和四肢与躯干动力学模型相结合的协调方法，其中四肢运动学模型分析各肢体的运动规律，描述四肢之间的相互约束关系，四肢与躯干动力学模分析四肢与躯干之间的力学协同作用规律；

所述四肢运动学模型基于 DH 参数方法表示各肢体的运动学链，并在运动链间添加联合约束，四肢运动学模型建立依次包括坐标系定义、DH参数表建立、同相变换矩阵确定、运动学方程构建、运动约束添加；

所述四肢与躯干动力学模型基于拉格朗日方法建立多刚体系统动力学方程，考虑四肢与躯干间的关节力约束，四肢与躯干动力学模型建立依次包括个体刚体动力学建模、刚体间约束确定、拉格朗日运动方程建立。

进一步的，步骤S3中所述后掠控制策略用于调整机器人躯干姿态，根据机器人的运行速度，确定后掠位置比例/>和重心高度/>，产生用于抵消向前惯性力矩的稳定力矩，所述的后掠位置比例/>随运行速度/>的增大而逐步增加；

所述跨越控制策略在后掠控制策略的基础上，进行快速修正，对机器人的选择起步和落地姿态进行优化，使机器人重心保持在支撑多边形内，控制关节角加速度，使机器人平稳过渡；

所述虚拟模型控制策略采用仿真虚拟样本训练控制器参数，在所述虚拟样本控制器中构建优化目标函数并指导样本生成，所述目标函数包括最小化关节加速度平方和。

本发明还提出了一种控制装置，适用于上述人形机器人四肢协同平衡控制方法，包括：检测模块，用于检测人形机器人四肢和躯干的三维运动状态，并生成反馈信息；

建模模块，利用反馈信息建立人形机器人四肢和躯干之间的运动协调模型；

控制模块，包括具有平衡控制策略的平衡控制器，基于运动协调模型和反馈信息对四肢运动进行协调规划，并生成控制指令；

执行模块，将控制指令转换为驱动信号，驱动四肢运动，完成平衡控制。

进一步的，所述检测模块包括视觉传感子系统、力觉传感子系统；

所述视觉传感子系统包括图像采集模块、图像处理模块和第一数据输出模块，所述图像采集模块包括高速摄像机和广角摄像机，所述高速摄像机设置于机器人头部，所述广角摄像机设置于机器人胸部及腰部，所述图像处理模块包括ARM嵌入式平台，所述ARM嵌入式平台内置卷积神经网络算法，用于实时执行人体关键点识别和三维姿态解析，所述第一数据输出模块用于对处理后的数据行编码封装并输出；

所述力觉传感子系统包括传感器信号采集模块、信号调理模块和第二数据输出模块，所述感器信号采集模块包括六轴力矩传感器，所述六轴力矩传感器设置于机器人各关节处，所述信号调理模块包括AD转换芯片，所述第二数据输出模块包括供CAN总线和USB数据接口。

进一步的，所述建模模块用于建立四肢运动学模型和四肢与躯干动力学模型，建立四肢运动学模型采用DH参数方法的运动学链建模，四肢运动学模型建立依次包括坐标系定义、DH参数表建立、同相变换矩阵确定、运动学方程构建、运动约束添加；

所述四肢与躯干动力学模型于拉格朗日方法建立多刚体系统动力学方程，四肢与躯干动力学模型建立依次包括个体刚体动力学建模、刚体间约束确定、拉格朗日运动方程建立。

进一步的，所述控制模块包括后掠控制器，用于根据机器人后掠状态调整躯干姿态；跨越控制器，用于根据跨越动作需求规划整体运动；虚拟模型控制器，用于优化生成四肢关节运动数据，稳定状态判断过程，根据姿态感知结果判定稳定性。

进一步的，所述执行模块包括电机、减速器和编码器，所述电机为无刷直流伺服电机，所述减速器为行星齿轮和谐波齿轮机构，所述编码器为绝对式多圈编码器。

本发明的有益效果：

（1）本发明通过建立人形机器人四肢与躯干的运动协调模型，能够准确描绘机器人运动过程中的力学特征，为平衡控制提供理论依据，模型建立科学合理，控制效果好；

（2）本发明基于多源状态检测和反馈，获得人形机器人实时的运动信息，为模型建立和控制指令生成提供可靠支撑，保证了控制的实时性和有效性；

（3）本发明采用了层级化的控制策略,使平衡控制既稳健又灵活,增强了人形机器人的整体动力学平衡能力和环境适应性；

（4）本发明实现了四肢的协同控制，不再过度依赖双足支撑，拓宽了平衡调节的手段，增强了机器人的灵活性。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提出的一种人形机器人四肢协同平衡控制装置的整体框图；

图2本发明实施例提出的一种人形机器人四肢协同平衡控制装置的检测模块布局示意图；

图3本发明实施例提出的一种人形机器人四肢协同平衡控制装置的四肢运动学模型图；

图4本发明实施例提出的一种人形机器人四肢协同平衡控制装置的四肢与躯干动力学模型图；

图5本发明实施例提出的一种人形机器人四肢协同平衡控制装置的四肢与躯干动力学模型流程图；

图6本发明实施例提出的一种人形机器人四肢协同平衡控制装置的三级控制策略流程图；

图7本发明实施例提出的一种人形机器人四肢协同平衡控制装置的虚拟样策略执行流程步骤；

图8本发明实施例提出的一种人形机器人四肢协同平衡控制装置的控制模块驱动控制流程图；

图9为本发明实施例提出的一种基于云资源监测的资源管理系统的终端设备示意图；

图10为本发明实施例提出的一种基于云资源监测的资源管理系统的的可读存储介质示意图；

图中，200-终端设备、210-存储器、211-RAM、212-高速缓存存储器、213-ROM、214-程序/实用工具、215-程序模块、220-处理器、230-总线、240-外部设备、250-I/O接口、260-网络适配器、300-程序产品。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

实施例1

本实施例提出一种人形机器人四肢协同平衡控制方法步骤1的具体实施方式。

参考图1，一种人形机器人四肢协同平衡控制方法，包括采用多源状态信息融合方法，获取人形机器人四肢和躯干的三维运动状态信息，即采用检测模块中的视觉传感，通过图像处理算法分析摄像头获取的视频流，检测出四肢和躯干关键部位在三维空间中的位置、运动轨迹和速度变化信息，采用检测模块中的力觉传感，通过压阻式和电容式传感器相结合，获得各维度的力数据，所述各维度指：

1、前后维度的力数据:传感器测量各关节在机器人身体前后的方向上受到的接触力或推拉力。

2、左右维度的力数据:传感器测量各关节在机器人身体左右方向上受到的接触力或推拉力。

3、上下维度的力数据:传感器测量各关节在机器人身体上下方向受到的压力或拉力。

4、扭转力矩维度的数据:传感器测量各关节及肢体在轴向旋转方向上受到的扭转力矩。

5、弯曲力矩维度的数据:传感器测量各关节及肢体在前后左右弯曲方向上产生的弯曲力矩。

即通过检测上述不同方向和形式的力数据,获得了各维度的力信息,从而全方位地监测到了各关节和肢体受力的情况,为建模控制提供丰富的力数据反馈。

具体的，检测模块采用视觉传感子系统与力觉传感子系统两部分进行信息融合,实现对人形机器人四肢及躯干的三维运动状态检测，视觉传感子系统包括图像采集模块、图像处理模块和第一数据输出模块，所述的图像采集模块包括两台高速度摄像机与三台广角摄像机，高速摄像机安装于机器人头部进行双目视觉捕捉，广角摄像机设置于机器人躯干胸部和腰部位置，摄像机采集频率至少200Hz，传感分辨率不低于2K，通过GigE接口形成图像采集网络，图像处理模块使用ARM架构的嵌入式处理平台，安装有卷积神经网络算法模块，图像处理模块的算法流程包括:人体检测和分割、特征点提取、多视角三维重建、运动状态解析，采用目标跟踪机制，确保在复杂环境中仍可维持高效稳定的功能输出，算法模块采用Tensorflow框架并进行模型压缩部署，第一数据输出模块收集处理结果,进行编码和封装后高速输出。

力觉传感子系统包括传感器信号采集模块、信号调理模块和第二数据输出模块，其中传感器信号采集模块采用分布式压阻传感器阵列,设置于机器人各关节，用于测量运动力矩、接触力和扭力信息，分布式压阻传感器阵列中使用的六轴力传感元件，传感分辨率达到0.1N，采样频率200Hz，单元厚度2mm，对各方向力敏感，传感范围为-20~20N,分辨力为0.1N，信号调理模块使用24位AD转换芯片，保证采样精度，信号调理模块设计多通道同步采样电路,降低采样频率误差，输出数据格式为标准CAN报文帧,并提供原始数据的USB接口,实现采集与输出的灵活配置，第二数据输出模块提供CAN总线和USB两种数据接口，综上所述,检测模块的视觉和力觉子系统相结合,可以获得精确全面的状态监测信息,为后续的建模控制提供基础。

本实施例还提出了图像处理模块的算法流程，包括以下步骤：

1、图像预处理，进行图像去噪、畸变校正等预处理，提高图像质量；

2、目标检测，使用深度学习目标检测算法，实时检测各关节和标志点；

3、特征点提取，检测出的关键点进行角点优化，提取明显特征；

4、多视角三维重建，采用类复眼算法，根据不同视角两两匹配计算三维坐标；

5、运动状态解析，计算目标关键点的运动轨迹和速度信息。

通过上述处理流程,实现稳定的三维关键点定位和状态解析，输出控制系统所需的状态信息。

基于上述流程本实施例中提出类复眼算法为复眼多视角三维重建算法：

本实施例中如设置安装了个摄像头，每个摄像头的内参矩阵为/>外参矩阵为/>，其中

，对于任意一点/>在不同摄像头下的坐标即为：,其中/>表示在第/>个摄像头坐标系下的坐标，通过立体校准,可以得到不同摄像头坐标系之间的转换关系：/>，其中/>表示/>个坐标系到第/>个坐标系的转换矩阵，则同一点在两摄像头下的坐标满足：/>，基于特征点匹配可以在不同视角下找到点/>的对应特征点/>,可以恢复点/>的三维坐标：/>。

其中内参矩阵表示摄像头的内在参数,反映了摄像头的几何特性，包含焦距、主点坐标以及像素比例尺寸等信息。

外参矩阵表示摄像头的外在参数，反映了摄像头相对于世界坐标系的位置和方向，确定了从世界坐标转换到摄像头坐标的变换关系。

通过内参和外参的组合，可以表示任意空间点在摄像头画面上的坐标/>的计算公式。

立体校准的目的是计算不同摄像头外参矩阵之间的转换关系，即矩阵，故可以统一到同一坐标系下进行处理。

利用对应点在不同视角下的坐标约束,以及内参、外参的已知信息,可以通过解方程组求解出点/>的三维坐标，这样就实现了多视图角度下二维点的匹配对应关系，并恢复出三维空间信息，通过处理每个特征点，就可以重建整个三维场景，具体的三维重建坐标变换计算公式：

，/>，其中/>表示不同摄像头的平面坐标系，/>表示目标点在不同视角下的像素坐标，/>表示不同摄像头的内参矩阵，表示不同摄像头的外参矩阵，/>表示目标点在世界坐标系下的三维坐标，通过解上述矩阵方程组，可以恢复目标点/>的三维空间坐标，需要明确的是，本实施例在三维重建的坐标变换计算部分中，/>坐标与/>坐标表示的是相同的概念，都指目标点在世界坐标系下的三维空间坐标。

具体的，公式中的表示不同摄像头的像平面坐标系，/>表示目标点在不同摄像头画面中的二维像素坐标，公式通过同时考虑两个摄像头的内参矩阵/>以及外参矩阵/>建立了从像素坐标/>到目标点世界坐标/>的映射关系。其中，最后求解出的/>就是目标点/>在世界坐标系下的三维空间坐标,表示该点的空间位置。因此三维重建中的/>和本实施例中提到的/>，本质上表示的是同一个意思,指的是一个空间点相对于世界坐标系的三维坐标值，这是经过视觉传感的三维重建后才能求解出来的目标点的坐标信息。

本实施例还提出了力觉传感的信号处理流程，包括以下步骤：

1、 传感矩阵构建，根据传感器分布，建立映射矩阵确定各传感值对应的关节位置；

2、 基准校准，使用标准值进行基准校准，消除传感器固有偏差；

3、 加权平均，考虑传感器敏感度差异,采用加权融合传感矩阵数据；

4、 滤波平滑，使用低通滤波器进行平滑处理,去除噪声；

5、 数据格式化，对处理后的力数据进行标准格式封装和输出。

基于上述流程本实施例中提出了一种应用于压阻传感器阵列的调理算法：

本实施例中压阻式传感器阵列共有个传感单元，每个传感单元在时刻/>的输出为/>，本实施例中考虑到每个传感单元的灵敏度不同，因此定义一个灵敏度系数为/>，为降低环境噪声的影响，对每个传感单元的输出添加一个时间相关性滤波模块，表达式为:/>，其中/>表示时间平滑系数，定义压阻传感阵列的整体输出为各单元输出的加权平均，/>表示对第/>个传感单元在时刻/>的输出信号进行时间相关滤波后的输出结果，/>表示第/>个传感单元在时刻/>的输出信号；权重系数为其灵敏度系数：/>，最后为消除个体差异的影响，引入一个基准传感单元，进行差分运算：/>，其中/>表示对整体输出信号进行基准校准后的最终输出，/>表示基准传感单元的输出，/>表示整个传感阵列在时刻/>的加权平均后的整体输出，/>表示/>个传感单元的灵敏度系数,反映其对压力变化的响应灵敏度，/>表示表示对第/>个传感单元在时刻/>的输出信号进行时间相关滤波后的输出，/>表示对各个传感单元的输出信号进行灵敏度加权的分子项，/>表示对各个传感单元灵敏度系数进行求和的分母项，用于归一化。

具体的，压阻式传感器阵列包含多个传感单元，每个传感单元输出一个与压力变化相关的电阻值，本方案中通过信号调理模块对传感器阵列的输出进行处理以提高信号质量，因此设计了考虑每个传感单元灵敏度的加权平均模块，给灵敏度高的单元更高权重,可以提升有用信号分量，此外还对信号进行平滑滤波，减少噪声干扰，新增了时间相关递归滤波器模块，为消除传感器个体差异带来的误差,控制系统引入了基准单元校准模块,可以有效提升检测精度，故经过以上信号调理模块，可以获得质量更好的传感阵列输出信号，供装置后续使用。

实施例2

本实施例提出一种人形机器人四肢协同平衡控制方法步骤2的具体实施方式。

一种人形机器人四肢协同平衡控制方法，包括根据三维运动状态信息，建立人形机器人四肢和躯干之间的运动协调模型；本实施例中采用四肢运动学模型和四肢与躯干动力学模型相结合的协调方法，其中四肢运动学模型分析各肢体的运动规律，描述四肢之间的相互约束关系，四肢与躯干动力学模分析四肢与躯干之间的力学协同作用规律；

所述四肢运动学模型基于 DH 参数方法表示各肢体的运动学链，并在运动链间添加联合约束，四肢运动学模型建立依次包括坐标系定义、DH参数表建立、同相变换矩阵确定、运动学方程构建、运动约束添加，具体的：

1、坐标系定义：（1）坐标系定义，为每个刚体定义机器人学中的标准DH坐标系，所述的坐标系遵循右手规则，如对于躯干刚体，在其质心处建立坐标系S定义S系下的x轴指向前方，y轴指向左侧，z轴指向上方；（2） DH参数表编制，根据人体骨骼结构和刚体联结，编制DH参数表，表中包含刚体的链接长度、刚体的链接扭角/>、关节的偏移/>以及关节角/>等信息，上述参数描述了相邻刚体之间的位置和方向关系；（3）同相变换矩阵，针对每个刚体间的联结，利用DH参数编写同相变换矩阵,矩阵形式为:

；其中/>表示绕 z 轴的旋转矩阵，角度为/>，这个矩阵用于描述关节角度的旋转；/>表示沿 z 轴的平移矩阵，距离为/>，这个矩阵用于描述关节的偏移；/>表示沿 x 轴的平移矩阵，距离为，这个矩阵用于描述相邻链接的长度；/>表示绕 x 轴的旋转矩阵，角度为/>，这个矩阵用于描述关节角度的旋转，因此上述矩阵反映刚体间坐标系的转换关系。

2、DH参数表建立：为准确描述人体四肢的骨骼结构及刚体联结关系，需要建立DH参数表。表中的参数根据人体解剖特征量化获得。（1）链接长度即相邻刚体联结点之间的距离，需要测量人体骨骼的长度并记录，如上臂刚体与躯干刚体的联结点为肩关节，则需要测量人体上臂骨的长度作为链接长度参数；（2）链接扭角/>表示相邻刚体在联结点上的相对扭转角度，需要根据人体骨骼的方位关系测定角度值。如大腿骨与髋骨的相对角度即为这两刚体链接的扭角；（3）关节偏移/>即刚体坐标系原点相对于前一坐标系原点的位移量，需要测量得到坐标系原点的空间位置关系，如上臂坐标系原点相对于躯干坐标系的位移就是关节偏移量；（4）关节角/>即相邻刚体间运动的关节变量，这是刚体运动学链中定义的自变量。

本实施例中在建模时将上述参数表示为符号量，在运动学分析时由具体的数值确定，需要为运动学链中的每个刚体确定上述参数，并记录到DH参数表中，为下一步建立运动学模型奠定基础。

3、同相变换矩阵确定，根据建立的DH参数表，可以通过DH变换方法确定相邻刚体之间的同相变换矩阵，同相变换矩阵表示不同坐标系之间的转换关系，本实施例中将两个相邻刚体坐标系定义为A系和B系，则刚体B相对于A的同相变换矩阵可以表示为:

；通过矩阵乘法可以进行多个变换的叠加，从而建立出从基坐标系到末端坐标系的全部变换过程。如对于包含基坐标系A、中间坐标系B和末端坐标系C的三刚体运动学链，三者之间的同相变换矩阵为:

；通过递归矩阵相乘，可以表示出刚体C相对于刚体A的运动学关系，其中/>表示从坐标系 B 到坐标系 C 的同相变换矩阵，描述了从 B 到 C 的坐标系之间的转换关系；/>表示从坐标系C 到坐标系B 的同相变换矩阵，描述了从 C到 B 的坐标系之间的转换关系；/>表示从坐标系A 到坐标系B 的同相变换矩阵，描述了从 A 到 B 的坐标系之间的转换关系，在这个方程中/>和/>表示相邻两个坐标系之间的变换关系，而/>表示通过两个变换/>和/>组合得到的总的变换关系。

4、运动学方程构建，确定了刚体之间的同相变换矩阵后，可以通过对矩阵求解，获取末端执行器在基坐标系下的运动学方程，需要明确的是末端执行器在本实施例中指机器人四肢末端的手掌或足端，对于运动学链末端的位置方程，可以表示为:

；

其中表示从基坐标系到末端坐标系的同相变换矩阵，/>表示末端在工具坐标系下的常量位置向量，通过展开矩阵乘法，可以得到一个仅以关节变量/>作为变量的三维向量方程，表示末端在基坐标系下的位姿，因此类似可以求出末端效器的速度方程和加速度方程:

；

；

其中表示末端速度，/>为雅可比矩阵，包含对/>的导数信息，/>表示关节角度，表示加速度，/>表示关节角速度，通过运动学分析求解，建立了一个包含前向运动学和反向运动学的完整的四肢运动学模型，该模型为四肢的运动规划与控制提供了基础。

5、运动约束添加，基于第4步在建立了每个肢体的运动学链模型后，需要在运动学链之间添加约束条件，用于描述不同肢体之间的运动关系，主要包括以下约束:

（1）位置约束表示不同刚体之间的固定距离或方位关系，如躯干与大腿的相对位置保持不变；

（2）速度约束表示不同刚体运动速度存在线性关系，如左右腿运动速度相等；

（3）加速度约束表示不同刚体间加速度满足约束，如手臂运动加速度不能超过规定阈值。

上述约束条件将各肢体的运动学模型联系起来，形成一个统一的全身运动学模型，约束使模型更准确地描述人体四肢的协同运动规律，至此完成了基于DH建模方法的人体四肢运动学模型的建立。

参考图3，本实施例还提出了一种基于述四肢运动学模型的建模模块，具体的所述建模模块包括：

1、姿态输入模块，用于接收来自检测模块的四肢和躯干的运动状态检测数据，输出当前时刻的关节角数据作为输入。

2、正运动学模块，基于输入的当前关节角数据，运用正运动学建模方法求解手掌或足端在基坐标系下的位置和姿态。

3、雅可比矩阵模块，用于构建雅可比矩阵，确定正运动学求解结果对输入关节角的导数关系，描述运动学属性。

4、运动学约束添加模块，在肢体间添加位置、速度等运动学约束条件,建立系统运动学模型。

5、逆运动学模块，基于期望的手掌足端运动轨迹,确定各关节的目标角度和变化量,完成逆运动学求解。

上述模块按顺序流程执行，实现人体四肢的运动学建模。其中正逆运动学相互配合，通过不断迭代求解优化模型，检测数据输入与关节角输出形成闭环。

参考图4-图5，所述四肢与躯干动力学模型基于拉格朗日方法建立多刚体系统动力学方程，考虑四肢与躯干间的关节力约束，四肢与躯干动力学模型建立依次包括个体刚体动力学建模、刚体间约束确定、拉格朗日运动方程建立，具体的：

1、个体刚体动力学建模，四肢与躯干动力学模型的建立首先对人体中的每个刚体进行单独建模，主要包括:（1）参数确定，根据人体的解剖学特征，测量每个刚体的几何形状、质量分布等参数，如刚体的总质量m可以通过实际测量确定，测量刚体的质心位置xyz和惯性张量Ixx、Iyy、Izz等，为建立动力学方程奠定基础；（2）质点动力学方程建立，根据牛顿第二定律，可以建立描述刚体运动的微分方程:，其中/>是作用在刚体上的外力和力矩，/>是刚体的质量，/>是刚体的质心加速度，通过确定质量和加速度，可以描述刚体的运动状态；（3）建立质心动力学方程，利用质心运动定理，得到表示刚体质心运动的向量微分方程:

这里是作用在质心的合外力，/>是角动量，/>是刚体的角速度，/>表示刚体的质量，为一个标量用来描述刚体的惯性，/>表示刚体质心的速度矢量，即刚体的线性运动速度,用来描述刚体的平动状态，该方程建立了力和力矩与运动之间的关系；（4）完整刚体动力学方程，综合上述方程可以建立表达刚体运动的完整微分方程组，包含线加速度、角加速度等刚体运动状态信息。

（5）反馈调节，收集人体运动数据，检测模型输出结果与实测结果的误差，设计增量PID调节器，以位置误差作为输入，更新模型刚体参数，使仿真输出结果逼近实际情况。

2、刚体间约束确定，在对每个刚体建立了单体动力学模型后，需要确定刚体之间的约束关系，主要包括:（1）位置约束，根据人体刚体的联结方式，确定其之间的位置约束方程。如躯干和大腿之间的相对位置满足一个矢量方程；（2）运动约束，根据人体刚体运动时的连续性，给出刚体间速度和加速度的约束方程。如相邻刚体的接触点速度相等；（3）力约束，根据牛顿第三定律，相互作用力在不同刚体间满足反作用力关系，确定作用力和反作用力之间的约束；（4）转动约束，刚体之间通常存在转动约束，使其运动受到一定限制，确定刚体间的转动自由度及其约束关系。通过建立刚体间的各种约束关系，可以将单个刚体动力学模型联系起来，描述复杂刚体系统的运动规律，为建立系统动力学方程奠定基础。

3、 拉格朗日运动方程建立，为建立四肢与躯干的动力学模型，本实施例采用拉格朗日方法得到运动方程，包括：（1）定义广义坐标，将刚体系统的位置和运动参数化定义为广义坐标，如可以选择刚体的关节角度作为广义坐标；（2）确定运动学能量，计算刚体系统的运动学能量包括齐性能量和势能，表达为广义坐标/>的函数；（3）确定势函数，根据刚体之间的约束关系，确定势函数/>，该函数反映约束引起的势能变化；（4）建立拉格朗日方程，将刚体系统的拉格朗日函数/>定义为/>，并应用拉格朗日运动方程，建立出描述刚体系统运动的一组微分方程，其中/>表示刚体系统动能；（5）求解方程组，对运动方程组进行符号化求解，可以得到表达刚体系统运动的解析，本实施例中也可以采用数值方法求解。

通过拉格朗日方法可以系统地建立多刚体系统的动力学模型，为仿真和控制应用提供理论基础。

本实施例还提出了一种用于验证所建立的四肢与躯干动力学模型正确性的方法，即采用虚拟样机仿真的方法进行验证: 为验证所建立的四肢与躯干动力学模型的正确性，可以采用虚拟样机仿真的方法进行验证:

（1）建立多体系统虚拟样机，根据人体四肢和躯干的几何结构、刚体参数等在计算机上建立虚拟的多刚体系统。

（2）导入动力学模型，将符号化的动力学方程组导入虚拟样机作为其运动学制约。

（3）添加外界激励，向虚拟样机施加类似实际情况下的外力或力矩等激励输入。

（4）启动仿真计算，运行虚拟样机的仿真计算，根据方程数值求解计算系统的动力学响应。

（5）结果分析，分析虚拟样机的运动结果，比对实际人体运动情况，验证模型的正确性。

（6）模型修正，如果存在偏差，对模型方程进行修正，直到仿真结果能够准确符合实际情况。

本实施例中提出的虚拟样机仿真验证了四肢与躯干动力学模型的建立过程。

实施例3

本实施例提出一种人形机器人四肢协同平衡控制方法步骤3的具体实施方式。

一种人形机器人四肢协同平衡控制方法，包括根据运动协调模型，建立基于四肢协同的平衡控制策略协调规划四肢运动，所述平衡控制策略包括后掠控制策略、跨越控制策略、虚拟模型控制策略，所述后掠控制策略用于调整机器人躯干姿态，根据机器人的运行速度，确定后掠位置比例/>和重心高度/>，产生用于抵消向前惯性力矩的稳定力矩，所述的后掠位置比例/>随运行速度/>的增大而逐步增加；

所述跨越控制策略在后掠控制策略的基础上，进行快速修正，对机器人的选择起步和落地姿态进行优化，使机器人重心保持在支撑多边形内，控制关节角加速度，使机器人平稳过渡；

所述虚拟模型控制策略采用仿真虚拟样本训练控制器参数，在所述虚拟样本控制器中构建优化目标函数并指导样本生成，所述目标函数包括最小化关节加速度平方和。

参考图6，本实施例在运动规划算法方面，设计了三级控制策略，包括后掠和跨越规划用于环境感知及运动控制，虚拟样本轨迹生成用于认知决策，使机器人既能灵活适应环境，又保证四肢协调一致。本实施例中集成了后掠控制、跨越控制、虚拟样本控制等策略，针对稳定、灵活性、协调性等方面进行了优化，采用自适应PD控制及增量控制手段提高了控制鲁棒性，具体的：

1、后掠控制策略，当机器人以一定的前进速度进行奔跑或跳跃运动时，由于身体受到向前的惯性力的作用，会产生一个前倾的力矩，如果不能很好地抵消这个力矩，机器人的身体就会发生前扑现象而失去平衡，为了防止这种情况发生，后掠控制的方法是调整身体重心在支撑多边形范围内的前后位置，使重心保持一定的后倾状态，即通过增加重心在支撑多边形后的位置比例，并适当提高重心高度/>，从而在重心和支撑多边形之间产生一个后倾的力矩，这个后倾力矩可以抵消前进过程中身体受到的向前倾倒的惯性力矩，保持身体的平衡稳定。随着前进速度的增加，惯性力矩也会变大，这时需要继续增加/>值，使重心倾斜幅度变大，以产生足够的后掠力矩。确定合理的K值和H值是后掠控制的关键。

为获得良好控制效果，需要明确的是后掠控制的关键参数和/>需要根据运动速度合理设定：在较低速度下，取较小的/>和/>，如速度2m/s时，取/>，随着速度提高，逐步增大/>和/>的值，当速度达到3.5m/s时，取/>。通过速度自适应调节/>和/>，后掠控制可以适应不同运动状态，持续提供足够的稳定力矩。

依据上述方法，本实施例还提出了一种后掠控制器，后掠控制器是四肢层级控制的基础模块之一，主要解决快速运行时身体保持平衡的控制问题。

后掠控制器主要包括：

（1）控制目标，使机器人在高速奔跑时保持平稳的身体态势，避免因前后摆动而翻倒；

（2）控制原理，通过调整身体重心在支撑多边形内的相对位置，使身体产生后倾抵消向前的平移力矩，保持平衡稳定。

（3）控制参数，关键参数为重心相对支撑多边形的前后位置比例，以及重心高度/>。

（4）模块实现，采用PID控制算法，根据速度反馈信号调节值，并联合逆运动学模块控制四肢运动实现重心调节。

依据上述说明，本实施例提出了一种后掠控制器的设计方案，后掠控制器主要由后掠参数计算单元、PID控制单元和四肢控制单元组成。

后掠参数计算单元会根据机器人的运动速度，计算出合适的重心位置比例和高度/>，作为后掠控制的目标参数。

PID控制单元采用增量PI控制算法，以反馈的实时重心位置和后掠目标参数为输入，计算出重心位置的增量控制量，使重心位置逐步接近目标参数，起到稳定控制作用。

四肢控制单元根据PID输出的重心位置增量控制量，结合预设的步态，运用逆运动学模型，计算每个关节的运动参数，并产生控制四肢执行机构的驱动信号，从而改变重心在支撑多边形内的相对位置。

后掠控制器的运作流程为:根据速度计算后掠参数，再PID控制器稳定控制重心到目标位置，最后通过四肢控制单元完成重心位置的调节，实现整体后掠控制效果。

本实施例中需要明确的是PID算法参数确定，在后掠控制的PID控制单元中，确定合理的参数对实现控制效果至关重要，在后掠控制的PID参数选择中，积分时间Ti取较大值0.5s，以保证控制平稳性；积分饱和取较大值50防止抖动；增益Kp取较小值50避免震荡，因此参数的确定衡量了响应速度与平稳性。后续将采用在线自适应调整，收集性能数据，采用extremum seeking方法优化参数，逼近理想控制效果。其中，积分时间Ti为积分时间越大，响应越缓慢，考虑到后掠控制追踪速度不需要太快，可以选择较大的Ti，例如0.5s，使控制平稳；比例增益Kp 为Kp越大，响应越快，但过大会引起震荡，可选取较小的Kp，如50，防止震荡；微分时间Td为Td可采用默认值，例如0.05s；积分饱和值为可以设置较大积分饱和值，如50，防止抖动；量程限制为根据重心位置变化范围合理设置控制量限幅，防止越限，针对重心位置控制的特点，PID参数可定在上述范围。

本实施中后掠控制器的四肢控制单元利用逆运动学算法，根据PID输出的重心调节量，计算每个关节的运动参数，实现重心定位。

先建立人体骨骼关节的运动学模型，采用DH 参数表示。

根据期望的重心调节量，确定各关节角度的增量范围，再应用逆运动学算法，求解每个关节的目标角度，形成关节空间目标，将关节目标转换为电机控制量，驱动执行机构完成运动，采用封闭链逆运动学算法，确保关节运动协调一致，保证重心控制的连续性和平稳性。

2、跨越控制策略，跨越控制的核心目标是使机器人像人一样能够跨过地面上的各种障碍继续行走或者奔跑，该策略的关键步骤包括:

（1）使用安装在机器人上的视觉或其他传感器，检测环境中可能出现的障碍物，判断障碍物的位置尺寸等参数，本实施例中可采用检测模块提供的传感器进行检测；

（2）根据自身状态快速规划多个可选的跨越路径，选择最优路径；

（3）通过虚拟仿真生成一个恰当的跨越步态，使机器人能够平稳跨过障碍；

（4）控制机器人的四肢运动系统执行步态，同时利用控制算法稳定身体，完成整个跨越过程。

本实施例中为确保跨越过程的动态稳定性，优化选择关键参数为：起步姿态取躯干后倾15°，双腿屈曲40°，使重心稳定在支撑多边形内；落地姿态取躯干前倾10°，双腿伸展20°，缓冲着陆；关节角加速度控制在20rad/s^2以内，使运动平稳过渡，需要说明的是上述这些参数的选择需要综合考虑动力学约束，通过仿真验证。

依据上述方法，本实施例还提出了一种跨越控制器，跨越控制是四肢层级控制的高级模块之一，主要解决机器人跨过的自主避障问题。

跨越控制器主要包括：

（1）控制目标，检测环境中的障碍物，规划可选的跨越路径，控制四肢运动实现平稳跨越；

（2）关键技术，障碍物检测、路径规划、步态生成、稳定控制；

（3）模块构成，传感器检测单元、路径规划单元、虚拟样本单元、四肢控制单元；

（4）控制参数，关键参数有起步姿态、落地姿态、关节角加速度等；

（5）实施路径，先检测计划，后虚拟仿真，最后四肢跨越运动；

依据上述说明，本实施例提出了一种跨越控制器的设计方案，跨越控制模块由传感检测、路径规划、虚拟样本生成、四肢控制和状态监测子模块组成。

传感检测子模块使用安装在机器人上的摄像头等传感器设备来获取环境中障碍物的位置和大小等信息，以支持后续决策;路径规划子模块则根据检测结果，结合自身状态快速规划出多个可选的跨越路径，并选择风险最小的最优路径；

虚拟样本生成子模块基于选定路径设计合适的跨越步态，通过迭代优化生成平稳的虚拟步态样本；

四肢控制子模块使用虚拟样本控制机器人四肢执行跨越步态，同时辅助模块提供实时的平衡控制；

状态监测子模块实时反馈机器人的姿态等信息，闭环控制跨越过程的稳定性，整个模块组合充分利用控制理论与仿真算法，使机器人智能感知并跨過障碍。

本实施例中采用：

（1）传感检测单元:使用摄像头、激光雷达等传感器检测环境中的障碍物，获取其位置、尺寸等信息；

（2）路径规划单元:根据检测到的障碍信息，结合机器人自身状态，利用算法快速规划出多个可行的跨越路径；

（3）虚拟样本生成单元:选取最优路径，基于动力学模型，通过迭代优化生成平稳的跨越步态作为控制的虚拟样本；

（4）四肢控制单元:根据虚拟样本，利用爬坡法计算关节控制输入，驱动机器人四肢执行跨越步态。同时辅助模块提供实时平衡控制；

（5）状态监测单元:监测机器人在跨越过程中的姿态角速度等状态信息，提供反馈进行闭环控制。

需要明确的是，在跨越控制中，路径规划和虚拟样本生成是实现智能跨越的关键，本实施例中路径规划采用A*或RRT等算法，将检测到的障碍物边界作为约束，快速搜索出多条满足运动学限制的可选跨越路径，评估路径长度、能耗等因素，选择全局最优解作为跨越路径。在选定跨越路径基础上，采用遍历优化算法生成跨越的虚拟步态样本，根据路径长度和起落时间，确定单支撑、双支撑等步态阶段。在每个支撑阶段，调整 COP、关节角等参数，基于动力学模型进行多次迭代仿真，使步态满足动力学稳定性约束，关键控制参数包括步态时间分配、关节角速度、冲击力等。调节参数使机器人在动力学仿真中能够平稳完成整个虚拟跨越过程。最终确定时空连贯且动态稳定的虚拟样本，包括各关节角度、步态时间等信息，作为四肢控制的输入，驱动机器人完成实际跨越运动。路径规划与虚拟样本生成实现了跨越控制策略的智能生成。

本实施例中当虚拟样本生成单元输出代表完整跨越运动的关节角度、步态时间等数据后，四肢控制系统就可以根据这些数据驱动机器人的四肢及关节进行实际的跨越运动，本实施例中四肢控制系统采用了典型的层级控制结构，上层控制器主要完成基于虚拟样本的数据的关节角度规划，通过爬坡法等算法计算出每个关节在不同时刻的角度目标，然后输出这些关节运动的控制量；下层控制系统接受上层输出的关节控制量，通过PID算法设计出一个关节轨迹跟踪控制器，以比较快的响应速度同时避免产生震荡，从而精确地控制每个关节按照规划的角度曲线进行运动，实现整体的四肢跨越运动轨迹；与此同时，下层控制系统还要直接向每个关节的电机发送当前所需的电流值，以精确实现伺服电机的驱动控制，此外为了处理跨越运动中可能出现的各种外部未知扰动，四肢控制系统中还设置了辅助控制模块，使用安装在关键部位的IMU单元、压力传感器等设备获取重要的状态反馈信息，包括机器人的姿态角、各关节力矩等，根据这些反馈实时调整关节控制，实现足端力控制、姿态控制等算法，从而保证机器人在整个跨越过程中能维持稳定的着地和良好的动态平衡能力。综合利用规划算法、控制技术和精确驱动，四肢控制系统使机器人得以稳定执行虚拟步态，最终实现对复杂环境障碍的智能跨越。

3、虚拟样本策略，虚拟样本控制是通过仿真软件生成描述完整运动的虚拟样本数据，在实际控制中使用这些数据作为四肢控制的参考输入，以指导机器人的四肢系统稳定地跟踪样本运动，相比简单的轨迹跟踪，虚拟样本包含全身各关节在整个运动过程中的角度、速度等时域信息，能够提供连续平滑的控制参考，虚拟样本的生成利用优化算法及运动学约束，使其满足动力学稳定条件，在四肢控制执行中，采用两层控制架构，上层进行虚拟样本跟踪以输出关节控制量，下层完成精确的关节伺服控制，同时辅助控制模块根据重心等状态反馈进行参数调节，保证运动的动态稳定性，相比依赖简单PID的直接轨迹跟踪，虚拟样本控制可获得更精细、连续且动态稳定的控制效果。

虚拟样本生成需要以优化目标函数为指导，本实施中采用最小化关节加速度平方和为目标，定义目标函数为，即最小化全部关节的加速度平方和，针对该目标函数，采用爬山法逐步调整样本中各关节曲线，使加速度平方和不断减小，通过迭代得到平稳的最优虚拟样本。所生成的样本满足了最小加速度平方和的优化目标。

参考图7，本实施例中提出了虚拟样本执行流程步骤:

（1）检测模块采用多源传感器融合方法，组合视觉、力觉和IMU传感器，实时检测人形机器人四肢和躯干的三维空间状态，生成精确的反馈信息，为虚拟样本控制提供基础；

（2）建模模块利用DH参数方法建立人形机器人四肢和躯干的正逆运动学模型，模型包含刚体运动学链的表达、运动学方程的构建等内容、准确描述四肢运动规律；

（3）虚拟样本生成模块首先定义优化目标函数，本实施例采用最小化关节加速度平方和为目标，基于运动学模型,使用迭代优化方法如爬山法搜索最优参数解，生成平稳的虚拟样本数据。样本数据包括各关节的角度、角速度、启停时间等运动信息。

（4）控制模块使用两层控制架构，上层控制采用爬坡法实现虚拟样本数据的跟踪，输出关节控制量，下层控制则利用PD算法计算关节力矩，驱动执行机构完成精确的运动跟踪同时辅助控制调节重心，保证整体动态稳定；

（5）执行模块使用电流闭环控制驱动各执行关节，完成虚拟样本描绘的四肢协同运动，形成从反馈检测到关节驱动的闭环控制；

通过上述模块组合使用虚拟样本控制策略，使四肢系统实现精确、连续、动态稳定的协同运动控制。

4、稳定状态判断，稳定状态判断过程具体包括:

1、构建inverted pendulum模型；

2、测量重心位置和关节角状态；

3、判断重心是否超出边界，输出稳定状态。

实施例4

本实施例在实施例3的基础上提出一种基于三种控制策略的层级控制方式。

本实施例提出了一种应用于四肢层级控制中的不同控制策略，不同层级采用了后掠控制、跨越控制和虚拟样本控制这三种关键控制策略，其中:

后掠控制属于低级稳定控制，主要实现在动力学层和运动学层，通过调节重心实现身体稳定控制。

跨越控制属于高级自主适应控制，主要实现在运动层，可以根据环境自主调整步态实现跨越运动。

虚拟样本控制属于最高级的协调规划控制，根据预设运动样本协调各关节运动，实现整体协调控制效果。

三个控制策略在不同层级发挥作用，相互配合协同工作，共同提升四肢的运动控制能力。

四肢控制的层级控制策略采用自底向上的方式，先设计关节PID或PD控制形成动力学层确保关节稳定跟踪，其次运动学层根据模型规划关节空间轨迹生成关节驱动量，再由运动层根据任务分析计划端点优化轨迹并转换到关节驱动，最高层进行身体协调控制实现多关节协同，每层控制通过定义明确的接口和信息交互形式实现控制任务的层级分解与协调控制，从而简化每一层的控制逻辑，并通过层间信息交互协调每层控制量输出，使复杂机器人的控制问题得以分而治之和有效解决。

具体的包括以下控制方式：

1、动力学层控制，在动力学层控制中，结合后掠控制器的重心调节方法，根据重心反馈信号微调关节PD控制器参数，优化电流控制环与速度控制环，提高重心控制的响应速度和稳定性。

（1） 动力学层控制的目的是实现对每个关节及执行器的低级控制，确保关节运动可精确跟踪位置、速度指令。

（2）每个关节配备光电编码器获取关节角度，磁性编码器获取关节转速，采用PID控制算法作为闭环控制器。

（3）PID控制器的输入为角度指令和反馈角度。输出为电流控制量。控制周期采样时间为1ms。

（4）PID参数采用自整定方法，在线标定每个关节的比例、积分、微分系数。采用抗积分饱和、抗微分饱和等措施提高鲁棒性。

（5）关节执行器为直流伺服电机与减速器组成，配置电流环PI控制器实现电流闭环，使电机产生需求转矩。

（6）考虑关节动力学因素，加入粘性、摩擦补偿等非线性控制量，改善系统鲁棒性。

（7）电机转矩经减速器传递到输出轴，带动关节运动，完成低级精确跟踪控制。

（8）采样关节角速度作为速度反馈信号，实现关节运动的速度控制回路。

（9）经调试，控制器达到零稳态误差，关节运动平稳无震荡。满足上层控制对运动跟踪精度需求。

2、运动学层控制，在运动学层控制中，根据后掠控制的重心调节目标，修正关节空间规划的轨迹，实现重心控制与关节控制的协同;并融合跨越控制的步态调整，使关节运动适应不同环境。

首先，建立完整的四肢运动学模型，采用DH的参数化方法描述各关节的坐标系，明确定义每个关节的DH参数，包括连杆长度、连杆扭角等，并列出整体的DH变换矩阵方程。在此基础上，通过运动学正逆解的导数运算，推导出运动学的解析解表达式，同时采用符号化数学工具，将解析解转化为程序代码，以保证求解过程的精度和计算速度。然后，根据四肢运动控制的具体任务，规划端点在笛卡尔空间的期望轨迹，采用多项式、B样条曲线等表达式描述连续光滑的端点轨迹曲线。轨迹曲线会考虑运动过程中的各种几何约束和奇异性要求。此外，还需要设计轨迹的速度规划、加速度规划，确保轨迹在时间域上也满足连续性需求。随后，使用轨迹规划算法，如最小关节加速度法等，将笛卡尔空间的期望端点轨迹映射到关节空间，进行关节运动轨迹的规划优化，使运动过程关节的加速度最小化，运动平滑协调。规划过程需要充分考虑各关节的运动范围约束。规划运算周期为5ms，满足实时控制需求。最后，将优化后的关节空间轨迹转换为连续的关节角度和关节转速规划曲线，作为低级控制的输入，并采用PD闭环控制算法，利用关节反馈实现控制跟踪，补偿模型误差，保证四肢端点精确遵循预定笛卡尔空间轨迹，满足运动控制的精度要求。

3、运动层控制，运动层控制的目标是在更高的层次上，根据四肢运动控制的具体任务需求，规划出能够满足任务约束条件和优化目标的端点运动轨迹和状态。首先，需要进行任务分解，明确端点在笛卡尔空间中需要实现的关键状态，如终点位置姿态、过程中需要满足的距离或力量限制等。然后，针对具体任务类型，选择合适的规划算法，采用数学优化的方法求解笛卡尔空间中端点满足约束条件下的最优轨迹，如最小化能耗、最小化终点误差等优化目标。规划算法可以采用随机搜索、粒子群等智能优化算法，并结合四肢运动特点设计目标函数和约束条件。在规划得到端点轨迹后，需要进行笛卡尔空间到关节空间的映射，将端点状态转化为关节角度、速度、加速度等空间的驱动量。利用逆运动学模型的映射关系，为处理映射过程中的奇异性，可以采用避障规划或基于自主切换的多个逆运动学解。最后，将规划得到的关节驱动指令，作为下层运动学控制的输入。并采用闭环控制确保关节驱动精确跟踪指令。运动层控制的算法确保了从更高层次的任务分解与规划，到底层的关节驱动映射的正确性与优化性。

在运动层控制中，虚拟样本控制为各关节生成运动规划提供参考，跨越控制根据环境自主调整虚拟样本，两者实现协同;后掠控制的重心反馈也参与规划，三者相互配合推动任务完成。

4、协调控制，在四肢运动控制系统中，需要有效协调各个关节和自由度，实现整体运动的协同性。因此在最高控制层级设置协调控制层。这层控制的目标是在复杂运动中确保身体的整体稳定性和各关节的协调性。具体的，协调控制层需要实时监测全身的质心状态，预测质心相对支持多边形的位置变化，以及计算全身的零时力动量，利用模型预测方法检测可能出现的不稳状态。当有不稳迹象时，需要自动规划身体姿态调整，发出四肢微调的关节调整指令，保持整体平衡稳定。此外，在复杂运动中可能出现多个关节的运动目标发生冲突的情况。协调控制层需要管理各关节的任务优先级和资源协调，处理好不同关节控制量之间的权衡关系。例如调整部分关节运动速度或加速度，允许另一关键关节完成对准动作，实现控制目标的协调优化。通过协调控制层的整体规划与协调，不同关节和自由度之间的相互制约关系能够得到充分考虑，使复杂四肢运动控制系统以协同的方式工作，完成复合任务

5、接口与信息交互，在四肢控制的分层架构中，不同控制层级之间需要明确定义信息交互的接口，以实现控制量的传递与协调。物理接口可采用串行总线或以太网等industry 4.0 工业互联网技术，以实现高速稳定的数据交流。每一控制层级需要规范定义自己的输入输出接口。如动力学层的输入是上层输出的关节角度、角速度控制量，输出是关节的实际反馈角度。运动学层的输入是来自上层的笛卡尔位置、速度目标值，输出是映射得到的关节空间轨迹。以此类推，每层接口需严格定义，形成标准化的层间信息交互协议。 同时，不同层级之间可以根据需要，选择同步传输或异步传输的信息交互方式。同步传输可保证控制时效性，异步方式则可以提升鲁棒性，防止反馈数据的泛洪效应。此外，层间交互需要考虑控制周期和数据配时的匹配关系，处理好上下层的信息同步问题。通过定义清晰标准化的接口与信息交互机制，四肢控制的分层架构可以实现模块解耦，使各层级内部控制逻辑得到简化，并促进各层级之间的协同运作，从而有效实现复杂控制任务的分解与协调。

实施例5

本实施例提出一种人形机器人四肢协同平衡控制方法步骤4的具体实施方式。

输出控制指令，将控制指令转换为驱动信号，驱动机器人四肢运动，完成平衡控制，四肢机器人的执行模块是实现精确力控与运动控制的关键，所述的执行模块包括驱动单元，驱动单元由电机、减速器、编码器三个核心部件组成。

具体的，电机的选择直接影响到驱动性能，本实施例中选用最大功率在200-500W范围内的高性能无刷直流伺服电机，电机电流给定值由外环PID控制器输出，采用矢量控制策略，实现电流环的精确控制，电机产生所需转矩。考虑到四肢运动的高速高加速特点，电机定子和转子都采用低惯性设计，如选用稀土永磁材料，抑制风格效应，减小转子惯性。

减速器放大电机功率，增大驱动关节的输出转矩，本实施例中减速器采用小型化的行星齿轮或谐波齿轮机构，传动效率高，满足轻量化需求。选择减速比在50-100的较大减速比，将高转速低扭矩的电机转矩放大为低速大扭矩，以驱动关节运动。减速器采用一体化设计，使得驱动单元结构紧凑。

编码器实现高精度的运动控制与反馈，采用绝对式多圈编码器，精度达到0.01度，通过专用接口采集编码器数据，实现对关节角度和转速的精确反馈，闭环控制系统依赖编码器实现对关节运动的准确跟踪。编码器选用磁性设计，不接触转动部件，避免机械磨损。

执行模块还包括控制系统，其采用快速实时控制器，包括基于FPGA的控制器，并采用内环加外环的双闭环控制，通过精确控制电机电流实现准确的转矩控制，再由速度环调节电流给定实现准确的速度跟踪性能。控制周期达到1ms，确保动态跟踪效果。

驱动单元的机械结构设计紧凑、轻量化，直接安装在关节转动轴上，设计紧凑的接口，使电机、减速器、编码器高度集成。输出轴与四肢骨骼连接，产生大扭矩直接驱动关节运动。

驱动单元之间以及与上层控制器采用工业以太网EtherCAT等通信协议，确保控制指令和反馈数据传输的实时性与稳定性。

为实现机器人四肢的高力矩、高速度和高精度运动控制，驱动系统选用直流伺服电机作为执行机构。本实施例中对机器人四肢选用电机参数如下：

（1）腿部驱动选择Maxon RE50 200W 有刷直流电机，选用数字位置式PID闭环控制器EPOS4进行电流环和速度环联合闭环控制，采用串联编码器HEDL5540进行高精度位置和速度反馈，减速比设定为1:30。

（2）手部驱动选择Maxon EC90 减速直流电机，选用EPOS4数字运动控制器，采用编码器HEDS55进行反馈，减速比设定为1:10。

（3）电机通过遥测技术与主控制器连接，采用CAN总线进行通信控制。主控制器根据规划运动学指令下达电流、速度指令给各关节电机控制器。

（4）关节控制器内部采用双闭环控制，外部速度环由主控制器给出速度指令并根据编码器反馈进行PI控制，内部电流环进行电流闭环调节实现高精度电机力矩控制。

参考图7，驱动控制流程:接收规划运动学指令->根据指令生成关节速度PROFILE->速度闭环控制输出电流指令->电流环PI控制电机->读取编码器反馈速度和位置->反馈给速度环和位置规划。

本实施例通过选配电机、编码器、减速器，结合高性能控制策略与通信机制，四肢的驱动单元实现了快速响应、大扭矩输出、高动态跟踪的综合性能指标，满足四肢精确运动对驱动性能的苛刻要求。

实施例6

如图9，在实施例1的基础上，本实施例提出一种人形机器人四肢协同平衡控制装置终端设备，终端设备200包括至少一个存储器210、至少一个处理器220以及连接不同平台系统的总线230。

存储器210可以包括易失性存储器形式的可读介质，例如RAM211和/或高速缓存存储器212，还可以进一步包括ROM213。

其中，存储器210还存储有计算机程序，计算机程序可以被处理器220执行，使得处理器220执行本申请实施例中上述任一项一种用于山区无人机遥感图像几何畸变校正应用，其具体实现方式与上述应用的实施例中记载的实施方式、所达到的技术效果一致，部分内容不再赘述。存储器210还可以包括具有一组(至少一个)程序模块215的程序/实用工具214，这样的程序模块包括但不限于：操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。

相应的，处理器220可以执行上述计算机程序，以及可以执行程序/实用工具214。

总线230可以为表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器、外围总线、图形加速端口、处理器或者使用多种总线结构中的任意总线结构的局域总线。

终端设备200也可以与一个或多个外部设备240例如键盘、指向设备、蓝牙设备等通信，还可与一个或者多个能够与该终端设备200交互的设备通信，和/或与使得该终端设备200能与一个或多个其它计算设备进行通信的任何设备(例如路由器、调制解调器等等)通信。这种通信可以通过I/O接口250进行。并且，终端设备200还可以通过网络适配器260与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。网络适配器260可以通过总线230与终端设备200的其它模块通信。应当明白，尽管图中未示出，可以结合终端设备200使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理器、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储平台等。

实施例7

如图10，本实施例提出一种人形机器人四肢协同平衡控制装置的可读存储介质，所述计算机可读存储介质上存储有指令，该指令被处理器执行时实现上述任一一种人形机器人四肢协同平衡控制装置其具体实现方式与上述应用的实施例中记载的实施方式、所达到的技术效果一致，部分内容不再赘述。

图10示出了本实施例提供的用于实现上述应用的程序产品300，其可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码，并可以在终端设备，例如个人电脑上运行。然而，本发明的程序产品300不限于此，在本实施例中，可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。程序产品300可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

计算机可读存储介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了可读程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。可读存储介质还可以是可读存储介质以外的任何可读介质，该可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。可读存储介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、有线、光缆、RF等等，或者上述的任意合适的组合。可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的程序代码，程序设计语言包括面向对象的程序设计语言诸如Java、C++等，还包括常规的过程式程序设计语言诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中，远程计算设备可以通过任意种类的网络，包括局域网(LAN)或广域网(WAN)，连接到用户计算设备，或者，可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (9)

1.一种人形机器人四肢协同平衡控制方法，其特征在于，包括以下步骤：

S1、采用多源状态信息融合方法，获取人形机器人四肢和躯干的三维运动状态信息；

S2、根据三维运动状态信息，建立人形机器人四肢和躯干之间的运动协调模型；

S3、根据运动协调模型，建立基于四肢协同的平衡控制策略协调规划四肢运动，所述平衡控制策略包括后掠控制策略、跨越控制策略、虚拟模型控制策略；

S4、输出控制指令，将控制指令转换为驱动信号，驱动机器人四肢运动，完成平衡控制。

2.根据权利要求1所述的一种人形机器人四肢协同平衡控制方法，其特征在于，步骤S1中采用视觉传感，通过图像处理算法分析摄像头获取的视频流，检测出四肢和躯干关键部位在三维空间中的位置、运动轨迹和速度变化信息；

采用力觉传感，通过压阻式和电容式传感器相结合，获得各维度的力数据。

3.根据权利要求1所述的一种人形机器人四肢协同平衡控制方法，其特征在于，步骤S2中采用四肢运动学模型和四肢与躯干动力学模型相结合的协调方法，其中四肢运动学模型分析各肢体的运动规律，描述四肢之间的相互约束关系，四肢与躯干动力学模分析四肢与躯干之间的力学协同作用规律；

所述四肢运动学模型基于 DH 参数方法表示各肢体的运动学链，并在运动链间添加联合约束，四肢运动学模型建立依次包括坐标系定义、DH参数表建立、同相变换矩阵确定、运动学方程构建、运动约束添加；

所述四肢与躯干动力学模型基于拉格朗日方法建立多刚体系统动力学方程，考虑四肢与躯干间的关节力约束，四肢与躯干动力学模型建立依次包括个体刚体动力学建模、刚体间约束确定、拉格朗日运动方程建立。

4.根据权利要求1所述的一种人形机器人四肢协同平衡控制方法，其特征在于，步骤S3中所述后掠控制策略用于调整机器人躯干姿态，根据机器人的运行速度，确定后掠位置比例/>和重心高度/>，产生用于抵消向前惯性力矩的稳定力矩，所述的后掠位置比例/>随运行速度/>的增大而逐步增加；

所述跨越控制策略在后掠控制策略的基础上，进行快速修正，对机器人的选择起步和落地姿态进行优化，使机器人重心保持在支撑多边形内，控制关节角加速度，使机器人平稳过渡；

所述虚拟模型控制策略采用仿真虚拟样本训练控制器参数，在所述虚拟样本控制器中构建优化目标函数并指导样本生成，所述目标函数包括最小化关节加速度平方和。

5.一种控制装置，其特征在于，适用于权利要求1-4任一项所述的人形机器人四肢协同平衡控制方法，包括：检测模块，用于检测人形机器人四肢和躯干的三维运动状态，并生成反馈信息；

建模模块，利用反馈信息建立人形机器人四肢和躯干之间的运动协调模型；

控制模块，包括具有平衡控制策略的平衡控制器，基于运动协调模型和反馈信息对四肢运动进行协调规划，并生成控制指令；

执行模块，将控制指令转换为驱动信号，驱动四肢运动，完成平衡控制。

6.根据权利要求5所述的一种控制装置，其特征在于，所述检测模块包括视觉传感子系统、力觉传感子系统；

所述视觉传感子系统包括图像采集模块、图像处理模块和第一数据输出模块，所述图像采集模块包括高速摄像机和广角摄像机，所述高速摄像机设置于机器人头部，所述广角摄像机设置于机器人胸部及腰部，所述图像处理模块包括ARM嵌入式平台，所述ARM嵌入式平台内置卷积神经网络算法，用于实时执行人体关键点识别和三维姿态解析，所述第一数据输出模块用于对处理后的数据行编码封装并输出；

所述力觉传感子系统包括传感器信号采集模块、信号调理模块和第二数据输出模块，所述感器信号采集模块包括六轴力矩传感器，所述六轴力矩传感器设置于机器人各关节处，所述信号调理模块包括AD转换芯片，所述第二数据输出模块包括供CAN总线和USB数据接口。

7.根据权利要求5所述的一种控制装置，其特征在于，所述建模模块用于建立四肢运动学模型和四肢与躯干动力学模型，建立四肢运动学模型采用DH参数方法的运动学链建模，四肢运动学模型建立依次包括坐标系定义、DH参数表建立、同相变换矩阵确定、运动学方程构建、运动约束添加；

所述四肢与躯干动力学模型于拉格朗日方法建立多刚体系统动力学方程，四肢与躯干动力学模型建立依次包括个体刚体动力学建模、刚体间约束确定、拉格朗日运动方程建立。

8.根据权利要求5所述的一种控制装置，其特征在于，所述控制模块包括后掠控制器，用于根据机器人后掠状态调整躯干姿态；跨越控制器，用于根据跨越动作需求规划整体运动；虚拟模型控制器，用于优化生成四肢关节运动数据，稳定状态判断过程，根据姿态感知结果判定稳定性。

9.根据权利要求5所述的一种控制装置，其特征在于，所述执行模块包括电机、减速器和编码器，所述电机为无刷直流伺服电机，所述减速器为行星齿轮和谐波齿轮机构，所述编码器为绝对式多圈编码器。