CN117389317A - 一种轮式双足机器人的运动控制方法及系统 - Google Patents

Abstract

本发明公开了一种轮式双足机器人的运动控制方法及系统，包括：模型预测控制器基于输入的运动控制指令输出需要向躯干施加的广义力旋量F；求解机器人的躯干质心与车轮中心点在x轴方向的距离Δx；基于离心力补偿策略计算躯干横滚角，基于地形适应策略计算躯干坐标系下的车轮位置；经过逆运动学求解得到期望的腿部关节角度、腿部关节速度和摆杆俯仰角，作为状态输入；全身控制器基于模型预测控制器求解得到的广义力旋量F和逆运动学求解得到的状态输入，融合动力学前馈和关节反馈，最终得到控制机器人运动的腿部关节力矩和轮关节力矩。本发明增强机器人在高速转向时的鲁棒性，防止发生侧翻。

Description

一种轮式双足机器人的运动控制方法及系统

技术领域

本发明涉及轮式双足机器人技术领域，尤其涉及一种轮式双足机器人的运动控制方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

随着科学技术的不断发展，移动机器人逐渐融入到日常生活工作中，这也导致人们对机器人的场景适应能力和工作效率提出了更高的需求。结合了轮式机器人能效高的优势和腿足式机器人地形适应能力强的特点，在许多应用场景中，轮-腿机器人成为更好的选择。但是，四轮腿或六轮腿的机器人在转向时存在转弯半径大或转弯速度慢的问题，这限制了它们的运动灵活性。拥有快速移动和零半径转弯能力的轮式双足机器人能够很好的解决这种痛点。

现有的技术中，通常对轮式双足机器人的车轮运动和腿部运动分开控制，即车轮控制器实现机器人平衡而腿部控制器实现地形适应，这种方式忽略了轮-腿之间的耦合作用，不利于机器人的稳定控制。同时，轮式双足机器人具有高速转向的运动潜力，然而转向速度越快发生侧翻的风险越大。

发明内容

为了解决上述问题，本发明提出了一种轮式双足机器人的运动控制方法及系统，建立轮-腿动力学模型，采用双腿配合主动使躯干产生横滚角的方式，通过重力的分量来抵消离心力，以增强机器人在高速转向时的鲁棒性，防止发生侧翻。

在一些实施方式中，采用如下技术方案：

一种轮式双足机器人的运动控制方法，包括：

基于躯干动力学模型构建模型预测控制器，基于轮-腿动力学模型构建全身控制器；

模型预测控制器基于输入的运动控制指令输出需要向躯干施加的广义力旋量F；

基于WIPM动力学求解机器人的躯干质心与车轮中心点在x轴方向的距离Δx；

基于离心力补偿策略计算躯干横滚角，基于地形适应策略计算躯干坐标系下的车轮位置；

基于得到的距离Δx、躯干横滚角和车轮位置，经过逆运动学求解得到期望的腿部关节角度、腿部关节速度和摆杆俯仰角，作为状态输入；

全身控制器基于模型预测控制器求解得到的广义力旋量F和逆运动学求解得到的状态输入，融合动力学前馈和关节反馈，最终得到控制机器人运动的腿部关节力矩和轮关节力矩。

在另一些实施方式中，采用如下技术方案：

一种轮式双足机器人的运动控制系统，包括：

控制器构建模块，用于基于躯干动力学模型构建模型预测控制器，基于轮-腿动力学模型构建全身控制器；

模型预测控制模块，用于通过模型预测控制器基于输入的运动控制指令输出需要向躯干施加的广义力旋量F；基于WIPM动力学求解机器人的躯干质心与车轮中心点在x轴方向的距离Δx；基于离心力补偿策略计算躯干横滚角，基于地形适应策略计算躯干坐标系下的车轮位置；

状态计算模块，用于基于得到的距离Δx、躯干横滚角和车轮位置，经过逆运动学求解得到期望的腿部关节角度、腿部关节速度和摆杆俯仰角，作为状态输入；

