[發(fā)明專利]一種空間6自由度位姿變換數(shù)據(jù)的擬合方法及擬合系統(tǒng)

在審

• 申請(qǐng)?zhí)枺?/td>: 202210692584.8
• 申請(qǐng)日: 2022-06-17
• 公開(kāi)（公告）號(hào): CN115033842A
• 公開(kāi)（公告）日: 2022-09-09
• 發(fā)明（設(shè)計(jì)）人: 周麗華;方素平;吳帆
• 申請(qǐng)（專利權(quán)）人: 合肥工業(yè)大學(xué)
• 主分類號(hào): G06F17/16
• 分類號(hào): G06F17/16;G06F17/18
• 代理公司: 合肥市澤信專利代理事務(wù)所(普通合伙) 34144
• 代理人: 方榮肖

1.一種空間6自由度位姿變換數(shù)據(jù)的擬合方法，其用于表征目標(biāo)物體的空間位姿變換規(guī)律；其中，6自由度位姿變換數(shù)據(jù)包括在空間坐標(biāo)系中目標(biāo)物體分別平行于X軸、Y軸和Z軸的位移量以及目標(biāo)物體分別相對(duì)于X軸、Y軸和Z軸的旋轉(zhuǎn)角度；其特征在于，所述擬合方法包括如下步驟：

S1：設(shè)置采樣周期；采集n組連續(xù)的空間位姿變換數(shù)據(jù)；所述空間位姿變換數(shù)據(jù)包括平移數(shù)據(jù)和角度數(shù)據(jù)；所述平移數(shù)據(jù)用于表征在空間坐標(biāo)系中當(dāng)前時(shí)刻位置相對(duì)于前一時(shí)刻位置的位置偏移量；所述角度數(shù)據(jù)用于表征在空間坐標(biāo)系中當(dāng)前時(shí)刻角度相對(duì)于前一時(shí)刻角度的角度偏移量；

S2：采用最小二乘的球心擬合方法將所述平移數(shù)據(jù)轉(zhuǎn)化為擬合平移數(shù)據(jù)；最小二乘的球心擬合方法包括如下過(guò)程：

S21：將每個(gè)平移數(shù)據(jù)對(duì)應(yīng)表達(dá)為空間坐標(biāo)系中的離散點(diǎn)；

S22：根據(jù)所述離散點(diǎn)構(gòu)建外接球體；

S23：將所述外接球體的球心坐標(biāo)作為擬合平移數(shù)據(jù)；

S3：采用基向量擬合方法將所述角度數(shù)據(jù)轉(zhuǎn)化為擬合角度數(shù)據(jù)；基向量擬合方法包括如下過(guò)程：

S31：將每個(gè)角度數(shù)據(jù)轉(zhuǎn)化為相應(yīng)的空間旋轉(zhuǎn)矩陣；將每個(gè)空間旋轉(zhuǎn)矩陣的三個(gè)列向量作為一組基向量；

S32：預(yù)設(shè)擬合角度數(shù)據(jù)；將預(yù)設(shè)的擬合角度數(shù)據(jù)轉(zhuǎn)化為相應(yīng)的擬合空間旋轉(zhuǎn)矩陣；將所述擬合空間旋轉(zhuǎn)矩陣的三個(gè)列向量作為預(yù)設(shè)擬合基向量；

S33：根據(jù)每組基向量與所述預(yù)設(shè)擬合基向量的夾角建立目標(biāo)函數(shù)；

S34：將所述夾角的余弦值與麥克勞林公式代入目標(biāo)函數(shù)；采用多元函數(shù)求極值方法在目標(biāo)函數(shù)中求出擬合基向量；

S35：根據(jù)擬合基向量與空間旋轉(zhuǎn)矩陣求出擬合角度數(shù)據(jù)；

S4：所述擬合平移數(shù)據(jù)與所述擬合角度數(shù)據(jù)共同構(gòu)成擬合空間位姿變換數(shù)據(jù)；所述擬合空間位姿變換數(shù)據(jù)用于表征下一時(shí)刻的空間位姿相對(duì)于當(dāng)前時(shí)刻的空間位姿的變化量。

2.根據(jù)權(quán)利要求1所述的空間6自由度位姿變換數(shù)據(jù)的擬合方法，其特征在于，在S23中，根據(jù)最小二乘法，設(shè)定擬合平移數(shù)據(jù)的擬合目標(biāo)函數(shù)為：

對(duì)擬合目標(biāo)函數(shù)求偏導(dǎo)并令偏導(dǎo)數(shù)為零，則擬合平移數(shù)據(jù)為：

其中，O₀(x₀，y₀，z₀)為擬合平移數(shù)據(jù)，x₀為目標(biāo)物體平行于X軸的擬合位移偏移量，y₀為目標(biāo)物體平行于Y軸的擬合位移偏移量，z₀為目標(biāo)物體平行于Z軸的擬合位移偏移量，O_i(x_i，y_i，z_i)，i＝1，2...n為平移數(shù)據(jù)，x_i為目標(biāo)物體在第i個(gè)時(shí)間周期內(nèi)平行于X軸的位移偏移量，y_i為目標(biāo)物體在第i個(gè)時(shí)間周期內(nèi)平行于Y軸的位移偏移量，z_i為目標(biāo)物體在第i個(gè)時(shí)間周期內(nèi)平行于Z軸的位移偏移量，|O_iO₀|表示點(diǎn)O_i和點(diǎn)O₀之間的距離。