CN111890372B - 一种随动型机械臂的控制方法 - Google Patents

Abstract

本发明提供了一种随动型机械臂的控制方法，对于任意输入的作用力指令，通过进行空间矢量分解运算，求解出三枚电机各自需要的输出力矩大小，进而驱动电机在持握点合成出所需的反馈力，从而能够将任意方向大小的三维空间力矢量向特定施力点进行分解，由此可以实现：在持握不同形状的物体时，其机械结构所处的位置不能提前预知，但输出的夹紧力仍可以保持在一定范围内。前臂末梢的持握点可到达一定空间内的任意位置，用户手持该持握点进行运动时，该随动型机械臂能随之自动调整连杆姿态，实现随行运动，使前臂末梢在一定工作空间内跟随人手移动，并同时按系统要求向人手施加作用力。

Description

一种随动型机械臂的控制方法

技术领域

本发明涉及数据处理技术领域，特别是涉及一种随动型机械臂的控制方法。

背景技术

机械臂是常用的机械自动化设备之一，机械臂能够代替或辅助人类手臂进行多种劳动量大、高精度、重复性高生产制造动作。例如，对于医疗行业的某些应用场景，使用者会持握机器臂进行手术操作等。

传统的机械臂控制系统面对的工作环境往往是针对固定工况，例如生产线机器人每次动作都相同，起吊机具每次升降的高度都基本接近等，但对于医疗行业的应用场景，经常遇到需要在特定位置施加特定大小力的情况，例如，对于持握机器臂，机械臂不仅需要完成持握动作，还需要对夹紧力进行精确控制，传统的机械控制系统显然无法实现这些功能。

发明内容

本发明的目的在于提供一种随动型机械臂的控制方法，以解决现有的机械臂控制系统无法在特定位置施加特定大小力的问题。

本发明提供一种随动型机械臂的控制方法，该随动型机械臂包括肩部组件、上臂、前臂；所述肩部组件包括底座、旋肩电机、旋肩传动结构、展臂电机、展臂传动结构、屈肘电机、屈肘传动结构，所述旋肩电机、所述展臂电机、所述屈肘电机均配备转子角度编码器；

所述旋肩电机固定在所述底座上，所述旋肩传动结构与所述旋肩电机连接，所述展臂电机和所述屈肘电机安装在所述旋肩传动结构上，所述旋肩电机用于带动所述随动型机械臂除所述底座和所述旋肩电机以外的部分绕竖直的旋肩轴旋转；

所述展臂传动结构连接在所述展臂电机的转子和所述屈肘电机的定子之间，所述展臂电机用于带动所述上臂和所述前臂绕水平的展臂轴旋转；

所述上臂的基部与所述屈肘电机的定子连接，所述前臂的一端与所述上臂铰接于肘关节处，并与所述屈肘传动结构连接，所述前臂的另一端延伸至持握点；

所述屈肘电机与所述屈肘传动结构连接，所述屈肘传动结构用于将所述屈肘电机的动力输出到所述肘关节，所述屈肘电机用于带动所述前臂绕水平的肘关节轴旋转；

所述旋肩轴和所述展臂轴在空间的交点为所述随动型机械臂的旋转中心，在所述旋肩电机、所述展臂电机、所述屈肘电机的联合带动下，所述持握点可到达一个球壳范围内的任意位置，球壳的外径等于所述上臂与所述前臂的长度之和，球壳内径等于所述上臂与所述前臂长度之差的绝对值，当使用者持握所述持握点时，所述随动型机械臂在使用者手部的移动带领下做随行运动，并同时向人手施加反力；

所述控制方法包括：

获取所述旋肩电机、所述展臂电机、所述屈肘电机的输出角；

根据所述旋转中心的位置、所述旋肩电机的输出角、所述上臂的长度、所述展臂电机的输出角，计算所述肘关节的位置；

根据所述展臂电机的输出角、所述屈肘电机的输出角计算所述前臂与水平面之间的前臂方向角；

