机械臂的控制方法和装置与流程

本发明实施例涉及控制应用技术领域，特别是涉及一种机械臂的控制方法和装置。

背景技术：

机器人已经被应用于多个领域，在工业化、智能化的发展中发挥着越来越突出的作用，其中，机械臂是机器人执行任务的重要部件。运动轨迹是机器人控制系统的一项基本任务。机械臂的运动轨迹通常有笛卡尔空间的直线、圆弧，以及关节空间的点到点运动轨迹。在实际的应用场合中，一般是由多条轨迹顺序连接完成一项操作任务，然而，相邻两条轨迹在连接处存在拐角，从而不能从一条轨迹平滑过渡到下一条轨迹。

技术实现要素：

有鉴于此，本发明提出一种机械臂的控制方法和装置，通过对实现了对机械臂运动过程中不同轨迹之间的平稳过渡，保证了机械臂运行的效率和精度。

第一方面，本发明实施例提供了一种机械臂的控制方法，所述方法包括：获取机械臂末端在当前移动直线上的所在点的第一状态信息和所述机械臂末端在下一移动直线上预设点的第二状态信息；根据所述第一状态信息和所述第二状态信息确定从所述所在点至所述预设点的移动轨迹的参数，所述机械臂末端在所述移动轨迹上的移动函数为连续函数；控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点。

进一步的，在控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点之后，还包括：所述机械臂末端从所述预设点根据所述下一移动直线的轨迹规划继续移动至所述下一移动直线的终点。

进一步的，所述第一状态信息包括所述机械臂末端在所述所在点的位置、速度和加速度；所述第二状态信息包括所述机械臂末端在所述预设点的位置、速度和加速度。

进一步的，所述获取机械臂末端在当前移动直线上的所在点的第一状态信息和所述机械臂末端在下一移动直线上预设点的第二状态信息包括：获取所述机械臂末端在当前移动直线上的所在点的在关节空间下的第一状态信息；通过逆运动学方法，将所述预设点的第二状态信息由笛卡尔坐标表示转换成关节空间坐标表示。

进一步的，所述根据所述第一状态信息和所述第二状态信息确定从所述所在点至所述预设点的移动轨迹的参数包括：通过S型速度规划法、多阶样条插值法和/或B样条插补法规划所述下一移动直线在笛卡尔坐标系下的插补点。

进一步的，所述移动函数包括：所述机械臂末端在所述移动轨迹上的位置函数、速度函数和加速度函数。

进一步的，所述根据所述第一状态信息和所述第二状态信息确定从所述所在点至所述预设点的移动轨迹的参数，所述机械臂末端在所述移动轨迹上的移动函数为连续函数还包括：通过多阶多项式和/或三角函数法，分别确定所述位置函数、速度函数和加速度函数对应的参数。

进一步的，所述通过逆运动学方法，将所述预设点的第二状态信息由笛卡尔坐标表示转换成关节空间坐标表示包括：根据公式进行笛卡尔坐标表示向关节空间坐标表示的转换，其中和分别为笛卡尔坐标系下机械臂末端的速度和加速度，和分别为关节空间坐标系下机械臂末端的速度和加速度，J为雅可比矩阵，为雅可比矩阵的一阶导，J⁺为J的伪逆。

进一步的，所述通过多阶多项式和/或三角函数法，分别确定所述位置函数、速度函数和加速度函数对应的参数包括：根据如下五阶多项式曲线公式，确定所述位置函数、速度函数和加速度函数对应的参数；

a₀＝Q₀

$a_1={\stackrel{\·}{Q}}_0$

$a_2=\frac{1}{2}{\stackrel{\·\·}{Q}}_0$

$a_3=\frac{1}{2t_{trans}^3}\[20h-(8{\stackrel{\·}{Q}}_s+12{\stackrel{\·}{Q}}_0)t_{trans}-(3{\stackrel{\·\·}{Q}}_0-{\stackrel{\·\·}{Q}}_s)t_{trans}^2\]$

