一种光伏清扫机器人的位置及运动信息采集方法和系统

技术领域

本发明属于机器人技术领域，具体涉及一种光伏清扫机器人的位置及运动信息采集方法和系统。

背景技术

随着智能化家居的广泛应用，智能清扫机器人的用途越来越广，智能清扫机器人引入至光伏发电领域。现在的用于清理光伏板的智能清扫机器人多采用单GPS模式，只简单采集位置信息，但GPS模式精度不高，受到作业地点GPS搜星数量制约，在环境复杂区域，GPS信号往往会受到干扰，使得无法获得精确的定位信息。

同时智能清扫机器人多采用固定速度运行模式，且需要提前预设好轨迹，在空旷无障碍区域才可以正常工作，但是定速运行时，耗时较大，在复杂环境下需要进行运动控制，因为缺少位置及运动信息采集系统，使得无法进行后续的运动控制。

发明内容

本发明的目的在于克服上述现有技术的缺点，提供一种光伏清扫机器人的位置及运动信息采集方法和系统，以解决现有技术中应用于光伏领域的清扫机器GPS定位不准确，运行轨迹固定且运行速度固定的问题。

为达到上述目的，本发明采用以下技术方案予以实现：

一种光伏清扫机器人的位置及运动信息采集方法，包括以下步骤：

步骤1，采集机器人的位置信息；

步骤2，采集机器人的运动信息，所述运动信息包括机器人的三轴加速度、三轴角速度和磁力方向；

步骤3，通过三轴角速度获得机器人的四元数，通过对三轴加速度进行低通滤波和归一化处理获得加速度偏差；通过磁力方向和机器人的位置信息获得磁力偏差；通过磁力偏差和加速度偏差更新四元数，获得规范化的四元数；对规范化的四元数反算后获得机器人的位置信息和运动信息。

本发明的进一步改进在于：

优选的，步骤1中，通过RTK或BTK采集机器人的位置信息。

优选的，步骤2中，通过三个加速度计采集三个三轴加速度；通过三个陀螺仪采集三个三轴角速度，通过磁力计采集磁力方向。

优选的，将采集的三个三轴加速度、三个三轴角速度置于链表中，依次通过可信度和比较优先级获得一个三轴加速度和一个三轴角速度。

优选的，步骤3中，四元数的计算公式为：

其中，ω

优选的，步骤3中，所述加速度偏差的计算公式为：

其中，

式中，

优选的，步骤3中，所述磁力偏差的计算公式为：

优选的，w的计算公式为：

其中，mag为机器人采集的位置信息中的磁力方向。

优选的，规范化的四元数的计算公式为：

一种光伏清扫机器人的位置及运动信息采集系统，包括：

位置信息采集模块，用于采集机器人的位置信息；

运动信息采集模块，用于采集机器人的运动信息，所述运动信息包括机器人的三轴加速度、三轴角速度和磁力方向；

数据融合解算模块，用于通过三轴角速度获得机器人的四元数，通过对三轴加速度进行低通滤波和归一化处理获得加速度偏差；通过磁力方向和机器人的位置信息获得磁力偏差；通过磁力偏差和加速度偏差更新四元数，获得规范化的四元数；对规范化的四元数反算后获得机器人的位置信息和运动信息。

与现有技术相比，本发明具有以下有益效果：

本发明公开了一种光伏清扫机器人的位置及运动信息采集方法和系统，该方法首先采集机器人的位置信息和运动信息，该方法在获得信息时，对信息进行低通滤波和归一化处理后，使得获得数据准确性高。该方法对采集的信息经数据融合后发送给控制器，进一步的能够精确解算位置及姿态信息，配有多套传感器，可靠性高。

附图说明

图1为RTK定位原理图；

图2为多传感器冗余机制示意图；

图3为互补滤波示意图；

图4为姿态解算流程图；

其中：1-；2-；3-。

具体实施方式

下面结合附图对本发明做进一步详细描述：

在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制；术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性；此外，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

