CN102122172B - 机器运动控制的摄像系统及其控制方法 - Google Patents

Abstract

本发明提供本发明提供了一种机器运动控制的摄像系统，包括：导轨10，机器人本体20，云台装置30，摄像机40，用于控制所述摄像机40的摄像机控制模块50，用于控制所述机器人本体20、云台装置30的机器人控制模块60，以及用于对所述摄像机控制模块50、机器人控制模块60进行联动控制的机器运动控制摄像模块70；其中，机器人本体20在所述导轨10上滑动，机器人本体20的机械手末端安装有云台装置30，云台装置30上安装至少一台所述摄像机40；所述导轨10、机器人本体20、云台装置30组成一九自由度的机器人。

Description

机器运动控制的摄像系统及其控制方法

技术领域

本发明涉及运动控制拍摄领域，特别涉及一种自动控制和精确重复拍摄轨迹的摄影系统与方法。

背景技术

影视合成技术是电影特技的重要手段之一，它是指将多层源素材混合成单一重复画面的处理过程。早期的影视合成技术主要是在胶片基础上实现的，诸如“抠像”、“叠画”等合成的方法与手段都在早期的影视制片中得到了较为广泛的应用。当合成的影片的拍摄轨迹是运动的而不是静止不变时，就需要维持高度精确的摄影机运动的控制。为了实现运动拍摄场景的合成效果，相同运动轨迹的移动拍摄需要重复多次。场景合成的效果取决于重复运动拍摄的精度。

国内现有的摄像系统将摄像机放置在一个固定的支架上，或放置在可以滑动的导轨上，或放置在一个高架臂上。在拍摄时人工手动控制拍摄的视角、轨迹和速度等。这种拍摄方式可以满足普通的拍摄需求，但如果需要对两个场景做无缝合成时，即使是富有经验和能力的摄影师，也无法用手工控制的方式精确重复相同拍摄轨迹和速度，这就影响了场景合成的质量。这就需要一套自动控制的摄影系统，使得摄影机都能完全重复实现相同的拍摄速度和拍摄轨迹。

为了实现这种重复精确拍摄的功能，需要由精密电动机器替代手工方式对摄影装置自动控制，运动的路线轨迹和摄像机参数需要事先编辑和模拟显示，同时需要保证规划的拍摄路径的安全性、可重复性，以及需要满足影视行业人士的拍摄要求。目前国内没有满足这一需求的拍摄设备和系统。而国外的相关设备是7轴运动控制设备，且部分机器关节采用液压动力，影响了整体拍摄的灵活性和精确性。

发明内容

本发明的目的是克服现有的自动摄像系统灵活性、精确性不够的缺陷，从而提供一种灵活、精确的摄像系统。

为了实现上述目的，本发明提供了一种机器运动控制的摄像系统，包括：导轨10，机器人本体20，云台装置30，摄像机40，用于控制所述摄像机40的摄像机控制模块50，用于控制所述机器人本体20、云台装置30的机器人控制模块60，以及用于对所述摄像机控制模块50、机器人控制模块60进行联动控制的机器运动控制摄像模块70；其中，

所述机器人本体20在所述导轨10上滑动，所述机器人本体20的机械手末端安装有所述云台装置30，所述云台装置30上安装至少一台所述摄像机40；所述导轨10、机器人本体20、云台装置30组成一九自由度的机器人，所述导轨10有第一自由度；所述机器人本体20的肩关节有第二自由度，肘关节有第三自由度，腕关节有第四自由度，手关节有第五、第六、第七自由度；所述云台装置30有第八和第九自由度；所述第七自由度、第八自由度、第九自由度的轴线重合在一点，分别映射三维笛卡儿空间的三个旋转方向；

所述机器运动控制摄像模块70根据用户设定的运动轨迹做运动轨迹规划与姿态规划，然后根据所述运动轨迹规划的结果求解所述九自由度机器人的前六个自由度的关节空间，根据所述姿态规划的结果求解所述九自由度机器人的后三个自由度的关节空间；接着通过仿真选取若干个用于运动驱动的关节空间点；再建立机器人动力学方程，结合所述机器人动力学方程、所述关节空间点以及机器人的约束条件建立优化模型，将所述优化模型转化为凸规划后，对其求解，得到所述九自由度机器人各个关节的运动量。

本发明还提供一种在所述的摄像系统上实现机器运动控制的方法，包括：

步骤10)、利用用户提供的所述机器人的末端手在笛卡儿空间中多个点上的位置与姿态的信息，实现所述机器人末端手的运动轨迹规划与姿态规划，得到机器人末端手在整个驱动过程中的位置以及机器人末端手在整个驱动过程中的姿态；

步骤20)、由步骤10)得到的机器人末端手的位置信息求解某一时间点所述机器人从第一自由度到第六自由度的关节空间，由步骤10)得到的机器人末端手的姿态信息求解所述时间点上所述机器人从第七自由度到第九自由度的关节空间；所得到的九个自由度的关节空间用于驱动机器人的运动；

步骤30)、利用步骤20)所得到的所述机器人在若干个时间点上的九个自由度的关节空间信息做仿真，根据仿真结果从所述若干个时间点上的九个自由度的关节空间信息中选取较佳值作为控制点；

步骤40)、利用前一步骤得到的结果实现所述九自由度机器人的机器运动控制。

上述技术方案中，在所述的步骤30)与步骤40之间还包括：

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

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

上述技术方案中，在所述的步骤1)中，所述的运动轨迹规划包括：

用户提供的所述机器人的末端手在笛卡儿空间中多个点上的位置信息做曲线拟合，得到至少一条运动轨迹曲线。

上述技术方案中，所述的曲线拟合采用B样条曲线法，或贝塞尔曲线法，或B样条曲线与贝塞尔曲线结合的方法。

上述技术方案中，在所述的步骤1)中，所述的姿态规划包括：

步骤a)、将笛卡儿空间中两个已知姿态信息的相邻点分别作为起始位置与目标位置，将所述起始位置与目标位置的姿态信息由欧拉角的形式修改为四元数的形式，分别用Q₁和Q₂表示；

步骤b)、求取所述起始位置与目标位置间任意时刻下的姿态，即：Q_n＝Q₁+(Q₂-Q₁)×(t_n-t₁)；

步骤c)、将步骤b)求得的姿态转换回欧拉角形式。

上述技术方案中，所述的步骤2)包括：

步骤2-1)、从步骤1)所得到的整个驱动过程中的位置信息与姿态信息中选取若干个时间点上的位置信息与姿态信息；

步骤2-2)、将步骤2-1)所得到的所述若干个时间点上的位置信息分别代入梯度投影法的计算公式，得到该若干个时间上各自的所述机器人从第一自由度到第六自由度的关节空间；

步骤2-3)、由步骤2-2)计算得到的所述机器人从第一自由度到第六自由度的关节空间，以及步骤2-1)所得到的所述若干个时间点上的姿态信息计算出姿态的调整量，由姿态的调整量计算出所述若干个时间点上各自的所述机器人从第七自由度到第九自由度的关节空间。

上述技术方案中，在所述的步骤2-2)中，所述的梯度投影法的计算公式为：

上述公式中，

表示关节速度，J是雅克比矩阵，J⁺是J的伪逆矩阵，

是所述位置对时间的一阶导数，

为自由向量的集合，α代表放大系数；其中，

β_W，β_D，β_L，β_R为加权系数，它们是一个值在[0，1]的数；