$a_4=\frac{1}{2t_{trans}^4}\[-30h+(14{\stackrel{\·}{Q}}_s+16{\stackrel{\·}{Q}}_0)t_{trans}+(3{\stackrel{\·\·}{Q}}_0-2{\stackrel{\·\·}{Q}}_s)t_{trans}^2\]$

$a_5=\frac{1}{2t_{trans}^5}\[12h-6({\stackrel{\·}{Q}}_s+{\stackrel{\·}{Q}}_0)t_{trans}+({\stackrel{\·\·}{Q}}_s-{\stackrel{\·\·}{Q}}_0)t_{trans}^2\]$

其中，Q₀、和分别为所述所在点的关节空间的位置、速度和加速度，Q_s、和分别为所述预设点在关节空间的位置、速度和加速度，h为所述所在点和所述预设点之间的距离，t_trans机械臂末端从所述所在点移动至所述预设点的时间。

进一步的，所述通过多阶多项式和/或三角函数法，分别确定所述位置函数、速度函数和加速度函数对应的参数还包括：根据如下公式计算机所述所在点至所述预设点之间任意时刻的机械臂末端的位置、速度和加速度；

Q_t＝Q₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

${\stackrel{\·}{Q}}_t=a_1+2a_{2}t+3a_3{t}^2+4a_4{t}^3+5a_5{t}^4$

${\stackrel{\·\·}{Q}}_t=2a_2+6a_{3}t+12a_4{t}^2+20a_5{t}^3$

其中，Q_t、和分别为所述机械臂末端在所述所在点至所述预设点之间任意点的位置、速度和加速度。

第二方面，本发明实施例提供了一种机械臂的控制装置，所述装置包括：获取单元，用于获取机械臂末端在当前移动直线上的所在点的第一状态信息和所述机械臂末端在下一移动直线上预设点的第二状态信息；确定单元，与所述获取单元相连，用于根据所述第一状态信息和所述第二状态信息确定从所述所在点至所述预设点的移动轨迹的参数，所述机械臂末端在所述移动轨迹上的移动函数为连续函数；控制单元，与所述确定单元相连，用于控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点。

进一步的，所述第一状态信息包括所述机械臂末端在所述所在点的位置、速度和加速度；所述第二状态信息包括所述机械臂末端在所述预设点的位置、速度和加速度，所述获取单元包括：信息获取子单元，用于获取所述机械臂末端在当前移动直线上的所在点的在关节空间下的第一状态信息；信息转换子单元，与所述信息获取子单元相连，用于通过逆运动学方法，将所述预设点的第二状态信息由笛卡尔坐标表示转换成关节空间坐标表示。

本发明实施例中，通过获取机械臂末端在当前移动直线上的所在点和机械臂末端在下一移动直线上预设点状态信息，规划从所述所在点至所述预设点的移动轨迹，控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点。实现了机械臂末端平稳的从一个运动方向切换至另一个运动方向，保证了机械臂运动的效率和精度。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1是本发明实施例一中的一种机械臂的控制方法的流程图；

图2是本发明实施例二中的一种机械臂的控制方法的流程图；

图3是本发明实施例三中的一种机械臂的控制方法的流程图；

图4为本发明实施例三中的X-Y平面内机械臂末端的轨迹规划图；

图5-图7为本发明实施例三中的机械臂的前三个轴在任意时刻的位置曲线图；

图8-图10为本发明实施例三中的机械臂的前三个轴在任意时刻的速度曲线图；

图11-图13为本发明实施例三中的机械臂的前三个轴在任意时刻的加速度曲线图；

图14为本发明实施例四中的一种机械臂的控制装置的结构图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。另外还需要说明的是，为了便于说明，以下实施例中示出了与本发明相关的示例，这些示例仅作为说明本发明实施例的原理所用，并不作为对本发明实施例的限定，同时，这些示例的具体数值会根据不同的应用环境和装置或者组件的参数不同而不同。

