一种实时交互式四足机器人多步态动力学建模方法及系统与流程

本发明涉及机器人动力学建模技术领域，具体涉及一种实时交互式四足机器人多步态动力学建模方法及系统。

背景技术：

机器人动力学建模方法为手动推导计算以及采用动力学软件求解，前者对于简单的机器人系统可行，但对于复杂的机器人系统，构建的动力学方程很复杂，很难获得正确的解析解；采用动力学软件建模相对容易。目前四足机器人动力学建模主要采用adams、recurdyn等动力学软件建模以及ode等一些开源的动力学引擎，前者建模方法虽然精度高，但是计算很复杂，很难保证控制的实时性，因此很难匹配实际样机的控制参数矫正要求，并且可拓展性较差，不便于二次开发；后者虽然可拓展性较强，但动力学高计算精确度与高实时性兼备的不够突出。

技术实现要素：

为解决上述现有技术中的不足，本发明的目的是提供一种实时交互式四足机器人多步态动力学建模方法及系统，基于vortex动力学引擎进行动力学实时解算，充分利用vortex高精确度、高实时交互、高拓展性的优质建模特性，解算周期采用系统时钟频率实现精确定时，基于udp(userdatagramprotocol)协议通过客户端-服务端通信的方式实时向图形引擎任务传输机器人的状态信息，进行平台可视化分析，完成多步态模型嵌入的动力学系统的建模。

本发明的目的是采用下述技术方案实现的：

本发明提供一种实时交互式四足机器人多步态动力学建模方法，其改进之处在于：

构建四足机器人的多步态动力学系统、四足机器人和地形的图形可视化模型；

所述多步态动力学系统与所述四足机器人和地形的图形可视化模型之间采用多线程通信进行信息传输。

进一步地：所述构建四足机器人的多步态动力学系统，包括：

构建四足机器人单腿模型；

基于vortex搭建四足机器人的动力学模型及地形动力学模型；

建立关节控制器，执行多步态控制模型产生的期望动力学行为。

进一步地：所述构建四足机器人单腿模型包括：

构建单腿运动学模型，包括腿部正运动学与逆运动学；

根据单腿运动学模型，计算腿部关节力雅可比矩阵j；

采用拉格朗日法构建无外力作用下的关节动力学模型；

将足端力向关节映射的力矩融入关节动力学模型；

优选的，所述腿部正运动学公式为：

腿部逆运动学公式为：

θ1＝atan2(py,px)

腿部正运动学是获得足部坐标系相对于髋关节坐标系的齐次变换矩阵t，足部坐标系指关节旋转轴z4所在的坐标系，髋关节坐标系指的关节旋转轴z1所在的坐标系；

腿部逆运动学是由足部相对髋关节坐标系的位置p＝(px,py,pz)求解关节角度；

式中：ai表示腿部关节连杆长度，si＝sinθi，ci＝cosθi，sij＝sin(θi+θj)，cij＝cos(θi+θj),θi表示第i关节的关节角度，i表示关节的序号，i＝1、2、3；kfh为前后腿关于角度符号的配置因子，前腿对应的kfh＝1，后腿对应的kfh＝-1；a1表示腿部关节自上而下连杆1的长度,a2表示腿部关节连杆2的长度,a3表示腿部关节连杆3的长度；c1＝cosθ1,s1＝sinθ1,c23＝cos(θ2+θ3),s23＝sin(θ2+θ3)；θ1、θ2、θ3分别表示第1、2、3关节的关节角度；px,py,pz分别表示足部相对髋关节坐标系x、y、z轴对应的位置；

优选的，所述关节动力学模型为：

足端力向关节映射的力矩融入关节动力学模型为：

τf＝j^tf

其中：q为关节角度，m为关节质量矩阵，c为哥氏力矩阵，g为重力矩阵，τ为关节驱动力矩，f为足端力，τf为足端力向关节映射的力矩；分别表示关节角加速度、关节角速度。

进一步地：所述基于vortex搭建四足机器人的动力学模型及地形动力学模型，包括：

