CN102554938A - 机器人的机械手末端轨迹跟踪方法 - Google Patents

Abstract

本发明提供一种机器人的机械手末端轨迹跟踪方法，包括：将用户预先定义的机器人的机械手末端运动轨迹用数学形式表示；该用数学形式表示的运动轨迹位于关节空间中；根据机器人的动力学方程、机器人的应用场景以及用户预先定义的机械手末端运动轨迹建立目标函数以及约束方程；所述目标函数、约束方程中的函数都能被转换为凸函数；将目标函数、约束方程中的函数都转换为凸函数，形成一凸规划的优化模型，将所述凸规划的优化模型做离散化后求解，得到机器人各个关节的运动量；用机器人各个关节运动量驱动机器人。本发明具有计算速度快、易于扩展的优点。

Description

机器人的机械手末端轨迹跟踪方法

技术领域

本发明涉及机器人控制领域，特别涉及一种机器人的机械手末端轨迹跟踪方法。

背景技术

机器人在工业生产领域已经得到了广泛应用，如利用机器人进行焊接或组装等危险系数较大、体力要求较高的工作。随着文化产业的快速推广，摄像机器人也开始得到普及与应用。与传统拍摄方式相比，摄像机器人具有可重复、高精度、高强度和高速可控等优点，而且也能代替人完成在危险场景中的拍摄。无论机器人应用于何种领域，都要求机器人在环境中优质、高效、安全地按照任务的要求开展工作。这就需要对机器人控制与运动规划进行研究。机器人的运动规划着重研究如何控制机器人的运动轨迹，使机器人沿规定的路径运动。由于运动规划要使得机器人的实际运动轨迹与预定义的运动轨迹尽量一致，因此运动规划也被称为轨迹跟踪。

对于工业机器人，轨迹跟踪的研究内容主要为机械手(即机器人末端)轨迹跟踪。机械手轨迹跟踪是在机器人运动学和动力学的基础上，讨论在关节空间和笛卡尔空间中机器人运动的轨迹和如何生成这些轨迹的方法。所谓轨迹是指机器人在运动过程中的位移、速度和加速度。机械手轨迹跟踪对机械手的运行效率、平稳性、作业精确度和能量消耗具有重要的意义。通常情况下，在轨迹跟踪中，机械手的期望轨迹是事先给定的一系列笛卡尔空间中的点，约束条件为机器人各关节运动所允许的最大速度、加速度以及力矩。轨迹跟踪的目的就是根据这些约束条件，计算出使得机械手准确、平稳地通过这些点的平滑轨迹。其中，准确指机械手运动轨迹满足路径约束；而平稳是指运行过程中轨迹平滑、速度处处连续，它不仅是机械手自身保持良好运行状态的前提，也常常是操作任务对机械手的一个必不可少的要求。

为了使得轨迹跟踪能够满足上述准确、平稳的要求，人们提出了各种各样最优轨迹的优化函数，如时间最短、加速度最小、能量消耗最小等，通过这些优化函数进行轨迹规划，从而得到用于描述最优轨迹的位移、速度和加速度等信息。现有的一些轨迹跟踪方法，如在参考文献1“机械手的运动学最短时间轨迹规划，叶桦，冯纯伯，东南大学学报，1990，20(3)74-79”以及参考文献2“Choi Y.K.，Park J.H.，Kim H.S.，Kim J.H.，Optimal trajectoryplanning and sliding mode control for robots using evolution strategy，Robotica(2000)，18(4)：423-428”中，仅仅考虑了机器人的运动学特性，对轨迹跟踪问题作了过度的简化，不能完全体现问题的动力学特征。而另外一些方法，如在参考文献3“王建滨，马培荪，徐军等.基于超冗余度机械臂动力学的时间最优轨迹规划.上海交通大学学报，2002，36(9)：1360-1364”以及参考文献4“Lee Y.D.Lee B.H.Kim H.G.An evolutionary approach fortime optimal trajectory planning for a robotic manipulator.InformationSciences，1999，113：245-260”中，则采用了局部搜索算法，其算法性能与初始条件的选取紧密相关，不能保证每次求解都能得到理想的结果。此外，现有的轨迹跟踪方法普遍计算量较大，计算速度较慢，不能很好地满足工业应用的需求。