W为可操作度函数， $W=\sqrt{\det \left({\mathrm{JJ}}^T\right)};$

D(q)为避障函数，

q代表关节空间中的某一姿态，i代表胶囊状的包围盒的编号，j代表障碍物的编号，d₀代表安全距离的阈值，η为一系数；

L(θ)为关节约束函数，为各关节允许范围的中值，q_imax表示第i个关节角度最大的值，q_imin表示第i个关节角度最小的值，n为关节数；

为一个9维向量，

λ表示

表示梯度值。

上述技术方案中，所述的步骤2-3)包括：

步骤2-3-1)、由步骤2-2)计算得到的所述机器人从第一自由度到第六自由度的关节空间计算出某一时刻的姿态R₁，由步骤1)的姿态规划所生成的姿态信息得到在目标位置的姿态为R₂，计算机器人末端手从某一时刻到目标位置所需要调整的姿态R：

$R=R_1^T{R}_2$

步骤2-3-2)、利用旋转变换求取等效转轴；

$R\left(i\right)=\mathrm{Rot}(f,q_t)$

$=\left[\begin{array}{cccc}{f}_x{f}_x{\mathrm{versq}}_t+{\cos q}_t& f_y{f}_x{\mathrm{versq}}_t-f_z{\sin q}_t& f_z{f}_x{\mathrm{versq}}_t+f_y{\sin q}_t& 0\\ f_y{f}_x{\mathrm{versq}}_t+f_z{\sin q}_t& f_y{f}_y{\mathrm{versq}}_t+{\cos q}_t& f_z{f}_y{\mathrm{versq}}_t-f_x{\sin q}_t& 0\\ f_z{f}_x{\mathrm{versq}}_t-f_y{\sin q}_t& f_y{f}_z{\mathrm{versq}}_t+f_x{\sin q}_t& f_z{f}_z{\mathrm{versq}}_t+{\cos q}_t& 0\\ 0& 0& 0& 1\end{array}\right]$

f_xf_xf_z为笛卡儿空间旋转变换的等效转轴，对应机器人末端手的第七、第八、第九自由度；q_t为时间t下姿态R₁与R₂的角度差；versq_t＝1-cosq_t；

步骤2-3-3)、由等效转轴求取机器人相对于初始姿态的调整量，即第七、第八、第九自由度的关节空间。

上述技术方案中，所述的步骤3)包括：

步骤3-1)、利用轴的速度和加速度求PTP运动的仿真结果；

步骤3-2)、求PTP运动的仿真结果与目标曲线的拟合相似度，根据拟合相似度从所述若干个时间点上的九个自由度的关节空间信息中选取较佳值作为控制点。

上述技术方案中，所述的步骤3-1)包括：

步骤3-1-1)、根据轴的速度和加速度参数得到机器人的第i个关节在PTP运动中所需的加速最长时间；

其中，

表示轴电机的最大速度，

表示轴电机的最大加速度；

表示变换的距离，

步骤3-1-2)、由前一步骤所得到的结果计算机器人整体的加速最长完成时间、匀速最长完成时间和减速最长完成时间；

t_di＝t_ei-t_ai

maxT_a＝max(t_a1，t_a2，…，t_ai)

maxT_d＝max(t_d1，t_d2，…，t_di)

maxT_e＝max(t_e1，t_e2，…，t_ei)

其中，t_ei表示结束时间，t_di表示匀速完成的时间；

步骤3-1-3)、、计算各个关节在统一的加速、匀速情况下的运行速度和运行加速度；

$A_i=\frac{V_i}{\max T_a}$

其中，V_i是关节i的运行速度，A_i是关节i的运行加速度；

步骤3-1-4)、由上述计算结果求得PTP运行中关节空间随时间的表达式q_t，代入运动学正解中得到仿真结果F_ptp(θ_1start，θ_iEnd，t)；其中，

$q_t=\left\{\begin{array}{cc}\frac{1}{2}{A}_i\&CenterDot;t^2& 0<t\&le;\max T_a\\ V_i\&CenterDot;t-\frac{1}{2}\frac{{V_i}^2}{A_i}& \max T_a<t\&le;\max T_d\\ V_i\&CenterDot;\max T_e-\frac{A_i}{2}\&CenterDot;{(\max T_e-t)}^2& \max T_d<t\&le;\max T_e\end{array}\right..$

上述技术方案中，所述的步骤3-2)包括：

步骤3-2-1)、利用自相关的原理求相似程度

步骤3-2-2)、将计算得到的的值与阈值

进行比较，若小于，则认为这段曲线拟合成功，否则在目标曲线在时间上的中间点补充一个关键点，将目标曲线分为两段后重复执行步骤3-1)，直至整条曲线拟合完成；在拟合过程中所要用到的关键点就是所述控制点。

上述技术方案中，在所述的步骤31)中，所述目标函数为：

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\&Sigma;}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-{T_l}^E\left|\right|$

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

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

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

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

所述约束方程为：

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

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

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

b′(s)＝2a(s)

b(s)≥0

$b\left(s\right)\&le;\stackrel{\&OverBar;}{b}\left(s\right)$

$\underset{\&OverBar;}{f}\left(s\right)\&le;f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\&le;\stackrel{\&OverBar;}{f}\left(s\right)$

$\underset{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)\&le;\mathrm{\&tau;}\left(s\right)\&le;\stackrel{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)$

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

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

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

f(s)、

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

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

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

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\&Sigma;}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-{T_l}^E\left|\right|$

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

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

步骤32-2-1)、对弧长参数s进行均匀采样，得到离散型变量(s⁰，s¹，…s^K)；

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

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

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

本发明的优点在于：

1、与同类设备相比，采用伺服电机驱动方式提高了拍摄的精确性，九个自由度的连接组建和云台的设计方式提高了系统的灵活性。

2、九轴关节空间的运动驱动方法和凸规划轨迹优化的运用，把用户指定的末端轨迹转化为机器轴的关节空间信息，并优化了拍摄的时间控制和各个关节的受力情况，使得拍摄轨迹更加稳定平滑，避免抖动出现和避免与障碍物发生碰撞，提高了拍摄控制的稳定性和安全性。使得用户可以方便的得到需要的拍摄轨迹。

附图说明

图1为本发明的机器运动控制的摄像系统在一个实施例中的结构示意图；

图2为本发明的机器运动控制的摄像系统在一个实施例中的实例图；

图3为本发明的机器运动控制的摄像系统在一个实施例中所涉及的九轴自由度关节标识示意图；

图4为本发明的机器运动控制的摄像系统在一个实施例中所涉及的云台原型图；

图5为本发明的机器运动控制的摄像系统在一个实施例中的云台关节标识图；

图6为对本发明的机器运动控制的摄像系统做运动驱动的步骤流程图；

图7为运动轨迹避障图；

图8为完全同步同工的PTP运动图。

具体实施方式

在对本发明做详细说明之前，首先对本发明中的相关概念做统一的描述。

1、笛卡儿空间：是指使用笛卡儿坐标系定义的线性空间。本申请中使用三维笛卡儿空间来表示广泛意义上机器人的运动空间。

2、关节空间：假定机器人具有n个自由度，那么机器人关节坐标系为n维空间，关节空间是由一系列n维空间组成的集合，该集合中的各个特征的运动学正解的结果相同(即关节空间中各个特征通过正向运动学求解出的机器人末端位置和姿态相同)。

3、梯度投影法：梯度投影法的基本公式为：

