本发明专利技术公开了一种光伏智能清扫机器人的多传感器运动航向姿态估计方法，首先在光伏智能清扫机器人中加入IMU传感器与GPS传感器，构建多传感器运动航向姿态估计系统；IMU传感器和GPS传感器的数据通过五组EKF数据融合，通过高斯和滤波器解算权重，最后计算出最佳的偏航角估计值，为清扫机器人工作及后续控制提供有效的导航数据。本发明专利技术可以为清扫机器人提供实时有效的偏航角数据，为其后续控制及工况稳定提供有效的数据保障。提供有效的数据保障。提供有效的数据保障。
【技术实现步骤摘要】
光伏智能清扫机器人的多传感器运动航向姿态估计方法
[0001]本专利技术属于机器人
，具体涉及一种机器人多传感器运动航向姿态估计方法。
技术介绍
[0002]近几十年来，光伏发电技术日趋成熟，光伏发电成为人类应对能源危机的一大有效方法。由于光伏电池板长期暴露在室外，表面容易积灰，而积灰则是影响光伏发电的关键因素之一。早期光伏板表面积灰会使玻璃板透光率下降，这也会造成标准小时发电量将降低，而清洁干净的光伏电池板发电功率比积灰严重的光伏电池板高出15％以上。由此可见，积灰会严重影响光伏发电质量，导致光伏发电站经济效益受损。因此，对光伏电池板进行周期性清洗是必然的需求。
[0003]目前常用的清扫方式有：人工清扫、电帘除尘、纳米自洁技术、光伏机器人清扫等。其中，传统的人工清扫方式费时费力，清扫效果也难以保证；电帘除尘技术及纳米自清洁技术成本极高，不适合大范围使用。与前三者相比，光伏机器人清扫效率、自动化程度较高，可实现无人值守清扫，是目前光伏发电站必不可少的维护设备。而航向估计可为清扫机器人提供实时有效的偏航角数据，为其后续控制及工况稳定提供有效的数据保障。
技术实现思路
[0004]为了克服现有技术的不足，本专利技术提供了一种光伏智能清扫机器人的多传感器运动航向姿态估计方法，首先在光伏智能清扫机器人中加入IMU传感器与GPS传感器，构建多传感器运动航向姿态估计系统；IMU传感器和GPS传感器的数据通过五组EKF数据融合，通过高斯和滤波器解算权重，最后计算出最佳的偏航角估计值，为清扫机器人工作及后续控制提供有效的导航数据。本专利技术可以为清扫机器人提供实时有效的偏航角数据，为其后续控制及工况稳定提供有效的数据保障。
[0005]本专利技术解决其技术问题所采用的技术方案包括如下步骤：
[0006]步骤1：在光伏智能清扫机器人中加入IMU传感器与GPS传感器，构建多传感器运动航向姿态估计系统；所述IMU传感器为机器人提供惯导数据；所述GPS传感器用于获取机器人的实时工作速度；
[0007]步骤2：在XY水平面上预置5个初始偏航角ψ
i
,i＝1,2,
…
,5，初始权值W1,W2,W3,W4,W5均取值为0.2；五个初始偏航角角度值设置为：
[0008]ψ3＝0，
[0009]令ψ表示机体坐标系相对于地球坐标系的航向角；
[0010]令V
NE
表示机体在世界坐标系中的向北和向东的速度；
[0011][0012]其中，V
N
表示大地坐标系下飞机北向速度，V
E
表示大地坐标系下飞机动向速度；
[0013]令Δv表示IMU的z轴在机体轴上的增量角度测量值，即增量航向角的测量值；
[0014]令ΔV
xy
表示IMU的x轴和y轴在机体轴上的增量速度测量值；
[0015][0016]其中，ΔV
x
表示机体坐标系下x轴速度增量，ΔV
y
表示机体坐标系下y轴速度增量；
[0017]步骤3：航向角更新方程为：
[0018]ψ
′
＝ψ+Δψ
[0019]速度更新方程为：
[0020][0021]其中
[0022][0023]定义状态向量为：
[0024][0025]计算状态转移矩阵：
[0026][0027]其中，表示k时刻先验状态估计值，表示k
‑
1时刻计算出的后验估计值；
[0028]传播协方差矩阵:
[0029]P
k∣k
‑1＝FP
k
‑
1∣k
‑1F
T
+Q
[0030]其中，P
k∣k
‑1表示k时刻先验估计协方差，P
k
‑
1∣k
‑1表示k
‑
1时刻更新的后验估计协方差；
[0031]过程噪声协方差矩阵Q为：
[0032]Q＝GDG
T
[0033]其中，D矩阵为传感器各自数据的噪声，均服从于正态分布；G表示控制影响矩阵；
[0034]定义控制向量：
[0035][0036]推导IMU噪声到状态噪声的3
×
3控制影响矩阵G：
[0037][0038]步骤4：测量更新部分；
[0039]由GPS提供的向北、向东速度观测值为：
[0040][0041]其中，表示由GPS测量出的无人机在大地坐标系下的北向速度，表示由GPS测量出的无人机在大地坐标系下的东向速度；
[0042]由GPS传感器提供的速度观测值，其对应的方差为由此构建2
×
2测量噪声矩阵：
[0043][0044]速度观测矩阵H
k
为：
[0045][0046]其中，h表示测量状态量的转换函数关系；
[0047]根据传感器的当前值和已经计算出的预测值计算残差：
[0048][0049]残差协方差为：
[0050]S＝HP
k∣k
‑1H
T
+R
[0051]扩展卡尔曼滤波算法EKF增益计算：
[0052]K＝P
k∣k
‑1H
T
S
‑1[0053]后验状态估计为：
[0054][0055]后验协方差为：
[0056]P
k∣k
＝P
k∣k
‑1‑
KSK
T
[0057]步骤5：对于5个初始偏航角ψ
i
，通过步骤3和步骤4分别得到5组扩展卡尔曼滤波算法EKF模型，针对5组EKF模型的状态分别计算高斯密度；
[0058]根据残差值及残差协方差计算每个模型的马氏距离：
[0059][0060]计算二维正态分布的密度：
[0061][0062]根据每个模型方差的加权平均值计算偏航状态新的权值：
[0063]W
i
′
＝Density
i
·
W
i
[0064]将每组的权重与初始偏航角相乘，最后相加得出最终估计的偏航角值：
[0065][0066][0067]ψ
GSF
＝arctan(y，x)
[0068]ψ
GSF
为最终估计的偏航角值。
[0069]本专利技术的有益效果如下：
[0070]本专利技术可以为清扫机器人提供实时有效的偏航角数据，为其后续控制及工况稳定提供有效的数据保障。
附图说明
[0071]图1是本专利技术方法流程图。
[0072]图2是本专利技术五个初始偏航角初始角度值示意图。
[0073]图3是本专利技术方法框架图。
[0074]图4是本专利技术多传感器运动航向姿态估计系统原理框图。本文档来自技高网...
【技术保护点】
【技术特征摘要】
1.一种光伏智能清扫机器人的多传感器运动航向姿态估计方法，其特征在于，包括以下步骤：步骤1：在光伏智能清扫机器人中加入IMU传感器与GPS传感器，构建多传感器运动航向姿态估计系统；所述IMU传感器为机器人提供惯导数据；所述GPS传感器用于获取机器人的实时工作速度；步骤2：在XY水平面上预置5个初始偏航角ψ
i
，i＝1，2，...，5，初始权值W1，W2，W3，W4，W5均取值为0.2；五个初始偏航角角度值设置为：令ψ表示机体坐标系相对于地球坐标系的航向角；令V
NE
表示机体在世界坐标系中的向北和向东的速度；其中，V
N
表示大地坐标系下飞机北向速度，V
E
表示大地坐标系下飞机动向速度；令Δψ表示IMU的z轴在机体轴上的增量角度测量值，即增量航向角的测量值；令ΔV
xy
表示IMU的x轴和y轴在机体轴上的增量速度测量值；其中，ΔV
x
表示机体坐标系下x轴速度增量，ΔV
y
表示机体坐标系下y轴速度增量；步骤3：航向角更新方程为：ψ
′
＝ψ+Δψ速度更新方程为：其中定义状态向量为：计算状态转移矩阵：其中，表示k时刻先验状态估计值，表示k
‑
1时刻计算出的后验估计值；传播协方差矩阵：P
k|k
‑1＝FP
k
‑
1|k
‑1F
T
+Q其中，P
k|k
‑1表示k时刻先验估计协方差，P
k
‑
1|k
‑1表示k
‑
1时刻更新的后验估计协方差；过程噪声协方差矩阵Q为：
Q＝GDG
T
...
【专利技术属性】
技术研发人员：高杰，沈楷清，
申请(专利权)人：西安万飞控制科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。