本發(fā)明涉及伺服控制，具體涉及一種探測用伺服機構(gòu)。

背景技術(shù)：

1、無人超高速艦艇，是一種無人操作的艦艇，主要有無人水面艦艇和無人潛航器兩種，主要用于執(zhí)行危險以及不適于有人船只執(zhí)行的任務(wù)。

2、上世紀(jì)90年代以后，人工智能和自動化技術(shù)的進步，讓水上水下的無人艦艇迎來了真正的活躍期，更復(fù)雜的水下機器人和水面機器人也從概念走向現(xiàn)實。西方國家的無人艦艇大多擔(dān)任偵查任務(wù)，具備攻擊能力的改型也已開始列裝。

3、無人超高速艦艇，行駛速度快，機動性好，要求列裝在無人超高速艦艇上的光學(xué)艙具有重量輕、視場大、穩(wěn)定性好等優(yōu)點。

4、目前，列裝在艦船上的光電探測光學(xué)艙,多數(shù)為兩軸穩(wěn)定平臺，在多數(shù)情況下，由于實際場景的需要，需要實現(xiàn)三軸穩(wěn)定平臺的功能，但對于已經(jīng)列裝艦船的光學(xué)艙，重新設(shè)計三軸穩(wěn)定平臺耗資巨大，也難以保證原有光學(xué)艙的性能指標(biāo)。

技術(shù)實現(xiàn)思路

1、有鑒于此，本發(fā)明提供了一種探測用伺服機構(gòu)，能夠解決光學(xué)艙的兩軸轉(zhuǎn)臺實現(xiàn)三軸穩(wěn)定的技術(shù)問題。

2、為了解決上述技術(shù)問題，本發(fā)明是這樣實現(xiàn)的。

3、一種無人艦艇光學(xué)艙的兩軸轉(zhuǎn)臺實現(xiàn)三軸穩(wěn)定的探測用伺服機構(gòu)，包括：

4、采集模塊：配置為在所述兩軸轉(zhuǎn)臺的滾動位置配置一雙軸陀螺，用于采集光學(xué)艙在滾動和方位向上的速率數(shù)據(jù)，俯仰位置配置一單軸陀螺，用于采集光學(xué)艙俯仰軸向上的速度數(shù)據(jù)；

5、積分模塊：配置為基于無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的俯仰角、滾動角、方位角，以及從滾動碼盤、俯仰碼盤得到的光學(xué)艙俯仰、滾動、方位框架角信息，解算兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)和俯仰坐標(biāo)；其中，俯仰框架角信息是由單軸陀螺積分得到的，俯仰、滾動框架角信息是由雙軸陀螺在對應(yīng)的方向上積分得到的；

6、解算模塊：配置為解算所述光學(xué)艙實際的方位、俯仰、滾動的三軸框架角信息；

7、補償模塊：配置為對所述三軸框架角信息分別進行微分，將方位框架角信息微分后的數(shù)值作為方位環(huán)速度值進行閉環(huán)，將俯仰框架角信息微分后的數(shù)值和滾動框架角信息微分后的數(shù)值作為補償速度，作為俯仰和滾動穩(wěn)定回路陀螺數(shù)據(jù)的漂移補償。

8、一種無人艦艇光學(xué)艙的兩軸轉(zhuǎn)臺實現(xiàn)三軸穩(wěn)定的方法，所述方法包括：

9、步驟s1：在所述兩軸轉(zhuǎn)臺的滾動位置配置一雙軸陀螺，用于采集光學(xué)艙在滾動和方位向上的速率數(shù)據(jù)，俯仰位置配置一單軸陀螺，用于采集光學(xué)艙俯仰軸向上的速度數(shù)據(jù)；

10、步驟s2：基于無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的俯仰角、滾動角、方位角，以及從滾動碼盤、俯仰碼盤得到的光學(xué)艙俯仰、滾動、方位框架角信息，解算兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)和俯仰坐標(biāo)；其中，俯仰框架角信息是由單軸陀螺積分得到的，俯仰、滾動框架角信息是由雙軸陀螺在對應(yīng)的方向上積分得到的；

11、步驟s3：解算所述光學(xué)艙實際的方位、俯仰、滾動的三軸框架角信息；