本发明实施例的一种机械臂的控制方法可以运行于安装有Windows(微软公司开发的操作系统平台)、Android(谷歌公司开发的用于便携式可移动智能设备的操作系统平台)、iOS(苹果公司开发的用于便携式可移动智能设备的操作系统平台)、Windows Phone(微软公司开发的用于便携式可移动智能设备的操作系统平台)等操作系统的终端中，该终端可以是台式机、笔记本电脑、移动电话、掌上电脑、平板电脑、数码相机、数码摄像机等等中的任意一种。

实施例一

图1是本发明实施例一中的一种机械臂的控制方法的流程图，该方法用于实现机械臂末端平稳的从一个运动方向切换至另一个运动方向，该方法可以由具有机械臂控制的装置来执行，该装置可以由软件和/或硬件方式实现，例如典型的是用户终端设备，例如手机、电脑等。本实施例中的用于触摸屏的操作方法包括：步骤S110、步骤S120和步骤S130。

步骤S110，获取机械臂末端在当前移动直线上的所在点的第一状态信息和所述机械臂末端在下一移动直线上预设点的第二状态信息。

具体的，在机械臂的任务操作过程中，机械臂的运动是指机械臂末端的运动，由机械臂末端来执行具体的操作任务。可以把机械臂在当前移动直线上的所在点作为该段轨迹的起点，该所在点记为O。机械臂按照用户指定的任务需求进行移动，移动至指定位置去执行对应操作，在机械臂的移动过程中，将下一直线上的预设点T作为过渡段的终点，下一移动直线的起点记为P₁，终点记为P₂，获取机械臂末端在点O和点T的状态信息。

可选的，所述第一状态信息包括所述机械臂末端在所述所在点的位置、速度和加速度；所述第二状态信息包括所述机械臂末端在所述预设点的位置、速度和加速度。

其中，第一状态信息包括机械臂末端所在点的位置、速度和加速度分别为Q₀、和Q₀、和均为n维列向量，向量维数机械臂的关节数目；第二状态信息包括机械臂末端在预设点的位置、速度和加速度分别为P_s、和P_s、和均为n维列向量，向量维数机械臂的关节数目

步骤S120，根据所述第一状态信息和所述第二状态信息确定从所述所在点至所述预设点的移动轨迹的参数，所述机械臂末端在所述移动轨迹上的移动函数为连续函数。

具体的，为了确定机械臂从所述所在点至所述预设点的移动轨迹，首先需要确定计算移动轨迹所需要的参数，该参数由所述第一状态信息和所述第二状态信息决定。移动轨迹是由移动函数生成的直线或曲线构成的，函数是描述每个输入值(自变量)对应唯一输出值(因变量)的对应关系，函数图像是平面或曲面上的点对的集合。机械臂末端的移动轨迹是由移动函数在平面或曲面上确定的点对的集合，这些点对形成了机械臂末端的移动轨迹。

机械臂末端在所述移动轨迹上的移动函数为连续函数，连续函数是指当自变量变化很小时，因变量变化也很小的对应关系，体现在图像上是一条没有断裂的连续曲线。机械臂末端的移动轨迹为连续函数是指，机械臂末端的相关状态信息的量在数值上没有跳变，体现在实际运动中是机械臂在任务操作中不发生抖动。

步骤S130，控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点。

具体的，得到从所述所在点至所述预设点的移动轨迹后，控制机械臂末端的运动，机械臂末端沿计算出来的移动轨迹从当前直线的所在点位置移动至下一移动直线的预设点位置。

可选的，在控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点之后，还包括：所述机械臂末端从所述预设点根据所述下一移动直线的轨迹规划继续移动至所述下一移动直线的终点。

具体的，机械臂末端沿所述移动轨迹从当前直线的所在点移动至下一移动直线的预设点T之后，完成了从所在点到预设点的过渡，按用户需求根据下一移动直线的轨迹移动至下一直线的终点P₂。按用户需求完成了沿下一移动直线的移动。

