一种基于多视点几何的多机器人协调运动方法

摘要

本发明公开了一种基于多视点几何的多机器人协调运动方法，包括如下步骤：指定多机器人系统中的两台机器人作为先导机，其余机器人作为跟随机；计算跟随机与两台先导机之间的两焦点张量并由此获得跟随机目标位置在图像上的两条投影直线，直线交点即为目标位置在跟随机图像上的投影点；令跟随机沿其摄像机图像的光学中心与目标位置投影点的连线运动，直到先导机在跟随机摄像机图像上的投影点位置与运动前的投影位置重合，对剩余跟随机逐一重复上述处理，实现剩余跟随机和两台先导机的相对位置与其运动前的相对位置保持一致。本发明提出将多视点几何应用于多机器人系统，引入多台摄像机相互投影的知识，提高了控制稳定性，推动多视点几何在机器人控制中的作用。

说明书

技术领域

本发明涉及机器人控制技术领域，具体涉及一种基于多视点几何的多机器人协调运动方法。

背景技术

多机器人协调运动在实际生活中有着很广泛的应用。一方面，有些工作仅用一个机器人很难完成，而通过多个机器人的合作就能很好的达到目的；另一方面，通过多机器人间的协调合作，可提高机器人系统在作业过程中的效率。在很多应用领域中，都要求多机器人协调运动，尤其是在完成军事、生产、消防等任务的情况下，还要求机器人作业和运动过程中保持一定的队形，如何控制机器人保持队形显得更为重要。这就是机器人的队形控制。

在多机器人协调作业的研究和应用领域，现在已有的控制方法，由于缺乏稳定性，机器人只能是在室内或平地等比较单一的环境中实现协调运动，极大限制了机器人的使用范围和应用条件，很难在更加普遍的复杂地形上实现多机器人的协调工作。

发明内容

发明目的：针对现有技术的不足，本发明提供一种基于多视点几何的多机器人协调运动方法，提高控制方法的稳定性，使得机器人适用于更复杂全动态的环境。

技术方案：本发明所述基于多视点几何的多机器人协调运动方法，用于多机器人系统，包括如下步骤：

(1)指定多机器人系统中的两台机器人作为先导机，其余机器人作为跟随机，两台先导机先运动；

(2)选取一台跟随机记为C₁，两台先导机分别记为C₂和C₃，C₁、C₂、C₃运动前的位置分别记为C₁^t、C₂^t、C₃^t，，C₂和C₃先运动，运动后的位置分别为C₂^t+1、C₃^t+1；

(3)计算C₁^t与C₂^t+1之间的两焦点张量F₁₂，利用公式l₂＝F₁₂e₂₁^t计算C₁目标位置C₁^t+1在C₁图像上的投影直线l₂，其中对极点e₂₁^t是C₁^t在C₂^t图像上的投影；

(4)计算C₁^t与C₃^t+1之间的两焦点张量F₁₃，利用公式l₃＝F₁₃e₃₁^t计算C₁目标位置C₁^t+1在C₁图像上的投影直线l₃，其中对极点e₃₁^t是C₁^t在C₃^t图像上的投影，投影直线l₃与步骤(3)所得投影直线l₂的交点x即为C₁目标位置C₁^t+1在C₁图像上的投影点；

(5)令C₁沿其摄像机图像的光学中心与C₁目标位置投影点x的连线运动，直到C₂、C₃在C₁摄像机图像上的投影点位置e₁₂、e₁₃与运动前的投影位置e₁₂^t、e₁₃^t重合，即实现C₁、C₂、C₃运动后的相对位置与其运动前的相对位置保持一致，完成一台跟随机与两台先导机协调运动队形保持的控制；

(6)对剩余跟随机逐一重复步骤(2)至(5)，实现剩余跟随机和两台先导机的相对位置与其运动前的相对位置保持一致，从而完成多机器人系统所有机器人协调运动队形保持的控制。

进一步完善上述技术方案，所述步骤(3)、步骤(4)中两焦点张量的算法如下：

x、x’是空间点X在两幅图像平面上的投影点，两焦点张量的元素是行列式展开式中x′^jxⁱ的系数，写为：

其中，aⁱ、bⁱ分别是空间点X投影到两幅图像上x、x’点的投影矩阵A和B中的行向量；对r，s，t＝1，2，3，张量ε_rst定义如下：

对给定i值，除非p、q不同于i且互不相同，否则张量ε_ipq为零，对ε_jrs同理；

由上可知，只要给出对应点，每组对应点将会提供1个双线性关系式：然后由将x₁写作展开后可得：

$\left(\begin{array}{ccccccccc}{\mathrm{uu}}^{\prime }& {\mathrm{vu}}^{\prime }& u^{\prime }& {\mathrm{uv}}^{\prime }& {\mathrm{vv}}^{\prime }& v^{\prime }& u& v& 1\end{array}\right)\left(\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ ·\\ ·\\ ·\\ f_{33}\end{array}\right)=0;$