机器人控制模块，用于通过全身控制器基于模型预测控制器求解得到的广义力旋量F和逆运动学求解得到的状态输入，融合动力学前馈和关节反馈，最终得到控制机器人运动的腿部关节力矩和轮关节力矩。

在另一些实施方式中，采用如下技术方案：

一种终端设备，其包括处理器和存储器，处理器用于实现指令；存储器用于存储多条指令，所述指令适于由处理器加载并执行上述的轮式双足机器人的运动控制方法。

在另一些实施方式中，采用如下技术方案：

一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行上述的轮式双足机器人的运动控制方法。

与现有技术相比，本发明的有益效果是：

(1)不同于对车轮与双足身体的解耦，本发明更强调轮-腿运动时的一致性，建立轮-腿动力学模型，揭示从车轮向髋关节的力传递关系。同时，开发了一种离心力补偿的控制策略，采用双腿配合主动使躯干产生横滚角的方式，通过重力的分量来抵消离心力，以增强机器人在高速转向时的鲁棒性，防止发生侧翻。

(2)本发明设计了一种基于模型预测控制的分层控制框架，融合离心力补偿控制策略和地形适应控制策略以增强机器人的高速转向鲁棒性和地形适应能力。在该控制框架下，轮式双足机器人能够表现出更好的动态运动能力。

本发明的其他特征和附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本方面的实践了解到。

附图说明

图1为本发明实施例中轮式双足机器人的运动控制方法示意图；

图2为本发明实施例中质心与轮轴位置关系示意图。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本发明使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

实施例一

在一个或多个实施方式中，公开了一种轮式双足机器人的运动控制方法，结合图1，具体包括如下过程：

一种轮式双足机器人的运动控制方法，包括：

(1)基于躯干动力学模型构建模型预测控制器，基于轮-腿动力学模型构建全身控制器；

(2)模型预测控制器基于输入的运动控制指令输出需要向躯干施加的广义力旋量F；

(3)基于WIPM动力学求解机器人的躯干质心与车轮中心点在x轴方向的距离Δx；

(4)基于离心力补偿策略计算躯干横滚角α_b，基于地形适应策略计算躯干坐标系下的车轮位置；

(5)基于得到的距离Δx、躯干横滚角α_b和车轮位置，经过逆运动学求解得到期望的腿部关节角度、腿部关节速度和摆杆俯仰角，作为状态输入；其中，从轮轴中心点指向质心的那条线就是WIPM模型中的摆杆，WIPM模型中摆杆与坐标轴Z之间的夹角即为摆杆俯仰角，即图2的θ；

(6)全身控制器基于模型预测控制器求解得到的广义力旋量F和逆运动学求解得到的状态输入，经过动力学和关节空间的关节力矩反馈控制，最终得到控制机器人运动的腿部关节力矩和轮关节力矩。

作为具体的实施方式，轮式双足机器人是典型的浮动基座系统，为了便于构造控制器，分别建立躯干子系统和轮腿子系统的动力学模型。

1、躯干动力学模型的构建具体如下：

轮式双足机器人是典型的浮动基座系统，为了便于构造控制器，分别建立躯干子系统和轮腿子系统的动力学模型。由轮式双足机器人的机械结构决定，轮腿子系统向躯干施加六维的广义力旋量F＝[F^L F^R]^T，其中， 分别表示左、右轮腿子系统提供给躯干的前向推动力，f_z ^L、f_z ^R分别表示左、右轮腿子系统提供给躯干的竖直支撑力。/>与控制坐标系的x轴方向一致，n_y表示沿控制坐标系y轴旋转的扭矩，f_z与控制坐标系z轴方向一致。本实施例定义躯干的广义坐标q和速度u为：