其中，

是需要求得的角度的时间导数，J是雅克比矩阵，J⁺是J的伪逆矩阵，

是位置和姿态向量对时间的一阶导数，

为自由向量(自由向量是满足维数的任意向量)的集合，α代表一标量系数。

自由向量

可以表示为关于关节空间q的一个向量，对自由向量时间上求导数可得到如下公式：

其中，

表示自由向量对关节空间的梯度值，[*]^T代表转置矩阵。

由上面的公式(1)和公式(2)，得到扩展后的公式(3)：

4、姿态：指机器人手末端的姿势朝向。

5、正向运动学：已知机器人各个关节的关节坐标，机器人每个末端的位姿随之确定，这种由关节空间到机器人末端笛卡儿空间之间的映射称为正向运动学。

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

凸规划的标准形式为：

$\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。

在对相关概念作了说明后，下面结合附图和具体实施方式对本发明进行说明。

在图1中给出了本发明的机器运动控制摄像系统在一个实施例中的结构示意图，从该图中可以看出，该系统包括有导轨10、机器人本体20、云台装置30；所述机器人本体20能够在所述导轨10上滑动，所述机器人本体20的机械手末端安装有云台装置30，所述云台装置30上能够安装至少一台摄像机40。该系统中还包括有用于控制所述摄像机40的摄像机控制模块50，用于控制所述机器人本体20、云台装置30的机器人控制模块60，以及用于对所述摄像机控制模块50、机器人控制模块60进行联动控制的机器运动控制摄像模块70。

在本发明的系统中，导轨10、机器人本体20以及云台装置30是系统主要的机械运动部分，通常被视为一个具有九个自由度的机器人，因此下面参考图2，首先对这三个部分加以描述。

机器人本体20是一个通用的六轴机器人，该机器人具有多轴联动功能，其运动由位于关节部位的伺服电机控制。导轨10位于机器人本体20的底部位置，导轨10和机器人本体20之间的运动为平行滑动，平行运动的方向通常对应机器人世界坐标系的Y轴方向。在本发明中，将导轨10作为九自由度机器人的第一自由度，将机器人本体20(即六轴机器人)的肩关节作为第二自由度，肘关节作为第三自由度，腕关节作为第四自由度，手关节作为第五、第六、第七自由度。在图3中对上述自由度做了标示。

云台装置30由三块安装了伺服电机，可以自由旋转的金属材料构成，如图4所示，其中一块金属材料安装在手关节的第七自由度上，该金属材料又与云台装置30的第二块金属材料轴连接，其连接轴为第二扩展轴(之前提到的导轨10是第一扩展轴)，而第二块金属材料又与云台装置30的第三块金属材料轴连接，该连接轴为第三扩展轴。上述第二扩展轴与第三扩展轴分别代表了九自由度机器人的第八自由度和第九自由度。第三扩展轴与摄影机40固定相连，提供无限旋转功能。第二扩展轴一端与第三扩展轴相连，另一端固定在机器人本体20上，提供270度范围内非旋转功能。如图5所示，由于手关节上的第七自由度与云台上的第八自由度与第九自由度的轴线重合在一点，分别映射三维笛卡儿空间的三个旋转方向，因此摄像机器人仅仅通过所述云台装置就能够改变姿态，并且这一设计方式使得摄像机器人的位置改变也仅仅与九自由度中的前六个自由度有关。

摄像机控制模块50用于对摄像机进行控制，该模块通常由摄像机自带的相应控制软件实现。

机器人控制模块60用于对由导轨10、机器人本体20以及云台装置30所组成的九自由度的摄像机器人进行驱动控制。该模块可由现有技术中常见的机器人控制软件实现。

机器运动控制摄像模块70用于对安装有摄像机的九自由度机器人进行运动控制，这一过程较为复杂，下面对其实现方法做详细说明。

机器运动控制摄像模块70在控制机器人做运动前，需要用户通过相关接口提供一些基本的信息，如摄像机镜头的运动轨迹中的几个关键点，摄像机镜头到达这些关键点的时间，摄像机镜头在某些关键点上停顿的时间等。所述的关键点的信息包括有这些点在笛卡儿空间中的x、y、z轴上的位置坐标，以及机器人手末端在这些点上的姿态向量(Roll、Pitch、Yaw)。这些关键点的信息实质上都是摄像机镜头(也就是摄像机器人手末端)的位置与姿态，本领域技术人员所公知的，要实现摄像机器人的运动驱动，需要知道摄像机器人的所有关节随时间角度变化的信息。在下文中就以上述基本信息为基础，结合图6对本实施例如何实现对摄像机器人的运动驱动进行说明。

步骤1、实现摄像机镜头的运动轨迹规划以及姿态规划。

本申请所涉及的摄像机器人为图3所示的9自由度机器人，由摄像机器人末端的位置与姿态计算9个自由度的关节速度时，需要计算(9×6)矩阵以及与之相应的(6×9)伪逆矩阵，因此会耗费大量的计算时间。正如之前所提到的，本申请所涉及的摄像机器人中的自转关节(即云台)上的第八自由度、第九自由度以及手关节上的第七自由度的轴线重合在一点，分别映射三维笛卡儿空间的三个旋转方向，因此摄像机器人仅仅通过所述第七自由度、第八自由度以及第九自由度就能够改变姿态，类似的，摄像机器人的位置改变也仅仅与9自由度中的前六个自由度有关。基于摄像机器人在结构上的上述特点，在本发明的一个实施例中可以将摄像机器人的位置与姿态分开考虑，即将摄像机器人中的摄像机镜头的运动轨迹规划与姿态规划分开求解，这样能够降低运算求解的复杂度，从而提高计算效率，保证实时完成相关计算。

基于上述考虑，步骤1包括以下步骤：

步骤1-1、用户通过相关接口提供给摄像机器人的若干个关键点实际上是笛卡儿空间中的几个离散的点，因此首先需要将这些离散的点转变成连续的用于代表摄像机镜头运动轨迹的曲线，这一转变过程也被称为运动轨迹规划。

所述的运动轨迹规划可采用现有技术中的相关方法，如采用B样条曲线的方法，该方法既可以保证运动的连续性，又具有方程求解简单高效的优点；或者采用贝塞尔曲线的方法，该方法在计算机作图中使用广泛，具有B样条曲线法所不具备的操作直观的优点。在本实施例中，作为一种优选的实现方式，可将上述的B样条曲线法和贝塞尔曲线法相结合，由所述离散的关键点在x、y、z轴上的位置坐标得到由离散的点所形成连续曲线。之前提到，用户提供的离散的点在笛卡儿空间中，因此由离散的点所生成的连续曲线在笛卡儿空间中。由于由若干个点所形成的曲线可能有多条，因此运动轨迹规划的结果可能有多个。综上所述，通过运动轨迹规划，可由用户输入的若干个离散的关键点的位置坐标生成多条包含有所述关键点的连续曲线，这些连续曲线可用函数加以表示。

步骤1-2、对摄像机器人做姿态规划。

之前提到，用户输入的关键点的信息除了步骤1-1中提到的位置信息外，还包括有姿态信息，这些姿态信息用(Roll、Pitch、Yaw)表示。但用户所输入的关键点的数目是有限的，在两个相邻关键点之间的某一个点的姿态并不能直接得到。姿态规划的目的就是要算出这些中间点的姿态信息。