根据所述肘关节的位置、所述前臂方向角、所述前臂的长度计算所述持握点的位置；

根据所述持握点的位置计算所述持握点到所述旋肩电机、所述展臂电机、所述屈肘电机的力臂矢量；

根据各个所述力臂矢量计算所述旋肩电机、所述展臂电机、所述屈肘电机向合力作用点输出分力的方向矢量；

根据各个所述方向矢量以及需要生成的反馈力合力矢量，计算出所述旋肩电机、所述展臂电机、所述屈肘电机的分力大小；

根据各个所述分力大小计算所述旋肩电机、所述展臂电机、所述屈肘电机的输出力矩。

进一步地，根据所述旋转中心的位置、所述旋肩电机的输出角、所述上臂的长度、所述展臂电机的输出角，计算所述肘关节的位置的步骤中，采用下式计算所述肘关节的位置：

其中，α₁为所述旋肩电机的输出角，α₂为所述展臂电机的输出角，[x₁,y₁,z₁]为所述旋转中心的位置，L_ua为所述上臂的长度，[x₂,y₂,z₂]为所述肘关节的位置；

根据所述展臂电机的输出角、所述屈肘电机的输出角计算所述前臂与水平面之间的前臂方向角的步骤中，采用下式计算所述前臂方向角：

其中，A₃为所述前臂方向角，α₃为所述屈肘电机的输出角。

进一步地，根据所述肘关节的位置、所述前臂方向角、所述前臂的长度计算所述持握点的位置的步骤中，采用下式计算所述持握点的位置：

其中，[x₃,y₃,z₃]为所述持握点的位置，L_fa为所述前臂的长度；

根据所述持握点的位置计算所述持握点到所述旋肩电机、所述展臂电机、所述屈肘电机的力臂矢量的步骤中，采用下式计算所述持握点到所述旋肩电机、所述展臂电机、所述屈肘电机的力臂矢量：

其中，

表示所述持握点到所述旋肩电机输出轴的垂线，

表示所述持握点到所述展臂电机输出轴的垂线，

表示所述前臂，向量i,j,k分别表示x,y,z方向上的单位向量；

根据各个所述力臂矢量计算所述旋肩电机、所述展臂电机、所述屈肘电机向合力作用点输出分力的方向矢量的步骤中，采用下式计算所述旋肩电机、所述展臂电机、所述屈肘电机向合力作用点输出分力的方向矢量：

其中，

表示所述旋肩电机向合力作用点输出分力的方向矢量，

表示所述展臂电机向合力作用点输出分力的方向矢量，

表示所述屈肘电机向合力作用点输出分力的方向矢量，

表示矢量叉乘运算，

为中间变量；

根据各个所述方向矢量以及需要生成的反馈力合力矢量，计算出所述旋肩电机、所述展臂电机、所述屈肘电机的分力大小的步骤中，采用下式计算所述旋肩电机、所述展臂电机、所述屈肘电机的分力大小：

其中，a₁、b₁、c₁表示所述旋肩电机分力方向单位向量f₁矢量在x,y,z方向上的分量，a₂、b₂、c₂表示所述展臂电机分力方向单位向量f₂矢量在x,y,z方向上的分量，a₃、b₃、c₃表示所述屈肘电机分力方向单位向量f₃矢量在x,y,z方向上的分量，

为所述反馈力合力矢量，F₁表示旋肩电机的分力大小，F₂表示展臂电机的分力大小，F₃表示屈肘电机的分力大小。

进一步地，根据各个所述分力大小计算所述旋肩电机、所述展臂电机、所述屈肘电机的输出力矩的步骤中，采用下式计算所述旋肩电机、所述展臂电机、所述屈肘电机的输出力矩：

或

