一种基于融合人体运动学约束的协调图双臂协作控制方法与流程

1.本发明属于智能机器人领域，尤其涉及人形机器人的双臂协作控制方法。


背景技术：

2.随着科技的不断发展，机器人应用至各行各业，所承担的任务越来越复杂。所以机器人技术的一种当前趋势是向具有拟人运动学的双臂机器人方向发展。仿人型双臂机器人由于采用仿人型设计，能够模仿人类动作，相比传统的单臂机器人，具有更大的作业范围，更高的运动灵活性和更大的负载能力，可以执行更加多样化的任务。
3.尽管现在有许多很成熟的机械臂控制算法和很多的工业双臂机器人，比如abb公司的yumi机器人，willow garage公司开发的pr2机器人和rethinkrobotics公司推出的baxter机器人，但是同时操作双臂依旧很困难。在双臂共同的工作区域内找到一条无碰撞的操作路径称为双臂协作问题，而双臂协作技术是仿人型双臂机器人执行动作的关键。传统的双臂控制多采用双臂轮循操作或rrt
‑
connect轨迹规划方法生成轨迹，再添加协作算法达到双臂协作控制。但这两种协作方式均有操作效率较低，系统运算量大，和操作动作不类人等缺点。所以为解决这一问题需要从协作轨迹本身入手，令协作轨迹类人化，不仅能够使得操作类人，同时可以减少轨迹碰撞和系统运算量。


技术实现要素：