与其他的工业机器人相比，摄像机器人的轨迹跟踪有特殊之处。摄像机机器人的机械手路径跟踪(即对摄像机运动轨迹的跟踪)中不要求机械手运行时间最优(即使得机械手在最短的时间内沿着指定轨迹运行结束)，而是要求机械手在指定的时间点到达轨迹上的某一点。这一特点也使得现有的应用于工业机器人的轨迹跟踪方法不能直接适用于摄像机器人的轨迹跟踪。

发明内容

本发明的目的是克服现有的轨迹跟踪方法计算量大、计算速度慢的缺陷，从而提供一种高速的机器人机械手末端轨迹跟踪方法。

为了实现上述目的，本发明提供了一种机器人的机械手末端轨迹跟踪方法，包括：

步骤1)、将用户预先定义的机器人的机械手末端运动轨迹用数学形式表示；该用数学形式表示的运动轨迹位于关节空间中；

步骤2)、根据机器人的动力学方程、机器人的应用场景以及步骤1)中用户预先定义的机械手末端运动轨迹建立目标函数以及约束方程；所述目标函数、约束方程中的函数都能被转换为凸函数；

步骤3)、将由步骤2)得到的目标函数、约束方程中的函数都转换为凸函数，形成一凸规划的优化模型，将所述凸规划的优化模型做离散化后求解，得到机器人各个关节的运动量；

步骤4)、用步骤3)得到的机器人各个关节运动量驱动机器人。

上述技术方案中，所述1)包括：

步骤1-1)、将用户预先定义的机器人的机械手末端运动轨迹用笛卡尔空间中的弧长参数表示：

步骤1-2)、将弧长参数表示的笛卡尔空间中的末端轨迹转换为弧长参数表示的关节运动轨迹。

上述技术方案中，所述目标函数为：

$\underset{a\left(s\right),b\left(s\right),\mathrm{\τ}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\Σ}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

其中，s表示弧长参数，L+1表示关键点的数量，p_l表示第l个关键点，

表示在p_l上的弧长参数s的值，

表示期望机器人末端由第l-1个关键点到达第l个关键点的时间，

分别为s的一阶导数和二阶导数；

所述约束方程为：

τ(s)＝M(s)a(s)+C(s)b(s)+G(s)

$b\left(0\right)={\stackrel{.}{s}}_0^2$

$b\left(1\right)={\stackrel{.}{s}}_T^2$

b′(s)＝2a(s)

b(s)≥0

$b\left(s\right)\≤\stackrel{\‾}{b}\left(s\right)$

$\underset{\‾}{f}\left(s\right)\≤f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\≤\stackrel{\‾}{f}\left(s\right)$

$\underset{\‾}{\mathrm{\τ}}\left(s\right)\≤\mathrm{\τ}\left(s\right)\≤\stackrel{\‾}{\mathrm{\τ}}\left(s\right)$

上述公式的s取值范围为[0，1]

其中，τ为关节力，τ(s)、分别表示关节力允许的最小、最大值；M为正定的质量矩阵；C为与科里奥利力和离心力相关的矩阵；G代表重力；

f(s)、

分别表示角速度平方的最大值、关节角加速度的最小值以及最大值；f(s)、h(s)为两个关于s的函数。

上述技术方案中，在所述的步骤3)中，将所述的目标函数转换为凸函数包括：

步骤3-1-1)、将所述目标函数转换为一个目标函数与一个约束方程：

$\underset{a\left(s\right),b\left(s\right),\mathrm{\τ}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\Σ}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

