1.一种双轮腿‑臂机器人全身运动规划与控制方法，包括：(1)在双轮腿系统力分析的基础上进行双轮腿‑臂机器人动力学分析，将手臂的运动与作业对机器人产生的稳定性影响映射为躯干广义力的一部分，进行统一规划与处理；

所述双轮腿‑臂机器人动力学分析，在双轮腿系统力分析的基础上，将手臂杆件与末端所受外力集中于一点，形成以躯干为基座的第二级桌子‑小车模型，将躯干作为轮腿‑手臂广义力传递的桥梁，将第二级小车模型所需广义力映射到第一级小车模型即躯干上去，作为外力进行前馈补偿，然后，进行轮腿力矩解算，实现手臂运动与作业时的双轮腿‑臂机器人动态稳定运动；

躯干单刚体受到来自左右轮腿子系统与手臂子系统的三组广义力，分别定义为Wl、Wr与Wa；躯干单刚体的动力学表达式可描述为：其中，mb为躯干质量，Ib为躯干的转动惯量，Pb为躯干的位姿向量，Jtorso与Gb分别为力雅克比矩阵与重力矩阵；Wl、Wr与Wa三组广义力在躯干质心处的合力需要推动躯干沿期望运动轨迹行进，而Wa的大小根据手臂交互力的变化进行规划；因此，对于躯干子系统而言，Wa作为广义力前馈补偿项，Wl、Wr作为驱动躯干运动的控制输入；

躯干状态空间表达式为：

T b

其中，u＝[Wl Wr]为左右轮腿对躯干的驱动力输入，Wa为Wa在躯干质心处的映射，S为输入矩阵；左右轮腿所提供的映射到躯干质心处的广义力即Wb可描述为：Wb＝Jbu                                   (3)T

其中，雅克比矩阵Jb＝S；

(2)根据实际需求所确定的手臂末端、躯干位姿任务空间，提出多任务空间分层优化方法，解决双轮腿‑臂机器人全身运动规划的冗余，同时实现多个具有不同优先级任务空间的参考轨迹规划，操纵目标物的同时驱动自身运动；

所述多任务空间分层优化方法，双轮腿‑臂机器人的控制目标为单一任务空间的运动或者同时提出两个任务空间的期望轨迹；两个任务空间并不是完全解耦的，存在相互制约的关系，因此，并不能满足任意期望轨迹的执行；为了保证控制器所接收执行信号的有效性，构造上层规划器，根据不同工况的实际情况进行任务空间优先级分层，采用优化方法规划出各任务空间的可行参考轨迹；根据分层优化算法，将最底层任务空间的优化输出作为广义关节加速度对速度与位置进行更新，得到参考轨迹；

T T

定义机器人的广义关节空间为广义坐标q＝[qb qa]，其中，qb＝[xb yb zb α]为躯干位T姿，由躯干三维位移xb、yb、zb以及航向角度α组成，qa＝[θ3 θ4 θ5]为手臂关节空间，由肩部航向关节角度θ3、肩部俯仰关节角度θ4以及肘俯仰关节角度θ5组成；不失一般性，假设存在n个可控的任务空间，其中，第j个任务空间的状态量定义为Xj，其期望的状态空间定义为使得任务空间的位姿与速度最大限度伺服到期望位姿与速度，从而得到广义关节空间的参考加速度

其中，Q为正定对角权重矩阵， 为任务j的参考加速度，通过比例‑微分控制器得到：其中，Kpj与Kdj分别为比例与微分系数；

由于机器人的冗余特性，仅以公式(4)为目标函数，并不能唯一的确定躯干与手臂关节运动，从而使得任务空间j的位置与速度尽可能达到 因此，将广义关节空间的运动位置增量进行加权平方，构造新的目标函数：其中，R为正定对角权重矩阵，q为本次控制率施加到机器人后产生的广义关节空间位置的预测，通过如下式的离散预测得到：

其中，qpre与 分别为上一采样周期广义关节的角度与角速度，采用传感器测得，Δt为采样时间；

