CN107584474B - 一种基于几何投影的球面关节双臂机器人协调运动方法 - Google Patents

Abstract

本发明公开了一种基于几何投影的球面关节双臂机器人协调运动方法，属于机器人技术领域，通过将复杂的、高度冗余的球面关节双臂机器人空间模型在空间直角坐标系中分别从y轴负向、x轴负向及z轴正向对三维模型进行投影，获得前视图、左视图和俯视图模型，运用平面几何方法综合斜率公式、插值法等对平面模型进行双臂协调；通过关节坐标系计算连杆的实时模型，便于下一工作过程继续运用几何投影计算，直至完成工作目标。本发明能够在特定的工况下实现对双臂机器人简单、快速的协调规划，同时为多解或无数解问题提供可靠的解决方案，这在一定程度上提高了双臂机器人的工作效率，能够提高工作的准确性，充分发挥双臂机器人的工作特点。

Description

一种基于几何投影的球面关节双臂机器人协调运动方法

技术领域

本发明涉及一种机器人协调运动方法，特别是涉及一种基于几何投影的球面关节双臂机器人协调运动方法，属于机器人技术领域。

背景技术

在航天航空、复杂加工、精密装配等特殊工况下，双臂或多臂机器人凭借工作空间大、承重能力强、冗余可操作、可协调作业等优点，可实现单臂机器人无法完成的作业而越来越受重视，球面关节具有三自由度驱动，可通过配合实现空间任意位姿，故球面多关节双臂机器人具有更加柔性、灵巧和协作化的特点，可满足不断涌现的新需求。

运动分析与规划是机器人动作控制的基础，高效实用的规划算法是实现双臂机器人协调运动控制的关键，目前，双臂运动求解方法有代数和几何解法，基于D-H矩阵变换的代数法是空间双臂协调运动分析的主要方法，然而通常是针对固定的、自由度不多的机器人构形进行计算求解，对于球面多关节机器人这种高度冗余的空间机器人，往往会得到多解或无数解，计算过程复杂且不直观，并不具有实用性，近年来神经网络、模糊控制、滑模控制等算法被大量引入到机器人协调控制中，但通常需要根据不同结果建立算法模型，且计算的精确性和时效性往往制约于计算机的性能，这使得算法研究更多是在理论研究阶段，缺少实用性。

国内外学者关于通过合理选择工作平面以解决机器人运动学和逆运动学问题做了一些工作，WenfuXu通过构造参考平面，然后确定满足一定条件的两个臂角，避免了算法的奇异性，然而构造平面工作量仍是很大，无法实现简单、快速解决实际问题，在中国专利，申请号为201310296056.1公开一种用于串联形式机器人逆运动学的通用求解方法是通过将工作构形分解成若干个构形平面以完成求解过程，但在一个运动过程中选择多个投影面对于多关节机器人运动求解显得麻烦，且该方法以单自由度基本模块为基础，对于多自由度球面关节不太适用。

目前关于通过构造平面解决机器人运动学问题的方法在一定程度上能够提高求解速度和求解精度，但通常是在一个运动过程中通过构造多个平面求解，平面不固定，求解过程也相对繁琐，针对多自由度关节机器人运动协调问题，目前尚无合适的几何求解方法。

发明内容

本发明的主要目的是为了提供一种基于几何投影的球面关节双臂机器人协调运动方法，以解决当前针对球面关节双臂机器人协调运动缺乏简单、实用、高效解决方法的问题。

本发明的目的可以通过采用如下技术方案达到：

一种基于几何投影的球面关节双臂机器人协调运动方法，包括如下步骤：

步骤1：将球面关节机器人简化为空间运动链模型；

步骤2：布置球面关节双臂机器人空间位置并建立空间直角坐标系；

步骤3：建立各关节空间直角坐标系；

步骤4：划分协调运动过程，减少运动过程分段；

步骤5：选择投影面，按照顺序，针对每一运动过程选择投影面，并根据几何投影制定协调运动方案；

步骤6：在几何投影协调运动的过程中，根据工作任务预先设定主动臂运动规划，根据工作任务结合主动臂运动规划确定从动臂运动规划；

