CN113263498B - 机械臂移动控制方法、装置、终端设备及存储介质 - Google Patents

Abstract

本申请提供了一种机械臂移动控制方法、装置、终端设备及存储介质，该方法包括：接收针对机械臂的姿态控制信息，以及根据机械臂的基座的姿态对姿态控制信息进行姿态补偿，得到姿态增量信息；根据姿态增量信息对机械臂的目标夹具的当前姿态进行姿态计算，得到目标姿态；计算目标夹具在目标姿态下，机械臂的目标关节信息；根据目标关节信息对机械臂上的各关节部位进行姿态调节。本申请通过计算目标夹具在目标姿态下机械臂的目标关节信息，并根据目标关节信息对机械臂上的关节部位进行姿态调节，基于对关节部位的姿态调节，能有效地将目标夹具调节为对应的目标姿态，无需采用人工的方式多次进行关节部位的调节，提高了机械臂移动控制的效率。

Description

机械臂移动控制方法、装置、终端设备及存储介质

技术领域

本申请涉及机器人技术领域，尤其涉及一种机械臂移动控制方法、装置、终端设备及存储介质。

背景技术

随着时代的发展和科技的进步，机械臂逐渐代替人工完成某些单调、频繁和重复的长时间的作业任务。实际应用中，机械臂通常是按照设定的程序或轨迹执行监控、抓取、搬运等工作。

现有的机械臂使用过程中，需要通过对机械臂上的夹具进行移动控制，以达到抓取或搬运物品等效果，但在目前的安防排爆行业、供配电行业使用的机械手臂，通常需要通过人工多次调节机械臂上的各关节部位的位置和角度，以达到控制机械臂的夹具移动至目标位置的效果。这种通过人工调节的方式，操作繁琐，导致降低了对机械臂进行移动控制的效率。

发明内容

有鉴于此，本申请实施例提供了一种机械臂移动控制方法、装置、终端设备及存储介质，以解决现有技术中，由于需要通过人工多次调节机械臂上部位关节的位置和角度，所导致的机械臂移动控制效率低下的问题。

本申请实施例的第一方面提供了一种机械臂移动控制方法，包括：

接收针对机械臂的姿态控制信息，以及根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿，得到姿态增量信息，所述姿态补偿用于根据所述基座的姿态对所述姿态控制信息进行信息校准；

根据所述姿态增量信息对所述机械臂的目标夹具的当前姿态进行姿态计算，得到目标姿态；

计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息，所述目标关节信息用于指示所述机械臂的各关节部位的目标关节姿态；

根据所述目标关节信息对所述机械臂上的各关节部位进行姿态调节。

进一步地，所述姿态控制信息包括位置控制信息和角度控制信息，所述根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿所采用的计算公式包括：

其中，(x，y，z)是所述基座的基座坐标系中的坐标参数，ΔT_xyz是所述位置控制信息在所述基座坐标系中x轴、y轴和z轴下的位置增量组成的向量，R_rot是所述基座的姿态中俯仰角度和横滚角度经过绕轴旋转公式计算后的旋转结果矩阵,

是R_rot的转置，ΔT′_xyz是所述姿态增量信息中的位置增量信息，所述位置增量信息为所述姿态增量信息在x轴、y轴和z轴下的位置增量；

其中，R是滚动角，P是俯仰角，Y是偏航角，ΔT_RPY是所述姿态控制信息中滚动角、俯仰角和偏航角对应角度增量的三阶矩阵，ΔT′_RPY是所述角度增量信息，所述角度增量信息为所述姿态增量信息中滚动角、俯仰角和偏航角对应的三阶矩阵。

进一步地，所述接收针对机械臂的姿态控制信息之后，还包括：

若所述姿态控制信息中任一位置增量小于第一增量阈值，则在所述姿态控制信息中删除所述位置增量；

若所述姿态控制信息中任一角度增量小于第二增量阈值，则在所述姿态控制信息中删除所述角度增量。

进一步地，所述目标关节姿态包括目标角度和目标位置，所述计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息之后，还包括：

分别判断不同所述关节部位是否能转动至对应的所述目标角度，及是否能移动至对应的所述目标位置；

若任一所述关节部位不能转动至对应的所述目标角度，和/或不能移动至对应的所述目标位置，则根据所述关节部位的部位标识发送姿态调节错误提示。

进一步地，所述分别判断不同所述关节部位是否能转动至对应的所述目标角度，及是否能移动至对应的所述目标位置之后，还包括：