其中，i=1,2,3，M₁表示旋肩电机的输出力矩，M₂表示展臂电机的输出力矩，M₃表示屈肘电机的输出力矩，N₁表示旋肩电机的传动减速比，N₂表示展臂电机的传动减速比，N₃表示屈肘电机的传动减速比，F₁表示旋肩电机的分力大小，F₂表示展臂电机的分力大小，F₃表示屈肘电机的分力大小，

表示所述持握点到所述旋肩电机输出轴的垂线，

表示所述持握点到所述展臂电机输出轴的垂线，

表示所述前臂。

本发明的有益效果：

本发明提供的随动型机械臂的控制方法，对于任意输入的作用力指令，通过进行空间矢量分解运算，求解出三枚电机各自需要的输出力矩大小，进而驱动电机在持握点合成出所需的反馈力，从而能够将任意方向大小的三维空间力矢量向特定施力点进行分解，由此可以实现：在持握不同形状的物体时，其机械结构所处的位置不能提前预知，但输出的夹紧力仍可以保持在一定范围内，从而实现了在特定位置施加特定大小力的效果。此外，前臂末梢的持握点可到达一定空间内的任意位置，用户手持该持握点进行运动时，该随动型机械臂能随之自动调整连杆姿态，实现随行运动，使前臂末梢在一定工作空间内跟随人手移动，并同时按系统要求向人手施加作用力。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是本发明一实施例的随动型机械臂的结构示意图；

图2是本发明另一实施例的随动型机械臂的结构示意图；

图3是本发明一实施例的随动型机械臂的控制方法逻辑流程图。

具体实施方式

为使本发明的目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。附图中给出了本发明的若干实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容更加透彻全面。

需要说明的是，当元件被称为“固设于”另一个元件，它可以直接在另一个元件上或者也可以存在居中的元件。当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件或者可能同时存在居中元件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”、“上”、“下”以及类似的表述只是为了说明的目的，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。本文所使用的术语“及／或”包括一个或多个相关的所列项目的任意的和所有的组合。

请参阅图1，本发明的一实施例提出的随动型机械臂，包括肩部组件、上臂200、前臂300。

所述肩部组件包括底座101、旋肩电机102、旋肩传动结构、展臂电机104、展臂传动结构105、屈肘电机106、屈肘传动结构107。

所述旋肩电机102、所述展臂电机104、所述屈肘电机106均配备转子角度编码器。优选的，所述旋肩电机102、所述展臂电机104、所述屈肘电机106均为直流无刷伺服电机，且均配置为力矩输出模式。

所述旋肩电机102固定在所述底座101上，所述旋肩传动结构与所述旋肩电机102连接，所述展臂电机104和所述屈肘电机106安装在所述旋肩传动结构上，所述旋肩电机102用于带动所述随动型机械臂除所述底座101和所述旋肩电机102以外的部分绕竖直的旋肩轴108旋转。旋肩电机102可带或不带减速机构，本实施例为不带减速机构的情况。

所述展臂传动结构105连接在所述展臂电机104的转子和所述屈肘电机106的定子之间，所述展臂电机104用于带动所述上臂200和所述前臂300绕水平的展臂轴109旋转。所述展臂电机104可带或不带减速机构，本实施例为不带减速机构的情况。

所述上臂200的基部与所述屈肘电机106的定子连接，所述前臂300的一端与所述上臂200铰接于肘关节400处，并与所述屈肘传动结构107连接，所述前臂300的另一端延伸至持握点。

所述屈肘电机106与所述屈肘传动结构107连接，所述屈肘传动结构107用于将所述屈肘电机106的动力输出到所述肘关节400，所述屈肘电机106用于带动所述前臂300绕水平的肘关节轴401旋转。其中，由于动力传递路线较长，所述屈肘传动结构107包括至少一级带式或链式传动机构。屈肘电机106可带或不带减速机构，本实施例为不带减速机构的情况。

所述上臂200和所述前臂300均为刚性连杆结构，当展臂电机104不带减速时，也可直接与展臂电机104转子固连。上臂200的连杆同时也提供了屈肘传动链的安装结构。

