一种车载式草莓采摘机器人稳像方法

1.本发明涉及草莓采摘机器人稳像技术领域，具体的说是一种车载式草莓采摘机器人稳像方法。


背景技术：

2.草莓为蔷薇科属多年生浆果类草本果树，是世界上栽培最为广泛的果树种类之一。在草莓种植的过程中，草莓的采摘占有重要地位，整个种植生产过程中约有1/4的时间被草莓采摘所占用，且由于草莓成熟后必须尽快采摘，所以在盛果期采摘作业的劳动强度非常大，常常耗费了巨大的人力物力。故随着5g技术和智慧农业的发展，以及人力成本的不断提高，将自动化技术引进草莓采摘作业环节，实现草莓的自动化采摘成为目前急需解决的问题。
3.目前，草莓采摘机器人基本采用车载式底盘，工作于田垄间，通过对ccd摄像机采集的彩色图像进行检测，计算出草莓的方位，通过距离传感器测量机械手到草莓的距离，并搭配多自由度机械手完成草莓自动采摘。但是由于规模化的草莓生产普遍采用大棚种植方式，草莓种植密度大，行间距较小，且不能硬化处理，地面不平整，导致草莓采摘机器人在行进过程中会产生抖动。该抖动会对图像采集造成影响，使得在进行采摘时发生草莓定位精准度下降的问题，导致草莓自动化采摘的效率降低。


技术实现要素：

