双轮腿-臂机器人全身运动规划与控制方法

1.本发明涉及一种用于双轮腿-臂机器人全身运动规划与动态平衡运动的控制器设计，属于机器人控制领域。


背景技术：

2.双轮腿-臂机器人是兼具手臂作业能力、不平坦地形适应能力、轮式灵活移动能力等优势的超仿生机器人。且能够适应人类工作环境，具备与人类自然交互的潜能，具有广阔的应用前景。它的运动优越性表现在：第一，腿式结构可主动隔振，即躯干运动轨迹与足端轨迹解耦，在起伏路面仍可保持躯干平稳；第二，腿式结构赋予其跨越障碍的能力，提高了对崎岖地面的适应性；第三，双轮移动形式可实现零半径转弯，提高了移动平台的灵活性；第四，轮式结构可在平坦地面上低功耗的快速移动，也可适应崎岖度较小的地形；第五，手臂可以搭载作业工具，配合人类实现协同作业任务。
3.双轮腿-臂机器人无法保持静态平衡，需要借助控制算法实现作业时的动态稳定，因此动态性能的好坏严重依赖于控制器设计的优劣。而双轮腿-臂机器人各结构间存在强耦合，腿臂作业任务直接影响动态平衡的保持。现存控制方法本质上多为模块化方法，忽略了各模块间的耦合特性，制约了动态稳定裕度。
4.如2020年发表于ieee robotics and automation letters的文章《lqr-assisted whole-body control of a wheeled bipedal robot with kinematic loops》(基于运动学闭链的双腿轮机器人辅助lqr全身控制)该控制方法的核心为多任务规划部分，站高、横滚、航向任务以及lqr任务(平衡任务)均在运动学层面进行规划，生成一组关节期望加速度。且在构造lqr任务时，将ascento简化为两轮倒立摆模型，规划出期望的虚拟摆杆俯仰角，作为任务空间。这一简化忽略了腿部动态运动对动态平衡的影响，制约了机器人的动态性能。
5.2021年发表于ieee international conference on robotics and automation(icra)的《balance control of a novel wheel-legged robot:design and experiments》(一类新型腿轮机器人的平衡控制：设计与实验)将机器人简化为两轮倒立摆，在采用传统控制率lqr的基础上，设计了基于ida-pbc(interconnection and damping assignment-passivity-based control)的非线性控制器，以获得更大的动态稳定裕度。
6.可见，现有的双轮腿机器人大多不具备手臂作业的能力，而就双轮腿机器人的运动控制而言，还处在解耦控制的阶段，通过将除轮以外的其他杆件简化为一个虚拟质心点来构造双轮倒立摆，控制轮关节电机的运动实现平台的平衡；其他关节用以配合轮的平衡或实现躯干与腿部姿态调整。这类控制方法所获取的动态稳定范围较小，当腿部动态运动产生的惯性力较大时，会影响平衡环节甚至导致平衡状态的发散。因此，如何实现双轮腿-臂机器人全身平衡与运动的统一规划与控制，是突破双轮腿-臂机器人运动性能的关键所在。因此构建一个可以将轮腿运动、手臂操作等功能集于一体的轮腿臂复合机器人控制框架，实现多目标运动任务与实时全身动态平衡是一个亟待解决的问题。
7.中国专利申请cn 113021299 a公开了一种双腿轮复合机器人全方位运动控制方法，在任务空间中构造虚拟广义力，根据结构分工将虚拟广义力进行多点力分配，基于所提出的耦合腿轮动力学模型构造腿轮力矩解算器，得到能够实现腿轮末端分配力的关节力矩，基于轮地接触力雅可比，将维持航向的轮地接触补偿力映射为关节力矩，构造了基于内部传感器信息的坡面估计器，针对不同坡面进行轮与躯干水平相对位置的力矩补偿，将各层控制器解算所得关节力矩进行融合，施加于关节执行器。该方法通过合理控制双腿轮的运动，使得躯干位姿不受地面坡度变化的影响，能够保持稳定的全方位运动，增强了机器人的地形适应性，提高了非结构化地形行走的稳定性。但是仅可进行快速稳定的移动并不是动基座机器人研制的最终目的。因此，基于前期工作，在双腿轮机器人的躯干上侧布置一条三自由度机械臂，形成双轮腿-臂机器人，从而赋予机器人作业的潜能。随之而来的，现有的控制器无法实现双轮腿-臂机器人的全身运动与作业控制。
8.与双腿轮机器人相比，双轮腿-臂机器人的控制难点有三：
9.其一：不同于各种静态稳定腿足机器人底盘或轮式底盘，双轮腿底盘只能保持动态平衡，因此，不存在稳定的手臂工作空间，需要实时进行双轮腿运动调整使得机器人系统整体处于动态平衡点；
10.其二：以手臂末端为任务空间时，双轮腿-臂机器人系统为冗余系统，对于同一任务存在多种运动序列；
11.其三：手臂作业时，需要同时控制手臂末端位置与力的输出，且末端力输出直接影响机器人的平衡状态。
12.为解决以上控制难点，实现动态平衡下的手臂作业，特提出一种双轮腿-臂机器人全身运动规划与控制方法。