所述旋肩轴108位于旋肩传动结构的末端，沿竖直方向。当旋肩传动结构不带减速时，旋肩传动结构可简化为只有一个肩关节窝103，此时旋肩轴108即为旋肩电机102的转轴。

所述展臂轴109位于展臂传动结构105的末端，沿水平走向。当展臂传动结构105不带减速时，展臂轴109即为展臂电机104的转轴。

所述旋肩轴108和所述展臂轴109在空间的交点为所述随动型机械臂的旋转中心，在所述旋肩电机102、所述展臂电机104、所述屈肘电机106的联合带动下，所述持握点可到达一个球壳范围内的任意位置，球壳的外径等于所述上臂200与所述前臂300的长度之和，球壳内径等于所述上臂200与所述前臂300长度之差的绝对值，当使用者持握所述持握点时，所述随动型机械臂在使用者手部的移动带领下做随行运动，并同时向人手施加反力。

本实施例中，所述旋肩传动结构具体包括肩关节窝103，所述肩关节窝103用于支撑所述随动型机械臂除所述底座101和所述旋肩电机102以外的部分，本实施例中，旋肩电机102动力输出不做减速。

本实施例中，所述肩部组件还包括导电滑环110，所述导电滑环110安装于所述旋肩轴108与所述肩关节窝103的相交处，导电滑环110的功能是连通旋转体两侧的电信号，设置导电滑环110的作用是允许机械臂绕旋肩轴108做360°全方位连续旋转。

此外，在具体实施时，可以增加一些减速机构，以对上述的随动型机械臂进行改进，请参阅图2，旋肩电机102带减速机构1021，展臂电机104和屈肘电机106均带相应的减速机构。

上述随动型机械臂通过开式三连杆结构实现了前臂末梢的持握点可到达一定空间内的任意位置，用户手持该持握点进行运动时，该随动型机械臂能随之自动调整连杆姿态，实现随行运动，使前臂末梢在一定工作空间内跟随人手移动，并同时按系统要求向人手施加作用力

上述的随动型机械臂（不带减速机构或不带减速机构）的另一个重要功能是需要在持握点向用户进行力反馈，且所需作用力矢量的大小和方向由上位机实时指定，无法事先预知。对于任意输入的作用力指令，本机械臂所带的运算单元要立即进行空间矢量分解运算，求解出三枚电机各自需要的输出力矩大小，进而驱动电机在持握点合成出所需的反馈力。

该随动型机械臂在工作中需要和三个外界对象进行交互：地面（机械交互）、用户（机械交互）、上位机（信息交互）。底座和前臂末梢分别与地面（固连）和用户（持握）进行机械交互。同时机械臂还需要与上位机或其他反力指挥单元进行信息交互，接受需要生成的反馈力矢量（大小和方向）指令。机械臂自身携带的运算模块将负责把所需的反馈力实现在持握点上。

因此，基于上述的随动型机械臂（不带减速机构或不带减速机构），本发明的实施例还提供一种随动型机械臂的控制方法，该控制方法所针对的具体工况是：1）所需的输出力矢量（大小/方向/施力点）由外部通讯决定，且不能事先预知；2）动力输出机构为三枚电机（即旋肩电机102、展臂电机104和屈肘电机106），电机所在位置需由传感器实时测定，亦不能事先预知；3）在运行中将输出力矢量向三电机实时分解，由三电机的输出合成所需的空间力矢量。

请结合图3，所述控制方法具体包括步骤S101~S108。

S101，获取所述旋肩电机、所述展臂电机、所述屈肘电机的输出角。

其中，可以通过多种方式获取三个电机的输出角。具体方法取决于配套电子系统的设计方案。例如编码器，角度传感器，惯性传感器等皆可。以通过编码器测算电机转角为例，其计算公式为：

