CN114290331B - 机器人运动控制方法、机器人及计算机可读存储装置 - Google Patents

Abstract

本申请公开了一种机器人的运动控制方法。该方法包括：确定第一目标位置；从最后一连杆开始，根据第一目标位置，依序确定每一连杆相对于前一关节的旋转角度；从最前一连杆开始，依序确定每个连杆按照旋转角度旋转后对应的关节位置，以确定最后一连杆的末端的第二目标位置；根据第一目标位置和第二目标位置，对机器人进行运动控制。本申请还公开了一种机器人以及一种计算机解读存储装置。通过上述方式，本申请能够快速准确的实现机器人运动的逆向运动计算。

Description

机器人运动控制方法、机器人及计算机可读存储装置

技术领域

本申请涉及控制领域，特别是涉及一种机器人运动控制方法、机器人及计算机可读存储装置。

背景技术

随着科学技术的不断发展，机器人在生活中的应用变得越来越广泛，小到清扫垃圾、快递分拣，大到航空航天，人工智能，机器人已经进入人类生活的方方面面。而有关机器人活动的机器人运动学，是对于研究机器人来说不可或缺的重要研究领域。机器人运动学包括正向运动学和逆向运动学。正向运动学即给定机器人各个关节位置的变量，以此来计算机器人末端的位置姿态。而逆向运动学即已知机器人的末端位置姿态，求解计算机器人对应位置的全部的关节变量。

一般正向运动学的结果是唯一的，并且容易获得，而逆向运动学往往有多个结果，而且分析过程也更为复杂。机器人的逆向运动分析是运动规划不受控制中的重要问题，但是由于机器人逆向运动问题的复杂性和多样性，无法建立起通用的解析算法。逆向运动学的求解问题实际上是一个复杂的非线性超越方程组的求解问题，其包括解的存在性、唯一性及求解的方法等一系列复杂问题。

发明内容

本申请主要目的是提供机器人运动控制方法，机器人以及计算机可读存储装置，能够解决机器人逆向运动学的求解问题。

为解决上述技术问题，本申请采用的第一个技术方案是：提供一种机器人的运动控制方法。该机器人包括依序连接的多个连杆，每一连杆可相对于连接前一连杆的关节旋转，该方法包括：确定第一目标位置；从最后一连杆开始，根据第一目标位置，依序确定每一连杆相对于前一关节的旋转角度；从最前一连杆开始，依序确定每个连杆按照旋转角度旋转后对应的关节位置，以确定最后一连杆的末端的第二目标位置；根据第一目标位置和第二目标位置，对机器人进行运动控制。

为解决上述技术问题，本申请采用的第二个技术方案是：提供一种机器人。该机器人包括存储器和处理器，存储器用于存储程序数据，程序数据能够被处理器执行，以实现第一个技术方案中的方法。

为解决上述技术问题，本申请采用的第三个技术方案是：提供一种计算机可读存储装置，存储有程序数据，能够被处理器执行，以实现第一个技术方案中的方法。

本申请的有益效果是：区别于现有技术的情况，本申请通过关节的运动前后的位置确定关节运动的目标向量，将该目标向量进行分解计算，再针对分向量进行处理求得前一关节的运动后的最终位置，进而根据前一关节运动前后的位置确定前一关节运动的目标向量，再次求得更前一关节的运动后的最终位置，依次递进。而根据关节运动前后的位置和其前一关节运动前的位置可求得该两关节之间连杆的旋转角度，根据正向运动学验证该旋转角度是否正确并指示机器人运动。本申请中的各个计算都依照最终的要求的计算结果得到的，各个计算参数之间层层递进，相互联系，并不需要单独对每个关节进行独立参数设定和计算，无需设定大量的参数，从而在保证计算结果准确性的同时减少计算失误和计算的复杂度。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请一机器人关节结构示意图；

图2是本申请关节旋转时旋转向量的示意图；

