CN114442649B - 一种双足机器人混杂动力学建模和运动规划方法 - Google Patents

Abstract

本发明属于机器人智能控制技术领域，涉及一种双足机器人混杂动力学建模和运动规划方法，具体包括：步骤1，初始化双足机器人，建立描述双足机器人运动的坐标***，建立双足机器人运动学模型；步骤2，设定双足机器人的关节角运动学约束；步骤3，分析双足机器人的广义动力学模型；步骤4，构建双足机器人的混杂动力学模型；步骤5，设计双足机器人基于混杂动力学模型的运动规划方法，设计优化的目标以及线性和非线性约束，对运动规划问题进行描述并通过非线性优化方法求解，获得双足机器人的运动轨迹。本发明在双足机器人的运动规划和控制方面具有较强的应用价值。

Description

一种双足机器人混杂动力学建模和运动规划方法

技术领域

本发明属于机器人智能控制技术领域，涉及一种双足机器人混杂动力学建模和运动规划方法。

背景技术

双足行走是人类以及鸟类等动物在世界上的主要行走方式，能够适应复杂多变的地面场景，具有非常大的灵活度。虽然目前双足机器人经历了飞速的发展，例如波士顿动力、Cassie等，双足机器人的研究及成果仍然局限于少数几个机构当中。作为世界最顶级的公司波士顿动力的双足研究成果向来不公开自己的研究成果，因而针对双足机器人的研究仍然具备非常重要的意义。双足机器人的研究成果主要目标在于保持自身的稳定，而其中涉及建模、规划和控制等非常复杂。双足机器人动力学建模主要有以下难点：1)双足机器人的多***耦合特性，双足机器人至少有两条腿跟驱干组成，自由度高，多***耦合，建模难度大；2)双足机器人是浮动基座动力学模型，整体运动通过腿部与地面作用力实现，而双足与地面的作用力复杂难以精确描述；3)双足机器人在行走过程中，包含多个相位，彼此间不连续，因而是一个混杂模型。

双足机器人全身动力学规划挑战多：1)双足机器人动力学维度高，运动规划复杂度大；2)双足机器人运动规划的代价函数选取难度大；3)全身动力学规划的约束复杂，包含自身的运动限制、地面摩擦副、ZMP等稳定性约束。

因此，双足机器人动力学建模与非线性动力学规划仍面临诸多难点。

发明内容

为了解决现有技术中存在的上述技术问题，本发明主要从双足机器人多***耦合及全身规划的角度展开研究，提出了一种双足机器人混杂动力学建模和运动规划方法，其具体技术方案如下：

一种双足机器人混杂动力学建模和运动规划方法，包括以下步骤：

步骤1，初始化双足机器人，建立描述双足机器人运动的坐标***，采用双足机器人各运动轴的齐次矩阵及雅阁比矩阵建立双足机器人运动学模型；

步骤2，通过约束各驱动关节的工作范围和各驱动关节的速度范围来设定双足机器人的关节角运动学约束；

步骤3，运用双足机器人的势能方程、动能方程以及动力学广义表达来分析双足机器人的广义动力学模型；

步骤4，使用单腿支撑、双腿支撑的约束方程构建双足机器人的混杂动力学模型；

步骤5，设计双足机器人基于混杂动力学模型的运动规划方法，设计优化的目标以及线性和非线性约束，对运动规划问题进行描述并通过非线性优化方法求解，获得双足机器人的运动轨迹。

进一步的，所述建立描述双足机器人运动的坐标***，具体包括：

世界坐标系o_W-x_Wy_Wz_W，描述机器人身体姿态的基座坐标系，该坐标系附在机器人中心o_B-x_By_Bz_B，以及左右腿足各旋转关节的中心建立坐标系o_L1-x_L1y_L1z_L1,o_R1-x_R1y_R1z_R1,根据上述坐标***的定义，建立如下状态坐标变量：

q＝[q_x,q_y,q_z,q_roll,q_pitch,q_yaw,q_L1,q_L2,q_L3,q_L4,q_L5,q_L6,q_R1,q_R2,q_R3,q_R4,q_R5,q_R6]，

其中，q_x,q_y,q_z表示双足机器人身体在世界坐标系的位置，q_roll,q_pitch,q_yaw依次对应为α,β,γ，分别表示翻滚角，俯仰角，偏航角。

进一步的，所述双足机器人各运动轴的齐次矩阵，具体包括：

