激光測距儀和機(jī)械臂末端關(guān)系的正交平面標(biāo)定方法及系統(tǒng)的制作方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及視覺測量領(lǐng)域，具體設(shè)及一種激光測距儀和機(jī)械臂末端的空間變換關(guān) 系的正交平面標(biāo)定方法及系統(tǒng)。
【背景技術(shù)】
[0002] 隨著電子工業(yè)水平的提高，計算機(jī)技術(shù)和光學(xué)傳感器技術(shù)的發(fā)展，使得機(jī)器視覺 在工業(yè)加工、航空、汽車、船舶、娛樂、醫(yī)療等領(lǐng)域都有廣泛的應(yīng)用。
[0003] 為了利用機(jī)械臂完成立體定向電極植入手術(shù)，需要將機(jī)械臂的坐標(biāo)系和病人的Ξ 維空間坐標(biāo)進(jìn)行配準(zhǔn)。而配準(zhǔn)的基礎(chǔ)是建立機(jī)械臂末端（比如爪手、法蘭盤等)和傳感器(相 機(jī)、結(jié)構(gòu)光設(shè)備、激光測距儀等)之間的變換關(guān)系，求解運(yùn)個變換關(guān)系的問題稱為手眼標(biāo)定 問題。
[0004] 對機(jī)械臂的手眼標(biāo)定問題，已經(jīng)有不少學(xué)者提出相應(yīng)的解決辦法，Zhuang將機(jī)械 臂和傳感器一起建模，用于機(jī)械臂運(yùn)動學(xué)參數(shù)校正，Tsai將手眼標(biāo)定分為旋轉(zhuǎn)和平移兩個 部分分別求解，Ma采用相互正交的純平移來線性化手眼矩陣的旋轉(zhuǎn)部分，對旋轉(zhuǎn)部分的求 解需要Ξ次有解的平移運(yùn)動，Malm采用光流場的法向?qū)?shù)方法，SMu等人將手眼標(biāo)定問題 歸結(jié)為求解AX=XB的問題。
[000引 在文獻(xiàn)[Theinternationaljournalofroboticsresearch, 1995,14(3): 195-210]中給出了通用的求解手眼標(biāo)定方法，對它進(jìn)行必要的修改可W適合相機(jī)、激光測距儀、 結(jié)構(gòu)光等不同的傳感器，但具體如何修改使得它適應(yīng)不同的傳感器，需要讀者做一定創(chuàng)造 性探索。在文獻(xiàn)[RoboticsandAutomation,IE邸Transactionson, 1998,14(3) :493-497]中給出一般情況下激光測距儀和機(jī)械臂的手眼標(biāo)定方法，但具體如何從將在機(jī)械臂坐 標(biāo)系下坐標(biāo)的變換轉(zhuǎn)換到激光測距儀坐標(biāo)系下的變換，該文獻(xiàn)[Roboticsand Automation,I邸ETransactionson, 1998,14(3) :493-497]中并沒有明確給出，要想實(shí)現(xiàn) 它也需要讀者做一定的創(chuàng)造性探索。并且當(dāng)前的方法在理論上可W得到很高的精度，但是 在實(shí)踐中由于約束條件不足，導(dǎo)致求解手眼標(biāo)定矩陣的可重復(fù)性較差。

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

[0006] 為了解決現(xiàn)有技術(shù)中存在的問題，本發(fā)明提出了一種激光測距儀和機(jī)械臂末端的 空間變換關(guān)系的正交平面標(biāo)定方法及系統(tǒng)，使用固定在機(jī)械臂末端的激光測距儀來實(shí)現(xiàn)對 Ξ維空間中物體上點(diǎn)的重建，它具有測量精度高，價格相對低廉的優(yōu)點(diǎn)。
[0007] 本發(fā)明的一個目的在于提出一種激光測距儀和機(jī)械臂末端的空間變換關(guān)系的正 交平面標(biāo)定系統(tǒng)。
[0008] 本發(fā)明的激光測距儀和機(jī)械臂末端的空間變換關(guān)系的正交平面標(biāo)定系統(tǒng)包括:機(jī) 械臂、機(jī)械臂末端、激光測距儀W及Ξ個相互正交的標(biāo)定平面;其中，激光測距儀固定在機(jī) 械臂末端，二者相對位置不變;在空間中放置Ξ個相互正交的第一至第Ξ標(biāo)定平面P1~P3， 標(biāo)定平面相對于機(jī)械臂的位置固定，標(biāo)定平面的表面平整;機(jī)械臂連接至機(jī)械臂的控制器， 從機(jī)械臂的控制器中直接讀取出機(jī)械臂末端坐標(biāo)系相對于機(jī)械臂坐標(biāo)系的旋轉(zhuǎn)矩陣和平 移列向量。
[0009]本發(fā)明的另一個目的在于提出一種激光測距儀和機(jī)械臂末端的空間變換關(guān)系的 正交平面標(biāo)定方法。
[0010] 本發(fā)明的激光測距儀和機(jī)械臂末端的空間變換關(guān)系的正交平面標(biāo)定方法，包括W 下步驟：
[0011] 1)建立空間變換關(guān)系：
[0012] 機(jī)械臂末端坐標(biāo)系為Om，激光測距儀坐標(biāo)系為化，激光測距儀坐標(biāo)系相對于機(jī)械臂 末端坐標(biāo)系只發(fā)生平移r，激光測距儀發(fā)出的激光束的單位方向向量在激光測距儀坐標(biāo)系 下為V，機(jī)械臂末端坐標(biāo)系下激光束照射到標(biāo)定平面上的點(diǎn)為X，即機(jī)械臂末端坐標(biāo)系的原 點(diǎn)到激光束照射到標(biāo)定平面上的點(diǎn)表示為X，激光測距儀測量得到的距離為d，即在激光測 距儀坐標(biāo)系下，激光測距儀坐標(biāo)系的原點(diǎn)到激光束照射到標(biāo)定平面的點(diǎn)X的距離為d，根據(jù) 機(jī)械臂末端坐標(biāo)系和激光測距儀坐標(biāo)系的關(guān)系，對于照射到標(biāo)定平面上的點(diǎn)表示為:x=r+ dv，其中，r為平移向量，V為激光束的單位方向向量，二者未知，下面分別求得r和V;由第一 標(biāo)定平面P1求得的激光束的單位方向向量為VI，平移向量為ri，由第二標(biāo)定平面P2求得的激 光束的單位方向向量為V2,平移向量為K，由第Ξ標(biāo)定平面P3求得的激光束的單位方向向量 為V3,平移向量為Γ3;
[0013] 2)通過控制機(jī)械臂運(yùn)動，使得激光測距儀發(fā)出的激光束分別照射到第一至第Ξ標(biāo) 定平面上，分別測量并求解得到VI和ri、V沸ηW及V3和η，其中，VI和η分別為由第一標(biāo)定平 面Ρ1求得的激光束的單位方向向量和平移向量，V2和η分別為由第二標(biāo)定平面Ρ2求得的激 光束的單位方向向量和平移向量，W及V3和η分別為由第Ξ標(biāo)定平面Ρ3求得的激光束的單 位方向向量和平移向量；
[0014] 3)優(yōu)化計算平移向量r和激光束的單位方向向量V:
[0015] 設(shè)定初始值、約束函數(shù)和目標(biāo)函數(shù)，采用迭代算法，直至滿足給定的收斂條件為 止，得到優(yōu)化計算后的平移向量r和激光束的單位方向向量V，完成激光測距儀和機(jī)械臂末 端的空間變換關(guān)系的標(biāo)定；
[0016] 4)對于激光束照射到Ξ維空間中物體的一個點(diǎn)，激光測距儀測量得到的距離為d，根據(jù)步驟3)得到的激光束的單位方向向量V和平移向量r，在機(jī)械臂末端坐標(biāo)系下，該點(diǎn)表 示為X=r+dv，完成對Ξ維空間中物體上該點(diǎn)在機(jī)械臂末端坐標(biāo)系下空間坐標(biāo)的測量。
[0017]其中，在步驟2)中，由第一至第Ξ標(biāo)定平面P1~P3,分別測量并求解得到VI和ri、V2 和Γ2W及V3和Γ3，包括W下步驟：
[0018] a)測量并求解由第一標(biāo)定平面P1求得的激光束的單位方向向量VI:
[0019] i.控制機(jī)械臂運(yùn)動，激光測距儀發(fā)出的激光束照射在第一標(biāo)定平面P1上，保持機(jī) 械臂不動，記錄運(yùn)時的激光測距儀測量的距離瑞1，并且直接從機(jī)械臂的控制器中讀取機(jī)械 臂末端坐標(biāo)系相對于機(jī)械臂坐標(biāo)系的旋轉(zhuǎn)矩陣i?以和平移列向量巧。1，記為初始位姿點(diǎn)；
[0020] ii.保持姿態(tài)不改變，機(jī)械臂末端相對于初始位姿點(diǎn)只做平移運(yùn)動，記錄第j次平 移后的第j位置的激光測距儀測量的距離并且直接從機(jī)械臂的控制器中讀取機(jī)械臂末 端坐標(biāo)系相對于機(jī)械臂坐標(biāo)系的旋轉(zhuǎn)矩陣和平移列向量If，多次平移后測量得到第一 標(biāo)定平面PI的第一組平動數(shù)據(jù)，在機(jī)械臂末端坐標(biāo)系下初始位姿點(diǎn)的第一標(biāo)定平面的單位 法向量端)滿足：
[0021]
[0022] 得到第一標(biāo)定平面P1的第一組約束，其中，j為含2的自然數(shù)，