步骤7：完成该协调运动过程后，重新计算参数模型，获得投影模型，并根据下一协调运动过程特点选择新的投影面，制定协调运动方案；

步骤8：重复所述步骤6和所述步骤7，直至完成全部协调运动过程。

进一步的，所述步骤1中，各关节简化为球面副，作为机器人的驱动，球面关节具有三自由度驱动，各关节通过配合实现空间任意位姿，机械臂简化为直线连杆，作为各球面副的连接，两臂均为开链结构。

进一步的，所述步骤1中，两臂由n个三自由度球面关节及n个直线连杆组成，n为大于0的任意整数，各关节简化为球面副，作为机器人的驱动。

进一步的，所述步骤2中，球面关节双臂机器人空间位置的条件为：取两臂与固定端连接关节连线中点为坐标原点，按右手法则建立空间直角坐标系，使两臂的工作空间分别位于相对的象限空间内，避免双臂机器人投影发生干涉，通过该坐标系分别获得前视图、左视图和俯视图的几何模型。

进一步的，所述步骤2对所述步骤1中的空间运动链模型建立空间直角坐标系，并分别从y轴负向、x轴负向及z轴正向对空间运动链模型的三维模型进行投影，获得前视图、左视图和俯视图，在前视图、左视图或俯视图中，当两臂与固定端连接关节的连线与水平坐标轴之间出现夹角δ时，0°＜δ≤90°，通过将投影几何图形绕原点顺时针或逆时针旋转与δ相同的角度，使之重合。

进一步的，所述步骤3中，建立各关节空间直角坐标系，以各关节中心为坐标原点按右手准则建立直角坐标系，令各个关节直角坐标系坐标轴的方向与空间直角坐标系坐标轴的方向一致。

进一步的，所述步骤3中，各关节空间直角坐标系坐标轴的方向不随关节旋转运动的过程变化，通过三自由度球面关节的配合，驱动连杆绕x轴、y轴或z轴做旋转运动，每个连杆的状态用一组转角序列(ω，ξ，κ)表示，ω、ξ、κ分别用杆件在yz、xz、xy平面内的投影与y、z、x轴正方向的夹角来表示，按右手法则来确定旋转角的正向，杆件末端点在各自空间坐标系的坐标为(x,y,z)，每个关节仅保留一个自由度工作，杆件末端点与转角序列间的关系有：

其中：l为杆件末端点的z轴坐标。

进一步的，所述步骤4中，划分协调运动过程，在一个平面内进行运动规划时，该平面内的连杆投影长度不变，且其他平面的投影可实时计算，并在一个工作平面内完成指定运动，再计算连杆实时参数，以得到在下一个工作平面内的投影，继续在下一个工作平面内完成指定运动，并对运动过程进行编号，并按顺序进行完成协调运动操作。

进一步的，所述步骤5中，投影面上的平面几何模型是空间坐标系下三维模型的投影，各连杆在平面内的实时数学模型通过在各关节坐标系下的连杆数学模型计算获得。

进一步的，所述步骤7中，所述投影模型的双臂各关节及执行器末端在投影面上的坐标计算公式如下：

或

或

或

其中：α、β、γ、μ、λ、η为平面协调运动规划时控制两臂的运动的驱动变量；

θ为执行器末端连线与y轴的夹角；

a、e表示平面投影中，大臂关节与原点距离；

b、c、d、f、g、h分别表示各连杆在平面投影的长度。

本发明的有益技术效果：按照本发明的基于几何投影的球面关节双臂机器人协调运动方法，本发明提供的基于几何投影的球面关节双臂机器人协调运动方法，解决了当前针对球面关节双臂机器人协调运动缺乏简单、实用、高效解决方法的问题，通过将复杂的、高度冗余的球面关节双臂机器人空间模型在空间直角坐标系中分别从y负向、x负向及z正向对三维模型进行投影，获得前视图、左视图和俯视图几何模型，运用平面几何方法综合斜率公式，插值法等对平面模型进行双臂协调，通过关节坐标系计算连杆的实时模型，便于下一工作过程继续运用几何投影计算，直至完成工作目标，能够在特定的工况下实现对双臂机器人简单、快速的协调规划，同时为多解或无数解问题提供可靠的解决方案，提高了双臂机器人的工作效率，能够提高工作的准确性，充分发挥双臂机器人的工作特点。

