液压驱动移动作业机器人的末端接触力控制方法及系统

1.本发明属于机器人末端力控制技术领域，尤其涉及液压驱动移动作业机器人的末端接触力控制方法及系统。


背景技术：

2.本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。
3.移动作业机器人集成了移动机器人和多连杆机械臂，扩展了工作空间的同时提高了复杂作业的能力，具有广阔的应用前景。由于液压系统具有高功率密度和爆发力强的特点，所以液压驱动移动作业机器人广泛应用于工程建设、抢险救灾、科学探索等多个大负载作业场景中，这是电驱动移动作业机器人所无法比拟的。
4.随着人-机-环境交互的任务的增加，对移动作业机器人的作业柔顺性有着较高的要求。当前,面向移动交互作业的研究在电驱动轻量化移动作业机器人上已经能够初步达到需求。而液压移动机器人大多用于搬运、清障等不需要考虑交互的作业任务。遗憾的是，对于液压平台交互作业控制研究较少，这会大大约束其应用。然而液压平台需要为特定任务提供精确的接触力，以提高操作质量。
5.然而，发明人发现，现有技术中对液压移动作业机器人面向交互作业的末端输出力进行调节时，调节范围较为有限，既没有充分发挥液压系统大负载作业的优势，也不能很好的兼顾液压移动作业机器人在交互作业中保持适当的接触力，不利于提升液压移动作业机器人作业的全能性。


技术实现要素：