分别获取所述关节部位转动至对应的所述目标角度，及移动至对应的所述目标位置下的姿态调节轨迹，并检测所述姿态调节轨迹中是否存在障碍物；

若所述姿态调节轨迹中存在所述障碍物，则对所述姿态调节轨迹对应的所述关节部位发送姿态调节错误提示。

进一步地，所述计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息，包括：

计算所述目标夹具在当前姿态下，所述机械臂的雅克比矩阵，并计算所述雅克比矩阵与所述雅克比矩阵的转置的乘积，得到乘积结果；

计算所述乘积结果的行列式，根据所述行列式计算所述雅克比矩阵的逆矩阵，并根据所述逆矩阵求解角度差，得到偏移角度；

根据所述偏移角度分别对所述关节部位的关节角度进行角度更新，并根据角度更新后的所述关节部位的关节角度对所述当前姿态进行姿态更新；

若姿态更新后的所述当前姿态与所述目标姿态之间的姿态差值大于差值阈值，则将姿态更新后的所述当前姿态，返回执行所述计算所述目标夹具在当前姿态下，所述机械臂的雅克比矩阵的步骤及后续步骤；

若姿态更新后的所述当前姿态与所述目标姿态的姿态差值小于或等于差值阈值，则将所述目标夹具在姿态更新后所述当前姿态下，所述机械臂的关节信息设置所述目标关节信息。

进一步地，所述根据角度更新后的所述关节部位的关节角度对所述当前姿态进行姿态更新之后，还包括：

若所述当前姿态的姿态更新次数大于次数阈值，则判定所述目标夹具不能调节至所述目标姿态，并输出机械臂移动控制错误提示。

本申请实施例的第二方面提供了一种机械臂移动控制装置，包括：

姿态补偿单元，用于接收针对机械臂的姿态控制信息，以及根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿，得到姿态增量信息，所述姿态补偿用于根据所述基座的姿态对所述姿态控制信息进行信息校准；

姿态计算单元，用于根据所述姿态增量信息对所述机械臂的目标夹具的当前姿态进行姿态计算，得到目标姿态；

关节信息计算单元，用于计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息，所述目标关节信息用于指示所述机械臂的各关节部位的目标关节姿态；

姿态调节单元，用于根据所述目标关节信息对所述机械臂上的各关节部位进行姿态调节。

本申请实施例的第三方面提供了一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在终端设备上运行的计算机程序，所述处理器执行所述计算机程序时实现第一方案提供的机械臂移动控制方法的各步骤。

本申请实施例的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现第一方案提供的机械臂移动控制方法的各步骤。

实施本申请实施例提供的一种机械臂移动控制方法、装置、终端设备及存储介质具有以下有益效果：通过根据机械臂上基座的姿态对姿态控制信息进行姿态补偿，防止了由于基座与地面之间的角度或位置偏差，所导致的机械臂移动控制准确性低下的现象，通过根据姿态增量信息对机械臂上目标夹具的当前姿态进行姿态增量处理，能有效地计算到用户对目标夹具所期望的目标姿态，通过计算目标夹具在目标姿态下机械臂的目标关节信息，并根据目标关节信息对机械臂上的关节部位进行姿态调节，基于对关节部位的姿态调节，能有效地将目标夹具调节为对应的目标姿态，无需采用人工的方式多次进行关节部位的调节，提高了机械臂移动控制的效率。

附图说明

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

图1是本申请实施例提供的一种机械臂移动控制方法的实现流程图；

图2是本申请另一实施例提供的一种机械臂移动控制方法的实现流程图；

图3是本申请另一实施例提供的一种机械臂移动控制方法的实现流程图；

图4是本申请实施例提供的一种机械臂移动控制装置的结构框图；

图5是本申请实施例提供的一种终端设备的结构框图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

本申请实施例所涉及的机械臂移动控制方法，可以由控制设备或终端(以下称“移动终端”)执行。

请参阅图1，图1示出了本申请实施例提供的一种机械臂移动控制方法的实现流程图，该机械臂移动控制可以应用于任一终端设备，该机械臂移动控制方法包括：

步骤S10，接收针对机械臂的姿态控制信息，以及根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿，得到姿态增量信息。

其中，该机械臂包括基座、与基座连接的各关节部位和与顶端的关节部位连接的目标夹具，该目标夹具用于对物品进行夹持，各关节部位上均设置有一个电机，通过调节电机的的输出角度，以达到调节对应关节部位角度的效果，基于关节部位角度的调节，以达到调节目标夹具位置的效果，该基座用于承载各关节部位和目标夹具。