附图说明

图1为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的工作流程图；

图2为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的球面关节双臂机器人空间运动链模型轴侧视图；

图3为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的双臂机器人投影前视图；

图4为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的双臂机器人投影左视图；

图5为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的双臂机器人投影俯视图；

图6为图5按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的双臂机器人投影俯视图逆时针旋转δ后；

图7为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的空间双臂机器人关节坐标系；

图8为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的平面投影通用模型；

图9为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的双臂机器人平面协调运动示意图；

图10为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的双臂机器人轴孔装配协调运动仿真过程俯视图；

图11为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的双臂机器人轴孔装配协调运动仿真过程左视图；

图12为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的双臂机器人轴孔装配协调运动仿真过程前视图；

图13为按照本发明的基于几何投影的球面关节双臂机器人协调运动方法的一优选实施例的双臂机器人轴孔装配协调运动仿真过程轴测图。

具体实施方式

为使本领域技术人员更加清楚和明确本发明的技术方案，下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

针对球面关节双臂机器人协调运动问题，本发明提出了基于几何投影的球面关节机器人协调运动方法，双臂机器人协调运动即两执行器位置与位姿的相互约束，靠各个关节协调运动来保证，但由于机构高度冗余，通常会出现多解或无数解情况，本方法即通过工作过程的合理划分结合投影法的运用为其提供简单、高效的解决方案，本方法为球面关节机器人协调运动提供可靠的解决方案，提高工作效率并充分发挥双臂机器人的特点。

如图1所示，本实施例提供的一种基于几何投影的球面关节双臂机器人协调运动方法，包括如下步骤：

步骤1：将球面关节机器人简化为空间运动链模型，各关节简化为球面副，作为机器人的驱动，球面关节具有三自由度驱动，各关节通过配合实现空间任意位姿，机械臂简化为直线连杆，作为各球面副的连接，两臂均为开链结构，两臂由n个三自由度球面关节及n个直线连杆组成，n为大于0的任意整数，各关节简化为球面副，作为机器人的驱动；

步骤2：布置球面关节双臂机器人空间位置并建立空间直角坐标系，球面关节双臂机器人空间位置的条件为：取两臂与固定端连接关节连线中点为坐标原点，按右手法则建立空间直角坐标系，使两臂的工作空间分别位于相对的象限空间内，避免双臂机器人投影发生干涉，通过该坐标系分别获得前视图、左视图和俯视图的几何模型；对所述步骤1中的空间运动链模型建立空间直角坐标系，并分别从y轴负向、x轴负向及z轴正向对空间运动链模型的三维模型进行投影，获得前视图、左视图和俯视图，在前视图、左视图或俯视图中，当两臂与固定端连接关节的连线与水平坐标轴之间出现夹角δ时，0°＜δ≤90°，通过将投影几何图形绕原点顺时针或逆时针旋转与δ相同的角度，使之重合；

步骤3：建立各关节空间直角坐标系，建立各关节空间直角坐标系，以各关节中心为坐标原点按右手准则建立直角坐标系，令各个关节直角坐标系坐标轴的方向与空间直角坐标系坐标轴的方向一致；各关节空间直角坐标系坐标轴的方向不随关节旋转运动的过程变化，通过三自由度球面关节的配合，驱动连杆绕x轴、y轴或z轴做旋转运动，每个连杆的状态用一组转角序列(ω，ξ，κ)表示，ω、ξ、κ分别用杆件在yz、xz、xy平面内的投影与y、z、x轴正方向的夹角来表示，按右手法则来确定旋转角的正向，杆件末端点在各自空间坐标系的坐标为(x,y,z)，每个关节仅保留一个自由度工作，杆件末端点与转角序列间的关系有：

其中：l为杆件末端点的z轴坐标；