在姿态规划过程中，将相邻关键点中时间在前的关键点作为起始位置，将时间在后的关键点作为目标位置，机器人在起始位置的姿态用为R₁表示，在目标位置的姿态用R₂表示。R₁和R₂中都包括有Roll、Pitch、Yaw分量。根据本领域的公知常识，首先可以将起始位置与目标位置的姿态信息改写成四元数(参见参考文献1《3D Math Primer for Graphics and GameDevelopment》190页，Fletcher Dunn，Wordware Publishing，Inc)，即R₁→Q₁，R₂→Q₂，然后求任意时刻下的Q_n＝Q₁+(Q₂-Q₁)×(t_n-t₁)；最后将四元数Q_n又转变为Roll，Pitch，Yaw的欧拉角形式。

通过姿态规划，可得到整个驱动过程中任意时刻下用(Roll、Pitch、Yaw)表示的姿态。

步骤1-3、保存运动轨迹规划与姿态规划的结果。

之前的步骤1-1与步骤1-2所得到的运动轨迹规划结果与姿态规划的结果都是连续的函数，机器人驱动过程中并不需要如此多的信息，只要在所述连续函数上取若干个离散的点即可。由于机器人控制都有最小时间因此，可以将机器人运动的整个时间划分为

的整数倍，然后代入前述步骤1-1和步骤1-2所生成的用于表示运动轨迹和姿态的函数中，得到机器人手末端在多个离散的点上的位置与姿态。将这些信息加以保存。本步骤中，所述

的整数倍的具体取值可以根据实际操作时的精度来确定。

步骤2、求解摄像机器人的关节空间。

在之前提到，为了计算求解的方便，将摄像机器人的位置与姿态分开考虑。摄像机器人的前6个自由度的关节空间的生成与摄像机器人末端手的位置信息有关，而后3个自由度的关节空间的生成与摄像机器人末端手的姿态信息有关。因此在下文中下面分别加以说明。

步骤2-1、求解摄像机器人前6个自由度的关节空间。

在已知机器人末端手的位置信息的基础上，通过机器人逆向运动学求解关节空间中与这些位置信息所对应的数组，即前6个自由度的关节空间。

所述的逆向运动学求解必需具备四个约束：1)保证机器人自身安全；2)避免碰撞；3)避免关节约束；4)避免关节奇异和算法奇异。

机器人的运动学方程可以表示为：

X＝f(q)(4)

上述公式中，X∈R^m，表示机器人在笛卡儿空间中的位置(由于位置与姿态的分离，因此，此处所提到的X只是位置信息，与之前公式(1)中X所代表的姿态和位置略有不同)；q∈Rⁿ表示机器人的关节空间，其中的n指代关节的数目，m为目标向量的维度，在本申请中，m指代笛卡儿空间维度，它具体包含6个维度(x，y，z，Roll.Pitch.Yaw)，对于具有冗余自由度的摄像机器人而言，m＜n。

对上面的公式(4)加以变换，可以得到用于表示摄像机器人的笛卡儿空间速度

与关节速度间关系的如下公式：

$\stackrel{\&CenterDot;}{X}=J\stackrel{\&CenterDot;}{q}---\left(5\right)$

已知上述公式中的雅克比矩阵J∈R^m×n，在给定摄像机器人手部末端在笛卡儿空间中的速度

(可通过由步骤1生成的在笛卡儿空间中的位置信息对时间一阶求导得到)后，对公式(5)进行求解，可以得到机器人运动学的如下反解：

上述的公式就是之前所提到的阶梯投影法的基本公式，即公式(1)，它是公式(5)的通解形式，其中的J⁺是雅可比矩阵J的Moore-Penrose伪逆，α∈R^I是一标量系数，

为可任意选取的自由向量。该公式的右边第一项是公式(5)的特解，第二项为齐次解。从物理意义上来说，第一项定义了机器人手部的运动，第二项则定义了不影响手部运动的关节空间自运动。关节空间自运动是指在确定函数映射后对给定目标值求解过程中，系列关节因为冗余的缘故能够变化而不影响映射结果。

从前面的描述可以知道，阶梯投影法的基本公式可以做进一步的扩展，扩展后的公式如公式(3)所示。

观察梯度投影法的相关公式可以看出，J、J⁺、

I是已知的或是能够通过已有数据计算得到的，因此要计算关节速度的关键在于如何求取自由向量

和被称为放大系数的标量系数α。

求解自由向量

在求解自由向量

时，需要考虑前述逆向运动学求解时所涉及到的四个约束条件：保证机器人自身安全；避免碰撞；避免关节约束；避免关节奇异和算法奇异。根据这些约束条件可以设立相应的目标函数，下面分别加以说明。

1、为了避免关节奇异和算法奇异，本申请中引入可操作度的函数。可操作度是Yoshikawa在研究冗余自由度机器人的工作能力时定义的，可操作度可以用W表示，可操作度的计算公式如下：

$W=\sqrt{\det \left({\mathrm{JJ}}^T\right)}---\left(6\right)$

W和J的最直接的关系可以表示为利用J的奇异值分解得到的J的m个奇异值的积

W＝σ₁·σ₂·…·σ_m(7)

式中σ₁≥σ₂≥…≥σ_m≥0为J矩阵的奇异值。按矩阵理论，行满秩的J矩阵的所有m个奇异值都大于零，而降秩的J矩阵的奇异值中必有为零的值.W＝0时对应的J矩阵降秩，也即此时的机器人处于关节奇异，这是在机器人的运动规划中最不希望看到的。要想避免运动到关节发生奇异的位置，必须使可操作度大于零。W值越大，关节离发生奇异的位置越远，可操作性就越好。因此可以把零值看作W的一个斥力源，把这种关系引入冗余自由度机器人的运动控制中，这将有利于避免运动到关节发生奇异的位置。

2、为了避免碰撞问题，可以设定避障函数。考虑到机器人基本上是以链杆作为其基本结构，因此在本实施例中可以使用如图7所示的最简单的胶囊状的结构来包围它。当障碍物到机器人的胶囊状结构间的距离小于一定值后，可得到如下的避障函数D(q)＝d(q，B)：

