一种低成本车辆自动驾驶设计实现方法技术领域：本发明涉及一种车辆自动驾驶设计实现方法，尤其涉及一种用于农业机械的低成本较高精度的自动驾驶实现方法。
背景技术：随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展以及国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势。中国是传统的农业大国，但是由于种种原因，目前还依然是人工或者半机械化的方式进行作业，这种作业人工投入较大、工作效率低、工作质量差，柴油、化肥、农药和种子等资源有相当大的浪费，而且中国老龄化程度正在慢慢加剧，如果改变目前农业种植方式，最终会出现年轻人不会种地、有经验的老年人不能种地的尴尬局面。像美国、日本和澳大利亚等发达国家，精准农业早在10多年前就已经开始推广使用，其产品目前也是最先进最稳定的。由于国内精准农业属于刚起步阶段，没有一款想对稳定的产品，导致国外产品对国内市场产生垄断。
发明内容：在农业机械自动驾驶控制过程中，车体的姿态、速度和位置信息能够实时反映出车体的运动和位置信息，这些信息能够为高精度的组合导航和控制算法提供重要的数据输入。捷联惯性导航(Strapdown Inertial Navigation System，SINS)具有自主导航、保密性好、抗干扰能力强、导航参数丰富和短时间内精度高等特点被广泛应用，但是由于惯性传感器固有误差的存在，使得导航误差随着时间积累长时间导航精度较差，需要其他误差稳定的导航系统辅助，例如高精度GPS-RTK。惯性导航系统是根据测得的车体加速度，经过积分运算求得速度和位置。为此，必须知道初始速度和位置。此外，以地理坐标系为导航坐标系的惯性系统中，物理平台和数学平台都是测量加速度的基准，而且平台必须准确地对准和跟踪地理坐标系，以避免平台误差引起加速度测量误差。初始对准的精度直接关系到导航系统的工作精度，也是重要关键技术之一。如果按照上述方案实施组合导航算法则需要3个陀螺仪、3个加速度计和3个地磁传感器，即9轴IMU进行全姿态算法结算和位置速度信息组合导航结算，这种方案的优势在于能够较高频率下实时计算姿态角、位置和速度信息，能够保证较高的控制频率，具有较高的控制精度。但是，这种方案存在的缺陷在于成本太高，众所周知这种方法的关键在于陀螺仪的精度，然而陀螺仪的价格也与其精度成正比，而且由于使用环境的要求地磁传感器极易受干扰，且不能不过3D校准来修正，这样航向角精度无法得到保障。为此本发明就准对精准农业这一领域设计了一种低成本高精度的农机具自动驾驶实施方法，以待解决上述问题。本发明由卫星定位、天线位置姿态修正、安装误差标定、路径规划和自动控制等核心技术组成。如图一拖拉机和系统模型图所示，卫星天线接收卫星信号(GPS，北斗等)，经基站电台RTK技术获取厘米级位置精度，控制器内陀螺仪1和加速度进行kalman滤波融合实时计算拖拉机倾斜 角度，使用此角度和路径规划策略对位置信号进行姿态修正，并计算出横向偏差，控制器内的陀螺仪2和卫星输出的航向角进行Kalman滤波融合实时计算拖拉机的航向角，将横向偏差和航向角最为控制系统信号输入，通过前轮角度传感器构成闭环控制系统，采用自动控制算法模块与电磁阀驱动拖拉机自动行使。卫星定位和RTK技术属成熟技术不在本发明讨论范围，本发明主要讨论姿态角计算方法、位置姿态修正方法、路径规划和自动控制方法。
具体实施方式：
1、姿态角计算
全姿态角计算方法目前常才用九轴IMU(三轴陀螺仪、三轴加速度计和三轴地磁传感器)通过Kalman滤波融合实现，众所周知这种方法的关键在于陀螺仪的精度，然而陀螺仪的价格也与其精度成正比，而且由于使用环境的要求地磁传感器极易受干扰，且不能不过3D校准来修正，这样航向角精度无法得到保障。在平面内俯仰角并不会对拖拉机横向位置产生影响，固本发明舍弃全姿态计算方法，而是通过陀螺仪加加速度计和陀螺仪加卫星输出的航向角来分别计算横滚角和航向角，这样不仅可以节省一个陀螺仪、两个加速度计和三个地磁传感器的硬件成本，而且其精度也可以得到保障。
横滚角计算模型和Kalman滤波
公式1为横滚角Kalman滤波实现状态和观测方程，其中
为系统变量，θa为系统横滚角，b为陀螺仪零漂；
为系统转移矩阵；
为系统输入转移矩阵；
u＝ωgyro为系统输入，ωgyro为陀螺仪1输出的角加速度值；
为系统误差，ωg为陀螺仪1白噪声；
H＝[1 0]为观测矩阵，υ为观测白噪声；
θ＝arc cos(a1)为加速度计1计算的倾斜角，a1为加速度1输出的加速度值。
航向角计算模型和Kalman滤波
陀螺仪模型建立：
公式2为陀螺仪模型建立，其中ωT为假设的陀螺仪真实输出，ωgyro为陀螺仪当前输出，α为陀螺仪比例因子，β为陀螺仪零偏，θT为陀螺仪积分算出的真实角度，T为角度计算周期，t为时间轴，γ为T时间内陀螺仪更新次数，Δt为陀螺仪更新周期。
公式3为航向角Kalman滤波实现状态和观测方程，其中
为系统变量，ψ为系统航向角变量，α为陀螺仪比例因子，β为陀螺仪零偏；
为系统转移矩阵，T为角度计算周期，θgyro陀螺仪积分角度，Δt为陀螺仪更新周期；
ω为系统白噪声；
υ观测白噪声；
H＝[1 0 0]为观测矩阵；
ψ为GPS输出的VTG航向角。
2、位置姿态修正
为了更好的接收信号，卫星天线的安装位置应高出拖拉机一定的距离。拖拉机在不平路面行驶过程中会出现倾斜或者俯仰，它们会影响卫星的定位精度，应该予以补偿或修正。
全姿态位置修正方程为公式4，其中
为姿态修正后的卫星真实位置坐标；
为卫星板卡输出的位置坐标；
为卫星天线在拖拉机坐标系X/Y/Z坐标轴上的投影；
为姿态修正矩阵，其中ψ、θ、φ分别为航向角、横滚角和俯仰角；
假设拖拉机的坐标系X、Y和Z分别对应右前上，由于拖拉机自动控制算法实现中关注的只是横向偏差，所以航向角和俯仰角对X方向的横向偏差产生不了太大的影响，但如果要考虑全姿态修正的话，成本就会大大提高，且增加了系统计算的复杂程度，为此本发明只考虑影响横向偏差最大的因素-----横滚角位置修正，即
3、路径规划
假设拖拉机直线作业，设定其耕作宽度，那么拖拉机将按照已定的直线的平行线即可完成直线作业，如果曲线作业只是将已定的直线变为由多条直线组成的曲线，拖拉机按照平行曲线即可完成曲线作业。由数学原理可知，两点确定一条直线，即由姿态修正后的两个点的位置即可规划直线路径，也就是直线一般方程公式五的三个参数，然后通过公式六点到直线距离实时计算拖拉机当前位置的横向偏差。
Ax+By+C＝0 (公式5)
其中，A、B、C为直线系数；
(x0，y0)为当前坐标点；
d为当前坐标点到导航线的距离，即横向偏差。
4、基于纯追踪模型的直线跟踪控制算法
纯追踪算法是一种计算方法，目的是计算移动拖拉机到达指定位置所需走过的弧长。该方法具有简单、直观和容易实现的特点，其核心是确定一个合适的前视距离，该算法的描述如图2所示。
图2中的车体坐标系O′x′y′中，点P(x′，y′)为路径上的目标点，L为连接车体坐标系原点和点P的弧段弦长，即前视距离，R为该弧段半径。x′、L和R的关系式为
由公式7得
公式8中x′可视为跟踪路径上目标点再车体坐标系下的横坐标。
其中，
pe为车体质心相对于跟踪路径的横向跟踪误差，在车体前进方向偏右为正，偏左为负；
ψe为车体当前航向角度与跟踪直线目标航向角度之间的差值。
根据简化二轮车辆模型，得到车体前轮转角和转弯半径之间的关系：
δ＝arctan(E/R) (公式10)
其中，δ为前轮转角，E为车体轴距。
由公式8～10得直线跟踪条件下纯追踪模型计算的转向轮偏角控制量：
式中，E为已知，pe和ψe可依据Kalman滤波器的状态估计结算获得。
前视距离L确定方法：
在综合最佳时间响应的性能指标中，一般都考虑了误差和发生误差过程所需要的时间这两个重要因素。通常的综合性能指标包括：误差平方的积分ISE、误差绝对值的积分IAE、时间乘绝对值误差的积分ITAE、时间乘误差平方的积分ITSE等。其中，根据ITAE准则设计的系统超调量小，阻尼适中，且具有良好的选择性，故本发明采用ITAE准则。
利用公式12得到的EITAE，即可对不同前视距离条件下得到的横向跟踪误差数据进行对比分析，使该值减至最小的前视距离即为最优值。
附图说明
图1是一种低成本车辆自动驾驶系统组成示意图
图2是纯追踪模型几何解析图。