基于四足机器人三维实体模型，参照三维实体模型的结构尺寸及质心、质量，并赋予质量、转动惯量和材料属性，赋予四足机器人动力学模型的动力学属性；

四足机器人动力学模型的杆件之间采用铰接约束，足部与膝关节采用弹簧约束，根据杆件质量、质心和转动惯量信息，结合杆件间的运动关系，构建连接副约束关系，基于地形模型动力学参数，建立典型的台阶、斜坡工况，赋予地形材料属性，并针对不同的工况简化为相应的碰撞几何体，同时将机器人足部简化为胶囊碰撞体，与地形进行碰撞检测，形成四足机器人和地形可实时交互的动力学系统。

进一步地：所述建立关节控制器，执行多步态控制模型产生的期望动力学行为，包括：

采用计算机时钟频率保证动力学解算的实时性，用于精确定时，并采用多线程技术进行通信；

在多步态动力学系统采用基于c++面向对象方式的多步态控制算法；

构建关节控制器，执行多步态动力学系统产生的机器人动作；

优选的，所述构建关节控制器为：

其中：关节控制器表示根据关节的位置和前馈力信息构建关节的驱动信号，θd为由单腿逆运动学模型计算期望的关节角度，θ为由动力学模型实时采集关节的位置信号，利用vortex动力学引擎实时迭代解算获得实际关节角度；

根据动力学系统采样频率，和分别表示实时计算关节的期望速度与实际速度，kp为关节控制器比例因子；kv为关节控制器微分因子，将关节解耦进行独立控制，根据腿部各个关节的动力学模型，以及关节的期望位置与实际位置，构建关节pd控制器；τff为期望足端力fd向关节的映射前馈力矩，表达式为τff＝-j^tfd，j为关节力雅可比矩阵，u为控制器输入；

优选的，所述关节pd控制器为：

式中：μpd表示关节pd控制器输入。

进一步地：所述构建动力学行为可再现的四足机器人和地形的图形可视化模型，包括：

基于机器人三维实体模型，采用vortex内置的图形引擎模块，构建四足机器人的图形模型；

基于地形三维实体模型，采用vortex内置的图形引擎模块，构建地形的图形模型；

在四足机器人与地形的图形可视化任务中运用多线程技术，并采用客户-服务端的方式与四足机器人与地形的动力学模型实时通信，将动力学实时解算的四足机器人与地形交互的动力学行为通过客户-服务端的方式传送至图形引擎模块中，动力学行为可再现的四足机器人和地形的图形可视化模型。

进一步地：所述多步态动力学系统与所述四足机器人和地形的图形可视化模型之间采用多线程通信进行信息传输，包括：

将多步态动力学系统作为客户端，在多线程技术中构建基于udp协议的客户端套接字；

将四足机器人与地形的图形可视化模型作为服务端，在多线程技术中构建基于udp协议的服务端套接字；

多步态动力学系统与四足机器人和地形的图形可视化模型之间的信息传输。

一种实时交互式四足机器人多步态动力学的建模系统，其改进之处在于，所述系统包括：

构建模块，用于构建四足机器人的多步态动力学系统、四足机器人和地形的图形可视化模型；

信息传输模块，用于所述多步态动力学系统与所述四足机器人和地形的图形可视化模型之间采用多线程通信进行信息传输。

进一步地，所述构建模块，包括：

第一构建子模块，用于构建四足机器人的多步态动力学系统；

第二构建子模块，用于构建四足机器人和地形的图形可视化模型；

优选的，第一构建子模块，进一步包括：

第一构建子单元，用于构建四足机器人单腿模型；

第二构建子单元，用于基于vortex搭建四足机器人的动力学模型及地形动力学模型；

第三构建子单元，用于建立关节控制器，执行多步态控制模型产生的期望动力学行为；

优选的，第二构建子模块，进一步包括：

第四构建子单元，用于基于机器人三维实体模型，采用vortex内置的图形引擎模块，构建四足机器人的图形模型；