至此，目标函数的构造同时考虑了手臂末端任务及关节空间运动的优化目标，同时，机器人自身的关节空间约束、执行器性能约束以及非完整约束等必须严格满足，因此，进行等式约束与不等式约束构造；

为约束广义关节空间的变量不超出上下界限，设置如下式的双边不等式约束其中，q与 分别为广义关节空间的位置下界与上界；

广义关节空间中包含了躯干的三维运动xb、yb、zb以及姿态α，另外，默认躯干的期望俯仰角与期望横滚角为0，不需要规划器实时生成；受到结构限制，机器人无法直接发生侧向移动，只能通过控制前向运动与航向的耦合来实现侧向运动；因此，当规划器进行躯干位姿实时规划时，添加约束以保证位姿间的耦合关系：其中，

同时，由于双轮腿需要保持实时动态稳定，受到外界环境的突发变化，机器人的平衡状态将由当前平衡点跳变为另一个不连续的动态稳定平衡点，为了保证转换过程的连续平滑，构造加速度变化量的约束：其中，ε为人为设定的阈值，以公式(6)为目标函数，以公式(8)、(9)、(10)为约束条件优化求解出面向任务j的广义关节加速度期望轨迹；

当同一时刻面临多个不同任务时，每一任务空间的优化解算都作为一个二次规划问题即Quadratic Programming，简称QP，定义Con为约束集合，包括等式约束与不等式约束，面向任务1时Con1中的约束为公式(8)、(9)、(10)；定义第j‑1任务空间的QP问题为QPj‑1，其解算输出定义为 该输出为符合约束条件前提下，完成任务j‑1的最优选择，为保证任务j‑

1的执行在后续任务规划中不被破坏，利用 构造如下新的约束，并更新约束集合Conj：其中， 为任务j‑1所解算的关节加速度向末端任务参考加速度的运动学映射，为后续规划任务的加速度变量；Jj‑1为任务空间j‑1的速度雅克比矩阵；

可见，任务j的新增约束条件如公式(12)所示：将最底层任务空间的优化输出 作为广义关节加速度对速度与位置进行更新，得到参考轨迹；

(3)手臂与环境的交互力对双轮腿‑臂机器人的动态稳定产生实时影响，以此交互力为控制目标，构造前馈项补偿项加入到全身力矩控制器中，补偿手臂末端广义力对全身关节力矩所产生的影响，从而实现对操作物动态力的前馈广义力补偿，增加了全身稳定鲁棒性；

轮腿一体化结构动力学描述方法如公式(13)所示：其中，下标i为左右轮腿的编号，qi为轮腿i的关节空间，ai为轮式动基座相对于惯性系的水平加速度，τi为轮腿i的主动关节力矩，Mi、Ci、Gi、Ii、Ji分别为轮腿i的惯性矩阵、科氏力离心力矩阵、重力项、初始惯性力矩阵以及末端力雅克比矩阵；

轮腿末端约束力：

躯干质心合力与轮腿末端力的分配关系整理为：

Wb′＝Jb′u′             (18)T T

其中，Wb′＝[Wb 0 0] ，u′＝[Wl Wr frα flα] ，frα和flα分别为轮地接触点处定义轮行进方向的水平力，Jb′为融合了力映射雅克比与约束系数的雅克比矩阵；

构造虚拟广义力Wb的期望值Wd由两部分组成：Wd＝ξ+Wc                                (19)T

其中，ξ＝[Fxfb Fzfb Nxfb Nyfb Nzfb] 为躯干位姿与速度误差反馈部分，Wc为前馈补偿部分，分别采用公式(20)与公式(21)计算得到：其中，kpz、kdz、kpγ、knγ、kpβ、kdβ、kpα、kdα、λ均为正反馈增益，躯干的水平运动速度vx、站高z、横滚角γ、俯仰角β、航向角α及其速度通过IMU实时测量得到，h为躯干单刚体的高度；

手臂驱动力前馈补偿：

手臂附肢的运动与手臂末端力输出的前提皆为躯干所提供的与手臂铰接点处的广义力Wa，Wa与手臂末端输出力、关节力矩、各杆件广义力均有关，将力与力矩沿手臂杆件由外向内迭代，建立力传递模型如公式(22)所示；