本发明实施例中，通过获取机械臂末端在当前移动直线上的所在点和机械臂末端在下一移动直线上预设点状态信息；规划从所述所在点至所述预设点的移动轨迹，控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点。实现了机械臂末端平稳的从一个运动方向切换至另一个运动方向，保证了机械臂运动的效率和精度。

实施例二

图2是本发明实施例二中一种机械臂的控制方法的流程图，本发明实施例实在实施例一的基础上，进一步解释了步骤S110，所述获取机械臂末端在当前移动直线上的所在点的第一状态信息和所述机械臂末端在下一移动直线上预设点的第二状态信息包括：获取所述机械臂末端在当前移动直线上的所在点的在关节空间下的第一状态信息；通过逆运动学方法，将所述预设点的第二状态信息由笛卡尔坐标表示转换成关节空间坐标表示。具体的，本实施例中机械臂的控制方法包括：步骤S210、步骤S220、步骤S230和步骤S240。

步骤S210，获取所述机械臂末端在当前移动直线上的所在点的在关节空间下的第一状态信息。

具体的，第一状态信息包括机械臂末端所在点的位置、速度和加速度分别为Q₀、和Q₀、和均为机械臂末端在关节空间下的状态表示，单位分别为rad、rad/s和rad/s²。

步骤S220，通过逆运动学方法，将所述预设点的第二状态信息由笛卡尔坐标表示转换成关节空间坐标表示。

具体的，第二状态信息包括机械臂末端在预设点的位置、速度和加速度分别为P_s、和P_s、和为均为机械臂在笛卡尔空间下的状态表示，单位分别为m、m/s和m/s²。根据逆运动学的变换方法，通过雅各比矩阵完成第二状态信息由笛卡尔坐标系表示到关节空间坐标表示的转换。

可选的，所述通过逆运动学方法，将所述预设点的第二状态信息由笛卡尔坐标表示转换成关节空间坐标表示包括：根据公式进行笛卡尔坐标表示向关节空间坐标表示的转换，其中和分别为笛卡尔坐标系下机械臂末端的速度和加速度，和分别为关节空间坐标系下机械臂末端的速度和加速度，J为雅可比矩阵，为雅可比矩阵的一阶导，J⁺为J的伪逆。

其中，根据逆运动学的变换关系，即关节空间的坐标表示和任务空间的坐标表示之间的关系得到

步骤S230，根据所述第一状态信息和所述第二状态信息确定从所述所在点至所述预设点的移动轨迹的参数，所述机械臂末端在所述移动轨迹上的移动函数为连续函数。

步骤S240，控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点。

本发明实施例通过获取所述机械臂末端在当前移动直线上的所在点的在关节空间下的第一状态信息，通过逆运动学方法，将所述预设点的第二状态信息由笛卡尔坐标表示转换成关节空间坐标表示，将第一状态信息和第二状态信息统一成关节空间的坐标表示，以便对机械臂末端从所在点到预设点的移动轨迹进行准确规划。

实施例三

图3是本发明实施例中的一种机械臂的控制方法的流程图，本实施例是在实施例一的基础上，进一步解释了步骤S120，通过S型速度规划法、多阶样条插值法和/或B样条插补法规划所述下一移动直线在笛卡尔坐标系下的插补点；通过多阶多项式和/或三角函数法，分别确定所述位置函数、速度函数和加速度函数对应的参数。具体的，本实施例中机械臂的控制方法包括：步骤S310、步骤S320、步骤S330和步骤S340。

步骤S310，获取机械臂末端在当前移动直线上的所在点的第一状态信息和所述机械臂末端在下一移动直线上预设点的第二状态信息。

步骤S320，通过S型速度规划法、多阶样条插值法和/或B样条插补法规划所述下一移动直线在笛卡尔坐标系下的插补点。

