基于任务分层优化的双足机器人级联控制方法和装置

1.本技术涉及双足机器人级联控制技术领域，特别是涉及一种基于任务分层优化的双足机器人级联控制方法和装置。


背景技术：

2.双足机器人具有与人类相似的运动方式，能够在人类活动的各种复杂环境中自由移动。同时，人类一直希望创造出与人类自身类似的机器人，用于娱乐教育、医疗保健、军事、工业制造、恶劣危险环境作业和救援等领域。双足机器人拥有巨大的发展潜力，已经成为机器人领域的研究热点，并代表着机器人技术的综合发展水平。
3.近年现有技术中，足式机器人研究重心转移到模型预测控制框架，主要在四足机器人上应用效果较好。该框架在足式机器人中应用可以使用各种模型，比如倒立摆模型、单刚体模型和整体模型，模型的复杂程度会直接影响mpc方法类型和求解速度。mpc框架的方法类型主要分为线性mpc、非线性mpc、鲁棒mpc和learning mpc。由于目前大部分控制对象都是非线性时变系统，使用鲁棒mpc和learning mpc可以提高系统的鲁棒性和跟踪精度。
4.然而，整体控制框架的内核是单步的pd控制，该类方法抵抗扰动和高速性能较弱；模型预测控制的优化算法计算效率仍然限制了这些方法的实时应用，使用简化模型的线性mpc又会导致动态特性的缺失。


技术实现要素：