步骤3-1-2)、将步骤3-1-1)所得到的目标函数与约束方程和其它已有的约束方程相结合，组成一凸规划的优化模型。

上述技术方案中，在所述的步骤3)中，将所述凸规划的优化模型做离散化后求解包括：

步骤3-2-1)、对弧长参数s进行均匀采样，得到离散型变量(s₀，s₁，…s_K)；

步骤3-2-2)、根据步骤3-2-1)所得到的K+1个离散型变量为所述凸规划的优化模型中与弧长参数s有关的函数进行离散化，形成离散化后的凸规划的优化模型。

上述技术方案中，在所述的步骤3)中，采用二阶锥规划、Subgradient投影法、内点法中的一种对离散化后的凸规划的优化模型进行求解。

上述技术方案中，所述运动量包括关节的离散速度、加速度以及力矩。

本发明的优点在于：

1、计算速度快、效率高。

2、对用户的需求简单，方便用户操作。

3、具有良好的扩展性。

附图说明

图1为在一个实施例中用户在拍摄前为摄像机器人预先设定的运动轨迹的示意图；

图2为本发明的一个实施例的流程图；

图3为跃迁函数的示意图；

图4为分段线性函数的示意图。

具体实施方式

下面结合附图和具体实施方式对本发明加以说明。

由于导演等摄像行业从业人员一般都不具备机器人运动控制领域的专业知识，因此用户通常只能给出粗略的基本信息，这些信息包括用户预先设定的机器人机械手末端(对于摄像机器人而言，其机械手的末端即为摄像机)的运行轨迹，包括：摄像机在运动时所要到达的关键点，到达这些关键点的时间，以及摄像机在关键点所在位置上的姿态等。如在图1中给出了用户在拍摄前为摄像机器人预先设定的运动轨迹的示意图。利用这些信息并不能直接控制摄像机器人按照预先设定的轨迹运动，本发明的方法所要做的就是根据预先定义的机器人机械手的运行轨迹，得到摄像机器人各个关节的加速度，在满足各关节约束以及关键点到达时间约束的同时，使得在这些关节加速度的控制下实际运行的运动轨迹与预先给定的运动轨迹尽量一致。这种如何令机器人的机械手末端以特定的姿态沿着预先给定的路径运动，并且在特定的时间点到达预先给定路径中特定点的问题就是本申请中所提到的轨迹跟踪问题。在对本发明做详细说明前，首先对本发明中所涉及到的一些基本概念进行说明。

凸规划：凸规划是一类比较简单，而且具有一些良好性质的非线性规划。凸规划的可行域为凸集，其局部极小点就是全局极小点，而且局部极小点的全体构成一个凸集。当凸规划的目标函数为严格凸函数时，其全局极小点唯一。

凸规划的标准形式为：

$\underset{x}{\min }f\left(x\right)$

s.t.g_i(x)≤0，i＝1，…m

    h_i(x)＝0，i＝1，…p

其中，f(x)、g_i(x)为凸函数，h_i(x)为仿射函数。

凸规划的相关内容可参见：

http://en.wikipedia.org/wiki/Convex_optimization。

关节空间与笛卡尔空间：在工业机器人中，有用于描述关节运动的关节坐标系，有用于描述机械人末端的位置和姿态的末端笛卡尔坐标系。假定机器人具有n个自由度，那么机器人关节坐标系为n维空间，称之为关节空间。一般情况下，末端笛卡尔坐标系用三维平动以及三维转动描述，称之为笛卡尔空间。

在对本发明中的相关概念做上述说明的基础上，下面结合图2，就用于实现本发明方法的相关实施例进行说明。

步骤1)、将用户预先定义的运动轨迹用数学形式表示。

一般来说，适用于本发明方法的摄像机器人会向用户提供一个人机交互接口，如触摸屏，用户可以在该人机交互接口上绘制所需要的运动轨迹。该接口接收到的运动轨迹表示在笛卡尔空间中，本实施例中将这些笛卡尔空间的运动轨迹转化为用弧长参数表示的数学形式，如B样条曲线，贝赛尔曲线。