具体的，当前移动直线和下一移动直线的状态信息可以是预先规划的，也可以是根据机械臂执行任务操作的过程中按照用户新的需求进行更新的。S型速度规划法，系统在加减速段的速度曲线的形状呈现S型，加减速控制的方式比较复杂，主要是在加减速的时候，通过对加速度的导数的控制，从而对加速度、速度和位移进行控制，最大限度的减少对系统造成的影响。多阶样条插值法是通过一系列形值点的一条光滑曲线，数学上通过求解三弯矩方程组得出曲线函数组的过程。B样条曲线插补具有几何不变性、凸包性、保凸性、变差减小性和局部支撑性等特性。通过S型速度规划法、多阶样条插值法和/或B样条插补法规划所述下一移动直线在笛卡尔坐标系下的插补点，P_t＝[x_t,y_t,z_t]^T,速度加速度t为时间点，x_t,y_t,z_t是机械臂末端在笛卡尔空间的三个坐标轴的位置分量，是机械臂末端在笛卡尔空间的三个坐标轴的速度分量，是机械臂末端在笛卡尔空间的三个坐标轴的加速度分量。

步骤S330，通过多阶多项式和/或三角函数法，分别确定所述位置函数、速度函数和加速度函数对应的参数。

具体的，多阶多项式法即构造多阶多项式曲线，该多阶多项式曲线即为轨迹规划中的移动曲线，多阶多项式法的参数与所述位置函数、速度函数和加速度函数相关。三角函数法，即通过正余弦等三角函数，通过位置函数、速度函数和加速度函数，确定其对应的参数。

步骤S340，控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点。

可选的，所述移动函数包括：所述机械臂末端在所述移动轨迹上的位置函数、速度函数和加速度函数。

可选的，所述通过多阶多项式和/或三角函数法，分别确定所述位置函数、速度函数和加速度函数对应的参数包括：根据如下五阶多项式曲线公式，确定所述位置函数、速度函数和加速度函数对应的参数；

a₀＝Q₀

$a_1={\stackrel{\·}{Q}}_0$

$a_2=\frac{1}{2}{\stackrel{\·\·}{Q}}_0$

$a_3=\frac{1}{2t_{trans}^3}\[20h-(8{\stackrel{\·}{Q}}_s+12{\stackrel{\·}{Q}}_0)t_{trans}-(3{\stackrel{\·\·}{Q}}_0-{\stackrel{\·\·}{Q}}_s)t_{trans}^2\]$

$a_4=\frac{1}{2t_{trans}^4}\[-30h+(14{\stackrel{\·}{Q}}_s+16{\stackrel{\·}{Q}}_0)t_{trans}+(3{\stackrel{\·\·}{Q}}_0-2{\stackrel{\·\·}{Q}}_s)t_{trans}^2\]$

$a_5=\frac{1}{2t_{trans}^5}\[12h-6({\stackrel{\·}{Q}}_s+{\stackrel{\·}{Q}}_0)t_{trans}+({\stackrel{\·\·}{Q}}_s-{\stackrel{\·\·}{Q}}_0)t_{trans}^2\]$

其中，Q₀、和分别为所述所在点的关节空间的位置、速度和加速度，Q_s、和分别为所述预设点在关节空间的位置、速度和加速度，h为所述所在点和所述预设点之间的距离，t_trans机械臂末端从所述所在点移动至所述预设点的时间。

具体的，通过五阶多项式曲线公式，确定所述位置函数、速度函数和加速度函数对应的参数a₀、a₁、a₂、a₃、a₄和a₅，各参数分别与机械臂末端的所在点的速度和加速度、预设点的速度和加速度、所在点和预设点的距离相关。

可选的，所述通过多阶多项式和/或三角函数法，分别确定所述位置函数、速度函数和加速度函数对应的参数还包括：根据如下公式计算机所述所在点至所述预设点之间任意时刻的机械臂末端的位置、速度和加速度；

Q_t＝Q₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