针对双足机器人的基座B或者躯干W建立描述机器人运动学规律的齐次方程：

c表示cos,s表示sin；

并相应建立双腿的运动学描述，即双腿的齐次方程描述；

所述雅阁比矩阵，具体包括：

刚体的点⁰p＝[x_p,y_p,z_p]^T在世界坐标系的表达：

该点的雅各比矩阵：

每个刚体的旋转角速度：

为此，刚体雅阁比矩阵可以表示为：

进一步的，所述各驱动关节的工作范围，各驱动关节的速度范围，具体包括：

左腿各关节运动约束：

q_Limin≤q_Li≤q_Limax,i＝1,2,3…,6

右腿各关节运动约束：

q_Rimin≤q_Ri≤q_Rimax,i＝1,2,3…,6

左腿各关节速度约束

右腿各关节速度约束

其中i＝1,2,3…,6表示机器人身体的6自由度位姿。

进一步的，所述双足机器人的势能方程、动能方程以及动力学广义表达，具体包括：

利用拉格朗日动力学方法得到动力学方程：

其中，M_i表示第i个刚体的质量矩阵，I_i表示第i个刚体的转动惯量，n表示刚体总数；

势能方程：

其中，m_i表示刚体质量，g表示重力加速度常量，p^z表示刚体质心高度；

因此，双足机器人动力学方程可以写成如下拉格朗日形式：

其中，u表示各驱动关节输入力矩，S表示输入矩阵，λ表示广义外部作用力，J表示外部作用力作用点的雅阁比；

进而可以得到质量矩阵，科里奥利力矩阵以及重力矩阵，进而得到如下形式的动力学模型：

其中，M表示广义质量矩阵，C表示科式矩阵，G表示重力矩阵，J_L,J_R表示双足机器人左右脚底的接触点的雅阁比，λ_L,λ_R表示脚反作用于机器人的作用力，表示加速度。

进一步的，所述单腿支撑、双腿支撑的约束方程，具体为：

1)单腿支撑：

双足机器人单腿支撑时，用6自由度描述位姿c_j(q)^T描述接触点，脚底的约束可以描述成，该点的加速度为0，因此得到如下方程：

其中，接触点c_j的雅阁比矩阵为J_j；

2)双脚支撑：

从单腿到双腿的支撑，是有一个冲击的过程，冲击前后速度不一致，分别用q^-和q⁺来描述前后的状态；从单腿支撑到双腿支撑，根据动量守恒定理以及脚底约束：

因此，得到如下变换表示上述方程的求解：

因此，可以得到如下的模型：

进一步的，所述步骤5的设计优化的目标以及线性和非线性约束，具体包括：

设定目标函数为：

J＝∫u²dt

其中，上述目标含义表示所有力求和最小；

采用右腿站立，左腿站立轮流走路的步态，其约束分析过程如下：

右腿站立连续动力学模型：

右腿站立的不等式约束：

左腿的脚底位置大于地面，根据运动学模型可以得到左脚底板的表达式：

其中表示左脚接触点高度；

为了保证支撑腿能够保持整体的平衡，那么摩擦力需要满足如下约束：

其中，表示左右脚接触点在x，y，z三个方向上的作用力大小；/>表示左右脚接触力矩；

左腿站立连续动力学模型：

左腿站立的不等式约束：

右腿的脚底位置大于地面，根据运动学模型可以得到右脚底板的表达式：

摩擦力约束：

离散模型切换约束：

H(q_R)＝0

该约束表示的是模型切换之前应该满足的约束,表明下一个时刻就要开始碰撞；

离散模型前后的冲击约束：

最后对运动规划问题进行描述并通过非线性优化方法求解，获得各关节的运动轨迹。

本发明具有的有益效果是：

本发明公开了一种双足机器人的混杂动力学建模及运动规划方法，针对具有浮动基座的双足机器人，分析机器人的高维非线性耦合特征，提出全身动力学建模方法。考虑双足机器人左右腿切换、站立等离散状态以及各状态间的状态切换规律，建立双足机器人混杂动力学模型，并基于此模型对双足机器人运动展开运动规划。本发明考虑双足机器人的连续动力学模型以及离散状态切换规律，建立双足机器人混杂动力学模型并进行运动规划的方法，在双足机器人的运动规划和控制方面具有较强的应用价值。

附图说明