第五构建子单元，用于基于地形三维实体模型，采用vortex内置的图形引擎模块，构建地形的图形模型；

第六构建子单元，用于在四足机器人与地形的图形可视化任务中运用多线程技术，并采用客户-服务端的方式与四足机器人与地形的动力学模型实时通信，将动力学实时解算的四足机器人与地形交互的动力学行为通过客户-服务端的方式传送至图形引擎模块中，形成动力学行为可再现的四足机器人和地形的图形可视化模型；

优选的，信息传输模块，包括：

客户端模块：用于将多步态动力学系统作为客户端，在多线程技术中构建基于udp协议的客户端套接字；

服务端模块，用于将四足机器人与地形图形可视化模型作为服务端，在多线程技术中构建基于udp协议的服务端套接字；

通信模块，用于多步态动力学系统与四足机器人和地形的图形可视化模型之间的信息传输。

进一步地，所述第一构建子单元，还用于：

确定腿部正运动学与逆运动学；

根据单腿运动学模型，计算腿部关节力雅可比矩阵；

采用拉格朗日法构建无外力作用下的关节动力学模型；

将足端力向关节映射的力矩融入关节动力学模型；

优选的：第二构建子单元，还用于：

基于四足机器人三维实体模型，参照三维实体模型的结构尺寸及质心、质量，并赋予质量、转动惯量和材料属性，赋予四足机器人动力学模型的动力学属性；

四足机器人动力学模型的杆件之间采用铰接约束，足部与膝关节采用弹簧约束，根据杆件质量、质心和转动惯量信息，结合杆件间的运动关系，构建连接副约束关系，基于地形模型动力学参数，建立典型的台阶、斜坡工况，赋予地形材料属性，并针对不同的工况简化为相应的碰撞几何体，同时将机器人足部简化为胶囊碰撞体，与地形进行碰撞检测，形成四足机器人和地形可实时交互的动力学系统；

优选的，第三构建子单元，还用于：

采用计算机时钟频率保证动力学解算的实时性，用于精确定时，并采用多线程技术进行通信；

在多步态动力学系统采用基于c++面向对象方式的多步态控制算法；

构建关节控制器，执行多步态动力学系统产生的机器人动作。

与最接近的现有技术相比，本发明提供的技术方案具有的有益效果是：

(1)采用计算机时钟频率的定时方式，定时精度高，多步态控制算法控制周期与采样周期得到保证采用多线程技术实现客户端与服务端的通信，减少内存占用，通信可靠性高；通过客户端与服务端的实时通信，实现四足机器人与地形实时交互的可视化，完成多步态模型嵌入的动力学系统的建模，并且人员可借助数据接口实现对机器人的操控，人机交互性强，且易于诊断信息故障。

(2)通过融合关节pd控制及力矩前馈控制，建立关节的柔顺性控制器，四足机器人足部与地面接触时具有较优的柔顺性，可适用于不同的步态，通用性强。

(3)基于vortex的动力学引擎进行开发，实时性高，动力学解算精度高；

(4)采用c++面向对象的方式建模，可拓展性强，便于二次开发。

附图说明

图1是本发明提供的四足机器人单腿模型示意图；

图2是本发明提供的四足机器人与地形动力学建模流程图；

图3是本发明提供的四足机器人与地形图形模型可视化示意图；

图4是本发明提供的多步态模型与动力学的嵌入框图；

图5是本发明提供的实时交互式四足机器人多步态动力学的建模系统的结构框图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步的详细说明。

以下描述和附图充分地示出本发明的具体实施方案，以使本领域的技术人员能够实践它们。其他实施方案可以包括结构的、逻辑的、电气的、过程的以及其他的改变。实施例仅代表可能的变化。除非明确要求，否则单独的组件和功能是可选的，并且操作的顺序可以变化。一些实施方案的部分和特征可以被包括在或替换其他实施方案的部分和特征。本发明的实施方案的范围包括权利要求书的整个范围，以及权利要求书的所有可获得的等同物。在本文中，本发明的这些实施方案可以被单独地或总地用术语“发明”来表示，这仅仅是为了方便，并且如果事实上公开了超过一个的发明，不是要自动地限制该应用的范围为任何单个发明或发明构思。