6.为克服上述现有技术的不足，本发明提供了液压驱动移动作业机器人的末端接触力控制方法及系统，设计了面向液压驱动移动机械臂的交互力控制策略，通过腿足浮动基座的输出力，实现作业末端的力/运动混合控制。
7.为实现上述目的，本发明的一个或多个实施例提供了如下技术方案：
8.本发明第一方面提供了液压驱动移动作业机器人的末端接触力控制方法。
9.液压驱动移动作业机器人的末端接触力控制方法，包括以下步骤：
10.定义坐标系，描述四足机器人的状态；
11.将机械臂视为连杆结构，基于四足机器人的状态，考虑机械臂作业末端的力和扭矩的运动学接触约束，同时考虑机器人足端接触约束，得到基于接触约束的机器人浮动基座动力学模型；
12.将交互作业任务分解为运动控制子任务和力控制子任务，其中，运动控制子任务采用比例微分的控制方式进行规划期望的加速度，力控制子任务通过基于直接力反馈的力控制策略，得到规划出的期望输出；
13.定义控制系统的决策变量，将系统实际输出和决策变量表示成线性关系，基于机
器人浮动基座动力学模型、机械臂作业末端的力和扭矩的运动学接触约束和足端接触约束，确定系统实际输出关于决策变量的线性关系表达式；
14.设计关于机器人末端接触力的控制架构，基于系统实际输出关于决策变量的线性关系表达式，采用二次型优化的方法求解决策变量，调节控制决策变量使系统实际输出趋向于期望输出；
15.基于求解出的决策变量，对机器人进行力控制。
16.进一步的，定义坐标系，描述四足机器人的状态，具体包括：
17.定义惯性坐标系{i}＝{oi,xi,yi,zi}任一放置于原点oi，单位向量为{xi,yi,zi}，其中zi的方向与重力方向相反；机器人躯干坐标系{r}＝{or,xr,yr,zr}固定连接在机器人躯干处，向量xr沿机器人前进方向，向量yr沿机器人前进方向左侧；定义{e}＝{oe,xe,ye,ze}为机器人末端坐标系，其中ze沿作业末端的轴向；在nj个关节自由度加上表示躯干位置和姿态的nb个虚拟自由度；机器人系统广义空间变量表示为：
18.q＝[q
bt
,q
jt
]
t
[0019]
其中：qb表示机器人躯干的位置和姿态，qj表示机器人nj个关节变量。
[0020]
进一步的，考虑机械臂作业末端的力和扭矩的运动学接触约束，同时考虑机器人足端接触约束，得到基于接触约束的机器人浮动基座动力学模型，具体包括：
[0021]
对机械臂作业末端的力和扭矩施加运动学接触约束，得到机械臂作业末端接触约束的加速度级数学表达式；对机械臂作业末端未施加位置约束的方向通过位置控制的方式完成任务，得到机械臂作业末端未加约束的方向的运动任务表达式；
[0022]
考虑机器人足端在触地时不发生滑移，增加足部触地约束，得到足端接触约束的加速度级数学表达式；基于腿部结构计算出腿部的雅可比矩阵，基于腿部的雅克比矩阵，得到机器人摆动腿的运动任务表达式；
[0023]
基于机械臂作业末端接触约束的加速度级数学表达式和足端接触约束的加速度级数学表达式，得到机器人浮动基座动力学模型。
[0024]
进一步的，机器人浮动基座动力学模型为：
[0025][0026]
其中，m表示机器人惯性矩阵；c为机器人科氏里奥力和向心力矩阵；g为机器人重力向量；s
τt
为驱动关节选择矩阵；τ为机器人驱动关节扭矩；fa表示作业负载末端施加在目标物上的力与扭矩，表示机器人足端与地面的交互力与扭矩；sa正半定矩阵，用来选择运动约束施加的方向，ja为将机器人作业负载末端的速度转化为广义关节速度的矩阵；是足端状态映射到广义关节空间的雅克比矩阵。
[0027]
进一步的，所述规划出的期望输出的表达式为：
[0028]
[0029]
其中：pr表示躯干运动、pj表示关节运动、fa表示接触力输出、表示末端运动、表示摆动腿运动；变量中标注上标d的代表期望的控制变量；k
p,t
、k
d,t
是正定矩阵，分别表示比例、微分增益矩阵，m＝{r,j,a,l}分别代表躯干运动、关节运动、末端运动和摆动腿运动；表示实际测量接触力；k
p,f
表示调节接触力的比例矩阵。
[0030]
进一步的，系统实际输出关于决策变量的线性关系表达式为：
[0031]
w＝au+b
[0032]
其中：u表示决策变量；ω表示为系统实际输出；a和b表示为：
[0033][0034][0035]
是位置约束施加的选择矩阵；表示腿部的雅克比矩阵。
[0036]
进一步的，关于决策变量、期望输出和系统实际输出的控制架构，具体为：
[0037][0038][0039]ul
≤u≤uu[0040]
hfc≤h
[0041]
其中，控制系统的决策变量为u＝[τ
t
,f
at
,f
lt
]
t
；其中，w是正定矩阵，用来设定机器人任务优先级，避免任务冲突导致的作业任务失败；s是正定矩阵，用来最小化系统输入u；u
l
、uu是决策变量u的上下界；表示包含机械臂末端和腿部末端运动的雅克比矩阵；h＝[h
ft
,h
jt
]
t
表示包含摩擦堆和关节限制的系统不等式约束。
[0042]
摩擦堆约束为腿足式机器人优化方法的公知，在此不再进行赘述。
[0043]
本发明第二方面提供了液压驱动移动作业机器人的末端接触力控制系统。
[0044]
液压驱动移动作业机器人的末端接触力控制系统，包括：
[0045]
状态描述模块，被配置为：定义坐标系，描述四足机器人的状态；
[0046]
机器人浮动基座动力学模型构建模块，被配置为：将机械臂视为连杆结构，基于四足机器人的状态，考虑机械臂作业末端的力和扭矩的运动学接触约束，同时考虑机器人足端接触约束，得到基于接触约束的机器人浮动基座动力学模型；
[0047]
期望输出规划模块，被配置为：将交互作业任务分解为运动控制子任务和力控制子任务，其中，运动控制子任务采用比例微分的控制方式进行规划期望的加速度，力控制子
任务通过基于直接力反馈的力控制策略，得到规划出的期望输出；
[0048]
线性关系表达式建立模块，被配置为：定义控制系统的决策变量，将系统实际输出和决策变量表示成线性关系，基于机器人浮动基座动力学模型、机械臂作业末端的力和扭矩的运动学接触约束和足端接触约束，确定系统实际输出关于决策变量的线性关系表达式；
[0049]
决策变量求解模块，被配置为：设计关于决策变量、期望输出和系统实际输出的控制架构，基于系统实际输出关于决策变量的线性关系表达式，采用二次型优化的方法求解决策变量，调节控制决策变量使系统实际输出趋向于期望输出；
[0050]
力控制模块，被配置为：基于求解出的决策变量，对机器人进行力控制。
[0051]
本发明第三方面提供了计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现如本发明第一方面所述的液压驱动移动作业机器人的末端接触力控制方法中的步骤。
[0052]
本发明第四方面提供了电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明第一方面所述的液压驱动移动作业机器人的末端接触力控制方法中的步骤。
[0053]
以上一个或多个技术方案存在以下有益效果：
[0054]
(1)本发明公开了一种液压驱动移动作业机器人的末端接触力控制方法及系统，在控制策略中将躯干运动、末端运动、交互力控制、腿部运动等多个任务以及关节限制、摩擦锥等多个约束转化为一个多目标优化的问题，并通过权重矩阵协调各个任务之间的优先级，避免多任务之间的冲突。
[0055]
(2)本发明通过运动约束分解位置控制与力控制空间，并分别在各自运动空间设计控制律进行控制，解决了基座输出力与当前腿足机器人基于动力学优化控制框架之间的冲突，实现躯干运动与力协调输出，可大范围调节末端输出力的范围，发挥液压系统可以大负载作业的优势，又能在交互作业中保持适当的接触力，从而提高液压移动作业机器人的作业的全能性。
[0056]
本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
[0057]
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
[0058]
图1为第一个实施例的坐标系建立示意图。
[0059]
图2为第一个实施例的方法流程图。
具体实施方式
[0060]
应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
[0061]
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根
据本发明的示例性实施方式。
[0062]
在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
[0063]
本发明提出的总体思路：
[0064]
由于移动液压机械臂的动态特性高度非线性，在控制过程中模型简化和参数不确定性导致了模型的不准确性，以及多连杆的液压机械臂大重量大作业距离导致运动惯量大，使得力控制具有挑战性。液压四足机器人腿部结构大多为三个主动关节而且结构简单，实现力控制的难度比多自由的机械臂低。另外，腿足作业机器人具有冗余特性，依靠四足基座可实现作业末端的运动和力控制。利用以上特征可以通过力控基座实现移动机械臂末端的力控制，从而避开多自由度液压机械臂力控制的难题。因此，本发明设计了面向液压驱动移动机械臂的交互力控制策略，通过腿足浮动基座的输出力，实现作业末端的力/运动混合控制。
[0065]
实施例一
[0066]
本实施例公开了液压驱动移动作业机器人的末端接触力控制方法。
[0067]
如图1-2所示，液压驱动移动作业机器人的末端接触力控制方法，包括以下步骤：
[0068]
定义坐标系，描述四足机器人的状态；
[0069]
将机械臂视为连杆结构，基于四足机器人的状态，考虑机械臂作业末端的力和扭矩的运动学接触约束，同时考虑机器人足端接触约束，得到基于接触约束的机器人浮动基座动力学模型；
[0070]
将交互作业任务分解为运动控制子任务和力控制子任务，其中，运动控制子任务采用比例微分的控制方式进行规划期望的加速度，力控制子任务通过基于直接力反馈的力控制策略，得到规划出的期望输出；
[0071]
定义控制系统的决策变量，将系统实际输出和决策变量表示成线性关系，基于机器人浮动基座动力学模型、机械臂作业末端的力和扭矩的运动学接触约束和足端接触约束，确定系统实际输出关于决策变量的线性关系表达式；
[0072]
设计关于机器人末端接触力的控制架构，基于系统实际输出关于决策变量的线性关系表达式，采用二次型优化的方法求解决策变量，调节控制决策变量使系统实际输出趋向于期望输出；
[0073]
基于求解出的决策变量，对机器人进行力控制。
[0074]
具体的，本发明的具体实施方式包含基于接触约束的动力学建模和设计多任务的接触力控制方法两个方面。
[0075]
1.基于接触约束的动力学建模
[0076]
如图1所示，为了描述四足机器人的状态，定义惯性坐标系{i}＝{oi,xi,yi,zi}任一放置于原点oi，单位向量为{xi,yi,zi}，其中zi的方向与重力方向相反。机器人躯干坐标系{r}＝{or,xr,yr,zr}固定连接在机器人躯干处，向量xr沿机器人前进方向，向量yr沿机器人前进方向左侧。定义{e}＝{oe,xe,ye,ze}为机器人末端坐标系，其中ze沿作业末端的轴向。为了描述浮动基座平台，在nj个关节自由度加上表示躯干位置和姿态的nb个虚拟自由度。机器人系统广义空间变量可以表示为：
[0077]
q＝[q
bt
,q
jt
]
t
ꢀꢀꢀꢀ
(1)
[0078]
其中qb表示机器人躯干的位置和姿态。qj表示机器人nj个关节变量。
[0079]
在文中用ha表示在惯性坐标系下作业负载与目标物接触的位置与姿态，用表示四足机器人足端与地面交互的位置与姿态。fa表示作业负载末端施加在目标物上的力与扭矩，表示机器人足端与地面的交互力与扭矩。
[0080]
将机载机械臂视为连杆结构，这样就够得到机械臂末端到躯干质心速度的映射关系：
[0081][0082]rab
为作业末端到躯干质心处的位置向量，r
ab
×
是表示叉乘的斜对称矩阵，i,0分别表示单位阵和零矩阵。ja可将机器人作业负载末端的速度转化为广义关节速度，更换不同的工具时仅需修改矩阵ja。
[0083]
假设机器人的作业对象是刚性的、平滑的，该假设适应于例如打磨、焊接等实际工业场景。力和扭矩施加的施加需要对应的位置约束，所以需要在移动机械臂末端施加运动学约束来描述交互接触模型：
[0084][0085]
其中sa正半定矩阵，用来选择运动约束施加的方向，运动约束方向的个数为nc个，在每个交互任务中sa的数值不变。利用运动学模型，将末端接触约束映射到广义关节空间：
[0086][0087]
其中将式(4)对时间进行微分可得到作业末端接触约束的加速度级数学表达式：
[0088][0089]
其余未施加位置约束的方向通过位置控制的方式完成任务，其表达形式约束可以写成：
[0090][0091]
其中里面是位置约束施加的选择矩阵,是矩阵sa的补充,va表示末端未施加位置约束方向的速度。
[0092]
对式(6)关于时间进行微分可以得到移动机械臂末端未加约束的方向的运动任务：
[0093][0094]
为了保证机器人的足端在触地时不发生滑移，我们添加了的限制，足端的触地约束的个数为nk个，足端触地约束在广义关节空间可以表示为：
[0095][0096]
这里是足端状态映射到广义关节空间的雅克比矩阵，对式(8)关于时间进行微
分可得到足端接触约束的加速度级数学表达式：
[0097][0098]
同时通过基于腿部结构计算出来的雅克比矩阵j
lf
可以实现摆动腿的末端速度v
l
到广义关节空间的映射：
[0099][0100]
对式(10)关于时间进行微分可以得到机器人摆动腿的运动任务：
[0101][0102]
结合上述推导移动机械臂末端和足端的接触约束，机器人浮动基座动力学模型可以表示为：
[0103][0104]
其中，m:机器人惯性矩阵，c为机器人科氏里奥力和向心力矩阵，g为机器人重力向量，s
τt
为驱动关节选择矩阵，τ为机器人驱动关节扭矩。
[0105]
2.基于多任务优化的接触力控制方法
[0106]
本发明的移动接触力控制方法框图如图2所示，机器人通过运动约束将接触力作业任务分成运动控制部分和力控制部分。力控制部分使用基于直接力反馈的控制方法，运动控制部分使用比例微分控制来设计期望加速度，最后将多个控制任务和多个约束转化为一个多任务二次优化问题进行求解。
[0107]
由于机器人接触作业涉及以下多个任务：躯干运动pr、关节运动pj、接触力输出fa、末端运动和摆动腿运动所以定义系统的输出为
[0108][0109]
另外，本文定义控制系统的决策变量为：
[0110]
u＝[τ
t
,f
at
,f
lt
]
t
ꢀꢀꢀꢀ
(14)
[0111]
(1)运动/力混合控制
[0112]
通过前文推导的运动约束将交互作业任务分解为运动控制子任务和力控制子任务，其中运动子任务采用比例微分的控制方式进行规划期望的加速度，而力控制子任务通过基于直接力反馈的力控制策略，加入实际力传感器的数据对例如摩擦力等干扰进行补偿。规划出的期望输出ω
*
可以表示为：
[0113][0114]
其中，变量中标注上标d的代表期望的控制变量。k
p,t
、k
d,t
是正定矩阵，分别表示比例、微分增益矩阵，t＝{r,j,a,l}分别代表躯干运动、关节运动、末端运动和摆动腿运动。表示实际测量接触力，k
p,f
表示调节接触力的比例矩阵。
[0115]
(2)多任务优化控制
[0116]
结合上文推导的基于接触约束的浮动基座动力学模型式(12)、机械臂末端运动任务(7)和摆动腿的运动任务(11)，可将系统输出ω和决策变量u的关系表示成以下形式：
[0117]
w＝au+b
ꢀꢀꢀꢀ
(16)
[0118]
其中a，b中不包含控制输入u，a和b可以表示为：
[0119][0120][0121]
通过调节控制输入量u使ω趋向于ω
*
，因为式(16)中ω和u是线性关系，故本文采用二次型优化的方法求解决策变量u，控制架构采用以下形式：
[0122][0123]
其中w是正定矩阵，用来设定机器人任务优先级，避免任务冲突导致的作业任务失败。s是正定矩阵，用来最小化系统输入u。u
l
、uu是控制输入变量u的上下界。表示包含机械臂末端和腿部末端运动的雅克比矩阵。h＝[h
ft
,h
jt
]
t
表示包含摩擦堆和关节限制的系统不等式约束。
[0124]
实施例二
[0125]
本实施例公开了液压驱动移动作业机器人的末端接触力控制系统。
[0126]
液压驱动移动作业机器人的末端接触力控制系统，包括：
[0127]
状态描述模块，被配置为：定义坐标系，描述四足机器人的状态；
[0128]
机器人浮动基座动力学模型构建模块，被配置为：将机械臂视为连杆结构，基于四足机器人的状态，考虑机械臂作业末端的力和扭矩的运动学接触约束，同时考虑机器人足端接触约束，得到基于接触约束的机器人浮动基座动力学模型；
[0129]
期望输出规划模块，被配置为：将交互作业任务分解为运动控制子任务和力控制子任务，其中，运动控制子任务采用比例微分的控制方式进行规划期望的加速度，力控制子任务通过基于直接力反馈的力控制策略，得到规划出的期望输出；
[0130]
线性关系表达式建立模块，被配置为：定义控制系统的决策变量，将系统实际输出和决策变量表示成线性关系，基于机器人浮动基座动力学模型、机械臂作业末端的力和扭矩的运动学接触约束和足端接触约束，确定系统实际输出关于决策变量的线性关系表达
式；
[0131]
决策变量求解模块，被配置为：设计关于机器人末端接触力的控制架构，基于系统实际输出关于决策变量的线性关系表达式，采用二次型优化的方法求解决策变量，调节控制决策变量使系统实际输出趋向于期望输出；
[0132]
力控制模块，被配置为：基于求解出的决策变量，对机器人进行力控制。
[0133]
实施例三
[0134]
本实施例的目的是提供计算机可读存储介质。
[0135]
计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如本公开实施例1所述的液压驱动移动作业机器人的末端接触力控制方法中的步骤。
[0136]
实施例四
[0137]
本实施例的目的是提供电子设备。
[0138]
电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本公开实施例1所述的液压驱动移动作业机器人的末端接触力控制方法中的步骤。
[0139]
以上实施例二、三和四的装置中涉及的各步骤与方法实施例一相对应，具体实施方式可参见实施例一的相关说明部分。术语“计算机可读存储介质”应该理解为包括一个或多个指令集的单个介质或多个介质；还应当被理解为包括任何介质，所述任何介质能够存储、编码或承载用于由处理器执行的指令集并使处理器执行本发明中的任一方法。
[0140]
本领域技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。
[0141]
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