[0023]iii.調(diào)整機(jī)械臂末端的位姿，使其相對于初始位姿點(diǎn)既有平移又有旋轉(zhuǎn)運(yùn)動，且 激光測距儀發(fā)出的激光束照射在第一標(biāo)定平面P1上，固定運(yùn)時的位姿，記為第二位姿點(diǎn)，記 錄運(yùn)時的激光測距儀測量的距離誠2，并且直接從機(jī)械臂的控制器中讀取機(jī)械臂末端坐標(biāo) 系相對于機(jī)械臂坐標(biāo)系的旋轉(zhuǎn)矩陣每^和平移列向量瑞，在機(jī)械臂末端坐標(biāo)系下第二位姿 點(diǎn)的第一標(biāo)定平面的單位法向量
其中，S話為在機(jī)械臂末端坐標(biāo)系下， 從原始位姿點(diǎn)到第二位姿點(diǎn)的旋轉(zhuǎn)矩陣，并且
[0024]iv.保持姿態(tài)不改變，機(jī)械臂末端相對于第二位姿點(diǎn)只做平移運(yùn)動，記錄第j次平 移后的第j位置的激光測距儀測量的距離，并且直接從機(jī)械臂的控制器中讀取旋轉(zhuǎn)矩陣 巧;和平移列向量巧2,多次平移后測量得到第一標(biāo)定平面P1的第二組平動數(shù)據(jù)，在機(jī)械臂 末端坐標(biāo)系下第二位姿點(diǎn)的第一標(biāo)定平面P1的單位法向量《5滿足：
[0025]
[0026] 得到第一標(biāo)定平面P1的第二組約束，其中，j為> 2的自然數(shù)