技术实现要素：

13.本发明的目的是为克服上述现有技术的不足，提供一种双轮腿-臂机器人全身运动规划与控制方法，该控制方法以机器人躯干位姿、手臂位置为任务空间，双轮腿基座根据多重任务实时规划关节力矩，从而实现动态平衡下的躯干运动与手臂作业。
14.为实现上述目的，本发明采用下述技术方案：
15.双轮腿-臂机器人全身运动规划与控制方法，包括基于分布式模型的双轮腿-臂机器人力传递分析、基于分层优化的广义关节空间运动规划方法、基于手臂-躯干交互力前馈补偿的关节力矩控制三个主要部分，具体如下：
16.(1)在双轮腿系统力分析的基础上进行了双轮腿-臂机器人动力学分析，将手臂的运动与作业对机器人产生的稳定性影响映射为躯干广义力的一部分，进行统一规划与处理；
17.(2)根据实际需求所确定的手臂末端、躯干位姿任务空间，提出多任务空间(躯干位姿任务、手臂末端位置任务)分层优化方法，同时实现多个具有不同优先级任务空间的参考轨迹规划；
18.(3)手臂与环境的交互力(操作物的质量以及运动状态)对双轮腿-臂机器人的动态稳定产生实时影响，以此交互力为控制目标，构造前馈补偿项加入到全身力矩控制器中，补偿手臂末端广义力对全身关节力矩所产生的影响。
19.本发明的有益效果是：在该全身规划与控制框架下，双轮腿-臂机器人以能耗为优化目标，确定同时满足手臂末端操作目标与躯干运动目标的唯一的全身运动参考轨迹，解决了双轮腿-臂机器人全身运动规划的冗余问题；对中国专利申请cn 113021299 a公开的一种双腿轮复合机器人全方位运动控制方法进行了扩展，对手臂与操作物动态力进行前馈广义力补偿，使得轮腿姿态主动调整以平衡手臂交互力，增加了全身稳定鲁棒性。
20.本发明针对双轮腿-臂机器人躯干、手臂协同运动以及手臂末端力输出需求，提出了基于分层优化的多任务空间规划方法，解决了全身运动规划冗余问题。基于二级桌子-小车模型对轮腿-躯干-手臂力传递进行分析，将手臂运动与作业虚拟广义力通过前馈补偿的形式纳入躯干广义力规划。分析左右轮腿力分配特点，提出了基于二次规划的自平衡全身力矩控制器，将各层非线性约束显性化，从而解算出满足全身动态平衡的轮腿力矩。通过该力矩的施加所生成的轮腿姿态能够保持双轮腿-臂机器人处于动态平衡点，提高了运动作业的动态稳定性。
附图说明
21.图1是双轮腿-臂机器人一种作业类型示意图；
22.图2是手臂-躯干交互广义力简化分析图；
23.图3是双轮腿-臂机器人控制框架图；
24.图4是轮腿-躯干交互力与姿态的关系示意图；
25.图5是基于二次规划的自平衡全身力矩控制器；
26.图6是手臂子系统力传递示意图。
具体实施方式
27.下面结合附图和实施例对本发明进一步说明。
28.本说明书附图所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容涵盖的范围内。同时，本说明书中所引用的如“上”、“下”、“左”、“右”、“中间”及“一”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。
29.如图1-图6所示，双轮腿-臂机器人全身运动规划与控制方法，包括基于分布式模型的双轮腿-臂机器人力传递分析、基于分层优化的广义关节空间运动规划方法、基于手臂-躯干交互力前馈补偿的关节力矩控制，具体如下：
30.1.基于分布式模型的双轮腿-臂机器人力传递分析的全身控制框架搭建
31.对双轮腿-臂机器人而言，在手臂作业的过程中，矢状面的动态平衡仍然是控制的重中之重。手臂运动、末端与环境的交互力均通过手臂、躯干传递给双轮腿子系统，对双轮腿的动态平衡运动产生影响，同时，双轮腿子系统的姿态根据动态平衡要求实时生成。因此，需要综合考虑躯干运动需求与手臂作业需求，对手臂子系统运动与作业所需力进行建模与前馈补偿，以期实现双轮腿-臂机器人的动态稳定运动与作业控制。
32.手臂运动与作业所需力本质上由轮-地交互力提供，但直接规划轮-地交互力非常复杂，而躯干作为各附肢互连、力传递的桥梁，适合于选取为合力的分析点。采用如图2所示的桌子-小车模型进行分析。在双轮腿系统的基础上，将手臂杆件与末端所受外力集中于一点，产生一组力f
hx
与f
hz
，形成以躯干为基座的第二级桌子-小车模型。此时，轮腿不仅需要提供维持躯干运动的广义力f
bx
与f
bz
，还需要提供维持手臂、末端力输出的广义力，同时根据所输出的综合广义力产生相应的姿态以保持动态平衡。本实施例将躯干作为轮腿-手臂广义力传递的桥梁，如图2所示，将第二级小车模型所需广义力映射到第一级小车模型(即躯干)上去，作为外力进行前馈补偿，然后，进行轮腿力矩解算，实现手臂运动与作业时的双轮腿-臂机器人动态稳定运动。
33.躯干单刚体受到来自左右轮腿子系统与手臂子系统的三组广义力，分别定义为w
l
、wr与wa。因此，躯干单刚体的动力学表达式可描述为：
[0034][0035]
其中，mb为躯干质量，ib为躯干的转动惯量，pb为躯干的位姿向量，j
torso
与gb分别为力雅克比矩阵与重力矩阵。w
l
、wr与wa三组广义力在躯干质心处的合力需要推动躯干沿期望运动轨迹行进，而wa的大小根据手臂交互力的变化进行规划。因此，对于躯干子系统而言，wa作为广义力前馈补偿项，w
l
、wr作为驱动躯干运动的控制输入。
[0036]
此时，躯干状态空间表达式为：
[0037][0038]
其中，u＝[w
l wr]
t
为左右轮腿对躯干的驱动力输入，bwa为wa在躯干质心处的映射，s为输入矩阵。此时，左右轮腿所提供的映射到躯干质心处的广义力即wb可描述为：
[0039]
wb＝jbu
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0040]
其中，雅克比矩阵jb＝s
t
。
[0041]
可见，轮腿子系统与手臂子系统作用于躯干的合力驱动躯干产生运动，然而，双轮腿-臂机器人的可选任务空间不仅为躯干运动空间，还有手臂末端相对于惯性系的运动空间以及手臂末端输出力。因此，双轮腿-臂机器人的运动控制主要有以下三种基本任务：
[0042]
(1)以手臂末端运动为控制目标；
[0043]
(2)以躯干运动为控制目标；
[0044]
(3)以手臂末端力输出为控制目标。
[0045]
当控制双轮腿-臂机器人执行某一任务时，一般为以上三种基本任务的叠加。为实现以上运动任务控制，对于手臂子系统而言，控制目标为手臂末端的运动与手臂末端输出力，输入为bwa。对于躯干子系统，其被控量为躯干运动以及传递给手臂子系统的广义力bwa，输入为w
l
、wr。对于轮腿子系统，其被控量为w
l
、wr，通过施加关节力矩为躯干子系统提供广义力。
[0046]
双轮腿-臂机器人的控制目标可以为单一任务空间的运动，如仅对手臂末端状态进行约束(其中，dph与为手臂末端位姿与位姿变化率的期望值在世
界坐标系中的表示)，不对躯干运动状态做出要求；也可以同时提出两个任务空间的期望轨迹(其中，dpb与为躯干位姿与位姿变化率的期望值在世界坐标系中的表示)。两个任务空间并不是完全解耦的，存在相互制约的关系，因此，并不能满足任意期望轨迹的执行。为了保证控制器所接收执行信号的有效性，构造上层规划器，根据不同工况的实际情况进行任务空间优先级分层，采用优化方法规划出各任务空间的可行参考轨迹。
[0047]
在控制层，构造基于二次规划的自平衡全身力矩控制器，在躯干虚拟广义力期望值wd构造时，对手臂广义力在躯干质心处的作用力bwa进行前馈补偿项。这样既保留了双轮腿系统与手臂子系统的广义力、速度传递关系，又使得各子系统相对独立，从而保证计算实时性。整体框架如图3所示。
[0048]
下面将对基于分层优化的广义关节空间运动规划方法与基于二次规划的自平衡全身力矩控制器展开介绍。
[0049]
2.基于分层优化的广义关节空间运动规划方法
[0050]
定义机器人的广义关节空间为广义坐标q＝[q
b qa]
t
，其中，qb＝[x
b y
b z
b α]
t
为躯干位姿，由躯干三维位移xb、yb、zb以及航向角度α组成，qa＝[θ
3 θ
4 θ5]
t
为手臂关节空间，由肩部航向关节角度θ3、肩部俯仰关节角度θ4以及肘俯仰关节角度θ5组成。不失一般性，假设存在n个可控的任务空间，其中，第j个任务空间的状态量定义为xj，其期望的状态空间定义为使得任务空间的位姿与速度最大限度伺服到期望位姿与速度，从而得到广义关节空间的参考加速度
[0051][0052]
其中，q为正定对角权重矩阵。为任务j的参考加速度，通过比例-微分控制器得到：
[0053][0054]
其中，k
pj
与k
dj
分别为比例与微分系数。
[0055]
由于机器人的冗余特性，仅以公式(4)为目标函数，并不能唯一的确定躯干与手臂关节运动，从而使得任务空间j的位置与速度尽可能达到因此，将广义关节空间的运动位置增量进行加权平方，构造新的目标函数：
[0056][0057]
其中，r为正定对角权重矩阵。q为本次控制率施加到机器人后产生的广义关节空间位置的预测，通过如下式的离散预测得到：
[0058][0059]
其中，q
pre
与分别为上一采样周期广义关节的角度与角速度，采用传感器测得。δt为采样时间。
[0060]
至此，目标函数的构造同时考虑了手臂末端任务及关节空间运动的优化目标，同
时，机器人自身的关节空间约束、执行器性能约束以及非完整约束等必须严格满足，因此，进行等式约束与不等式约束构造。
[0061]
为约束广义关节空间的变量不超出上下界限，设置如下式的双边不等式约束
[0062][0063]
其中，q与分别为广义关节空间的位置下界与上界。
[0064]
广义关节空间中包含了躯干的三维运动xb、yb、zb以及姿态α，另外，默认躯干的期望俯仰角与期望横滚角为0，不需要规划器实时生成。受到结构限制，机器人无法直接发生侧向移动，只能通过控制前向运动与航向的耦合来实现侧向运动。因此，当规划器进行躯干位姿实时规划时，添加约束以保证位姿间的耦合关系：
[0065][0066]
其中，
[0067]
同时，由于双轮腿需要保持实时动态稳定，受到外界环境的突发变化，机器人的平衡状态将由当前平衡点跳变为另一个不连续的动态稳定平衡点，为了保证转换过程的连续平滑，构造加速度变化量的约束：
[0068][0069]
其中，ε为人为设定的阈值，可通过多次实验确定。以公式(6)为目标函数，以公式(8)、(9)、(10)为约束条件便可优化求解出面向任务j的广义关节加速度期望轨迹。
[0070]
当同一时刻面临多个不同任务时，每一任务空间的优化解算都作为一个二次规划问题(quadratic programming，qp)，定义con为约束集合，包括等式约束与不等式约束，面向任务1时con1中的约束为公式(8)，(9)，(10)。定义第j-1任务空间的qp问题为qp
j-1
，其解算输出定义为该输出为符合约束条件前提下，完成任务j-1的最优选择，为保证任务j-1的执行在后续任务规划中不被破坏，利用构造如下新的约束，并更新约束集合conj：
[0071][0072]
其中，为任务j-1所解算的关节加速度向末端任务参考加速度的运动学映射，为后续规划任务的加速度变量。j
j-1
为任务空间j-1的速度雅克比矩阵。可见，任务j的新增约束条件如公式(12)所示。
[0073][0074]
分层优化算法的执行流程如下所示：
[0075]
(1)输入各层任务的期望值：
[0076]
(2)输入各任务空间当前反馈值：x1，x2……
xn；
[0077]
(3)输入初始约束
[0078]
(4)计算当前任务空间的参考值
[0079]
(5)以conj为约束，计算使得实际值与参考值误差最小的关节空间加速度
[0080]
(6)更新约束集合使得低优先级任务空间的规划结果不会破坏高优先级执行需求；
[0081]
(7)当j＜n+1，跳转到(4)，否则，结束程序，输出
[0082]
将最底层任务空间的优化输出作为广义关节加速度对速度与位置进行更新，得到参考轨迹。
[0083]
3.基于二次规划的自平衡全身力矩控制器
[0084]
发明人在前期工作中，提出了一种轮腿一体化结构动力学描述方法如公式(13)所示，并申请发明专利。
[0085][0086]
其中，下标i为左右轮腿的编号(下文采用相同定义方式)。qi为轮腿i的关节空间。ai为轮式动基座相对于惯性系的水平加速度，τi为轮腿i的主动关节力矩。mi、ci、gi、ii、ji分别为轮腿i的惯性矩阵、科氏力离心力矩阵、重力项、初始惯性力矩阵以及末端力雅克比矩阵。
[0087]
基于该模型构造以末端输出力为任务空间的关节力矩解算器时，通过观测信号对初始状态量造成的杆件惯性力进行前馈补偿，实验验证了该方法具有以下特征：
[0088]
1)轮腿末端纵向力与水平力的输出与轮腿姿态具有如图3所示的关系。
[0089]
2)根据所规划的轮腿末端期望力，直接进行关节力矩解算与控制，便能够得到使轮腿处于动态平衡的姿态，不需要进行轮腿姿态控制。
[0090]
以此为基础进行基于二次规划的自平衡全身力矩控制器。
[0091]
3.1轮腿末端力约束分析
[0092]
左右轮腿的末端力wi＝[f
ix f
iz n
iy
]
t
为躯干运动的输入，其中，f
ix
、f
iz
、n
iy
分别为水平推力，纵向推力与俯仰力矩。当进行左右轮腿的末端力分配时，末端力的分配比例直接影响左右轮腿所呈的姿态。为了使左右侧轮腿同时保持相同的姿态角，末端力分配需要满足以下约束条件：
[0093][0094]
躯干受到的航向合力矩(定义为nz)用来控制躯干航向姿态，但要想实现轮腿系统的航向控制，需要同时驱动躯干与左右轮腿的转动，且不能破坏矢状面平衡姿态。因此，对于航向控制来说，将躯干处的虚拟力矩分配到轮腿-躯干铰接点是不合理的，应分配到轮地
接触点处，使其在不改变矢状面与冠状面平衡状态的前提下，推动系统整体的转动。定义驱动整体系统转动的力矩为n
zw
，轮腿末端的广义力均为系统内部力，不能够对n
zw
产生影响。因此，在轮地接触点处定义轮行进方向的水平力f
rα
和f
lα
作为推动机器人产生航向运动，f
rα
和f
lα
为轮-地接触力中的一部分，且满足公式(15)：
[0095][0096]
其中，w为躯干宽度。
[0097]
另外，n
zw
所驱动的转弯运动旋转轴为过躯干质心的纵向轴，因此，f
rα
和f
lα
生成的运动为自转，为了不影响前进维度的控制，两者应满足约束：
[0098]frα
+f
lα
＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0099]
因此，航向运动由水平运动与自转运动耦合而成，可生成任意半径的运动。
[0100]
同时，由于髋关节处仅存在一个俯仰关节，为了保持躯干俯仰角调节的一致性，对左右轮腿进行虚拟俯仰力矩分配时，需要满足以下约束：
[0101]nry
＝n
ly
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0102]
将约束(15)、(16)、(17)与公式(3)合并，躯干质心合力与轮腿末端力的分配关系整理为：
[0103]
wb′
＝jb′u′ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0104]
其中，wb′
＝[w
b 0 0]
t
，u
′
＝[w
l w
r f
rα f
lα
]
t
，jb′
为融合了力映射雅克比与约束系数的雅克比矩阵。
[0105]
为了控制躯干位姿跟踪期望轨迹dpb＝[x
d z
d γ
d β
d αd]
t
，假设躯干通过五维虚拟关节与大地坐标系固连，躯干受到五维虚拟广义力的“驱动”沿期望轨迹运动。构造虚拟广义力wb的期望值wd由两部分组成：
[0106]
wd＝ξ+wcꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0107]
其中，ξ＝[f
xfb f
zfb n
xfb n
yfb n
zfb
]
t
为躯干位姿与速度误差反馈部分，wc为前馈补偿部分，分别采用公式(20)与公式(21)计算得到：
[0108][0109][0110]
其中，k
pz
、k
dz
、k
pγ
、k
nγ
、k
pβ
、k
dβ
、k
pα
、k
dα
、λ均为正反馈增益，躯干的水平运动速度v
x
、站高z、横滚角γ、俯仰角β、航向角α及其速度通过imu实时测量得到。h为躯干单刚体的高度。
[0111]
3.2手臂驱动力前馈补偿分析
[0112]
手臂附肢的运动与手臂末端力输出的前提皆为躯干所提供的与手臂铰接点处的广义力wa。如图6所示，wa与手臂末端输出力、关节力矩、各杆件广义力均有关，将力与力矩沿手臂杆件由外向内迭代，建立力传递模型如公式(22)所示。
[0113][0114]
其中，项补偿关节加速度产生的杆件惯性力；cw项补偿杆件运动所产生的科氏力与离心力；gw项补偿手臂杆件重力以及躯干动基座带来的初始惯性力；j
ftfobj
补偿手臂末端输出力，其中，jf为手臂末端力向手臂-躯干铰接点处映射的力雅克比矩阵。
[0115]
手臂一般需要与环境或操作对象接触，针对具体应用场景，有时需要精确的位置控制，同时以末端输出力为控制目标。以搬运重物为例，对手臂系统进行受力分析，如图6所示，手臂与重物接触点处，受到来自重物的力f
obj
，同时施加大小相等、方向相反的力与力矩于重物。手臂末端输出广义力的大小与重物的质量、运动状态息息相关。当确定广义关节空间期望运动状态qd、后，能够预测手臂末端期望加速度为其中，ja为广义关节空间向手臂末端操作空间映射的速度雅克比矩阵。因此，以操作对象为目标进行动力学分析。
[0116]
简化目标重物为单刚体，定义其位姿为其中j≤6。重物的动力学模型为：
[0117][0118]
其中，为重物单刚体的惯性矩阵；为重物单刚体的重力向量。将公式(23)代入公式(22)，得到：
[0119][0120]
此处假设wp
obj
＝wph，因此，公式(24)可进一步整理为：
[0121][0122]
躯干期望广义力的构造需要同时考虑躯干运动状态，手臂、负载重物所需广义力的补偿。公式(19)对躯干广义力的构造更新为公式(26)：
[0123]
wd＝ξ+wc+bwaꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0124]
其中，为手臂所需期望驱动力在躯干质心处的映射。
[0125]
同时，手臂关节力矩构造如下：
[0126][0127]
其中，τa为手臂关节力矩向量，k
pa
与k
da
为正反馈增益。与为期望关节位置与速度参考值。
[0128]
3.3全身力矩控制器设计
[0129]
将左右轮腿的力与力矩解算问题转化为二次规划解算问题(quadratic programming problem，简称qp)，定义该qp解算的变量为：
[0130]
x＝[f
rx f
rz n
ry f
lx f
lz n
ly f
rα f
lα τ
r0 τ
r1 τ
r2 τ
l0 τ
l1 τ
l2
]
t
ꢀꢀꢀꢀꢀꢀꢀ
(28)
[0131]
其中，τ
r0
、τ
r1
、τ
r2
、τ
l0
、τ
l1
、τ
l2
分别为右轮腿的轮关节、膝关节、髋关节力矩与左轮腿的轮关节、膝关节、髋关节力矩。
[0132]
为使轮腿-躯干接触力与躯干所需广义力满足公式(18)，将其整理为如下形式：
[0133]
b1＝a1x
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(29)
[0134]
其中，b1＝wb′
，a1＝[jb′ꢀ06
×6]。
[0135]
基于轮腿一体化模型如公式(13)，以及两连杆腿部雅克比矩阵，构造力矩τ＝[τ
r0 τ
r1 τ
r2 τ
l0 τ
l1 τ
l2
]
t
解算部分为：
[0136][0137]
其中，而j
wi
为两连杆腿部雅克比矩阵，j
wi
(1)为取矩阵j
wi
的第一列。u＝[f
rx f
rz n
ry f
lx f
lz n
ly
]
t
为左右轮腿的末端力。c对关节力矩进行重新排列。
[0138]
将(30)整理为qp标准形式为：
[0139]
b2＝a2x
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(31)
[0140]
其中，
[0141]
公式(29)所示的上层控制器解算部分与公式(31)所示的力矩解算部分合并为一个qp解算问题：
[0142]
ax＝b
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(32)
[0143]
其中，
[0144]
将力分配约束公式(14)整理为如下形式：
[0145]
x(0)x(4)-x(1)x(3)＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(33)
[0146]
由于末端广义力与姿态一一对应，如图4所示，其中，l为轮地接触点与髋关节的直线距离，l
x
为髋关节与轮地接触点的水平距离，lz为髋关节垂直高度，f
x
与fz为躯干受到的二维合力。考虑到轮腿受到关节运动范围的限制，腿部伸缩与调整能力是有限的，必须满足的约束，其中，l、分别为髋关节与轮地接触点的最小允许距离与最大允许距离。因此，姿态的运动反向限制约束力的分配，f
x
＝f
rx
+f
lx
与fz＝f
rz
+f
lz
应满足如下约束：
[0147][0148]
将该约束整理为两个单边约束：
[0149]
[0150][0151]
同时，因为地面只能提供支持力，而不能提供吸引力，因此纵向力fz必须为正，即：
[0152][0153][0154]
为保证所计算的各关节力矩在关节电机输出扭矩范围之内，使得：
[0155][0156]
至此，能够构成一个完整的具有凸约束的qp解算问题：
[0157][0158]
其中，qp的输出向量x中关节力矩τ＝[τ
r τ
l
]
t
作为最终的执行力矩。μ为地面摩擦系数。
[0159]
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