在两视点的情况下，给出8组对应点，则可以得到如下线性方程组：

$\left(\begin{array}{ccccccccc}{u}_1{u}_1^{\prime }& v_1{u}_1^{\prime }& u_1^{\prime }& u_1{v}_1^{\prime }& v_1{v}_1^{\prime }& v_1^{\prime }& u_1& v_1& 1\\ ·& & & & & & & & ·\\ ·& & & & & & & & ·\\ ·& & & & & & & & ·\\ u_8{u}_8^{\prime }& v_8{u}_8^{\prime }& u_8^{\prime }& u_8{v}_8^{\prime }& v_8{v}_8^{\prime }& v_8^{\prime }& u_8& v_8& 1\end{array}\right)\left(\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ ·\\ ·\\ ·\\ f_{\mathrm{33}}\end{array}\right)=\left(\begin{array}{c}0\\ ·\\ ·\\ ·\\ 0\end{array}\right);$

上述齐次方程组，被确认到只差常数因子，在秩为8时，能够采用线性算法求得唯一解。

进一步地，所述步骤(3)、步骤(4)中两焦点张量的算法如下：和

定义一个矢量t，矢量t是由所组成：在两视点的情况下，三视点的情况下，四视点的情况下，如果有N个对应点，会有如下的关系：Mt＝0，M分别是N*9，9N*27，81N*81的矩阵；

假设从给出的对极点中得到N₁个线性独立关系式，M₁t＝0，从对应点中得到N₂个线性独立关系式，M₂t＝0，将它们联立成一个线性关系式的系统：在M₁中加入0行，来创造出一个方阵M'₁，从SVD(M'₁)＝UDV^T中提取出V，并通过删去V的前N₁列获得V'，从中解得t'，最终从t＝V't'得到结果t。

进一步地，所述步骤(5)直到C₂和C₃在C₁^t图像上的e₁₂、e₁₃与运动前的投影位置e₁₂^t、e₁₃^t的欧氏距离都小于等于5pixel时，即实现C₁、C₂、C₃运动后的相对位置与其运动前的相对位置保持一致。

有益效果：与现有技术相比，本发明的优点：多视点几何技术运用到自动控制运动领域已经有二十年的历史，比如三维重建，视觉诱导等，广泛运用于多个机器人的自动控制、汽车的自动驾驶、摄像机系统、运动识别等，不过传统的多视点几何大都应用于静态的环境，对于动态的环境很少适用。本发明首先提出新的多视点几何，引入多台摄像机相互投影的知识，提升多视点几何的研究到更高的领域，它的稳定性相比于传统多视点几何有了很大提高，推动多视点几何在机器人控制中的作用，使得机器人适用于更复杂全动态的环境，很好地解决稳定性等技术难题。基于此理论可以制造适用性更强的机器人，满足人们日益增长的生产生活要求。

附图说明

图1为利用两焦点张量实现机器人协调运动的原理图。

具体实施方式

下面通过附图对本发明技术方案进行详细说明。

实施例1：本发明所述的基于多视点几何的多机器人协调运动方法，包括如下步骤：

(1)将多机器人系统中的两个机器人作为先导机，其余机器人作为跟随机，首先考虑一台跟随机与先导机之间的协调运动方法，跟随机记为C₁，两台先导机分别记为C₂和C₃；

(2)三台机器人运动前的位置分别为C₁^t、C₂^t、C₃^t，运动后的位置记为C₁^t+1、C₂^t+1、C₃^t+1，当先导机运动到C₂^t+1、C₃^t+1时，跟随机还在初始位置C₁^t、如图1所示，通过后续计算确定C₁^t+1的位置；

(3)计算C₁^t和C₂^t+1之间的两焦点张量F₁₂，根据多视点几何可得到C₁^t+1在图像上的投影直线l₂，计算公式为l₂＝F₁₂e₂₁^t，其中对极点e₂₁^t是C₁^t在C₂^t上的投影，e₂₁^t用来记忆移动前的队形；

(4)同步骤(3)，计算C₁和C₃^t+1之间的两焦点张量F₁₃，利用两焦点张量和对极点e₃₁^t之间关系式l₃＝F₁₃e₃₁^t，算出C₁^t+1在C₁^t上的投影直线l₃，e₃₁^t是C₁^t在C₃^t上的投影，结合步骤(3)得到的投影直线l₂，可得两条直线的交点X，也就是目的位置C₁^t+1在C₁^t图像上的投影；

(5)令跟随机C₁沿着C₁^t与X的连线运动，直到C₂和C₃在C₁^t图像上的e₁₂与e₁₃的位置和e₁₂^t与e₁₂^t的位置的欧氏距离都小于等于5pixel，以实现C₁^t+1、C₂^t+1、C₃^t+1的相对位置和C₁^t、C₂^t、C₃^t的位置保持一致，即完成对机器人协调运动队形保持的控制，同理其他的机器人也可用这种方法“跟进”。