$D_{\mathrm{ij}}\left(q\right)=\left\{\begin{array}{c}\frac{1}{2}\mathrm{\&eta;}(\frac{1}{d_{\mathrm{ij}}\left(q\right)}-\frac{1}{d_0}),d_{\mathrm{ij}}\left(q\right)\&le;d_0\\ 0,d_{\mathrm{ij}}\left(q\right)>d_0\end{array}\right.---\left(8\right)$

上述公式中，q代表关节空间中的某一姿态，B代表障碍物，i代表胶囊状的包围盒的编号，j代表障碍物的编号，d₀代表安全距离的阈值，η为一系数，其值通常取1。

3、为了避免关节约束问题，需要保证关节尽量在约束的中间位置。优化关节运动的方法是把关节运动范围的中间位置(q_imax+q_imin)/2作为基准，构造关节约束函数如下：

$L\left(q\right)=\frac{1}{n}{\mathrm{\&Sigma;}}_{i=1}^n{\left(\frac{q{-a}_i}{a_i-q_{i\max }}\right)}^2---\left(9\right)$

式中a_i＝(q_imax+q_imin)/2为各关节允许范围的中值，q_imax表示第i个关节角度最大的值，q_imin表示第i个关节角度最小的值，n为关节数。对关节运动范围的优化就是使上述关节约束函数L(q_i)的值最小。

之前提到，四个约束条件中还包括有“保证机器人自身安全”的约束条件，这一约束条件在解决上述三个约束条件的同时实质上也已经得到解决。

除了上面公式(6)、公式(8)、公式(9)这三个目标函数外，考虑到摄像机器人的移动关节上的第一自由度(由于移动关节实质上是在导轨上所做的滑动，因此第一自由度也可以被称为导轨自由度或滑动自由度)，对解算有很大的影响，并且滑动自由度的单位和转动自由度并不是在一个数量级，因此在这里通过目标函数(9维向量)将它单独作为一个自由向量。这个自由向量只对导轨有影响。

在生成上述四个目标函数后，下面就如何建立一个统一的优化指标进行讨论。

公式(3)的自运动解和特解要保证在一个数量级，那么四个目标函数在物理上要保持意义一致，这样才能保证四个约束都能在发挥功用的时候有意义，因此每个目标函数各需要一个参数来调配到同一个物理级别上。由于在关节发生奇异的位置附近，机器人的操作性变得很坏，所以需要把这些区域作为禁区予以重视。把可操作度作为避关节和算法奇异优化指标的效果并不很明显，这是因为W在“零”附近存在一个较大值。因此选取1/W(q)作为衡量关节奇异对自运动的影响的参数。类似的，以(1/D(q))来实现避障的目标优化比用其他要好得多。将四者运动优化用具有相同趋势的势能场函数表示，这样就具有相同的物理意义。

在势能场函数中不仅仅要考虑势能场的趋势，还要考虑数量级。举例来说，选定一个适当的可操作度值W₀(W＝W₀对应关节奇异位置附近的一个边界值)，当W＞W₀时，机器人远离关节奇异，没有必要再考虑关节奇异的影响，即没有必要调整机器人关节空间自运动使W(q)增大。相反如果仍把W(q)使增加的因素考虑进去，就有可能影响避障或其它运动优化的实现。同时避障问题也应该把势力场的范围考虑进去，与前者不同，各关节运动范围的优化则是在运动规划中应始终要求的，因为L(q)的值变化范围有限，对前者的运动优化影响不大，因此可建立如下统一优化指标

其中β_W，β_D，β_L，β_R为加权系数，它们根据需求可以定义为一个值在[0，1]的数。

求解放大系数α

将统一的自由向量

求得后，接着考虑放大系数α。放大系数直接控制路径跟踪优化的效果。如果小了，优化效果并不能体现，出现的结果是约束条件破坏；如果大了，使得自运动结果大于基本解，造成结果错误，不满足逆运动学解。

选取放大系数的原则是能够保证

随着时间的变化而减小，对于所选取的优化指标函数式

由于

通过选取放大系数α来满足或影响

变化的趋势，为此令

这里λ＜0，于是对应的

随时间T的变化而减小。使上式成立的α为

在求得自由向量

与放大系数α后，就可以将它们代入公式前述的公式(6)，可计算出摄像机器人的前6轴(即前6个自由度)的关节速度

在求得前6个自由度的关节速度后，可以根据

dt求得具体时刻的角度信息，根据正向运动学可以求得机器人末端的位置与姿态。

步骤2-2、求后3个自由度的关节空间

步骤2-2-1、根据机器人关节的属性，调整关节中的第七、第八、第九自由度不会改变位置，所以需要用这3个自由度去调整姿态。在得到前6个自由度的关节空间后，结合正向运动学可以求解出摄像机器人末端手在某一时刻的姿态R₁，而由步骤1的姿态规划所生成的姿态信息可以得到在目标位置的姿态为R₂，则机器人末端手从某一时刻到目标位置所需要调整的姿态R为：

$R=R_1^T{R}_2---\left(11\right)$

步骤2-2-2、利用旋转变换求取等效转轴。

$R\left(i\right)=\mathrm{Rot}(f,q_t)=$

$\left[\begin{array}{cccc}{f}_x{f}_x{\mathrm{versq}}_t+{\cos q}_t& f_y{f}_x{\mathrm{versq}}_t-f_z{\sin q}_t& f_z{f}_x{\mathrm{versq}}_t+f_y{\sin q}_t& 0\\ f_y{f}_x{\mathrm{versq}}_t+f_z{\sin q}_t& f_y{f}_y{\mathrm{versq}}_t+{\cos q}_t& f_z{f}_y{\mathrm{versq}}_t-f_x{\sin q}_t& 0\\ f_z{f}_x{\mathrm{versq}}_t-f_y{\sin q}_t& f_y{f}_z{\mathrm{versq}}_t+f_x{\sin q}_t& f_z{f}_z{\mathrm{versq}}_t+{\cos q}_t& 0\\ 0& 0& 0& 1\end{array}\right]---\left(12\right)$

其中f＝[f_x f_y f_z]^T为通用旋转变换的等效转轴，对应机器人末端手的第七、第八、第九自由度；q_t为时间t下姿态R₁与R₂的角度差；versq_t＝1-cosq_t。

由于公式(12)中的q_t是已知的，各个三角函数是已知的，而姿态调整量R可由之前的公式(11)计算得到，因此可以计算出机器人末端手的第七、第八、第九自由度各自的等效转轴f_x、f_y、f_z。

步骤2-2-3、在计算出f_x、f_y、f_z后，可求取机器人相对于初始姿态的调整量，即第七、第八、第九自由度的关节空间。

通过以上步骤，可以得到完整的关节空间随时间变化的所有信息。将计算得到的关节空间q随时间变化的值补充到步骤1-3所产生的数据结构中。

步骤3、利用九个自由度的关节空间信息做仿真，将仿真后所得到的曲线与不同的拟合曲线比较，选取最佳值作为驱动过程中的控制点。

通过之前的步骤，得到了摄像机器人的关节空间，有了这些就能够驱动机器人进行运动。前文提到，关节空间与摄像机器人的末端手在笛卡儿空间中的某一个位置存在对应关系，而摄像机器人的末端手在笛卡儿空间中的某一个位置就是步骤1-3中所提到的离散的点。这些离散点的数目越多，与这些离散点所对应的关节空间也就越多，所要传输给摄像机器人的控制信息也就越多。从理论上来说，越多关节空间信息，越有利于提高对所要驱动的摄像机器人的控制精度。但在实际的驱动过程中，用于驱动机器人的装置(如伺服电机)的启动、停止或速度改变都要有一个运行的过程，很容易理解，当摄像机器人从静止到某一关节速度的过程中，必然要经历先加速，再匀速的过程。但如果外部输入的控制信息过于频繁，则机器人的前一关节速度尚未到达，就要执行新的加速或减速过程，关节运动在不停的加速或减速过程中被消耗掉，这使得机器人的各个关节的运行速度很可能无法达到实际值。

正是出于上述原因，因此，需要对前面步骤所得到的诸多值进行选择，在保证驱动过程准确的前提下，所选取的离散的点应该尽可能的少。

本实施例中，采用仿真PTP运动实现由关节速度求机器人运动轨迹的过程。在这一过程中，通过仿真算法挑选步骤2得到的关节空间q，得到机器人驱动所需的最少控制点。

作为本领域技术人员的公知常识，在机器人驱动过程中，关节联动有三种模式：异步同工、同步同工、完全同步同工。在本实施例中，仿真PTP过程采用完全同步同工模式。

PTP运动经过三个阶段，加速、常速度与减速。其中，t＝0至t＝t_a(t_a指加速时间)为加速阶段，t＝t_a至t＝t_d(t_d指匀速时间)为匀速阶段，最后至t＝t_e(t_e指减速完成时间)为减速阶段。如图8所示，完全同步同工的PTP运动，需要保证机器人所有的关节在运动时t_a1＝t_a2＝…＝t_ai，t_d1＝t_d2＝…＝t_di，t_e1＝t_e2＝…＝t_ei。在本实施例中以完全同步同工模式为基础，采用折半插入关键点的方法来求取必要的控制点。其具体实现步骤如下：

步骤3-1、利用轴的速度和加速度求PTP运动的仿真结果F_ptp(q₁，q₂，t)＝X。

步骤3-1-1、根据轴的速度和加速度参数得到机器人的第i个关节在PTP运动中所需的加速最长时间；

其中，

表示轴电机的最大速度，

表示轴电机的最大加速度；表示变换的距离，

步骤3-1-2、由前一步骤所得到的结果计算机器人整体的加速最长完成时间、匀速最长完成时间和减速最长完成时间。

具体的说，首先，通过下列公式(15)和(16)可以计算出由机器人在第i个关节的匀速和减速的完成时间

t_di＝t_ei-t_ai(16)

其中，t_ei表示减速完成时间，t_di表示匀速完成的时间；

然后再通过下列公式计算出机器人整体的加速最长完成时间，匀速最长完成时间和减速最长完成时间

maxT_a＝max(t_a1，t_a2，…，t_ai)

maxT_d＝max(t_d1，t_d2，…，t_di)(17)

maxT_e＝max(t_e1，t_e2，…，t_ei)

步骤3-1-3、计算各个关节在统一的加速、匀速情况下的运行速度和运行加速度。

$A_i=\frac{V_i}{\max T_a}---\left(19\right)$

其中，V_i是关节i的运行速度，A_i是关节i的运行加速度。

步骤3-1-4、由上述计算结果求得PTP运行中关节空间随时间的表达式q_t

$q_t=\left\{\begin{array}{cc}\frac{1}{2}{A}_i\&CenterDot;t^2& 0<t\&le;\max T_a\\ V_i\&CenterDot;t-\frac{1}{2}\frac{{V_i}^2}{A_i}& \max T_a<t\&le;\max T_d\\ V_i\&CenterDot;\max T_e-\frac{A_i}{2}\&CenterDot;{(\max T_e-t)}^2& \max T_d<t\&le;\max T_e\end{array}\right.---\left(20\right)$

将关节空间q_t代入运动学正解中得到仿真结果F_ptp(q_1start，q_iEnd，t)。

步骤3-2、求拟合的相关度

利用对应时间上PTP运动和步骤1中运动轨迹规划得到的目标曲线(如：贝塞尔Bezieer)的笛卡儿空间距离差与目标曲线的笛卡儿空间长度的比值求拟合的相关度。拟合相关度的计算公式如下：

上述公式计算得到的的值在阈值q_max下，则认为这段曲线拟合成功。否则在目标曲线在时间上的中间点补充一个关键点。将目标曲线分为两段去求PTP拟合，然后重复步骤3-1，直至整条曲线拟合完成，如不能拟合则调整阈值q_max。

通过上述三个步骤可以得到多个关节空间点，这些关节空间点可以用来驱动本发明的九自由度的摄像机器人。但直接利用这些关节空间点做机器人运动驱动时，由于没有考虑各个轨迹点之间的运动平滑性以及拍摄时间和能量控制等。因此，在本发明的一个优选实施例中，还包括有利用凸规划优化求解的相关步骤，具体如下：

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

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

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

$\mathrm{\&tau;}=M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+C(q,\stackrel{\&CenterDot;}{q}){\stackrel{\&CenterDot;\&CenterDot;}{q}}^2+F_s\left(q\right)\mathrm{sgn}\left(\stackrel{\&CenterDot;}{q}\right)+G\left(q\right)---\left(22\right)$

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

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

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

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

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

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

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

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

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

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

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

根据步骤51)和步骤52)所建立的目标函数以及约束条件，可以得到如下的优化模型，所述的优化模型的数学表达式如下(公式中s.t.为subject to的缩写，在数学中表示“受约束于”)：

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\&Sigma;}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-{T_l}^E\left|\right|---\left(23\right)$