如何将运动轨迹用数学方式表示为本领域公知常识，在参考文献1：“萧树铁主编的《代数与几何》(高等教育出版社，2003年7月第2版)”中对此也有详细说明。

步骤2)、根据用户预先定义的运动轨迹计算摄像机器人在关节空间中的运动轨迹。

在机器人运动学中，由机器人关节坐标系的坐标到机器人末端的位置和姿态之间的映射，称为机器人的正向运动学。由机器人末端的位置和姿态到机器人关节坐标系的坐标之间的映射，称为逆向运动学。显而易见，用户预先定义的运动轨迹就是摄像机器人在末端的位置和姿态，因此由用户预先定义的运动轨迹计算摄像机器人在关节空间中的运动轨迹属于逆向运动学。根据现有技术中已知的逆向运动学方法可以将用户预定义的运动轨迹从笛卡尔空间转换到关节空间，从而得到在关节空间中由弧长参数表示的运动轨迹。

如何利用逆向运动学方法求解关节空间运动轨迹可参见参考文献2：“谭民等编著的《先进机器人控制》(高等教育出版社，2007年5月第1版)”。

步骤3)、根据机器人的结构建立该机器人的动力学方程。

假设摄像机器人的自由度为n；摄像机器人的关节角度为q，关节角速度为

关节角加速度为

(Rⁿ表示n维实数空间)；关节力为τ，τ∈Rⁿ；则机器人的动力学方程表示如下：

$\mathrm{\τ}=M\left(q\right)\stackrel{..}{q}+C(q,\stackrel{.}{q}){\stackrel{..}{q}}^2+F_s\left(q\right)\mathrm{sgn}\left(\stackrel{.}{q}\right)+G\left(q\right)---\left(1\right)$

其中，M(q)∈R^n×n为正定的质量矩阵，

为与科里奥利力和离心力相关的矩阵，F_s(q)∈R^n×n为摩擦力，G(q)代表重力。在本申请中，摩擦力忽略不计。

本步骤中所涉及的建立机器人动力学方程的实现方法也是本领域技术人员所公知的，在参考文献3：“L.Sciavicco and B.Siciliano，Modelingand Control of Robot Manipulators.New York：McGraw-Hill，1996”中有详细描述。

步骤4)、根据步骤2)得到的用户预定义的机器人机械手末端的运动轨迹，以及由机器人的应用场景所得到的运动初始点的约束、运动末端点的约束以及各个力学约束建立由目标函数与约束方程所组成的优化模型。本步骤中所涉及的力学约束包括步骤3)建立的机器人动力学方程，各关节速度最大值最小值，加速度最大值最小值，各个关节驱动力的最大值最小值。

步骤5)、将步骤4)生成的优化模型转化为凸规划，然后对其求解，以得到机器人各个关节的运动量。

由于对由目标函数与约束条件所生成的优化模型的常规求解方法较慢，且求得的解未必能保证全局最优，因此，在本步骤中需要将所述优化模型转化为凸规划后再求解。

步骤6)、将步骤5)得到的各个关节的运动量作为输入，驱动摄像机器人运动，即可得到用户要求的机械手轨迹。

下面对步骤4)中的具体内容做进一步说明。

步骤41)、建立最优化的目标函数。

由于摄像机器人的机械手运行轨迹需要在给定的时间到达给定的关键路径点。所以在优化过程中将各个关键点到达时间的误差作为优化的目标函数。优化时，要使得目标函数最优化，就应使得此目标函数尽可能地接近于零。目标函数接近于零，说明运行轨迹经过各个路径关键点的时间与预期相符。

步骤42)、添加约束条件。

