一种实现山地机械等间距直线行驶的方法

摘要

本发明公开了一种实现山地机械等间距直线行驶的方法，将山地机械所走的直线分为若干等长的线段，建立了在等分点处的左右偏向检测法和航向角计算法，利用在等分点处测试得到的实时数据计算出航向角的大小；山地机械根据航向角的大小通过PID控制算法推算出前轮转向角的大小自动纠正偏差，实现直线行驶。本发明对山地机械进行分段式纠正航向偏差，简单可靠；能使山地机械在误差允许范围内直线行驶，且波动小、稳定性好，显著提高了山地机械作业精度和效率，有效的降低了驾驶员的劳动强度。

主权项

一种实现山地机械等间距直线行驶的方法，其特征在于包括下述步骤：(1)将山地机械所行驶的基准直线分为等长的x段，设各段长度为L；(2)在平行于山地机械所行驶的基准直线外的一条直线上设置两个硬件结构完全相同的固定节点，分别固定在高于农作物高度的支架上，在山地机械的正左边靠近山地机械起始点的位置设置第一固定节点，在第一固定节点正前方设置第二固定节点，在第一固定节点上设置第一激光发射器，在第二固定节点上设置第二激光发射器，所述的两个激光发射器，其初始发射的两束激光线在俯视图上的交点刚好落在山地机械的起始点上；(3)第一激光发射器和第二激光发射器由其各自固定节点上的电机控制转动，所述的电机每次转动的角度与山地机械所行驶直线的等分线段长度L采用类正比控制方法匹配，其角速度的大小与山地机械的行驶速度采用类正比控制方法匹配，控制第一激光发射器和第二激光发射器对行驶的山地机械进行追踪，并始终保持发射的两束激光线在俯视图上的交点刚好落在山地机械所走的基准直线上的下一个等分点上；(4)在山地机械上安装一个移动节点，在移动节点上设置惯性传感器、激光测距传感器、第一激光接收器和第二激光接收器，所述的两个激光接收器在竖直方向上共线，安装高度分别与第一激光发射器和第二激光发射器相同；惯性传感器为陀螺仪和加速度计，且陀螺仪和加速度计分别对山地机械的角速度和加速度进行测量，经过计算后得到山地机械的水平航向角和垂直方向上的侧倾角测量值，各自精确度小于0.5°，垂直方向上的最大可倾斜的角度设置成固定数值，当侧倾角大于固定值时，则整个系统停止运行，预防出现交通事故，系统一旦停止运行，则需要重新启动方能再次运行；(5)山地机械行驶到等分点处，根据左右偏向检测法判断是否向左、右偏，再由航向角计算法推算出航向角的大小，航向角用α_n表示，再根据航向角的大小通过PID控制算法推算出前轮转向角的大小自动纠正偏差，实现直线行驶；(6)山地机械行驶到基准直线末端，由人工操作掉头，并根据激光测距传感器测量山地机械与固定节点的距离大小，快速地把山地机械开到两个激光接收器能同时接收到激光的位置，确定山地机械即将行驶直线的起点，实现山地机械即将行驶直线与刚行驶直线的间距保持一定值。