光伏智能清扫机器人的多传感器运动航向姿态估计方法与流程

本发明属于机器人，具体涉及一种机器人多传感器运动航向姿态估计方法。

背景技术：

1、近几十年来，光伏发电技术日趋成熟，光伏发电成为人类应对能源危机的一大有效方法。由于光伏电池板长期暴露在室外，表面容易积灰，而积灰则是影响光伏发电的关键因素之一。早期光伏板表面积灰会使玻璃板透光率下降，这也会造成标准小时发电量将降低，而清洁干净的光伏电池板发电功率比积灰严重的光伏电池板高出15％以上。由此可见，积灰会严重影响光伏发电质量，导致光伏发电站经济效益受损。因此，对光伏电池板进行周期性清洗是必然的需求。

2、目前常用的清扫方式有：人工清扫、电帘除尘、纳米自洁技术、光伏机器人清扫等。其中，传统的人工清扫方式费时费力，清扫效果也难以保证；电帘除尘技术及纳米自清洁技术成本极高，不适合大范围使用。与前三者相比，光伏机器人清扫效率、自动化程度较高，可实现无人值守清扫，是目前光伏发电站必不可少的维护设备。而航向估计可为清扫机器人提供实时有效的偏航角数据，为其后续控制及工况稳定提供有效的数据保障。

技术实现思路

1、为了克服现有技术的不足，本发明提供了一种光伏智能清扫机器人的多传感器运动航向姿态估计方法，首先在光伏智能清扫机器人中加入imu传感器与gps传感器，构建多传感器运动航向姿态估计系统；imu传感器和gps传感器的数据通过五组ekf数据融合，通过高斯和滤波器解算权重，最后计算出最佳的偏航角估计值，为清扫机器人工作及后续控制提供有效的导航数据。本发明可以为清扫机器人提供实时有效的偏航角数据，为其后续控制及工况稳定提供有效的数据保障。

2、本发明解决其技术问题所采用的技术方案包括如下步骤：

3、步骤1：在光伏智能清扫机器人中加入imu传感器与gps传感器，构建多传感器运动航向姿态估计系统；所述imu传感器为机器人提供惯导数据；所述gps传感器用于获取机器人的实时工作速度；

4、步骤2：在xy水平面上预置5个初始偏航角ψi,i＝1,2,…,5，初始权值w1,w2,w3,w4,w5均取值为0.2；五个初始偏航角角度值设置为：

5、ψ3＝0，

6、令ψ表示机体坐标系相对于地球坐标系的航向角；

7、令vne表示机体在世界坐标系中的向北和向东的速度；

8、

9、其中，vn表示大地坐标系下飞机北向速度，ve表示大地坐标系下飞机动向速度；

10、令δv表示imu的z轴在机体轴上的增量角度测量值，即增量航向角的测量值；

11、令δvxy表示imu的x轴和y轴在机体轴上的增量速度测量值；

12、

13、其中，δvx表示机体坐标系下x轴速度增量，δvy表示机体坐标系下y轴速度增量；

14、步骤3：航向角更新方程为：

15、ψ′＝ψ+δψ

16、速度更新方程为：

17、

18、其中

19、

20、定义状态向量为：

21、

22、计算状态转移矩阵：

23、

24、其中，表示k时刻先验状态估计值，表示k-1时刻计算出的后验估计值；

25、传播协方差矩阵:

26、pk∣k-1＝fpk-1∣k-1ft+q

27、其中，pk∣k-1表示k时刻先验估计协方差，pk-1∣k-1表示k-1时刻更新的后验估计协方差；

28、过程噪声协方差矩阵q为：

29、q＝gdgt

30、其中，d矩阵为传感器各自数据的噪声，均服从于正态分布；g表示控制影响矩阵；

31、定义控制向量：

32、

33、推导imu噪声到状态噪声的3×3控制影响矩阵g：

34、

35、步骤4：测量更新部分；

36、由gps提供的向北、向东速度观测值为：

37、

38、其中，表示由gps测量出的无人机在大地坐标系下的北向速度，表示由gps测量出的无人机在大地坐标系下的东向速度；

39、由gps传感器提供的速度观测值，其对应的方差为由此构建2×2测量噪声矩阵：

40、

41、速度观测矩阵hk为：

42、

43、其中，h表示测量状态量的转换函数关系；

44、根据传感器的当前值和已经计算出的预测值计算残差：

45、

46、残差协方差为：

47、s＝hpk∣k-1ht+r

48、扩展卡尔曼滤波算法ekf增益计算：

49、k＝pk∣k-1hts-1

50、后验状态估计为：

51、

52、后验协方差为：

53、pk∣k＝pk∣k-1-kskt

54、步骤5：对于5个初始偏航角ψi，通过步骤3和步骤4分别得到5组扩展卡尔曼滤波算法ekf模型，针对5组ekf模型的状态分别计算高斯密度；

55、根据残差值及残差协方差计算每个模型的马氏距离：

56、

57、计算二维正态分布的密度：

58、

59、根据每个模型方差的加权平均值计算偏航状态新的权值：

60、wi′＝densityi·wi

61、将每组的权重与初始偏航角相乘，最后相加得出最终估计的偏航角值：

62、

63、

64、ψgsf＝arctan(y，x)

65、ψgsf为最终估计的偏航角值。

66、本发明的有益效果如下：

67、本发明可以为清扫机器人提供实时有效的偏航角数据，为其后续控制及工况稳定提供有效的数据保障。

技术特征：

1.一种光伏智能清扫机器人的多传感器运动航向姿态估计方法，其特征在于，包括以下步骤：

技术总结
本发明公开了一种光伏智能清扫机器人的多传感器运动航向姿态估计方法，首先在光伏智能清扫机器人中加入IMU传感器与GPS传感器，构建多传感器运动航向姿态估计系统；IMU传感器和GPS传感器的数据通过五组EKF数据融合，通过高斯和滤波器解算权重，最后计算出最佳的偏航角估计值，为清扫机器人工作及后续控制提供有效的导航数据。本发明可以为清扫机器人提供实时有效的偏航角数据，为其后续控制及工况稳定提供有效的数据保障。

技术研发人员：高杰,沈楷清
受保护的技术使用者：西安万飞控制科技有限公司
技术研发日：
技术公布日：2024/1/13