[发明专利]一种车载式草莓采摘机器人稳像方法

权利要求书

1.一种车载式草莓采摘机器人稳像方法，其特征在于：其是借助搭设于车载式草莓采摘机器人上的稳像系统实现的，所述稳像系统包括工作平台、上位机、主控制器、电机驱动模块、三轴电机以及九轴姿态传感器，其中，所述工作平台为具有俯仰轴、横滚轴以及航向轴的三轴云台，所述工作平台的上方安装有草莓采摘机器人的图像采集模块，所述工作平台的下方设有九轴姿态传感器，所述三轴电机分别为控制图像采集模块沿航向轴方向活动的航向轴电机、控制图像采集模块沿横滚轴方向活动的横滚轴电机以及控制图像采集模块沿俯仰轴方向活动的俯仰轴电机；

车载式草莓采摘机器人稳像方法主要包括如下步骤：

(1)、通过九轴姿态传感器实时测量图像采集模块的三轴角速度、三轴加速度以及三轴磁感应强度，并将测量的数据解算成姿态角度信息，之后采用卡尔曼滤波算法对采集的姿态角度信息进行融合处理，进而对图像采集模块的姿态角度信息进行修正；

(2)、将修正的姿态角度信息与上位机中预设的姿态角度信息作差以求取角度误差，将角度误差作为PID调节器的输入，通过PID调节器对角度误差进行调节，将调节后的角度误差量作为控制三轴电机转动的依据；

(3)、将经PID调节器调节后的角度误差作为三轴电机所需转动的角度，通过主控制器向电机驱动模块发送控制信息进而分别控制航向轴电机、横滚轴电机、俯仰轴电机转过相应的角度，从而对图像采集模块的姿态进行实时调整，以保证图像采集模块始终稳定在上位机预设的姿态上。

2.根据权利要求1所述的一种车载式草莓采摘机器人稳像方法，其特征在于，步骤(1)中，通过九轴姿态传感器内部集成的三轴陀螺仪、三轴加速度计和三轴磁力计分别获取图像采集模块的三轴角速度、三轴加速度以及三轴磁感应强度，然后通过下列算法能够将测量的数据处理成图像采集模块的姿态角度信息，其中：

(a)、通过下式能够计算出三轴角速度所对应的角度值：

θ_t＝(ω_t-ω)d_t+θ_t-1

其中，θ_t为当前时刻的角度值，θ_t-1为前一时刻的角度值，ω_t为陀螺仪测量当前时刻的角速度，ω为当前时刻角速度的偏移量，d_t为积分时间，即数据的采样周期；

(b)、通过下式能够解算出三轴加速度所对应的角度信息：

其中，A_x、A_y、A_z分别是X轴、Y轴、Z轴的加速度分量，ρ为俯仰角，φ为横滚角，γ为Z轴与重力加速度的夹角；

(c)、由于X轴和Y轴的磁感应强度合成后指向地磁北极，因此可通过测量三轴磁力计当前方向与地磁北极的夹角来实现航向角的测量，其在水平位置且无外加磁场干扰时，航向角可通过如下公式进行计算：

其中，Hx和Hy分别为X轴和Y轴输出的磁感应强度值。

3.根据权利要求1所述的一种车载式草莓采摘机器人稳像方法，其特征在于，步骤(1)中，采用卡尔曼滤波算法对采集的姿态角度信息进行融合处理，进而通过下式对姿态角度信息进行修正：

其中，为基于k-1次对k次的预估计，为对k-1次真实值X_k-1的估计，θ_k为当前时刻的角度值，T_s为采样周期，ω_k和ω_k-1分别为陀螺仪测量k时刻以及k-1时刻的角速度。

4.根据权利要求1所述的一种车载式草莓采摘机器人稳像方法，其特征在于，步骤(2)中，PID调节器输出与输入的关系如下式所示：

其中，e(k)为当前测量所得角度误差，e(n)为第n次测量所得角度误差，e(k-1)为上次测量所得角度误差，u(k)为最终所需控制电机转动的角度信息，K_p为比例增益，K_i为积分增益，K_d为微分增益。

5.根据权利要求1所述的一种车载式草莓采摘机器人稳像方法，其特征在于，选用的九轴姿态传感器的型号为MPU-9250。