考虑到摄像机器人的机械手的各个关节运动速度、加速度约束以及各个关节力矩都具有一定范围，故在优化模型中添加这些内容作为优化的约束条件，得到约束方程。此外，摄像机器人的运动需要满足动力学方程，因此机器人的动力学方程也要加入到约束方程当中。

根据步骤41)和步骤42)所建立的目标函数以及约束条件，可以得到如下的优化模型，所述的优化模型的数学表达式如下：

$\underset{a\left(s\right),b\left(s\right),\mathrm{\τ}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\Σ}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|---\left(2\right)$

τ(s)＝M(s)a(s)+C(s)b(s)+G(s)              (3)

$b\left(0\right)={\stackrel{.}{s}}_0^2---\left(4\right)$

$b\left(1\right)={\stackrel{.}{s}}_T^2---\left(5\right)$

b′(s)＝2a(s)                              (6)

b(s)≥0                                    (7)

$b\left(s\right)\≤\stackrel{\‾}{b}\left(s\right)---\left(8\right)$

$\underset{\‾}{f}\left(s\right)\≤f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\≤\stackrel{\‾}{f}\left(s\right)---\left(9\right)$

$\underset{\‾}{\mathrm{\τ}}\left(s\right)\≤\mathrm{\τ}\left(s\right)\≤\stackrel{\‾}{\mathrm{\τ}}\left(s\right)---\left(10\right)$

上述公式的s取值范围为[0，1]

其中，s表示弧长参数，

分别为s的一阶导数和二阶导数，L+1表示关键点的数量，p_l表示第l个关键点，

表示在p_l上的弧长参数s的值，

表示期望机器人末端由第l-1个关键点到达第l个关键点的时间，

τ(s)、分别表示关节力允许的最小、最大值。

f(s)、

分别表示角速度平方的最大值、关节角加速度的最小值以及最大值。f(s)、h(s)为两个关于s的函数。

在上述优化模型的各个数学表达式中，数学表达式(2)表示之前提到的目标函数；数学表达式(3)为作为约束条件的机器人动力学方程；数学表达式(4)、(5)为机器人初始点、末端点的速度约束；数学表达式(6)、(7)可以由 推导得到：

即即b(s)≥0。数学表达式(8)、(9)为摄像机器人各关节速度与加速度的约束，推导过程如下：

第i个关节的角速度约束为：

$-{\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)\≤{\stackrel{.}{q}}_i\left(s\right)\≤{\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)$

$\⇔{\left({\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)\right)}^2={\left(q_i^{\′}\left(s\right)\stackrel{.}{s}\right)}^2={\left(q_i^{\′}\left(s\right)\right)}^{2}b\left(s\right)\≤{\left({\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)\right)}^2$

$\⇔b\left(s\right)\≤\frac{{\left({\stackrel{\‾}{\stackrel{.}{q}}}_i\left(s\right)\right)}^2}{{\left(q_i^{\′}\left(s\right)\right)}^2}$

令

将各个关节的角速度约束写作向量形式，就可以得到数学表示式(8)。

第i个关节的角加速度约束为：

${\underset{\‾}{\overset{..}{q}}}_i\left(s\right)\≤{\stackrel{..}{q}}_i\left(s\right)\≤{\stackrel{\‾}{\stackrel{..}{q}}}_i\left(s\right)$

$\⇔{\underset{\‾}{\overset{..}{q}}}_i\left(s\right)\≤q_i^{\′}\left(s\right)\stackrel{..}{s}+q_i^{\′\′}\left(s\right){\stackrel{.}{s}}^2\≤{\stackrel{\‾}{\stackrel{..}{q}}}_i\left(s\right)$

$\⇔{\underset{\‾}{\overset{..}{q}}}_i\left(s\right)\≤q_i^{\′}\left(s\right)a\left(s\right)+q_i^{\′\′}\left(s\right)b\left(s\right)\≤{\stackrel{\‾}{\stackrel{..}{q}}}_i\left(s\right)$

令将各个关节的角速度约束写作向量形式，就可以得到数学表示式(9)。