图3是本申请机器人的运动控制方法第一实施例的流程示意图；

图4是本申请机器人的运动控制方法第二实施例的流程示意图；

图5是本申请机器人的运动控制方法第三实施例的流程示意图；

图6是本申请机器人的运动控制方法第四实施例的流程示意图；

图7是本申请计算旋转角度过程一示意图；

图8是本申请计算旋转角度过程又一示意图；

图9是本申请机器人的运动控制方法第五实施例的流程示意图；

图10是本申请机器人的运动控制方法第六实施例的流程示意图；

图11是本申请机器人一实施例的结构示意图；

图12是本申请计算机可读存储装置一实施例的结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

本申请中的术语“第一”、“第二”等是用于区别不同对象，而不是用于描述特定顺序。此外，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

在详细说明本申请之前，先介绍一下与本申请相关的基本概念。

机器人运动学包括有正向运动学和逆向运动学。正向运动学相较于逆向运动学来说更为简单。机器人正向运动学的求解通常使用D-H参数法和指数积方法。1955年，Denavit和Hartenberg在“ASME Journal of Applied Mechanics”期刊发表了一篇论文，后来的机器人表示和建模就依据了这篇论文，成为表示机器人和机器人运动进行建模的标准方法，被称为Denavit-Hartenberg参数模型(Denavit-Hartenberg parameters)，简称D-H模型。D-H模型表示了对机器人连杆和关节进行建模的一种非常简单的方法，对任何机器人构型都适用，无论机器人的结构顺序和复杂程度如何。另一种正向运动学求解方法是基于旋量理论的指数积(Product of Exponential,POE)方法。POE公式直接来自刚体运动的指数坐标表示，它无须建立连杆坐标系，只有基坐标系和末端坐标系是必需的，且它们可以任意选择。

在使用D-H模型进行机器人的正向运动学计算时，首先需要对每个关节确定D-H坐标系，然后再确定每个关节对应的四个D-H参数，并且将所有关节的D-H参数都确定下来，建立D-H参数表，在进行正向运动学计算时根据D-H参数表代入公式进行计算。D-H参数法是机器人运动学求解的经典方法，但该方法与机器人的构型有关，对不同结构的机器人所建立的机器人运动学方程不同，即便同一结构类型的机器人，坐标系建立的不同，对应的D-H是也不一样，运动学方程也不一样，这也使得在实际使用时容易发生错误，其容错率太低，任何一个D-H参数的确定错误都将导致计算结果的错误。并且对于D-H参数法建立的机器人运动学，所有运动参数都是关于x和z轴，无法表示关于y轴的运动，不能非常直观地表达运动学的关系。

旋量理论方法POE在计算时更接近刚体运动的物理本质，其无需建立连杆坐标系，也不需要区分旋转和移动关节，但与D-H方法相比使用了更多的参数。

而本申请中使用的正向运动学的计算方法是一种新的思路，本申请通过逐一计算各关节在空间中绕旋转轴旋转后的坐标来推算末端的位置。这种旋转是使用四元数来进行表达的。相应地，本申请也使用四元数的逆向运动学的求解方法，按照倒序从末端关节到第一个关节逐一计算应该旋转的角度，更新每一个关节的角度，逐渐让末端逼近目标点。本申请的示例中演示的是二维平面内的运动学求解思路。

四元数，是一种特殊的复数。一个四元数包括一个实部和三个虚部。即q＝a+x·i+y·j+z·k，其中a是实部，x、y、z为虚部，i、j、k为虚部单位。通过四元数计算位置运动的过程示例如下。

假设空间中点P(p₁,p₂,p₃)绕轴

旋转θ角度，那么旋转后的四元数为p'＝q·p·q^-1，旋转轴/>

表示为起点是坐标原点的向量。其中，p为空间中的点P的坐标构成的四元数，实部为0，虚部为点P的坐标。

p＝0+p₁·i+p₂·j+p₃·k

q为轴

