隨機(jī)生成空間坐標(biāo)系的數(shù)學(xué)分析和實(shí)現(xiàn)

賀京杰

（北京電影學(xué)院 未來影像高精尖創(chuàng)新中心,北京100088）

1 引言

本實(shí)驗(yàn)室開發(fā)攝影機(jī)器人為PRRPR-S 型雙冗余度機(jī)器人,主要用于攝像機(jī)的運(yùn)動(dòng)控制。由于冗余機(jī)器人位置級(jí)運(yùn)動(dòng)學(xué)反解有無窮個(gè),故優(yōu)化反解算法不唯一,且優(yōu)化結(jié)果不穩(wěn)定。對(duì)于不同的反解算法,需要測(cè)試算法是否可以收斂,算法的運(yùn)行速度以及收斂程度。通過一定精度的遍歷算法,可確定單個(gè)目標(biāo)位姿全局最優(yōu)解的所在小區(qū)間。以此為基準(zhǔn),可判定其他算法的收斂效果。算法效率的驗(yàn)證,一般采用實(shí)驗(yàn)例。在位置級(jí)空間中,第一種實(shí)驗(yàn)例是目標(biāo)位姿,即使用不同的算法反解同一個(gè)特定的目標(biāo)位姿；第二種實(shí)驗(yàn)例是目標(biāo)軌跡,但是軌跡只是工作空間的一個(gè)較小子集。位置級(jí)實(shí)驗(yàn)例方法在一定程度上可以了解算法性能,但是缺少對(duì)完整工作空間的全面認(rèn)識(shí)。

在速度級(jí)空間中,除了上述兩種位置級(jí)空間的實(shí)驗(yàn)例形式外,還有一種實(shí)驗(yàn)例是目標(biāo)球面,即在選定機(jī)器人初始狀態(tài)后,以當(dāng)前末端點(diǎn)為圓心,半徑為r的球面上以平均分布概率選取目標(biāo)方向。這種方案主要用于速度級(jí)反解性能評(píng)價(jià),無法全面反應(yīng)位置級(jí)目標(biāo)空間。

面向完整工作空間的位置級(jí)算法性能描述主要有三種方式:第一種是人為設(shè)置多目標(biāo)位姿樣本,使其盡可能布滿工作空間,由于本機(jī)型工作空間大,不僅有位置信息,還有姿態(tài)信息,故該方法無法在較短時(shí)間內(nèi)得到全面性結(jié)果,如果人為設(shè)置少量目標(biāo)位姿樣本,測(cè)試方法有失全面性；第二種是通過數(shù)學(xué)論證,該方法可以精確描述反解算法性能,但是缺少對(duì)性能直觀感受；第三種采用蒙特卡洛法,即在工作空間內(nèi)隨機(jī)生成多個(gè)目標(biāo)位姿樣本,記錄不同算法的結(jié)果空間距離,通過統(tǒng)計(jì)計(jì)算判定算法性能,該方法具有直觀性和一定程度的全面性。但是目前只有隨機(jī)生成位置目標(biāo)樣本的算法。本文基于蒙特卡洛法,詳細(xì)論述隨機(jī)生成位姿目標(biāo)樣本(空間坐標(biāo)系)的數(shù)學(xué)分析,通過編寫程序在攝影機(jī)器人工作空間中生成隨機(jī)位姿樣本空間,為位置級(jí)運(yùn)動(dòng)學(xué)反解算法評(píng)價(jià)提供目標(biāo)空間的全面描述。

本文第2部分論述了如何生成一個(gè)隨機(jī)三維坐標(biāo)系。首先給定坐標(biāo)系的數(shù)學(xué)描述,之后按照重要度排序,考慮向量之間的約束關(guān)系,依次對(duì)坐標(biāo)系原點(diǎn)、關(guān)鍵姿態(tài)軸、第二關(guān)鍵姿態(tài)軸以及第三姿態(tài)軸進(jìn)行數(shù)學(xué)分析。同時(shí)有推論結(jié)果與實(shí)際情況匹配,佐證論述的正確性。第3部分通過計(jì)算機(jī)編程,在攝影機(jī)器人工作空間內(nèi)隨機(jī)生成了多目標(biāo)位姿樣本。第4部分對(duì)文章進(jìn)行了總結(jié)。

2 笛卡爾坐標(biāo)系隨機(jī)生成數(shù)學(xué)分析

設(shè)N、O 和A 分別是坐標(biāo)系X 軸、Y 軸和Z軸的單位向量,P為坐標(biāo)系原點(diǎn)位置。記,

則用4×4矩陣表示空間坐標(biāo)系。記,

隨機(jī)生成笛卡爾坐標(biāo)系過程分為四步。第一步,隨機(jī)生成坐標(biāo)系原點(diǎn)位置；第二步,隨機(jī)生成關(guān)鍵姿態(tài)向量A；第三步,隨機(jī)生成姿態(tài)向量N；第四步,隨機(jī)生成姿態(tài)向量O。

2.1 隨機(jī)生成坐標(biāo)系原點(diǎn)位置

坐標(biāo)系原點(diǎn)位置的三個(gè)坐標(biāo)之間沒有約束條件,只需要控制原點(diǎn)坐標(biāo)在工作空間約束范圍內(nèi)隨機(jī)選取。

