机械臂的运动控制方法和系统与流程

1.本发明涉及机器人及其控制技术领域，尤其涉及一种机械臂的运动控制方法和系统。


背景技术：

2.随着仓储自动化和智能物流装备迅速发展，库房内仓储运营效率大幅提升。但在连接仓储和运输的装卸货物作业仍以人工或叉车装卸方式为主，费时费力，成为影响作业效率与物流成本的瓶颈之一。随着劳动力日益短缺，人力成本逐年上升，以及对物流时效要求不断提高，越来越多的企业在寻求自动装卸解决方案，以取代人工作业方式。自动装卸技术的应用，可以节省人力和相关成本，减少了叉车数量和相关人员数量，节省叉车维护和维修成本；装卸效率大幅提高，缩短车辆滞留时间，提高效率；货物摆放整齐，减少货物损坏与设备受损风险。自动装卸技术还有很多难题待突破，需要持续研究探索。


技术实现要素：

3.鉴于此，本发明提供了一种机械臂的运动控制方法和系统，可具体应用于自动装卸系统，用于至少部分地解决上述技术问题。
4.第一方面，本发明提供了一种机械臂的运动控制方法，所述机械臂包括基台、前臂、后臂、第一控制支链和第二控制支链，所述基台由第一转轴控制转动，所述后臂的第一端铰接于所述基台的顶面且第二端与所述前臂的第一端铰接，所述第一控制支链包括第一主动杆和第一从动杆，所述第一主动杆的第一端通过第二转轴转动连接于所述基台的顶面且第二端铰接于所述第一从动杆的第一端，所述第一从动杆的第二端铰接于所述前臂上，所述第二控制支链包括第二主动杆和第二从动杆，所述第二主动杆的第一端通过第三转轴转动连接于所述基台的顶面且第二端铰接于所述第二从动杆的第一端，所述第二从动杆的第二端铰接于所述后臂上，所述第三转轴平行于所述第二转轴且垂直于所述第一转轴，且所述第一转轴的轴线经过所述后臂的第一端与所述基台的铰接点；所述运动控制方法包括：
5.a：获取所述机械臂的所述前臂的第二端端点的目标位置和所述第一转轴、所述第二转轴和所述第三转轴的当前旋转角度；
6.b：利用机械臂运动的正解模型根据所述第一转轴、所述第二转轴和所述第三转轴的当前旋转角度计算所述前臂的第二端端点的当前位置，并利用所述机械臂运动的逆解模型根据所述目标位置计算所述第一转轴、第二转轴和第三转轴的目标旋转角度；
7.c：控制所述第一转轴、所述第二转轴和所述第三转轴从所述当前旋转角度旋转到所述目标旋转角度以控制所述机械臂的所述前臂的第二端端点从所述当前位置旋转到所述目标位置。
8.在一种可能的实现方式中，所述运动控制方法还包括：
9.对于所述前臂的第二端端点从第一位置到第二位置的预定轨迹执行插补运动，并
且对于每个插补周期：依次执行步骤a至步骤c，并且利用计算的后一插补周期的所述当前位置校验前一插补周期的所述目标位置以指导所述插补运动。
10.在一种可能的实现方式中，所述机械臂的运动分解为由第二、第三转轴控制在xoz平面坐标系上的俯仰运动和由所述第一转轴控制绕z轴在xoy平面坐标系上的旋转运动，所述xoz平面坐标系和所述xoy平面坐标系构成xyz空间坐标系；
11.所述机械臂运动的正逆解模型根据以下步骤构建：
12.对于所述机械臂执行的一所述俯仰运动的姿态，基于该姿态投影在所述xoz平面坐标系上的几何模型，建立所述机械臂的所述俯仰运动的正逆解模型，其中所述俯仰运动的正解模型是根据第二、第三转轴的旋转角度解算出所述前臂的第二端端点在所述xoz平面坐标系上投影的坐标的运动学模型；
13.对于所述机械臂执行的一所述旋转运动的姿态，基于该姿态投影在所述xoy平面坐标系上的几何模型和所述第一转轴执行该旋转运动的旋转角度，建立所述机械臂的所述旋转运动的正逆解模型，其中所述机械臂的所述旋转运动的正解模型是根据第一转轴的旋转角度解算出所述前臂的第二端端点在所述xoy平面坐标系上投影的坐标的运动学模型；
14.将所述机械臂的所述俯仰运动的正逆解模型和所述旋转运动的正逆解模型进行合成以将坐标转换到所述xyz空间坐标系中，得到所述机械臂运动的正逆解模型。
15.在一种可能的实现方式中，构建所述机械臂运动的正逆解模型之前，还包括：
16.构建所述xyz空间坐标系，其中选定所述后臂的第一端与所述基台的铰接点为原点，以所述第一转轴的轴线为z轴、将第一转轴的旋转角度为0度时的所述俯仰运动的面作为所述xoz平面坐标系以确定x轴，根据右手定则确定y轴。
17.在一种可能的实现方式中，所述对于所述机械臂执行的一所述俯仰运动的姿态，基于该姿态投影在所述xoz平面坐标系上的几何模型，建立所述机械臂的所述俯仰运动的正逆解模型进一步包括：
18.对于所述机械臂的一所述俯仰运动的姿态，将所述后臂、前臂、第一控制支链等效成一个异形的并联机械臂并基于所述后臂、前臂和第一控制支链投影在所述xoz平面坐标系上的几何模型建立所述后臂、前臂、第一控制支链运动的正逆解模型，其中所述后臂、前臂、第一控制支链运动的正解模型是根据所述后臂、第二转轴的旋转角度解算所述前臂的第二端端点在所述xoz平面坐标系上投影的坐标的运动学模型；
19.基于所述后臂和所述第二控制支链投影在所述xoz平面坐标系上的几何模型建立所述后臂的旋转角度与所述第三转轴的旋转角度的关系模型；
20.根据所述后臂、前臂、第一控制支链运动的正逆解模型和所述关系模型，确定所述第二、第三转轴的旋转角度与所述前臂的第二端端点的坐标的正逆解模型。
21.在一种可能的实现方式中，所述机械臂运动的正解模型表示为：
[0022][0023]
所述机械臂运动的逆解模型表示为：
[0024][0025]
其中，l1、l2、l3、l4、l5、l6、l8、l9分别为所述后臂、所述第一主动杆、所述第一从动杆、所述前臂的第一端侧臂段、所述前臂的第二端侧臂段、所述后臂的第一端与铰接支座之间的臂段、所述第二从动杆、所述第二主动杆在所述xoz平面坐标系上的投影的长度，l7为所述铰接支座的铰接点到所述后臂在所述xoz平面坐标系上的投影的垂直距离，α、θ2、θ3分别为所述第一转轴、所述第二转轴、所述第三转轴的旋转角度，θ1为所述后臂的旋转角度，og为所述第一转轴与所述基台的顶面的交点和所述第三转轴轴心的连线在所述xoz平面坐标系上的投影的长度，om为所述第一转轴与所述基台的顶面的交点和所述铰接支座的铰接点的连线在所述xoz平面坐标系上的投影的长度，h1为所述第三转轴轴心在所述xoz平面坐标系上的投影点到所述基台的台面的垂直距离，d1为第三转轴轴心在所述xoz平面坐标系上的投影点到所述第一转轴的垂直距离，h2为第二转轴轴心在所述xoz平面坐标系上的投影点到所述基台的台面的垂直距离，d2为第二转轴轴心在所述xoz平面坐标系上的投影点到所述第一转轴的垂直距离，β1为所述第一主动杆与第一从动杆的铰接点和所述第二主动杆与第二从动杆的铰接点的连线在所述xoz平面坐标系上的投影与所述xoy平面坐标系的夹角，β2为所述前臂在所述xoz平面坐标系上的投影与所述xoy平面坐标系的夹角，β为所述第一转轴与所述基台的顶面的交点和所述第二从动杆与所述后臂的铰接点的连线与该交点到所述第三转轴轴心的连线在所述xoz平面坐标系上的投影的夹角，xf、yf、zf为所述前臂
的第二端端点在所述xyz空间坐标系中的坐标。
[0026]
第二方面，本发明提供了一种机械臂的运动控制系统，包括：
[0027]
所述机械臂，其包括基台、前臂、后臂、第一控制支链和第二控制支链，所述基台由第一转轴控制转动，所述后臂的第一端铰接于所述基台的顶面且第二端与所述前臂的第一端铰接，所述第一控制支链包括第一主动杆和第一从动杆，所述第一主动杆的第一端通过第二转轴转动连接于所述基台的顶面且第二端铰接于所述第一从动杆的第一端，所述第一从动杆的第二端铰接于所述前臂上，所述第二控制支链包括第二主动杆和第二从动杆，所述第二主动杆的第一端通过第三转轴转动连接于所述基台的顶面且第二端铰接于所述第二从动杆的第一端，所述第二从动杆的第二端铰接于所述后臂上，所述第三转轴平行于所述第二转轴且垂直于所述第一转轴，且所述第一转轴的轴线经过所述后臂的第一端与所述基台的铰接点；
[0028]
控制器，其包括：
[0029]
获取模块，用于获取所述前臂的第二端端点的目标位置和所述机械臂的第一转轴、第二转轴和第三转轴的当前旋转角度；
[0030]
计算模块，用于利用所述机械臂运动的正解模型根据所述第一转轴、第二转轴和第三转轴的当前旋转角度计算所述前臂的第二端端点的当前位置，并利用所述机械臂运动的逆解模型根据所述目标位置计算所述第一转轴、第二转轴和第三转轴的目标旋转角度；
[0031]
控制模块，用于控制所述第一转轴、第二转轴和第三转轴从所述当前旋转角度旋转到所述目标旋转角度以控制所述机械臂的所述前臂的第二端端点从所述当前位置旋转到所述目标位置。
[0032]
在一种可能的实现方式中，所述运动控制系统还包括插补模块，所述插补模块用于对于所述前臂的第二端端点从第一位置到第二位置的预定轨迹执行插补运动，并且对于每个插补周期，调用所述获取模块、计算模块和控制模块并执行，并且利用计算的后一插补周期的所述当前位置校验前一插补周期的所述目标位置以指导所述插补运动。
[0033]
在一种可能的实现方式中，所述机械臂的运动分解为由第二、第三转轴控制在xoz平面坐标系上的俯仰运动和由所述第一转轴控制绕z轴在xoy平面坐标系上的旋转运动，所述xoz平面坐标系和所述xoy平面坐标系构成xyz空间坐标系；
[0034]
所述控制器还包括用于存储所述机械臂运动的正逆解模型的存储模模块和模型构建模块，所述模型构建模块用于对于所述机械臂执行的一所述俯仰运动的姿态，基于该姿态投影在所述xoz平面坐标系上的几何模型建立所述机械臂的所述俯仰运动的正逆解模型，其中所述俯仰运动的正解模型是根据第二、第三转轴的旋转角度解算出所述前臂的第二端端点在所述xoz平面坐标系上投影的坐标的运动学模型，以及对于所述机械臂执行的一所述旋转运动的姿态，基于该姿态投影在所述xoy平面坐标系上的几何模型和所述第一转轴执行该旋转运动的旋转角度，建立所述机械臂的所述旋转运动的正逆解模型，其中所述机械臂的所述旋转运动的正解模型是根据第一转轴的旋转角度解算出所述前臂的第二端端点在所述xoy平面坐标系上投影的坐标的运动学模型，并且将所述机械臂的所述俯仰运动的正逆解模型和所述旋转运动的正逆解模型进行合成以将坐标转换到所述xyz空间坐标系中得到所述机械臂运动的正逆解模型。
[0035]
在一种可能的实现方式中，所述xyz空间坐标系的原点为所述后臂的第一端与所
述基台的铰接点，z轴为所述第一转轴的轴线、x轴根据将所述第一转轴的旋转角度为0度时的所述俯仰运动的面作为所述xoz平面坐标系来确定，y轴根据右手定则确定。
[0036]
在一种可能的实现方式中，所述模型构建模块还用于对于所述机械臂的一所述俯仰运动的姿态，将所述后臂、前臂、第一控制支链等效成一个异形的并联机械臂并基于所述后臂、前臂和第一控制支链投影在所述xoz平面坐标系上的几何模型建立所述后臂、前臂、第一控制支链运动的正逆解模型，其中所述后臂、前臂、第一控制支链运动的正解模型是根据所述后臂、第二转轴的旋转角度解算所述前臂的第二端端点在所述xoz平面坐标系上投影的坐标的运动学模型，以及基于所述后臂和所述第二控制支链投影在所述xoz平面坐标系上的几何模型建立所述后臂的旋转角度与所述第三转轴的旋转角度的关系模型，并且根据所述后臂、前臂、第一控制支链运动的正逆解模型和所述关系模型，确定所述第二、第三转轴的旋转角度与所述前臂的第二端端点的坐标的正逆解模型。
[0037]
在一种可能的实现方式中，所述机械臂运动的正解模型表示为：
[0038][0039]
所述机械臂运动的逆解模型表示为：
[0040][0041]
其中，l1、l2、l3、l4、l5、l6、l8、l9分别为所述后臂、所述第一主动杆、所述第一从动杆、所述前臂的第一端侧臂段、所述前臂的第二端侧臂段、所述后臂的第一端与铰接支座之间的臂段、所述第二从动杆、所述第二主动杆在所述xoz平面坐标系上的投影的长度，l7为所述铰接支座的铰接点到所述后臂在所述xoz平面坐标系上的投影的垂直距离，α、θ2、θ3分
别为所述第一转轴、所述第二转轴、所述第三转轴的旋转角度，θ1为所述后臂的旋转角度，og为所述第一转轴与所述基台的顶面的交点和所述第三转轴轴心的连线在所述xoz平面坐标系上的投影的长度，om为所述第一转轴与所述基台的顶面的交点和所述铰接支座的铰接点的连线在所述xoz平面坐标系上的投影的长度，h1为所述第三转轴轴心在所述xoz平面坐标系上的投影点到所述基台的台面的垂直距离，d1为第三转轴轴心在所述xoz平面坐标系上的投影点到所述第一转轴的垂直距离，h2为第二转轴轴心在所述xoz平面坐标系上的投影点到所述基台的台面的垂直距离，d2为第二转轴轴心在所述xoz平面坐标系上的投影点到所述第一转轴的垂直距离，β1为所述第一主动杆与第一从动杆的铰接点和所述第二主动杆与第二从动杆的铰接点的连线在所述xoz平面坐标系上的投影与所述xoy平面坐标系的夹角，β2为所述前臂在所述xoz平面坐标系上的投影与所述xoy平面坐标系的夹角，β为所述第一转轴与所述基台的顶面的交点和所述第二从动杆与所述后臂的铰接点的连线与该交点到所述第三转轴轴心的连线在所述xoz平面坐标系上的投影的夹角，xf、yf、zf为所述前臂的第二端端点在所述xyz空间坐标系中的坐标。
[0042]
在本发明的实施例中，由第一转轴控制机械臂整体绕第一转轴左右旋转和由第二和第三转轴联合控制机械臂前后及俯仰运动，以只通过对第一、第二和第三转轴的三个转轴的旋转角度的调整就可以实现机械臂末端在空间坐标系中点到点的位置移动。并且进一步地提出机械臂运动的正逆解模型，基于所提出的机械臂运动的正逆解模型的运动控制结果通过仿真验证可以实现精准定位，并且重复定位精度高。此外，将本发明的实施例配合拆码垛程序，特别适用对标准货箱货物的装卸车场景。
附图说明
[0043]
图1是根据发明的一个实施例的机械臂的简易结构示意图。
[0044]
图2是根据发明的一个实施例的机械臂的运动控制方法的流程示意图。
[0045]
图3是根据发明的机械臂运动的正解过程的俯仰运动姿态投影在xoz平面坐标系上和旋转运动姿态投影在xoy坐标系上的几何模型。
[0046]
图4是根据发明的机械臂运动的逆解过程的俯仰运动姿态投影在xoz平面坐标系上和旋转运动姿态投影在xoy坐标系上的几何模型。
[0047]
图5是根据发明的一个实施例的机械臂的运动控制系统的结构示意图。
[0048]
附图标记列表：
[0049]
基台110
[0050]
前臂120
[0051]
后臂130
[0052]
铰接支座131
[0053]
第一控制支链140
[0054]
第一主动杆141
[0055]
第一从动杆142
[0056]
第二控制支链150
[0057]
第二主动杆151
[0058]
第二从动杆152
[0059]
底座160
[0060]
第一转轴a1
[0061]
第二转轴a2
[0062]
第三转轴a3
[0063]
机械臂单元510
[0064]
数据反馈单元511
[0065]
控制器520
[0066]
获取模块521
[0067]
计算模块522
[0068]
控制模块523
[0069]
存储模块524
[0070]
模型构建模块525
[0071]
插补模块526
具体实施方式
[0072]
为使本技术的目的、技术方案、及优点更加清楚明白，以下参照附图、示例和实施例，对本技术进一步详细说明。显然，所描述的示例、实施例仅仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员所获得的所有其他技术方案，都属于本技术保护的范围。
[0073]
图1示出了一个实施例的机械臂100的结构示意图。参见图1，机械臂100包括基台110、前臂120、第一控制支链140、后臂130、第二控制支链150，基台110由第一转轴a1控制绕第一转轴a1转动，后臂130的第一端铰接于基台110的顶面且第二端与前臂120的第一端铰接，第一控制支链140包括第一主动杆141和第一从动杆142，第一主动杆141的第一端通过第二转轴a2转动连接于基台110的顶面且第二端铰接于第一从动杆142的第一端，第一从动杆142的第二端铰接于前臂120上，第二控制支链150包括第二主动杆151和第二从动杆152，第二主动杆151的第一端通过第三转轴a3转动连接于基台110的顶面且第二端铰接于第二从动杆152的第一端，第二从动杆152的第二端铰接于后臂130上，第三转轴a3平行于第二转轴a2且垂直于第一转轴a1，第一转轴a1的轴线经过后臂130的第一端与所述基台110的铰接点。进一步地，例如但不限于由偏航电机驱动第一转轴a1转动，由前伸电机驱动第二转轴a2转动，由俯仰电机驱动第三转轴a3转动。并且，可选地，前臂120、后臂130、第一控制支链140和第二控制支链150的几何模型共面，第一转轴a1的轴线位于前臂120和后臂130的俯仰运动面上。另外，可选地，第一主动杆141和第二主动杆151分别通过第二转轴a2和第三转轴a3与固定于基台110的顶面上的第一支架和第二支架转动连接。以及，可选地，第二从动杆152与垂直固定于后臂130上的铰接支座131铰接连接。需要指出，前述实施例的机械臂100只是为了更好的说明本发明的典型实施例，本发明涉及的机械臂100的结构不限于此。
[0074]
转至图2，其示出了一个实施例的运动控制方法200的流程示意图。运动控制方法200主要用于对前述实施例的机械臂100的运动控制，如图2所示，运动控制方法200包括：
[0075]
步骤s202：获取前臂120的第二端端点的目标位置和机械臂100的第一转轴a1、第二转轴a2和第三转轴a3的当前旋转角度。
[0076]
步骤s204：利用机械臂运动的正解模型根据第一转轴a1、第二转轴a2和第三转轴a3的当前旋转角度计算前臂120的第二端端点的当前位置，并利用机械臂运动的逆解模型根据目标位置计算第一转轴a1、第二转轴a2和第三转轴a3的目标旋转角度。
[0077]
步骤s206：控制第一转轴a1、第二转轴a2和第三转轴a3从当前旋转角度旋转到目标旋转角度以使得前臂120的第二端端点从当前位置运动到目标位置。
[0078]
在一个具体实现方式中，运动控制方法200还包括：对于所述前臂120的第二端端点从第一位置到第二位置执行预定轨迹(例如但不限于直线)的插补运动，并且对于每个插补周期：依次执行步骤s202至步骤s206，并且利用计算的后一插补周期的所述当前位置校验前一插补周期的所述目标位置以指导所述插补运动。其中，每个插补周期的开始位置对应前述当前位置以及每个插补周期的结束位置对应前述目标位置，并且前一插补周期的结束位置就是前一插补周期的开始位置。并且，可以理解，步骤s204中主要利用机械臂运动的逆解模型来解算目标旋转角度以及利用正解模型反解目标位置以保证解算准确度，尤其利于需要插补运动控制的每个插补点的定位精度。
[0079]
可选地，前述步骤s204中所述的机械臂运动的正解模型是根据第一转轴a1、第二转轴a2和第三转轴a3的旋转角度解算出前臂120的第二端端点的位置的运动学模型，表示为：
[0080][0081]
可选地，前述步骤s204中所述的机械臂运动的逆解模型是根据前臂120的第二端端点的位置解算出第一转轴a1、第二转轴a2和第三转轴a3的旋转角度的运动学模型，可以表示为：
[0082][0083]
其中，l1、l2、l3、l4、l5、l6、l8、l9分别为后臂130、第一主动杆141、第一从动杆142、前臂120的第一端侧臂段(即从第一端到前臂120和第一从动杆141之间的铰接点的臂段)、前臂120的第二端侧臂段(即从第二端到前臂120和第一从动杆141之间的铰接点的臂段)、后臂130的第一端与位于后臂130上的铰接支座131之间的臂段、第二从动杆152、第二主动杆151在xoz平面坐标系上的投影的长度，l7为铰接支座131的铰接点到后臂130在xoz平面坐标系上的投影的垂直距离，α、θ2、θ3分别为第一转轴a1、第二转轴a2、第三转轴a3的旋转角度，θ1为后臂130的旋转角度，og为第一转轴a1与基台110的顶面的交点和第三转轴a3轴心的连线在xoz平面坐标系上的投影的长度，om为第一转轴a1与基台110的顶面的交点和铰接支座131的铰接点的连线在xoz平面坐标系上的投影的长度，h1为第三转轴a3轴心在xoz平面坐标系上的投影点到基台110的顶面的垂直距离，d1为第三转轴a3轴心在xoz平面坐标系上的投影点到第一转轴a1的垂直距离，h2为第二转轴a2轴心在xoz平面坐标系上的投影点到基台110的台面的垂直距离，d2为第二转轴a2轴心在xoz平面坐标系上的投影点到第一转轴a1的垂直距离，β1为第一主动杆141与第一从动杆142的铰接点和第二主动杆151与第二从动杆152的铰接点的连线在xoz平面坐标系上的投影与xoy平面坐标系的夹角，β2为前臂120在xoz平面坐标系上的投影与xoy平面坐标系的夹角，β为第一转轴a1与基台110的顶面的交点和第二从动杆152与后臂130的铰接点的连线与该交点到第三转轴a3轴心的连线在xoz平面坐标系上的投影的夹角，xf、yf、zf为前臂120的第二端端点在xyz空间坐标系中的坐标。
[0084]
下面详细描述前述正逆解模型的构建过程。首先，在机械臂运动的正逆解模型之前，较佳地选定后臂130的第一端与基台110的铰接点为原点，以第一转轴a1的轴线为z轴，构建xyz空间坐标系，其中xyz空间坐标中的xoz平面坐标系垂直于第二转轴a2或第三转轴a3，xoy平面坐标系垂直于xoz平面坐标系。进一步地，可以用该xyz空间坐标系中的坐标(xf、yf、zf)表示机械臂100的前臂120的第二端端点的位置，即可以控制前臂120的第二端端点从xyz空间坐标系中的第一坐标的当前位置运动到第二坐标的目标位置。
[0085]
其次，在构建机械臂运动的正逆解模型时，机械臂100的运动分解为由第二转轴
a2、第三转轴a3控制在xoz平面坐标系上的俯仰运动和由第一转轴a1控制绕z轴在xoy平面坐标系上的旋转运动，xoz平面坐标系和xoy平面坐标系构成xyz空间坐标系。并且，在构建xyz空间坐标系时，进一步地较佳地将第一转轴a1的旋转角度为0度时的俯仰运动的面作为xoz平面坐标系以确定x轴，根据右手定则确定y轴，来构建xyz空间坐标系。
[0086]
更进一步地，基于所构建的xyz空间坐标系根据以下步骤构建机械臂运动的正逆解模型：第一步，对于机械臂100执行的一俯仰运动的姿态，基于该姿态投影在xoz平面坐标系(与俯仰运动面共面或平行)上的几何模型，建立机械臂100的俯仰运动的正逆解模型，其中俯仰运动的正解模型是根据第二转轴a2、第三转轴a3的旋转角度解算出前臂120的第二端端点在xoz平面坐标系上投影的坐标的运动学模型；第二步，对于机械臂100执行的一旋转运动的姿态，基于该姿态投影在xoy平面坐标系上的几何模型和第一转轴a1执行该旋转运动的旋转角度，建立机械臂100的旋转运动的正逆解模型，其中机械臂100的旋转运动的正解模型是根据第一转轴a1的旋转角度解算出前臂120的第二端端点在xoy平面坐标系上投影的坐标的运动学模型；第三步，将机械臂100的俯仰运动的正逆解模型和旋转运动的正逆解模型进行合成以将坐标转换到xyz空间坐标系中，得到机械臂运动的正逆解模型。
[0087]
并且，前述“第一步”还可以较佳地实现为：对于机械臂100的一俯仰运动的姿态，将后臂130、前臂120、第一控制支链140等效成一个异形的并联机械臂100并基于后臂130、前臂120和第一控制支链140投影在xoz平面坐标系上的几何模型，建立后臂130、前臂120、第一控制支链140运动的正解和逆解模型(正解模型是根据后臂130的旋转角度、第二转轴a2的旋转角度解算前臂120的第二端端点在xoz平面坐标系上投影的坐标的运动学模型)；基于后臂130和第二控制支链150投影在xoz平面坐标系上的几何模型建立后臂130的旋转角度与第三转轴a3的旋转角度的关系模型，则第二转轴a2的旋转角度、第三转轴a3的旋转角度与前臂120的第二端端点在xoz平面坐标系投影的坐标的正解模型即为“后臂130、前臂120、第一控制支链140运动的正解模型”和“后臂130的旋转角度与第三转轴a3的旋转角度的关系模型”的组合)。
[0088]
此外，下面以图2实施例的机械臂100为例描述前述的机械臂100运动的正解和逆解模型的构建过程：
[0089]
(1)机械臂运动的正解模型：
[0090]
(1.1)基于图3(a)的一俯仰运动的姿态的几何模型，根据第三转轴a3的旋转角度θ3，求后臂130的旋转角度θ1，得到：
[0091][0092][0093]
on2＝og2+gn
2-2
×
og
×
gn cosθ3[0094][0095][0096]
[0097][0098]
(1.2)基于图3(b)的一俯仰运动的姿态的几何模型，根据第三转轴a3的旋转角度θ3、第二转轴a2的旋转角度θ2计算f点在xoz坐标系上投影的坐标(xf′
zf′
)：
[0099]
cd2＝(xc′-xd′
)2+(zc′-zd′
)2[0100]
xc′
＝l2×
cos(θ
2-θ)+d2[0101]
zc′
＝-l2×
sin(θ
2-θ)+h2[0102]
xd′
＝l1×
cos(θ1+θ)
[0103]
zd′
＝l1×
sin(θ1+θ)
[0104][0105][0106]
求得：
[0107]
xf′
＝(l4+l5)sin(β1+β2)+xd′
[0108]
zf′
＝zf＝-(l4+l5)cos(β1+β2)+zd′
[0109]
(1.3)如图3(c)的几何模型，计算f点在xoy坐标系上投影的坐标(xf″
，yf″
)：
[0110]
xf″
＝xf＝xf′
×
cosα
[0111]
yf″
＝yf＝xf′
×
sinα
[0112]
(1.4)转换得到：
[0113]
xf＝((l4+l5)sin(β1+β2)+xd′
)
×
cosα
[0114]
yf＝((l4+l5)sin(β1+β2)+xd′
)
×
sinα
[0115]
zf＝-(l4+l5)cos(β1+β2)+zd′
[0116]
(2)机械臂运动的逆解模型：
[0117]
(2.1)基于图4(a)的几何模型，已知f点在xyz空间坐标系中的坐标(xf，yf，zf)求第一转轴a1的旋转角度α和长度of：
[0118][0119][0120]
(2.2)基于图4(b)的几何模型，首先，计算f点在xoz平面坐标系上投影的坐标(xf′
，zf′
)：
[0121]
xf′
＝of
[0122]
zf′
＝zf[0123]
然后，根据f点在xoz平面坐标系上投影的坐标(xf′
，zf′
)求后臂130的旋转角度θ1和第二转轴a2的旋转角度θ2：
[0124]
df2＝(xd′-xf′
)2+(zd′-zf′
)2[0125]
xd′
＝l1×
cos(θ1+θ)
[0126]
zd′
＝l1×
sin(θ1+θ)
[0127]
令则
[0128]
得到，关于t的一元二次方程：at2+bt+c＝0，其中，
[0129][0130]
b＝-4l1zf′
；
[0131][0132]
求得，正解
[0133]
得到，θ1＝2tan-1
t
[0134]
同理，根据ce长度求第二转轴a2的旋转角度θ2：
[0135][0136][0137]
ce2＝(xc′-xe′
)2+(zc′-ze′
)2[0138]
xc′
＝d2+l2×
cos(θ
2-θ)
[0139]
zc′
＝h
2-l2×
sin(θ
2-θ)
[0140]
令则
[0141]
得到，关于t的一元二次方程a
′
t
′2+b
′
t
′
+c
′
＝0，其中，
[0142][0143]
b’＝-4l2(h
2-ze′
)
[0144][0145]
求得，正解
[0146]
得到，θ2＝2tan-1
t
′
[0147]
(2.3)如图4(c)的几何模型，根据后臂130的旋转角度θ1，求第三转轴a3的旋转角度θ3，得到：
[0148][0149][0150][0151]
gm2＝og2+om
2-2
×
og
×
om
×
cosβ
[0152][0153][0154]
θ3＝β3+β4[0155]
那么，机械臂运动的逆解模型可以为上述公式的组合。
[0156]
需要说明的时，图3和图4中a为后臂、第一转轴与基台的顶面的交点，b为第二转轴的轴心，c为第一主动杆和第一从动杆之间的铰接点，d为前臂与后臂之间的铰接点，e为前臂与第一从动杆之间的铰接点，f为前臂的第二端端点，g为第三转轴的轴心，h为铰接支座和后臂的交点，m为第二从动臂与铰接支座之间的铰接点，n为第二主动杆和第二从动杆之间的铰接点，前述涉及的所有公式中任意两个字符组合表示两个字符对应的点之间的连线长度，(xc′
，zc′
)表示c点投影在xoz平面坐标系上的坐标，(xd′
，zd′
)表示d点投影在xoz平面坐标系上的坐标，(xe′
，ze′
)表示e点投影在xoz平面坐标系上的坐标。
[0157]
由上述可以看出，本运动控制方法的实施例主要通过第一转轴控制机械臂整体左右旋转和通过第二和第三转轴联合控制机械手大臂前后及俯仰运动，并且通过控制三个转轴的旋转角度实现机械臂的第二端端点或设置在第二端端点处的手部等在空间坐标系中点对点的精准位置移动。将本实施例机械臂的运动控制方法配合拆码垛程序，特别适用对标准货箱货物的装卸车场景。
[0158]
尽管本运动控制方法的实施例中涉及的各步骤以特定的顺序示出和描述，但是各步骤的操作顺序可以改变，使得某些操作可以以相反的顺序执行，或者使得某些操作可以至少部分地与其它操作同时执行。
[0159]
为了实现上述运动控制方法的实施例，本发明的另一个实施例还提供了一种运动控制系统500，如图5所示。并且，由于下述的实施例是为了实现前述的方法实施例，故该运动控制系统500中的各模块都是为了实现前述方法的各步骤而设，因此本发明不限于下述的实施例，任何可以实现上述方法的系统、单元或模块都应包含在本发明的保护范围内。并且，为了节约篇幅，与前述方法部分相同内容在此省略。因此可以理解，前述运动控制方法实施例技术细节均可用于下述运动控制系统实施例。
[0160]
参见图5，系统500包括机械臂单元510和控制器520。机械臂单元510包括前述实施例的机械臂100、数据反馈单元511。其中，数据反馈单元511用于向给控制器520反馈机械臂100的前臂120端点的第二端端点的目标位置以及机械臂100的第一转轴a1、第二转轴a2和第三转轴a3的当前旋转角度。
[0161]
可选地，机械臂100还可以包括转轴驱动单元(例如但不限于电机)，用于驱动第一转轴a1转动的偏航电机，用于驱动第二转轴a2转动的前伸电机，用于驱动第三转轴a3转动的俯仰电机。
[0162]
控制器520包括获取模块521、计算模块522和控制模块523。获取模块521用于从数据反馈单元511获取前臂120的第二端端点的目标位置以及机械臂100的第一转轴a1、第二转轴a2和第三转轴a3的当前旋转角度。计算模块522用于利用所述机械臂运动的正解模型根据所述第一转轴a1、第二转轴a2和第三转轴a3的当前旋转角度计算所述前臂120的第二端端点的当前位置，并利用所述机械臂运动的逆解模型根据所述目标位置计算所述第一转轴a1、第二转轴a2和第三转轴a3的目标旋转角度。控制模块523用于控制所述第一转轴a1、第二转轴a2和第三转轴a3从所述当前旋转角度旋转到所述目标旋转角度以控制所述机械臂100的所述前臂120的第二端端点从所述当前位置旋转到所述目标位置。
[0163]
可选地，所述运动控制系统还包括插补模块526，插补模块526用于对于所述前臂120的第二端端点从第一位置到第二位置的预定轨迹执行插补运动，并且对于每个插补周期，调用获取模块521、计算模块522和控制模块523并执行，并且利用计算的后一插补周期
的所述当前位置校验前一插补周期的所述目标位置以指导所述插补运动。
[0164]
可选地，控制器520还包括用于存储机械臂运动的正逆解模型的存储模块524。较佳地，机械臂运动的正解模型表示为：
[0165][0166]
相应地，机械臂运动的逆解模型表示为：
[0167][0168]
其中，l1、l2、l3、l4、l5、l6、l8、l9分别为后臂130、第一主动杆141、第一从动杆142、前臂120的第一端侧臂段、前臂120的第二端侧臂段、后臂130的第一端与铰接支座131之间的臂段、第二从动杆152、第二主动杆151在xoz平面坐标系上的投影的长度，l7为铰接支座131的铰接点到后臂130在xoz平面坐标系上的投影的垂直距离，α、θ2、θ3分别为第一转轴a1、第二转轴a2、第三转轴a3的旋转角度，θ1为后臂130的旋转角度，og为第一转轴a1与基台110的顶面的交点和第三转轴a3轴心的连线在xoz平面坐标系上的投影的长度，om为第一转轴a1与基台110的顶面的交点和铰接支座131的铰接点的连线在xoz平面坐标系上的投影的长度，h1为第三转轴a3轴心在xoz平面坐标系上的投影点到基台110的台面的垂直距离，d1为第三转轴a3轴心在xoz平面坐标系上的投影点到第一转轴a1的垂直距离，h2为第二转轴a2轴心在xoz平面坐标系上的投影点到基台110的台面的垂直距离，d2为第二转轴a2轴心在xoz平面坐标系上的投影点到第一转轴a1的垂直距离，β1为第一主动杆141与第一从动杆142的铰接点和第二主动杆151与第二从动杆152的铰接点的连线在xoz平面坐标系上的投影与xoy平面坐标系的夹角，β2为前臂120在xoz平面坐标系上的投影与xoy平面坐标系的夹角，β
为第一转轴a1与基台110的顶面的交点和第二从动杆152与后臂130的铰接点的连线与该交点到第三转轴a3轴心的连线在xoz平面坐标系上的投影的夹角，xf、yf、zf为前臂120的第二端端点在xyz空间坐标系中的坐标。
[0169]
存储模块524还存储有xyz空间坐标系，其中后臂130的第一端与基台110的铰接点为原点，以第一转轴a1的轴线为z轴，xyz空间坐标中的xoz平面坐标系垂直于第二转轴a2或第三转轴a3，xoy平面坐标系垂直于xoz平面坐标系。并且较佳地将第一转轴a1的旋转角度为0度时的俯仰运动的面作为xoz平面坐标系以确定x轴，根据右手定则确定y轴，来构建xyz空间坐标系。
[0170]
可选地，控制器520还包括用于基于前述xyz空间坐标系构建机械臂运动的正逆解模型的模型构建模块525。模型构建模块525在构建机械臂运动的正逆解模型时，将机械臂100的运动分解为由第二转轴a2、第三转轴a3控制在xoz平面坐标系上的俯仰运动和由第一转轴a1控制绕z轴在xoy平面坐标系上的旋转运动，xoz平面坐标系和xoy平面坐标系构成xyz空间坐标系。
[0171]
具体的，模型构建模块525用于对于机械臂100执行的一俯仰运动的姿态，基于该姿态投影在xoz平面坐标系上的几何模型建立机械臂100的俯仰运动的正逆解模型(俯仰运动的正解模型是根据第二转轴a2、第三转轴a3的旋转角度解算出前臂120的第二端端点在xoz平面坐标系上投影的坐标的运动学模型)；对于机械臂100执行的一旋转运动的姿态，基于该姿态投影在xoy平面坐标系上的几何模型和第一转轴a1执行该旋转运动的旋转角度，建立机械臂100的旋转运动的正逆解模型(机械臂100的旋转运动的正解模型是根据第一转轴a1的旋转角度解算出前臂120的第二端端点在xoy平面坐标系上投影的坐标的运动学模型)，并且将机械臂100的俯仰运动的正逆解模型和旋转运动的正逆解模型进行合成以将坐标转换到xyz空间坐标系中得到机械臂运动的正逆解模型。
[0172]
并且，更具体的，模型构建模块525还用于对于机械臂100的一俯仰运动的姿态，将后臂130、前臂120、第一控制支链140等效成一个异形的并联机械臂100并基于后臂130、前臂120和第一控制支链140投影在xoz平面坐标系上的几何模型建立后臂130、前臂120、第一控制支链140运动的正逆解模型(其中正解模型是根据后臂130、第二转轴a2的旋转角度解算前臂120的第二端端点在xoz平面坐标系上投影的坐标的运动学模型)；基于后臂130和第二控制支链150投影在xoz平面坐标系上的几何模型建立后臂130的旋转角度与第三转轴a3的旋转角度的关系模型；以及，根据后臂130、前臂120、第一控制支链140运动的正逆解模型和关系模型，确定第二转轴a2、第三转轴a3的旋转角度与前臂120的第二端端点的坐标的正逆解模型。
[0173]
此外，可选地，根据所应用的场景的不同，机械臂100的前臂120的第二端可以根据需要安装或固定各种类型的手部，以及本实施例的控制方法还包括“根据前臂120的第二端端点的位置并结合前臂120的第二端端点与手部的位置关系，控制手部从一个位置运动到另一个位置”。并且，可选地，机械臂100还可以包括固定底座或可移动底座(例如但不限于agv/ems小车等)，以扩大前臂120的第二端或手部的运动范围。
[0174]
需要说明的是，在本专利的权利要求和说明书中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其
任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
[0175]
显然，本领域的技术人员应该明白，上述的本说明书实施例的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本说明书实施例不限制于任何特定的硬件和软件结合。
[0176]
虽然通过参照本发明的某些优选实施方式，已经对本发明进行了图示和描述，但本领域的普通技术人员应该明白，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。上文通过附图和优选实施例对本发明进行了详细展示和说明，然而本发明不限于这些已揭示的实施例，基与上述多个实施例本领域技术人员可以知晓，可以组合上述不同实施例中的代码审核手段得到本发明更多的实施例，这些实施例也在本发明的保护范围之内。