s.t.τ(s)＝M(s)a(s)+C(s)b(s)+G(s)(24)

$b\left(0\right)={\stackrel{\&CenterDot;}{s}}_0^2---\left(25\right)$

$b\left(1\right)={\stackrel{\&CenterDot;}{s}}_T^2---\left(26\right)$

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

b(s)≥0                           (28)

$b\left(s\right)\&le;\stackrel{\&OverBar;}{b}\left(s\right)---\left(29\right)$

$\underset{\&OverBar;}{f}\left(s\right)\&le;f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\&le;\stackrel{\&OverBar;}{f}\left(s\right)---\left(30\right)$

$\underset{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)\&le;\mathrm{\&tau;}\left(s\right)\&le;\stackrel{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)---\left(31\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的函数。

在上述优化模型的各个数学表达式中，数学表达式(23)表示之前提到的目标函数；数学表达式(24)为作为约束条件的机器人动力学方程；数学表达式(25)、(26)为机器人初始点、末端点的速度约束；数学表达式(27)、(28)可以由

推导得到：

即

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

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

$-{\stackrel{\&OverBar;}{\stackrel{\&CenterDot;}{q}}}_i\left(s\right)\&le;{\stackrel{\&CenterDot;}{q}}_i\left(s\right)\&le;{\stackrel{\&OverBar;}{\stackrel{\&CenterDot;}{q}}}_i\left(s\right)$

$\&DoubleLeftRightArrow;{\left({\stackrel{\&CenterDot;}{q}}_i\left(s\right)\right)}^2={\left(q_i^{\&prime;}\left(s\right)\stackrel{\&CenterDot;}{s}\right)}^2={\left(q_i^{\&prime;}\left(s\right)\right)}^{2}b\left(s\right)\&le;{\left({\stackrel{\&OverBar;}{\stackrel{\&CenterDot;}{q}}}_i\left(s\right)\right)}^2$

$\&DoubleLeftRightArrow;b\left(s\right)\&le;\frac{{\left({\stackrel{\&OverBar;}{\stackrel{\&CenterDot;}{q}}}_i\left(s\right)\right)}^2}{{\left(q_i^{\&prime;}\left(s\right)\right)}^2}$

令

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

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

$\underset{\&OverBar;}{{\stackrel{\&CenterDot;\&CenterDot;}{q}}_i}\left(s\right)\&le;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_i\left(s\right)\&le;{\stackrel{\&OverBar;}{\stackrel{\&CenterDot;\&CenterDot;}{q}}}_i\left(s\right)$

$\&DoubleLeftRightArrow;\underset{\&OverBar;}{{\stackrel{\&CenterDot;\&CenterDot;}{q}}_i}\left(s\right)\&le;q_i^{\&prime;}\left(s\right)\stackrel{\&CenterDot;\&CenterDot;}{s}+q_i^{\&prime;\&prime;}\left(s\right){\stackrel{\&CenterDot;}{s}}^2\&le;{\stackrel{\&OverBar;}{\stackrel{\&CenterDot;\&CenterDot;}{q}}}_i\left(s\right)$

$\&DoubleLeftRightArrow;\underset{\&OverBar;}{{\stackrel{\&CenterDot;\&CenterDot;}{q}}_i}\left(s\right)\&le;q_i^{\&prime;}\left(s\right)a\left(s\right)+q_i^{\&prime;\&prime;}\left(s\right)b\left(s\right)\&le;{\stackrel{\&OverBar;}{\stackrel{\&CenterDot;\&CenterDot;}{q}}}_i\left(s\right)$

令

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

上述的公式推导中，

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

分别表示第i个关节的角度、角速度以及角加速度；

表示第i个关节角速度的最小值、最大值，

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

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

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

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

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{l=1}{\overset{L}{\mathrm{\&Sigma;}}}\left|\right|\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-{T_l}^E\left|\right|$

从而得到新的优化模型

$\underset{a\left(s\right),b\left(s\right),\mathrm{\&tau;}\left(s\right)}{\mathrm{MIN}}\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}b\left(s\right)\mathrm{ds}---\left(32\right)$

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

