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

在審

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

本發(fā)明涉及一種空間6自由度位姿變換數(shù)據(jù)的擬合方法。擬合方法包括如下步驟：S1：設(shè)置采樣周期。采集n組連續(xù)的空間位姿變換數(shù)據(jù)。S2：采用最小二乘的球心擬合方法將平移數(shù)據(jù)轉(zhuǎn)化為擬合平移數(shù)據(jù)。S3：采用基向量擬合方法將角度數(shù)據(jù)轉(zhuǎn)化為擬合角度數(shù)據(jù)。S4：擬合平移數(shù)據(jù)與擬合角度數(shù)據(jù)共同構(gòu)成擬合空間位姿變換數(shù)據(jù)。本發(fā)明采用最小二乘的球心擬合方法對平移數(shù)據(jù)擬合，采用基向量擬合方法對角度數(shù)據(jù)擬合，解決了傳統(tǒng)空間6自由度位姿變換數(shù)據(jù)求解穩(wěn)定性和精度不足的問題，尤其提高了角度數(shù)據(jù)的擬合精度，彌補(bǔ)了現(xiàn)有技術(shù)的空白。

技術(shù)領(lǐng)域

本發(fā)明涉及位姿控制技術(shù)領(lǐng)域，特別是涉及一種空間6自由度位姿變換數(shù)據(jù)的擬合方法、一種空間6自由度位姿變換數(shù)據(jù)的擬合系統(tǒng)。

背景技術(shù)

隨著現(xiàn)代導(dǎo)航精度的提高，對空間姿態(tài)變換的要求也越來越高。空間姿態(tài)變換一般是由空間坐標(biāo)變換矩陣表示，而空間坐標(biāo)變換矩陣通常是由空間6自由度位姿變換數(shù)據(jù)表示的?？臻g6自由度包括在空間坐標(biāo)系中目標(biāo)物體分別平行于X軸、Y軸、Z軸的平移以及目標(biāo)物體分別相對于X軸、Y軸、Z軸的旋轉(zhuǎn)。如何精確求解空間6自由度參數(shù)，成為精確控制姿態(tài)變換的關(guān)鍵。然而現(xiàn)有的空間6自由度位姿變換數(shù)據(jù)計算方法適用性不強(qiáng)、精度不高。

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

基于此，有必要針對現(xiàn)有空間6自由度位姿變換數(shù)據(jù)計算方法適用性不強(qiáng)、精度不高的問題，提供一種空間6自由度位姿變換數(shù)據(jù)的擬合方法。擬合方法包括如下步驟：

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

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

S21：將每個平移數(shù)據(jù)對應(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ù)?；蛄繑M合方法包括如下過程：

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

S32：預(yù)設(shè)擬合角度數(shù)據(jù)。將預(yù)設(shè)的擬合角度數(shù)據(jù)轉(zhuǎn)化為相應(yīng)的擬合空間旋轉(zhuǎn)矩陣。將擬合空間旋轉(zhuǎn)矩陣的三個列向量作為預(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ù)用于表征下一時刻的空間位姿相對于當(dāng)前時刻的空間位姿的變化量。

上述空間6自由度位姿變換數(shù)據(jù)的擬合方法，采用最小二乘的球心擬合方法對平移數(shù)據(jù)擬合，采用基向量擬合方法對角度數(shù)據(jù)擬合，解決了傳統(tǒng)空間6自由度位姿變換數(shù)據(jù)求解穩(wěn)定性和精度不足的問題，尤其提高了角度數(shù)據(jù)的擬合精度，彌補(bǔ)了現(xiàn)有技術(shù)的空白。

在其中一個實(shí)施例中，根據(jù)最小二乘法，設(shè)定擬合平移數(shù)據(jù)的擬合目標(biāo)函數(shù)為：

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