机械臂位置力混合控制方法及装置

1.本发明涉及机械臂位置力混合控制领域，尤其是涉及一种机械臂位置 力混合控制。


背景技术：

2.在现代的物流码垛、航天空间、工业加工等领域，机械臂承担货物装 载、设备操作、产品加工等任务，也因某些环境特殊，衍生出特种机械臂。 本发明是基于一种双开链结构的重负载大行程的刚性伸缩臂，该机械臂可 运用于货物搬运具有巨大需求量的企业，帮助企业完成巨大货物量的码垛 工作。而在机械臂运动控制中，机械臂末端执行器在装载货物进行搬运的 时候，由于机械臂本身是刚性长伸缩臂和重负载能力的结构设计，容易出 现执行器工作精度降低，双级联动的伸缩臂末端振荡现象，在作业工程中， 该问题容易导致降低效率、甚至损伤环境。


技术实现要素：

3.本发明的目的在于提供机械臂位置力混合控制方法，旨在解决机械臂 位置力混合控制方法。
4.本发明提供一种机械臂位置力混合控制方法，包括：
5.s1、获取机械臂期望位置；
6.s2、根据机械臂期望位置计算期望机械臂关节位置，根据动力学模型 得出期望控制力矩；
7.s3、实时获取机械臂末端传感器反馈的机械臂实际位置和力数据；
8.s4、根据改进型混合模控制方法进行位置空间控制关节力矩，根据pd 控制进行力空间控制关节力矩；
9.s5、判断机械臂实际位置和力数据是否在允许误差范围内，若是，达 到目标位置后，控制结束，若否，重新执行s2到s5。
10.本发明实施例还提供一种机械臂位置力混合控制装置，包括：存储器、 处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所 述计算机程序被所述处理器执行时实现上述方法的步骤。
11.本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储 介质上存储有信息传递的实现程序，所述程序被处理器执行时实现上述 方法的步骤。
12.采用本发明实施例，针对双级联动的重载伸缩臂末端抖振问题，设计 一种改进型混合幂次函数，利用其更为平稳、光滑特性的滑模控制器来提 高机械臂系统的柔顺特性。
13.上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的 技术手段，依照说明书的内容予以实施，并且为了让本发明的上述和其它 目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。
附图说明
14.为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下 面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍， 显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普 通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获 得其他的附图。
15.图1是本发明实施例的机械臂位置力混合控制方法流程图；
16.图2是本发明实施例的机械臂位置力混合控制方法具体流程图；
17.图3是本发明实施例的机械臂位置力混合控制方法的混合控制框示意 图；
18.图4是本发明实施例的机械臂位置力混合控制装置示意图。
具体实施方式
19.下面将结合实施例对本发明的技术方案进行清楚、完整地描述，显然， 所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发 明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得 的所有其他实施例，都属于本发明保护的范围。
20.方法实施例
21.根据本发明实施例，提供了一种机械臂位置力混合控制方法流程图， 图1是本发明实施例的机械臂位置力混合控制方法流程图，如图1所示， 具体包括：
22.机械臂是一种非线性、具有强耦合特点的复杂系统，特种大行程伸缩 臂应用于重型装载，在搬运庞大货物量的情况下，由于不同的货物重量、 陆续不断的物件以及较多情况下的货物仍为重物，末端执行器在逐渐进行 长距离定点运载的时候，容易出现精度降低、振荡问题。
23.本发明目的是针对由于双级联动的齿驱伸缩臂在运载重物的情况引起 的末端抖振问题，将双切正切函数、阶跃函数与幂次函数结合，构造一种 新型幂次函数，提出一种改进型混合滑模控制策略，不仅使幂次趋近律提 高控制系统进入滑模面的趋近速率，还能进一步保证滑模开关的切换函数 连续且更具光滑特性，以抑制以往滑模切换函数仍具有的弱抖振问题，进 一步达到轨迹跟踪控制要求，削弱末端执行器的高频振荡，满足机械臂系 统的作业需求。
24.1.建立大行程重载刚性伸缩臂含摩擦力的动力学模型：
25.为了实现上述目的，需要先建立关于该刚体机械臂系统的动力学模型， 在考虑摩擦力情况下，得到关节空间动力学模型：
[0026][0027]
其中，分别为机械臂关节空间中的关节位移变量、速 度和加速度，d(q)∈r6×6为机械臂的正定惯性矩阵，为 机械臂的离心力和哥氏力矩阵，g(q)∈r6为重力矩阵，fv为粘滞摩擦系 数，fs为静摩擦系数，τ为机械臂在关节空间中的输入控制力矩，sign为符 号函数。
[0028]
由于机械臂工作在笛卡尔空间下，根据机器人学中的力雅克比矩阵 j
t
(q)进行动力学模型的空间转换：
[0029]
τ＝j
t
(q)f
x
[0030]
其中，j
t
(q)为机械臂系统的雅克比转置矩阵，f
x
为工作空间下末端执 行器的接触力。
[0031]
得到工作空间下末端执行器接触力的描述：
[0032][0033]
其中：
[0034]gx
(q)＝j
t
(q)g(q)、f
vx
＝j
t
(q)f
vj
(q)、(q)、则是关节工作空间中的速度和加速度， j

