CN113021299B - 一种双腿轮复合机器人全方位运动控制方法 - Google Patents

Abstract

一种双腿轮复合机器人全方位运动控制方法，在任务空间中构造虚拟广义力，根据结构分工将虚拟广义力进行多点力分配，基于所提出的耦合腿轮动力学模型构造腿轮力矩解算器，得到能够实现腿轮末端分配力的关节力矩，基于轮地接触力雅可比，将维持航向的轮地接触补偿力映射为关节力矩，构造了基于内部传感器信息的坡面估计器，针对不同坡面进行轮与躯干水平相对位置的力矩补偿，将各层控制器解算所得关节力矩进行融合，施加于关节执行器。该方法通过合理控制双腿轮的运动，使得躯干位姿不受地面坡度变化的影响，能够保持稳定的全方位运动，增强了机器人的地形适应性，提高了非结构化地形行走的稳定性。

Description

一种双腿轮复合机器人全方位运动控制方法

技术领域

本发明涉及一种用于双腿轮复合机器人的全方位运动控制方法，属于机器人控制技术领域。

背景技术

在双腿机器人的肢腿末端安装主动驱动轮，形成了双腿轮复合式机器人。该种构型的机器人具备足够的主动自由度，理论上借助左右腿伸缩配合以及双轮的平衡与转向能力，能够实现连续不平坦地面上的全方位稳定行进。但双腿轮复合机器人运动时不存在静态稳定平衡点，需要施加反馈控制使其保持动态稳定，在此基础上，如何设计腿轮运动控制器实现机器人非平坦地面的全方位运动是有个有待解决的问题。

双腿轮复合构型的机器人出现较晚，目前公开的控制方法对双腿轮复合机器人的运动控制做了初步的探索，但是均无法充分发挥腿轮复合移动方式的优势。如发表于2019IEEE-RAS19^th International Conference on Humanoid Robots的文章《ModelDecoupling and Control of the wheeled Humanoid Robot Moving in SagittalPlane》(轮式人形机器人矢状面运动的模型解耦与控制)只探讨了机器人在矢状面中的运动与平衡控制，无法实现全方位运动；2020年发表于IEEE Robotics and AutomationLetters的《LQR-Assisted Whole-Body Control of a Wheeled Bipedal Robot WithKinematic Loops》(基于LQR辅助的轮式双足机器人运动学闭环的全身控制)实现了全方位运动，但未提到对非结构化地形的运动控制测试。2020年，Songyan Xin等人在网络平台上发表了相关工作《Online Dynamic Motion Planning and Control for Wheeled BipedRobots》(轮式双足机器人的在线动态运动规划与控制)，在全身控制框架下规划与实现了矢状面运动与侧向运动，并在不平坦地面进行了测试，但未将其他维度的控制归纳于总体控制框架之下。

以上现有控制方法，仍将主要关注点集中于轮式运动对双腿轮复合机器人的平衡控制，但腿与轮复合的形式有更大的运动潜力，将轮作为腿轮复合结构的一部分进行建模与控制，不对其简化处理，能够实现更好的控制效果。同时，现有方法未见将五维运动归纳在同一控制框架下的研究，缺乏系统性与实践性。

发明内容

针对现有双腿轮复合机器人运动控制技术存在的不足，本发明以双腿轮复合机器人为被控对象，提出一种面向非平坦地形的双腿轮复合机器人全方位运动控制方法，该方法以机器人躯干为任务空间，旨在通过合理控制双腿轮的运动，使得躯干位姿不受地面坡度变化的影响，能够保持稳定的全方位运动。

本发明的双腿轮复合机器人全方位运动控制框架，包括基于虚拟模型的双腿轮多点力分配、地面角度估计和平衡补偿，具体内容包括：

(1)给出躯干五维位姿的期望状态量，以躯干处的惯性测量单元实时检测到的位姿作为反馈，基于躯干单刚体动力学模型构造虚拟广义力；

(2)根据施力点与躯干的实时位姿关系，进行广义力分配，分别将对应的广义力分配为腿轮末端的期望广义力与轮地接触点的期望广义力；

(3)基于腿轮动力学模型构造腿轮子控制器，将腿轮末端期望广义力解算为关节力矩；

(4)基于轮地接触点的运动学闭环约束建立航向子控制器，将轮地接触点的期望广义力映射为关节力矩；

(5)利用内部传感器信息(不借助外部传感器)，对地面坡度进行实时估计，并保证估计值的平滑性(采用常规的卡尔曼滤波)；