该步骤中，该姿态补偿用于补偿基座与水平面之间的偏差角，所导致的机械臂移动控制的偏差，当基座所处地面为斜坡或坑洼路面等情况时，基座与水平面之间存在偏差角，因此，通过根据基座的姿态对姿态控制信息进行姿态补偿，有效地提高了机械臂移动控制的准确性。

可选的，该步骤中，用户可以基于一3D操纵杆进行该姿态控制信息的发送，该3D操纵杆与终端设备之间的通讯连接，该姿态控制信息可以包括六个不同的模拟量信息，该模拟量信息可以为滚动角控制信息、俯仰角控制信息、偏航角控制信息、第一方向移动信息、第二方向移动信息和第三方向移动信息，该第一方向、第二方向和第三方向分别为，基座的基座坐标系中的x轴、y轴和z轴对应的方向。

该步骤中，若用户需要控制目标夹具向左移动，则需要将3D操纵杆往左扳动，以输出控制目标夹具向左移动对应的姿态控制信息，同理进行向上移动、下移动、左移动、右移动、前移动和后移动移动对应的姿态控制信息，若用户需要目标夹具往左旋转，则需要将操纵杆往左进行角度旋转，以输出控制目标夹具向左旋转对应的姿态控制信息，同理可以往右旋转、往上旋转、往下旋转、顺时针旋转和逆时针旋转对应的姿态控制信息。

该步骤中，所述姿态控制信息包括位置控制信息和角度控制信息，所述根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿所采用的计算公式包括：

其中，(x，y，z)是所述基座的基座坐标系中的坐标参数，ΔT_xyz是所述位置控制信息在所述基座坐标系中x轴、y轴和z轴下的位置增量组成的向量，R_rot是所述基座的姿态中俯仰角度和横滚角度经过绕轴旋转公式计算后的旋转结果矩阵,

是R_rot的转置，ΔT′_xyz是所述姿态增量信息中的位置增量信息，所述位置增量信息为所述姿态增量信息在x轴、y轴和z轴下的位置增量，该位置增量为用户期望目标夹具的移动参数，例如，位置控制信息中x轴对应的位置增量为，用户期望目标夹具在x轴上移动的距离，位置控制信息中y轴对应的位置增量为，用户期望目标夹具在y轴上移动的距离，位置控制信息中z轴对应的位置增量为，用户期望目标夹具在z轴上移动的距离。

R_rot是当前基座坐标系到大地水平坐标系的旋转矩阵，其中，设基座坐标系的x轴和y轴，与大地水平面的偏移角度分别为

和

则

其中，R是滚动角，P是俯仰角，Y是偏航角，ΔT_RPY是所述姿态控制信息中滚动角、俯仰角和偏航角对应角度增量的三阶矩阵，ΔT′_RPY是所述角度增量信息，所述角度增量信息为所述姿态增量信息中滚动角、俯仰角和偏航角对应的三阶矩阵。

进一步地，该步骤中，所述接收针对机械臂的姿态控制信息之后，还包括：

若所述姿态控制信息中任一位置增量小于第一增量阈值，则在所述姿态控制信息中删除所述位置增量，其中，该第一增量阈值可以根据需求进行设置，通过设置该第一增量阈值，以判断该姿态控制信息中的位置增量对应的操作是否是误操作，即，当姿态控制信息中的位置增量小于第一增量阈值时，则判定该位置增量是由于对3D操纵杆的不小心碰撞所导致的，因此，通过在姿态控制信息中删除小于第一增量阈值的位置增量，有效地提高了机械臂移动控制的准确性，防止了由于意外碰撞3D操纵杆所导致的误操作。

若所述姿态控制信息中任一角度增量小于第二增量阈值，则在所述姿态控制信息中删除所述角度增量，可以理解的，该第二增量阈值可以根据需求进行设置，通过设置该第二增量阈值，以判断该姿态控制信息中的角度增量对应的操作是否是误操作。

更进一步地，该3D操纵杆上的每个输出方向均设置有对应的输出上限值，通过设置该输出上限值，能有效地限制机械臂移动时的最高速度，提高了机械臂移动控制的安全性。

步骤S20，根据所述姿态增量信息对所述机械臂的目标夹具的当前姿态进行姿态计算，得到目标姿态。

其中，该目标姿态包括期望角度和期望位置，该姿态增量信息包括x轴位置增量、y轴位置增量、z轴位置增量、滚动角增量、俯仰角增量和偏航角增量。