${\stackrel{\·}{Q}}_t=a_1+2a_{2}t+3a_3{t}^2+4a_4{t}^3+5a_5{t}^4$

${\stackrel{\·\·}{Q}}_t=2a_2+6a_{3}t+12a_4{t}^2+20a_5{t}^3$

其中，Q_t、和分别为所述机械臂末端在所述所在点至所述预设点之间任意点的位置、速度和加速度。

具体的，计算出参数a₀、a₁、a₂、a₃、a₄和a₅后，根据该参数的值设计函数表达式，计算所在点至所述预设点之间任意时刻的机械臂末端的位置、速度和加速度。该函数表达式根据用户的需要进行设定，可以是线性函数表达式，也可以是非线性函数表达式。

本发明实施例中，通过规划所述下一移动直线在笛卡尔坐标系下的插补点，并确定所述位置函数、速度函数和加速度函数对应的参数，确定机械臂末端从所在点到预设点的移动轨迹，机械臂末端沿该轨迹进行移动。

为了使方案的表述更易理解，以实施例一至实施例三为基础，可选的，以一个实际应用的例子进行说明。对一个SCARA型四轴机械臂末端从坐标[0.6,0,0]^T直线运行到[0.289,0.156,-0.078]^T，然后使用上述实施例提出的算法，从当前位置规划出一个过渡段曲线到[0.189,0.2,-0.094]^T，然后直线运行到终点[0,0.2,0]^T值得注意的是，以下参数取值只是一种示例性取值，还可以存在其他取值，具体数值不同，机械臂末端移动轨迹的精度也不同，可以根据情况进行相关调整。图4为X-Y平面内机械臂末端的轨迹规划图，机械臂末端位置的X轴分量和机械臂末端位置的Y轴分量的单位均为米，曲线410为机械臂末端在该规划轨迹中的实际移动曲线。图5-图7为机械臂的前三个轴在任意时刻的位置，时间单位均为秒(s)，位置单位均为弧度(rad),曲线510、曲线610和曲线710分别代表机械臂前三个轴的位置曲线，点5101和点5102之间的曲线、点6101和点6102之间的曲线、点7101和点7102之间的曲线分别代表机械臂末端从所在点到预设点的位置曲线。图8-图10为机械臂的前三个轴在任意时刻的速度，时间单位均为秒(s)，速度单位均为弧度每秒(rad/s),曲线810、曲线910和曲线1010分别代表机械臂前三个轴的速度曲线，点8101和点8102之间的曲线、点9101和点9102之间的曲线、点10101和点10102之间的曲线分别代表机械臂末端从所在点到预设点的速度曲线。图11-图13为机械臂的前三个轴在任意时刻的加速度，时间单位均为秒(s)，加速度单位均为弧度每二次方秒(rad/s²)。曲线1110、曲线1210和曲线1310分别代表机械臂前三个轴的加速度曲线，点11101和点11102之间的曲线、点12101和点12102之间的曲线、点13101和点13102之间的曲线分别代表机械臂末端从所在点到预设点的加速度曲线。从图可以看到，在关节空间坐标系的各个轴的位置、速度和加速度都连续。

实施例四

图14是本发明实施例四中的一种机械臂的控制装置的结构图。该装置适用于执行本发明实施例一至三中提供的机械臂的控制方法，该装置具体包括：获取单元1410、确定单元1420和控制单元1430。

获取单元1410，用于获取机械臂末端在当前移动直线上的所在点的第一状态信息和所述机械臂末端在下一移动直线上预设点的第二状态信息；

确定单元1420，与获取单元1410相连，用于根据所述第一状态信息和所述第二状态信息确定从所述所在点至所述预设点的移动轨迹的参数，所述机械臂末端在所述移动轨迹上的移动函数为连续函数；

控制单元1430，与确定单元1420相连，用于控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点。

