[发明专利]一种基于陀螺仪辅助测量伺服电机角度的方法

权利要求书

1.一种基于陀螺仪辅助测量伺服电机角度的方法，其特征在于，所述方法包括：

S1：伺服电机开机运行，获取伺服电机k时刻的编码器角度和k时刻的陀螺仪角速度；

S2：将k时刻的陀螺仪角速度转换为k时刻的陀螺仪角度；

S3：设置伺服电机k-1时刻的实际输出角度，计算k时刻的编码器角度与k-1时刻的实际输出角度的变化率；

S4：设置变化率阈值，比较变化率与变化率阈值的大小；若变化率大于变化率阈值，则计算k时刻的编码器角度和k时刻的陀螺仪角度的差值，执行步骤S5；否则执行步骤S6；

S5：设置差值阈值，比较差值与差值阈值的大小；若差值大于差值阈值，则将k时刻的陀螺仪角度作为伺服电机k时刻的实际输出角度；否则进行步骤S6；

S6：将k时刻的编码器角度与k时刻的陀螺仪角度进行卡尔曼角度融合，将卡尔曼角度融合结果作为伺服电机k时刻的实际输出角度。

2.根据权利要求1所述的基于陀螺仪辅助测量伺服电机角度的方法，其特征在于，利用伺服电机原有的编码器获取伺服电机的编码器角度。

3.根据权利要求2所述的基于陀螺仪辅助测量伺服电机角度的方法，其特征在于，所述伺服电机原有的编码器为光电编码器或磁编码器。

4.根据权利要求1所述的基于陀螺仪辅助测量伺服电机角度的方法，其特征在于，利用陀螺仪获得伺服电机的陀螺仪角速度，具体方法为：

利用陀螺仪检测伺服电机转轴的旋转，输出检测信号转换值；

根据陀螺仪的灵敏度对应表确认当前陀螺仪的灵敏度；

根据信号转换值和灵敏度计算陀螺仪角速度：陀螺仪角速度＝信号转换值/灵敏度。

5.根据权利要求1所述的基于陀螺仪辅助测量伺服电机角度的方法，其特征在于，设置采样周期，每间隔一个采样周期，获取一次伺服电机的编码器角度和伺服电机的陀螺仪角速度；所述采样周期根据陀螺仪的采样频率确定，具体为：

式中，B表示采样周期，f表示陀螺仪的采样频率。

6.根据权利要求5所述的基于陀螺仪辅助测量伺服电机角度的方法，其特征在于，所述步骤S2中，将k时刻的陀螺仪角速度转换为k时刻的陀螺仪角度的具体方法为：

U_k＝u_k*B

式中，U_k表示k时刻的陀螺仪角度，u_k表示k时刻的陀螺仪角速度，B表示采样周期。

7.根据权利要求6所述的基于陀螺仪辅助测量伺服电机角度的方法，其特征在于，所述S3中，计算k时刻的编码器角度与k-1时刻的实际输出角度的变化率的具体方法为：

式中，α_k表示k时刻的编码器角度与k-1时刻偏伺服电机的实际输出角的变化率，z_k表示k时刻的原编码器角度，表示k-1时刻的实际输出角度。