该步骤中，通过将姿态增量信息中的x轴位置增量、y轴位置增量和z轴位置增量与目标夹具的当前姿态中的坐标位置进行线性加减运算，以得到该期望位置，该期望位置包括用户期望目标夹具分别在x轴、y轴和z轴上对应的坐标位置。

可选的，该步骤中，针对滚动角增量、俯仰角增量和偏航角增量，可以通过姿态矩阵，与目标夹具的当前姿态中的滚动角、俯仰角和偏航角进行矩阵运算，以得到该期望角度，该目标角度包括用户期望目标夹具的滚动角、俯仰角和偏航角对应的角度。

该步骤中，该目标夹具的姿态中的位置信息记为T_{xyz_present}，姿态中滚动角、俯仰角和偏航角对应的三阶矩阵记为T_{RPY_present}，则目标姿态中的期望位置为：

T_{xyz_target}＝T_{xyz_present}+ΔT_x ^′ _yz

目标姿态中滚动角、俯仰角和偏航角对应的三阶矩阵为：

T_{RPY_target}＝ΔT_R ^′ _PYT_{RPY_present}

步骤S30，计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息。

其中，该目标关节信息用于指示机械臂的各关节部位的目标关节姿态，该目标关节姿态包括目标角度和目标位置，该步骤中，通过计算目标夹具在目标姿态下，机械臂的目标关节信息，有效地提高了后续各关节部位的姿态调节的准确性。

可选的，该步骤中，所述计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息包括：

确定各关节部位的结构形式，例如，各关节部位之间结构的连接长度、各关节部位的有效转动中心或转动轴之间的偏置关系、各关节部位之间是否存在共线等，并根据正运动学的DH参数表示方法，列写出机械臂上基座到目标夹具之间各关节部位的关节参数，例如，当该基座到目标夹具之间的关节部位为六个时，则六个关节部位的关节参数对应的四阶矩阵分别记为A₁、A₂、A₃、A₄、A₅、A₆。

将目标姿态转换为四阶矩阵，并计算该四阶矩阵的解析解，其中，目标姿态对应的四阶矩阵为：

求取解析解的核心公式是：

A₁·A₂·A₃·A₄·A₅·A_６＝T_{pose_target}

若目标姿态对应的四阶矩阵能实现解析解的求解，则通过引入逆矩阵

销项，可以分别求取六个关节部位的电机角度，

该步骤中，若目标姿态对应的四阶矩阵存在多组不同的解析解，则采用最小二乘法或加权的最小二乘法，选取一组最优解，并将求取到的最优解设置为该目标关节信息。

步骤S40，根据所述目标关节信息对所述机械臂上的各关节部位进行姿态调节。

其中，根据目标姿态对应的四阶矩阵的解析解，能有效地计算到各关节部位对应的电机角度，根据计算到的电机角度对各关节部位对应的电机进行调节，以达到对该各关节部位进行姿态调节的效果，使得姿态调节下的各关节部位均处于对应的目标关节姿态，此时，由于各关节部位均处于对应的目标关节姿态，使得目标夹具会处于目标姿态下，进而提高了机械臂移动控制的准确性。

本实施例中，通过根据机械臂基座相对于大地水平面的姿态对姿态控制信息进行姿态补偿，防止了由于基座与地面之间的角度偏差，所导致的机械臂移动控制准确性低下的现象，通过根据姿态增量信息对机械臂上目标夹具的当前姿态进行姿态增量处理，能有效地计算到用户对目标夹具所期望的目标姿态，通过计算目标夹具在目标姿态下机械臂的目标关节信息，并根据目标关节信息对机械臂上的关节部位进行姿态调节，基于对关节部位的姿态调节，能有效地将目标夹具调节为对应的目标姿态，无需采用人工的方式多次进行关节部位的调节，提高了机械臂移动控制的效率。

请参阅图2，图2是本申请另一实施例提供的一种机械臂移动控制方法的实现流程图。相对于图1实施例，本实施例提供的机械臂移动控制方法用于对图1实施例中的步骤S30之后的步骤作进一步细化，包括：

步骤S50，分别判断不同所述关节部位是否能转动至对应的所述目标角度，及是否能移动至对应的所述目标位置。

其中，通过分别查询该关节部位对应的角度阈值和预设位置区域，并判断同一关节部位对应的目标角度是否小于角度阈值，以判断关节部位是否能转动至对应的目标角度，通过判断同一关节部位对应的目标位置是否在预设位置区域内，以判断关节部位是否能转动至对应的目标位置。

步骤S60，若任一所述关节部位不能转动至对应的所述目标角度，和/或不能移动至对应的所述目标位置，则根据所述关节部位的部位标识发送姿态调节错误提示。