在三个方向的分量和旋转角度θ构成的四元数。

旋转后点P的新位置P'就是旋转后四元数p'的虚部。

下面结合附图对本申请求解正向运动学和逆向运动学进行进一步详细的说明。如图1所示，图1为一机器人的示意图。J为该机器人的各个关节，关节之间的连线为连接各个关节的连杆，可自由转动。每个关节的旋转动作即作为绕轴向量旋转一定角度的过程。而轴向量表示起点为坐标原点的向量，引入轴向量上的一个参考点来描述旋转向量，如图2所示。旋转向量

O_i代表起点为J_i并且与J_i转动平面相垂直的向量终端上的某点坐标。最后一个关节由于不进行旋转，所以没有O_n。已知关节位置，各个关节的旋转向量，各个关节的旋转角度，即可以得到各个关节转动指定角度后的新的位置。

而逆向运动学求解时，根据各个关节的位置以及最后一个关节的最终位置，得到各个关节运动的旋转角度，进而根据正向运动学判断该求解是否正确，从而得到逆向运动学的正确求解。该逆向运动求解的过程通过以下实施例进一步的详细描述。

如图3所示，图3为本申请机器人的运动控制方法第一实施例的流程示意图。该机器人包括依序连接的多个连杆，每一个连杆可相对于连接前一连杆的关节旋转。该方法包括以下步骤：

S11：确定第一目标位置。

确定最后一个关节运动完成之后的最终位置。该位置即为第一目标位置。

S12：从最后一连杆开始，根据第一目标位置，依序确定每一连杆相对于前一关节的旋转角度。

根据确定的第一目标位置和最后一个关节的前一关节的位置，确定最后一个关节与其前一关节的之间的连杆的旋转角度，根据该旋转角度和更前一关节的位置，即可得到前一关节与更前一关节之间的连杆的旋转角度，以此类推，即可得到所有关节的旋转角度。详细的过程通过以下的实施例进行描述。

S13：从最前一连杆开始，依序确定每个连杆按照旋转角度旋转后对应的关节位置，以确定最后一连杆的末端的第二目标位置。

在得到了各个关节的旋转角度之后，通过正向运动学，使用该旋转角度进行计算，以得到基于该旋转角度进行运动后的最后一个关节的位置，即第二目标位置。

S14：根据第一目标位置和第二目标位置，对机器人进行运动控制。

判断第一目标位置和第二目标位置，根据第一目标位置和第二目标位置的偏差大小，对机器人进行相应的操作处理。

如图4所示，图4为本申请机器人的运动控制方法第二实施例的流程示意图。该方法是对步骤S12的进一步扩展，其包括以下步骤：

S21：从最后一连杆开始，依序确定每一连杆的终点位置。

终点位置为每一连杆的末端将要移动的位置。在一实施例中，确定最后一连杆的终点位置时，根据最后关节的前一关节的位置可得到最后关节与其前一关节之间的连杆的旋转角度。在根据连杆长度可得该前一关节的最终位置。再根据得到的前一关节的最终位置计算前一关节与更前一关节之间的连杆的旋转角度，进而再得到更前一关节的最终位置。最终依序可求得所有连杆的旋转角度以及各个关节的最终位置。在另一实施例中，根据连杆末端运动前后的位置确定运动向量，对运动向量进行分解、平移，也可求的前一关节的最终位置。该确定连杆最终位置的过程可通过以下实施例实现。

S22：根据每一连杆的首端位置和末端位置的连线、和首端位置与终点位置的连线之间的夹角，确定每一连杆相对于前一关节的旋转角度。

每一连杆首端运动之前的位置与分别与末端运动前的位置和末端运动后的最终位置的两条连线的夹角，即为该连杆的旋转角度。

如图5所示，图5为本申请机器人的运动控制方法第三实施例的流程示意图。该方法是对步骤S21的进一步扩展，其包括以下步骤：