2.2 隨機(jī)生成關(guān)鍵姿態(tài)向量A

攝影機(jī)器人應(yīng)用環(huán)境中,攝像機(jī)的拍攝方向?yàn)殛P(guān)鍵方向,用Z軸的單位向量A 表示。

約束1:A 向量長度為1。有,

在滿足式 (1)時(shí),隨機(jī)確定A。由于A和A尚未確定,有,

選定A后,隨機(jī)確定A,在滿足式 (1)時(shí),有,

選定A后,在滿足式(1)時(shí),有,

其中,A正負(fù)值各有50%的選擇概率。

2.3 隨機(jī)生成姿態(tài)向量N

在確定Z軸單位向量A 之后,X 軸單位向量N必過P點(diǎn),且與Z軸垂直。所以X 軸只有一個(gè)自由度。只要確定一個(gè)參數(shù),X 軸就確定。

約束1:N 向量與A 向量相互垂直。有,

約束2:N 向量長度為1。有,

當(dāng)A≠0時(shí),在滿足式(5)條件下,有,

代入式(6),有,

整理得,

為保證N有實(shí)數(shù)解,根號(hào)內(nèi)參數(shù)有限制,

整理得,

在此范圍內(nèi)隨機(jī)選取N。

由式(8)得,

在N和N沒有確定的情況下,上述推論結(jié)果符合實(shí)際情況。同時(shí),N的取值受限于A,也符合實(shí)際直觀感受。同理其他兩個(gè)分量,N和N,也有同樣的結(jié)論。

隨機(jī)選定N后,由式 (7),N正負(fù)取值概率各為50%。這個(gè)結(jié)果符合直觀感受,單位向量A 確定后,N確定后,N 向量只有兩種對(duì)稱的可能情況。再根據(jù)式(5),可以唯一確定N。有,

當(dāng)A=0時(shí),由式(1)有,

同時(shí),由式(5)有,

當(dāng)A=0 時(shí),那么由式 (1),必有A=±1。由于在隨機(jī)生成關(guān)鍵姿態(tài)向量步驟中,A 向量已經(jīng)確定了,故無需討論此時(shí)A的取值概率。由式(5),有,

由式(6)有,

整理得,

通過式(13),在N隨機(jī)選定后,可確定N。N正負(fù)選取概率各為50%。

為保證N有實(shí)數(shù)解,根號(hào)內(nèi)參數(shù)有限制,

整理得,

在此范圍內(nèi)隨機(jī)選取N。

對(duì)于N 向量,在N和N沒有確定的情況下,N的取值范圍與上述此推導(dǎo)取值范圍一致。

當(dāng)A≠0時(shí),由式(11)有:

代入式(6),有,

聯(lián)立式(10),有,

整理得,

通過式(16),在N隨機(jī)選定后,可確定N。N正負(fù)選取概率各50%。

為保證N有實(shí)數(shù)解,根號(hào)內(nèi)參數(shù)有限制,

整理得,

在此范圍內(nèi)隨機(jī)選取N。

同樣的,對(duì)于向量N,在N和N沒有確定的情況下,N的取值范圍與上述此推導(dǎo)取值范圍一致。

2.4 隨機(jī)生成姿態(tài)向量O

Y 軸的單位向量O 可通過Z軸向量A 和X 軸向量N 獲得(右手坐標(biāo)系),

綜上,通過公式 (2)、公式 (3)、公式 (4),可以隨機(jī)生成向量A。

當(dāng)A≠0時(shí),根據(jù)式 (9)、式 (8)和式 (7),可以隨機(jī)生成向量N。

當(dāng)A=0,A=0 時(shí),根據(jù)式 (14)、式 (13)和式(12),可以隨機(jī)生成向量N。

當(dāng)A=0,A≠0 時(shí),根據(jù)式 (17)、式 (15)和式(16),可以隨機(jī)生成向量N。

根據(jù)式(18),可以隨機(jī)生成向量O。

3 笛卡爾坐標(biāo)系隨機(jī)樣本程序?qū)崿F(xiàn)

在攝影機(jī)器人項(xiàng)目中,隨機(jī)生成目標(biāo)位姿的空間為長方體。具體為,在軌道方向1到10米,寬7米,高6米的工作空間內(nèi)隨機(jī)生成滿足均勻分布的500個(gè)目標(biāo)位姿坐標(biāo)系。算法采用Matlab軟件環(huán)境實(shí)現(xiàn),為降低圖像顯示復(fù)雜度,目標(biāo)位姿顯示Z軸(實(shí)線)和X 軸(虛線),不顯示Y 軸。程序結(jié)果如圖1所示。

圖1 隨機(jī)生成目標(biāo)位姿樣本

4 結(jié)論

為采用蒙特卡洛法描述多軸機(jī)器人運(yùn)動(dòng)學(xué)反解算法性能,首先需要得到工作空間內(nèi)的目標(biāo)位姿的隨機(jī)樣本。本文對(duì)隨機(jī)生成笛卡爾坐標(biāo)系進(jìn)行了詳細(xì)的數(shù)學(xué)分析,通過編寫程序獲得了攝影機(jī)器人工作空間內(nèi)的目標(biāo)位姿隨機(jī)樣本。本文的成果可以作為基礎(chǔ)元素應(yīng)用到其他研究中,提高程序編寫效率。