其中两焦点张量F₁₂和F₁₃的算法如下：

(1)对应点法：

x、x’是空间点X在两幅图像平面上的投影点，两焦点张量的元素是行列式展开式中x′^jxⁱ的系数。可以写为：

其中，aⁱ、bⁱ分别是空间点X投影到两幅图像上x、x’点的投影矩阵A和B中的行向量；对r，s，t＝1，2，3，张量ε_rst定义如下：

ε_ipq、ε_jrs定义与ε_rst相同，因此对于给定i值，除非p、q不同于i且互不相同，否则张量ε_ipq为零，对ε_jrs同理。因此这个和式只包含四个非零项。而且出现于这四项中的行列式都包含矩阵A和B的同样的四行，从而除了符号外它们具有相等的值。然而，ε_ipqε_jrs的值使得这四项都有相同的符号并相等。

由上可以得知，只要给出对应点，每组对应点将会提供1个双线性关系式。然后由将x₁写作展开后可得：

$\left(\begin{array}{ccccccccc}{\mathrm{uu}}^{\prime }& {\mathrm{vu}}^{\prime }& u^{\prime }& {\mathrm{uv}}^{\prime }& {\mathrm{vv}}^{\prime }& v^{\prime }& u& v& 1\end{array}\right)\left(\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ ·\\ ·\\ ·\\ f_{33}\end{array}\right)=0.$

若给出8组匹配点，则可以得到如下线性方程组：

$\left(\begin{array}{ccccccccc}{u}_1{u}_1^{\prime }& v_1{u}_1^{\prime }& u_1^{\prime }& u_1{v}_1^{\prime }& v_1{v}_1^{\prime }& v_1^{\prime }& u_1& v_1& 1\\ ·& & & & & & & & ·\\ ·& & & & & & & & ·\\ ·& & & & & & & & ·\\ u_8{u}_8^{\prime }& v_8{u}_8^{\prime }& u_8^{\prime }& u_8{v}_8^{\prime }& v_8{v}_8^{\prime }& v_8^{\prime }& u_8& v_8& 1\end{array}\right)\left(\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ ·\\ ·\\ ·\\ f_{\mathrm{33}}\end{array}\right)=\left(\begin{array}{c}0\\ ·\\ ·\\ ·\\ 0\end{array}\right)$

这是一个齐次方程组，可以被确认到只差常数因子。如果存在一个解，左边的矩阵秩必然最多为8，如果秩正好为8，则解是唯一的，而且可以用线性算法求解。

可以看出，在两视点的情况下，计算出多视点几何的线性解法所需对应点个数为8。

(2)相互投影法

如果在第一幅图像上有第二个摄像机的投影，就可以得到近似的对极点，记做e₁₂，同理可以定义出e₂₁。这两个对极点和基本矩阵有如下的关系：

和

其中每一个关系都会提供三个线性约束。然而，这两个线性关系并不是相互独立的，若将两个关系联立，就只会提供五个线性约束。

当有两个摄像机时，会有两种投影情况：

(1)仅有1个摄像机被投影到另一个摄像机上；

(2)2个摄像机相互投影。

情况(1)时会获得1个对极点，这个对极点会提供3个独立线性约束。

情况(2)时会获得2个对极点，这2个对极点会提供5个独立线性关系约束。

因此，为了计算出两焦点张量，在不考虑相互投影时，需要8个对应点。在考虑相互投影时，第一种情况下只需要5个对应点，第二种情况下只需要3个对应点。

现在来介绍一种实用的方法，可以在相互投影情况下线性计算出多焦点张量。

定义一个矢量t，矢量t是由所组成：在两视点的情况下，三视点的情况下，四视点的情况下，如果有N个对应点，会有如下的关系：Mt＝0，M分别是N*9，9N*27，81N*81的矩阵；

假设从给出的对极点中得到N₁个线性独立关系式，M₁t＝0，从对应点中得到N₂个线性独立关系式，M₂t＝0，将它们联立成一个线性关系式的系统：

${[M_1^T,M_2^T]}^{T}t=0$

如果在这个系统中有比要求更多的线性关系式，就会有一个常用的方法去找出最小二乘解。最小二乘解会使得代数距离最小，即然而该解并不能严格满足对极点给出的约束。从结果来看，通过标准的最小二乘法从t中计算出的对极点并不能与给出的对极点相对应。为了解决这个问题，发现最小二乘法将最小化为M₁t＝0。奇异值分解可以有效的运用在这里。具体来说，通过在M₁中加入0行，来创造出一个方阵M'₁。从SVD(M'₁)＝UDV^T中提取出V，并通过删去V的前N₁列获得V'。从中解得t'。最终从t＝V't'得到结果t。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。