步骤4：划分协调运动过程，减少运动过程分段，划分协调运动过程，在一个平面内进行运动规划时，该平面内的连杆投影长度不变，且其他平面的投影可实时计算，并在一个工作平面内完成指定运动，再计算连杆实时参数，以得到在下一个工作平面内的投影，继续在下一个工作平面内完成指定运动，并对运动过程进行编号，并按顺序进行完成协调运动操作；

步骤5：选择投影面，按照顺序，针对每一运动过程选择投影面，并根据几何投影制定协调运动方案，投影面上的平面几何模型是空间坐标系下三维模型的投影，各连杆在平面内的实时数学模型通过在各关节坐标系下的连杆数学模型计算获得；

步骤6：在几何投影协调运动的过程中，根据工作任务预先设定主动臂运动规划，根据工作任务结合主动臂运动规划确定从动臂运动规划；

步骤7：完成该协调运动过程后，重新计算参数模型，获得投影模型，并根据下一协调运动过程特点选择新的投影面，制定协调运动方案；

所述投影模型的双臂各关节及执行器末端在投影面上的坐标计算公式如下：

或

或

或

其中：α、β、γ、μ、λ、η为平面协调运动规划时控制两臂的运动的驱动变量；

θ为执行器末端连线与y轴的夹角；

a、e表示平面投影中，大臂关节与原点距离；

b、c、d、f、g、h分别表示各连杆在平面投影的长度。

步骤8：重复所述步骤6和所述步骤7，直至完成全部协调运动过程。

采用的球面关节双臂机器人通用运动链模型如图2所示，双臂可由n个三自由度球面关节及n个连杆组成，各关节简化为球面副，作为机器人的驱动，球面关节具有三自由度驱动，各关节可通过配合实现空间任意位姿。机械臂简化为直线连杆，作为各球面副的连接，两臂均为开链结构。通常情况下各单臂为不大于3关节3连杆SSS型开链机构，此时各单臂自由度为9，在实现确定位置和位姿情况下，已是自由度冗余状态，能够满足双臂协调运动要求。若连杆过多，会造成结构的不稳定以及结构过于冗余等问题，为不失一般性，同时具有代表性，以下所述模型为双臂分别为空间三关节三连杆机构，可作为球面关节机器人的典型结构，现对该模型对本发明公共实施方式作进一步详细描述，所述方法可用于该模型但不仅限于该模型。

如图2所示，即步骤1中所述的空间运动链模型，为方便描述，从与固定端连接的关节开始A、B和C，E、F和G分别代表右臂和左臂的大臂关节、小臂关节和执行器关节，AB、BC和CD，EF、FG和GH分别代表右臂和左臂大臂、小臂和执行器。

如图2所示，对照步骤2，取两臂与固定端连接关节连线中点为坐标原点，按右手法则建立空间直角坐标系，并保证在工作过程中两臂的工作空间分别位于相对的象限空间内，以此建立了空间直角坐标系。

如图3、图4、图5、图6所示，对步骤1中空间运动链模型分别从y轴负向，x轴负向及z轴正向对三维模型进行投影，获得前视图、左视图和俯视图，如出现如附图5所示的情况，可投影几何图形绕原点旋转一个角度，这里设为δ，δ为锐角，以便于计算，若逆时针旋转，则各臂投影与x轴夹角比原夹角值大δ，执行器末端连线与y轴夹角比原夹角值小δ，如图6所示，若顺时针旋转则反之，杆件间夹角不变，由于在步骤2建立坐标系的过程中使两臂的工作空间分别位于相对的象限空间内，故如图3、图4和图5所示的双臂机器人几何投影不会发生干涉，保证了几何投影法能够实现的同时也避免了双臂发生碰撞。

如图7所示，即步骤3中所建立的关节坐标系，以各关节中心为坐标原点按右手准则建立直角坐标系，且各个直角坐标系坐标轴的方向与空间直角坐标系一致，在机构运动过程中，各关节坐标系坐标轴的方向不随关节旋转运动的过程变化，可通过三自由度球面关节的配合，驱动连杆绕x，y，z轴做旋转运动。