技术特征：
1.液压驱动移动作业机器人的末端接触力控制方法，其特征在于，包括以下步骤：定义坐标系，描述四足机器人的状态；将机械臂视为连杆结构，基于四足机器人的状态，考虑机械臂作业末端的力和扭矩的运动学接触约束，同时考虑机器人足端接触约束，得到基于接触约束的机器人浮动基座动力学模型；将交互作业任务分解为运动控制子任务和力控制子任务，其中，运动控制子任务采用比例微分的控制方式进行规划期望的加速度，力控制子任务通过基于直接力反馈的力控制策略，得到规划出的期望输出；定义控制系统的决策变量，将系统实际输出和决策变量表示成线性关系，基于机器人浮动基座动力学模型、机械臂作业末端的力和扭矩的运动学接触约束和足端接触约束，确定系统实际输出关于决策变量的线性关系表达式；设计关于机器人末端接触力的控制架构，基于系统实际输出关于决策变量的线性关系表达式，采用二次型优化的方法求解决策变量，调节控制决策变量使系统实际输出趋向于期望输出；基于求解出的决策变量，对机器人进行力控制。2.如权利要求1所述的液压驱动移动作业机器人的末端接触力控制方法，其特征在于，定义坐标系，描述四足机器人的状态，具体包括：定义惯性坐标系{i}＝{o
i
,x
i
,y
i
,z
i
}任一放置于原点o
i
，单位向量为{x
i
,y
i
,z
i
}，其中z
i
的方向与重力方向相反；机器人躯干坐标系{r}＝{o
r
,x
r
,y
r
,z
r
}固定连接在机器人躯干处，向量x
r
沿机器人前进方向，向量y
r
沿机器人前进方向左侧；定义{e}＝{o
e
,x
e
,y
e
,z
e
}为机器人末端坐标系，其中z
e
沿作业末端的轴向；在n
j
个关节自由度加上表示躯干位置和姿态的n
b
个虚拟自由度；机器人系统广义空间变量表示为：q＝[q
bt
,q
jt
]
t
其中：q
b
表示机器人躯干的位置和姿态，q
j
表示机器人n
j
个关节变量。3.如权利要求1所述的液压驱动移动作业机器人的末端接触力控制方法，其特征在于，考虑机械臂作业末端的力和扭矩的运动学接触约束，同时考虑机器人足端接触约束，得到基于接触约束的机器人浮动基座动力学模型，具体包括：对机械臂作业末端的力和扭矩施加运动学接触约束，得到机械臂作业末端接触约束的加速度级数学表达式；对机械臂作业末端未施加位置约束的方向通过位置控制的方式完成任务，得到机械臂作业末端未加约束的方向的运动任务表达式；考虑机器人足端在触地时不发生滑移，增加足部触地约束，得到足端接触约束的加速度级数学表达式；基于腿部结构计算出腿部的雅可比矩阵，基于腿部的雅克比矩阵，得到机器人摆动腿的运动任务表达式；基于机械臂作业末端接触约束的加速度级数学表达式和足端接触约束的加速度级数学表达式，得到机器人浮动基座动力学模型。4.如权利要求2所述的液压驱动移动作业机器人的末端接触力控制方法，其特征在于，机器人浮动基座动力学模型为：
其中，m表示机器人惯性矩阵；c为机器人科氏里奥力和向心力矩阵；g为机器人重力向量；s
τt
为驱动关节选择矩阵；τ为机器人驱动关节扭矩；f
a
表示作业负载末端施加在目标物上的力与扭矩，表示机器人足端与地面的交互力与扭矩；s
a
正半定矩阵，用来选择运动约束施加的方向，j
a
为将机器人作业负载末端的速度转化为广义关节速度的矩阵；是足端状态映射到广义关节空间的雅克比矩阵。5.如权利要求4所述的液压驱动移动作业机器人的末端接触力控制方法，其特征在于，所述规划出的期望输出的表达式为：其中：p
r
表示躯干运动、p
j
表示关节运动、f
a
表示接触力输出、表示末端运动、表示摆动腿运动；变量中标注上标d的代表期望的控制变量；k
p,t
、k
d,t
是正定矩阵，分别表示比例、微分增益矩阵，t＝{r,j,a,l}分别代表躯干运动、关节运动、末端运动和摆动腿运动；表示实际测量接触力；k
p,f
表示调节接触力的比例矩阵。6.如权利要求5所述的液压驱动移动作业机器人的末端接触力控制方法，其特征在于，系统实际输出关于决策变量的线性关系表达式为：w＝au+b其中：u表示决策变量；ω表示为系统实际输出；a和b表示为：其中：u表示决策变量；ω表示为系统实际输出；a和b表示为：其中：u表示决策变量；ω表示为系统实际输出；a和b表示为：其中：u表示决策变量；ω表示为系统实际输出；a和b表示为：是位置约束施加的选择矩阵；表示腿部的雅克比矩阵。7.如权利要求6所述的液压驱动移动作业机器人的末端接触力控制方法，其特征在于，关于决策变量、期望输出和系统实际输出的控制架构，具体为：关于决策变量、期望输出和系统实际输出的控制架构，具体为：u
l
≤u≤u
u
hf
c
≤h其中，控制系统的决策变量为u＝[τ
t
,f
at
,f
lt
]
t
；其中，w是正定矩阵，用来设定机器人任务优先级，避免任务冲突导致的作业任务失败；s是正定矩阵，用来最小化系统输入u；u
l
、u
u
是决策变量u的上下界；表示包含机械臂末端和腿部末端运动的雅克比矩阵；h＝[h
ft
,h
jt
]
t
表示包含摩擦堆和关节限制的系统不等式约束。8.液压驱动移动作业机器人的末端接触力控制系统，其特征在于：包括：状态描述模块，被配置为：定义坐标系，描述四足机器人的状态；机器人浮动基座动力学模型构建模块，被配置为：将机械臂视为连杆结构，基于四足机器人的状态，考虑机械臂作业末端的力和扭矩的运动学接触约束，同时考虑机器人足端接触约束，得到基于接触约束的机器人浮动基座动力学模型；期望输出规划模块，被配置为：将交互作业任务分解为运动控制子任务和力控制子任务，其中，运动控制子任务采用比例微分的控制方式进行规划期望的加速度，力控制子任务通过基于直接力反馈的力控制策略，得到规划出的期望输出；线性关系表达式建立模块，被配置为：定义控制系统的决策变量，将系统实际输出和决策变量表示成线性关系，基于机器人浮动基座动力学模型、机械臂作业末端的力和扭矩的运动学接触约束和足端接触约束，确定系统实际输出关于决策变量的线性关系表达式；决策变量求解模块，被配置为：设计关于机器人末端接触力的控制架构，基于系统实际输出关于决策变量的线性关系表达式，采用二次型优化的方法求解决策变量，调节控制决策变量使系统实际输出趋向于期望输出；力控制模块，被配置为：基于求解出的决策变量，对机器人进行力控制。9.计算机可读存储介质，其上存储有程序，其特征在于，该程序被处理器执行时实现如权利要求1-7任一项所述的液压驱动移动作业机器人的末端接触力控制方法中的步骤。10.电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-7任一项所述的液压驱动移动作业机器人的末端接触力控制方法中的步骤。

技术总结
本发明提出液压驱动移动作业机器人的末端接触力控制方法，涉及机器人力控制领域。包括定义坐标系，描述四足机器人的状态；基于四足机器人的状态，考虑机械臂作业末端的力和扭矩的运动学接触约束，同时考虑机器人足端接触约束，得到基于接触约束的机器人浮动基座动力学模型；将交互作业任务分解为运动控制子任务和力控制子任务，得到规划出的期望输出；定义控制系统的决策变量，确定系统实际输出关于决策变量的线性关系表达式；设计控制架构，求解决策变量，调节控制决策变量使系统实际输出趋向于期望输出；基于求解出的决策变量对机器人进行力控制。本发明通过腿足浮动基座的输出力，实现作业末端的力/运动混合控制。实现作业末端的力/运动混合控制。实现作业末端的力/运动混合控制。


技术研发人员：柴汇 刘松 宋锐 李贻斌 李岳炀 李骐光 付鹏 刘建利 杨志远
受保护的技术使用者：山东大学
技术研发日：2023.04.18
技术公布日：2023/7/20