12、步驟s4：對所述三軸框架角信息分別進行微分，將方位框架角信息微分后的數(shù)值作為方位環(huán)速度值進行閉環(huán)，將俯仰框架角信息微分后的數(shù)值和滾動框架角信息微分后的數(shù)值作為補償速度，作為俯仰和滾動穩(wěn)定回路陀螺數(shù)據(jù)的漂移補償。

13、優(yōu)選地，所述雙軸陀螺為滾動陀螺，所述單軸陀螺為俯仰陀螺。

14、優(yōu)選地，所述步驟s1，其中，采集的數(shù)據(jù)分別通過滾動碼盤、俯仰碼盤以角度形式輸出。

15、優(yōu)選地，所述步驟s2：基于無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的俯仰角、滾動角、方位角，以及從滾動碼盤、俯仰碼盤得到的光學(xué)艙俯仰、滾動、方位框架角信息，解算兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)和俯仰坐標(biāo)；其中，俯仰框架角信息是由單軸陀螺積分得到的，俯仰、滾動框架角信息是由雙軸陀螺在對應(yīng)的方向上積分得到的，包括：

16、步驟s21：基于無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的俯仰角、滾動角、方位角，進行變量初始化

17、x?sin＝sin(θx)；

18、x?cos＝cos(θx)；

19、y?sin＝sin(θy)；

20、y?cos＝cos(θy)；

21、z?sin＝sin(θz)

22、z?cos＝cos(θz)

23、θx為無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的俯仰角；θy為無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的滾動角；θz為無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的方位角；

24、x0sin＝sin(φx)；

25、x0cos＝cos(φx)；

26、y0sin＝sin(φy)；

27、y0cos＝cos(φy)；

28、z0sin＝sin(φz)；

29、z0cos＝cos(φz)；

30、φx為光學(xué)艙俯仰框架角，φy為光學(xué)艙滾動框架角，φz為光學(xué)艙航向框架角；步驟s22：計算中間變量

31、t11＝z?cos*y?cos+x?sin*y?sin*z?sin；

32、t12＝z?sin*x?cos；

33、t13＝z?cos*y?sin-x?sin*y?cos*z?sin；

34、t21＝-z?sin*y?cos+x?sin*y?sin*z?cos；

35、t22＝z?cos*x?cos；

36、t23＝-z?sin*y?sin-x?sin*y?cos*z?cos；

37、t31＝-y?sin*x?cos；

38、t32＝x?sin；

39、t33＝x?cos*y?cos；

40、t111＝t11*(y0?cos*z0?sin+y0?sin*x0?sin*z0?cos)+t12*(x0?cos*z0?cos)

41、+t13*(-y0sin*z0?sin+y0?cos*x0?sin*z0?cos)；

42、t211＝t21*(y0?cos*z0?sin+y0?sin*x0?sin*z0?cos)+t22*(x0?cos*z0?cos)

43、+t23*(-y0sin*z0?sin+y0?cos*x0?sin*z0?cos)；

44、t311＝t31*(y0?cos*z0?sin+y0?sin*x0?sin*z0?cos)+t32*(x0?cos*z0?cos)

45、+t33*(-y0sin*z0?sin+y0?cos*x0?sin*z0?cos)；

46、步驟s23：設(shè)置邊界點

47、當(dāng)t211為零，t111大于零時，兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)為ad＝3.1415926/2.0；

48、當(dāng)t211為零，t111小于零時，兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)為ad＝-3.1415926/2.0；

49、步驟s24：對t111和t211進行符號判斷，解算得到兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)和俯仰坐標(biāo)

50、當(dāng)t211大于零，t111大于或等于零時，兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)為ad＝arctan(t111*1/t211)；

51、當(dāng)t211小于零，t111大于或等于零時，兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)為ad＝arctan(t111*1/t211)+π；

52、當(dāng)t211小于零，t111小于零時，兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)為ad＝arctan(t111*1/t211)-π；

53、當(dāng)t211大于零，t111小于或等于零時，兩軸轉(zhuǎn)臺對大地坐標(biāo)系的方位坐標(biāo)為ad＝arctan(t111*1/t211)；步驟s25：解算得到兩軸轉(zhuǎn)臺對大地坐標(biāo)系的俯仰坐標(biāo)ed

54、ed＝asin(t311)。

55、步驟s3：解算所述光學(xué)艙實際的方位、俯仰、滾動的三軸框架角信息。

56、步驟s31：進行變量初始化

