一种移动双臂机器人柔顺控制方法与流程

1.本发明属于智能机器人技术领域，具体涉及一种移动双臂机器人柔顺控制方法。


背景技术：

2.移动双臂机器人比单臂机器人具有更广的应用场景以及更灵活的操作能力，在很多领域逐渐得到越来越多的应用。柔顺控制是双臂机器人常用的一种控制技术，用于实现双臂机械手在柔顺接触或者操作物体如打磨、装配时保护机器人、工件以及工作人员的安全。为了实现柔顺控制须让机器人拥有力感知的能力，在运动过程中遇到阻碍而产生较大的接触力时，能够允许与期望的目标轨迹产生一定的偏移量进而起到柔顺接触、柔顺操作的效果。柔顺控制效果通常采用阻抗控制或者导纳控制方法来实现。
3.在现有技术中，柔顺控制没有被应用于轮式移动机器人上，即在移动机器人的机械臂末端实现了柔顺控制，而没有在移动本体上实现柔顺控制，也没有以移动双臂机器人整体为控制对象的柔顺控制方法。


技术实现要素：

4.有鉴于此，本发明提出一种移动双臂机器人柔顺控制方法，用于控制移动本体和/或机械臂在受到达到一预设阈值的施加的外力时对所述施加的外力进行柔性避让，使移动本体和两个机械臂均实现柔顺控制，使移动双臂机器人的移动操作更安全、末端执行精度更高，应对复杂、苛刻、极限环境的能力提高。
5.为了实现上述技术目的，本发明所采用的具体技术方案为：
6.一种移动双臂机器人柔顺控制方法，所述机器人包括移动本体以及安装在移动本体上的两个机械臂；
7.所述机器人上配置有所述柔顺控制方法；所述柔顺控制方法用于控制所述机器人的移动本体和/或机械臂在受到达到一预设阈值的施加的外力时对所述施加的外力进行柔性避让；
8.所述移动本体上设置有至少一组惯性测量装置，所述惯性测量装置用于测量所述移动本体的倾角及加速度。
9.进一步的，当两个所述机械臂之一受到施加的外力时：
10.当所述施加的外力达到一预设阈值时两个所述机械臂以及所述移动本体均分别执行所述柔顺控制方法；否则仅受到施加的外力的所述机械臂执行所述柔顺控制方法。
11.进一步的，当两个所述机械臂均受到施加的外力时：
12.当两个所述机械臂之一受到的施加的外力达到一预设阈值时，两个所述机械臂以及所述移动本体均分别执行所述柔顺控制方法；否则两个所述机械臂分别执行所述柔顺控制方法；
13.进一步的，所述移动本体执行所述柔顺控制方法的施加的外力的幅值等于两个机械臂所受的施加的外力的幅值的平方之和开二次方根号。
14.进一步的，当所述移动本体受到的施加的外力的力的方向与行走方向垂直时：
15.当施加的外力达到一预设阈值时所述移动双臂机器人产生减速信号和/或报警信号。
16.进一步的，当所述移动本体受到的施加的外力的力的方向与行走方向不垂直时，所述移动本体基于施加的外力在所述移动本体行走方向的分力执行所述柔顺控制方法。
17.进一步的，所述移动本体所承受的施加的外力基于所述惯性测量装置所测量的力方向、力幅值和加速度以及所述移动本体的驱动轮力矩和驱动轮速度得出；
18.所述机械臂所承受的施加的外力基于所述机械臂的关节伺服驱动器和关节力矩传感器所测得的力矩、速度和位置得出。
19.进一步的，所述移动本体上安装有两个惯性测量装置；两个所述惯性测量装置均分布在所述移动本体的重心线上；
20.进一步的，所述惯性测量装置所测量的应用至所述柔顺控制方法的力方向、力幅值和加速度基于倾角分量和加速度分量得出；
21.所述倾角分量基于以下公式计算：
22.θx＝θx1-θx2；θy＝θy1-θy2；θz＝θz1-θz2；
23.所述加速度分量基于以下公式计算：
24.λx＝λx1-λx2；λy＝λy1-λy2；λz＝λz1-λz2；
25.其中，
26.θx、θy和θz分别表示由所述惯性测量装置计算而得的移动本体在x、y、z方向的倾角；
27.θx1和θx2分别表示由两个所述惯性测量装置在x方向测得的倾角；
28.θy1和θy2分别表示由两个所述惯性测量装置在y方向测得的倾角；
29.θz1和θz2分别表示由两个所述惯性测量装置在z方向测得的倾角；
30.λx、λy和λz分别表示由所述惯性测量装置计算而得的移动本体在x、y、z方向的加速度；
31.λx1和λx2分别表示由两个所述惯性测量装置在x方向测得的加速度值；
32.λy1和λy2分别表示由两个所述惯性测量装置在y方向测得的加速度值；
33.λz1和λz2分别表示由两个所述惯性测量装置在z方向测得的加速度值。
34.进一步的，所述柔顺控制方法基于导纳控制器实现；
35.所述导纳控制器的输入值包括初始期望轨迹指令以及所述施加的外力的幅值；
36.所述导纳控制器采用以下控制算法：
[0037][0038]
其中：ft为所述施加的外力幅值；md为所述移动本体或所述机械臂的惯性特性值；dd为所述移动本体或所述机械臂的阻尼特性值；kd为所述移动本体或所述机械臂的刚度特性值。
附图说明
[0039]
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域
普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
[0040]
图1为本发明具体实施方式中柔顺控制方法的传感器数据采集示意图；
[0041]
图2为本发明具体实施方式中柔顺控制方法的逻辑框图；
[0042]
图3为本发明具体实施方式中一个机械臂受到施加的外力时的柔顺控制方法逻辑框图；
[0043]
图4为本发明具体实施方式中两个机械臂均受到施加的外力时的柔顺控制方法逻辑框图；
[0044]
图5为本发明具体实施方式中移动本体受到施加的外力时的柔顺控制方法逻辑框图；
[0045]
图6为本发明具体实施方式中具有柔顺控制方法的移动双臂机器人的结构示意图；
[0046]
图7为本发明具体实施方式中移动本体的受力计算及控制逻辑示意图；
[0047]
其中：1、移动本体；2、第一机械臂；3、第二机械臂；11、第一陀螺仪；12、第二陀螺仪。
具体实施方式
[0048]
下面结合附图对本发明实施例进行详细描述。
[0049]
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0050]
要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本发明，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。
[0051]
另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践所述方面。
[0052]
在本发明的一个实施例中，提出一种移动双臂机器人柔顺控制方法，如图6所示，机器人包括可移动的移动本体1以及安装在移动本体1上的两个机械臂；
[0053]
机器人上配置有柔顺控制方法；如图3-5所示，柔顺控制方法用于控制机器人的移动本体1和/或机械臂在受到达到一预设阈值的施加的外力时对施加的外力进行柔性避让；
[0054]
移动本体1上设置有至少一组惯性测量装置，惯性测量装置用于测量移动本体1的倾角及加速度。
[0055]
本实施例的移动本体1具有移动能力，设置有伺服电机驱动的滚动轮；两个机械臂由关节电机驱动，均设置有末端执行机构以执行相互协调配合的类人操作功能；各伺服电机的电流、电压以及电机编码器数据，或者力矩传感器数据，均反馈至机器人控制系统，机器人控制系统基于这些数据以及期望轨迹指令进行比对，分析得出移动本体1以及机械臂的移动是否收到施加的外力以及施加的外力的大小、方向等数据。
[0056]
本实施例的柔顺控制方法具体为：机器人正在动作，突然被施加了一个外力，机器人和机械臂将顺着这个外力的方向或这个外力的分力方向进行运动，当外力撤销之后，再回复到之前的运动状态。柔顺控制方法通过分别建立移动本体1和机械臂的正向动力学模型及逆向动力学模型，依靠机械臂伺服驱动器数据(如力矩大小，位置角)以及移动本体1驱动伺服驱动器数据(如驱动力矩大小，旋转速度)形成反馈量输入导纳控制器，同时，上位机器人控制器产生的轨迹数据，分解为移动本体1行走轨迹和机械臂动作轨迹输入导纳控制器，导纳控制器的输出数据至逆动力学模型，最终形成控制量至各伺服驱动器，此时移动本体1及机械臂便具有了经上述柔顺控制方法所控制的避让功能。
[0057]
本实施例的机器人在移动过程中，移动本体1突然受到外力撞击时，则瞬间移动本体1的柔顺控制起作用，移动本体1会朝着撞击的力的方向偏移一小段距离，相当于被撞了之后，自行“躲”了一下，从而主动消散撞击力，起到缓冲的作用，自然也起到了保护的作用，因此安全性提高。
[0058]
需要指出的是，导纳控制仅是本发明的实施例之一，还有其他柔顺控制方法，如阻抗控制方法，当采用相似的实现方式用于轮式移动机器人的柔顺控制实现类似的效果时，也属于本发明的保护范围。
[0059]
本实施例的柔顺控制方法基于导纳控制器实现；导纳控制器的输入值包括初始期望轨迹指令以及施加的外力的幅值；
[0060]
导纳控制器采用以下控制算法：
[0061][0062]
其中：ft为施加的外力的幅值；md为移动本体1或机械臂的惯性特性值；dd为移动本体1或机械臂的阻尼特性值；kd为移动本体1或机械臂的刚度特性值。
[0063]
如图2所示，机械臂或移动本体1的控制包括轨迹发生器、导纳控制器、机械臂控制器以及移动本体控制器。
[0064]
图中，
[0065]
xr1、xr2：分别是移动本体1和机械臂的初始期望轨迹指令；
[0066]
xd1、xd2：导纳控制器的输出；
[0067]
f1、f2：分别是移动本体控制器和机械臂控制器输出的力；
[0068]
ft1、ft2：分别是移动本体1和机械臂受到的外力；
[0069]
x1、x2：分别是移动本体1产生的反馈量和机械臂产生的反馈量；
[0070]
机器人控制器中的轨迹发生器模块产生轨迹指令后，输入到轨迹分解单元，轨迹分解单元的作用是把整体轨迹指令分解为移动本体1的运动轨迹xr1和机械臂的运动轨迹xr2，并分别输入到后续环节。
[0071]
xr1和xr2输入至导纳控制器，导纳控制器输出xd1，移动本体控制器和移动本体1
组成了闭环的速度/力矩控制系统，机械臂控制器和机械臂组成了闭环的速度/力矩控制系统。当外力ft1作用于移动本体1本体上时，外力ft1会叠加在导纳控制器的输入上，相当于导纳控制器的输入增加了一个偏移量，导纳控制器的输出xd1将跟随(xr1+xt1)。当外力ft1撤销时，则导纳控制器的输出xd1仍然将跟随输入xr1；本实施例的机械臂也采用上述同样的方法进行分析。
[0072]
在一个实施例中，本发明的两个机械臂和移动本体1的柔顺控制方法相互结合，如图3所示，当两个机械臂之一受到施加的外力时：当施加的外力达到一预设阈值时两个机械臂以及移动本体1均分别执行柔顺控制方法；否则仅受到施加的外力的机械臂执行柔顺控制方法。
[0073]
如图3所示，当双臂机器人中的第一机械臂2和第二机械臂3之一受到外力ft时，如果外力的幅度在某设置的门限范围之内，如f
threshold
范围之内，则同时仅对第一机械臂2和第二机械臂3执行基于外力ft的柔顺控制算法，直到ft消失。这种柔顺控制方法，有助于在某一个机械臂受到外力时，另一个未受力的机械臂也跟随受力的机械臂同方向运动，以避免两机械臂之间互相碰撞的可能性，从而提高安全性。当外力ft高于f
threshold
时，说明该外力幅值已经大于某一幅值，如某机械臂突然与外物碰撞，则不仅两个机械臂要执行柔顺控制，同时移动本体1也执行基于ft或者基于ftx的柔顺控制，以从机器人整体上产生柔顺控制效果，进一步降低风险，提升安全性；此处的ftx是ft在x方向(移动本体1移动方向或反方向)的分量；具体以ft还是ftx为基准进行移动本体1本体的柔顺控制，可通过设定的方式，或者依据机器人具体应用进行设定。
[0074]
在一个实施例中，当两个机械臂均受到施加的外力时：如图4所示，当两个机械臂之一受到的施加的外力达到一预设阈值时，两个机械臂以及移动本体1均分别执行柔顺控制方法；否则两个机械臂分别执行柔顺控制方法；
[0075]
其中：移动本体1执行柔顺控制方法的施加的外力的幅值等于两个机械臂所受的施加的外力的幅值的平方之和开二次方根号。
[0076]
本实施例中两个机械臂同时受到外力作用，两机械臂同时执行柔性控制；当两个机械臂之一个作用力高于设定门限值时，则移动本体1也执行柔顺控制，控制流程框图如图4所示，图中：fta、ftb分别表示第一机械臂2和第二机械臂3受到的外力作用；
[0077]
当fta或者ftb其中一个值高于设定门限值f
threshold
时，则移动本体1也执行柔顺控制，基于的值进行柔顺控制。
[0078]
本实施例将移动本体1的导纳控制与机械臂的导纳控制相结合，使得机器人的机械臂以及移动本体1形成一个整体，使整体机器人在遇到施加的外力时具有更好的柔顺性。
[0079]
在一个实施例中，移动本体1由两对驱动轮进行驱动，移动本体1若对施加的外力进行转向避让则会延迟时间过长使柔顺控制方法失效，因此当移动本体1受到的施加的外力的力的方向与行走方向垂直时：
[0080]
当施加的外力达到一预设阈值时移动双臂机器人产生减速信号和/或报警信号。
[0081]
当移动本体1受到的施加的外力的方向与行走方向不垂直时，基于施加的外力在移动本体1行走方向的分力执行柔顺控制方法。
[0082]
如图5所示，当移动本体1本体受到外力ft时，系统判断外力方向与当前行走方向的关系，如果垂直于当前行走方向的外力fty，幅度大于设定值ftysh，则表明机器人侧向受
到较大的力的冲击，机器人自动减速并报警，直到停机等待检查。反之，如果系统判断，在机器人行走轴线方向(包含前行或者后退方向)有外力存在，则据此计算出外力在该方向上的分外力ftx，并依据此执行移动本体1柔顺控制，直至此外力消失，继续执行之前的移动。该控制策略有助于在移动本体1受到与行走方向相同或者相反的外力冲击时，机器人通过柔顺控制方法能够随外力进行一定程度的移动，从而起到缓冲作用，降低可能造成的对移动本体1或者外力施加者造成的损害。
[0083]
在一个实施例中，移动本体1所承受的施加的外力基于惯性测量装置所测量的力方向、力幅值和加速度以及移动本体1的驱动轮力矩和驱动轮速度得出；
[0084]
机械臂所承受的施加的外力基于机械臂的关节伺服驱动器和关节力矩传感器所测得的力矩、速度和位置得出。
[0085]
在一个实施例中，如图6所示，移动本体1上安装有两个惯性测量装置；两个惯性测量装置均分布在移动本体1的重心线上；本实施例的惯性测量装置为陀螺仪传感器，第一陀螺仪11安装于移动本体1上部，靠近头颈部位置；第二陀螺仪12安装于机器人底盘位置。
[0086]
本实施例建立了基于陀螺仪传感器2安装中心位置的xyz坐标系。设水平平面为xy平面，机器人行走正前方为x轴，垂直于行走方向为y轴，z轴垂直于xy平面向上。
[0087]
当移动本体1受到外力作用时，2个陀螺仪传感器均会有数据输出，如倾角，加速度等数据，机器人控制系统据此计算出作用在移动本体1上的作用力，分为3个方向的分量，如在x方向的ftx，y方向的fty，z方向的ftz。具体处理原理如图6所示。
[0088]
在本实施例中，惯性测量装置所测量的应用至柔顺控制方法的力方向、力幅值和加速度基于倾角分量和加速度分量得出；
[0089]
倾角分量基于以下公式计算：
[0090]
θx＝θx1-θx2；θy＝θy1-θy2；θz＝θz1-θz2；
[0091]
加速度分量基于以下公式计算：
[0092]
λx＝λx1-λx2；λy＝λy1-λy2；λz＝λz1-λz2；
[0093]
其中，
[0094]
θx、θy和θz分别表示由惯性测量装置计算而得的移动本体在x、y、z方向的倾角；
[0095]
θx1和θx2分别表示由两个惯性测量装置在x方向测得的倾角；
[0096]
θy1和θy2分别表示由两个惯性测量装置在y方向测得的倾角；
[0097]
θz1和θz2分别表示由两个惯性测量装置在z方向测得的倾角；
[0098]
λx、λy和λz分别表示由惯性测量装置计算而得的移动本体在x、y、z方向的加速度；
[0099]
λx1和λx2分别表示由两个惯性测量装置在x方向测得的加速度值；
[0100]
λy1和λy2分别表示由两个惯性测量装置在y方向测得的加速度值；
[0101]
λz1和λz2分别表示由两个惯性测量装置在z方向测得的加速度值。
[0102]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。