(6)基于线性倒立摆模型的动力学特性，构造一个平衡姿态补偿控制器，解算出施加于轮关节的力矩补偿，为轮的水平方向运动增加阻尼，减小机器人运动的不稳定性；

(7)将各层控制器的输出进行整合，得到各关节的最终施加力矩，通过关节驱动器进行运动的执行。

所述步骤(1)中的虚拟广义力是构造五维虚拟广义力W_d；根据机器人各结构的具体功能，将W_d的不同维度分别分配为两个腿轮末端期望广义力F_ra与F_la，以及两轮地接触点的水平力F_rw与F_lw；基于建立的腿轮动力学模型构建腿轮力矩解算器，利用F_ra、F_la以及动力学前馈补偿获得驱动关节力矩τ_up；基于轮地力学模型将F_rw与F_lw解算为各关节力矩τ_lower；对机器人行进地形的坡度

进行实时估计，并以此为轮与躯干水平位置调整的依据，构造轮水平位置调整力矩τ_c，各层控制器所解算的力矩进行相加，施加于关节执行器。

所述步骤(3)中腿轮动力学模型(腿轮复合结构)的耦合建模方法是：

在对单侧腿轮进行动力学建模时，将转动的轮作为动基座处理，此时，小腿连杆在轮轴处获得了初始角加速度

与线加速度

采用牛顿-欧拉递归方法，得到单侧腿轮动力学模型：

其中，q＝[q₁ q₂ q₃]^T分别为小腿相对于轮转过的角度、大腿相对于小腿转过的角度、髋关节相对于初始位置转过的角度，τ＝[τ₁ τ₂ τ₃]^T分别为轮关节力矩、膝关节力矩与髋关节力矩，模型中关于q₁与θ₀的项都是一一对应的，进行合并后，动力学模型变为：

其中，q＝[q_b q₂ q₃]^T，q_b＝θ₀+q₁为小腿杆与竖直方向所呈角度。q_b状态量的控制对应于轮轴力矩τ₁的施加。

所述步骤(5)中对地面坡度进行实时估计的具体过程为：

基于内部传感信息设计坡度估计器，将历史时刻的轮轴位置依次记录到轮轴位置向量X_W与Z_W中，通过向量首个数据与尾部数据的操作，得到坡面估计值

所述坡度估计器中，运行频率为1000Hz，为保证坡面估计值的平滑，X_W与Z_W均为100维向量，坡面的估计公式为：

为进一步滤除内部传感器噪声与外部扰动造成的坡度估计不平衡，基于卡尔曼滤波方法对惯性测量单元与编码器抖动的外扰进行滤除；

其中，T为采样时间，

p₁＝p+R_Pnoise，p＝p₁(1-k)。所述观测噪声R_Onoise＝300，过程噪声R_Pnoise＝1。

本发明基于虚拟模型控制思想，将躯干五维虚拟广义力进行多点力分配，并以腿轮复合动力学模型为基础解算为各关节力矩；基于坡面估计算法，对轮与躯干的水平相对位置进行实时调整与控制；将多层控制器的力矩进行融合，施加于关节驱动器，实现了双腿轮复合机器人在连续非结构化地形上的全方位运动控制，适用于连续非结构化地形。

附图说明

图1是双腿轮复合机器人结构简图。

图2是双腿轮复合机器人力分配示意图。

图3是坡面估计原理图。

图4是总体控制框图。

具体实施方法

本发明提出了一种针对连续非结构化地形的双腿轮复合机器人全方位运动控制方法，以机器人躯干为任务空间，旨在通过合理控制双腿轮的运动，使得躯干位姿不受地面坡度变化的影响，能够保持稳定的全方位运动。在任务空间中构造虚拟广义力，根据结构分工将虚拟广义力进行多点力分配(参见图2)。基于所提出的耦合腿轮动力学模型构造腿轮力矩解算器，得到能够实现腿轮末端分配力的关节力矩；基于轮地接触力雅可比，将维持航向的轮地接触补偿力映射为关节力矩；构造了基于内部传感器信息的坡面估计器，针对不同坡面，进行轮与躯干水平相对位置的力矩补偿。将各层控制器解算所得关节力矩进行融合，施加于关节执行器。从而增强了机器人的地形适应性，提高了非结构化地形行走的稳定性。

本发明以图1所示机器人躯干的位姿为任务空间，将两腿轮作为主动悬挂，隔离了地面不平坦对躯干位姿的影响，从而实现躯干多维位姿的主动控制。

本发明的控制框架包括基于虚拟模型的双腿轮控制、航向控制、地面角度估计和平衡补偿四个主要部分；具体内容为：