57、x?sin＝sin(θx)；

58、x?cos＝cos(θx)；

59、y?sin＝sin(θy)；

60、y?cos＝cos(θy)；

61、z?s?in＝sin(θz)

62、z?cos＝cos(θz)

63、θx為無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的俯仰角；θy為無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的滾動角；θz為無人艦艇慣導(dǎo)設(shè)備輸出的無人艦艇當(dāng)前的方位角；

64、adsin＝sin(αd)

65、adcos＝cos(αd)

66、edsin＝sin(εd)

67、edcos＝cos(εd)

68、rsin＝sin(γ)

69、rcos＝cos(γ)

70、αd為無人艦艇航向視軸對大地地理坐標(biāo)信息；εd為無人艦艇俯仰視軸對大地地理坐標(biāo)信息；γ為光學(xué)艙滾動碼盤輸出的滾動角度；

71、計算中間變量：

72、t'11＝z?cos*y?cos+x?sin*y?sin*z?sin；

73、t'12＝-z?sin*y?cos+x?sin*y?sin*z?cos；

74、t'13＝-y?sin*x?cos；

75、t'21＝x?cos*z?sin；

76、t'22＝z?cos*x?cos；

77、t'23＝x?sin；

78、t'31＝y(tǒng)?sin*z?cos-x?sin*y?cos*z?sin；

79、t'32＝-y?sin*z?sin-x?sin*y?cos*z?cos；

80、t'33＝x?cos*y?cos；

81、t'111＝t11'*ed?cos*ad?sin+t12'ed?cos*ad?cos+t13'*ed?sin；

82、t'211＝t'21*edcos*adsin+t'22edcos*adcos+t'23*edsin；

83、t'311＝t'31*edcos*adsin+t'32edcos*adcos+t'33*edsin

84、步驟s32：光學(xué)艙實際的滾動框架角信息為零，即

85、γ＝0；

86、步驟s33：解算得出光學(xué)艙實際的方位框架角信息以及俯仰框架角信息

87、a_end＝arcsin(t′111)，為反正弦；

88、e_end＝arctan(t′311/t′211)，為反正切；

89、r_end＝γ；

90、a_end為光學(xué)艙實際的方位框架角信息，e_end為光學(xué)艙實際的俯仰框架角信息，r_end為光學(xué)艙實際的滾動框架角信息。

91、優(yōu)選地，所述步驟s4，包括：使用pid雙環(huán)控制算法，設(shè)置兩個環(huán)，分別是方位環(huán)、俯仰和滾動穩(wěn)定回路環(huán)；在俯仰和滾動穩(wěn)定回路環(huán)的內(nèi)環(huán)分別將得到的滾動框架角信息微分后的數(shù)值和俯仰框架角信息微分后的數(shù)值配置滾動陀螺輸出、俯仰陀螺輸出，用于光軸漂移的校正。

92、一種計算機可讀存儲介質(zhì)，所述存儲介質(zhì)中存儲有多條指令；所述多條指令，用于由處理器加載并執(zhí)行如前所述方法。

93、一種電子設(shè)備，其特征在于，所述電子設(shè)備，包括：

94、處理器，用于執(zhí)行多條指令；

95、存儲器，用于存儲多條指令；

96、其中，所述多條指令，用于由所述存儲器存儲，并由所述處理器加載并執(zhí)行如前所述方法。

97、本發(fā)明所帶來的有益技術(shù)效果：

98、(1)本發(fā)明在現(xiàn)有硬件平臺，即艦艇光學(xué)艙的兩軸轉(zhuǎn)臺上進行技術(shù)改進，應(yīng)用兩軸穩(wěn)定平臺實現(xiàn)三軸穩(wěn)定的功能，同時保持光學(xué)艙性能指標(biāo)不變。能夠節(jié)省硬件成本、減少重新設(shè)計時間。

99、(2)本發(fā)明通過光學(xué)艙的速度回路和定位回路，解決了配置于無人艦艇上光學(xué)艙光軸漂移的問題。

100、(3)本發(fā)明在解決無人艦艇上的光學(xué)艙的光學(xué)艙的光軸漂移問題的基礎(chǔ)上，實現(xiàn)了解決光學(xué)艙漂移問題，滿足產(chǎn)品的性能指標(biāo)要求。

101、(4)本發(fā)明控制方式簡單，易于實現(xiàn)。