本发明公开了一种光伏清扫机器人的位置及运动信息采集系统，该信息采集系统包括：位置信息采集模块、运动信息采集模块和数据融合解算模块。

位置信息采集模块，采用工业级的IMU：ICM-20689加速计&陀螺仪，RM3100电子罗盘，内置u-blox F9P多星多频卫星接收机，支持GPS，Beidou，Glonass，Galieo四大卫星系统，并支持四卫星系统同时接收。定位精度： RTK 0.01m+1ppm CEP。可同时支持RTK厘米级定位导航和RAW数据记录，用于后差分解算。支持快门触发和热靴同步；并且采用了CAN通信协议，兼容多种开源自驾仪。该模块支持RTK&PPK功能，最高支持25Hz原始数据记录。通过位置信息采集模块能够获得机器人在地方坐标系中的实时坐标，该坐标包括经度、维度和磁力方向。

具体的，RTK时实时动态载波相位差分技术，是实时处理两个测量站载波相位观测量的差分方法。将基准站采集的载波相位发送给智能清扫机器人上的接收机，进行求差解算坐标。具体是将一台GPS接收设备机置于基准站，第二台GPS接收设备机置于智能清扫机器人，在智能清扫机器人运动过程中，基准站与移动站同时接收相同时间的同一GPS卫星发射的信号，通过实施动态测量的软件对基准站所接收的测量值与已知“基准位置”进行比较，计算GPS差分改正值。之后数据传输系统通过无线电数据链将差分改正值发送至与其共视卫星的智能清扫机器人与之接收到的卫星信号实施联合解算，从而得到经过差分修正后较为精准的实时位置信息。

2)PPK载波相位后差分技术，由基准站和流动站组成。工作原理是使用同步观测的一台基准站和智能清扫机器人同时对卫星的载波相位进行测量且实时记录，初始化后的智能清扫机器人在每个待定点移动过程中持续保持对卫星的连续测量，将整周模糊度传递至下一待定点。当整体测量完成之后，使用GPS软件将基准站和智能清扫机器人同步接收的不同载波相位数据数据在计算机中进行线性组合，得到虚拟载波相位观测量值，通过虚拟载波相位的观测量值计算接收端之间精确的相对位置，通过坐标转换得到智能清扫机器人在地方坐标系中的坐标情况。

RTK模式下的智能清扫机器人需要实时的接收差分改正信号，但是如果遇到特殊地形环境影响如山体遮挡、高楼等，航测容易出现卫星失锁情况，航测定位精度就会无法保证，给后期数据处理带来困难和麻烦。在PPK模式下，基准测量端和移动测量端之间不需要实时链路，不会受到链路受到干扰或者直接中断的影响，基于此，在PPK加持下智能清扫机器人还能获得更广的作业范围，在长距离和大型作业区域，特别是皮带区域，如输电线路，公路，铁路，石油和天然气管道，PPK将是最佳选择。在具体实施中，RTK模式更加便捷更加容易实现，因此作为主要的定位模式，PPK模式作为辅助模式。

本发明以RTK为主要工作模式，PPK为辅助模式，在RTK模式受限的情况下，启用PPK模式。

本发明的实施例之一为公开了一种运动信息采集模块，该运动信息采集模块能够获得机器人实时的加速度、角速度和方向。为了提高作业时的可靠性，采用冗余策略，配置三个加速度计和陀螺仪和一个电子罗盘。

具体的加速度计采用ICM-20602、ICM-20689、BMI055，加速度计用于测量智能清扫机器人加速度的仪表，加速度计作用：

(1)当智能清扫机器人具有很大的加速度时，物体及其所载的仪器设备和其他无相对加速度的物体均受到能产生同样大的加速度的力，即受到动载荷，通过测出加速度来计算动载荷。

(2)智能清扫机器人所在的空间位置，可通过惯性导航连续地测出其加速度，然后经过积分运算得到速度分量，再次积分得到一个方向的位置坐标信号，而三个坐标方向的仪器测量结果就综合出运动曲线并给出每瞬时航行器所在的空间位置。