(1)给出躯干五维位姿的期望状态量，以躯干处的惯性测量单元实时检测到的位姿作为反馈，基于躯干单刚体动力学模型构造虚拟广义力；

(2)根据施力点与躯干的实时位姿关系，进行广义力分配，分别将对应的广义力分配为腿轮末端的期望广义力与轮地接触点的期望水平力；

(3)基于腿轮动力学模型构造腿轮子控制器，将腿轮末端期望广义力解算为关节力矩；

(4)基于轮地接触点的运动学闭环约束建立航向子控制器，将轮地接触点的期望广义力映射为关节力矩；

(5)在不借助外部传感器的前提下，利用内部传感器信息，对地面坡度进行实时估计，并采用常规的卡尔曼滤波保证估计值的平滑性；

(6)为维持机器人系统内部的零动态，不同的运动状态下，轮轴与躯干在水平方向的相对位置不同，同时受到躯干加速度、地面坡度以及外力扰动等的影响。因此，基于线性倒立摆模型的动力学特性，构造了一个平衡姿态补偿控制器，解算出施加于轮关节的力矩补偿，为轮的水平方向运动增加阻尼，大大减小机器人运动的不稳定性。

(7)将各层控制器的输出进行整合，得到各关节的最终施加力矩，通过关节驱动器进行运动的执行。

以下结合附图对本发明的控制方法详细说明。

一.基于虚拟模型的躯干子控制器

将左腿轮施加于躯干的广义力看作两个虚拟作动器，受制于腿轮的结构，虚拟作动器的主动输出广义力为三维广义力F_ra＝[F_rx F_rz N_ry]^T、F_la＝[F_lx F_lz N_ly]^T，另外被动输出广义力为F_rp＝[F_ry N_rx N_rz]^T、F_lp＝[F_ly N_lx N_lz]^T。期望对躯干的前进速度v_x、站高z_b、俯仰角β、航向角α以及横滚角γ进行同时控制，其期望值为X_d＝[v_xd z_d γ_d β_d α_d]^T。以当前时刻的躯干位姿X、

关节空间状态量q、

为已知量，构造躯干虚拟广义力为：W_d＝W_FB+W_DM+W_P，分别由反馈部分W_FB、前馈动力学补偿部分W_DM以及被动广义力补偿部分W_P组成。其中，W_FB＝[F_x F_z N_x N_y N_z]^T为

F_x＝s+sat(s)c

N_z＝k_ω(ω_d-ω)

W_DM主要对重力、惯性力进行补偿，W_P对F_rp与F_lp作用于躯干的合力进行补偿。

二.多点力分配

双腿轮复合机器人的腿部与轮在功能实现上各有分工。腿部的运动主要实现本体姿态的调整，包括前向速度、站高、横滚以及俯仰角度，双轮差速能够实现机器人在三维空间的航向运动。因此，将F_x、F_z、N_x、N_y分配为左右腿轮的末端点处的广义力(参见图2)，将N_z分配为左右腿轮与地面接触点处的力。

其中，w为躯干宽度。显然，力分配雅可比J_F为行满秩的，考虑到受制于结构的全身姿态合理性，如左右髋关节同一时刻施加于躯干的力矩在不影响腿部运动的情况下应该保持N_ry＝N_ly；F_rx＝F_lx在本控制方法中能够保证左右腿轮姿态的一致性；当左右轮地接触点的水平推力F_rw＝F_lw时，能够在不影响水平速度的同时实现航向运动的驱动。将此三个约束条件加入上式后，力分配雅可比J_F的秩为8，可唯一的将躯干虚拟力分配为各点驱动力。

三.腿轮力矩解算

在对单侧腿轮进行动力学建模时，进行适当的简化，将转动的轮作为动基座处理，此时，小腿连杆在轮轴处获得了初始角加速度

与线加速度：

采用牛顿-欧拉递归方法，得到单侧腿轮动力学模型：

其中，q＝[q₁ q₂ q₃]^T分别为小腿相对于轮转过的角度、大腿相对于小腿转过的角度、髋关节相对于初始位置转过的角度。τ＝[τ₁ τ₂ τ₃]^T分别为轮关节力矩、膝关节力矩与髋关节力矩。模型中关于q₁与θ₀的项都是一一对应的，进行合并后，动力学模型变为：

其中，q＝[q_b q₂ q₃]^T，q_b＝θ₀+q₁为小腿杆与竖直方向所呈角度。q_b状态量的控制对应于轮轴力矩τ₁的施加。

基于公式进行力矩解算，为避免关节加速度测量误差对控制器的影响，只补偿动基座惯性力I(q)、重力G(q)以及科氏力、离心力

四.基于轮地力学模型的轮地接触力映射