上述的公式推导中，b’(s)、

分别表示b(s)对s求一阶偏导数以及对t求导数；q_i(s)、

分别表示第i个关节的角度、角速度以及角加速度； 表示第i个关节角速度的最小值、最大值，

分别表示第i个关节角加速度的最小值、最大值；q_i’(s)、q_i”(s)表示q_i(s)对s求一阶、二阶偏导数。

上述优化模型为未转化前的优化模型，因此标记为优化模型(I)。

由于优化模型(I)中的目标函数为非凸函数，所以优化模型(I)属于非凸规划。鉴于之前所提到的凸规划的优点，在步骤5)中需要将优化模型转化为凸规划，然后进行离散化得到离散的凸规划模型。下面对该步骤做进一步说明。

步骤51)、首先，将上述优化的目标函数进行如下替换

$\underset{a\left(s\right),b\left(s\right),\mathrm{\τ}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\Σ}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

从而得到新的优化模型

$\underset{a\left(s\right),b\left(s\right),\mathrm{\τ}\left(s\right)}{\mathrm{MIN}}\underset{s_0}{\overset{s_L}{\∫}}b\left(s\right)\mathrm{ds}---\left(11\right)$

τ(s)＝M(s)a(s)+C(s)b(s)+G(s)

$\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\≤0---\left(12\right)$

上述公式中l分别取以下数值：1…L

$b\left(0\right)={\stackrel{.}{s}}_0^2$

$b\left(1\right)={\stackrel{.}{s}}_T^2$

b′(s)＝2a(s)

b(s)≥0

$b\left(s\right)\≤\stackrel{\‾}{b}\left(s\right)$

$\underset{\‾}{f}\left(s\right)\≤f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\≤\stackrel{\‾}{f}\left(s\right)$

$\underset{\‾}{\mathrm{\τ}}\left(s\right)\≤\mathrm{\τ}\left(s\right)\≤\stackrel{\‾}{\mathrm{\τ}}\left(s\right)$

上述公式中s取值范围为[0，1]

经过上述转化后的优化模型中的目标函数以及约束方程中的函数都是凸函数，所以上述优化模型为凸规划。将这一优化模型记为优化模型(II)。

步骤52)、将优化模型转化为凸规划后，下面对优化模型(II)进行离散化。

首先，将摄像机器人的运行轨迹均匀离散化为K段，得到K+1个离散变量。之前提到，本实施例中，摄像机器人的运行轨迹用弧长参数s表示，因此所生成的离散变量用(s₀，s₁，…s_K)表示。

接着，根据之前所得到的K+1个离散型变量为前述凸规划的优化模型(II)中与弧长参数s有关的函数进行离散化，形成离散化后的凸规划的优化模型。

具体的说，对与弧长参数s有关的函数进行离散化包括：

a)、对目标函数

使用梯形公式计算定积分，得到

该计算结果等价于

梯形公式计算定积分可参见文献4：“关冶 陆金甫编《数值分析基础》(高等教育出版社，1998年5月第1版)”。

b)、之前提到，

它表示用于控制摄像机器人运动的输入信号，故假设其为如图3所示的阶跃函数，而

为如图4所示的分段线性函数，因此b(s)可分段表示为以下线性函数：

$b\left(s\right)=b^k+\left(\frac{b^{k+1}-b^k}{s^{k+1}-s^k}\right)(s-s^k)$

进一步的，优化模型(II)中约束

中的积分项在[s_k，s_k+1]可解析计算得：