实施例一、

本发明提供一种面向对象的实时交互式四足机器人多步态动力学建模方法，包括动力学系统的构建、多步态模型的嵌入、多步态期望动力学行为的执行、动力学行为的可视化，多步态控制模型产生期望的行走动作，基于单腿模型，利用vortex内置的碰撞检测模块实时动力学解算迭代获得关节角度、力，在此基础上构建关节控制器驱动，从而实现四足机器人期望的动力学行为，将产生的动力学行为在图形模型中再现，并可称为具有动力学行为的四足机器人虚拟样机。

s1、构建四足机器人的多步态动力学系统、四足机器人和地形的图形可视化模型；所述步骤s1进一步包括：

步骤1，构建四足机器人单腿模型；

步骤2，基于vortex搭建四足机器人与地形动力学模型；

s2、所述多步态动力学系统与所述四足机器人和地形的图形可视化模型之间采用多线程通信进行信息传输。

具体步骤内容如下：

如图1所示，步骤1，构建四足机器人单腿模型。具体包括：

步骤11：构建单腿运动学模型，包括腿部正运动学与逆运动学，通过构建单腿的坐标系，采用坐标齐次变换的方法，并根据腿的结构尺寸、几何关系以及髋关节的布局，得到足端相对于髋关节的齐次变换矩阵及足端位置的逆解。

腿部正运动学是获得足部坐标系相对于髋关节坐标系的齐次变换矩阵t，足部坐标系指的如图1中的关节旋转轴z4所在的坐标系，髋关节坐标系指的如图1中的关节旋转轴z1所在的坐标系，

公式中，ai表示腿部关节连杆长度，si＝sinθi，ci＝cosθi，sij＝sin(θi+θj)，cij＝cos(θi+θj),θi表示第i关节的关节角度，i表示关节的序号，i＝1、2、3；kfh为前后腿关于角度符号的配置因子，前腿对应的kfh＝1，后腿对应的kfh＝-1；a1表示腿部关节自上而下连杆1的长度,a2表示腿部关节连杆2的长度,a3表示腿部关节连杆3的长度；c1＝cosθ1,s1＝sinθ1,c23＝cos(θ2+θ3),s23＝sin(θ2+θ3)；θ1、θ2、θ3分别表示第1、2、3关节的关节角度；px,py,pz分别表示足部相对髋关节坐标系x、y、z轴对应的位置；

腿部逆运动学是由足部相对髋关节坐标系的位置p＝(px,py,pz)求解关节角度，求解结果如下：

θ1＝atan2(py,px)

公式中，kfh为前后腿关于角度符号的配置因子，前腿对应的kfh＝1，后腿对应的kfh＝-1。

步骤12：根据腿部运动学关系，求解腿部关节力雅可比矩阵j。

腿部关节力雅可比矩阵为足部基于髋关节坐标系描述的力/速度与关节力/速度的映射关系，求解结果：

步骤13：根据关节角度q，关节质量矩阵m，哥氏力矩阵c，重力矩阵g，关节驱动力矩τ，采用拉格朗日法构建无外力作用下的关节拉格朗日动力学方程。

步骤14：考虑足端力f向关节空间的映射关系，将足端力向关节映射的力矩τf融入关节动力学模型。

τf＝j^tf(3)