当得到用于控制机器人航向运动的轮地接触力F_rw与F_lw时，其施加的前提是不影响腿部姿态的生成。轮地接触力的施加与各个关节的力矩均有关，而不再仅与轮轴力矩相关。因此，本部分首先基于速度闭环关系推导雅可比矩阵，然后利用该雅可比矩阵将轮地接触力映射为关节力矩。当腿轮处于支撑状态时，机器人形成运动学闭链，为使得轮地接触不发生滑动，假设躯干俯仰角为零，下式成立：

通过对时间求导得到雅可比矩阵J_yaw，即可将轮地接触力映射为关节力矩。

τ_lower＝J_yaw ^TF_w

其中，τ_lower＝[τ_lower1 τ_lower2 τ_lower3]^T分别为髋关节力矩、膝关节力矩、轮关节力矩。

五.运动姿态对坡面角度的自适应调整能够增加控制方法的鲁棒性。因此，基于内部传感信息设计了坡度估计器，其基本原理如图3所示。

由于双腿轮复合机器人无法基于单一时刻的状态参数确定地面坡度，将历史时刻的轮轴位置依次记录到轮轴位置向量X_W与Z_W中，通过向量首个数据与尾部数据的操作，得到坡面估计值

在本控制器(坡度估计器)中，运行频率为1000Hz，为保证坡面估计值的平滑，X_W与Z_W均为100维向量。坡面的估计公式为：

为进一步滤除内部传感器噪声与外部扰动造成的坡度估计不平衡，基于卡尔曼滤波方法对惯性测量单元与编码器抖动的外扰进行滤除。

其中，T为采样时间。

p₁＝p+R_Pnoise，p＝p₁(1-k)。本控制器中取观测噪声R_Onoise＝300，过程噪声R_Pnoise＝1，取得了较好的坡度估计效果。

六.通过第三步中腿部姿态的控制，机器人能够自主地向前或向后调整重心，τ_up中的轮关节力矩为实现q_b运动空间的控制会配合腿部运动实时保持平衡。

但当轮地接触点处受到主动阻力或推力时，轮轴与躯干的水平相对位置会偏离动态平衡点，从而使得机器人失去动态平衡状态。为提高应对地面外扰力的鲁棒性，对轮与躯干水平相对位置进行控制，构造一个轮关节补偿力矩τ_c。首先，根据当前躯干的期望加速度

速度跟随误差以及估计所得坡度构造期望的轮与躯干相对位置Δx：

其中，k_c、k_d为增益参数，R为轮的半径。然后，构造阻抗控制器，调节轮轴实际位置与期望位置之间的阻尼，产生防止轮轴偏离期望位置的阻尼力。

参数整定为k_p＝2，k_d＝6时，收到了良好的抑制轮的水平位置前后波动的效果。

七.总体控制框架如图4所示，其中，髋关节力矩与膝关节力矩由两部分组成，轮关节力矩由三部分组成，最终施加于各关节的力矩为：

Claims (3)

1.一种双腿轮复合机器人全方位运动控制方法，其特征是，包括以下步骤：

(1)给出躯干五维位姿的期望状态量，以躯干处的惯性测量单元实时检测到的位姿作为反馈，基于躯干单刚体动力学模型构造虚拟广义力；

(2)根据施力点与躯干的实时位姿关系，进行广义力分配，分别将对应的广义力分配为腿轮末端的期望广义力与轮地接触点的期望水平力；

(3)基于腿轮动力学模型构造腿轮子控制器，将腿轮末端期望广义力解算为关节力矩；

(4)基于轮地接触点的交互力学模型建立航向子控制器，将轮地接触点的期望广义力映射为关节力矩；

(5)利用内部传感器信息，对地面坡度进行实时估计，并保证估计值的平滑性；

(6)基于线性倒立摆模型的动力学特性，构造一个平衡姿态补偿控制器，解算出施加于轮关节的力矩补偿，为轮的水平方向运动增加阻尼，减小机器人运动的不稳定性；

(7)将各层控制器的输出进行整合，得到各关节的最终施加力矩，通过关节驱动器进行运动的执行；

所述步骤(3)中腿轮子控制器构造方法为：

在对单侧腿轮进行动力学建模时，进行适当的简化，将转动的轮作为动基座处理，此时，小腿连杆在轮轴处获得了初始角加速度

与线加速度

采用牛顿-欧拉递归方法，得到单侧腿轮动力学模型：