图1为双足机器人的结构简化及坐标系示意图；

图2是本发明的方法流程示意图；

图中，1：双足机器人身躯，2：右腿，3：脚板，4：左腿。

具体实施方式

为了使本发明的目的、技术方案和技术效果更加清楚明白，以下结合说明书附图和实施例，对本发明作进一步详细说明。

一种双足机器人混杂动力学建模和运动规划方法，对双足机器人进行混杂动力学建模和运动规划，所述双足机器人包括机器人身躯1、驱动电机、若干编码器、实时控制器、两条多自由度腿足，如图1所示，两条多自由度腿足包括：左腿4和右腿2，左右腿下方均设有脚板3，机器人身躯1与两条多自由度腿足相连，若干编码器测量双足机器人的姿态；

其中，所述混杂动力学建模采用双足机器人坐标***建立模块、运动学分析及建模模块、广义动力学建模模块以及混杂状态分析模块；所述运动学分析及建模模块用于建立双足机器人运动学模型，求解双足机器人的齐次矩阵以及雅阁比矩阵；所述广义动力学建模模块用于分析双足机器人的广义动力学模型，求解得到双足机器人的拉格朗日动力学方程描述；所述混杂状态分析模块用于构建双足机器人的混杂动力学模型，分析单腿站立以及双腿站立机器人的约束方程。

所述双足机器人运动规划方法运用目标函数模块、约束定义模块以及优化求解模块，其中，所述目标函数模块用于设定双足机器人运动规划时须满足的目标；所述约束定义模块用于分析运动规划时满足的动力学约束、运动学约束以及冲击约束等；所述优化求解模块用于求解各关节的运动轨迹。

具体的，如图2所示，该方法，包括以下步骤：

步骤1，初始化双足机器人，建立双足机器人运动学模型，所述双足机器人运动学模型包括：双足机器人各运动轴的齐次矩阵及雅阁比矩阵，具体包括：

如图1所示，建立一个描述双足机器人运动的坐标***，坐标***由以下几个部分组成，世界坐标系o_W-x_Wy_Wz_W，描述机器人身体姿态的基座坐标系，该坐标系附在机器人中心o_B-x_By_Bz_B，以及左右腿足各旋转关节的中心建立坐标系o_L1-x_L1y_L1z_L1,o_R1-x_R1y_R1z_R1,根据上述坐标***的定义，建立如下状态坐标变量：

q＝[q_x,q_y,q_z,q_roll,q_pitch,q_yaw,q_L1,q_L2,q_L3,q_L4,q_L5,q_L6,q_R1,q_R2,q_R3,q_R4,q_R5,q_R6]，其中，q_x,q_y,q_z表示双足机器人身体在世界坐标系的位置，q_roll,q_pitch,q_yaw依次对应为α,β,γ，分别表示翻滚角，俯仰角，偏航角。

基于上述坐标系以及定义的状态坐标变量，对双足机器人进行运动分析及模型建立，并通过以下步骤实现：

1)针对双足机器人的基座B或者躯干W建立描述机器人运动学规律的齐次方程：

c表示cos,s表示sin。

2)相应的建立双腿的运动学描述，通过以下双腿齐次描述方程：

3)雅阁比矩阵求解：

刚体的点⁰p＝[x_p,y_p,z_p]^T在世界坐标系的表达：

该点的雅各比矩阵：

每个刚体的旋转角速度：

为此，刚体雅阁比矩阵可以表示为：

步骤2，设定双足机器人的关节角运动学约束，所述约束包括：各驱动关节的工作范围，各驱动关节的速度范围，具体包括：

左腿各关节运动约束：

q_Limin≤q_Li≤q_Limax,i＝1,2,3…,6

右腿各关节运动约束：

q_Rimin≤q_Ri≤q_Rimax,i＝1,2,3…,6

左腿各关节速度约束

右腿各关节速度约束

其中i＝1,2,3...,6表示机器人身体的6自由度位姿。

步骤3，分析双足机器人的广义动力学模型，所述模型包括：双足机器人的势能方程、动能方程以及动力学广义表达，具体包括：

利用拉格朗日动力学方法得到动力学方程：

其中，M_i表示第i个刚体的质量矩阵，I_i表示第i个刚体的转动惯量，n表示刚体总数；

势能方程：

其中，m_i表示刚体质量，g表示重力加速度常量，p^z表示刚体质心高度；