如图2、图3所示，步骤2，基于vortex搭建四足机器人与地形动力学模型。vortex为多刚体动力学引擎，而且是基于c++面向对象的动力学建模工具，可进行多自由度机器人的多刚体动力学建模及地形建模，具有高精度、高实时、高拓展性的优质特性，四足机器人动力学建模相当于构建具有动力学特性的虚拟样机，以取代实际样机，其关节实际的力和位置可由vortex内置的碰撞检测模块实时动力学解算迭代完成，要完成四足机器人虚拟样机的多种步态的行走建模，需将多步态控制模型嵌入基于vortex二次开发的四足机器人动力学系统中，并将多步态控制模型转化为关节驱动信号，完成虚拟样机的行走驱动，为保证动力学系统的实时性，将四足机器人动力学建模、多步态模型、关节驱动通过多线程技术构建实时任务(客户端)，为清晰分析、诊断动力学建模的有效性，基于vortex内置的图形模块，采用多线程技术构建地形及机器人图形可视化分析任务(服务端)，通过udp通信实时将动力学信号传输至图形模块，实现四足机器人的多步态动力学行为在图形可视化任务中的再现，形成完整的四足机器人多步态动力学系统。

具体包括：

步骤21：基于现有的四足机器人三维实体模型，参照实体的结构尺寸及质心、质量，做一些简化处理，并赋予质量、转动惯量、材料等属性，赋予四足机器人仿真模型的动力学属性。

步骤22：杆件之间采用铰接约束，足部与膝关节采用弹簧约束，根据杆件质量、质心、转动惯量等信息，结合杆件间的运动关系，构建连接副约束关系，实现四足机器人机构物理模型与图形模型的建模。

步骤23：根据工况需求构建典型的行走工况，通过构建台阶、斜坡等典型工况物理模型与图形模型，进而测试多种步态控制算法的地形适应性。

步骤24：四足机器人与地形的物理模型与图形模型之间采用客户-服务端的方式进行实时通信，将动力学实时解算的机器人的行为通过客户-服务端的方式传送至图形引擎中实现图形可视化。

图4为多步态模型与动力学的嵌入框图，包括多步态模型的嵌入、关节控制器的构建、多线程技术的引入、控制周期与采样周期的精确时钟定时，以及多线程任务中的客户端-服务端的通信。具体包括以下步骤：

步骤31：采用计算机时钟频率实现精确定时保证控制周期与采样周期。

步骤32：在客户端与服务端均引入多线程技术，减少对主线程任务内存的占用，以保证通信的可靠性。

步骤33：以c++面向对象的方式将几种典型的步态控制算法融入动力学系统，构成多步态动力学系统。

步骤34：构建关节控制器，具体为：由单腿逆运动学模型，求解期望的关节角度θd，并由关节位置传感器实时采集关节的位置信号θ，并根据系统采样频率，实时求解关节的期望速度与实际速度引入关节控制器比例因子kp、微分因子kv，将关节解耦进行独立控制，根据腿部各个关节的动力学模型，以及关节的期望位置与实际位置，构建关节pd控制器，利用关节力雅可比矩阵j，将期望足端力fd向关节的映射力矩τff＝-j^tfd作为关节前馈力，构建关节控制器，控制器输入为u，实现关节位置/力的控制，进而实现足部的位置/力控制。

所述构建关节控制器为：

其中：关节控制器表示根据关节的位置和力信息构建关节的驱动信号，θd为由单腿逆运动学模型计算期望的关节角度，θ为由动力学模型实时采集关节的位置信号，利用vortex动力学引擎实时迭代解算获得实际关节角度；

优选的，所述关节pd控制器为：

上述四步为执行动力学解算(动力学模型)与再现动力学行为(图形模型)的必要工作，第一步是保证动力学解算的实时性，第二步保证客户端与服务端通信任务的构建，第三步是利用多步态控制模型产生期望的机器人动作，第四步是执行多步态动力学系统产生的机器人动作，前两步是动力学模型及图形模型的构建框架，是并行关系，后两步是动力学模型及图形模型的多步态动作执行，是顺序关系。

步骤4，基于udp协议在多线程任务中构建客户端-服务端的通信，完成动力学系统与图形可视化之间的信息传输。具体包括：

步骤41：将多步态动力学系统作为客户端，在多线程任务中构建基于udp协议的客户端套接字。

步骤42：将四足机器人与地形图形可视化任务作为服务端，在多线程任务中构建基于udp协议的服务端套接字。

