一种货箱举升角度检测方法及无人驾驶矿车与流程

本发明属于适用于货运物体的车辆，尤其涉及一种货箱举升角度检测方法及无人驾驶矿车。

背景技术：

1、目前，无人驾驶矿车一般使用两个角度传感器，一个角度传感器安装在车架上检测当前车辆停放角度，另一个角度传感器安装在货箱上检测货箱角度，通过这两个角度传感器计算货箱角度提供给上位机控制货箱举升。

2、然而，当两个角度传感器中任意一个发生损坏时，无人驾驶矿车的举升功能将会失效，此时只能更换备件后才能恢复使用，停车维修时间较长，导致生产线的停滞和生产能力的降低。

技术实现思路

1、鉴于以上现有技术的不足，发明的目的在于提供一种货箱举升角度检测方法及无人驾驶矿车，角度融合控制器可以检测车架角度传感器和货箱角度传感器是否异常，当其中一个存在异常时，可以由双传感器检测转变为单传感器检测，不影响货箱举升功能的使用，避免长时间停车维修，保持生产线的正常运行，保障生产线的生产能力。

2、本发明的第一方面，提出了一种货箱举升角度检测方法，应用于无人驾驶矿车，所述无人驾驶矿车包括：车架、货箱、车架角度传感器、货箱角度传感器和角度融合控制器，所述车架角度传感器设置在所述车架上，所述货箱角度传感器设置在所述货箱上，所述车架角度传感器和所述货箱角度传感器均与所述角度融合控制器通信连接，所述货箱举升角度检测方法包括：

3、s1，所述角度融合控制器接收来自所述车架角度传感器和所述货箱角度传感器检测的角度值；

4、s2，所述角度融合控制器检测所述车架角度传感器和所述货箱角度传感器是否异常；

5、s3，当所述车架角度传感器和所述货箱角度传感器均正常时，所述角度融合控制器根据所述车架角度传感器和所述货箱角度传感器检测的角度值计算当前货箱举升角度；

6、当所述车架角度传感器异常时，所述角度融合控制器根据所述货箱角度传感器检测的角度值计算当前货箱举升角度；

7、当所述货箱角度传感器异常时，发出所述货箱角度传感器异常的提示，以使维修人员将所述车架角度传感器与所述货箱角度传感器互换安装位置，所述角度融合控制器根据所述车架角度传感器检测的角度值计算当前货箱举升角度。

8、进一步地，所述s3中的所述角度融合控制器根据所述车架角度传感器和所述货箱角度传感器检测的角度值计算当前货箱举升角度，具体为：

9、所述角度融合控制器根据以下方程组，计算当前货箱举升角度：

10、货箱角度传感器测量方程：m1(t)＝m1(t)+v1(t)+n1(t)

11、车架角度传感器测量方程：m2(t)＝m2(t)+v2(t)+n2(t)

12、货箱角度传感器测量误差方程：v1(t)＝c1·q1

13、车架角度传感器测量误差方程：v2(t)＝c2·q2

14、其中，m1(t)表示货箱角度传感器反馈的测量值，m1(t)表示货箱角度传感器的真实测量值，v1(t)表示货箱角度传感器的测量误差，n1(t)表示货箱角度传感器的测量噪声，c1表示货箱角度传感器的误差系数，q1表示货箱角度传感器的固有误差，m2(t)表示车架角度传感器反馈的测量值，m2(t)表示车架角度传感器的真实测量值，v2(t)表示车架角度传感器的测量误差，n2(t)表示车架角度传感器的测量噪声，c2表示车架角度传感器的误差系数，q2表示车架角度传感器的固有误差。

15、进一步地，所述s3中的所述角度融合控制器根据所述货箱角度传感器检测的角度值计算当前货箱举升角度，具体为：

16、所述角度融合控制器根据以下方程组，计算当前货箱举升角度：

17、m＝m2-m1

18、m(t+1)＝m(t)+δm(t)+v1(t)+n1(t)

19、δm(t)＝m(t)-m(t-1)

20、其中，m表示货箱举升角度，m1表示举升前货箱角度传感器检测的角度值，m2表示举升后货箱角度传感器检测的角度值，m(t+1)表示t+1时刻预测的举升角度，m(t)表示t时刻货箱角度传感器实际反馈的角度，m(t-1)表示t-1时刻货箱角度传感器实际反馈的角度。

21、进一步地，所述s3中的所述角度融合控制器根据所述货箱角度传感器检测的角度值计算当前货箱举升角度，具体为：

22、所述角度融合控制器根据以下方程组，计算当前货箱举升角度：

23、m＝m2-m1

24、m(t+1)＝m(t)+δm(t)+v1(t)+n1(t)

25、δm(t)＝ω·δt

26、其中，ω表示货箱角度传感器检测的角速度，δt表示单位时间差。