如图7所示，每个连杆的状态可以用一组转角序列(ω，ξ，κ)表示，为方便描述和计算，ω、ξ、κ可分别用杆件在yz、xz、xy平面内的投影与y、z、x轴正方向的夹角来表示，按右手法则来确定旋转角的正向，设杆件末端点在各自空间坐标系的坐标为(x,y,z)，存在两种特殊情况：当长为l的杆件与坐标轴重合时，如与z轴重合且方向为z轴正向，末端点坐标(0,0,l)，此时球面关节仅作以关节坐标系z轴为轴线的转动；当杆件在某一平面上时，如在yz平面上，有末端点坐标(0,y,z)，且有：

式(6)表示当杆件在某一平面上时，如在yz平面上，杆件末端点坐标与转角序列间的关系。

此时球面关节仅做以关节坐标系x轴为轴线的转动，当出现以上两种情况时，实际上是球关节问题的简化，使每个关节仅保留一个自由度工作，为不失一般性，以下方法主要针对一般情况做介绍，但其方法可用于上述两种特殊情况，一般情况下，有：

式(1)表示在一般情况下，末端点坐标与转角序列间的关系。

如图7所示，从连杆所在的坐标系来看，以仅绕z轴旋转为例，z坐标保持不变，κ发生变化且已知，故式(1)中有4个未知数x、y、ω、ξ，可以通过方程解出，得到连杆投影位置，如仅绕z轴转动，则连杆在xy平面的投影长度保持不变；以此类推。这是投影法运用的重要基础。

如图1所示，按步骤4，划分了运动过程，要求在能够完成预期工作任务的情况下，应尽量较少的运动分段，由于在一个平面内进行运动规划时，该平面内的连杆投影长度不变，且其他平面的投影可实时计算，故可通过划分工作过程，选择合适的工作平面，可在一个工作平面内完成指定运动，再计算连杆实时参数，以得到在下一个工作平面内的投影，继续在下一个工作平面内完成指定运动，可对运动过程进行编号，并按顺序进行以完成协调运动操作。

按步骤5，针对每一过程选择合适的投影面，投影面上的平面几何模型是空间坐标系下三维模型的投影，各连杆在平面内的实时数学模型须通过在各关节坐标系下的连杆数学模型计算获得。

按步骤6，针对几何投影作协调运动规划，一般根据工作任务预先设定主动臂运动，再根据工作任务结合主动臂运动规划确定从动臂运动规划。

综合两执行器的位置和位姿约束以及执行器可求出执行器关节连线长度实时变化情况，设为l(t)，可综合两执行器位姿约束及执行器关节位置约束，由大小臂关节的协调运动完成执行器关节的位置约束，这里主要是根据坐标公式，综合斜率公式，运用平面几何方法完成。在这个过程中，一般根据工作任务预先设定主动臂运动，再根据工作任务结合主动臂运动规划反解从动臂各关节角运动情况。

之后根据大小臂关节的运动规划执行器关节的运动，以完成执行器末端的位置、位姿约束，在这个过程中，一般根据工作任务预先设定主动臂执行器关节运动，再根据工作任务结合主动臂运动规划反解从动臂执行器关节运动情况。

设运动时间为T，且各关节仅绕z轴转动，κ的变化会引起ω、ξ的变化，可通过执行器关节绕x轴与y轴转动的协同配合，实现位姿协调，设执行器关节间的距离l(t)(0≤t≤T)，从动臂末端执行器关节在主动臂执行器关节坐标系的位置为(x(t),y(t),z(t))，其连线在主动臂关节坐标系内旋转角为(ω(t),ξ(t),κ(t))，其中，z(t)在大小臂关节仅绕各自z轴旋转情况下是定值，κ(t)在进行xy平面投影协调过程中已确定，l(t)已确定，故式(1)存在4个未知变量x(t)、y(t)、ω(t)、ξ(t)，可以实时求解，从动臂执行器关节角可综合位姿约束及主动臂关节角得到。

如图1所示，按步骤7完成该协调运动过程后，重新计算参数模型，并根据下一协调运动过程特点选择新的投影面，获得该投影面上投影视图几何模型，制定合适的协调运动方案。