技术特征：
1.一种机械臂的运动控制方法，其特征在于，所述机械臂(100)包括基台(110)、前臂(120)、后臂(130)、第一控制支链(140)和第二控制支链(150)，所述基台(110)由第一转轴(a1)控制转动，所述后臂(130)的第一端铰接于所述基台(110)的顶面且第二端与所述前臂(120)的第一端铰接，所述第一控制支链(140)包括第一主动杆(141)和第一从动杆(142)，所述第一主动杆(141)的第一端通过第二转轴(a2)转动连接于所述基台(110)的顶面且第二端铰接于所述第一从动杆(142)的第一端，所述第一从动杆(142)的第二端铰接于所述前臂(120)上，所述第二控制支链(150)包括第二主动杆(151)和第二从动杆(152)，所述第二主动杆(151)的第一端通过第三转轴(a3)转动连接于所述基台(110)的顶面且第二端铰接于所述第二从动杆(152)的第一端，所述第二从动杆(152)的第二端铰接于所述后臂(130)上，所述第三转轴(a3)平行于所述第二转轴(a2)且垂直于所述第一转轴(a1)，所述第一转轴(a1)的轴线经过所述后臂(130)的第一端与所述基台(110)的铰接点；所述运动控制方法包括：a：获取所述机械臂(100)的所述前臂(120)的第二端端点的目标位置和所述第一转轴(a1)、所述第二转轴(a2)和所述第三转轴(a3)的当前旋转角度；b：利用机械臂运动的正解模型根据所述第一转轴(a1)、所述第二转轴(a2)和所述第三转轴(a3)的当前旋转角度计算所述前臂(120)的第二端端点的当前位置，并利用机械臂运动的逆解模型根据所述目标位置计算所述第一转轴(a1)、所述第二转轴(a2)和所述第三转轴(a3)的目标旋转角度；c：控制所述第一转轴(a1)、所述第二转轴(a2)和所述第三转轴(a3)从所述当前旋转角度旋转到所述目标旋转角度以控制所述机械臂(100)的所述前臂(120)的第二端端点从所述当前位置旋转到所述目标位置。2.根据权利要求1所述的机械臂的运动控制方法，其特征在于，所述运动控制方法还包括：对于所述前臂(120)的第二端端点从第一位置到第二位置的预定轨迹执行插补运动，并且对于每个插补周期：依次执行步骤a至步骤c，并且利用计算的后一插补周期的所述当前位置校验前一插补周期的所述目标位置以指导所述插补运动。3.根据权利要求1或2所述的机械臂的运动控制方法，其特征在于，所述机械臂(100)的运动分解为由所述第二转轴(a2)、所述第三转轴(a3)控制在的xoz平面坐标系上的俯仰运动和由所述第一转轴(a1)控制绕z轴在xoy平面坐标系上的旋转运动，所述xoz平面坐标系和所述xoy平面坐标系构成xyz空间坐标系；所述机械臂运动的正逆解模型根据以下步骤构建：对于所述机械臂执行的一所述俯仰运动的姿态，基于该姿态投影在所述xoz平面坐标系上的几何模型，建立所述机械臂(100)的所述俯仰运动的正逆解模型，其中所述俯仰运动的正解模型是根据所述第二转轴(a2)、所述第三转轴(a3)的旋转角度解算出所述前臂(120)的第二端端点在所述xoz平面坐标系上投影的坐标的运动学模型；对于所述机械臂(100)执行的一所述旋转运动的姿态，基于该姿态投影在所述xoy平面坐标系上的几何模型和所述第一转轴(a1)执行该旋转运动的旋转角度，建立所述机械臂(100)的所述旋转运动的正逆解模型，其中所述机械臂(100)的所述旋转运动的正解模型是根据第一转轴(a1)的旋转角度解算出所述前臂(120)的第二端端点在所述xoy平面坐标系
上投影的坐标的运动学模型；将所述机械臂(100)的所述俯仰运动的正逆解模型和所述旋转运动的正逆解模型进行合成以将坐标转换到所述xyz空间坐标系中，得到所述机械臂运动的正逆解模型。4.根据权利要求3所述的机械臂的运动控制方法，其特征在于，构建所述机械臂运动的正逆解模型之前，还包括：构建所述xyz空间坐标系，其中选定所述后臂(130)的第一端与所述基台(110)的铰接点为原点，以所述第一转轴(a1)的轴线为z轴，将第一转轴(a1)的旋转角度为0度时的所述俯仰运动的面作为所述xoz平面坐标系以确定x轴，根据右手定则确定y轴。5.根据权利要求4所述的机械臂的运动控制方法，其特征在于，所述机械臂运动的正解模型表示为：所述机械臂运动的逆解模型表示为：其中，l1、l2、l3、l4、l5、l6、l8、l9分别为所述后臂(130)、所述第一主动杆(141)、所述第一从动杆(142)、所述前臂(120)的第一端侧臂段、所述前臂(120)的第二端侧臂段、所述后臂(130)的第一端与铰接支座(131)之间的臂段、所述第二从动杆(152)、所述第二主动杆(151)在xoz平面坐标系上的投影的长度，l7为所述铰接支座(131)的铰接点到所述后臂(130)在所述xoz平面坐标系上的投影的垂直距离，α、θ2、θ3分别为所述第一转轴(a1)、所述第二转轴(a2)、所述第三转轴(a3)的旋转角度，θ1为所述后臂(130)的旋转角度，og为所述
第一转轴(a1)与所述基台(110)的顶面的交点和所述第三转轴(a3)轴心的连线在所述xoz平面坐标系上的投影的长度，om为所述第一转轴(a1)与所述基台(110)的顶面的交点和所述铰接支座(131)的铰接点的连线在所述xoz平面坐标系上的投影的长度，h1为所述第三转轴(a3)轴心在所述xoz平面坐标系上的投影点到所述基台(110)的台面的垂直距离，d1为第三转轴(a3)轴心在所述xoz平面坐标系上的投影点到所述第一转轴(a1)的垂直距离，h2为第二转轴(a2)轴心在所述xoz平面坐标系上的投影点到所述基台(110)的台面的垂直距离，d2为第二转轴(a2)轴心在所述xoz平面坐标系上的投影点到所述第一转轴(a1)的垂直距离，β1为所述第一主动杆(141)与第一从动杆(142)的铰接点和所述第二主动杆(151)与第二从动杆(152)的铰接点的连线在所述xoz平面坐标系上的投影与xoy平面坐标系的夹角，β2为所述前臂(120)在所述xoz平面坐标系上的投影与所述xoy平面坐标系的夹角，β为所述第一转轴(a1)与所述基台(110)的顶面的交点和所述第二从动杆(152)与所述后臂(130)的铰接点的连线与该交点到所述第三转轴(a3)轴心的连线在所述xoz平面坐标系上的投影的夹角，x
f
、y
f
、z
f
为所述前臂(120)的第二端端点在xyz空间坐标系中的坐标，所述xoz平面坐标系和所述xoy平面坐标系构成xyz空间坐标系。6.一种机械臂的运动控制系统，其特征在于，包括：所述机械臂(100)，其包括基台(110)、前臂(120)、后臂(130)、第一控制支链(140)和第二控制支链(150)，所述基台(110)由第一转轴(a1)控制转动，所述后臂(130)的第一端铰接于所述基台(110)的顶面且第二端与所述前臂(120)的第一端铰接，所述第一控制支链(140)包括第一主动杆(141)和第一从动杆(142)，所述第一主动杆(141)的第一端通过第二转轴(a2)转动连接于所述基台(110)的顶面且第二端铰接于所述第一从动杆(142)的第一端，所述第一从动杆(142)的第二端铰接于所述前臂(120)上，所述第二控制支链(150)包括第二主动杆(151)和第二从动杆(152)，所述第二主动杆(151)的第一端通过第三转轴(a3)转动连接于所述基台(110)的顶面且第二端铰接于所述第二从动杆(152)的第一端，所述第二从动杆(152)的第二端铰接于所述后臂(130)上，所述第三转轴(a3)平行于所述第二转轴(a2)且垂直于所述第一转轴(a1)，所述第一转轴(a1)的轴线经过所述后臂(130)的第一端与所述基台(110)的铰接点；控制器(520)，其包括：获取模块(521)，用于获取所述机械臂(100)的所述前臂(120)的第二端端点的目标位置和所述第一转轴(a1)、所述第二转轴(a2)和所述第三转轴(a3)的当前旋转角度；计算模块(522)，用于利用机械臂运动的正解模型根据所述第一转轴(a1)、所述第二转轴(a2)和所述第三转轴(a3)的当前旋转角度计算所述前臂(120)的第二端端点的当前位置，并利用机械臂运动的逆解模型根据所述目标位置计算所述第一转轴(a1)、所述第二转轴(a2)和所述第三转轴(a3)的目标旋转角度；控制模块(523)，用于控制所述第一转轴(a1)、所述第二转轴(a2)和所述第三转轴(a3)从所述当前旋转角度旋转到所述目标旋转角度以控制所述机械臂(100)的所述前臂(120)的第二端端点从所述当前位置旋转到所述目标位置。7.根据权利要求6所述的机械臂的运动控制系统，其特征在于，所述运动控制系统还包括插补模块(526)，所述插补模块(526)用于对于所述前臂(120)的第二端端点从第一位置到第二位置的预定轨迹执行插补运动，并且对于每个插补周期，调用所述获取模块(521)、
计算模块(522)和控制模块(523)并执行，并且利用计算的后一插补周期的所述当前位置校验前一插补周期的所述目标位置以指导所述插补运动。8.根据权利要求6或7所述的机械臂的运动控制系统，其特征在于，所述机械臂(100)的运动分解为由所述第二转轴(a2)、所述第三转轴(a3)控制在xoz平面坐标系上的俯仰运动和由所述第一转轴(a1)控制绕z轴在xoy平面坐标系上的旋转运动，所述xoz平面坐标系和所述xoy平面坐标系构成xyz空间坐标系；所述控制器(520)还包括用于存储所述机械臂运动的正逆解模型的存储模模块和模型构建模块(525)，所述模型构建模块(525)用于对于所述机械臂(100)执行的一所述俯仰运动的姿态，基于该姿态投影在所述xoz平面坐标系上的几何模型建立所述机械臂(100)的所述俯仰运动的正逆解模型，其中所述俯仰运动的正解模型是根据第二转轴(a2)、第三转轴(a3)的旋转角度解算出所述前臂(120)的第二端端点在所述xoz平面坐标系上投影的坐标的运动学模型，以及对于所述机械臂(100)执行的一所述旋转运动的姿态，基于该姿态投影在所述xoy平面坐标系上的几何模型和所述第一转轴(a1)执行该旋转运动的旋转角度，建立所述机械臂(100)的所述旋转运动的正逆解模型，其中所述机械臂(100)的所述旋转运动的正解模型是根据第一转轴(a1)的旋转角度解算出所述前臂(120)的第二端端点在所述xoy平面坐标系上投影的坐标的运动学模型，并且将所述机械臂(100)的所述俯仰运动的正逆解模型和所述旋转运动的正逆解模型进行合成以将坐标转换到所述xyz空间坐标系中得到所述机械臂运动的正逆解模型。9.根据权利要求8所述的机械臂的运动控制系统，其特征在于，所述xyz空间坐标系的原点为所述后臂(130)的第一端与所述基台(110)的铰接点，z轴为所述第一转轴(a1)的轴线、x轴根据将所述第一转轴(a1)的旋转角度为0度时的所述俯仰运动的面作为所述xoz平面坐标系来确定，y轴根据右手定则确定。10.根据权利要求9所述的机械臂的运动控制系统，其特征在于，所述机械臂运动的正解模型表示为：所述机械臂运动的逆解模型表示为：
其中，l1、l2、l3、l4、l5、l6、l8、l9分别为所述后臂(130)、所述第一主动杆(141)、所述第一从动杆(142)、所述前臂(120)的第一端侧臂段、所述前臂(120)的第二端侧臂段、所述后臂(130)的第一端与铰接支座(131)之间的臂段、所述第二从动杆(152)、所述第二主动杆(151)在xoz平面坐标系上的投影的长度，l7为所述铰接支座(131)的铰接点到所述后臂(130)在所述xoz平面坐标系上的投影的垂直距离，α、θ2、θ3分别为所述第一转轴(a1)、所述第二转轴(a2)、所述第三转轴(a3)的旋转角度，θ1为所述后臂(130)的旋转角度，og为所述第一转轴(a1)与所述基台(110)的顶面的交点和所述第三转轴(a3)轴心的连线在所述xoz平面坐标系上的投影的长度，om为所述第一转轴(a1)与所述基台(110)的顶面的交点和所述铰接支座(131)的铰接点的连线在所述xoz平面坐标系上的投影的长度，h1为所述第三转轴(a3)轴心在所述xoz平面坐标系上的投影点到所述基台(110)的台面的垂直距离，d1为第三转轴(a3)轴心在所述xoz平面坐标系上的投影点到所述第一转轴(a1)的垂直距离，h2为第二转轴(a2)轴心在所述xoz平面坐标系上的投影点到所述基台(110)的台面的垂直距离，d2为第二转轴(a2)轴心在所述xoz平面坐标系上的投影点到所述第一转轴(a1)的垂直距离，β1为所述第一主动杆(141)与第一从动杆(142)的铰接点和所述第二主动杆(151)与第二从动杆(152)的铰接点的连线在所述xoz平面坐标系上的投影与xoy平面坐标系的夹角，β2为所述前臂(120)在所述xoz平面坐标系上的投影与所述xoy平面坐标系的夹角，β为所述第一转轴(a1)与所述基台(110)的顶面的交点和所述第二从动杆(152)与所述后臂(130)的铰接点的连线与该交点到所述第三转轴(a3)轴心的连线在所述xoz平面坐标系上的投影的夹角，x
f
、y
f
、z
f
为所述前臂(120)的第二端端点在xyz空间坐标系中的坐标，所述xoz平面坐标系和所述xoy平面坐标系构成xyz空间坐标系。

技术总结
本申请实施例提供了一种机械臂的运动控制方法和系统。该方法包括：获取机械臂的所述前臂的第二端端点的目标位置和第一转轴、第二转轴和第三转轴的当前旋转角度；利用所述机械臂运动的正解模型根据所述第一转轴、第二转轴和第三转轴的当前旋转角度计算所述前臂的第二端端点的当前位置，并利用所述机械臂运动的逆解模型根据所述目标位置计算所述第一转轴、第二转轴和第三转轴的目标旋转角度；控制所述第一转轴、第二转轴和第三转轴从所述当前旋转角度旋转到所述目标旋转角度以控制所述机械臂的所述前臂的第二端端点从所述当前位置旋转到所述目标位置。本申请实施例可实现机械臂末端在空间坐标系中点对点的精准定位移动。末端在空间坐标系中点对点的精准定位移动。末端在空间坐标系中点对点的精准定位移动。


技术研发人员：王智涛 闫磊 许逸舟
受保护的技术使用者：西门子（中国）有限公司
技术研发日：2023.03.31
技术公布日：2023/7/31