一種基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)方法
【專利摘要】本發(fā)明公開了一種基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)方法，包括如下步驟：指定多機(jī)器人系統(tǒng)中的兩臺(tái)機(jī)器人作為先導(dǎo)機(jī)，其余機(jī)器人作為跟隨機(jī)；計(jì)算跟隨機(jī)與兩臺(tái)先導(dǎo)機(jī)之間的兩焦點(diǎn)張量并由此獲得跟隨機(jī)目標(biāo)位置在圖像上的兩條投影直線，直線交點(diǎn)即為目標(biāo)位置在跟隨機(jī)圖像上的投影點(diǎn)；令跟隨機(jī)沿其攝像機(jī)圖像的光學(xué)中心與目標(biāo)位置投影點(diǎn)的連線運(yùn)動(dòng)，直到先導(dǎo)機(jī)在跟隨機(jī)攝像機(jī)圖像上的投影點(diǎn)位置與運(yùn)動(dòng)前的投影位置重合，對(duì)剩余跟隨機(jī)逐一重復(fù)上述處理，實(shí)現(xiàn)剩余跟隨機(jī)和兩臺(tái)先導(dǎo)機(jī)的相對(duì)位置與其運(yùn)動(dòng)前的相對(duì)位置保持一致。本發(fā)明提出將多視點(diǎn)幾何應(yīng)用于多機(jī)器人系統(tǒng)，引入多臺(tái)攝像機(jī)相互投影的知識(shí)，提高了控制穩(wěn)定性，推動(dòng)多視點(diǎn)幾何在機(jī)器人控制中的作用。
【專利說明】
-種基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及機(jī)器人控制技術(shù)領(lǐng)域，具體設(shè)及一種基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào) 運(yùn)動(dòng)方法。
【背景技術(shù)】
[0002] 多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)在實(shí)際生活中有著很廣泛的應(yīng)用。一方面，有些工作僅用一個(gè) 機(jī)器人很難完成，而通過多個(gè)機(jī)器人的合作就能很好的達(dá)到目的；另一方面，通過多機(jī)器人 間的協(xié)調(diào)合作，可提高機(jī)器人系統(tǒng)在作業(yè)過程中的效率。在很多應(yīng)用領(lǐng)域中，都要求多機(jī)器 人協(xié)調(diào)運(yùn)動(dòng)，尤其是在完成軍事、生產(chǎn)、消防等任務(wù)的情況下，還要求機(jī)器人作業(yè)和運(yùn)動(dòng)過 程中保持一定的隊(duì)形，如何控制機(jī)器人保持隊(duì)形顯得更為重要。運(yùn)就是機(jī)器人的隊(duì)形控制。
[0003] 在多機(jī)器人協(xié)調(diào)作業(yè)的研究和應(yīng)用領(lǐng)域，現(xiàn)在已有的控制方法，由于缺乏穩(wěn)定性， 機(jī)器人只能是在室內(nèi)或平地等比較單一的環(huán)境中實(shí)現(xiàn)協(xié)調(diào)運(yùn)動(dòng)，極大限制了機(jī)器人的使用 范圍和應(yīng)用條件，很難在更加普遍的復(fù)雜地形上實(shí)現(xiàn)多機(jī)器人的協(xié)調(diào)工作。

【發(fā)明內(nèi)容】