按步骤8，重复步骤6、7，直至完成全部协调运动过程。

如图8所示，建立球面三关节三连杆双臂机器人平面投影通用模型，α、β、γ、μ、λ、η为平面协调运动规划时控制两臂的运动的驱动变量，θ为执行器末端连线与y轴的夹角；a、e表示平面投影中，大臂关节与原点距离；b、c、d、f、g、h分别表示各连杆在平面投影的长度。由MATLAB进行计算并通过双臂协调配合完成轴孔装配，设有坐标公式：

式(2)、(3)、(4)、(5)为双臂各关节及执行器末端在投影面上的坐标计算公式。

如图9所示，当左臂由EFG运动到EF’G’位置时，θ变为θ'，根据内角和一定，即μ(t)+λ(t)+η(t)+(π-θ(t))+(π-ε)＝3π，有θ(t)＝μ(t)+λ(t)+μ(t)-3π/2。

根据斜率公式，综合G点坐标、θ(t)及l_xy(t)(0≤t≤T)可以求出C点实时坐标。

式(7)为综合斜率公式得到的C点实时坐标的计算公式，用于计算C点实时坐标。

计算过程中，

将t(0≤t≤T)中间插入n-1个时间节点，则两个点之间的时间差为

将包括终点内的共n个点记为C₁,C₂,C₃C_i C_n，第i个点位置为(x_Ci,y_Ci)，有

以α_i，β_i表示对应于C_i点的大臂关节角和小臂关节角，根据(1)、(2)式可知：

式(8)表示C_i点坐标与α_i，β_i间的关系，在C_i点坐标已知情况下，可用于求解α_i，β_i。

式(8)存在两组解，选择α_i，β_i相对于初始关节角变化较小的一组解。

至此，完成平面投影执行器关节协调，再按末端位姿协调方法计算，轴孔装配情况下，执行器在中轴线保持始终共线的情况下相向运动，若经计算，主动臂执行器关节旋转角为(ω(t),ξ(t),κ(t))，有从动臂末端执行器关节角与之相对应，为(-ω(t),-ξ(t),-κ(t))。

如图10、11、12、13所示，按上述方法在matlab中模拟完成轴孔装配任务，图10表示按照绕z轴旋转，在xy面投影进行双臂协调计算，其在该投影面上末端执行器投影始终共线；由图11和图12可知，在yz、xz平面上也有末端执行器投影始终共线；故如图13所示，末端执行器能够始终在三维空间内共线，并按预定规律相向运动，仿真实例对球关节双臂机器人通过几何投影法完成轴孔装配协调操作进行了验证，说明了方法的可行性。

综上所述，在本实施例中，按照本实施例的基于几何投影的球面关节双臂机器人协调运动方法，本实施例提供的基于几何投影的球面关节双臂机器人协调运动方法，解决了当前针对球面关节双臂机器人协调运动缺乏简单、实用、高效解决方法的问题，通过将复杂的、高度冗余的球面关节双臂机器人空间模型在空间直角坐标系中分别从y负向、x负向及z正向对三维模型进行投影，获得前视图、左视图和俯视图几何模型，运用平面几何方法综合斜率公式，插值法等对平面模型进行双臂协调，通过关节坐标系计算连杆的实时模型，便于下一工作过程继续运用几何投影计算，直至完成工作目标，能够在特定的工况下实现对双臂机器人简单、快速的协调规划，同时为多解或无数解问题提供可靠的解决方案，提高了双臂机器人的工作效率，能够提高工作的准确性，充分发挥双臂机器人的工作特点。

以上所述，仅为本发明进一步的实施例，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明所公开的范围内，根据本发明的技术方案及其构思加以等同替换或改变，都属于本发明的保护范围。

Claims (6)

1.一种基于几何投影的球面关节双臂机器人协调运动方法，其特征在于，包括如下步骤：

步骤1：将球面关节机器人简化为空间运动链模型；