实施例二、

基于同样的发明构思，本发明还提供一种实时交互式四足机器人多步态动力学的建模系统，其结构图如图5所示，包括：

构建模块，用于构建四足机器人的多步态动力学系统、四足机器人和地形的图形可视化模型；

信息传输模块，用于所述多步态动力学系统与所述四足机器人和地形的图形可视化模型之间采用多线程通信进行信息传输。

进一步地，所述构建模块，包括：

第一构建子模块，用于构建四足机器人的多步态动力学系统；

第二构建子模块，用于构建四足机器人和地形的图形可视化模型；

优选的，第一构建子模块，进一步包括：

第一构建子单元，用于构建四足机器人单腿模型；

第二构建子单元，用于基于vortex搭建四足机器人的动力学模型及地形动力学模型；

第三构建子单元，用于建立关节控制器，执行多步态控制模型产生的期望动力学行为；

优选的，第二构建子模块，进一步包括：

第四构建子单元，用于基于机器人三维实体模型，采用vortex内置的图形引擎模块，构建四足机器人的图形模型；

第五构建子单元，用于基于地形三维实体模型，采用vortex内置的图形引擎模块，构建地形的图形模型；

第六构建子单元，用于在四足机器人与地形的图形可视化任务中运用多线程技术，并采用客户-服务端的方式与四足机器人与地形的动力学模型实时通信，将动力学实时解算的四足机器人与地形交互的动力学行为通过客户-服务端的方式传送至图形引擎模块中，形成动力学行为可再现的四足机器人和地形的图形可视化模型；

优选的，信息传输模块，包括：

客户端模块：用于将多步态动力学系统作为客户端，在多线程技术中构建基于udp协议的客户端套接字；

服务端模块，用于将四足机器人与地形图形可视化模型作为服务端，在多线程技术中构建基于udp协议的服务端套接字；

通信模块，用于多步态动力学系统与四足机器人和地形的图形可视化模型之间的信息传输。

进一步地，所述第一构建子单元，还用于：

确定腿部正运动学与逆运动学；

根据单腿运动学模型，计算腿部关节力雅可比矩阵；

采用拉格朗日法构建无外力作用下的关节动力学模型；

将足端力向关节映射的力矩融入关节动力学模型；

优选的：第二构建子单元，还用于：

基于四足机器人三维实体模型，参照三维实体模型的结构尺寸及质心、质量，并赋予质量、转动惯量和材料属性，赋予四足机器人动力学模型的动力学属性；

四足机器人动力学模型的杆件之间采用铰接约束，足部与膝关节采用弹簧约束，根据杆件质量、质心和转动惯量信息，结合杆件间的运动关系，构建连接副约束关系，基于地形模型动力学参数，建立典型的台阶、斜坡工况，赋予地形材料属性，并针对不同的工况简化为相应的碰撞几何体，同时将机器人足部简化为胶囊碰撞体，与地形进行碰撞检测，形成四足机器人和地形可实时交互的动力学系统；

优选的，第三构建子单元，还用于：

采用计算机时钟频率保证动力学解算的实时性，用于精确定时，并采用多线程技术进行通信；

在多步态动力学系统采用基于c++面向对象方式的多步态控制算法；

构建关节控制器，执行多步态动力学系统产生的机器人动作。

本发明提供的一种面向对象的四足机器人和地形的图形可视化模型建模方法及系统。基于vortex动力学引擎进行动力学实时解算，充分利用vortex高精确度、高实时交互、高拓展性的优质建模特性，解算周期采用系统时钟频率实现精确定时，基于udp(userdatagramprotocol)协议通过客户端-服务端通信的方式实时向图形引擎任务传输机器人的状态信息，进行平台可视化分析，完成多步态模型嵌入的动力学系统的建模。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员依然可以对本发明的具体实施方式进行修改或者等同替换，这些未脱离本发明精神和范围的任何修改或者等同替换，均在申请待批的本发明的权利要求保护范围之内。