其中，若任一关节部位不能转动至对应的目标角度，和/或不能移动至对应的目标位置，通过根据关节部位的部位标识发送姿态调节错误提示，以提示用户该姿态调节错误提示中携带的部位标识对应的关节部位存在调节错误现象。

可选的，本实施例中，所述分别判断不同所述关节部位是否能转动至对应的所述目标角度，及是否能移动至对应的所述目标位置之后，还包括：

分别获取所述关节部位转动至对应的所述目标角度，及移动至对应的所述目标位置下的姿态调节轨迹，并检测所述姿态调节轨迹中是否存在障碍物；

其中，通过检测姿态调节轨迹中是否存在障碍物，以判断该姿态调节轨迹对应的关节部位进行角度转动和位置移动过程中，是否与障碍物之间会发生碰撞，提高了机械臂移动控制的安全性。

可选的，该步骤中，可以通过将该姿态调节轨迹中各轨迹点坐标与预存储的障碍物坐标表进行匹配，以判断该姿态调节轨迹中是否存在障碍物。

若所述姿态调节轨迹中存在所述障碍物，则对所述姿态调节轨迹对应的所述关节部位发送姿态调节错误提示。

本实施例中，通过分别判断不同关节部位是否能转动至对应的目标角度，及是否能移动至对应的目标位置，有效地提高了关节部位进行角度调节和位置移动过程中的安全性，若任一关节部位不能转动至对应的目标角度，和/或不能移动至对应的目标位置，通过根据关节部位的部位标识发送姿态调节错误提示，以提示用户该姿态调节错误提示中携带的部位标识对应的关节部位存在调节错误现象。

请参阅图3，图3是本申请另一实施例提供的一种机械臂移动控制方法的实现流程图。相对于图1实施例，本实施例提供的机械臂移动控制方法用于对图1实施例中的步骤S30作进一步细化，若第一实施例中的目标姿态对应的四阶矩阵无解析解时，则该步骤S30包括：

步骤S31，计算所述目标夹具在当前姿态下，所述机械臂的雅克比矩阵，并计算所述雅克比矩阵与所述雅克比矩阵的转置的乘积，得到乘积结果。

步骤S32，计算所述乘积结果的行列式，根据所述行列式计算所述雅克比矩阵的逆矩阵，并根据所述逆矩阵求解角度差，得到偏移角度。

步骤S33，根据所述偏移角度分别对所述关节部位的关节角度进行角度更新，并根据角度更新后的所述关节部位的关节角度对所述当前姿态进行姿态更新。

其中，通过分别计算偏移角度与各关节部位的关节角度之间的角度和，以达到对该关节部位的关节角度的角度更新效果，并根据偏移角度与各关节部位的关节角度之间的角度和，对当前姿态进行姿态更新。

步骤S34，若姿态更新后的所述当前姿态与所述目标姿态之间的姿态差值大于差值阈值，则将姿态更新后的所述当前姿态，返回执行所述计算所述目标夹具在当前姿态下，所述机械臂的雅克比矩阵的步骤及后续步骤。

其中，该差值阈值可以根据需求进行设置，该差值阈值用于检测姿态更新前后的当前姿态与目标姿态之间的差值是否满足预设的迭代收敛条件，即，当姿态更新后的当前姿态与目标姿态之间的姿态差值大于差值阈值，则判定该当前姿态未收敛，通过执行所述计算所述目标夹具在当前姿态下，所述机械臂的雅克比矩阵的步骤及后续步骤，以达到对该当前姿态持续进行迭代更新的效果，最终计算出准确的目标姿态。

步骤S35，若姿态更新后的所述当前姿态与所述目标姿态的姿态差值小于或等于差值阈值，则将所述目标夹具在姿态更新后所述当前姿态下，所述机械臂的关节信息设置所述目标关节信息。

其中，若姿态更新后的当前姿态与目标姿态之间的姿态差值小于或等于差值阈值，则判定该当前姿态收敛，即，目标夹具在收敛后的当前姿态下，机械臂的关节信息为目标关节信息。

可选的，本实施例中，所述根据角度更新后的所述关节部位的关节角度对所述当前姿态进行姿态更新之后，还包括：

若所述当前姿态的姿态更新次数大于次数阈值，则判定所述目标夹具不能调节至所述目标姿态，并输出机械臂移动控制错误提示；