[0027] V.調(diào)整機(jī)械臂末端的位姿，使其相對于初始位姿點(diǎn)既有平移又有旋轉(zhuǎn)運(yùn)動，且激 光測距儀發(fā)出的激光束照射在第一標(biāo)定平面P1上，固定運(yùn)時的位姿使其不同于初始位姿點(diǎn) 和第二位姿點(diǎn)，記為第Ξ位姿點(diǎn)，記錄運(yùn)時的激光測距儀測量的距離，并且直接從機(jī)械 臂的控制器中讀取機(jī)械臂末端坐標(biāo)系相對于機(jī)械臂坐標(biāo)系的旋轉(zhuǎn)矩陣巧器和平移列向量 枯在機(jī)械臂末端坐標(biāo)系下第Ξ位姿點(diǎn)的第一標(biāo)定平面P1的單位法向量
其中，i?為為在機(jī)械臂末端坐標(biāo)系下，從原始位姿點(diǎn)到第Ξ位姿點(diǎn)的旋轉(zhuǎn)矩陣，并且
[0028] Vi.保持姿態(tài)不改變，機(jī)械臂末端相對于第Ξ位姿點(diǎn)只做平移運(yùn)動，記錄第j次平 移后的第j位置的激光測距儀測量的距離^3,并且直接從機(jī)械臂的控制器中讀取旋轉(zhuǎn)矩陣 時;'和平移列向量巧3,多次平移后測量得到第一標(biāo)定平面PI的第Ξ組平動數(shù)據(jù)，在機(jī)械臂 末端坐標(biāo)系下第Ξ位姿點(diǎn)的第一標(biāo)定平面的單位法向量苗f。滿足：
[0029]
[0030] 得到第一標(biāo)定平面P1的第Ξ組約束，其中，j為> 2的自然數(shù)，.瑣/ =(媒Γ'(皆-域)，
[0031]vii.使用上面Ξ組第一標(biāo)定平面P1的約束和單位向量模的約束I誠。If=1，求解出 城和pll、pl嘴pU的值；
[0032]viii.由
，求解出由第一標(biāo)定平面P1 求得的激光束的單位方向向量VI;
[0033] b)測量并求解由第一標(biāo)定平面P1求得的平移向量ri:
[0034] 控制機(jī)械臂相對于初始位姿點(diǎn)做旋轉(zhuǎn)平移運(yùn)動，至少旋轉(zhuǎn)平移k次，且激光測距儀 發(fā)出的激光束照射在第一標(biāo)定平面P1上，記錄第j次旋轉(zhuǎn)平移后的第j位置的激光測距儀測 量的距離并且直接從機(jī)械臂的控制器中讀取旋轉(zhuǎn)矩陣成,4和平移列向量3?4，滿足約束 方程：
[0035]
[0036] 其中，1幻' ^，k為含3的自然數(shù)，端為在機(jī)械臂末端坐標(biāo)系下初始位姿點(diǎn)的第一 標(biāo)定平面P1的單位法向量，在步驟a)中已經(jīng)得到，
對于每一次的旋轉(zhuǎn)平移，分別列出約束方程，一共k個約束方程，k個約束方程聯(lián)立得到由第 一標(biāo)定平面P1求得的平移向量ri;
[0037] C)測量并求解由第二標(biāo)定平面P2求得的激光束的單位方向向量V2:
[0038] i.控制機(jī)械臂運(yùn)動，激光測距儀發(fā)出的激光束照射在第二標(biāo)定平面P2上，保持機(jī) 械臂不動，記錄運(yùn)時的激光測距儀測量的距離拆1，:并且直接從機(jī)械臂的控制器中讀取機(jī)械 臂末端坐標(biāo)系相對于機(jī)械臂坐標(biāo)系的旋轉(zhuǎn)矩陣和平移列向量記/，記為第二標(biāo)定平面P2 的初始位姿點(diǎn)；
[0039] ii.保持姿態(tài)不改變，機(jī)械臂末端相對于第二標(biāo)定平面P2的初始位姿點(diǎn)只做平移 運(yùn)動，記錄第j次平移后的第j位置的激光測距儀測量的距離并且直接從機(jī)械臂的控制 器中讀取機(jī)械臂末端坐標(biāo)系相對于機(jī)械臂坐標(biāo)系的旋轉(zhuǎn)矩陣成/和平移列向量巧1，多次平 移后測量得到第二標(biāo)定平面P2的第一組平動數(shù)據(jù)，在機(jī)械臂末端坐