其中， 项补偿关节加速度产生的杆件惯性力；Cw项补偿杆件运动所产生的科氏力T

与离心力；Gw项补偿手臂杆件重力以及躯干动基座带来的初始惯性力；JFFobj补偿手臂末端输出力，其中，JF为手臂末端力向手臂‑躯干铰接点处映射的力雅克比矩阵；

简化目标重物为单刚体，定义其位姿为 其中j≤6，重物的动力学模型为：

其中， 为重物单刚体的惯性矩阵； 为重物单刚体的重力向量，将公式(23)代入公式(22)，得到：

w w

此处假设Pobj＝Ph，因此，公式(24)进一步整理为：躯干期望广义力的构造需要同时考虑躯干运动状态，手臂、负载重物所需广义力的补偿，公式(19)对躯干广义力的构造更新为公式(26)：b

Wd＝ξ+Wc+Wa                            (26)其中， 为手臂所需期望驱动力在躯干质心处

的映射；

同时，手臂关节力矩构造如下：

其中，τA为手臂关节力矩向量，Kpa与Kda为正反馈增益， 与 为期望关节位置与速度参考值；

所述全身力矩控制器，将左右轮腿的力与力矩解算问题转化为二次规划解算问题，定义QP解算的变量为：

T

x＝[Frx Frz Nry Flx Flz Nly frα flα τr0 τr1 τr2 τl0 τl1 τl2]   (28)其中，Fix、Fiz、Niy分别为轮腿i与躯干铰接点处的水平力、竖直力以及俯仰力矩，fiα为轮腿i的轮地接触点处推动机器人航向运动的水平力；τr0、τr1、τr2、τl0、τl1、τl2分别为右轮腿的轮关节、膝关节、髋关节力矩与左轮腿的轮关节、膝关节、髋关节力矩；

同时将力分配约束、轮腿姿态约束、关节力矩约束分别转化为二次规划的目标函数与约束，构成一个完整的具有凸约束的QP解算问题：(1)目标函数构造：

轮腿‑躯干接触力与躯干所需广义力应满足：

b1＝A1x                              (29)其中，b1＝Wb′，A1＝[Jb′ 06×6]，其中，Wb′为期望广义力矩阵，Jb′为轮腿‑躯干铰接点处的广义力向躯干质心映射的雅克比矩阵的扩展形式；

基于轮腿一体化模型公式(13)，以及两连杆腿部雅克比矩阵，构造力矩τ＝[τr0 τr1 τr2 Tτl0 τl1 τl2]解算部分为：

其中， 而Jwi为两连杆腿部雅克比矩阵，Jwi(1)为取矩阵Jwi的第一T

列，u＝[Frx Frz Nry Flx Flz Nly]为左右轮腿的末端力，C对关节力矩进行重新排列；

将公式(30)整理为QP标准形式为：

b2＝A2x                                           (31)其中，

其中，Gi、Ii、Ji分别为轮腿i的重力项、初始惯性力矩阵以及末端力雅克比矩阵；Jwi为两连杆腿部雅克比矩阵，Jwi(1)为取矩阵Jwi的第一列，将公式(29)与公式(31)合并为一个QP解算问题：Ax＝b                                                    (32)其中，

(2)约束构造：

将力分配约束公式整理为如下形式：

x(0)x(4)‑x(1)x(3)＝0                                 (33)将轮腿的伸展长度需要满足以下约束：

其中，Lx为髋关节与轮地接触点的水平距离，Lz为髋关节垂直高度，Fx与Fz为躯干受到的二维合力；L、分别为髋关节与轮地接触点的最小允许距离与最大允许距离；

将该约束整理为两个单边约束：

同时，因为地面只能提供支持力，而不能提供吸引力，因此纵向力Fz必须为正，即：x(1)≥0                         (37)x(4)≥0                           (38)为保证所计算的各关节力矩在关节电机输出扭矩范围 之内，使得：至此，能够构成一个完整的具有凸约束的QP解算问题：T

其中，QP的输出矩阵中关节力矩τ＝[τrτl]作为最终的执行力矩，μ为地面摩擦系数。