$\underset{s^k}{\overset{s^{k+1}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}=\frac{2(s^{k+1}-s^k)}{\sqrt{b^{k+1}}+\sqrt{b^k}}$

基于上述描述，优化模型(II)离散化后，得到如下凸规划：

$\underset{a^k,b^k,{\mathrm{\τ}}^k}{\mathrm{Min}}\underset{k=0}{\overset{K}{\mathrm{\Σ}}}\left(b^k\right)---\left(14\right)$

s.t.τ^k＝m(s^k)a^k+c(s^k)s^k+g(s^k)                  (15)

$b^0={\stackrel{.}{s}}_0^2---\left(16\right)$

$b^K={\stackrel{.}{s}}_k^2---\left(17\right)$

(b^k+1-b^k)＝2a^k(s^k+1-s^k)                         (18)

b^k≥0andb^K≥0                                   (19)

$b^k\≤\stackrel{\‾}{b}\left(s^k\right)\mathrm{and}{b}^K\≤\stackrel{\‾}{b}\left(s^K\right)---\left(20\right)$

f(s^k+1/2)≤f(s^k+1/2)a^k+h(s^k+1/2)b^k+1/2          (21)

$f\left(s^{k+1/2}\right)a^k+h\left(s^{k+1/2}\right)b^{k+1/2}\≤\stackrel{\‾}{f}\left(s^{k+1/2}\right)---\left(22\right)$

$\underset{\‾}{\mathrm{\τ}}\left(s^{k+1/2}\right)\≤{\mathrm{\τ}}^k\≤\stackrel{\‾}{\mathrm{\τ}}\left(s^{k+1/2}\right)---\left(23\right)$

上述公式中k分别取以下数值：0…K-1

$\underset{k=k_{\mathrm{li}}}{\overset{k_{\mathrm{le}}}{\mathrm{\Σ}}}\frac{2(s^{k+1}-s^k)}{\sqrt{b^{k+1}}+\sqrt{b^k}}\≤T_l^E---\left(24\right)$

上述公式中l分别取以下数值：0…L-1

上述公式中，x^k(x可以为a，b，τ，s)、x^k+1分别表示x在第k、k+1个离散点的值，用x^k+1/2表示x在第k、k+1个离散点中间点的值，x⁰、x^K表示x在初始点、末端点处的取值；k_li…k_le为

，对应的离散序列号。上述经过离散化的凸规划模型记为优化模型(III)。

步骤53)、对凸规划的优化模型进行求解。

由于经过转换的优化模型为凸规划模型，因此可以对该模型采用通用的非线性求解工具进行求解。在本实施例中，可以将上述优化模型转化为二阶锥规划，然后使用专门求解二阶锥规划的求解器实现更加有效、快速的求解。但在其它实施例中，也可以采用诸如Subgradient投影法、内点法等方法对已经转化为凸规划模型的优化模型进行求解。对优化模型进行求解所得到的结果为各个关节的离散速度b、加速度a、力矩τ。

在上述关于摄像机器人的实施例中，目标函数与约束方程中的函数都可以转换为凸函数，因此可以将由目标函数与约束方程所形成的优化模型转换为凸规划模型，然后利用凸规划模型的特性进行快速求解。在其他实施例中，本发明的方法也可以适用于其他类型机器人的机械手末端轨迹跟踪，只要这些机器人的目标函数与约束方程中的约束函数也能转换为凸函数。

本发明的优点在于：

1、本发明方法在优化求解过程中，将优化模型转化为凸规划模型，利用凸规划模型的特点实现了有效、快捷的求解过程。

2、本发明方法使用简单，用户只要提供一条机械手轨迹即可实现轨迹跟踪。

3、本发明方法具有良好的扩展性，可以在优化模型中添加其他的约束函数与目标函数。只要约束函数与目标函数是凸函数，那么新的问题也能够有效快捷的得到求解。

基于上述优点，利用本发明所公开的方法，能够有效快速地求解得到各关节空间中加速度，从而来控制各关节运动，得到精确跟踪导演给定轨迹，且满足各关节力学约束。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (7)

1.一种机器人的机械手末端轨迹跟踪方法，包括：

步骤1)、将用户预先定义的机器人的机械手末端运动轨迹用数学形式表示；该用数学形式表示的运动轨迹位于关节空间中；