(3)在运动控制系统中，常需要加速度信号作为产生控制作用所需的信息的一部分。

陀螺仪采用ICM-20602、ICM-20689和BMI055，用于测量三轴姿态角速度。

磁力计(电子罗盘)采用IST8310，用于测量航向配合惯性导航使用，能够获得机器人的运动方向。

本发明的实施例之一为公开了一种数据融合解算模块，该模块包括冗余模块、转换模块、矫正模块和更新模块；

冗余模块，通过投票机制选择多个传感器里面最可信的那一组数据，给EKF2做姿态解算和位置解算。上面的传感器不断的发送采集获得数据，具体包括三个加速度计采集的三个三轴加速度，三个陀螺仪采集的三个三轴角速度和一个磁力计采集的磁场强度。通过投票机制选择出最可信的一组传感器数据，完成多冗余过程。具体的冗余过程为，所有传感器采集的数据组成一个链表，从链表的开始进行数据的检索，对每一个数据进行判断，如果发现满足这样就做到了多冗余机制。将传感器组成一个链表，从链表开始的地方开始检索，对每个数据进行判断，首先判断可信度，如果可信度不满足要求则直接剔除，如果满足要求，对于同一类型数据进行优先级的比较，优先选择优先级的高的数据。更为具体的，三个三轴角速度进行比较，三个三轴加速度进行比较，最终选择出一个三轴角速度和一个三轴加速度。

转换模块，该模块用于通过数学公式将采集的数据转换为数学模式，方便计算。具体的包括两个步骤：

(1)计算出四元数

欧拉角与机体角速度的关系：

b

其中，θ为俯仰角，φ为滚转角，ψ为偏航角。

旋转矩阵(机体坐标系到地球固连坐标系)

[b

四元数与机体角速度的关系：

其中，ω

(2)地球固连坐标系到机体坐标系的旋转四元数

在预测环节，由三轴陀螺仪测得的角速度，通过式(4)计算出四元数姿态预测，通过下式(5)将

矫正模块，用于对获得的四元数进行矫正；

在预测环节得到的四元数

通过加速度计得到加速度变化量

当磁力计可读时，通过磁力计变化量

1)加速度计校正

加速度计信号首先经过低通滤波器(消除高频噪声)：

获得y(k)为加速度变化量

基于加速度变化计算加速度偏差：

式中，v表示重力向量在机体坐标系的向量，其计算公式如下式(8)。

为导航坐标系到机体坐标系的旋转矩阵。

此时，由v与加速度计向量垂直分量叉乘，得到误差值。两个物理意义相同的向量，理论上叉乘为零。

2)磁力计校正

数据预处理与加速度计相同，先滤波，然后归一化，得到

计算磁力误差：

式中，w计算过程分为正常情况下的有GPS信息校准，以及特殊情况无法连接上GPS信号，使用无GPS校准。

GPS校准的过程为，在px4中，磁力计使用GPS信息[0,0,mag]进行校准，即位置信息采集模块中通过RTK或PPK获得的磁力方向(mag)。

此时，由w与磁力计向量叉乘，得到误差值。两个物理意义相同的向量，理论上叉乘为零。

无GPS校准时：

磁力计的输出(m)在机体坐标系下，将其转换到导航坐标系：

为机体坐标系到导航坐标系的旋转矩阵。

导航坐标系的x轴与正北对齐，故可以将磁力计在xoy平面的投影折算到x轴。

再次变换到机体坐标系：

3)更新四元数

由加速度计和磁力计校准得到的误差值：

e＝e

由该误差值得到修正值δ：(只有ki修正bias)

δ＝K

修正后的角速度值：

ω＝ω

通过修正后的角速度值，基于一阶龙格库塔方法求解一阶微分方程：

可以求出四元数微分方程的差分形式：

进一步的获得，规范化的四元数：

其中/>

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。