其中，该次数阈值可以根据需求进行设置，该当前姿态的姿态更新次数为当前姿态的迭代次数，若当前姿态的姿态更新次数大于次数阈值，则判定该当前姿态不收敛，即，判定该目标夹具无法达到该目标姿态，通过输出机械臂移动控制错误提示，以提示用户目标夹具无法达到该目标姿态。

本实施例中，通过采用迭代法计算目标关节信息，使得能在短时间内完成目标关节信息的计算，提高了目标关节信息计算的实时性，通过雅克比矩阵和雅克比矩阵的转置的乘积，以及乘积的行列式的值，来近似计算雅克比矩阵的逆矩阵，有效减小了雅克比逆矩阵计算的计算复杂度。

请参阅图4，图4是本申请实施例提供的一种机械臂移动控制装置100的结构框图。本实施例中该机械臂移动控制装置100包括的各单元用于执行图1、图2对应的实施例中的各步骤。具体请参阅图1、图2以及图1、图2所对应的实施例中的相关描述。为了便于说明，仅示出了与本实施例相关的部分。参见图4，机械臂移动控制装置100包括：姿态补偿单元10、姿态计算单元11、关节信息计算单元12和姿态调节单元13，其中：

姿态补偿单元10，用于接收针对机械臂的姿态控制信息，以及根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿，得到姿态增量信息，所述姿态补偿用于根据所述基座的姿态对所述姿态控制信息进行信息校准。

可选的，所述姿态控制信息包括位置控制信息和角度控制信息，所述根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿所采用的计算公式包括：

其中，(x，y，z)是所述基座的基座坐标系中的坐标参数，ΔT_xyz是所述位置控制信息在所述基座坐标系中x轴、y轴和z轴下的位置增量组成的向量，R_rot是所述基座的姿态中俯仰角度和横滚角度经过绕轴旋转公式计算后的旋转结果矩阵,

是R_rot的转置，ΔT′_xyz是所述姿态增量信息中的位置增量信息，所述位置增量信息为所述姿态增量信息在x轴、y轴和z轴下的位置增量；

其中，R是滚动角，P是俯仰角，Y是偏航角，ΔT_RPY是所述姿态控制信息中滚动角、俯仰角和偏航角对应角度增量的三阶矩阵，ΔT′_RPY是所述角度增量信息，所述角度增量信息为所述姿态增量信息中滚动角、俯仰角和偏航角对应的三阶矩阵。

可选的，该姿态补偿单元10还用于：若所述姿态控制信息中任一位置增量小于第一增量阈值，则在所述姿态控制信息中删除所述位置增量；

若所述姿态控制信息中任一角度增量小于第二增量阈值，则在所述姿态控制信息中删除所述角度增量。

姿态计算单元11，用于根据所述姿态增量信息对所述机械臂的目标夹具的当前姿态进行姿态计算，得到目标姿态。

关节信息计算单元12，用于计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息，所述目标关节信息用于指示所述机械臂的各关节部位的目标关节姿态。

其中，该关节信息计算单元12还用于：分别判断不同所述关节部位是否能转动至对应的所述目标角度，及是否能移动至对应的所述目标位置；

若任一所述关节部位不能转动至对应的所述目标角度，和/或不能移动至对应的所述目标位置，则根据所述关节部位的部位标识发送姿态调节错误提示。

进一步地，该关节信息计算单元12还用于：分别获取所述关节部位转动至对应的所述目标角度，及移动至对应的所述目标位置下的姿态调节轨迹，并检测所述姿态调节轨迹中是否存在障碍物；

若所述姿态调节轨迹中存在所述障碍物，则对所述姿态调节轨迹对应的所述关节部位发送姿态调节错误提示。

更进一步地，该关节信息计算单元12还用于：计算所述目标夹具在当前姿态下，所述机械臂的雅克比矩阵，并计算所述雅克比矩阵与所述雅克比矩阵的转置的乘积，得到乘积结果；

计算所述乘积结果的行列式，根据所述行列式计算所述雅克比矩阵的逆矩阵，并根据所述逆矩阵求解角度差，得到偏移角度；

根据所述偏移角度分别对所述关节部位的关节角度进行角度更新，并根据角度更新后的所述关节部位的关节角度对所述当前姿态进行姿态更新；

若姿态更新后的所述当前姿态与所述目标姿态之间的姿态差值大于差值阈值，则将姿态更新后的所述当前姿态，返回执行所述计算所述目标夹具在当前姿态下，所述机械臂的雅克比矩阵的步骤及后续步骤；

若姿态更新后的所述当前姿态与所述目标姿态的姿态差值小于或等于差值阈值，则将所述目标夹具在姿态更新后所述当前姿态下，所述机械臂的关节信息设置所述目标关节信息。