其中，a为电机实时转角，e为电机编码器实时输出。a₀和e₀分别为初始标定时电机的转角和编码器输出值，e_max为编码器最大输出，也即电机整周旋转时编码器的输出编码范围。脚标i为电机序号（1表示旋肩电机102，2表示展臂电机104，3表示屈肘电机106）。

S102，根据所述旋转中心的位置、所述旋肩电机的输出角、所述上臂的长度、所述展臂电机的输出角，计算所述肘关节的位置。

其中，具体采用下式计算所述肘关节的位置：

其中，α₁为所述旋肩电机的输出角，α₂为所述展臂电机的输出角，[x₁,y₁,z₁]为所述旋转中心的位置，L_ua为所述上臂的长度，[x₂,y₂,z₂]为所述肘关节的位置。

S103，根据所述展臂电机的输出角、所述屈肘电机的输出角计算所述前臂与水平面之间的前臂方向角。

其中，具体采用下式计算所述前臂方向角：

其中，A₃为所述前臂方向角，α₃为所述屈肘电机的输出角。

S104，根据所述肘关节的位置、所述前臂方向角、所述前臂的长度计算所述持握点的位置。

其中，具体采用下式计算所述持握点的位置：

其中，[x₃,y₃,z₃]为所述持握点的位置，L_fa为所述前臂的长度。

S105，根据所述持握点的位置计算所述持握点到所述旋肩电机、所述展臂电机、所述屈肘电机的力臂矢量。

其中，具体采用下式计算所述持握点到所述旋肩电机、所述展臂电机、所述屈肘电机的力臂矢量：

其中，

表示所述持握点到所述旋肩电机输出轴的垂线，

表示所述持握点到所述展臂电机输出轴的垂线，

表示所述前臂，向量i,j,k分别表示x,y,z方向上的单位向量，0i+0j+1k表示从（0，0，0）指向（0，0，1）的矢量；

S106，根据各个所述力臂矢量计算所述旋肩电机、所述展臂电机、所述屈肘电机向合力作用点输出分力的方向矢量。

其中，每一轴电机分力的方向垂直于该轴力臂与电机轴所组成的平面，具体采用下式计算所述旋肩电机、所述展臂电机、所述屈肘电机向合力作用点输出分力的方向矢量：

其中，

表示所述旋肩电机向合力作用点输出分力的方向矢量，

表示所述展臂电机向合力作用点输出分力的方向矢量，

表示所述屈肘电机向合力作用点输出分力的方向矢量，

表示矢量叉乘运算，

为中间变量。

需要指出的是，因为需要取单位向量，计算中可以旋肩电机分力矢量代替展臂电机、屈肘电机轴矢量，从而简化程序。

S107，根据各个所述方向矢量以及需要生成的反馈力合力矢量，计算出所述旋肩电机、所述展臂电机、所述屈肘电机的分力大小。

其中，具体采用下式计算所述旋肩电机、所述展臂电机、所述屈肘电机的分力大小：

其中，a₁、b₁、c₁表示所述旋肩电机分力方向单位向量f₁矢量在x,y,z方向上的分量，a₂、b₂、c₂表示所述展臂电机分力方向单位向量f₂矢量在x,y,z方向上的分量，a₃、b₃、c₃表示所述屈肘电机分力方向单位向量f₃矢量在x,y,z方向上的分量，

为所述反馈力合力矢量，其是从上位机经通讯协议下发的反馈力指令中读取出来的，F₁表示旋肩电机的分力大小，F₂表示展臂电机的分力大小，F₃表示屈肘电机的分力大小。

S108，根据各个所述分力大小计算所述旋肩电机、所述展臂电机、所述屈肘电机的输出力矩。

其中，具体采用下式计算所述旋肩电机、所述展臂电机、所述屈肘电机的输出力矩：

或

其中，i=1,2,3，分别表示所述旋肩电机102、所述展臂电机104、所述屈肘电机106，M_i表示相应电机的输出力矩，N_i表示相应电机的传动减速比，对于不含减速机构的电机，传动减速比等于1。

