[发明专利]一种升降倒伏机构及其控制方法

权利要求书

1.一种升降倒伏机构装置，其特征在于：所述装置包括垂直升降机构体（1）、倒伏调平机构体（2）；

所述垂直升降机构体（1）的顶部为所述倒伏调平机构体（2），所述垂直升降机构体（1）采用电机驱动丝杆丝母实现垂直升降，用于支撑其上部两自由度倒伏调平机构体（2）定高升降；

所述两自由度倒伏调平机构体（2）采用蜗轮蜗杆作为一级减速装置、减速机作为二级减速装置，用于实现其上部承载平台（3）的方位、俯仰旋转。

2.根据权利要求1所述的一种升降倒伏机构装置，其特征在于：所述垂直升降机构（1）包含杆体部分、丝杆丝母模块、减速机、升降伺服电机（4）；升降伺服电机输出轴与减速机输入轴直连，减速机输出轴与丝杆丝母模块之间通过直齿轮传动，驱动丝杆丝母模块，丝杆丝母模块推动杆体实现升降功能。

3.根据权利要求1所述的一种升降倒伏机构装置，其特征在于：所述倒伏调平机构（2）包含方位蜗轮蜗杆、俯仰蜗轮蜗杆、方位减速机、俯仰减速机、方位伺服电机（6）、俯仰伺服电机（7）、倾角仪（9），方位伺服电机（6）与方位蜗轮蜗杆相连，实现一级减速，方位蜗轮蜗杆与方位减速机相连实现二级减速，驱动方位转动；俯仰伺服电机（7）与俯仰蜗轮蜗杆连接实现一级减速，俯仰蜗轮蜗杆与俯仰减速机相连实现二级减速，驱动俯仰转动；倾角仪安装于承载平台（3）下面，其与所述承载平台一同倾转；所述锁销电机（8）与俯仰零位同轴线安装。

4.根据权利要求2所述的一种升降倒伏机构装置，其特征在于：所述垂直升降机构（1）中，所述升降伺服电机（4）的转动轴上安装有高精度机械末端编码器（5）、接近开关，所述编码器（5）、接近开关分别与升降主控器相连，升降伺服电机（4）通过升降驱动器与升降主控器相连。

5.根据权利要求1所述的一种升降倒伏机构装置，其特征在于：所述倒伏调平机构（2）中，倒伏主控器通过方位驱动器、俯仰驱动器分别与方位伺服电机、俯仰伺服电机相连，锁销电机、倾角仪与主控器直接相连。

6.如权利要求1所述的升降倒伏机构装置的控制方法，其特征在于：所述控制方法包括以下步骤：

以方位旋转轴作为Z轴，俯仰旋转轴为Y轴，以正交于Y轴、Z轴且与Y轴在同一平面的为X轴，并且以XYZ建立空间坐标系；

获取承载平台（3）的倾角度数，根据两自由度倒伏调平算法计算出达到调平状态所需转动的方位角；

控制方位伺服电机（6）从当前位置旋转上述计算出的方位角度；

获取方位旋转到位后承载平台（3）的倾角度数，直接得到调平状态所需要转动的俯仰角度；

控制俯仰伺服电机（7）转动上述的俯仰角度，即可完成调平。

7.根据权利要求6所述的升降倒伏机构装置的控制方法，其特征在于：所述根据两自由度倒伏调平算法计算出达到调平状态所需转动的方位角具体指：

根据当前读取到的倾角仪（9）X轴矢量倾角θ_X、Y轴矢量倾角θ_Y，即倾角度数，以及从俯仰伺服电机获取的矢量俯仰角θ_Pit，计算从当前位置转向调平状态所需的方位旋转角ξ_Yaw =Φ（θ_X，θ_Y，θ_Pit）。

8.根据权利要求6所述的升降倒伏机构装置的控制方法，其特征在于：所述获取方位旋转到位后承载平台（3）的倾角度数，直接得到调平状态所需要转动的俯仰角度具体指：

当方位旋转过计算出的角度后，重新读取到的倾角仪（9）X轴倾角矢量数据θ_Xnew，计算俯仰旋转角度ξ_Pit，ξ_Pit=|θ_Xnew|。