因此，双足机器人动力学方程可以写成如下拉格朗日形式：

其中，u表示各驱动关节输入力矩，S表示输入矩阵，λ表示广义外部作用力，J表示外部作用力作用点的雅阁比；

进而可以得到质量矩阵，科里奥利力矩阵以及重力矩阵，进而得到如下形式的动力学模型：

其中，M表示广义质量矩阵，C表示科式矩阵，G表示重力矩阵，J_L,J_R表示双足机器人左右脚底的接触点的雅阁比，λ_L,λ_R表示脚反作用于机器人的作用力，表示加速度；

步骤4，构建双足机器人的混杂动力学模型，所述混杂动力学包括：单腿支撑、双腿支撑的约束方程，具体为：

机器人走路的时候通常出现单腿支撑、双腿支撑的切换现象，需要构建一套混杂模型描述其运动状态：单腿支撑的时候，另一条腿称之为摆动腿；双腿支撑时两个脚底都有约束，另外还要注意的是两者碰撞的瞬间模型的切换，具体包括：

1)单腿支撑：

双足机器人单腿支撑时，用6自由度描述位姿c_j(q)^T描述接触点；

那么脚底的约束可以描述成，该点的加速度为0，因此得到如下方程：

其中，接触点c_j的雅阁比矩阵为J_j；

2)双脚支撑：

从单腿到双腿的支撑，是有一个冲击的过程，冲击前后速度不一致，分别用q^-和q⁺来描述前后的状态；从单腿支撑到双腿支撑，根据动量守恒定理以及脚底约束：

因此，得到如下变换表示上述方程的求解：

因此，可以得到如下的模型：

步骤5，设计双足机器人基于混杂动力学模型的运动规划方法，设计优化的目标以及线性和非线性约束，通过非线性优化方法求解，获得双足机器人的运动轨迹，具体为：

目标函数：

J＝∫u²dt

其中，上述目标含义表示所有力求和最小：

为了更加清晰描述约束，采用右腿站立，左腿站立轮流走路的步态，其约束分析过程如下：

右腿站立连续动力学模型：

右腿站立的不等式约束：该约束的含义是，满足上述动力学方程需要满足的条件，就是左腿的脚底位置大于地面，根据运动学模型可以得到左脚底板的表达式：

其中表示左脚接触点高度；

为了保证支撑腿能够保持整体的平衡，那么摩擦力需要满足如下约束：

其中，表示左右脚接触点在x，y，z三个方向上的作用力大小；/>表示左右脚接触力矩；

左腿站立连续动力学模型：

左腿站立的不等式约束：该约束的含义是，满足上述动力学方程需要满足的条件，就是右腿的脚底位置大于地面，根据运动学模型可以得到右脚底板的表达式：

摩擦力约束：

离散模型切换约束：

H(q_R)＝0

该约束表示的是模型切换之前应该满足的约束,表明下一个时刻就要开始碰撞了；

离散模型前后的冲击约束：

/>

优化问题描述：

s.t.

q_Rmin≤q_R≤q_Rmax

u_Rmin≤u_R≤u_Rmax

通过非线性优化方法求解上述优化方程，获得各关节的运动轨迹。

以上所述，仅为本发明的优选实施案例，并非对本发明做任何形式上的限制。虽然前文对本发明的实施过程进行了详细说明，对于熟悉本领域的人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行同等替换。凡在本发明精神和原则之内所做修改、同等替换等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种双足机器人混杂动力学建模和运动规划方法，其特征在于，包括以下步骤：

步骤1，初始化双足机器人，建立描述双足机器人运动的坐标***，采用双足机器人各运动轴的齐次矩阵及雅阁比矩阵建立双足机器人运动学模型；

步骤2，通过约束各驱动关节的工作范围和各驱动关节的速度范围来设定双足机器人的关节角运动学约束；

步骤3，运用双足机器人的势能方程、动能方程以及动力学广义表达来分析双足机器人的广义动力学模型；

步骤4，使用单腿支撑、双腿支撑的约束方程构建双足机器人的混杂动力学模型；

步骤5，设计双足机器人基于混杂动力学模型的运动规划方法，设计优化的目标以及线性和非线性约束，对运动规划问题进行描述并通过非线性优化方法求解，获得双足机器人的运动轨迹；

所述建立描述双足机器人运动的坐标***，具体包括：