步骤2：布置球面关节双臂机器人空间位置并建立空间直角坐标系，并分别从y轴负向、x轴负向及z轴正向对空间运动链模型的三维模型进行投影，获得前视图、左视图和俯视图，在前视图、左视图和俯视图中，当两臂与固定端连接关节的连线与水平坐标轴之间出现夹角δ时，δ为锐角，通过将投影几何图形绕原点顺时针或逆时针旋转与δ相同的角度，使之重合；

步骤3：建立各关节空间直角坐标系，建立各关节空间直角坐标系，以各关节中心为坐标原点按右手准则建立直角坐标系，令各个关节直角坐标系坐标轴的方向与空间直角坐标系坐标轴的方向一致，各关节空间直角坐标系坐标轴的方向不随关节旋转运动的过程变化，通过三自由度球面关节的配合，驱动连杆绕x轴、y轴或z轴做旋转运动，每个连杆的状态用一组转角序列表示(ω，ξ，κ)，ω、ξ、κ分别用杆件在yz、xz、xy平面内的投影与y、z、x轴正方向的夹角来表示，按右手法则来确定旋转角的正向，杆件末端点在各自空间坐标系的坐标为(x,y,z)，每个关节仅保留一个自由度工作，杆件末端点与转角序列间的关系有：

其中：l为杆件末端点的z轴坐标；

步骤4：划分协调运动过程，减少运动过程分段；

步骤5：选择投影面，按照顺序，针对每一运动过程选择投影面，并根据几何投影制定协调运动方案；

步骤6：在几何投影协调运动的过程中，根据工作任务预先设定主动臂运动规划，根据工作任务结合主动臂运动规划确定从动臂运动规划；

步骤7：完成该协调运动过程后，重新计算参数模型，获得投影模型，并根据下一协调运动过程特点选择新的投影面，制定协调运动方案，所述投影模型的双臂各关节及执行器末端在投影面上的坐标计算公式如下：

其中：α、β、γ、μ、λ、η为平面协调运动规划时控制两臂的运动的驱动变量；

θ为执行器末端连线与y轴的夹角；

a、e表示平面投影中，大臂关节与原点距离；

b、c、d、f、g、h分别表示各连杆在平面投影的长度；

步骤8：重复所述步骤6和所述步骤7，直至完成全部协调运动过程。

2.根据权利要求1所述的一种基于几何投影的球面关节双臂机器人协调运动方法，其特征在于，所述步骤1中，各关节简化为球面副，作为机器人的驱动，球面关节具有三自由度驱动，各关节通过配合实现空间任意位姿，机械臂简化为直线连杆，作为各球面副的连接，两臂均为开链结构。

3.根据权利要求2所述的一种基于几何投影的球面关节双臂机器人协调运动方法，其特征在于，所述步骤1中，两臂由n个三自由度球面关节及n个直线连杆组成，n为大于0的任意整数，各关节简化为球面副，作为机器人的驱动。

4.根据权利要求1所述的一种基于几何投影的球面关节双臂机器人协调运动方法，其特征在于，所述步骤2中，球面关节双臂机器人空间位置的条件为：取两臂与固定端连接关节连线中点为坐标原点，按右手法则建立空间直角坐标系，使两臂的工作空间分别位于相对的象限空间内，避免双臂机器人投影发生干涉，通过该坐标系分别获得前视图、左视图和俯视图的几何模型。

5.根据权利要求1所述的一种基于几何投影的球面关节双臂机器人协调运动方法，其特征在于，所述步骤4中，划分协调运动过程，在一个平面内进行运动规划时，该平面内的连杆投影长度不变，且其他平面的投影可实时计算，并在一个工作平面内完成指定运动，再计算连杆实时参数，以得到在下一个工作平面内的投影，继续在下一个工作平面内完成指定运动，并对运动过程进行编号，并按顺序进行完成协调运动操作。

6.根据权利要求1所述的一种基于几何投影的球面关节双臂机器人协调运动方法，其特征在于，所述步骤5中，投影面上的平面几何模型是空间坐标系下三维模型的投影，各连杆在平面内的实时数学模型通过在各关节坐标系下的连杆数学模型计算获得。

Images