$\underset{s_{P_{l-1}}}{\overset{s_{P_l}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}-{T_l}^E\&le;0---\left(33\right)$

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

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

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

b′(s)＝2a(s)

b(s)≥0

$b\left(s\right)\&le;\stackrel{\&OverBar;}{b}\left(s\right)$

$\underset{\&OverBar;}{f}\left(s\right)\&le;f\left(s\right)a\left(s\right)+h\left(s\right)b\left(s\right)\&le;\stackrel{\&OverBar;}{f}\left(s\right)$

$\underset{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)\&le;\mathrm{\&tau;}\left(s\right)\&le;\stackrel{\&OverBar;}{\mathrm{\&tau;}}\left(s\right)$

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

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

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

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

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

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

a)、对目标函数

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

该计算结果等价于

梯形公式计算定积分可参见文献3：“关冶陆金甫编《数值分析基础》(高等教育出版社，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}}{\&Integral;}}\frac{1}{\sqrt{b\left(s\right)}}\mathrm{ds}=\frac{2(s^{k+1}-s^k)}{\sqrt{b^{k+1}}+\sqrt{b^k}}---\left(34\right)$

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

$\underset{a^k,b^k,{\mathrm{\&tau;}}^k}{\mathrm{MIN}}\underset{k=0}{\overset{K}{\mathrm{\&Sigma;}}}\left(b^k\right)---\left(35\right)$

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

$b^0={\stackrel{\&CenterDot;}{s}}_0^2---\left(37\right)$

$b^K={\stackrel{\&CenterDot;}{s}}_k^2---\left(38\right)$

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

b^k≥Oandb^K≥0(40)

$b^k\&le;\stackrel{\&OverBar;}{b}\left(s^k\right)\mathrm{and}{b}^K\&le;\stackrel{\&OverBar;}{b}\left(s^K\right)---\left(41\right)$

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

$f\left(s^{k+1/2}\right)a^k+h\left(s^{k+1/2}\right)b^{k+1/2}\&le;\stackrel{\&OverBar;}{f}\left(s^{k+1/2}\right)---\left(43\right)$

$\underset{\&OverBar;}{\mathrm{\&tau;}}\left(s^{k+1/2}\right)\&le;{\mathrm{\&tau;}}^k\&le;\stackrel{\&OverBar;}{\mathrm{\&tau;}}\left(s^{k+1/2}\right)---\left(44\right)$

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

$\underset{{k=k}_{\mathrm{li}}}{\overset{k_{\mathrm{le}}}{\mathrm{\&Sigma;}}}\frac{2(s^{k+1}-s^k)}{\sqrt{b^{k+1}}+\sqrt{b^k}}\&le;{T_l}^E---\left(45\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)。

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

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

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

步骤7)、在步骤6)得到机器人各个关节的运动量后，机器运动控制摄像模块70将这些关节的运动量转换成机器控制数据和命令，所述的机器控制数据和命令又被传输到摄像机控制模块50、机器人控制模块60，由摄像机控制模块50向摄像机40发送开关机、变焦、调节光圈等控制指令，由机器人控制模块60向导轨10、机器人本体20、云台装置30发送电机运动控制指令

本发明的优点在于：与同类设备相比，采用伺服电机驱动方式提高了拍摄的精确性，九个自由度的连接组建和云台的设计方式提高了系统的灵活性。九轴关节空间的运动驱动方法和凸规划轨迹优化的运用，把用户指定的末端轨迹转化为机器轴的关节空间信息，并优化了拍摄的时间控制和各个关节的受力情况，使得拍摄轨迹更加稳定平滑，避免抖动出现和避免与障碍物发生碰撞，提高了拍摄控制的稳定性和安全性。使得用户可以方便的得到需要的拍摄轨迹。

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

Claims (17)

1.一种机器运动控制的摄像系统，其特征在于，包括：导轨(10)，机器人本体(20)，云台装置(30)，摄像机(40)，用于控制所述摄像机(40)的摄像机控制模块(50)，用于控制所述机器人本体(20)、云台装置(30)的机器人控制模块(60)，以及用于对所述摄像机控制模块(50)、机器人控制模块(60)进行联动控制的机器运动控制摄像模块(70)；其中，

所述机器人本体(20)在所述导轨(10)上滑动，所述机器人本体(20)的机械手末端安装有所述云台装置(30)，所述云台装置(30)上安装至少一台所述摄像机(40)；所述导轨(10)、机器人本体(20)、云台装置(30)组成一九自由度的机器人，所述导轨(10)有第一自由度；所述机器人本体(20)的肩关节有第二自由度，肘关节有第三自由度，腕关节有第四自由度，手关节有第五、第六、第七自由度；

所述云台装置(30)由三块安装了伺服电机，可以自由旋转的金属材料构成，其中一块金属材料安装在手关节的第七自由度上，该金属材料又与云台装置(30)的第二块金属材料轴连接，其连接轴为第二扩展轴，导轨(10)是第一扩展轴，而第二块金属材料又与云台装置(30)的第三块金属材料轴连接，该连接轴为第三扩展轴；上述第二扩展轴与第三扩展轴分别代表了九自由度机器人的第八自由度和第九自由度，第三扩展轴与摄影机(40)固定相连，提供无限旋转功能，第二扩展轴一端与第三扩展轴相连，另一端固定在机器人本体(20)上，提供270度范围内非旋转功能；

所述第七自由度、第八自由度、第九自由度的轴线重合在一点，分别映射三维笛卡儿空间的三个旋转方向；

所述机器运动控制摄像模块(70)根据用户设定的运动轨迹做运动轨迹规划与姿态规划，然后根据所述运动轨迹规划的结果求解所述九自由度机器人的前六个自由度的关节空间，根据所述姿态规划的结果求解所述九自由度机器人的后三个自由度的关节空间；接着通过仿真选取若干个用于运动驱动的关节空间点；再建立机器人动力学方程，结合所述机器人动力学方程、所述关节空间点以及机器人的约束条件建立优化模型，将所述优化模型转化为凸规划后，对其求解，得到所述九自由度机器人各个关节的运动量。 

2.一种在权利要求1所述的摄像系统上实现机器运动控制的方法，包括：

步骤10)、利用用户提供的所述机器人的末端手在笛卡儿空间中多个点上的位置与姿态的信息，实现所述机器人末端手的运动轨迹规划与姿态规划，得到机器人末端手在整个驱动过程中的位置以及机器人末端手在整个驱动过程中的姿态；