可选的，该关节信息计算单元12还用于：若所述当前姿态的姿态更新次数大于次数阈值，则判定所述目标夹具不能调节至所述目标姿态，并输出机械臂移动控制错误提示。

姿态调节单元13，用于根据所述目标关节信息对所述机械臂上的各关节部位进行姿态调节。

本实施例中，通过根据机械臂上基座的姿态对姿态控制信息进行姿态补偿，防止了由于基座与地面之间的角度或位置偏差，所导致的机械臂移动控制准确性低下的现象，通过根据姿态增量信息对机械臂上目标夹具的当前姿态进行姿态增量处理，能有效地计算到用户对目标夹具所期望的目标姿态，通过计算目标夹具在目标姿态下机械臂的目标关节信息，并根据目标关节信息对机械臂上的关节部位进行姿态调节，基于对关节部位的姿态调节，能有效地将目标夹具调节为对应的目标姿态，无需采用人工的方式多次进行关节部位的调节，提高了机械臂移动控制的效率。

图5是本申请另一实施例提供的一种终端设备2的结构框图。如图5所示，该实施例的终端设备2包括：处理器20、存储器21以及存储在所述存储器21中并可在所述处理器20上运行的计算机程序22，例如机械臂移动控制方法的程序。处理器20执行所述计算机程序23时实现上述各个机械臂移动控制方法各实施例中的步骤，例如图1所示的S10至S40，或者图2所示的S50至S60，或者图3所示的S31至S35。或者，所述处理器20执行所述计算机程序22时实现上述图4对应的实施例中各单元的功能，例如，图4所示的单元10至13的功能，具体请参阅图4对应的实施例中的相关描述，此处不赘述。

示例性的，所述计算机程序22可以被分割成一个或多个单元，所述一个或者多个单元被存储在所述存储器21中，并由所述处理器20执行，以完成本申请。所述一个或多个单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序22在所述终端设备2中的执行过程。例如，所述计算机程序22可以被分割成姿态补偿单元10、姿态计算单元11、关节信息计算单元12和姿态调节单元13，各单元具体功能如上所述。

所述终端设备可包括，但不仅限于，处理器20、存储器21。本领域技术人员可以理解，图5仅仅是终端设备2的示例，并不构成对终端设备2的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述终端设备还可以包括输入输出设备、网络接入设备、总线等。

所称处理器20可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器21可以是所述终端设备2的内部存储单元，例如终端设备2的硬盘或内存。所述存储器21也可以是所述终端设备2的外部存储设备，例如所述终端设备2上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器21还可以既包括所述终端设备2的内部存储单元也包括外部存储设备。所述存储器21用于存储所述计算机程序以及所述终端设备所需的其他程序和数据。所述存储器21还可以用于暂时地存储已经输出或者将要输出的数据。

本申请实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时可实现上述各个方法实施例中的步骤。

以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (9)

1.一种机械臂移动控制方法，其特征在于，包括：

接收针对机械臂的姿态控制信息，以及根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿，得到姿态增量信息，所述姿态补偿用于根据所述基座的姿态对所述姿态控制信息进行信息校准；

根据所述姿态增量信息对所述机械臂的目标夹具的当前姿态进行姿态计算，得到目标姿态；

计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息，所述目标关节信息用于指示所述机械臂的各关节部位的目标关节姿态；

根据所述目标关节信息对所述机械臂上的各关节部位进行姿态调节；

所述姿态控制信息包括位置控制信息和角度控制信息，所述根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿所采用的计算公式包括：

其中，(x，y，z)是所述基座的基座坐标系中的坐标参数，ΔT_xyz是所述位置控制信息在所述基座坐标系中x轴、y轴和z轴下的位置增量组成的向量，R_rot是所述基座的姿态中俯仰角度和横滚角度经过绕轴旋转公式计算后的旋转结果矩阵,

是R_rot的转置，ΔT′_xyz是所述姿态增量信息中的位置增量信息，所述位置增量信息为所述姿态增量信息在x轴、y轴和z轴下的位置增量；

其中，R是滚动角，P是俯仰角，Y是偏航角，ΔT_RPY是所述姿态控制信息中滚动角、俯仰角和偏航角对应角度增量的三阶矩阵，ΔT′_RPY是角度增量信息，所述角度增量信息为所述姿态增量信息中滚动角、俯仰角和偏航角对应的三阶矩阵。

2.根据权利要求1所述的机械臂移动控制方法，其特征在于，所述接收针对机械臂的姿态控制信息之后，还包括：