其中，是躯干的位置，Θ∈SO(3)表示躯干的旋转，/>表示躯干的角速度。x、y、z、α、β、γ分别表示三维坐标跟旋转，v表示速度，假设车轮与地面之间不存在滑动，受到轮式结构的非完整约束，躯干在世界坐标系xoy平面中的运动存在公式中的耦合关系：

其中，v_b表示躯干在世界坐标系中的前向速度。可以发现：

R_z(γ)是沿世界坐标系z轴的旋转矩阵。由于这种耦合关系，在控制坐标系中更适合描述机器人的运动。把控制坐标系下的躯干广义坐标和速度定义为：

q_b＝[p_b Θ_b]^T＝[x_b z_b Θ_b]^T

其中，z_b表示躯干的z方向的坐标，躯干的简化线性动力学模型为：

其中，N＝[0 n_y 0]^T，G＝[0 g]^T表示重力加速度，m_b表示躯干的重量，/>表示控制坐标系下躯干惯性张量矩阵，/>表示控制坐标系下躯干质心到髋关节的向量。ω_b、/>分别表示躯干的角速度、角加速度、加速度。

ω×(Iω_b)相对于较小，因此，公式可以近似为：

同时，控制坐标系下的惯性张量矩阵可以得到：

其中，_BI是躯干坐标系下的惯性张量矩阵，表示从躯干到控制坐标系的变换矩阵。

2、基于躯干动力学模型构建模型预测控制器，具体如下：

模型预测控制作为一种基于模型的控制方法，利用模型在每个控制周期内预测系统在未来一段时间(预测时域)的动态特性，进而寻求当前控制周期内的有限时域开环最优控制输入。将躯干简化动力学整理成如下形式：

r₁、r₂表示左右两个髋关节指向躯干的位置向量。

通过把重力加入到状态变量中，得到动力学方程的状态空间形式为：

其中，为了满足实际机器人的求解需求，本实施例基于零阶保持器离散化线性时变状态空间模型，得到离散状态空间模型：

其中，Δt是控制器的采样时间，x_k和u_k表示k时刻系统的状态和输入向量，x_k+1表示基于模型求出的k+1时刻系统状态量。假设预测步长为n，为了求出当前时刻的最优状态输入，定义优化函数为：

其中，U＝(u₀,u₁,…,u_n-1)表示预测步长内各个时刻的控制输入，k＝0,1,…,n-1；Q、R₁和R₂分别为权重矩阵，表示期望相邻时刻的控制输入不要出现跳变现象，以免造成机器人的不稳定；λ表示约束矩阵，使机器人始终保持在可控的运动状态中；Δt是控制器的采样时间，/>表示k时刻躯干的期望状态，x_k、x_k+1分别表示k和k+1时刻躯干的当前反馈状态，/>表示上一时刻的躯干输入力旋量，C和/>表示约束矩阵，/>和/>表示状态空间方程中的状态转移矩阵；对模型预测控制器进行求解，取当前时刻的最优解u₀作为需要向躯干施加的广义力旋量F。

3、保持动态平衡是实现轮式双足机器人稳定运动的基础。为了简化分析，结合图2，本实施例把Skater降阶为轮式倒立摆模型去分析动力学约束下质心位置与运动状态的关系，有利于规划机器人运动以快速收敛到稳定姿态。轮与地面间可看作是点接触，因此接触点即为零力矩点(ZMP)。WIPM与标准LIPM具有相似的动力学特性，因此他们的动力学方程也采用类似的形式：

其中，分别是躯干质心在这两个方向的加速度，x_b是质心的x轴坐标、x_w车轮中心的x轴坐标；s_w表示车轮中心点，Δx表示躯干质心与车轮中心点在x轴方向的距离。由于腿部关节电机及电器元件全部集成在躯干中，并且支撑腿的材料是质量较轻的铝合金并且做了镂空减重处理，因此，除去车轮，机器人重量主要集中在躯干，腿部质量相对于躯干可以忽略不计，本实施例可以把躯干质心近似作为WIPM中的质心。根据躯干动力学，质心的加速度可由牛顿第二定律推导出：