27、进一步地，所述s2具体包括：

28、s201，所述角度融合控制器根据以下方程组，计算货箱角度传感器或者车架角度传感器的预测值：

29、m1(t)＝m1(t)+v1(t)+n1(t)

30、m2(t)＝m2(t)+v2(t)+n2(t)

31、m(t+1)＝m(t)+δm(t)+v1(t)+n1(t)

32、δm(t)＝m(t)-m(t-1)

33、s202，当货箱角度传感器或者车架角度传感器的预测值大于阈值时，确定货箱角度传感器或者车架角度传感器异常。

34、本发明的第二方面，提出了一种无人驾驶矿车，包括：车架、货箱、车架角度传感器、货箱角度传感器和角度融合控制器，所述车架角度传感器设置在所述车架上，所述货箱角度传感器设置在所述货箱上，所述车架角度传感器和所述货箱角度传感器均与所述角度融合控制器通信连接，所述无人驾驶矿车还包括：

35、接收模块，用于所述角度融合控制器接收来自所述车架角度传感器和所述货箱角度传感器检测的角度值；

36、检测模块，用于所述角度融合控制器检测所述车架角度传感器和所述货箱角度传感器是否异常；

37、计算模块，用于当所述车架角度传感器和所述货箱角度传感器均正常时，所述角度融合控制器根据所述车架角度传感器和所述货箱角度传感器检测的角度值计算当前货箱举升角度；

38、当所述车架角度传感器异常时，所述角度融合控制器根据所述货箱角度传感器检测的角度值计算当前货箱举升角度；

39、当所述货箱角度传感器异常时，发出所述货箱角度传感器异常的提示，以使维修人员将所述车架角度传感器与所述货箱角度传感器互换安装位置，所述角度融合控制器根据所述车架角度传感器检测的角度值计算当前货箱举升角度。

40、进一步地，所述计算模块具体用于：

41、所述角度融合控制器根据以下方程组，计算当前货箱举升角度：

42、货箱角度传感器测量方程：m1(t)＝m1(t)+v1(t)+n1(t)

43、车架角度传感器测量方程：m2(t)＝m2(t)+v2(t)+n2(t)

44、货箱角度传感器测量误差方程：v1(t)＝c1·q1

45、车架角度传感器测量误差方程：v2(t)＝c2·q2

46、其中，m1(t)表示货箱角度传感器反馈的测量值，m1(t)表示货箱角度传感器的真实测量值，v1(t)表示货箱角度传感器的测量误差，n1(t)表示货箱角度传感器的测量噪声，c1表示货箱角度传感器的误差系数，q1表示货箱角度传感器的固有误差，m2(t)表示车架角度传感器反馈的测量值，m2(t)表示车架角度传感器的真实测量值，v2(t)表示车架角度传感器的测量误差，n2(t)表示车架角度传感器的测量噪声，c2表示车架角度传感器的误差系数，q2表示车架角度传感器的固有误差。

47、进一步地，所述计算模块具体用于：

48、所述角度融合控制器根据以下方程组，计算当前货箱举升角度：

49、m＝m2-m1

50、m(t+1)＝m(t)+δm(t)+v1(t)+n1(t)

51、δm(t)＝m(t)-m(t-1)

52、其中，m表示货箱举升角度，m1表示举升前货箱角度传感器检测的角度值，m2表示举升后货箱角度传感器检测的角度值，m(t+1)表示t+1时刻预测的举升角度，m(t)表示t时刻货箱角度传感器实际反馈的角度，m(t-1)表示t-1时刻货箱角度传感器实际反馈的角度。

53、进一步地，所述计算模块具体用于：

54、所述角度融合控制器根据以下方程组，计算当前货箱举升角度：

55、m＝m2-m1

56、m(t+1)＝m(t)+δm(t)+v1(t)+n1(t)

57、δm(t)＝ω·δt

58、其中，ω表示货箱角度传感器检测的角速度，δt表示单位时间差。

59、进一步地，所述检测模块具体用于：

60、所述角度融合控制器根据以下方程组，计算货箱角度传感器或者车架角度传感器的预测值：

61、m1(t)＝m1(t)+v1(t)+n1(t)

62、m2(t)＝m2(t)+v2(t)+n2(t)

63、m(t+1)＝m(t)+δm(t)+v1(t)+n1(t)

64、δm(t)＝m(t)-m(t-1)

65、当货箱角度传感器或者车架角度传感器的预测值大于阈值时，确定货箱角度传感器或者车架角度传感器异常。

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

67、本发明提供的方法和系统，角度融合控制器可以检测车架角度传感器和货箱角度传感器是否异常，当其中一个存在异常时，可以由双传感器检测转变为单传感器检测，不影响货箱举升功能的使用，避免长时间停车维修，保持生产线的正常运行，保障生产线的生产能力。