本發(fā)明屬于傳感器測量相關，更具體地，涉及一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)及方法。

背景技術：

1、集成多旋轉自由度的球關節(jié)可大大降低傳動結構的復雜度，工業(yè)應用場景與日俱增。在球關節(jié)運動控制中，球關節(jié)多自由度姿態(tài)反饋的高精度和實時性要求是應用及控制球關節(jié)的基礎，然而多旋轉自由度的耦合效應增加了姿態(tài)測量的難度。

2、現有球關節(jié)多自由度姿態(tài)測量一般采用接觸式測量和使用單傳感器的非接觸式測量。球關節(jié)接觸式姿態(tài)測量通常選擇三軸加速度計、三軸陀螺儀和三軸地磁計的組合來解算姿態(tài)，但該方案僅能輸出球關節(jié)運動端相對于慣性系的角度而無法輸出運動端相對固定端的角度，并且與運動端接觸會引入外部擾動對控制效果造成影響，同時加速度計易受到高頻干擾、陀螺儀有積分漂移誤差、地磁計無法應用于強磁環(huán)境，這些原因導致該方案使用場景有較大的局限性；對于非接觸式測量，研究主要使用單類傳感器，可以輸出運動端相對固定端的角度且不會有外部擾動的干擾，但單類傳感器測量存在積分漂移誤差以致難以長時間測量，無零點位置只能從姿態(tài)處于零點開始計算，或者存在較大噪聲使得測量精度不高。

技術實現思路

1、針對現有技術的以上缺陷或改進需求，本發(fā)明提供了一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)及方法，解決接觸式測量引入外部擾動、無法輸出運動端相對角度與非接觸式測量噪聲大、無零點位置、有漂移誤差的問題。

2、為實現上述目的，按照本發(fā)明的一個方面，提供了一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)，其特征在于，該系統(tǒng)包括磁場測量模塊、光場測量模塊和計算模塊，其中：

3、所述磁場測量模塊通過測量球關節(jié)運動端相對于固定端運動時磁場的變化；

4、所述光場測量模塊通過測量球關節(jié)運動端相對于固定端運動時光信號的變化；

5、所述計算模塊用于根據磁場測量模塊測量的磁場變化和光場測量模塊測量的光信號的變化分別計算當前時刻球關節(jié)運動端相對于固定端運動的角度和角速度，然后將該當前時刻的角度和角速度融合獲得下一時刻球關節(jié)運動端相對于固定端運動旋轉的角度。

6、進一步優(yōu)選地，所述磁場測量模塊包括永磁體和磁傳感器，所述永磁體設置在球關節(jié)運動端上，所述磁傳感器設置在球關節(jié)固定端上。

7、進一步優(yōu)選地，所述光場測量模塊包括檢測面、光傳感器和透鏡，其中，所述光傳感器和透鏡設置在球關節(jié)固定端，所述檢測面設置在球關節(jié)運動端，所述透鏡設置在光傳感器和檢測面之間，所述光傳感器發(fā)出光經過所述透鏡照射在所述檢測面上被反射或散射，反射或散射的光線被透鏡匯聚進入光傳感器，以此實現光信號的測量。

8、進一步優(yōu)選地，永磁體的數量大于等于1，磁傳感器的數量大于等于3。

9、進一步優(yōu)選地，所述檢測面、光傳感器和透鏡的數量相同，光傳感器的數量大于等于2。

10、進一步優(yōu)選地，所述檢測面和透鏡底部之間的直線距離為1.6～2.4mm。

11、按照本發(fā)明的另一個方面，提供了一種球關節(jié)多自由度姿態(tài)測量的方法，該方法包括下列步驟：

12、s1球關節(jié)運動端相對于固定端旋轉運動，利用上述所述的測量系統(tǒng)中磁場的變化和光信號的變化；

13、s2利用測量的磁場變化計算當前時刻球關節(jié)運動端相對于固定端的角度；

14、s3利用光場的變化計算當前時刻球關節(jié)運動端相對于固定端的角速度；

15、s4將步驟s2和s3計算獲得的當前時刻的角速度采用卡爾曼濾波器融合獲得下一個時刻球關節(jié)運動端相對于固定端旋轉的角度。

16、進一步優(yōu)選地，所述角度按照下列步驟獲得：

17、構建球關節(jié)運動端相對于固定端旋轉的角度和磁傳感器測量的磁場對應的向量一一對應的數據集，其中，所述角度通過imu測量獲得；

18、利用所述數據集訓練深度學習模型，該深度學習模型的輸入是磁傳感器測量的磁場，輸出是球關節(jié)運動端相對于固定端旋轉的角度；

19、利用磁傳感器測量球關節(jié)運動端相對于固定端旋轉時的磁場獲得磁場對應的向量，將該向量輸入訓練后的深度學習模型中，輸出球關節(jié)運動端相對于固定端旋轉實時的角度。

20、進一步優(yōu)選地，所述角速度按照下列公式計算：

21、

22、其中，q′opt表示光場測量模塊計算的球關節(jié)運動端相對于固定端的角速度，qkf是球關節(jié)運動端相對于固定端上一個時刻的角度，v表示所有光傳感器測量的速度值組成的列向量，r表示傳感器球坐標參數中的半徑，γl表示光傳感器數量為l時的運動學方程組，是γl的偽逆，l表示光傳感器的數量。

23、進一步優(yōu)選地，所述下一個時刻球關節(jié)運動端相對于固定端旋轉的角度按照下列公式計算：

24、