(q)＝[j
t
(q)j(q)]-1jt
(q)为该5自由度的大行程重载刚性伸缩臂伪逆雅克 比矩阵，f
x
为工作空间下末端执行器的接触力。
[0035]
建立含摩擦力的动力学模型能为后续控制系统提供计算驱动力作铺 垫。
[0036]
2.建立一种力空间的pd控制
[0037]
本机械臂控制策略采用力/位置混合控制，在力空间上，采用一种特殊 pd控制：
[0038][0039]
其中fd为动力学前馈期望力，fe为传感器反馈实际力，k
fp
为力空间的 比例调节系数，k
fd
为力空间的微分调节系数，为末端执行器期望速度， xd为传感器反馈末端速度，ff为力空间的控制律。
[0040]
该力方向控制器的pd控制不同于传统的误差及其微分进行系数调节， 在动力学前馈控制下，先加入力误差的比例调节，后将末端工具速度进行 微分调节，可使得在力空间的控制上对末端工具移动的突变速度进行微分 调节。
[0041]
3.建立一种改进型混合幂次趋近律的滑模控制
[0042]
先建立滑模控制变量，定义跟踪误差：
[0043]
e(t)＝xd(t)-xe(t)
[0044][0045]
其中λ为正定矩阵，xd(t)为末端期望位置，xe(t)为末端实际位置， 分别代表工作空间下末端执行器位置、速度和加速度 的期望与实际误差，分别代表末端执行器的理想位置、 速度和加速度，为末端执行器期望速度。
[0046]
定义滑模面：
[0047][0048]
其中λ为正定矩阵。
[0049]
为了减小控制量的抖振，设计一种改进型混合幂次趋近律滑模控制； 针对阶跃函数sign(x)与双曲正切函数的函数陡度特性，当时，其对应的幂次函数
比|x|
α
sign(x)函数在可调节陡度的同时， 使更具切换函数更具平滑特性。
[0050]
设计一种改性型混合幂次开关切换函数作为切换函数，代替传统滑模 变结构的阶跃函数或普通幂次函数，定义一种改进型非线性函数：
[0051][0052]
式中幂次函数的指数α1》0,α2》0，nfal(s,α1,α2,μ)函数在原点附近正负 对称线段的区间长度为0《μ《1，幂次函数陡度s为滑模函数，而 当滑模函数小于区间长度，即|s|《μ，则滑模自变量增益较小，反之则 增益较大。
[0053]
本发明设计的改进型混合幂次滑模趋近律如下：
[0054][0055]
ε为趋近律的收敛速率，当|s|《μ滑模系统接近于平衡点时，即s
→
0， 滑模趋近律由主导，此时nfal(s,α1,α2,μ)的趋近速度逐渐增大，由于零 点附近为连续光滑的幂次函数项，幂次函数的高陡度能保证控制系统可以 快速达到滑模面，相较于阶跃函数的切换开关，阶跃函数在转折处更显突 变性、不连续性，而在|s|》μ时，以函数在切换时，尤其在处 比与sign(x)更为平稳、光滑，因此通过高比重的收敛速率ε与 结合，前者能保证降低高频振动问题的同时，后者可起到 平稳输入、降低信号幅度的效果，仍能保证较大的速度趋近滑模模态。
[0056]
建立一种改进型混合幂次的滑模控制器：
[0057][0058]
在考虑摩擦力和不确定性扰动后，根据动力学模型和滑模控制器，取以下 滑模控制律：
[0059][0060]
其中:
[0061]fvx
为粘滞摩擦力、f
sx
为静摩擦力和δf为不确定性干扰项f
dis
的混合项。下标加 x是表达在笛卡尔空间下。
[0062]
在位置空间上完成一种带平滑特性的改进型混合滑模控制。
[0063]
4.建立大行程重载刚性伸缩臂系统的力/位混合控制器：
[0064]
为了对机械臂将工作任务的控制在笛卡尔空间中分解，利用关节空间 中可分为
位置空间和力空间进行混合控制。对于笛卡尔空间下末端力与关 节空间中各个关节力矩的关系，可由以下变换：
[0065]
τ＝j
t
(q)f
[0066]
因此通过选择矩阵s分别对位置空间和力空间进行力/位混合控制：
[0067]
τe＝sτ
p
(i-s)τf[0068]
其中i为单位矩阵，τ
p
为位置空间的控制向量，τf为力空间的控制向量， τe为控制系统的输出力矩，亦即控制机械臂各个关节输入力矩。
[0069]
本发明提出的改进型滑模趋近律中的幂次函数在原点附近 两侧的的区间长度内能切换更为平滑的连续幂次函数，改进 型幂次函数相较于|x|
α
sign(x)函数在原点的
±
0.1处更具连续特性，由于幂次 函数中的双曲正切函数的陡度值选取时常大于幂次函数的原点正负对称的 区间长度，在改进型混合幂次趋近律中，nfal(s,α1,α2,μ)起主导作用，因此 更能避免滑模系统在平衡点处出现高频抖动现象。
[0070]
而在机械臂系统中，主要为位置空间控制，双级联动的关节与高低幅 度关节采用改进型混合幂次滑模控制方法，而在工作空间中的微运动关节， 为降低控制系统工作负荷，采用特殊的pd力调节，其中的微分调节则作为 微运动关节速度突变作速度惩罚，从力空间上辅助整个机械臂系统的抖振 问题。
[0071]
在本实施例中，采用以改进型混合幂次滑模控制为主的力/位置混合控 制策略，相对比传统的机械臂控制方法具有更好的跟踪效果；在上位机启 动机械臂系统设置控制器参数以及反馈传感器数据后，采用上述力/位置混 合控制技术方案，对机械臂末端执行器上加载400kg的重物进行测试，与 以阶跃函数以及常用幂次函数的滑模控制器进行对比，可得到以下关于末 端执行器运动轨迹与控制时间图：
[0072]
关于大行程重载刚性伸缩臂的控制器参数表如下：
[0073]
表1
[0074][0075]
在xyz方向上，幂次滑模趋近律明显比以阶跃函数为切换开关函数的 滑模趋近律减少了多数抖振，且抖振的幅度也相对降低，幂次函数作为滑 模切换函数相比于阶跃函数，减少了大部分的抖振，表明控制系统在滑模 趋近律下更快地向平衡点趋近，体现幂次函数快速趋近滑模状态的特性。 对于常用幂次滑模趋近律，本发明所提出的改进型混合幂次趋近律，结合 了幂次函数与双曲正切函数的特点进行改进，进一步提高切换函数的连续 且光滑的特性，在x方向上能提高30％的跟踪精度，末段的振荡频率相对 降低，以及低频抖振的幅度也相应减小；在y和z方向上，则能提高 30％-40％的跟踪精度，在y方向上仍能表现出降低微抖振的幅度。因此本 发明提出的改进型混合幂次滑模趋近律能提高大行程重载伸缩臂系统的跟 踪能力，削弱大部分的高频振荡以及降低多数低频振动的幅度，保证了机 械臂控制系统的稳定性，能满足该特种机械臂的大范围重载搬运的性能需 求。
[0076]
大行程重载刚性伸缩臂系统的控制系统：
[0077]
参数设置模块：用于在上位机设定该大行程重载刚性伸缩臂中各连杆 关节参数：
[0078]
该特种机械臂的关节类型为：prrppr(p为移动关节、r为转动关节)， 其d-h模型参数表如下：
[0079]
表2大行程重载刚性伸缩臂d-h模型参数表
[0080][0081]
传感器读取模块：用于上位机系统实时获取机械臂的运动动态，可实 时采集机械臂末端执行器在笛卡尔空间中的位置数据，所采集数据信息能 被用于控制模块。
[0082]
动力学建立模块：用于上位机的控制系统，根据机械臂模型参数，计 算当前关节所需要的输入驱动力矩。
[0083]
力/位置混合控制模块：用于上位机在对机械臂系统进行控制，根据当 前机械臂末端执行器位置与期望位置的误差进行调整，通过控制模块的机 械臂关节输入力矩。图2是本发明实施例的机械臂位置力混合控制方法具 体流程图，如图2所示：
[0084]
s1、获取机械臂期望位置；
[0085]
s2、根据机械臂期望位置计算期望机械臂关节位置，根据动力学模型 得出期望控制力矩；
[0086]
s3、实时获取机械臂末端传感器反馈的机械臂实际位置和力数据；
[0087]
s4、根据改进型混合模控制方法进行位置空间控制关节力矩，根据pd 控制进行力空间控制关节力矩；
[0088]
s5、判断机械臂实际位置和力数据是否在允许误差范围内，若是，达 到目标位置后，控制结束，若否，重新执行s2到s5。
[0089]
根据改进型混合模控制方法进行位置空间控制关节力矩具体包括：
[0090]
计算跟踪误差，定义滑模控制变量；
[0091]
定义滑模面和设计改进型混合滑模控制策略；
[0092]
选择矩阵选择位置控制关节力矩。
[0093]
根据pd控制进行力空间控制关节力矩具体包括：
[0094]
计算期望力误差，进行力比例调节；
[0095]
计算速度误差，进行微分调节；
[0096]
选择矩阵选择力控制关节力矩。
[0097]
设计改进型混合滑模控制策略具体包括：
[0098]
定义改进型非线性函数；
[0099]
设计改进型混合幂次滑模趋近律；
[0100]
建立改进型混合幂次的滑模控制器；
[0101]
在考虑摩擦力和不确定性扰动后，根据动力学模型和滑模控制器，得 到滑模控制律；
[0102]
在位置空间上完成一种带平滑特性的改进型混合滑模控制。
[0103]
定义改进型非线性函数具体包括：
[0104]
定义如下改进型非线性函数：
[0105][0106]
式中幂次函数的指数α1》0,α2》0，nfal(s,α1,α2,μ)函数在原点附近正负 对称线段的区间长度为0《μ《1，幂次函数陡度s为滑模函数，而 当滑模函数小于区间长度，即|s|《μ，则滑模自变量增益较小，反之则 增益较大。
[0107]
设计改进型混合幂次滑模趋近律具体包括：
[0108]
设计改进型混合幂次滑模趋近律公式如下：
[0109][0110]
ε为趋近律的收敛速率，当|s|《μ滑模系统接近于平衡点时，即s
→
0， 滑模趋近律由主导，此时nfal(s,α1,α2,μ)的趋近速度逐渐增大，由于零 点附近为连续光滑的幂次函数项，幂次函数的高陡度能保证控制系统快速 达到滑模面。
[0111]
建立改进型混合幂次的滑模控制器具体包括：建立改进型混合幂次的 滑模控制器采用如下公式：
[0112][0113]
在考虑摩擦力和不确定性扰动后，根据动力学模型和滑模控制器，得 到滑模控制律具体包括：得到滑模控制律采用如下公式：
[0114][0115]
其中为摩擦力 和不确定性干扰项。
[0116]
图3是本发明实施例的机械臂位置力混合控制方法的混合控制框示意 图，如图3所示：
[0117]
由于本发明对象的大行程重载刚性伸缩臂更注重位置跟踪，因此技术 方案更多
的是放在位置控制上。在主导的位置空间上采用提出的改进型混 合幂次滑模控制律，通过结合幂次函数与双曲正切函数来达到双级联动的 长伸缩臂中，降低由于大移动范围的关节而使末端工具出现的高频微振动； 而在力空间上针对微运动的关节，则采用特殊pd控制，实现动力学前馈控 制和力反馈后，利用速度惩罚来对关节的突变抖振进行抑制。通过选择矩 阵，则可实现位置空间和力空间的混合混合控制。
[0118]
装置实施例一
[0119]
本发明实施例提供一种机械臂位置力混合控制装置，如图4所示，包 括：存储器40、处理器42及存储在存储器40上并可在处理器42上运行的 计算机程序，计算机程序被处理器执行时实现上述方法实施例中的步骤。
[0120]
装置实施例二
[0121]
本发明实施例提供一种计算机可读存储介质，计算机可读存储介质上 存储有信息传输的实现程序，程序被处理器42执行时实现上述方法实施例 中的步骤。
[0122]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非 对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的 普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进 行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或 者替换本发明各实施例技术方案，并不使相应技术方案的本质脱离本方案 的范围。