将其带入到公式中可得：

由此得到，保持机器人的动态平衡需要满足该公式反映的质心与轮轴之间在x轴方向的相对位置关系。

3、轮-腿动力学模型构建过程如下：

相较于足式机器人，轮式双足机器人在非跳跃相位时，双腿几乎一直处在支撑状态，在给躯干提供输出力的同时，需要保持自身处于稳定姿态。本实施例定义关节角度、速度和力矩为：

其中，表示各个关节的角度值，/>表示各个关节的速度，j＝leg或wheel；代表腿部的髋关节和膝关节，/>表示轮关节，/>表示驱动关节的力矩值，τ_leg表示驱动腿关节的力矩值，τ_wheel表示驱动轮关节的力矩值；i＝L或R；/>代表腿部的髋关节和膝关节的力矩值。

在运动过程中，尤其是当机器人的运动状态有较大变化时，车轮与腿部的互相扰动无法忽略不计，对它们之间传递的力或力矩的分析是有必要的。轮腿子系统可以看作为与躯干连接的两个三自由度机械臂。由关节结构特点决定，轮关节在为躯干提供驱动力的同时带动子系统跟随躯干的运动。因此以轮为动基座(基坐标系)，髋关节处的末端力旋量作为输出力，基于牛顿-欧拉动力学公式求出轮腿动力学方程如下所示：

M(q_j)表示质量矩阵，表示科氏力和离心力项，G(q_j)表示重力项，J表示雅可比矩阵，F表示需要向躯干施加的广义力旋量。

4、高速转向的离心力补偿控制策略具体如下：

由于结构限制，在转弯时，轮腿无法为躯干提供侧向力以抵消离心力的影响。一种离心力补偿(CFC)的控制策略被提出，采用双腿配合使躯干产生横滚角的方式，通过重力的分量来抵消离心力，以增强机器人在高速转向时的鲁棒性，防止发生侧翻。横滚角与速度和偏航角速度间的关系为：

尽管这种方式能够有效避免离心力的影响，机器人仍然受到零力矩点和站高腿部伸缩裕度的限制。首先，躯干质心在地面的投影需要在C_l和C_r之间。其次，腿部伸缩裕度决定了横滚角的大小，比如，当机器人的高度处于最大或最小时则无法使躯干产生横滚。因此，躯干横滚角被计算为：

st.C_l≤z_bsinα_b≤C_r

其中，α_b为躯干横滚角，v_b表示躯干在世界坐标系中的前向速度，表示躯干的偏航角速度，g表示重力加速度，C_l和C_r分别表示左右轮腿的轮地接触点，z_b表示躯干的z方向的坐标，f_m(z_b,z_bmax,z_bmin)＝min(|z_b-z_bmax|,|z_b-z_bmin|)，表示z_b距离z_bmax和z_bmin之间的最小值，C_r-C_l表示接触点间的距离。

5、地形适应策略具体如下：

轮式双足机器人无法通过落足点的位置来估算地形。面对起伏地形，可以通过躯干的姿态信息来做出实时调整。在躯干坐标系下车轮坐标被定义为和l表示左腿，r表示右腿。

根据几何关系可以推导出横滚角与车轮位置间的关系为：

其中，d表示轮地接触点间的距离。同时，需要保持机器人高度不变：

在上坡或下坡时，调整身体重心的位置以适应不同的坡度是很重要的。根据对轮式倒立摆模型的分析，躯干与车轮间的唯一是确定的。同时为了保证双腿的稳定配合运动，需要保持轮轴在同一平面内，满足以下约束条件：

结合以上公式，可以得到以下等式：

本实施例可以发现由地形条件导致的躯干状态变化可通过改变车轮位置来适应。因为W_T是满秩的，求得解为：

6、基于轮-腿动力学模型构建全身控制器，具体为：