[0004] 發(fā)明目的：針對(duì)現(xiàn)有技術(shù)的不足，本發(fā)明提供一種基于多視點(diǎn)幾何的多機(jī)器人協(xié) 調(diào)運(yùn)動(dòng)方法，提高控制方法的穩(wěn)定性，使得機(jī)器人適用于更復(fù)雜全動(dòng)態(tài)的環(huán)境。
[0005] 技術(shù)方案:本發(fā)明所述基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)方法，用于多機(jī)器人 系統(tǒng)，包括如下步驟：
[0006] (1)指定多機(jī)器人系統(tǒng)中的兩臺(tái)機(jī)器人作為先導(dǎo)機(jī)，其余機(jī)器人作為跟隨機(jī)，兩臺(tái) 先導(dǎo)機(jī)先運(yùn)動(dòng)；
[0007] (2)選取一臺(tái)跟隨機(jī)記為Cl,兩臺(tái)先導(dǎo)機(jī)分別記為C2和〔3，(：1八2^3運(yùn)動(dòng)前的位置分 別記為Cit、Cst、Cst，，C沸C3先運(yùn)動(dòng)，運(yùn)動(dòng)后的位置分別為C2"l、C3"l;
[000引（3)計(jì)算Cl*與C2"1之間的兩焦點(diǎn)張量Fi2,利用公式l2 = Fi2e2it計(jì)算Cl目標(biāo)位置Ci"i 在Cl圖像上的投影直線h，其中對(duì)極點(diǎn)e2it是Cit在Cst圖像上的投影；
[0009] (4)計(jì)算Clt與C3"l之間的兩焦點(diǎn)張量Fi3,利用公式l3 = Fl363lt計(jì)算Cl目標(biāo)位置Cl"l 在Cl圖像上的投影直線13,其中對(duì)極點(diǎn)e3it是Cit在C3咽像上的投影，投影直線l3與步驟(3) 所得投影直線b的交點(diǎn)X即為Cl目標(biāo)位置Ci"i在Cl圖像上的投影點(diǎn)；
[0010] (5)令Cl沿其攝像機(jī)圖像的光學(xué)中屯、與Cl目標(biāo)位置投影點(diǎn)X的連線運(yùn)動(dòng)，直到C2、C3 在Cl攝像機(jī)圖像上的投影點(diǎn)位置ei2、ei3與運(yùn)動(dòng)前的投影位置ei2\ei3t重合，即實(shí)現(xiàn)Ci、C2、C3 運(yùn)動(dòng)后的相對(duì)位置與其運(yùn)動(dòng)前的相對(duì)位置保持一致，完成一臺(tái)跟隨機(jī)與兩臺(tái)先導(dǎo)機(jī)協(xié)調(diào)運(yùn) 動(dòng)隊(duì)形保持的控制；
[0011] (6)對(duì)剩余跟隨機(jī)逐一重復(fù)步驟(2)至(5)，實(shí)現(xiàn)剩余跟隨機(jī)和兩臺(tái)先導(dǎo)機(jī)的相對(duì) 位置與其運(yùn)動(dòng)前的相對(duì)位置保持一致，從而完成多機(jī)器人系統(tǒng)所有機(jī)器人協(xié)調(diào)運(yùn)動(dòng)隊(duì)形保 持的控制。
[0012] 進(jìn)一步完善上述技術(shù)方案，所述步驟(3)、步驟(4)中兩焦點(diǎn)張量的算法如下：
[OOU] 例'y'，.v-'，'v|)t =??'巧二Q ;
[0014] x、x'是空間點(diǎn)X在兩幅圖像平面上的投影點(diǎn)，兩焦點(diǎn)張量巧i的元素是行列式展開 式中V的系數(shù)，寫為：
[0015]
[0016] 其中，a\bi分別是空間點(diǎn)X投影到兩幅圖像上x、x'點(diǎn)的投影矩陣A和帥的行向量； 對(duì);r，s，t = l，2,3,張量Erst定義如下：
[0017]
[0018] 對(duì)給定i值，除非p、q不同于i且互不相同，否則張量Eipq為零，對(duì)Ejrs同理；
[0019] 由上可知，只要給出對(duì)應(yīng)點(diǎn)，每組對(duì)應(yīng)點(diǎn)將會(huì)提供1個(gè)雙線性關(guān)系式：然后由 材
將Xi寫作V，展開后可得：
[0020]
[0021 ]在兩視點(diǎn)的情況下，給出8組對(duì)應(yīng)點(diǎn)，則可W得到如下線性方程組：
[002；
~fn f。
[0023] 上述齊次方程組，乂3被確認(rèn)到只差常數(shù)因子，在秩為別寸，能夠采用線性算法求 乂3 - 得唯一解。
[0024] 進(jìn)一步地，所述步驟(3)、步驟(4)中兩焦點(diǎn)張量的算法如下:誠巧=Oi和咕丐=0,:
[00巧]定義一個(gè)矢量*，矢量*是由^，咬所組成:在兩視點(diǎn)的情況下：
； S視點(diǎn)的情況下，
；四視點(diǎn)的情況下
；如果有N個(gè) 對(duì)應(yīng)點(diǎn)，會(huì)有如下的關(guān)系:化= 〇，M分別是N蝴，9N*27,81N*81的矩陣；
[0026] 假設(shè)從給出的對(duì)極點(diǎn)中得到化個(gè)線性獨(dú)立關(guān)系式，Mit = O,從對(duì)應(yīng)點(diǎn)中得到化個(gè)線 性獨(dú)立關(guān)系式，M2t = 0,將它們聯(lián)立成一個(gè)線性關(guān)系式的系綺
：在M沖加入0 行，來創(chuàng)造出一個(gè)方陣M ' 1，從SVD(M ' 1) =UDyT中提取出V，并通過刪去V的前化列獲得V '，從
中解得t'，最終從t = V't'得到結(jié)果t。
[0027] 進(jìn)一步地，所述步驟(5)直到C2和C3在Cit圖像上的ei2、ei3與運(yùn)動(dòng)前的投影位置ei2\ ei3t的歐氏距離都小于等于Spixel時(shí)，即實(shí)現(xiàn)Ci、C2、C3運(yùn)動(dòng)后的相對(duì)位置與其運(yùn)動(dòng)前的相對(duì) 位置保持一致。
[0028] 有益效果:與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點(diǎn)：多視點(diǎn)幾何技術(shù)運(yùn)用到自動(dòng)控制運(yùn)動(dòng) 領(lǐng)域已經(jīng)有二十年的歷史，比如=維重建，視覺誘導(dǎo)等，廣泛運(yùn)用于多個(gè)機(jī)器人的自動(dòng)控 審IJ、汽車的自動(dòng)駕駛、攝像機(jī)系統(tǒng)、運(yùn)動(dòng)識(shí)別等，不過傳統(tǒng)的多視點(diǎn)幾何大都應(yīng)用于靜態(tài)的 環(huán)境，對(duì)于動(dòng)態(tài)的環(huán)境很少適用。本發(fā)明首先提出新的多視點(diǎn)幾何，引入多臺(tái)攝像機(jī)相互投 影的知識(shí)，提升多視點(diǎn)幾何的研究到更高的領(lǐng)域，它的穩(wěn)定性相比于傳統(tǒng)多視點(diǎn)幾何有了 很大提高，推動(dòng)多視點(diǎn)幾何在機(jī)器人控制中的作用，使得機(jī)器人適用于更復(fù)雜全動(dòng)態(tài)的環(huán) 境，很好地解決穩(wěn)定性等技術(shù)難題。基于此理論可W制造適用性更強(qiáng)的機(jī)器人，滿足人們?nèi)?益增長的生產(chǎn)生活要求。
【附圖說明】
[0029] 圖1為利用兩焦點(diǎn)張量實(shí)現(xiàn)機(jī)器人協(xié)調(diào)運(yùn)動(dòng)的原理圖。
【具體實(shí)施方式】
[0030] 下面通過附圖對(duì)本發(fā)明技術(shù)方案進(jìn)行詳細(xì)說明。
[0031] 實(shí)施例1:本發(fā)明所述的基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)方法，包括如下步 驟：
[0032] (1)將多機(jī)器人系統(tǒng)中的兩個(gè)機(jī)器人作為先導(dǎo)機(jī)，其余機(jī)器人作為跟隨機(jī)，首先考 慮一臺(tái)跟隨機(jī)與先導(dǎo)機(jī)之間的協(xié)調(diào)運(yùn)動(dòng)方法，跟隨機(jī)記為Cl,兩臺(tái)先導(dǎo)機(jī)分別記為C2和C3; [003引（2);臺(tái)機(jī)器人運(yùn)動(dòng)前的位置分別為扣\〔2\〔3^運(yùn)動(dòng)后的位置記為(：1"1心"1心* +1，當(dāng)先導(dǎo)機(jī)運(yùn)動(dòng)到C2WX3W時(shí)，跟隨機(jī)還在初始位置Ci\如圖1所示，通過后續(xù)計(jì)算確定Cit +1的位置；
[0034] (3)計(jì)算Cl哺C2"i之間的兩焦點(diǎn)張量Fi2,根據(jù)多視點(diǎn)幾何可得到Ci"i在圖像上的 投影直線12,計(jì)算公式為b = Fl2e2lt，其中對(duì)極點(diǎn)是Clt在Cst上的投影，用來記憶移動(dòng) 前的隊(duì)形；
[00對(duì) （4)同步驟(3)，計(jì)算Cl和C3"1之間的兩焦點(diǎn)張量Fi3,利用兩焦點(diǎn)張量和對(duì)極點(diǎn)63lt 之間關(guān)系式l3 = Fl363lt，算出Clt+1在Clt上的投影直線l3，e3lt是Cit在Cst上的投影，結(jié)合步驟 (3)得到的投影直線12,可得兩條直線的交點(diǎn)X，也就是目的位置Cl"唯Cit圖像上的投影； [0036] (5)令跟隨機(jī)Cl沿著Cit與X的連線運(yùn)動(dòng)，直到C2和C3在Cit圖像上的ei2與ei3的位置和 eut與eut的位置的歐氏距離都小于等于5pixel，W實(shí)現(xiàn)Cit+i、C2t+i、C3t+i的相對(duì)位置和Ci\ C2\C3t的位置保持一致，即完成對(duì)機(jī)器人協(xié)調(diào)運(yùn)動(dòng)隊(duì)形保持的控制，同理其他的機(jī)器人也 可用運(yùn)種方法"跟進(jìn)"。
[0037]其中兩焦點(diǎn)張量內(nèi)2和Fi3的算法如下：
[003引（1)對(duì)應(yīng)點(diǎn)法：（八-、:'-'，/.>心-'，-、-:，.1叩=八、->'1,=0;
[0039] x、x'是空間點(diǎn)X在兩幅圖像平面上的投影點(diǎn)，兩焦點(diǎn)張量<F的元素是行列式展開 式中?l的系數(shù)?？蒞寫為：
[0040]
[0041] 其中，a\bi分別是空間點(diǎn)X投影到兩幅圖像上x、x'點(diǎn)的投影矩陣A和帥的行向量； 對(duì);r，s，t = l，2,3,張量Erst定義如下：
[0042]
[0043] eipq、￡jrs定義與Erst相同，因此對(duì)于給定i值，除非p、q不同于i且互不相同，否則張 量Eipq為零，對(duì)Ejrs同理。因此運(yùn)個(gè)和式只包含四個(gè)非零項(xiàng)。而且出現(xiàn)于運(yùn)四項(xiàng)中的行列式都 包含矩陣A和B的同樣的四行，從而除了符號(hào)外它們具有相等的值。然而，EipqE^s的值使得運(yùn) 四項(xiàng)都有相同的符號(hào)并相等。
[0044] 由上可W得知，只要給出對(duì)應(yīng)點(diǎn)，每組對(duì)應(yīng)點(diǎn)將會(huì)提供1個(gè)雙線性關(guān)系式。然后由 U
。將Xl寫作T，展開后可得：
[0045；
[0046] 若給出8組陽配點(diǎn)，則可W得到化下線忡方巧組：
[0047]
7ii" f。
[004引運(yùn)是一個(gè)齊次方程組，乂3可W被確認(rèn)到只差常數(shù)因子。如果存在一個(gè)解，左邊的 一 矩陣秩必然最多為8，如果秩正好為8，則解是唯一的，而且可W用線性算法求解。
[0049]可W看出，在兩視點(diǎn)的情況下，計(jì)算出多視點(diǎn)幾何的線性解法所需對(duì)應(yīng)點(diǎn)個(gè)數(shù)為 8。
[(K)加](2)相互投影法
[0051]如果在第一幅圖像上有第二個(gè)攝像機(jī)的投影，就可W得到近似的對(duì)極點(diǎn)，記做612， 同理可W定義出621。運(yùn)兩個(gè)對(duì)極點(diǎn)和基本矩陣有如下的關(guān)系：
[0化2]嗦巧和 e'2 巧,'=0,。
[0053] 其中每一個(gè)關(guān)系都會(huì)提供=個(gè)線性約束。然而，運(yùn)兩個(gè)線性關(guān)系并不是相互獨(dú)立 的，若將兩個(gè)關(guān)系聯(lián)立，就只會(huì)提供五個(gè)線性約束。
[0054] 當(dāng)有兩個(gè)攝像機(jī)時(shí)，會(huì)有兩種投影情況：
[0055] (1)僅有1個(gè)攝像機(jī)被投影到另一個(gè)攝像機(jī)上；
[0056] (2) 2個(gè)攝像機(jī)相互投影。
[0057] 情況(1)時(shí)會(huì)獲得1個(gè)對(duì)極點(diǎn)，運(yùn)個(gè)對(duì)極點(diǎn)會(huì)提供3個(gè)獨(dú)立線性約束。
[0058] 情況(2)時(shí)會(huì)獲得2個(gè)對(duì)極點(diǎn)，運(yùn)2個(gè)對(duì)極點(diǎn)會(huì)提供5個(gè)獨(dú)立線性關(guān)系約束。
[0059] 因此，為了計(jì)算出兩焦點(diǎn)張量，在不考慮相互投影時(shí)，需要8個(gè)對(duì)應(yīng)點(diǎn)。在考慮相互 投影時(shí)，第一種情況下只需要5個(gè)對(duì)應(yīng)點(diǎn)，第二種情況下只需要3個(gè)對(duì)應(yīng)點(diǎn)。
[0060] 現(xiàn)在來介紹一種實(shí)用的方法，可W在相互投影情況下線性計(jì)算出多焦點(diǎn)張量。
[0061] 定義一個(gè)矢量t，矢量t是由取，"T, 〇_所組成:在兩視點(diǎn)的情況下，t =[巧1，....，巧,]T; 立視點(diǎn)的情況下
四視點(diǎn)的情況下，*4隻…\....，公333了棚果有N個(gè) 對(duì)應(yīng)點(diǎn)，會(huì)有如下的關(guān)系:化= 〇，M分別是N蝴，9N*27,81N*81的矩陣；
[0062] 假設(shè)從給出的對(duì)極點(diǎn)中得到化個(gè)線性獨(dú)立關(guān)系式，Mit = O,從對(duì)應(yīng)點(diǎn)中得到化個(gè)線 性獨(dú)立關(guān)系式，M2t = 0,將它們聯(lián)立成一個(gè)線性關(guān)系式的系統(tǒng)：
[0063] 昕，
[0064] 如果在運(yùn)個(gè)系統(tǒng)中有比要求更多的線性關(guān)系式，就會(huì)有一個(gè)常用的方法去找出最 小二乘解。最小二乘解會(huì)使得代數(shù)距離最小，即，雌了 1。然而該解并不能嚴(yán)格滿足對(duì) 極點(diǎn)給出的約束。從結(jié)果來看，通過標(biāo)準(zhǔn)的最小二乘法從t中計(jì)算出的對(duì)極點(diǎn)并不能與給出 的對(duì)極點(diǎn)相對(duì)應(yīng)。為了解決運(yùn)個(gè)問題，發(fā)現(xiàn)最小二乘法將了 t|最小化為Mit = 0。奇 異值分解可W有效的運(yùn)用在運(yùn)里。具體來說，通過在化中加入0行，來創(chuàng)造出一個(gè)方陣M'l。從 SVD (M ' 1) = UDyT中提取出V，并通過刪去V的前化列獲得V '。從[Mf，巧:' =Q中解得t '。最 終從t = V't'得到結(jié)果t。
[0065]如上所述，盡管參照特定的優(yōu)選實(shí)施例已經(jīng)表示和表述了本發(fā)明，但其不得解釋 為對(duì)本發(fā)明自身的限制。在不脫離所附權(quán)利要求定義的本發(fā)明的精神和范圍前提下，可對(duì) 其在形式上和細(xì)節(jié)上作出各種變化。
【主權(quán)項(xiàng)】
1. 一種基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)方法，用于多機(jī)器人系統(tǒng)，其特征在于:包 括如下步驟： (1) 指定多機(jī)器人系統(tǒng)中的兩臺(tái)機(jī)器人作為先導(dǎo)機(jī)，其余機(jī)器人作為跟隨機(jī)，兩臺(tái)先導(dǎo) 機(jī)先運(yùn)動(dòng)； (2) 選取一臺(tái)跟隨機(jī)記為&，兩臺(tái)先導(dǎo)機(jī)分別記為&和(：3，(：1、(：2、(： 3運(yùn)動(dòng)前的位置分別記 為C。、C，、Cf，C2和C3先運(yùn)動(dòng)，運(yùn)動(dòng)后的位置分別為C 2t+1、C3t+1; (3) 計(jì)算Ci13與C2t+1之間的兩焦點(diǎn)張量F12,利用公式l2 = Fi2e2it計(jì)算Ci目標(biāo)位置Cit+1在Ci 圖像上的投影直線12,其中對(duì)極點(diǎn)e21t是c。在c2t圖像上的投影； ⑷計(jì)算Clt與C3t+1之間的兩焦點(diǎn)張量Fl3,利用公式l3 = Fl3e3lt計(jì)算Cl目標(biāo)位置Clt+1在Cl 圖像上的投影直線13,其中對(duì)極點(diǎn)在C3t圖像上的投影，投影直線13與步驟⑶所得 投影直線h的交點(diǎn)X即為&目標(biāo)位置(^ +1在&圖像上的投影點(diǎn)； (5) 令(^沿其攝像機(jī)圖像的光學(xué)中心與&目標(biāo)位置投影點(diǎn)X的連線運(yùn)動(dòng)，直到(^、(^在心 攝像機(jī)圖像上的投影點(diǎn)位置e 12、e13與運(yùn)動(dòng)前的投影位置eiAeJ重合，即實(shí)現(xiàn)&、(：2、(： 3運(yùn)動(dòng) 后的相對(duì)位置與其運(yùn)動(dòng)前的相對(duì)位置保持一致，完成一臺(tái)跟隨機(jī)與兩臺(tái)先導(dǎo)機(jī)協(xié)調(diào)運(yùn)動(dòng)隊(duì) 形保持的控制； (6) 對(duì)剩余跟隨機(jī)逐一重復(fù)步驟(2)至(5)，實(shí)現(xiàn)剩余跟隨機(jī)和兩臺(tái)先導(dǎo)機(jī)的相對(duì)位置 與其運(yùn)動(dòng)前的相對(duì)位置保持一致，從而完成多機(jī)器人系統(tǒng)所有機(jī)器人協(xié)調(diào)運(yùn)動(dòng)隊(duì)形保持的 控制。2. 根據(jù)權(quán)利要求1所述基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)方法，其特征在于:所述步 驟(3)、步驟(4)中兩焦點(diǎn)張量的算法如下：x、x'是空間點(diǎn)X在兩幅圖像平面上的投影點(diǎn)，兩焦點(diǎn)張量巧:的元素是行列式展開式中 W的系數(shù)，寫為：其中，a1 A1分別是空間點(diǎn)X投影到兩幅圖像上x、x'點(diǎn)的投影矩陣A和B中的行向量;ε1Μ、 ejrs定義為:對(duì)r，s，t = l，2,3,張量erst為：對(duì)給定i值，除非p、q不同于i且互不相同，否則張量￡iPq為零，對(duì)￡jrs同理； 由上可知，只要給出對(duì)應(yīng)點(diǎn)，每組對(duì)應(yīng)點(diǎn)將會(huì)提供1個(gè)雙線性關(guān)系式:然后由χΓΦ? =0， 將幻寫{展開后可得：在兩視點(diǎn)的情況下，給出8組對(duì)應(yīng)點(diǎn)，則可以得到如下線性方程組：上述齊次方程組確認(rèn)到只差常數(shù)因子，在秩為8時(shí)，能夠采用線性算法求得唯 一解。3. 根據(jù)權(quán)利要求1所述基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)方法，其特征在于:所述步 驟(3)、步驟(4)中兩焦點(diǎn)張量的算法如下：zOjPe 1#:, =0」； 定義一個(gè)矢量t，矢量t是由<F，T，〇_所組成:在兩視點(diǎn)的情況下，t = ;三視 點(diǎn)的情況下，t = [￥；"，....，Tf ;四視點(diǎn)的情況下，t =[y ;如果有N個(gè)對(duì)應(yīng) 點(diǎn)，會(huì)有如下的關(guān)系:Mt = 0，M分別是Ν*9,9Ν*27,81Ν*81的矩陣； 假設(shè)從給出的對(duì)極點(diǎn)中得到Νι個(gè)線性獨(dú)立關(guān)系式，Mit = 0，從對(duì)應(yīng)點(diǎn)中得到Ν2個(gè)線性獨(dú) 立關(guān)系式，M2t = 0，將它們聯(lián)立成一個(gè)線性關(guān)系式的系統(tǒng)：t = 0 ;在Μι中加入0行， 來創(chuàng)造出一個(gè)方陣M'i，從SVDm'OzUDVT*提取出V，并通過刪去V的前Ni列獲得V'，從 [M「,M2 T了 V't' = 0中解得t '，最終從t = V ' t '得到結(jié)果t。4. 根據(jù)權(quán)利要求1所述基于多視點(diǎn)幾何的多機(jī)器人協(xié)調(diào)運(yùn)動(dòng)方法，其特征在于:所述步 驟(5)中直到C#PC3在圖像上的 612、613與運(yùn)動(dòng)前的投影位置的歐氏距離都小于等 于5p ixe 1時(shí)，即實(shí)現(xiàn)Ci、C2、C3運(yùn)動(dòng)后的相對(duì)位置與其運(yùn)動(dòng)前的相對(duì)位置保持一致。
【文檔編號(hào)】G05D1/02GK105955271SQ201610333164
【公開日】2016年9月21日
【申請日】2016年5月18日
【發(fā)明人】萬程
【申請人】南京航空航天大學(xué)