[发明专利]一种云台漂移补偿方法

权利要求书

1.一种云台漂移补偿方法，其特征在于，所述方法包括：

利用陀螺仪得到惯性角速度作为稳定回路的反馈信号；

在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的测角元件得到所述电机旋转的角位置初值和变化量；

获取预先配置的陀螺漂移时间阈值和陀螺漂移角度阈值中最先到达的阈值并根据所述最先到达的阈值计算陀螺漂移速度和陀螺漂移方向；其中，根据|α-α₀+ζ-ζ₀|＞TH_angle ortime＞TH_time计算陀螺漂移速度V_fwbias如下：

其中，TH_time为陀螺漂移时间阈值，TH_angle为陀螺漂移角度阈值，云台惯性稳定角漂移载荷的视线角在[-TH_angle,TH_angle]之内，α表示通过惯性测量单元获得的惯性角位置，α₀表示通过惯性测量单元获得的惯性角位置初值，ζ表示通过测角元件获取转动的绝对角位置，ζ₀表示通过测角元件获取转动的绝对角位置初值；

根据陀螺仪的初始信号和所述陀螺漂移速度确定陀螺漂移速度修正参数；

根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿。

2.根据权利要求1所述的云台漂移补偿方法，其特征在于，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿之后，所述方法还包括：

当完成陀螺漂移补偿后，重复执行上述步骤。

3.根据权利要求1所述的云台漂移补偿方法，其特征在于，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿之后，所述方法还包括：

判断补偿后的陀螺漂移速度是否小于误差允许阈值，若小于则结束漂移补偿。

4.根据权利要求1所述的云台漂移补偿方法，其特征在于，所述陀螺漂移时间阈值为5秒至10秒。

5.根据权利要求2所述的云台漂移补偿方法，其特征在于，

根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿，其中，陀螺漂移速度修正参数为：

其中，

V_cmd表示陀螺漂移速度修正参数，V_opt表示陀螺漂移补偿速度，V_fwbias表示陀螺漂移速度。

6.根据权利要求5所述的云台漂移补偿方法，其特征在于，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿，包括：

根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台采用匀加速或匀减速沿所述方向的反向进行漂移补偿，其中，陀螺漂移补偿速度为

其中，ACC为固定加速度，0.5·ACC·T_opt²＝|α-α₀+ζ-ζ₀|，t表示修正漂移过程的时间，t∈[0,T_opt]，T_opt表示修正漂移的最大时间。

7.根据权利要求6所述的云台漂移补偿方法，其特征在于，当完成陀螺漂移补偿后，重复执行上述步骤，包括：

通过判断|α-α₀+ζ-ζ₀|≤TH_sta是否成立，若不成立则继续进行陀螺漂移补偿；其中，TH_sta表示陀螺漂移误差允许的角度阈值。