进一步的，所述第一状态信息包括所述机械臂末端在所述所在点的位置、速度和加速度；所述第二状态信息包括所述机械臂末端在所述预设点的位置、速度和加速度，获取单元1410包括：

信息获取子单元，用于获取所述机械臂末端在当前移动直线上的所在点的在关节空间下的第一状态信息；

信息转换子单元，与所述信息获取子单元相连，用于通过逆运动学方法，将所述预设点的第二状态信息由笛卡尔坐标表示转换成关节空间坐标表示。

进一步的，确定单元1420具体用于：通过S型速度规划法、多阶样条插值法和/或B样条插补法规划所述下一移动直线在笛卡尔坐标系下的插补点。

进一步的，所述移动函数包括：所述机械臂末端在所述移动轨迹上的位置函数、速度函数和加速度函数；确定单元1420还用于通过多阶多项式和/或三角函数法，分别确定所述位置函数、速度函数和加速度函数对应的参数。

进一步的，确定单元1420具体用于：根据公式进行笛卡尔坐标表示向关节空间坐标表示的转换，其中和分别为笛卡尔坐标系下机械臂末端的速度和加速度，和分别为关节空间坐标系下机械臂末端的速度和加速度，J为雅可比矩阵，为雅可比矩阵的一阶导，J⁺为J的伪逆。

进一步的，确定单元1420还用于根据如下五阶多项式曲线公式，确定所述位置函数、速度函数和加速度函数对应的参数；

a₀＝Q₀

$a_1={\stackrel{\·}{Q}}_0$

$a_2=\frac{1}{2}{\stackrel{\·\·}{Q}}_0$

$a_3=\frac{1}{2t_{trans}^3}\[20h-(8{\stackrel{\·}{Q}}_s+12{\stackrel{\·}{Q}}_0)t_{trans}-(3{\stackrel{\·\·}{Q}}_0-{\stackrel{\·\·}{Q}}_s)t_{trans}^2\]$

$a_4=\frac{1}{2t_{trans}^4}\[-30h+(14{\stackrel{\·}{Q}}_s+16{\stackrel{\·}{Q}}_0)t_{trans}+(3{\stackrel{\·\·}{Q}}_0-2{\stackrel{\·\·}{Q}}_s)t_{trans}^2\]$

$a_5=\frac{1}{2t_{trans}^5}\[12h-6({\stackrel{\·}{Q}}_s+{\stackrel{\·}{Q}}_0)t_{trans}+({\stackrel{\·\·}{Q}}_s-{\stackrel{\·\·}{Q}}_0)t_{trans}^2\]$

其中，Q₀、和分别为所述所在点的关节空间的位置、速度和加速度，Q_s、和分别为所述预设点在关节空间的位置、速度和加速度，h为所述所在点和所述预设点之间的距离，t_trans为机械臂末端从所述所在点移动至所述预设点的时间。

进一步的，确定单元1420还用于根据如下公式计算机所述所在点至所述预设点之间任意时刻的机械臂末端的位置、速度和加速度；

Q_t＝Q₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

${\stackrel{\·}{Q}}_t=a_1+2a_{2}t+3a_3{t}^2+4a_4{t}^3+5a_5{t}^4$

${\stackrel{\·\·}{Q}}_t=2a_2+6a_{3}t+12a_4{t}^2+20a_5{t}^3$

其中，Q_t、和分别为所述机械臂末端在所述所在点至所述预设点之间任意点的位置、速度和加速度。

本发明实施例中，通过获取机械臂末端在当前移动直线上的所在点和机械臂末端在下一移动直线上预设点状态信息，规划从所述所在点至所述预设点的移动轨迹，控制所述机械臂末端沿所述移动轨迹从所述所在点移动至所述预设点。实现了机械臂末端平稳的从一个运动方向切换至另一个运动方向，保证了机械臂运动的效率和精度。

显然，本领域技术人员应该明白，上述产品可执行本发明任意实施例所提供的方法，具备执行方法相应的功能模块和有益效果。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。