4.本发明所要解决的问题，提供一种双臂协作控制方法，同时从动作和任务层面进行协作。在双臂动作层面，要求双臂同时工作时发生碰撞的概率最小，并且动作要类人化。在完整的任务操作层面，要求完成完整双臂协作任务的操作时间最短，且总碰撞最少。
5.一种基于融合人体运动学约束的协调图双臂协作控制方法，包含以下步骤：
6.步骤1.1、操作者穿戴自制人体姿态采集服，通过遥操作方式控制人形双臂机器人完成双臂协作任务，进行遥操作动作示教。
7.步骤1.2、对步骤1.1所生成的双臂协作的轨迹信息进行滤波处理，而后对轨迹信息进行采样，获取示教轨迹，存入示教轨迹池。
8.步骤1.3、完成示教轨迹的生成之后，根据步骤1.2存入示教轨迹池的轨迹构建双臂协调图，并依据示教动作顺序为横纵坐标建立双臂协调图矩阵。
9.步骤1.4、根据步骤1.3生成的双臂协调图矩阵结果生成双臂协作对，并存入协作池。
10.步骤1.5、机器人从步骤1.4中协作池提取双臂协作图信息，生成对应控制命令，进行双臂协调动作，完成协作任务。
11.步骤1.2对步骤1.1所生成的轨迹信息进行滤波处理并采样，为后续进行协调图生成等操作提供动作轨迹信息基础。双臂协作控制中采用各关节角度作为控制信号，操作者穿戴人体姿态采集服进行动作示教，存储其动作映射到机器人本体上的各关节角度为示教轨迹信息。但是由于遥操作示教过程中操作者动作易发生抖动与重复，故而需对示教轨迹
进行滤波。同时为后续建立协调图，需对完整的示教动作轨迹进行采样，将采样点信息作为后续生成协调图的轨迹点信息。
12.步骤1.3中要进行协调图的生成，并且以示教动作的顺序为横纵坐标建立完整的双臂协调图矩阵。其中协调图的x,y轴分别为左臂和右臂的时间轨迹参数,而不是一般进行轨迹规划采用的空间轨迹参数.并且在协调图中将手臂之间发生碰撞的机器人关节配置为图中”障碍物”，并标记黑色。然后循环所有动作轨迹生成协调图，并依据示教动作的顺序建立协调图矩阵。
13.步骤1.4依据协调图矩阵结果以总碰撞最少为目标，生成双臂协作对，不以操作者动作示教时的动作顺序进行操作，重新对左右臂操作进行配对，生成双臂协作对，并将双臂协作对所对应的协调图存储协作池。
14.最后，步骤1.5从双臂协作池中读取双臂协作对的协调图信息，根据各协作对的碰撞信息协作对操作顺序进行重新排序，无碰撞协作对在前，有碰撞协作对在后进行操作，并且根据协调图信息生成对应的操作控制命令控制机器人完成协作操作。
15.本发明同时从“动作”层面和“操作任务”层面进行双臂协作控制，以追求以最高效率并且无碰撞完成完整协作任务的目标。在动作协作层面，我们通过遥操作示教技术获取机器人协作轨迹，操作者穿戴自制人体姿态采集服遥操作机器人示教完成协作动作，通过四元数姿态轨迹映射方法将人类动作映射至机器人端并优化存储，作为后续协作的初始轨迹。而后根据轨迹碰撞情况生成协调图，协调图以左右臂操作时间为横纵坐标，设置图中机器人会发生碰撞的机器人配置为黑色，无碰撞配置为白色，形成一个完整的协调图。在协调图内采用rrt
‑
connect算法结合开源动作规划库(ompl)中的轨迹优化算法的方式进行轨迹规划，生成协作律，完成动作层面的双臂协作。设计了协作对生成器，根据输入的轨迹特点以及协调图内的碰撞信息对操作对进行重组，重新分配左右臂操作的对应关系，不单纯以输入的左右臂对应关系进行协作。在机器人执行协作过程中于操作顺序层面再进行协作，对输入的协作对根据碰撞情况先进行排序，无碰撞协作对先于有碰撞协作对操作，同时由于在进行无碰撞协作对操作时无协作计算，采用多线程方式进行有碰撞协作对的协作律计算，在完成无碰撞协作对的操作之后，有碰撞协作对根据前面的协作律计算结果进行协作，完成完整的协作操作，使得操作时间最短，操作效率最大化。
附图说明
16.图1双臂协作方法示意图
17.图2双臂协作系统流程图
18.图3惯性式可穿戴人体姿态采集服
19.图4为采用带有人体运动学约束的动作协调流程图
20.图5协调图
21.图6协调图示意图
22.图7协作对协调图对比图
23.图8机器人从协作池获取协作命令并执行操作流程图
具体实施方式
24.下面结合附图对本发明方法做进一步说明，如图1所示为本发明方法示意图，采用遥操作示教思想，将人体运动学约束融入协调图进行人形机器人双臂协作控股。方法工作流程：参照图2，本发明是一种基于融合人体运动学约束的协调图双臂协作控制方法，使得人形双臂机器人能够无碰撞且高效的完成双臂协作任务。该方法的整体流程主要包括：获取示教动作部分，操作者穿着人体姿态采集服进行遥操作动作示教，对示教动作的轨迹进行处理，滤波掉重复和抖动的数据，并以抓取位置为分割进行等量采样，而后将处理后的示教动作轨迹存入示教轨迹池。协作运算部分，从示教轨迹池内提取示教轨迹，根据示教轨迹轮循构建双臂协调图，而后根据协调图结果生成双臂协作对，并将协作对结果存入协作池。最后机器人从协作池提取协作对信息，并进行协调动作。
25.1.1操作者穿戴自制人体姿态采集服，通过遥操作方式控制人形双臂机器人完成双臂协作任务，进行遥操作动作示教。
26.本发明双臂协作方法的创新之一是将人类动作融入协作过程，通过人类自己本身双臂的运动学约束简化机器人协作过程中的运算难度。人体的各关节绕其关节运动轴能够表现出屈伸、收展、旋转、和环转等不同运动形式，且每个关节都具有特定的运动幅度。一般来说每个关节的运动幅度都是不同的，并且即使是同一关节处于不同姿态时其运动幅度也不尽相同，故而难以模拟人体本身的运动学约束，所以本发明采用遥操作示教方式将人体运动学约束融入双臂协作过程。在过程中采用人体动作采集技术对操作者的动作进行捕捉，操作者穿戴如图2所示自主研发的惯性式可穿戴人体姿态采集服进行操作示教。采用这样的人体捕捉方法目的是能够直接对人体进行接触测量，在光线、角度和障碍物等复杂外部环境下依然可以稳定可靠地获取操作者的动作姿态数据，在计算要求上远低于分布式视觉采集系统，也不受机械式采集系统结构复杂，移动不便和精度较低的局限。
27.1.2对步骤1.1所生成的轨迹信息进行滤波处理，而后对轨迹进行采样，获取示教轨迹，存入示教轨迹池。
28.由于操作者在穿戴人体姿态采集服示教过程中，容易出现动作抖动和重复的现象，需要对获取的示教动作信息进行滤波处理，以剔除其中的抖动和重复性动作。然后以操作物品的时刻为分界，分为“抓取”和“放置”两部分轨迹，分别对两部分轨迹进行采样，将采样点以采样时刻进行排序，生成协作轨迹，并存入协作轨迹池，以供后续操作使用。
29.1.3完成示教轨迹的生成之后，根据步骤1.2存入示教轨迹池的轨迹构建双臂协调图，并依据示教动作顺序为横纵坐标建立双臂协调图矩阵。
30.协作对运算过程如图4所示，本发明设计了协作对生成器，协作对生成器包括生成协调图矩阵以及根据协调图矩阵重构双臂操作顺序两部分。首先从初始轨迹池读取初始的轨迹值,并从每段轨迹中获取轨迹插值点。将插值点对应的关节动作数据输入双臂机器人碰撞检测系统，获取该时刻的机器人碰撞情况。由于在进行示教时采用左右臂分别记录轨迹,故而无法从一段轨迹中直接提取双臂动作进行碰撞检测,需将两段轨迹进行左右臂轨迹合并,将合并后的双臂动作数据输入碰撞检测系统进行碰撞检测,并输出碰撞结果。
31.本发明采用的是固定轨迹协作的思想进行双臂协作控制。此种方法不对轨迹本身进行修改,而是对每个轨迹进行一些限制和参数化以避免碰撞.此协作算法的生成是在双臂的协作空间内,以所生成的协调图为基础完成的。如图5所示为协作池内存储的一个具有
碰撞的协调图,需要对其进行协作,找到一条不经过碰撞区域的轨迹.根据协调图的特性和我们进行时域控制的需要,设置协调图的x,y轴分别为左臂和右臂的时间轨迹参数,而不是一般进行轨迹规划采用的空间轨迹参数.并且在协调图中将手臂之间发生碰撞的机器人关节配置设置为图中”障碍物”并标记黑色.如图6所示,为协调图内有碰撞部分的示意图.存在多种方式通过此段区域.可采用双臂轮循操作通过此区域的方式通过此区域,如图中红色轨迹和黑色轨迹所示,此种方式虽然能够保证手臂之间没有碰撞发生,但是时间消耗最大,未实现高效协作目标.而如果直接采用最短路径,则会多次经过双臂碰撞区域.虽消耗时间最短,却会发生碰撞.所以可以采用如图所示方法,寻一条无碰撞且路径最短的路径通过此区域,然后将此轨迹对应的双臂操作动作存入协作池。以上所述只是协作图中带有碰撞区域的示意图,而要在实际协调图中解决协作问题,即为在此协调图内寻找一条无碰撞轨迹联通双臂的起点和终点，本发明中采用rrt
‑
connect与ompl中实现的路径优化结合使用。
32.1.4根据步骤1.3生成的双臂协调图矩阵结果生成双臂协作对，并存入协作池。
33.步骤1.3生成协调图后只在动作层面进行了协调，而本发明目标是用最短时间完成双臂协作任务。这不仅需要双臂之间的动作协调，还需要对整体的操作顺序进行规划。我们设计的协作对生成器中根据协调图矩阵重构双臂操作顺序部分的功能以规划后协作图内总碰撞区域最小为目标，根据输入的轨迹特点进行操作顺序的重构。如图所示以一种更加直观的方式解释我们为何在双臂协作系统中添加cpg。在图7右图中所示，如果以输入轨迹顺序进行操作，需要左臂和右臂同时对左
‑
1和右
‑
1号物块进行操作，当物块摆放较为松散的情况下，此种操作可行。但是当物块摆放很紧凑的情况下，对这两个物块同时进行操作会发生很多碰撞，在双臂协作系统中会进行多次协作操作来规避碰撞，可以看出在协调图中碰撞区域也很大。而在cpg中生成协作对之后，会根据各操作对的协调图结果重新生成协作对，会将左
‑
1物块和右
‑
5物块同时操作，碰撞次数减少，可以从图7作图中看出协调图碰撞区域明显小于右图。在一些协作对中甚至不发生碰撞，从而减少协作过程，提升执行效率。
34.1.5机器人从步骤1.4中协作池提取双臂协作图信息，生成对应控制命令，进行双臂协调动作，完成协作任务。
35.机器人协作模块作为计划部分到操作部分的连接,同时在操作中进行基于协调图结果的协作,其工作过程如图8所示.使用cinterval表示在协调图内进行碰撞搜索的路径长度,由于双臂操作的特殊性,需要保证在一段路径内操作无碰撞发生才可进行动作,而不是以单独动作为检测标准,否则极易发生碰撞.
36.首先从协作池中读取生成的协调图.并根据协调图是否具有碰撞进行动作对排序,无碰撞动作对先于有碰撞动作对执行操作。为了能够将时间利用率最大化,采用多线程工作方式同时兼顾嵌入式系统计算能力，在机器人进行计算量很小的无碰撞协作对的操作过程中同步进行协作算法的解算。协作算法的解算方法已经于上文说明。
37.在将动作对排序后,先进行无碰撞动作对操作,并在进行此操作过程中创建分支进程对有碰撞动作对进行协作算法计算,并将结算结果存储于协作算法栈中。在完成机械臂无碰撞动作对的操作后,根据子进程所计算的协作算法对有碰撞动作对进行操作。