步骤2)、根据机器人的动力学方程、机器人的应用场景以及步骤1)中用户预先定义的机械手末端运动轨迹建立目标函数以及约束方程；所述目标函数、约束方程中的函数都能被转换为凸函数；

步骤3)、将由步骤2)得到的目标函数、约束方程中的函数都转换为凸函数，形成一凸规划的优化模型，将所述凸规划的优化模型做离散化后求解，得到机器人各个关节的运动量；

步骤4)、用步骤3)得到的机器人各个关节运动量驱动机器人。

2.根据权利要求1所述的机器人的机械手末端轨迹跟踪方法，其特征在于，所述1)包括：

步骤1-1)、将用户预先定义的机器人的机械手末端运动轨迹用笛卡尔空间中的弧长参数表示：

步骤1-2)、将弧长参数表示的笛卡尔空间中的末端轨迹转换为弧长参数表示的关节运动轨迹。

3.根据权利要求1所述的机器人的机械手末端轨迹跟踪方法，其特征在于，所述目标函数为：

$\underset{a\left(s\right),b\left(s\right),\mathrm{\τ}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\Σ}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

其中，s表示弧长参数，L+1表示关键点的数量，p_l表示第l个关键点，表示在p_l上的弧长参数s的值，

表示期望机器人末端由第l-1个关键点到达第l个关键点的时间，

分别为s的一阶导数和二阶导数；

所述约束方程为：

τ(s)＝M(s)a(s)+C(s)b(s)+G(s)

$b\left(0\right)={\stackrel{.}{s}}_0^2$

$b\left(1\right)={\stackrel{.}{s}}_T^2$

b′(s)＝2a(s)

b(s)≥0

$b\left(s\right)\≤\stackrel{\‾}{b}\left(s\right)$

$\underset{\‾}{f}\left(s\right)\≤f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\≤\stackrel{\‾}{f}\left(s\right)$

$\underset{\‾}{\mathrm{\τ}}\left(s\right)\≤\mathrm{\τ}\left(s\right)\≤\stackrel{\‾}{\mathrm{\τ}}\left(s\right)$

上述公式的s取值范围为[0，1]

其中，τ为关节力，τ(s)、

分别表示关节力允许的最小、最大值；M为正定的质量矩阵；C为与科里奥利力和离心力相关的矩阵；G代表重力；

f(s)、

分别表示角速度平方的最大值、关节角加速度的最小值以及最大值；f(s)、h(s)为两个关于s的函数。

4.根据权利要求3所述的机器人的机械手末端轨迹跟踪方法，其特征在于，在所述的步骤3)中，将所述的目标函数转换为凸函数包括：

步骤3-1-1)、将所述目标函数转换为一个目标函数与一个约束方程：

$\underset{a\left(s\right),b\left(s\right),\mathrm{\τ}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\Σ}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\∫}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-T_l^E\left|\right|$

步骤3-1-2)、将步骤3-1-1)所得到的目标函数与约束方程和其它已有的约束方程相结合，组成一凸规划的优化模型。

5.根据权利要求4所述的机器人的机械手末端轨迹跟踪方法，其特征在于，在所述的步骤3)中，将所述凸规划的优化模型做离散化后求解包括：

步骤3-2-1)、对弧长参数s进行均匀采样，得到离散型变量(s₀，s₁，…s_K)；

步骤3-2-2)、根据步骤3-2-1)所得到的K+1个离散型变量为所述凸规划的优化模型中与弧长参数s有关的函数进行离散化，形成离散化后的凸规划的优化模型。

6.根据权利要求5所述的机器人的机械手末端轨迹跟踪方法，其特征在于，在所述的步骤3)中，采用二阶锥规划、Subgradient投影法、内点法中的一种对离散化后的凸规划的优化模型进行求解。

7.根据权利要求1所述的机器人的机械手末端轨迹跟踪方法，其特征在于，所述运动量包括关节的离散速度、加速度以及力矩。

Images