S31：确定最后一连杆的终点位置为所第一目标位置。

将连杆的末端的运动后的位置，即终点位置作为第一目标位置。

S32：根据最后一连杆的末端位置和终点位置确定目标向量。

根据连杆的末端运动前后的位置确定目标向量，目标向量由末端运动前的位置指向末端运动后的位置。

S33：将目标向量分解为相互垂直的第一向量和第二向量。

将目标向量进行垂直分解，分解为第一向量和第二向量。第一向量垂直于连杆的首端位置和末端终点位置，即末端运动后位置的连线。

S34：根据第二向量确定前一连杆的终点位置。

根据分解得到的第二向量计算得到连杆首端运动后的位置，也即前一连杆的末端的运动后的位置。

如图6所示，图6为本申请机器人的运动控制方法第四实施例的流程示意图。该方法是对步骤S34的进一步扩展，其包括以下步骤：

S41：确定以前一连杆的末端位置为起点的新的目标向量。

新的目标向量的大小和方向与第二向量相同。也可以认为，将第二向量平移至前一连杆的末端位置。由于第一向量与第二向量垂直，而第一向量是垂直与连杆的首端位置和末端终点位置，即末端运动后位置的连线的，因此第二向量是位于连杆的首端位置和末端终点位置的连线上的。

S42：将新的目标向量的终点作为前一连杆的终点位置。

将第二向量平移至前一连杆的末端位置后，将第二向量的向量终点作为前一连杆的末端的终点位置，以用于后续的旋转角度的计算。

如图7所示，图7为旋转角度计算过程一示意图。其中，关节J_i为最后一个关节，J_i-1为前一个关节，此时J_i就是末端关节。为了便于描述，引入A、B，A是

向量在/>

上的投影点，B是/>

分解的向量在垂直与/>

上的投影点，则/>

向量分解的两个向量为/>

和/>

即

和/>

而θ_i-1为/>

和/>

的夹角，也就是最后一连杆的旋转角度。计算倒数第二个关节的旋转角度时，将分解的/>

平移至J_i-1关节处，而此时/>

指向的终点位置即为J_i-1关节运动的最终位置。将/>

再次分解为/>

和/>

是垂直于关节J_i-2和/>

指向的终点位置之间的连线，进而可得关节J_i-2与关节J_i-1运动前后位置的连线之间的夹角θ_i-2，其就是关节J_i-2与关节J_i-1之间连杆的旋转角度。

进一步参照图8，为了便于描述，引入P和J_i'，J_i'为与J_i对应的

向量方向上的点，两点至J_i-1的距离相等，P为旋转角度的角平分线上一点。而旋转角度的计算可通过以下的公式获取：

如图9所示，图9为本申请机器人的运动控制方法第五实施例的流程示意图。该方法是对步骤S13的进一步扩展，其包括以下步骤：

S51：确定每一关节的初始位。

S52：确定每一关节的旋转向量。

在一实施例中，采用以下公式确定每一关节的旋转向量：

其中，J_i表示对应关节的初始位置，O_i表示起点为J_i并且与J_i转动平面相垂直的向量终端上的一点坐标。

可参照图1和图2，J为各个关节，关节之间为连接各个关节的连杆，可自由转动。J即为每一关节运动的初始位置，而每个关节的旋转动作即认为以关节为节点绕轴向量旋转一定角度的过程。轴向量表示起点为坐标原点的向量，进而引入轴向量上的一个参考点来描述旋转向量。设定一点O_i，即可得旋转向量

S53：从最前一连杆开始，依序根据每一关节的初始位置、每一关节对应的旋转向量、每一关节对应的旋转角度，确定每一关节的终止位置，以确定最后一连杆的末端的第二目标位置。

在一实施例中，采用以下公式确定每一关节的终止位置：

J_i'＝q·J_i·q^-1

其中，J_i为初始位置，J_i'为终止位置，q为基于所述旋转向量的三个分量和旋转角度θ确定的四元数，i²＝j²＝k²＝-1，m，n，l为系数。