5.基于此，有必要针对上述技术问题，提供一种基于任务分层优化的双足机器人级联控制方法，能够更加精确地控制浮动基座的姿态和高度，更好地协调机器人的稳定性，提高了机器人抗扰能力和运动速度，实现双足机器人的高速运动级联控制。
6.基于任务分层优化的双足机器人级联控制方法，包括：
7.获取双足机器人的期望速度和期望角速度，根据所述期望速度和所述期望角速度，计算期望基座状态；
8.获取双足机器人的运动状态，并根据所述运动状态进行基座状态估计和足端接触力估计；
9.根据所述期望基座状态和所述基座状态估计，构建单刚体模型，计算双足机器人支撑腿的控制力旋量；
10.根据所述期望速度，计算双足机器人摆动腿的落足点，并得到轨迹规划；
11.根据所述控制力旋量以及所述轨迹规划，使所述足端接触力跟踪所述控制力旋量，计算松弛变量；根据当前足端接触力和所述松弛变量计算双足机器人的关节控制力矩；
12.根据所述关节控制力矩，实时控制双足机器人运动。
13.在一个实施例中，根据所述控制力旋量以及所述轨迹规划，使所述足端接触力跟踪所述控制力旋量，计算松弛变量；根据当前足端接触力和所述松弛变量计算双足机器人的关节控制力矩包括：
14.根据所述控制力旋量以及所述轨迹规划，构建不同的优先级任务；根据优先级任务，使所述足端接触力跟踪所述控制力旋量，构建分级优化算法；
15.按照优先级任务的顺序对所述分级优化算法依次进行求解，得到松弛变量；
16.根据当前足端接触力和所述松弛变量，使用动力学分解模型，计算双足机器人的关节控制力矩。
17.在一个实施例中，根据所述控制力旋量以及所述轨迹规划，构建不同的优先级任务；根据优先级任务，使所述足端接触力跟踪所述控制力旋量，构建分级优化算法包括：
18.根据所述控制力旋量以及所述轨迹规划，构建不同的优先级任务；
19.第一优先级任务为动力学约束：
[0020][0021][0022][0023]
式中，m为机器人整体动力学的惯性矩阵，表示机器人广义向量，包含了浮动基座惯性位姿和关节角度，h表示离心力、科氏力项、重力项，为主动关节的选择矩阵，t为矩阵的转置，为主动驱动关节力矩，为足端接触雅克比矩阵，为地面对机器人施加的足端接触力旋量，f
c_mpc
表示模型预测控制计算的足端力旋量，δfc为需要计算的足端接触力补偿量，动力学的三个矩阵分别分解为m＝[mb，ma]
t
，h＝[hb，ha]
t
，ijc＝[
ijcb ijca
]，mb为浮动基座部分的惯性矩阵，hb为浮动基座部分的离心力、科氏力项、重力项，
ijcb
为浮动基座部分的足端接触雅克比矩阵，ma为腿的惯性矩阵，ha为腿的离心力、科氏力项、重力项，
ijca
为腿的足端接触雅克比矩阵；
[0024]
第二优先级任务为足端接触不滑动约束：
[0025][0026][0027]
第三优先级任务为浮动基座姿态和高度：
[0028][0029][0030]
式中，x
d_b
，为浮动基座姿态和高度期望轨迹的位置、速度、加速度，为反馈控制计算得到的控制量，xb和为实际状态，k
p_b
和k
d_b
为控制器对角增益矩阵；为雅克比矩阵j
if_task
的导数；
[0031]
第四优先级任务为摆动腿末端位姿：
[0032][0033][0034]
式中，x
d_f
，和为摆动腿末端位姿期望轨迹的位置、速度、加速度，为反馈控制计算得到的控制量，xf和为实际状态，k
p_f
和k
d_f
为控制器对角增益矩阵；为雅克比矩阵j
if_task
的导数；
[0035]
第五优先级任务为关节力矩最小：
[0036][0037][0038]
式中，ma为腿的惯性矩阵，ha为腿的离心力、科氏力项、重力项，
ijca
为腿的足端接触雅克比矩阵；
[0039]
第六优先级任务为足端接触力补偿量最小：
[0040][0041]
式中，i为基座惯量矩阵；
[0042]
根据优先级任务，使所述足端接触力跟踪所述控制力旋量，构建分级优化算法：
[0043][0044]
式中，ai指第一至第六优先级的左边项矩阵，指优化变量矩阵，bi指第一至第六优先级的右边项矩阵，i＝1,2,3
…
6。
[0045]
在一个实施例中，在按照优先级任务的顺序对所述分级优化算法依次进行求解时，还需要满足等式约束和不等式约束；
[0046]
等式约束：
[0047][0048]
上式的物理含义是第i个优先级任务的优化解对应的要与之前(i-1)个分别相等，式中，aj指第一至第六优先级的左边项矩阵，在计算过程中它是与ai同步对应的，指优化变量矩阵，在计算过程中它是与同步对应的j＝1,2,3
…
6；
[0049]
不等式约束：
[0050]
τ
min
≤τ≤τ
max
[0051]u·ijc≤0
[0052]
式中，τ，τ
min
，τ
max
指关节力矩，最小力矩，最大力矩；u指约束矩阵，jc指足端接触矩阵。
[0053]
在一个实施例中，根据所述期望基座状态和所述基座状态估计，构建单刚体模型，
计算双足机器人支撑腿的控制力旋量包括：
[0054]
根据所述期望基座状态和所述基座状态估计，构建轨迹跟踪的代价函数，并在满足单刚体模型动力学约束、不等式约束和等式约束的条件下，对所述代价函数进行求解，得到双足机器人支撑腿的控制力旋量。
[0055]
在一个实施例中，满足单刚体模型动力学约束的条件包括：
[0056]
x(k 1)＝ak·
x(k) bk·
u(k)
[0057][0058][0059]
x(k)＝[iq
b,r
，iq
b,p
，iωb，iυb，g]
t
[0060][0061][0062][0063][0064]
kg＝[0 0
ꢀ‑
1]
t
[0065]kτ
＝[0 0 1]
t
[0066]
式中，x(k 1)表示控制系统的未来一步状态，x(k)表示控制系统的当前状态，即机器人的状态向量，u(k)表示控制系统的控制量，ak表示控制系统的系统矩阵，bk表示控制系统的控制矩阵；δt为离散时间步长，一般为控制时间步长；为基座在惯性系下的惯量矩阵；ip
bc
(q)为支撑腿足端在惯性系下相对于基座系原点的位置，方向为从基座系原点指向足端，下标l和r分别表示左右腿；表示左腿足端力旋量，表示右腿足端力旋量；iq
b,r
基座姿态角(滚转、俯仰、偏航)，iq
b,p
基座在惯性系下的位置；iωb和iυb分别为惯性系下的角速度和线速度；g为重力加速度；q
b,yaw
为基座偏航角，q
b,p
it
ch
基座俯仰角。
[0067]
在一个实施例中，满足不等式约束的条件包括：
[0068][0069]fz
≤f
z,max
[0070]-μfz≤f
x
≤μfz[0071]-μfz≤fy≤μfz[0072][0073][0074]
式中，表示足端力旋量，表示脚掌与地面接触位置的法线向量，fz指垂直方向上的足端力，μ指摩擦系数，f
x
指水平方向上的足端力，fy指侧向方向上的足端力，τ
yaw
指偏航方向上的力矩，l
toe
表示踝关节在地面投影点到前接触点的距离，l
heel
表示踝关节在地面投影点到后接触点的距离。
[0075]
在一个实施例中，满足等式约束的条件包括：
[0076][0077][0078]
式中，k指k时刻，i指i步。
[0079]
在一个实施例中，根据所述期望速度，计算双足机器人摆动腿的落足点，并得到轨迹规划包括：
[0080]
根据所述期望速度，计算双足机器人摆动腿的落足点：
[0081][0082]ivb＝[iv
bx ivby 0]
t
[0083]iv
b,des
＝[iv
bx,des ivby,des 0]
t
[0084]
式中，p
f,des
为落足点，第一项p
com
为惯性系下浮动基座实时的位置，第二项r
ibb
p
b_hip
为惯性系下从基座系到髋部的位置，第三项和第四项为raibert提出的启发式落足点计算公式，ivb表示机器人在惯性系下的实际速度，iv
b,des
表示机器人在惯性系下的期望速度，第五项为偏航转向时的落足补偿项，iq
b,pz
表示机器人高度，iω
bz,des
表示期望偏航角速度；
[0085]
根据所述落足点，计算轨迹规划：
[0086]
x
d_f
＝(1-m)3p0 3m(1-m)2p1 3m2(1-m)p2 m3pd[0087]
pd＝p
f,des
[0088]
式中，m∈[01]表示归一化后的时间量，p0表示抬足点，p1表示中间控制点，p2表示中间控制点，p1≠p2，pd表示落足点。
[0089]
基于任务分层优化的双足机器人级联控制装置，包括：
[0090]
获取模块，用于获取双足机器人的期望速度和期望角速度，根据所述期望速度和所述期望角速度，计算期望基座状态；
[0091]
估计模块，用于获取双足机器人的运动状态，并根据所述运动状态进行基座状态估计和足端接触力估计；
[0092]
模型预测模块，用于根据所述期望基座状态和所述基座状态估计，构建单刚体模型，计算双足机器人支撑腿的控制力旋量；
[0093]
规划模块，用于根据所述期望速度，计算双足机器人摆动腿的落足点，并得到轨迹规划；
[0094]
分级优化模块，用于根据所述控制力旋量以及所述轨迹规划，使所述足端接触力跟踪所述控制力旋量，计算松弛变量；根据当前足端控制力和所述松弛变量计算双足机器人的关节控制力矩；
[0095]
控制模块，用于根据所述关节控制力矩，实时控制双足机器人运动。
[0096]
上述基于任务分层优化的双足机器人级联控制方法，提出了考虑腿部动态的模型预测与任务分级优化整体控制相结合的高速运动轨迹跟踪方法。基于欠驱动机器人的分时控制框架，在上层设计了基于忽略腿部的单刚体模型mpc，计算支撑期的姿态和高度方向的力旋量，同时采用落足点原理来控制机器人前进和侧向速度；在下层使用考虑腿部动态的任务分层优化整体控制器去跟踪力旋量；在优化变量中设计了足端接触力松弛变量，使用该变量补偿单刚体模型与完整动力学模型中两个足端接触力的差异，从而搭建了上层模型预测控制器与下层整体控制器的纽带关系。液压驱动的大重量的腿和外界冲击环境对缺少驱动自由度的双足机器人的高速运动来说更具挑战性，通过行走步态高速前进实验和外部冲击稳定性仿真实验验证了该方法在高速运动和扰动下的鲁棒性，并将机器人最大行走速度由模型预测方法的1.9m/s提升到2.5m/s。
附图说明
[0097]
图1为一个实施例中基于任务分层优化的双足机器人级联控制方法流程图；
[0098]
图2为一个实施例中串级控制框架的示意图；
[0099]
图3为一个实施例中机器人高速动态运动过程的示意图；a为0.2s、0m/s，b为3.3s、0.3m/s，c为5s、0.5m/s，d为8.5s、1.2m/s，e为11.4s、2m/s，f为12.8s、2.2m/s，g为14.5s、2.4m/s，h为19s、2.5m/s；
[0100]
图4为一个实施例中z轴方向的足端接触力和力矩的示意图；a为左腿z方向接触力，b为左腿偏航力矩，c为右腿z方向接触力，d为右腿偏航力矩；
[0101]
图5为一个实施例中浮动基座欧拉角的示意图；a为滚转角，b为俯仰角，c为偏航角；
[0102]
图6为一个实施例中浮动基座位置的示意图；a为x轴，b为y轴，c为z轴高度；
[0103]
图7为一个实施例中x和y轴方向的速度的示意图；a为x轴，b为y轴；
[0104]
图8为另一个实施例中浮动基座欧拉角的示意图；a为滚转角，b为俯仰角，c为偏航角；
[0105]
图9为一个实施例中速度和高度轨迹的示意图；a为x轴方向速度，b为y轴方向速
度，c为z轴方向高度；
[0106]
图10为一个实施例中基于任务分层优化的双足机器人级联控制装置的结构框图。
具体实施方式
[0107]
为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本技术进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本技术，并不用于限定本技术。
[0108]
如图1所示，本技术提供的一种基于任务分层优化的双足机器人级联控制方法，在一个实施例中，包括以下步骤：
[0109]
步骤102，获取双足机器人的期望速度和期望角速度，根据所述期望速度和所述期望角速度，计算期望基座状态。
[0110]
期望速度包括期望的前进速度和侧向速度。
[0111]
根据期望速度和期望角速度计算期望基座状态属于现有技术。
[0112]
步骤104，获取双足机器人的运动状态，并根据所述运动状态进行基座状态估计和足端接触力估计。
[0113]
具体的估计过程属于现有技术，在此不再赘述。
[0114]
步骤106，根据所述期望基座状态和所述基座状态估计，构建单刚体模型，计算双足机器人支撑腿的控制力旋量。具体的：
[0115]
根据所述期望基座状态和所述基座状态估计，构建轨迹跟踪的代价函数，并在满足单刚体模型动力学约束、不等式约束和等式约束的条件下，对所述代价函数进行求解，得到双足机器人支撑腿的控制力旋量。
[0116]
更具体的：
[0117]
满足单刚体模型动力学约束的条件包括浮动基座单刚体模型离散状态方程：
[0118]
x(k 1)＝ak·
x(k) bk·
u(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0119][0120][0121]
x(k)＝[iq
b,r
，iq
b,p
，iωb，iυb，g]
t
[0122]
[0123][0124][0125][0126]
kg＝[0 0
ꢀ‑
1]
t
[0127]kτ
＝[0 0 1]
t
[0128]
式中，x(k 1)表示控制系统的未来一步状态，x(k)表示控制系统的当前状态，即机器人的状态向量，u(k)表示控制系统的控制量，ak表示控制系统的系统矩阵，bk表示控制系统的控制矩阵；δt为离散时间步长，一般为控制时间步长；为基座在惯性系下的惯量矩阵；ip
bc
(q)为支撑腿足端在惯性系下相对于基座系原点的位置，方向为从基座系原点指向足端，下标l和r分别表示左右腿；表示左腿足端力旋量，表示右腿足端力旋量；iq
b,r
基座姿态角(滚转、俯仰、偏航)，iq
b,p
基座在惯性系下的位置；iωb和iυb分别为惯性系下的角速度和线速度；g为重力加速度；q
b,yaw
为基座偏航角，q
b,pitch
基座俯仰角。
[0129]
为了跟踪期望轨迹，设计了一个代价函数来反映被控系统的跟踪性能。代价函数中包含两项，第一项是状态轨迹跟踪误差加权尽量小，第二项是控制序列加权尽量小，优化变量为足端接触力旋量，u＝f
c_mpc
；然后加入动力学约束和物理约束，构造了轨迹跟踪的优化问题如下：
[0130][0131]
满足动力学约束：
[0132]
s.t.x(k i|t)＝a
k,t
·
x(k i-1|t) b
k,t
·
u(k i-1|t),i＝1...n
p
ꢀꢀ
(2b)
[0133]
满足不等式约束：
[0134][0135]
满足等式约束：
[0136]di
u(k i-1)＝0, i＝1...n
p
ꢀꢀ
(2d)
[0137]
式中，x(k i|t)为状态实际轨迹，x
ref
(k i|t)为状态期望轨迹。
[0138]np
为预测步数，nc为控制步数，i为i步，k为k时刻，q和r分别为权重矩阵。
[0139]
其中，满足不等式约束的条件，主要包含摩擦锥约束，包括：
[0140]
1)在足端与地面的接触过程中，地面只对足端施加地面反作用力，而不能形成地面对足端的拉力，所以在垂直方向上的足端力fz必须大于等于零。考虑到地面有起伏，足端接触力在地面法线方向上的分量大于等于零。另外受关节驱动器的最大物理驱动能力的限制，足端力fz存在最大值。
[0141]
[0142]fz
≤f
z,max
[0143]
2)为了防止足端在地面打滑引发不可控状态，前进和侧向方向接触力限制在摩擦锥范围内。
[0144]-μfz≤f
x
≤μfz[0145]-μfz≤fy≤μfz[0146]
3)另外足端接触偏航力矩会在前后接触点处形成侧向力。这个侧向力会叠加到侧向接触力上，所以也需要限制在摩擦锥范围内。
[0147][0148][0149]
式中，表示足端力旋量，表示脚掌与地面接触位置的法线向量，fz指垂直方向上的足端力，μ指摩擦系数，f
x
指水平方向上的足端力，fy指侧向方向上的足端力，τ
yaw
指偏航方向上的力矩，l
toe
表示踝关节在地面投影点到前接触点的距离，l
heel
表示踝关节在地面投影点到后接触点的距离。
[0150]
满足等式约束的条件，主要是步态约束，通过选择足端接触力旋量将步态序列融入优化求解中，包括：
[0151]
1)如果腿处于摆动期时，使用等式约束强制足端接触力旋量为零；如果腿处于支撑期，此处足端接触力旋量不约束，其值由其他约束计算得出。将左右腿统一到一个框架后，在一个时刻处就会得到一个8
×
8维度的矩阵。当处于左腿支撑，右腿摆动状态时：
[0152][0153]
2)当处于右腿支撑，左腿摆动状态时：
[0154][0155]
式中，k指k时刻，i指i步。
[0156]
基于式(1)动力学模型，将模型预测控制优化问题(2a)转化成线性二次规划问题(qp)标准形式，在控制周期内通过优化求解可以得到足端接触力旋量f
c_mpc
。
[0157]
浮动基座单刚体模型是从整体动力学中忽略了腿的动态部分而推导出的，所以上层的模型预测控制器规划的足端接触力f
c_mpc
与下层的任务分层优化控制器需要的期望足端接触力f
c_wbc
并不相等，一般情况下，f
c_wbc
是大于f
c_mpc
，所以本文设计一个足端接触力松弛变量δfc，使用该变量补偿上述两个足端接触力的差异，得到关系式f
c_wbc
＝f
c_mpc
δfc，这样就搭建了上层模型预测控制器与下层整体控制器的纽带关系。由于上层控制器已经提供了稳定的足端接触力期望中值，松弛变量δfc在整体动力学约束下向f
c_mpc
靠近，整体表现是f
c_wbc
在跟踪f
c_mpc
，从而构成模型预测任务分层优化控制器的串级控制器。
[0158]
对于本技术的双足机器人，单腿只有四个主动关节，在这个统一框架下，只需要将权重矩阵q中对应x、y两个自由度的增益值设置为0，即可放弃在支撑期对这两个自由度的控制。倘若不进行这样的处理，使用四个主动自由度去控制六个自由度量，将会因欠驱动而使控制效果很差。
[0159]
步骤108，根据所述期望速度，计算双足机器人摆动腿的落足点，并得到轨迹规划。具体的：
[0160]
根据所述期望速度，计算双足机器人在惯性坐标系下的摆动腿的落足点：
[0161][0162]ivb＝[iv
bx ivby 0]
t
[0163]iv
b,des
＝[iv
bx,des ivby,des 0]
t
[0164]
式中，p
f,des
为落足点，第一项p
com
为惯性系下浮动基座实时的位置，第二项r
ibb
p
b_hip
为惯性系下从基座系到髋部的位置，第三项和第四项为raibert提出的启发式落足点计算公式，ivb表示机器人在惯性系下的实际速度，iv
b,des
表示机器人在惯性系下的期望速度，第五项为偏航转向时的落足补偿项，iq
b,pz
表示机器人高度，iω
bz,des
表示期望偏航角速度；
[0165]
通过公式计算可以得到落足点在惯性系下的x、y方向位置，而z方向位置则要基于机器人的站立平面下探10mm以保证机器人足端稍微提前接触地面。
[0166]
考虑轨迹加速度平滑的需求，摆动腿足端轨迹采用三阶bezier曲线，其形式见下式。在固定时间周期内，其曲线轨迹由四个控制点来决定，分别是抬足点p0、中间控制点p1、中间控制点p2和落足点pd，其中四个控制点要求都在摆动腿可达空间内。
[0167]
根据所述落足点，为了完成抬腿高度和过程平滑过渡，实时计算摆动腿轨迹规划：
[0168]
x
d_f
＝(1-m)3p0 3m(1-m)2p1 3m2(1-m)p2 m3pd[0169]
pd＝p
f,des
[0170]
式中，m∈[01]表示归一化后的时间量，p0表示抬足点，p1表示中间控制点，p2表示中间控制点，p1≠p2，pd表示落足点。
[0171]
对规划的摆动腿位置轨迹x
d_f
进行微分后得到期望速度和加速度
[0172]
步骤110，根据所述控制力旋量以及所述轨迹规划，使所述足端接触力跟踪所述控制力旋量，计算松弛变量；根据当前足端接触力和所述松弛变量计算双足机器人的关节控制力矩。
[0173]
需要说明：当前足端接触力是指跟踪控制力旋量后的足端接触力。
[0174]
具体的：
[0175]
根据所述控制力旋量以及所述轨迹规划，构建不同的优先级任务；根据优先级任务，使所述足端接触力跟踪所述控制力旋量，构建分级优化算法；
[0176]
按照优先级任务的顺序对所述分级优化算法依次进行求解，得到松弛变量；
[0177]
根据当前足端接触力和所述松弛变量，使用动力学分解模型，计算双足机器人的关节控制力矩。
[0178]
更具体的：
[0179]
任务分层优化控制器的基本思想是优先约束机器人的全身动力学模型，将所有任务按优先级排序，然后按先后顺序进行优化求解，低优先级任务的解要满足高优先级任务。在每一个任务的优化求解中，在外环层面设计对应空间(任务空间/关节空间)的控制器，在内环层面设计等式和不等式约束下的轨迹跟踪优化控制器。使用广义加速度和接触力补偿量作为优化变量现将该控制器详述如下。
[0180]
根据所述控制力旋量以及所述轨迹规划，构建不同的优先级任务；
[0181]
1)第一优先级任务：动力学约束，机器人整体动力学模型为，
[0182][0183]
为了与优化变量对应，取整体动力学的无驱动部分模型，
[0184][0185]
构造成含优化变量的函数，
[0186][0187]
式中，m为机器人整体动力学的惯性矩阵，表示机器人广义向量，包含了浮动基座惯性位姿和关节角度，h表示离心力、科氏力项、重力项，为主动关节的选择矩阵，t为矩阵的转置，为主动驱动关节力矩，为足端接触雅克比矩阵，为地面对机器人施加的足端接触力旋量，f
c_mpc
表示模型预测控制计算的足端力旋量，δfc为需要计算的足端接触力补偿量，动力学的三个矩阵分别分解为m＝[mb，ma]
t
，h＝[hb，ha]
t
，ijc＝[
ijcb ijca
]，mb为浮动基座部分的惯性矩阵，hb为浮动基座部分的离心力、科氏力项、重力项，
ijcb
为浮动基座部分的足端接触雅克比矩阵，ma为腿的惯性矩阵，ha为腿的离心力、科氏力项、重力项，
ijca
为腿的足端接触雅克比矩阵；下标a1和b1将用于优化算法中代价函数的设计。
[0188]
2)第二优先级任务：足端接触不滑动约束，
[0189][0190][0191]
3)第三优先级任务：浮动基座姿态和高度，浮动基座姿态和高度的期望值与上层模型预测控制器中的期望值一致，则外环控制器为，
[0192][0193]
构造成含优化变量的函数，
[0194][0195]
姿态和高度任务的优先级为姿态优先级大于高度；
[0196]
式中，x
d_b
，为浮动基座姿态和高度期望轨迹的位置、速度、加速度，为反馈控制计算得到的控制量，xb和为实际状态，k
p_b
和k
d_b
为控制器对角增益矩阵；为雅克比矩阵j
if_task
的导数。
[0197]
4)第四优先级任务：摆动腿末端位姿，外环控制器，
[0198][0199]
构造成含优化变量的函数，
[0200][0201]
式中，x
d_f
，和为摆动腿末端位姿期望轨迹的位置、速度、加速度，为反馈控制计算得到的控制量，xf和为实际状态，k
p_f
和k
d_f
为控制器对角增益矩阵；为雅克比矩阵j
if_task
的导数。
[0202]
5)第五优先级任务：关节力矩最小，根据整体动力学的主动驱动部分，
[0203][0204]
期望关节力矩τ趋近于0，构造含优化变量的函数，
[0205][0206]
式中，ma为腿的惯性矩阵，ha为腿的离心力、科氏力项、重力项，
ijca
为腿的足端接触雅克比矩阵。
[0207]
6)第六优先级任务：足端接触力补偿量最小，期望足端接触力补偿量δfc趋近于0，构造含优化变量的函数，
[0208][0209]
式中，i为基座惯量矩阵。
[0210]
确定优先级任务后，根据优先级任务，使足端接触力跟踪所述控制力旋量，构建分级优化算法：
[0211][0212]
式中，ai指第一至第六优先级的左边项矩阵，指优化变量矩阵，bi指第一至第六优先级的右边项矩阵，i＝1,2,3
…
6。
[0213]
优化算法按任务优先级的顺序从第一个至第六个个任务依次进行优化求解，在按照优先级任务的顺序对所述分级优化算法依次进行每次优化求解时，还需要满足等式约束和不等式约束；
[0214]
等式约束的物理含义是第i个优先级任务的优化解对应的要与之前(i-1)个分别相等，其任务是使第i个优先级任务的解不影响此前(i-1)个高优先级任务的解
对应的控制目标，实现严格的优先级分层：
[0215][0216]
上式的物理含义是第i个优先级任务的优化解对应的要与之前(i-1)个分别相等，式中，aj指第一至第六优先级的左边项矩阵，在计算过程中它是与ai同步对应的，指优化变量矩阵，在计算过程中它是与同步对应的j＝1,2,3
…
6；
[0217]
不等式约束包括关节力矩约束和摩擦锥约束：
[0218]
τ
min
≤τ≤τ
max
[0219]u·ijc≤0
[0220]
式中，τ，τ
min
，τ
max
指关节力矩，最小力矩，最大力矩；u指约束矩阵，jc指足端接触矩阵。
[0221]
在优化问题求解获得优化解后，使用动力学分解模型即可计算得到主动驱动关节力矩，即关节控制力矩：
[0222][0223]
步骤112，根据所述关节控制力矩，实时控制双足机器人运动。
[0224]
需要说明，本技术中变量上带有和统一表示为变量的一阶导数和二阶导数；x,y,z轴分别指向机器人的前进方向、左侧方向、垂直朝上方向。
[0225]
在本实施例中，如图2所示，操作员通过遥控手柄给出机器人前进和侧向速度v
b,x
,v
b,y
及偏航角速度ω
b,z
，通过速度积分和多项式轨迹规划获得浮动基座的期望状态xd。有限状态机根据接触力估计值进行接触事件判断，得到控制器模式(control_mode)和腿工作模式(leg_mode)，引入到模型预测控制器和摆动腿轨迹规划器及任务分层优化控制器中。浮动基座期望状态xd和实际状态估计值通过mpc计算后获得支撑腿的接触力，另外摆动腿x,y和z方向的落足点和轨迹规划在同步计算，都实时输出给任务分层优化控制器，在产生关节控制力矩后驱动机器人运动。浮动基座状态估计由imu加速度信息与支撑腿运动学信息融合得到，由于不是本文关注重点，这里不再累述。
[0226]
串级控制框架的核心是模型预测任务分层优化整体控制器，主要分为两个部分：第一部分是上层的基于浮动基座单刚体模型的模型预测控制器，第二部分是下层的基于整体动力学模型的任务分层优化控制器。
[0227]
(1)在上层中，本文采用欠驱动分时控制原理，将一个步态周期分割成支撑期和摆动期两个部分。对应所研究的双足机器人，在一条腿处于支撑期时，采用基于单刚体模型的mpc来控制机器人姿态和高度，另一条腿处于摆动期，通过落足点来控制机器人前进和侧向速度。然后将计算得到的控制力旋量以及摆动腿的足端轨迹期望值，发送给下层控制器。
[0228]
(2)在下层中，任务包括全身动力学约束，足端不滑动约束、位姿控制任务和力矩最小任务，基于任务分层优化控制器，优化地计算机器人主动驱动关节力矩。这样保证了在分时控制中每个部分的目标量都是可控的，然后左右腿交替往复运动，从而实现了大惯量腿双足机器人高速动态运动的三维全向运动和抗扰控制。
[0229]
上述基于任务分层优化的双足机器人级联控制方法，提出了考虑腿部动态的模型预测与任务分级优化整体控制相结合的高速运动轨迹跟踪方法。基于欠驱动机器人的分时
控制框架，在上层设计了基于忽略腿部的单刚体模型mpc，计算支撑期的姿态和高度方向的力旋量，同时采用落足点原理来控制机器人前进和侧向速度；在下层使用考虑腿部动态的任务分层优化整体控制器去跟踪力旋量；在优化变量中设计了足端接触力松弛变量，使用该变量补偿单刚体模型与完整动力学模型中两个足端接触力的差异，从而搭建了上层模型预测控制器与下层整体控制器的纽带关系。液压驱动的大重量的腿和外界冲击环境对缺少驱动自由度的双足机器人的高速运动来说更具挑战性，通过行走步态高速前进实验和外部冲击稳定性仿真实验验证了该方法在高速运动和扰动下的鲁棒性，并将机器人最大行走速度由模型预测方法的1.9m/s提升到2.5m/s。
[0230]
为了验证本技术提出的控制框架算法的高动态抗扰性能，搭建了基于多体动力学虚拟样机分析软件recurdyn与数学计算软件matlab的双足机器人多体动力学仿真平台，仿真机器人的机械结构和构件重量由制造实物机器人的三维模型直接生成和导入。
[0231]
控制算法参数设置如下：模型预测控制器的控制频率为100hz，权重设置为q＝diag([3e6,3e6,1e7,0,0,1e9,1e3,1e3,1e4,0,0,1e6,0]),r＝diag([0.1,0.1,0.1,0.1，0.1,0.1,0.1,0.1])，预测步数n
p
＝3，控制步数nc＝1，设置较小的步数主要是为了降低矩阵维度，提高单次优化的计算速度。在基于事件和时间混合的有限状态机中，设置摆动期最长时间为0.25s；在摆动腿规划中的步高为0.15m。摩擦锥约束中z轴方向最大足端力为600n，摩擦系数为0.6。模型预测控制方案中机器人模型参数如表1所示。
[0232]
表1机器人模型参数
[0233][0234][0235]
任务序列优化控制频率为500hz，设置浮动基座任务的姿态pd增益为k
p_pos
＝20
·
diag([3,3,1])，k
d_pos
＝2
·
diag([3,3,1])，对应顺序分别是滚转角、俯仰角和偏航角。高度的pd增益为k
p_h
＝300，k
d_body
＝30。摆动腿任务的pd增益为k
p_swing
＝5
·
diag([1,1,1,7])，k
d_swing
＝0.5
·
diag([1,1,1,7])。由于该算法设计的目标是提高机器人运动速度和抗扰能力，所以进行了两组仿真实验，分别是(a)行走步态高速前进实验，(b)外部冲击稳定性实验。
[0236]
仿真实验(a)：行走步态高速前进运动
[0237]
使用多项式拟合方法拟定一个平滑过渡的前进速度轨迹，从零到最大速度的过渡时间为15s，设置最大期望速度2.5m/s，期望前进位置使用速度的积分获取。浮动基座高度设置为0.75m，其他期望状态都设置为0。在平地上进行高速运动仿真实验，得到了机器人高动态运动过程图见图3、z轴方向的足端接触力和力矩曲线见图4、机器人浮动基座欧拉角和在惯性系下的位置跟踪曲线见图5图6及x和y轴方向的速度跟踪曲线见图7。
[0238]
从图4的z轴方向的足端接触力可以看出，从10s至15s，机器人速度从1.8m/s提升到2.5m/s，支撑阶段的足端接触力越来越大，说明控制器提供了与越来越快的前进速度相适应的力来控制机器人姿态和高度。
[0239]
在图5浮动基座欧拉角轨迹曲线中，由于是前进运动，滚转角可以均匀地控制在
±
0.05rad范围内，俯仰角和偏航角在从0-2.5m/s的加速阶段呈现出了等速增加的现象，在速度稳定在2.5m/s之后，这两个姿态角也分别在-0.08～0.05rad和-0.11～0.1rad之间有限幅度随着步态切换而周期稳定地波动。
[0240]
在图6和图7中，相对于vmc最大速度1.4m/s，mpc的则是1.9m/s，串联控制框架的机器人的位置和前进速度轨迹较好地跟踪上了期望轨迹，最大前进速度达到了2.5m/s，比单独使用的模型预测控制器1.9m/s的最大前进速度提高了0.6m/s，提升比例为31.5％，说明任务分层优化控制在姿态和高度的稳定控制及整体动力学模型的使用对机器人动态运动速度的提升具有较大的帮助。在侧向位置和速度控制上，由于机器人在高速状态下运动，导致侧向速度控制有一定的偏移，侧向位移从第10s至第20s缓慢偏移了0.25m距离。该偏移量在如此高动态运动中是可以接受的。机器人高度则一直控制在期望值0.75m位置，说明整体控制基于整体模型的动态补偿精度较高，将模型预测控制方法中的0.73m高度提升了0.02m。
[0241]
仿真实验(b)：外部冲击稳定性
[0242]
为了验证控制器应对外界冲击的稳定性，本实验在机器人前进过程中分别从后向、侧向和垂直向下三个方向在浮动基座上施加持续一定时间的冲击力。在多次测试后获得满足机器人动态平衡的最大冲量分别为：在3.5-4s区间沿x轴正方向施加冲击力60n，冲量为30n.s；在6.5-7s区间沿y轴正方向施加冲击力115n，冲量为57.5n.s；在8.5-9s区间沿z轴负方向施加冲击力150n，冲量为75n.s。图8为浮动基座欧拉角跟踪曲线，图9展示了x和y轴方向的速度跟踪曲线及高度曲线。
[0243]
在姿态控制方面：在3.5-4s区间受到后向冲击时，随着冲量的累加，最先产生影响的是俯仰角在该区间逐步增加直到第4s时刻达到最大峰值-0.096rad，由于冲量在支撑腿上形成的旋转力矩，也使偏航角发生较大偏转，正负向峰值分别达到0.173rad和-0.16rad，同时滚转角最大峰值为-0.145(rad)。在6.5-7s区间的侧向冲击中，受影响较大的是滚转角，在第7秒时达到最大峰值0.17rad。随后通过模型预测控制产生的足端接触力旋量和序列优化wbc的优先级及力跟踪控制，经过1秒时间的支撑腿控制，将上述姿态都快速地控制在了较小幅值的稳态波动范围内。
[0244]
在速度和高度控制方面：从后向冲击开始时刻第3.5s至终止时刻第4s，前向速度从1m/s增加到1.6m/s。在侧向冲击时，侧向速度从0增加到0.7m/s。机器人突然偏离期望速度。
[0245]
使控制器重新计算落足点，通过多步跨步重新建立新的平衡点。高度在受到冲击
后小幅度下降，然后迅速恢复到0.75m位置。
[0246]
总之，级联控制框架继承了模型预测方法的抗扰动性能，并通过任务分层优化控制提高了机器人浮动基座姿态和高度控制的稳定性，大幅提升了机器人行走步态动态运动的前进速度。在本文的仿真实验中使用的是双足机器人实物三维模型，物理量数值与实物接近，所以本文所完成的机器人轨迹跟踪高速运动抗扰控制方法研究对在实物上实现动态控制具有较好的理论指导意义。
[0247]
通过将任务分层优化控制与模型预测控制相结合，在基于整体模型的分层优化作用下，通过更加精确地控制浮动基座的姿态和高度，更好地协调机器人的稳定性，提高了机器人抗扰能力和运动速度，从而提出了考虑腿部动态的双足机器人高速运动级联控制框架。通过行走步态高速前进运动和外部冲击稳定性仿真实验，在保持模型预测方法抗扰性能的同时，将机器人行走步态动态最大前进速度从模型预测控制的1.9m/s大幅提高到2.5m/s。
[0248]
应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
[0249]
如图10所示，本技术还提供一种车载多波段立体视觉感知装置，在一个实施例中，包括：获取模块1002、估计模块1004、模型预测模块1006、规划模块1008、分级优化模块1010和控制模块1012，具体的：
[0250]
获取模块1002，用于获取双足机器人的期望速度和期望角速度，根据所述期望速度和所述期望角速度，计算期望基座状态；
[0251]
估计模块1004，用于获取双足机器人的运动状态，并根据所述运动状态进行基座状态估计和足端接触力估计；
[0252]
模型预测模块1006，用于根据所述期望基座状态和所述基座状态估计，构建单刚体模型，计算双足机器人支撑腿的控制力旋量；
[0253]
规划模块1008，用于根据所述期望速度，计算双足机器人摆动腿的落足点，并得到轨迹规划；
[0254]
分级优化模块1010，用于根据所述控制力旋量以及所述轨迹规划，使所述足端接触力跟踪所述控制力旋量，计算松弛变量；根据当前足端接触力和所述松弛变量计算双足机器人的关节控制力矩；
[0255]
控制模块1012，用于根据所述关节控制力矩，实时控制双足机器人运动。
[0256]
关于一种基于任务分层优化的双足机器人级联控制装置的具体限定可以参见上文中对于一种基于任务分层优化的双足机器人级联控制方法的限定，在此不再赘述。上述装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
[0257]
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例
中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0258]
以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本技术构思的前提下，还可以做出若干变形和改进，这些都属于本技术的保护范围。因此，本技术专利的保护范围应以所附权利要求为准。