根据上述的控制方法，对于任意输入的作用力指令，通过进行空间矢量分解运算，求解出三枚电机各自需要的输出力矩大小，进而驱动电机在持握点合成出所需的反馈力，从而能够将任意方向大小的三维空间力矢量向特定施力点进行分解，由此可以实现：在持握不同形状的物体时，其机械结构所处的位置不能提前预知，但输出的夹紧力仍可以保持在一定范围内。

具体实施时，该随动型机械臂的前臂末梢（即持握点）由使用者持握，机械臂在使用者手部的移动带领下做随行运动，并同时向人手施加反力。移动中机械臂不断与上位机进行双工通讯，由上位机下发在机械臂末梢需模拟出的反力大小和方向。机械臂配备三枚执行电机，各电机皆配备编码器，且配置为力矩输出模式。机械臂在工作中从上位机指令中实时获取反力合力矢量，并通过电机编码器获得电机角度，并按照上述方法计算各轴电机力矩。实际系统中上述算法实现于STM32嵌入式系统，单次运算小于1ms，与上位机通讯频率可达1khz。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (4)

1.一种随动型机械臂的控制方法，其特征在于，所述随动型机械臂包括肩部组件、上臂、前臂；所述肩部组件包括底座、旋肩电机、旋肩传动结构、展臂电机、展臂传动结构、屈肘电机、屈肘传动结构，所述旋肩电机、所述展臂电机、所述屈肘电机均配备转子角度编码器；

所述旋肩电机固定在所述底座上，所述旋肩传动结构与所述旋肩电机连接，所述展臂电机和所述屈肘电机安装在所述旋肩传动结构上，所述旋肩电机用于带动所述随动型机械臂除所述底座和所述旋肩电机以外的部分绕竖直的旋肩轴旋转；

所述展臂传动结构连接在所述展臂电机的转子和所述屈肘电机的定子之间，所述展臂电机用于带动所述上臂和所述前臂绕水平的展臂轴旋转；

所述上臂的基部与所述屈肘电机的定子连接，所述前臂的一端与所述上臂铰接于肘关节处，并与所述屈肘传动结构连接，所述前臂的另一端延伸至持握点；

所述屈肘电机与所述屈肘传动结构连接，所述屈肘传动结构用于将所述屈肘电机的动力输出到所述肘关节，所述屈肘电机用于带动所述前臂绕水平的肘关节轴旋转；

所述旋肩轴和所述展臂轴在空间的交点为所述随动型机械臂的旋转中心，在所述旋肩电机、所述展臂电机、所述屈肘电机的联合带动下，所述持握点可到达一个球壳范围内的任意位置，球壳的外径等于所述上臂与所述前臂的长度之和，球壳内径等于所述上臂与所述前臂长度之差的绝对值，当使用者持握所述持握点时，所述随动型机械臂在使用者手部的移动带领下做随行运动，并同时向人手施加反力；

所述控制方法包括：

获取所述旋肩电机、所述展臂电机、所述屈肘电机的输出角；

根据所述旋转中心的位置、所述旋肩电机的输出角、所述上臂的长度、所述展臂电机的输出角，计算所述肘关节的位置；

根据所述展臂电机的输出角、所述屈肘电机的输出角计算所述前臂与水平面之间的前臂方向角；

根据所述肘关节的位置、所述前臂方向角、所述前臂的长度计算所述持握点的位置；

根据所述持握点的位置计算所述持握点到所述旋肩电机、所述展臂电机、所述屈肘电机的力臂矢量；

根据各个所述力臂矢量计算所述旋肩电机、所述展臂电机、所述屈肘电机向合力作用点输出分力的方向矢量；

根据各个所述方向矢量以及需要生成的反馈力合力矢量，计算出所述旋肩电机、所述展臂电机、所述屈肘电机的分力大小；

根据各个所述分力大小计算所述旋肩电机、所述展臂电机、所述屈肘电机的输出力矩。