以一例子来更直观地说明本申请中确定关节最终位置的计算过程。

假设，将P(1,0,1)绕轴向量

旋转90°，则旋转后得到的P'坐标的计算如下：

p＝0+1·i+0·j+1·k＝i+k

因此，旋转后的P'坐标为新的四元数p'的虚部，即(1，0，-1)。

如图10所示，图10为本申请机器人的运动控制方法第六实施例的流程示意图。该方法是对步骤S14的进一步扩展，其包括以下步骤：

S61：确定第一目标位置。

S62：从最后一连杆开始，根据第一目标位置，依序确定每一连杆相对于前一关节的旋转角度。

S63：从最前一连杆开始，依序确定每个连杆按照旋转角度旋转后对应的关节位置，以确定最后一连杆的末端的第二目标位置。

以上步骤的实现可参照上述实施例所描述的方法实现，在此不再赘述。

S64：判断第一位置和第二位置之间的距离是否小于设定阈值。

若是，则执行步骤S65。

S65：根据确定的旋转角度对机器人进行运动控制。

否则执行步骤S62。即，重新执行从最后一连杆开始，根据第一目标位置，依序确定每一连杆相对于前一关节的旋转角度，以及从最前一连杆开始，依序确定每个连杆按照旋转角度旋转后对应的关节位置，以确定最后一连杆的末端的第二目标位置的步骤。

在根据要求的最后一连杆的最终位置求得各个关节的旋转角度后，根据本申请中的正向运动学重新计算各个关节的最终位置，若求得的最后关节的最终位置与要求的最终位置之间的距离小于设定阈值，即表明最后关节的位置在要求的最终位置的较小的邻域范围内，可视为计算完成，进而依照计算的各个关节的旋转角度对机器人进行运动指示。而若求得的最后关节的最终位置与要求的最终位置之间的距离不小于设定阈值，即表明最后关机的最终位置没有落在要求的最终位置的邻域范围内，视为计算错误，重新进行旋转角度的求解以及最终位置的误差判断，直至最后关节的最终位置与要求的最终位置之间的距离小于设定阈值。

如图11所示，图11为本申请机器人一实施例的结构示意图。

该机器人包括处理器110和存储器120。

处理器110控制机器人的操作，处理器110还可以称为CPU(Central ProcessingUnit，中央处理单元)。处理器110可能是一种集成电路芯片，具有信号序列的处理能力。处理器110还可以是通用处理器、数字信号序列处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器120存储处理器110工作所需要的指令和程序数据。

处理器110用于执行指令以实现本申请前述机器人的运动控制方法的任一实施例及可能的组合所提供的方法。

一实施例中，该机器人还可包括关节类结构，与处理器110连接，以进一步实现上述的机器人的运动控制方法及可能的组合。

如图12所示，图12为本申请计算机可读存储装置一实施例的结构示意图。

本申请可读存储装置一实施例包括存储器210，存储器210存储有程序数据，该程序数据被执行时实现本申请机器人的运动控制方法任一实施例及可能的组合所提供的方法。

存储器210可以包括只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、闪存(Flash Memory)、硬盘、光盘等。

综上所述，本申请通过关节的运动前后的位置确定关节运动的目标向量，将该目标向量进行分解计算，再针对分向量进行处理求得前一关节的运动后的最终位置，进而根据前一关节运动前后的位置确定前一关节运动的目标向量，再次求得更前一关节的运动后的最终位置，依次递进。而根据关节运动前后的位置和其前一关节运动前的位置可求得该两关节之间连杆的旋转角度，根据正向运动学验证该旋转角度是否正确并指示机器人运动。本申请中的各个计算都依照最终的要求的计算结果得到的，各个计算参数之间层层递进，相互联系，并不需要单独对每个关节进行独立参数设定和计算，无需设定大量的参数，从而在保证计算结果准确性的同时减少计算失误和计算的复杂度。