世界坐标系o_W-x_Wy_Wz_W，描述机器人身体姿态的基座坐标系，该坐标系附在机器人中心o_B-x_By_Bz_B，以及左右腿足各旋转关节的中心建立坐标系o_L1-x_L1y_L1z_L1,o_R1-x_R1y_R1z_R1,根据上述坐标***的定义，建立如下状态坐标变量：

q＝[q_x,q_y,q_z,q_roll,q_pitch,q_yaw,q_L1,q_L2,q_L3,q_L4,q_L5,q_L6,q_R1,q_R2,q_R3,q_R4,q_R5,q_R6]，

其中，q_x,q_y,q_z表示双足机器人身体在世界坐标系的位置，q_roll,q_pitch,q_yaw依次对应为α,β,γ，分别表示翻滚角，俯仰角，偏航角；

所述双足机器人各运动轴的齐次矩阵，具体包括：

针对双足机器人的基座B或者躯干W建立描述机器人运动学规律的齐次方程：

c表示cos,s表示sin；

并相应建立双腿的运动学描述，即双腿的齐次方程描述；

所述雅阁比矩阵，具体包括：

刚体的点⁰p＝[x_p,y_p,z_p]^T在世界坐标系的表达：

该点的雅各比矩阵：

每个刚体的旋转角速度：

为此，刚体雅阁比矩阵可以表示为：

所述各驱动关节的工作范围，各驱动关节的速度范围，具体包括：

左腿各关节运动约束：

q_Limin≤q_Li≤q_Limax,i＝1,2,3…,6

右腿各关节运动约束：

q_Rimin≤q_Ri≤q_Rimax,i＝1,2,3…,6

左腿各关节速度约束

右腿各关节速度约束

其中i＝1,2,3...,6表示机器人身体的6自由度位姿；

所述双足机器人的势能方程、动能方程以及动力学广义表达，具体包括：利用拉格朗日动力学方法得到动力学方程：

其中，M_i表示第i个刚体的质量矩阵，I_i表示第i个刚体的转动惯量，n表示刚体总数；

势能方程：

其中，m_i表示刚体质量，g表示重力加速度常量，p^z表示刚体质心高度；

因此，双足机器人动力学方程可以写成如下拉格朗日形式：

其中，u表示各驱动关节输入力矩，S表示输入矩阵，λ表示广义外部作用力，J表示外部作用力作用点的雅阁比；

进而可以得到质量矩阵，科里奥利力矩阵以及重力矩阵，进而得到如下形式的动力学模型：

其中，M表示广义质量矩阵，C表示科式矩阵，G表示重力矩阵，J_L,J_R表示双足机器人左右脚底的接触点的雅阁比，λ_L,λ_R表示脚反作用于机器人的作用力，表示加速度；

所述单腿支撑、双腿支撑的约束方程，具体为：

1)单腿支撑：

双足机器人单腿支撑时，用6自由度描述位姿c_j(q)^T描述接触点，脚底的约束可以描述成，该点的加速度为0，因此得到如下方程：

其中，接触点c_j的雅阁比矩阵为J_j；

2)双脚支撑：

从单腿到双腿的支撑，是有一个冲击的过程，冲击前后速度不一致，分别用q^-和q⁺来描述前后的状态；从单腿支撑到双腿支撑，根据动量守恒定理以及脚底约束：

因此，得到如下变换表示上述方程的求解：

因此，可以得到如下的模型：

所述步骤5的设计优化的目标以及线性和非线性约束，具体包括：

设定目标函数为：

J＝∫u²dt

其中，上述目标含义表示所有力求和最小；

采用右腿站立，左腿站立轮流走路的步态，其约束分析过程如下：

右腿站立连续动力学模型：

右腿站立的不等式约束：

左腿的脚底位置大于地面，根据运动学模型可以得到左脚底板的表达式：

其中表示左脚接触点高度；

为了保证支撑腿能够保持整体的平衡，那么摩擦力需要满足如下约束：

其中，表示左右脚接触点在x，y，z三个方向上的作用力大小；表示左右脚接触力矩；

左腿站立连续动力学模型：

左腿站立的不等式约束：

右腿的脚底位置大于地面，根据运动学模型可以得到右脚底板的表达式：

摩擦力约束：

离散模型切换约束：

H(q_R)＝0

该约束表示的是模型切换之前应该满足的约束,表明下一个时刻就要开始碰撞；

离散模型前后的冲击约束：