与腿足式机器人不同，轮式双足机器人没有摆动相，这意味着控制任务只包括躯干平移和姿态旋转。根据轮式倒立摆模型的分析，机器人的每一种不同的运动状态对应一个独特的内部稳定姿态:CoM位置、摆长和倾斜角。因此，躯干和车轮之间的平移和旋转是确定的。基于上述策略中的公式，逆运动学用于求出期望的关节角度和关节速度/>然而，由于躯干相对于动基座的运动幅度和加速度较小，忽略逆运动学求出的关节加速度而由PD控制率求出：

在动力学方程中，与/>相关项主要是为了补偿动基座带来的惯性力，因此，只需要将反馈值带入到动力学方程中即可。那么关节前馈力矩可由以下公式求出：

由于机器人的动力学模型存在不可避免的误差，对腿部关节力矩引入PD反馈控制：

轮式双足机器人作为具有非最小相位特性的一类机器人，如果只依靠前馈力对轮关节进行控制的话，容易出现偏离平衡点的现象，无法保证机器人在运动过程中的稳定性。不仅如此，当在机器人加速前进或后退时，车轮需要向相反方向移动一段距离，以满足WIPM模型中的ZMP约束。因此，本实施例引入LQR最优反馈控制率以辅助机器人快速收敛到平衡点。

选择LQR的状态向量为其中θ,/>v,ω分别表示摆杆的俯仰角、俯仰角速度、平均轮毂速度和偏航角速度。在平衡点处，本实施例通过近似线性化得到了系统的线性状态空间方程：/>其中/> 分别表示LQR输出的左腿和右腿的反馈力矩。

其中，a_ij,b_ij表示轮式倒立摆模型参数的函数。假设在控制器的运行周期T_s上零阶保持，本实施例离散化该状态空间方程为：

通过求解离散时间代数理卡蒂方程(DARE)，得到了LQR无限视界问题的最优解，最终求出最优反馈增益矩阵K。

其中，Q和R表示权重矩阵。值得注意的是，摆杆长度会随着机器人站高的变化而变化，因此，最优反馈增益矩阵K是摆杆长度的函数，系统的控制输入由LQR控制率得到：

U＝-K(L)(X^d-X)

其中，摆杆长度L和状态变量反馈值X由状态估计器给出，状态变量的期望值为其中俯仰角和俯仰角速度的期望值通常设置为0，为了使机器人尽快达到WIPM模型中的动态平衡姿态，加快对给定速度的跟踪，θ^d可根据WIPM的ZMP约束求出：

由此，可以得到轮关节力矩为：

其中，和/>分别为期望的腿关节加速度、角度和关节速度，q_leg、/>分别为实际的腿关节角度和关节速度，τ^ff为关节前馈力矩，J表示雅可比矩阵，F表示需要向躯干施加的广义力旋量，K_p1、K_d1、K_P2、K_D2分别表示PD控制中的比例系数和微分系数；α_b为躯干横滚角，τ_leg和/>表示驱动腿关节的力矩值和腿关节的前馈力矩，τ_wheel、/>分别表示驱动轮关节的力矩值和轮关节的前馈力矩；/>表示质量矩阵，/>表示科氏力和离心力项，/>表示重力项，J表示雅可比矩阵，F表示需要向躯干施加的广义力旋量；θ^d表示俯仰角的期望值，K(L)表示最优反馈增益矩阵，X、X^d分别表示状态变量反馈值和状态变量期望值。

最终，将得到的腿关节力矩和轮关节力矩经过状态估计器(比如卡尔曼滤波器)得到机器人的当前运动状态数据，分别反馈至模型预测控制器和全身控制器。

实施例二

在一个或多个实施方式中，公开了一种轮式双足机器人的运动控制系统，包括：

控制器构建模块，用于基于躯干动力学模型构建模型预测控制器，基于轮-腿动力学模型构建全身控制器；

