[发明专利]一种用于农田建图的定位系统

权利要求书

1.一种用于农田建图的定位系统，其特征在于，包括底盘、控制组件、激光调整装置、激光发射与接收装置和激光信号反馈装置；

所述底盘用于对整个定位系统提供支撑和动力；

所述控制组件设置于底盘内，用于对整个定位系统进行控制以及信息交互；

所述激光调整装置设置于底盘上方，且与控制组件电连接，用于与控制组件进行信息交互；

所述激光发射与接收装置设置于激光调整装置的上方，用于发射或接收激光，并与所述激光调整装置电连接，以通过所述激光调整装置调节上下位移；

所述激光信号反馈装置包括若干，且设置于所需建图的农田边界处，用于向激光发射与接收装置反馈激光，以提示激光发射与接收装置发出的激光高度；

所述定位系统进行定位时包括以下步骤：

S1：启动工作，操作人员遥控控制底盘，给控制组件发送运动指令，控制组件驱动底盘在农田内行走，并启动激光发射与接收装置、激光调整装置；

S2：所述激光调整装置自动调节激光发射与接收装置的高度，使激光发射与接收装置的激光发射中心线始终与激光信号反馈装置的中心线齐平；

S3：利用卡尔曼滤波KEF算法，计算底盘当前建图的位姿和协方差矩阵Q，以建立农田的二维地图。

2.根据权利要求1所述的一种用于农田建图的定位系统，其特征在于，步骤S2中，所述激光调整装置自动调节激光发射与接收装置高度的具体步骤为：

S21：所述激光发射与接收装置不断向所述激光信号反馈装置发射激光，所述激光信号反馈装置通过反射将激光反馈回给激光发射与接收装置，所述激光发射与接收装置通过检测所发射和所接收激光的位置，判断底盘在竖直方向上发生的位移ΔZmm以及每一激光信号反馈装置相对于底盘的坐标信息，并将位移信息和坐标信息传输给激光调整装置；

S22：所述激光调整装置接收到激光发射与接收装置传输来的位移信息后，根据位移信息向上或向下移动ΔZmm，所述激光发射与接收装置随着激光调整装置的移动而移动，以使激光发射与接收装置的激光发射中心线与激光信号反馈装置的中心线齐平。

3.根据权利要求1所述的一种用于农田建图的定位系统，其特征在于，步骤S3中，计算底盘当前建图的位姿和协方差矩阵Q的具体计算步骤为：

S31：获取底盘初始的状态空间参数，并计算t时刻底盘的协方差预测方程；

S32：编写观测模型，并计算观测对状态空间总的雅克比矩阵；

S33：计算增益并更新协方差和状态空间。

4.根据权利要求3所述的一种用于农田建图的定位系统，其特征在于，步骤S31中，获取底盘初始的状态空间参数，并计算t时刻底盘的协方差预测方程的具体步骤为：

以底盘为中心建立坐标系，设定底盘当前的位姿为x，y，θ；其中，x为底盘在x轴的坐标，y为底盘在y轴的坐标，θ为底盘的转动角度；

设定当前的状态空间中有N个反光柱，并设定指第N个反光柱的x、y坐标；其中，N0，此时，N个反光柱相对于底盘当前位姿的状态空间坐标集合X为：

设定底盘初始的位姿为x₀,y₀,θ₀；则第t时刻，底盘的位姿为x_t，y_t，θ_t；设定盘轮速计的线速度σ_v和角速度σ_ω均服从均值为0的高斯分布，根据协方差的计算公式：

可得初始时刻，底盘轮速计控制的协方差∑_ξ，0为：

其中，设定∑_x,∑_y,∑_θ的初始值为1；

而在第t时刻，底盘轮速计控制的协方差∑_t为：

计算后得状态量在t时刻的协方差预测方程∑_t为：

其中，G_ξ,t是运动模型关于机器人位姿ξ_t-1雅可比矩阵；

G_u是运动模型关于控制(轮速计线速度和角速度)的雅可比矩阵；