若所述姿态控制信息中任一位置增量小于第一增量阈值，则在所述姿态控制信息中删除所述位置增量；

若所述姿态控制信息中任一角度增量小于第二增量阈值，则在所述姿态控制信息中删除所述角度增量。

3.根据权利要求1所述的机械臂移动控制方法，其特征在于，所述目标关节姿态包括目标角度和目标位置，所述计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息之后，还包括：

分别判断不同所述关节部位是否能转动至对应的所述目标角度，及是否能移动至对应的所述目标位置；

若任一所述关节部位不能转动至对应的所述目标角度，和/或不能移动至对应的所述目标位置，则根据所述关节部位的部位标识发送姿态调节错误提示。

4.根据权利要求3所述的机械臂移动控制方法，其特征在于，所述分别判断不同所述关节部位是否能转动至对应的所述目标角度，及是否能移动至对应的所述目标位置之后，还包括：

分别获取所述关节部位转动至对应的所述目标角度，及移动至对应的所述目标位置下的姿态调节轨迹，并检测所述姿态调节轨迹中是否存在障碍物；

若所述姿态调节轨迹中存在所述障碍物，则对所述姿态调节轨迹对应的所述关节部位发送姿态调节错误提示。

5.根据权利要求1所述的机械臂移动控制方法，其特征在于，所述计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息，包括：

计算所述目标夹具在当前姿态下，所述机械臂的雅克比矩阵，并计算所述雅克比矩阵与所述雅克比矩阵的转置的乘积，得到乘积结果；

计算所述乘积结果的行列式，根据所述行列式计算所述雅克比矩阵的逆矩阵，并根据所述逆矩阵求解角度差，得到偏移角度；

根据所述偏移角度分别对所述关节部位的关节角度进行角度更新，并根据角度更新后的所述关节部位的关节角度对所述当前姿态进行姿态更新；

若姿态更新后的所述当前姿态与所述目标姿态之间的姿态差值大于差值阈值，则将姿态更新后的所述当前姿态，返回执行所述计算所述目标夹具在当前姿态下，所述机械臂的雅克比矩阵的步骤及后续步骤；

若姿态更新后的所述当前姿态与所述目标姿态的姿态差值小于或等于差值阈值，则将所述目标夹具在姿态更新后所述当前姿态下，所述机械臂的关节信息设置所述目标关节信息。

6.根据权利要求5所述的机械臂移动控制方法，其特征在于，所述根据角度更新后的所述关节部位的关节角度对所述当前姿态进行姿态更新之后，还包括：

若所述当前姿态的姿态更新次数大于次数阈值，则判定所述目标夹具不能调节至所述目标姿态，并输出机械臂移动控制错误提示。

7.一种机械臂移动控制装置，其特征在于，包括：

姿态补偿单元，用于接收针对机械臂的姿态控制信息，以及根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿，得到姿态增量信息，所述姿态补偿用于根据所述基座的姿态对所述姿态控制信息进行信息校准；

姿态计算单元，用于根据所述姿态增量信息对所述机械臂的目标夹具的当前姿态进行姿态计算，得到目标姿态；

关节信息计算单元，用于计算所述目标夹具在所述目标姿态下，所述机械臂的目标关节信息，所述目标关节信息用于指示所述机械臂的各关节部位的目标关节姿态；

姿态调节单元，用于根据所述目标关节信息对所述机械臂上的各关节部位进行姿态调节；

其中，所述姿态控制信息包括位置控制信息和角度控制信息，所述根据所述机械臂的基座的姿态对所述姿态控制信息进行姿态补偿所采用的计算公式包括：

其中，(x，y，z)是所述基座的基座坐标系中的坐标参数，ΔT_xyz是所述位置控制信息在所述基座坐标系中x轴、y轴和z轴下的位置增量组成的向量，R_rot是所述基座的姿态中俯仰角度和横滚角度经过绕轴旋转公式计算后的旋转结果矩阵,

是R_rot的转置，ΔT′_xyz是所述姿态增量信息中的位置增量信息，所述位置增量信息为所述姿态增量信息在x轴、y轴和z轴下的位置增量；

其中，R是滚动角，P是俯仰角，Y是偏航角，ΔT_RPY是所述姿态控制信息中滚动角、俯仰角和偏航角对应角度增量的三阶矩阵，ΔT′_RPY是角度增量信息，所述角度增量信息为所述姿态增量信息中滚动角、俯仰角和偏航角对应的三阶矩阵。

8.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至6任一项所述方法的步骤。

9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述方法的步骤。

Images