技术特征：
1.一种双轮腿-臂机器人全身运动规划与控制方法，包括：(1)在双轮腿系统力分析的基础上进行了双轮腿-臂机器人动力学分析，将手臂的运动与作业对机器人产生的稳定性影响映射为躯干广义力的一部分，进行统一规划与处理；(2)根据实际需求所确定的手臂末端、躯干位姿任务空间，提出多任务空间分层优化方法，解决双轮腿-臂机器人全身运动规划的冗余，同时实现多个具有不同优先级任务空间的参考轨迹规划，操纵目标物的同时驱动自身运动；(3)手臂与环境的交互力对双轮腿-臂机器人的动态稳定产生实时影响，以此交互力为控制目标，构造前馈项补偿项加入到全身力矩控制器中，补偿手臂末端广义力对全身关节力矩所产生的影响，从而实现对操作物动态力的前馈广义力补偿，增加了全身稳定鲁棒性。2.如权利要求1所述的双轮腿-臂机器人全身运动规划与控制方法，其特征是，所述双轮腿-臂机器人动力学分析，在双轮腿系统力分析的基础上，将手臂杆件与末端所受外力集中于一点，形成以躯干为基座的第二级桌子-小车模型，将躯干作为轮腿-手臂广义力传递的桥梁，将第二级小车模型所需广义力映射到第一级小车模型即躯干上去，作为外力进行前馈补偿，然后，进行轮腿力矩解算，实现手臂运动与作业时的双轮腿-臂机器人动态稳定运动。3.如权利要求1所述的双轮腿-臂机器人全身运动规划与控制方法，其特征是，所述多任务空间分层优化方法，双轮腿-臂机器人的控制目标为单一任务空间的运动或者同时提出两个任务空间的期望轨迹；两个任务空间并不是完全解耦的，存在相互制约的关系，因此，并不能满足任意期望轨迹的执行；为了保证控制器所接收执行信号的有效性，构造上层规划器，根据不同工况的实际情况进行任务空间优先级分层，采用优化方法规划出各任务空间的可行参考轨迹；根据分层优化算法，将最底层任务空间的优化输出作为广义关节加速度对速度与位置进行更新，得到参考轨迹。4.如权利要求1所述的双轮腿-臂机器人全身运动规划与控制方法，其特征是，所述全身力矩控制器，将左右轮腿的力与力矩解算问题转化为二次规划解算问题(quadratic programming，qp)，定义该qp解算的变量为：x＝[f
rx f
rz n
ry f
lx f
lz n
ly f
rα f
lα τ
r0 τ
r1 τ
r2 τ
l0 τ
l1 τ
l2
]
t
其中，f
ix
、f
iz
、n
iy
分别为轮腿i与躯干铰接点处的水平力、竖直力以及俯仰力矩，f
iα
为轮腿i的轮地接触点处推动机器人航向运动的水平力；τ
i0
τ
i1
τ
i2
分别为轮腿i的轮关节力矩、膝关节力矩与髋关节力矩；同时将力分配约束、轮腿姿态约束、关节力矩约束分别转化为二次规划的目标函数与约束，构成一个完整的具有凸约束的qp解算问题：(1)目标函数构造：轮腿-躯干接触力与躯干所需广义力应满足：b1＝a1x其中，b1＝w
b
′
，a1＝[j
b
′ꢀ06
×6]，其中，w
b
′
为期望广义力矩阵，j
b
′
为轮腿-躯干铰接点处的广义力向躯干质心映射的雅克比矩阵的扩展形式；力矩τ＝[τ
r0 τ
r1 τ
r2 τ
l0 τ
l1 τ
l2
]
t
解算部分应满足：b2＝a2x
其中，其中，g
i
、i
i
、j
i
分别为轮腿i的重力项、初始惯性力矩阵以及末端力雅克比矩阵；而j
wi
为两连杆腿部雅克比矩阵，j
wi
(1)为取矩阵j
wi
的第一列，将以上力分布部分与关节力矩解算部分合并为一个qp解算问题：ax＝b其中，(2)约束构造：将力分配约束公式整理为如下形式：x(0)x(4)-x(1)x(3)＝0将轮腿的伸展长度需要满足以下约束：其中，l
x
为髋关节与轮地接触点的水平距离，l
z
为髋关节垂直高度，f
x
与f
z
为躯干受到的二维合力；l、分别为髋关节与轮地接触点的最小允许距离与最大允许距离；将该约束整理为两个单边约束：将该约束整理为两个单边约束：同时，因为地面只能提供支持力，而不能提供吸引力，因此纵向力f
z
必须为正，即：x(1)≥0x(4)≥0为保证所计算的各关节力矩在关节电机输出扭矩范围之内，使得：至此，能够构成一个完整的具有凸约束的qp解算问题：
s.t.x(0)x(4)-x(1)x(3)＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)(1)x(1)≥0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)x(4)≥0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)|x(0)+x(3)|-μ|x(1)+x(4)|≤0
ꢀꢀꢀꢀ
(7)其中，qp的输出矩阵中关节力矩τ＝[τ
r τ
l
]
t
作为最终的执行力矩，μ为地面摩擦系数。

技术总结
本发明公开了一种双轮腿-臂机器人全身运动规划与控制方法，针对双轮腿-臂机器人躯干、手臂协同运动以及手臂末端力输出需求，提出了基于分层优化的多任务空间规划方法，解决了全身运动规划冗余问题。基于二级桌子-小车模型对轮腿-躯干-手臂力传递进行分析，将手臂运动与作业虚拟广义力通过前馈补偿的形式纳入躯干广义力规划。分析左右轮腿力分配特点，通过构造提出了基于二次规划的自平衡全身力矩控制器，将各层非线性约束显性化，从而解算出获得满足全身动态平衡的轮腿力矩。通过该力矩的施加所生成的轮腿姿态能够保持双轮腿-臂机器人处于动态平衡点，提高了运动作业的动态稳定性。性。性。


技术研发人员：辛亚先 荣学文 李贻斌 阮久宏 范永
受保护的技术使用者：山东交通学院
技术研发日：2022.05.18
技术公布日：2022/7/5