2.根据权利要求1所述的随动型机械臂的控制方法，其特征在于，根据所述旋转中心的位置、所述旋肩电机的输出角、所述上臂的长度、所述展臂电机的输出角，计算所述肘关节的位置的步骤中，采用下式计算所述肘关节的位置：

其中，α₁为所述旋肩电机的输出角，α₂为所述展臂电机的输出角，[x₁,y₁,z₁]为所述旋转中心的位置，L_ua为所述上臂的长度，[x₂,y₂,z₂]为所述肘关节的位置；

根据所述展臂电机的输出角、所述屈肘电机的输出角计算所述前臂与水平面之间的前臂方向角的步骤中，采用下式计算所述前臂方向角：

其中，A₃为所述前臂方向角，α₃为所述屈肘电机的输出角。

3.根据权利要求2所述的随动型机械臂的控制方法，其特征在于：

根据所述肘关节的位置、所述前臂方向角、所述前臂的长度计算所述持握点的位置的步骤中，采用下式计算所述持握点的位置：

其中，[x3,y3,z3]为所述持握点的位置，L_fa为所述前臂的长度；

根据所述持握点的位置计算所述持握点到所述旋肩电机、所述展臂电机、所述屈肘电机的力臂矢量的步骤中，采用下式计算所述持握点到所述旋肩电机、所述展臂电机、所述屈肘电机的力臂矢量：

其中，

表示所述持握点到所述旋肩电机的力臂矢量，

表示所述持握点到所述展臂电机的力臂矢量，

表示所述持握点到所述屈肘电机的力臂矢量，向量

,

,

分别表示x,y,z方向上的单位向量；

根据各个所述力臂矢量计算所述旋肩电机、所述展臂电机、所述屈肘电机向合力作用点输出分力的方向矢量的步骤中，采用下式计算所述旋肩电机、所述展臂电机、所述屈肘电机向合力作用点输出分力的方向矢量：

其中，

表示所述旋肩电机向合力作用点输出分力的方向矢量，

表示所述展臂电机向合力作用点输出分力的方向矢量，

表示所述屈肘电机向合力作用点输出分力的方向矢量，

表示矢量叉乘运算，

为中间变量；

根据各个所述方向矢量以及需要生成的反馈力合力矢量，计算出所述旋肩电机、所述展臂电机、所述屈肘电机的分力大小的步骤中，采用下式计算所述旋肩电机、所述展臂电机、所述屈肘电机的分力大小：

其中，a₁、b₁、c₁表示所述旋肩电机分力方向单位向量f₁矢量在x,y,z方向上的分量，a₂、b₂、c₂表示所述展臂电机分力方向单位向量f₂矢量在x,y,z方向上的分量，a₃、b₃、c₃表示所述屈肘电机分力方向单位向量f₃矢量在x,y,z方向上的分量，

为所述反馈力合力矢量，F₁表示旋肩电机的分力大小，F₂表示展臂电机的分力大小，F₃表示屈肘电机的分力大小。

4.根据权利要求3所述的随动型机械臂的控制方法，其特征在于，根据各个所述分力大小计算所述旋肩电机、所述展臂电机、所述屈肘电机的输出力矩的步骤中，采用下式计算所述旋肩电机、所述展臂电机、所述屈肘电机的输出力矩：

或

其中，i=1,2,3，M₁表示旋肩电机的输出力矩，M₂表示展臂电机的输出力矩，M₃表示屈肘电机的输出力矩，N₁表示旋肩电机的传动减速比，N₂表示展臂电机的传动减速比，N₃表示屈肘电机的传动减速比，F₁表示旋肩电机的分力大小，F₂表示展臂电机的分力大小，F₃表示屈肘电机的分力大小，

表示所述持握点到所述旋肩电机的力臂矢量，

表示所述持握点到所述展臂电机的力臂矢量，

表示所述持握点到所述屈肘电机的力臂矢量。

Images