模型预测控制模块，用于通过模型预测控制器基于输入的运动控制指令输出需要向躯干施加的广义力旋量F；基于WIPM动力学求解机器人的躯干质心与车轮中心点在x轴方向的距离Δx；基于离心力补偿策略计算躯干横滚角，基于地形适应策略计算躯干坐标系下的车轮位置；

状态计算模块，用于基于得到的距离Δx、躯干横滚角和车轮位置，经过逆运动学求解得到期望的腿部关节角度、腿部关节速度和摆杆俯仰角，作为状态输入；

机器人控制模块，用于通过全身控制器基于模型预测控制器求解得到的广义力旋量F和逆运动学求解得到的状态输入，融合动力学前馈和关节反馈，最终得到控制机器人运动的腿部关节力矩和轮关节力矩。

上述各模块的具体实现方式已经在实施例一中进行了说明，此处不再详述。

实施例三

在一个或多个实施方式中，公开了一种终端设备，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现实施例一中的轮式双足机器人的运动控制方法。为了简洁，在此不再赘述。

应理解，本实施例中，处理器可以是中央处理单元CPU，处理器还可以是其他通用处理器、数字信号处理器DSP、专用集成电路ASIC，现成可编程门阵列FPGA或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器可以包括只读存储器和随机存取存储器，并向处理器提供指令和数据、存储器的一部分还可以包括非易失性随机存储器。例如，存储器还可以存储设备类型的信息。

在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。

实施例四

在一个或多个实施方式中，公开了一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行实施例一中所述的轮式双足机器人的运动控制方法。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种轮式双足机器人的运动控制方法，其特征在于，包括：

基于躯干动力学模型构建模型预测控制器，基于轮-腿动力学模型构建全身控制器；

模型预测控制器基于输入的运动控制指令输出需要向躯干施加的广义力旋量F；

基于WIPM动力学求解机器人的躯干质心与车轮中心点在x轴方向的距离Δx；

基于离心力补偿策略计算躯干横滚角，基于地形适应策略计算躯干坐标系下的车轮位置；

基于得到的距离Δx、躯干横滚角和车轮位置，经过逆运动学求解得到期望的腿部关节角度、腿部关节速度和摆杆俯仰角，作为状态输入；

全身控制器基于模型预测控制器求解得到的广义力旋量F和逆运动学求解得到的状态输入，融合动力学前馈和关节反馈，最终得到控制机器人运动的腿部关节力矩和轮关节力矩。

2.如权利要求1所述的一种轮式双足机器人的运动控制方法，其特征在于，基于躯干动力学模型构建模型预测控制器，具体为：

其中，U＝(u₀,u₁,…,u_n-1)表示预测步长内各个时刻的控制输入，k＝0,1,…,n-1；Q、R₁和R₂分别为权重矩阵，表示期望相邻时刻的控制输入不要出现跳变现象，以免造成机器人的不稳定；λ表示约束矩阵，使机器人始终保持在可控的运动状态中；Δt是控制器的采样时间，/>表示k时刻躯干的期望状态，x_k、x_k+1分别表示k和k+1时刻躯干的当前反馈状态，/>表示上一时刻的躯干输入力旋量，C和/>表示约束矩阵，/>和/>表示状态空间方程中的状态转移矩阵；对模型预测控制器进行求解，取当前时刻的最优解u₀作为需要向躯干施加的广义力旋量F。

3.如权利要求1所述的一种轮式双足机器人的运动控制方法，其特征在于，所述轮-腿动力学模型，具体为：

其中，q_j表示关节的角度值，j＝leg或wheel；表示各个关节的速度，τ表示驱动关节的力矩值；/>代表腿部的髋关节和膝关节，/>表示轮关节，τ_leg表示驱动腿关节的力矩值，τ_wheel表示驱动轮关节的力矩值；i＝L或R；M(q_j)表示质量矩阵，/>表示科氏力和离心力项，G(q_j)表示重力项，J表示雅可比矩阵，F表示需要向躯干施加的广义力旋量。