步骤20)、由步骤10)得到的机器人末端手的位置信息求解某一时间点所述机器人从第一自由度到第六自由度的关节空间，由步骤10)得到的机器人末端手的姿态信息求解所述时间点上所述机器人从第七自由度到第九自由度的关节空间；所得到的九个自由度的关节空间用于驱动机器人的运动；

步骤30)、利用步骤20)所得到的所述机器人在若干个时间点上的九个自由度的关节空间信息做仿真，根据仿真结果从所述若干个时间点上的九个自由度的关节空间信息中选取较佳值作为控制点；

步骤40)、利用前一步骤得到的结果实现所述九自由度机器人的机器运动控制。

3.根据权利要求2所述的机器运动控制的方法，其特征在于，在所述的步骤30)与步骤40之间还包括：

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

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

4.根据权利要求2或3所述的机器运动控制的方法，其特征在于，在所述的步骤10)中，所述的运动轨迹规划包括：

用户提供的所述机器人的末端手在笛卡儿空间中多个点上的位置信息做曲线拟合，得到至少一条运动轨迹曲线。

5.根据权利要求4所述的机器运动控制的方法，其特征在于，所述的曲线拟合采用B样条曲线法，或贝塞尔曲线法，或B样条曲线与贝塞尔曲线结合的方法。

6.根据权利要求2或3所述的机器运动控制的方法，其特征在于， 在所述的步骤10)中，所述的姿态规划包括：

步骤a)、将笛卡儿空间中两个已知姿态信息的相邻点分别作为起始位置与目标位置，将所述起始位置与目标位置的姿态信息由欧拉角的形式修改为四元数的形式，分别用Q₁和Q₂表示；

步骤b)、求取所述起始位置与目标位置间任意时刻下的姿态，即：Q_n＝Q₁+(Q₂-Q₁)×(t_n-t₁)；

步骤c)、将步骤b)求得的姿态转换回欧拉角形式。

7.根据权利要求2或3所述的机器运动控制的方法，其特征在于，所述的步骤20)包括：

步骤2-1)、从步骤10)所得到的整个驱动过程中的位置信息与姿态信息中选取若干个时间点上的位置信息与姿态信息；

步骤2-2)、将步骤2-1)所得到的所述若干个时间点上的位置信息分别代入梯度投影法的计算公式，得到该若干个时间上各自的所述机器人从第一自由度到第六自由度的关节空间；

步骤2-3)、由步骤2-2)计算得到的所述机器人从第一自由度到第六自由度的关节空间，以及步骤2-1)所得到的所述若干个时间点上的姿态信息计算出姿态的调整量，由姿态的调整量计算出所述若干个时间点上各自的所述机器人从第七自由度到第九自由度的关节空间。

8.根据权利要求7所述的机器运动控制的方法，其特征在于，在所述的步骤2-2)中，所述的梯度投影法的计算公式为：

上述公式中， 

表示关节速度，J是雅克比矩阵，J⁺是J的伪逆矩阵， 是所述位置对时间的一阶导数， 

为自由向量的集合，α代表放大系数；其中，

β_W，β_D，β_L，β_R为加权系数，它们是一个值在[0，1]的数；

W为可操作度函数， 

D(q)为避障函数，

q代表关节空间中的某一姿态，i代表胶囊状的包围盒的编号，j代表障碍物的编号，d₀代表安全距离的阈值，η为一系数； 

L(θ)为关节约束函数， a_i＝(q_imax+q_imin)/2为各关节允许范围的中值，q_imax表示第i个关节角度最大的值，q_imin表示第i个关节角度最小的值，n为关节数；

为一个9维向量， 

λ表示 

▽表示梯度值。

9.根据权利要求7所述的机器运动控制的方法，其特征在于，所述的步骤2-3)包括：

步骤2-3-1)、由步骤2-2)计算得到的所述机器人从第一自由度到第六自由度的关节空间计算出某一时刻的姿态R₁，由步骤10)的姿态规划所生成的姿态信息得到在目标位置的姿态为R₂，计算机器人末端手从某一时刻到目标位置所需要调整的姿态R：

步骤2-3-2)、利用旋转变换求取等效转轴；

f_x f_y f_z为笛卡儿空间旋转变换的等效转轴，对应机器人末端手的第七、第八、第九自由度；q_t为时间t下姿态R₁与R₂的角度差；

versq_t=1-cosq_t；

步骤2-3-3)、由等效转轴求取机器人相对于初始姿态的调整量，即第七、第八、第九自由度的关节空间。

10.根据权利要求2或3所述的机器运动控制的方法，其特征在于，所述的步骤30)包括：

步骤3-1)、利用轴的速度和加速度求PTP运动的仿真结果；

步骤3-2)、求PTP运动的仿真结果与目标曲线的拟合相似度，根据拟合相似度从所述若干个时间点上的九个自由度的关节空间信息中选取较佳值作为控制点。 

11.根据权利要求10所述的机器运动控制的方法，其特征在于，所述的步骤3-1)包括：

步骤3-1-1)、根据轴的速度和加速度参数得到机器人的第i个关节在PTP运动中所需的加速最长时间；

其中， 

表示轴电机的最大速度， 

表示轴电机的最大加速度； 

表示变换的距离， 

步骤3-1-2)、由前一步骤所得到的结果计算机器人整体的加速最长完成时间、匀速最长完成时间和减速最长完成时间；

t_di＝t_ei-t_ai

maxT_a＝max(t_a1，t_a2，…，t_ai)

maxT_d＝max(t_d1，t_d2，…，t_di)

maxT_e＝max(t_e1，t_e2，…，t_ei)

其中，t_ei表示结束时间，t_di表示匀速完成的时间；

步骤3-1-3)、计算各个关节在统一的加速、匀速情况下的运行速度和运行加速度；

其中，V_i是关节i的运行速度，A_i是关节i的运行加速度；

步骤3-1-4)、由上述计算结果求得PTP运行中关节空间随时间的表达 式q_t，代入运动学正解中得到仿真结果F_ptp(θ_1Start，θ_iEnd，t)；其中，

12.根据权利要求11所述的机器运动控制的方法，其特征在于，所述的步骤3-2)包括：

步骤3-2-1)、利用自相关的原理求相似程度 

步骤3-2-2)、将计算得到的 

的值与阈值 

进行比较，若小于，则认为这段曲线拟合成功，否则在目标曲线在时间上的中间点补充一个关键点，将目标曲线分为两段后重复执行步骤3-1)，直至整条曲线拟合完成；在拟合过程中所要用到的关键点就是所述控制点。

13.根据权利要求3所述的机器运动控制的方法，其特征在于，在所述的步骤31)中，所述目标函数为：

其中，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′(s)＝2a(s)

b(s)≥0

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

其中，τ为关节力， 

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

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

14.根据权利要求3所述的机器运动控制的方法，其特征在于，在所述的步骤32)中，将所述的目标函数转换为凸函数包括：

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

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

15.根据权利要求14所述的机器运动控制的方法，其特征在于，在所述的步骤32)中，将所述凸规划的优化模型做离散化后求解包括：

步骤32-2-1)、对弧长参数s进行均匀采样，得到离散型变量(s⁰，s¹，…s^K)；

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

16.根据权利要求3所述的机器运动控制的方法，其特征在于，在所述的步骤32)中，采用二阶锥规划、Subgradient投影法、内点法中的一种对离散化后的凸规划的优化模型进行求解。

17.根据权利要求3所述的机器运动控制的方法，其特征在于，所述运动量包括关节的离散速度、加速度以及力矩。 

Images