在本申请所提供的几个实施方式中，应该理解到，所揭露的方法以及设备，可以通过其它的方式实现。例如，以上所描述的设备实施方式仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。

另外，在本申请各个实施方式中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

上述其他实施方式中的集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本申请的实施例，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (8)

1.一种机器人的运动控制方法，其特征在于，所述机器人包括依序连接的多个连杆，每一连杆可相对于连接前一连杆的关节旋转，所述方法包括：

确定第一目标位置；

从最后一连杆开始，确定最后一连杆的终点位置为所述第一目标位置；

根据最后一连杆的末端位置和终点位置确定目标向量；

将所述目标向量分解为相互垂直的第一向量和第二向量，所述第一向量垂直于最后一连杆的首端位置和终点位置的连线；

根据所述第二向量确定前一连杆的终点位置，所述终点位置为每一连杆的末端将要移动的位置；

根据每一连杆的首端位置和末端位置的连线、和首端位置与终点位置的连线之间的夹角，确定每一连杆相对于前一关节的旋转角度；

从最前一连杆开始，依序确定每个连杆按照所述旋转角度旋转后对应的关节位置，以确定最后一连杆的末端的第二目标位置；

根据所述第一目标位置和所述第二目标位置，对所述机器人进行运动控制。

2.根据权利要求1所述的方法，其特征在于，

所述根据所述第二向量确定前一连杆的终点位置，包括：

确定以前一连杆的末端位置为起点的新的目标向量，所述新的目标向量的大小和方向与所述第二向量相同；

将所述新的目标向量的终点作为前一连杆的终点位置。

3.根据权利要求1所述的方法，其特征在于，

所述从最前一连杆开始，依序确定每个连杆按照所述旋转角度旋转后对应的关节位置，以确定最后一连杆的末端的第二目标位置，包括：

确定每一关节的初始位置；以及

确定每一关节的旋转向量；

从最前一连杆开始，依序根据所述每一关节的初始位置、每一关节对应的旋转向量、每一关节对应的旋转角度，确定每一关节的终止位置，以确定最后一连杆的末端的第二目标位置。

4.根据权利要求3所述的方法，其特征在于，

所述确定每一关节的旋转向量，包括：

采用以下公式确定每一关节的旋转向量：

其中，J_i表示对应关节的初始位置，O_i表示起点为J_i并且与J_i转动平面相垂直的向量终端上的一点坐标。

5.根据权利要求3所述的方法，其特征在于，

所述根据所述每一关节的初始位置、每一关节对应的旋转向量、每一关节对应的旋转角度，确定每一关节的终止位置，包括：

J_i'＝q·J_i·q^-1；

其中，J_i为初始位置，J_i'为终止位置，q为基于所述旋转向量的三个分量和旋转角度θ确定的四元数，i²＝j²＝k²＝-1，m，n，l为系数。

6.根据权利要求1所述的方法，其特征在于，

所述根据所述第一目标位置和所述第二目标位置，对所述机器人进行运动控制，包括：

若所述第一目标位置和所述第二目标位置之间的距离小于设定阈值，则根据确定的旋转角度对所述机器人进行运动控制，或

若所述第一目标位置和所述第二目标位置之间的距离大于所述设定阈值，则执行所述从最后一连杆开始，根据所述第一目标位置，依序确定每一连杆相对于前一关节的旋转角度，以及所述从最前一连杆开始，依序确定每个连杆按照所述旋转角度旋转后对应的关节位置，以确定最后一连杆的末端的第二目标位置的步骤。

7.一种机器人，其特征在于，包括存储器和处理器，所述存储器用于存储程序数据，所述程序数据能够被所述处理器执行，以实现如权利要求1-6中任一项所述的方法。

8.一种计算机可读存储装置，其特征在于，存储有程序数据，能够被处理器执行，以实现如权利要求1-6中任一项所述的方法。

Images