4.如权利要求1所述的一种轮式双足机器人的运动控制方法，其特征在于，基于WIPM动力学求解机器人的躯干质心与车轮中心点在x轴方向的距离Δx，具体为：

其中，z_b表示躯干的z方向的坐标，分别表示左、右轮腿子系统提供给躯干的前向推动力，/>分别表示左、右轮腿子系统提供给躯干的竖直支撑力。

5.如权利要求1所述的一种轮式双足机器人的运动控制方法，其特征在于，

基于离心力补偿策略计算躯干横滚角，具体为：

st.C_l≤z_b sinα_b≤C_r

其中，α_b为躯干横滚角，v_b表示躯干在世界坐标系中的前向速度，表示躯干的偏航角速度，g表示重力加速度，C_l和C_r分别表示左右轮腿的轮地接触点，z_b表示躯干的z方向的坐标，f_m(z_b,z_bmax,z_bmin)＝min(|z_b-z_bmax|,|z_b-z_bmin|)，表示z_b距离z_bmax和z_bmin之间的最小值，C_r-C_l表示接触点间的距离。

6.如权利要求1所述的一种轮式双足机器人的运动控制方法，其特征在于，基于地形适应策略计算躯干坐标系下的车轮位置，具体为：

其中，x_w、y_w、z_w分别表示车轮在躯干坐标系中的坐标。

7.如权利要求1所述的一种轮式双足机器人的运动控制方法，其特征在于，全身控制器基于得到的广义力旋量、距离Δs、躯干横滚角和车轮位置，求解得到腿关节加速度、腿部关节前馈力矩、轮关节前馈力矩和摆杆俯仰角，具体为：

其中，和/>分别为期望的腿关节加速度、角度和关节速度，q_leg、/>分别为实际的腿关节角度和关节速度，τ^ff为关节前馈力矩，J表示雅可比矩阵，F表示需要向躯干施加的广义力旋量，K_p1、K_d1、K_P2、K_D2分别表示PD控制中的比例系数和微分系数；α_b为躯干横滚角，τ_leg和/>表示驱动腿关节的力矩值和腿关节的前馈力矩，τ_wheel、/>分别表示驱动轮关节的力矩值和轮关节的前馈力矩；/>表示质量矩阵，/>表示科氏力和离心力项，表示重力项，J表示雅可比矩阵，F表示需要向躯干施加的广义力旋量；θ^d表示俯仰角的期望值，K(L)表示最优反馈增益矩阵，X、X^d分别表示状态变量反馈值和状态变量期望值。

8.一种轮式双足机器人的运动控制系统，其特征在于，包括：

控制器构建模块，用于基于躯干动力学模型构建模型预测控制器，基于轮-腿动力学模型构建全身控制器；

模型预测控制模块，用于通过模型预测控制器基于输入的运动控制指令输出需要向躯干施加的广义力旋量F；基于WIPM动力学求解机器人的躯干质心与车轮中心点在x轴方向的距离Δx；基于离心力补偿策略计算躯干横滚角，基于地形适应策略计算躯干坐标系下的车轮位置；

状态计算模块，用于基于得到的距离Δx、躯干横滚角和车轮位置，经过逆运动学求解得到期望的腿部关节角度、腿部关节速度和摆杆俯仰角，作为状态输入；

机器人控制模块，用于通过全身控制器基于模型预测控制器求解得到的广义力旋量F和逆运动学求解得到的状态输入，融合动力学前馈和关节反馈，最终得到控制机器人运动的腿部关节力矩和轮关节力矩。

9.一种终端设备，其包括处理器和存储器，处理器用于实现指令；存储器用于存储多条指令，其特征在于，所述指令适于由处理器加载并执行权利要求1-7任一项所述的轮式双足机器人的运动控制方法。

10.一种计算机可读存储介质，其中存储有多条指令，其特征在于，所述指令适于由终端设备的处理器加载并执行权利要求1-7任一项所述的轮式双足机器人的运动控制方法。