25、其中，qkf是球關節(jié)運動端相對于固定端上一個時刻的角度，x表示狀態(tài)變量，x1、x2、x3分別表示狀態(tài)變量x的第1、2、3個分量，k表示當前時刻，k+1表示下一時刻。

26、總體而言，通過本發(fā)明所構思的以上技術方案與現有技術相比，具備下列有益效果：

27、1.本發(fā)明通過采用光場測量模塊和磁場測量模塊分別從光信號和磁場信號兩個角度獲得當前時刻球關節(jié)運動端相對于固定端的角度和角速度，然后將二者融合計算獲得下一個時刻運動端相對于固定端的角度，其中，磁傳感用于角度測量可有效避免零漂和漂移誤差，光傳感憑借其高分辨率與追蹤速度得到高測量精度與低噪聲；優(yōu)化兩類傳感器布置并通過融合算法結合可以實現優(yōu)勢互補并提高系統(tǒng)的精度與魯棒性。

28、2.本發(fā)明提供的測量系統(tǒng)可直接同時輸出球關節(jié)多自由度運動系統(tǒng)的運動端相對固定端的角度且具有非接觸性，相較于編碼器與imu等傳統(tǒng)測量模組，解決了需要接觸式測量引入外部擾動(針對編碼器與imu)、測量多軸旋轉需要分散布置編碼器造成集成度低(針對編碼器)、僅能輸出運動系相對于慣性系的角度(針對imu)、在強磁環(huán)境下無法使用地磁計校準偏航角(針對imu)等問題，有較強的泛用性。

29、3.本發(fā)明采用卡爾曼濾波器將當前時刻球關節(jié)運動端相對于固定端的角度和角速度融合算法，該融合算法將非接觸式測量方法和接觸式測量方法的優(yōu)勢互補，磁測量提供角度，可以消除無零點位置和漂移誤差問題，光測量因光傳感器有非常高的光學分辨率可以獲得高精度的角速度，彌補磁傳感器測量噪聲大的缺陷。合適的融合算法可以實現有零點位置、無漂移誤差、低噪聲的測量效果；卡爾曼濾波器是一種針對數據融合的統(tǒng)計遞歸數據處理算法，其目標是在最小化誤差協方差的估計過程中最小化噪聲誤差，可以根據實時從噪聲來實時調整比例系數來計算結果最優(yōu)估計值，能提供更為平滑和準確的跟蹤信號，此外任何與姿態(tài)有映射關系的測量方式均可作為姿態(tài)解算卡爾曼濾波的數據來源，應用范圍比mahony濾波和madgwick濾波更廣。

技術特征：

1.一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)，其特征在于，該系統(tǒng)包括磁場測量模塊、光場測量模塊和計算模塊，其中：

2.如權利要求1所述的一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)，其特征在于，所述磁場測量模塊包括永磁體(3)和磁傳感器(7)，所述永磁體(3)設置在球關節(jié)運動端上，所述磁傳感器(7)設置在球關節(jié)固定端上。

3.如權利要求1所述的一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)，其特征在于，所述光場測量模塊包括檢測面(5)、光傳感器(6)和透鏡(9)，其中，所述光傳感器(6)和透鏡(9)設置在球關節(jié)固定端，所述檢測面(5)設置在球關節(jié)運動端，所述透鏡(9)設置在光傳感器(6)和檢測面(5)之間，所述光傳感器(6)發(fā)出光經過所述透鏡(9)照射在所述檢測面(5)上被反射或散射，反射或散射的光線被透鏡(9)匯聚進入光傳感器(6)，以此實現光信號的測量。

4.如權利要求1所述的一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)，其特征在于，永磁體(3)的數量大于等于1，磁傳感器(7)的數量大于等于3。

5.如權利要求1所述的一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)，其特征在于，所述檢測面(5)、光傳感器(6)和透鏡(9)的數量相同，光傳感器(6)的數量大于等于2。

6.如權利要求1所述的一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)，其特征在于，所述檢測面(5)和透鏡(9)底部之間的直線距離為1.6～2.4mm。

7.一種球關節(jié)多自由度姿態(tài)測量的方法，其特征在于，該方法包括下列步驟：

8.如權利要求7所述的方法，其特征在于，所述角度按照下列步驟獲得：

9.如權利要求7所述的方法，其特征在于，所述角速度按照下列公式計算：

10.如權利要求7所述的方法，其特征在于，所述下一個時刻球關節(jié)運動端相對于固定端旋轉的角度按照下列公式計算：

技術總結
本發(fā)明屬于傳感器測量相關技術領域，并公開了一種球關節(jié)多自由度姿態(tài)測量系統(tǒng)及方法。該系統(tǒng)包括磁場測量模塊、光場測量模塊和計算模塊，其中：所述磁場測量模塊通過測量球關節(jié)運動端相對于固定端運動時磁場的變化；所述光場測量模塊通過測量球關節(jié)運動端相對于固定端運動時光信號的變化；所述計算模塊用于根據磁場測量模塊測量的磁場變化和光場測量模塊測量的光信號的變化分別計算當前時刻球關節(jié)運動端相對于固定端運動的角度和角速度，然后將該當前時刻的角度和角速度融合獲得下一時刻球關節(jié)運動端相對于固定端運動旋轉的角度。通過本發(fā)明，實現優(yōu)勢互補并提高測量系統(tǒng)的精度與魯棒性。

技術研發(fā)人員：白坤,吳煊
受保護的技術使用者：華中科技大學
技術研發(fā)日：
技術公布日：2025/5/8