其中，q＝[q₁ q₂ q₃]^T分别为小腿相对于轮转过的角度、大腿相对于小腿转过的角度、髋关节相对于初始位置转过的角度，τ＝[τ₁ τ₂ τ₃]^T分别为轮关节力矩、膝关节力矩与髋关节力矩；将模型中关于q₁与θ₀的项进行合并后，动力学模型变为：

其中，q＝[q_b q₂ q₃]^T，q_b＝θ₀+q₁为小腿杆与竖直方向所呈角度，q_b状态量的控制对应于轮轴力矩τ₁的施加，基于腿轮一体化动力学模型分别构造左右腿轮的力矩解算器，为避免关节加速度测量误差对控制器的影响，只补偿动基座惯性力I(q)、重力G(q)以及科氏力、离心力

其中，F_ra＝[F_rx F_rz N_ry]^T、F_la＝[F_lx F_lz N_ly]^T为通过广义力分配方法得到的腿轮-躯干交互期望力，解算得到τ_iup为腿轮i关节力矩的第一部分；

所述步骤(4)中航向子控制器的构造具体过程为：

通过广义力分配方法得到用于控制机器人航向运动的轮地接触力F_rw与F_lw时，其施加的前提是不影响腿部姿态的生成，轮地接触力的施加与各个关节的力矩均有关，而不再仅与轮轴力矩相关，因此，本部分首先基于速度闭环关系推导雅可比矩阵，然后利用该雅可比矩阵将轮地接触力映射为关节力矩，当腿轮处于支撑状态时，机器人形成运动学闭链，为使得轮地接触不发生滑动，假设躯干俯仰角为零，下式成立：

通过对时间求导得到雅可比矩阵J_yaw，即可将轮地接触力映射为关节力矩；

τ_lower＝J_yaw ^TF_w；

其中，τ_lower＝[τ_lower1 τ_lower2 τ_lower3]^T分别为髋关节力矩、膝关节力矩、轮关节力矩的第二部分；

所述步骤(5)中对地面坡度进行实时估计的具体过程为：

基于内部传感信息设计坡度估计器，将历史时刻的轮轴位置依次记录到轮轴位置向量X_W与Z_W中，通过向量首个数据与尾部数据的操作，得到坡面估计值

所述坡度估计器中，运行频率为1000Hz，为保证坡面估计值的平滑，X_W与Z_W均为100维向量，坡面的估计公式为：

为进一步滤除内部传感器噪声与外部扰动造成的坡度估计不平衡，基于卡尔曼滤波方法对惯性测量单元与编码器抖动的外扰进行滤除；

其中，T为采样时间，

p₁＝p+R_Pnoise，p＝p₁(1-k)，其中，R_Onoise为观测噪声，R_Pnoise为过程噪声；

所述平衡姿态补偿控制器的构造方法为：

通过第三步中关节力矩的控制，机器人能够自主地向前或向后调整重心，τ_up中的轮关节力矩为实现q_b运动空间的控制会配合腿部运动实时保持平衡，但当轮地接触点处受到主动阻力或推力时，轮轴与躯干的水平相对位置会偏离动态平衡点，从而使得机器人失去动态平衡状态，为提高应对地面外扰力的鲁棒性，对轮与躯干水平相对位置进行控制，构造一个轮关节补偿力矩τ_c，首先，根据当前躯干的期望加速度

速度跟随误差以及估计所得坡度构造期望的轮轴与躯干质心的水平相对位置Δx：

其中，k_c、k_d为增益参数，R为轮的半径；然后，构造阻抗控制器，调节轮轴实际位置与期望位置之间的阻尼，产生防止轮轴偏离期望位置的阻尼力：

参数整定为k_p＝2，k_d＝6时，收到了良好的抑制轮的水平位置前后波动的效果。

2.根据权利要求1所述的双腿轮复合机器人全方位运动控制方法，其特征是：

所述步骤(1)中的虚拟广义力是构造五维虚拟广义力W_d；根据机器人各结构的具体功能，将W_d的不同维度分别分配为两个腿轮末端期望广义力F_ra与F_la，以及两轮地接触点的水平力F_rw与F_lw；基于建立的腿轮动力学模型构建腿轮力矩解算器，利用F_ra、F_la以及动力学前馈补偿获得驱动关节力矩τ_up；基于轮地力学模型将F_rw与F_lw解算为各关节力矩τ_lower；对机器人行进地形的坡度

进行实时估计，并以此为轮与躯干水平位置调整的依据，构造轮水平位置调整力矩τ_c，各层控制器所解算的力矩进行相加，施加于关节执行器。

3.根据权利要求1所述的双腿轮复合机器人全方位运动控制方法，其特征是，所述观测噪声R_Onoise＝300，过程噪声R_Pnoise＝1。

Images