4.本发明的目的是针对现有技术中草莓采摘机器人视觉的抖动问题，提出了一种车载式草莓采摘机器人稳像方法，其保证了草莓采摘机器人图像采集模块的稳定性，提高草莓识别精准度，进而显著提高草莓的采摘效率。
5.为了实现上述目的，本发明采用的具体方案为：一种车载式草莓采摘机器人稳像方法，其是借助搭设于车载式草莓采摘机器人上的稳像系统实现的，所述稳像系统包括工作平台、上位机、主控制器、电机驱动模块、三轴电机以及九轴姿态传感器，其中，所述工作平台为具有俯仰轴、横滚轴以及航向轴的三轴云台，所述工作平台的上方安装有草莓采摘机器人的图像采集模块，所述工作平台的下方设有九轴姿态传感器，所述三轴电机分别为控制图像采集模块沿航向轴方向活动的航向轴电机、控制图像采集模块沿横滚轴方向活动的横滚轴电机以及控制图像采集模块沿俯仰轴方向活动的俯仰轴电机；车载式草莓采摘机器人稳像方法主要包括如下步骤：(1)、通过九轴姿态传感器实时测量图像采集模块的三轴角速度、三轴加速度以及三轴磁感应强度，并将测量的数据解算成姿态角度信息，之后采用卡尔曼滤波算法对采集的姿态角度信息进行融合处理，进而对图像采集模块的姿态角度信息进行修正；(2)、将修正的姿态角度信息与上位机中预设的姿态角度信息作差以求取角度误差，将角度误差作为pid调节器的输入，通过pid调节器对角度误差进行调节，将调节后的角
度误差量作为控制三轴电机转动的依据；(3)、将经pid调节器调节后的角度误差作为三轴电机所需转动的角度，通过主控制器向电机驱动模块发送控制信息进而分别控制航向轴电机、横滚轴电机、俯仰轴电机转过相应角度，从而对图像采集模块的姿态进行实时调整，以保证图像采集模块始终稳定在上位机预设的姿态。
6.进一步地，步骤(1)中，通过九轴姿态传感器内部集成的三轴陀螺仪、三轴加速度计和三轴磁力计分别获取图像采集模块的三轴角速度、三轴加速度以及三轴磁感应强度，然后通过下列算法能够将测量的数据处理成图像采集模块的姿态角度信息，其中：(a)、三轴陀螺仪测量的是三轴角速度，通过下式即能够计算出对应的角度值：θ
t
＝(ω
t-ω)d
t
+θ
t-1
其中，θ
t
为当前时刻的角度值，θ
t-1
为前一时刻的角度值，ω
t
为陀螺仪测量当前时刻的角速度，ω为当前时刻角速度的偏移量，d
t
为积分时间，即数据的采样周期；(b)、三轴加速度计测量的是三轴加速度分量，通过下式即能够解算出角度信息：其中，a
x
、ay、az分别是x轴、y轴、z轴的加速度分量，ρ为俯仰角，φ为横滚角，γ为z轴与重力加速度的夹角；(c)、三轴磁力计测量的是三轴的磁感应强度，由于x轴和y轴的磁感应强度合成后指向地磁北极，因此可通过测量三轴磁力计当前方向与地磁北极的夹角来实现航向角的测量，其在水平位置且无外加磁场干扰时，航向角可通过如下公式进行计算：其中，hx和hy分别为x轴和y轴输出的磁感应强度值。
7.进一步地，步骤(1)中，采用卡尔曼滤波算法对测量的姿态角度信息进行融合处理，进而通过下式能够得到计算的姿态角度信息：其中，为基于k-1次对k次的预估计，为对k-1次真实值xk-1的估计，θk为当前时刻的角度值，ts为采样周期，ωk和ω
k-1
分别为陀螺仪测量k时刻以及k-1时刻的角速度。
8.进一步地，步骤(2)中，pid调节器输出与输入的关系如下式所示：
其中，e(k)为当前测量所得角度误差，e(n)为第n次测量所得角度误差，e(k-1)为上次测量所得角度误差，u(k)为最终所需控制电机转动的角度信息，k
p
为比例增益，ki为积分增益，kd为微分增益。
9.进一步地，选用的九轴姿态传感器的型号为mpu-9250。
10.有益效果：(1)本发明中的稳像方法依据高精度的九轴姿态传感器实时测量数据，通过卡尔曼滤波算法计算出图像采集模块的姿态信息。主控制器依据计算的姿态角度信息与上位机预设的姿态信息计算出角度误差，再利用pid调节器对角度误差进行调节，将调节后的误差量作为驱动三轴电机转动的依据。电机驱动模块依据主控制器发送的控制信号控制三轴电机转动，从而使图像采集模块保持稳定。所述稳像方法通过实时调节姿态阻断了车载式草莓采摘机器人车身抖动向图像采集模块传递的过程，解决了因图像采集模块抖动造成草莓定位精准度降低的问题，从而提高了草莓自动化采摘的效率，有利于降低草莓产业的整体生产成本，提高我国草莓深加工行业的市场竞争力。(2)本发明所选用的九轴姿态传感器的型号为mpu-9250，其可以每秒钟完成100次姿态角度测量，再依据测量的角度信息对图像采集模块的姿态进行调整，真正实现了实时闭环控制，从而保证了图像采集模块的稳定性。
附图说明
11.图1是本发明中稳像系统的整体结构示意图。
12.图2是本发明中稳像系统的工作流程示意图。
具体实施方式
13.下面将结合具体实施例对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
14.一种车载式草莓采摘机器人稳像方法，其是借助搭设于车载式草莓采摘机器人上的稳像系统实现的，请参考图1和图2，所述稳像系统包括工作平台、上位机、主控制器、电机驱动模块、三轴电机以及九轴姿态传感器，其中，所述工作平台为具有俯仰轴、横滚轴以及航向轴的三轴云台，所述工作平台的上方安装有草莓采摘机器人的图像采集模块，所述工作平台的下方设有九轴姿态传感器，所述三轴电机分别为控制图像采集模块沿航向轴方向活动的航向轴电机、控制图像采集模块沿横滚轴方向活动的横滚轴电机以及控制图像采集模块沿俯仰轴方向活动的俯仰轴电机；车载式草莓采摘机器人稳像方法主要包括如下步骤：1、测量解算图像采集模块的姿态角度信息(a)、三轴陀螺仪测量的是三轴角速度，通过下式即能够计算出对应的角度值：
θ
t
＝(ω
t-ω)d
t
+θ
t-1
ꢀꢀ
(1)其中，θ
t
为当前时刻的角度值，θ
t-1
为前一时刻的角度值，ω
t
为陀螺仪测量当前时刻的角速度，ω为当前时刻角速度的偏移量，d
t
为积分时间，即数据的采样周期；(b)、三轴加速度计测量的是三轴加速度分量，通过下式即能够解算出角度信息：其中，a
x
、ay、az分别是x轴、y轴、z轴的加速度分量，ρ为俯仰角，φ为横滚角，γ为z轴与重力加速度的夹角；(c)、三轴磁力计测量的是三轴的磁感应强度，由于x轴和y轴的磁感应强度合成后指向地磁北极，因此可通过测量三轴磁力计当前方向与地磁北极的夹角来实现航向角的测量，其在水平位置且无外加磁场干扰时，航向角可通过如下公式进行计算：其中，hx和hy分别为x轴和y轴输出的磁感应强度值。(d)、卡尔曼滤波本发明中的稳像方法采用了卡尔曼滤波算法对采集的姿态角度信息进行融合处理。假设系统的状态方程和测量方差可用如下线性方程描述：则卡尔曼滤波递推公式可以概括为5个核心公式：状态量的预估计：误差协方差估计：p
k|k-1
＝ap
k-1at
+q
ꢀꢀ
(6)卡尔曼增益更新：kk＝p
k|k-1ht
[hp
k|k-1ht
+r]-1
ꢀꢀ
(7)现状态估计值更新：误差协方差更新：pk＝(i-kkh)p
k|k-1
ꢀꢀ
(9)对于姿态检测系统，陀螺仪的角度信息可用式(1)来表示，加速度计和磁力计测量的角度信息可作为观测反馈值。为了抑制陀螺仪的漂移，系统还需要对偏移量进行估计处理，所以系统的状态方程可用如下公式描述：
其中，为基于k-1次对k次的预估计，为对k-1次真实值x
k-1
的估计，θk为当前时刻的角度值，ts为采样周期，ωk和ω
k-1
分别为陀螺仪测量k时刻以及k-1时刻的角速度。完成姿态角度信息的采集和修正后，便能够依据修正后的姿态角度信息计算需要调节的角度误差。2、计算角度误差，并进行pid调节将修正后的图像采集模块的姿态角度信息与上位机预设角度信息作差，得出角度误差，将所得的角度误差作为pid调节器的输入，将pid调节器的输出作为控制三轴电机转动的最终依据，pid调节器输出与输入的关系如下式所示：其中：kp为比例增益，ki为积分增益，kd为微分增益，e为误差，t为当前时间。公式(11)中，u(t)是pid调节器的输出，在这里实际意义同公式(12)中的u(k)。由于对姿态角度信息的测量在时间上不是连续的，故需要对上式进行离散化处理，处理后的公式如下所示：其中，e(k)为当前测量所得角度误差，e(n)为第n次测量所得角度误差，e(k-1)为上次测量所得角度误差，u(k)为最终所需控制电机转动的角度信息，k
p
为比例增益，ki为积分增益，kd为微分增益。3、发送三轴电机控制信息，调节图像采集模块的姿态将经pid调节器调节后的角度误差作为三轴电机所需转动的角度，通过主控制器向电机驱动模块发送控制信息进而分别控制航向轴电机、横滚轴电机、俯仰轴电机转过相应角度，从而对图像采集模块的姿态进行实时调节，以保证图像采集模块始终稳定在上位机预设的姿态，进而提高草莓采摘机器对草莓定位的精准度。
[0015]
本发明中的稳像方法依据高精度的九轴姿态传感器实时测量数据，通过卡尔曼滤波算法计算出图像采集模块的姿态信息。主控制器依据计算的姿态角度信息与上位机预设的姿态信息计算出角度误差，再利用pid调节器对角度误差进行调节，将调节后的误差量作为驱动三轴电机转动的依据。电机驱动模块依据主控制器发送的控制信号控制三轴电机转动，从而使图像采集模块保持稳定。所述稳像方法通过实时调节姿态阻断了车载式草莓采摘机器人车身抖动向图像采集模块传递的过程，解决了因图像采集模块抖动造成草莓定位精准度降低的问题，从而提高了草莓自动化采摘的效率，有利于降低草莓产业的整体生产成本，提高我国草莓深加工行业的市场竞争力。
[0016]
以上所述，仅是本发明的较佳实施例而已，并非随本发明作任何形式上的限制。凡根据本发明的实质所做的等效变换或修饰，都应该涵盖在本发明的保护范围之内。