一种低成本车辆自动驾驶设计实现方法
【专利摘要】一种低成本车辆自动驾驶设计实现方法。本发明涉及一种车辆自动驾驶设计实现方法，尤其涉及一种用于农业机械的低成本较高精度的自动驾驶实现方法。本发明主要讨论姿态角计算方法、位置姿态修正方法、路径规划和自动控制方法。
【专利说明】
-种低成本车辆自动驾驶设计实现方法
技术领域 [0001] :本发明设及一种车辆自动驾驶设计实现方法，尤其设及一种用于农业 机械的低成本较高精度的自动驾驶实现方法。
【背景技术】 [0002] ：随着MEMs(Micro-Electro-Mechanical-System)传感器、导航和控制技 术的发展W及国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势。中国 是传统的农业大国，但是由于种种原因，目前还依然是人工或者半机械化的方式进行作业， 运种作业人工投入较大、工作效率低、工作质量差，柴油、化肥、农药和种子等资源有相当大 的浪费，而且中国老龄化程度正在慢慢加剧，如果改变目前农业种植方式，最终会出现年轻 人不会种地、有经验的老年人不能种地的尴尬局面。像美国、日本和澳大利亚等发达国家， 精准农业早在10多年前就已经开始推广使用，其产品目前也是最先进最稳定的。由于国内 精准农业属于刚起步阶段，没有一款想对稳定的产品，导致国外产品对国内市场产生垄断。
[0003]
【发明内容】
:在农业机械自动驾驶控制过程中，车体的姿态、速度和位置信息能够实 时反映出车体的运动和位置信息，运些信息能够为高精度的组合导航和控制算法提供重要 的数据输入。捷联惯性导航（Strapdown Inertial化Vigation System,SINS)具有自主导 航、保密性好、抗干扰能力强、导航参数丰富和短时间内精度高等特点被广泛应用，但是由 于惯性传感器固有误差的存在，使得导航误差随着时间积累长时间导航精度较差，需要其 他误差稳定的导航系统辅助，例如高精度GPS-RTK。惯性导航系统是根据测得的车体加速 度，经过积分运算求得速度和位置。为此，必须知道初始速度和位置。此外，W地理坐标系为 导航坐标系的惯性系统中，物理平台和数学平台都是测量加速度的基准，而且平台必须准 确地对准和跟踪地理坐标系，W避免平台误差引起加速度测量误差。初始对准的精度直接 关系到导航系统的工作精度，也是重要关键技术之一。如果按照上述方案实施组合导航算 法则需要3个巧螺仪、3个加速度计和3个地磁传感器，即9轴IMU进行全姿态算法结算和位置 速度信息组合导航结算，运种方案的优势在于能够较高频率下实时计算姿态角、位置和速 度信息，能够保证较高的控制频率，具有较高的控制精度。但是，运种方案存在的缺陷在于 成本太高，众所周知运种方法的关键在于巧螺仪的精度，然而巧螺仪的价格也与其精度成 正比，而且由于使用环境的要求地磁传感器极易受干扰，且不能不过3D校准来修正，运样航 向角精度无法得到保障。为此本发明就准对精准农业运一领域设计了一种低成本高精度的 农机具自动驾驶实施方法，W待解决上述问题。本发明由卫星定位、天线位置姿态修正、安 装误差标定、路径规划和自动控制等核屯、技术组成。如图一拖拉机和系统模型图所示，卫星 天线接收卫星信号(GPS，北斗等），经基站电台RTK技术获取厘米级位置精度，控制器内巧螺 仪1和加速度进行kalman滤波融合实时计算拖拉机倾斜角度，使用此角度和路径规划策略 对位置信号进行姿态修正，并计算出横向偏差，控制器内的巧螺仪2和卫星输出的航向角进 行Kalman滤波融合实时计算拖拉机的航向角，将横向偏差和航向角最为控制系统信号输 入，通过前轮角度传感器构成闭环控制系统，采用自动控制算法模块与电磁阀驱动拖拉机 自动行使。卫星定位和RTK技术属成熟技术不在本发明讨论范围，本发明主要讨论姿态角计 算方法、位置姿态修正方法、路径规划和自动控制方法。
【具体实施方式】：
[0004] 1、姿态角计算
[0005] 全姿态角计算方法目前常才用九轴IMlK =轴巧螺仪、=轴加速度计和=轴地磁传 感器)通过Kalman滤波融合实现，众所周知运种方法的关键在于巧螺仪的精度，然而巧螺仪 的价格也与其精度成正比，而且由于使用环境的要求地磁传感器极易受干扰，且不能不过 3D校准来修正，运样航向角精度无法得到保障。在平面内俯仰角并不会对拖拉机横向位置 产生影响，固本发明舍弃全姿态计算方法，而是通过巧螺仪加加速度计和巧螺仪加卫星输 出的航向角来分别计算横滚角和航向角，运样不仅可W节省一个巧螺仪、两个加速度计和 =个地磁传感器的硬件成本，而且其精度也可W得到保障。
[0006] 横滚角计算模型和Kalman滤波
[0007]
(公式 1)
[000引公式1为横滚角Kalman滤波实现状态和观测方程，其中
[0009] 綠变量，03为系统横滚角，b为巧螺仪零漂；
[0010] 为系统转移矩阵；
[0011] 発输入转移矩阵；
[0012] I统输入，Wgyr。为巧螺仪1输出的角加速度值；
[0013] I统误差，《8为巧螺仪1白噪声；
[0014] H=[l 0]为观测矩阵，U为观测白噪声；
[0015] 0=a;rc cos(ai)为加速度计1计算的倾斜角，ai为加速度1输出的加速度值。
[0016] 航向角计算模型和Kalman滤波
[0017] 巧螺仪模型建立：
[001引
（公式2)
[0019] 公式2为巧螺仪模型建立，其中COT为假设的巧螺仪真实输出，COgyr。为巧螺仪当前 输出，a为巧螺仪比例因子，e为巧螺仪零偏，为巧螺仪积分算出的真实角度，T为角度计算 周期，t为时间轴，Y为T时间内巧螺仪更新次数，A t为巧螺仪更新周期。
[0020](公式 3)
[0021] 公式3为
航向角Kalman滤波实现状态和观测方程，其中
[0022] 系统航向角变量，a为巧螺仪比例因子，e为巧螺仪零偏；
[0023] %系统转移矩阵，T为角度计算周期，0gyr。巧螺仪积分角 度，At为巧螺仪更新周期；
[0024] ?为系统白噪声；
[00剧的见测白噪声；
[00%] H= [1 0 0]为观测矩阵；
[0027] 4为61^输出的VTG航向角。
[002引 2、位置姿态修正
[0029] 为了更好的接收信号，卫星天线的安装位置应高出拖拉机一定的距离。拖拉机在 不平路面行驶过程中会出现倾斜或者俯仰，它们会影响卫星的定位精度，应该予W补偿或 修正。
[0030]
(公式 4)
[0031] 全姿态位置修正方程为公式4，其中
[0032] 为姿态修正后的卫星真实位置坐标；
[0033] 为卫星板卡输出的位置坐标；
[0034] 1星天线在拖拉机坐标系X/Y/Z坐标轴上的投影；
[0035]
%姿 态修正矩阵，其中4、0、4分别为航向角、横滚角和俯仰角；
[0036] 假设拖拉机的坐标系X、Y和Z分别对应右前上，由于拖拉机自动控制算法实现中关 注的只是横向偏差，所W航向角和俯仰角对X方向的横向偏差产生不了太大的影响，但如果 要考虑全姿态修正的话，成本就会大大提高，且增加了系统计算的复杂程度，为此本发明只 考虑影响横向偏差最大的因素--横滚角位置修正，即
[0037]
[0038] 3、路径规划
[0039] 假设拖拉机直线作业，设定其耕作宽度，那么拖拉机将按照已定的直线的平行线 即可完成直线作业，如果曲线作业只是将已定的直线变为由多条直线组成的曲线，拖拉机 按照平行曲线即可完成曲线作业。由数学原理可知，两点确定一条直线，即由姿态修正后的 两个点的位置即可规划直线路径，也就是直线一般方程公式五的=个参数，然后通过公式 六点到直线距离实时计算拖拉机当前位置的横向偏差。
[0040] Ax+By+C = 0 (公式 5)
[0041 ]
(公式 6)
[0042] 其中，A、B、C为直线系数；
[0043] (xo，yo)为当前坐标点；
[0044] d为当前坐标点到导航线的距离，即横向偏差。
[0045] 4、基于纯追踪模型的直线跟踪控制算法
[0046] 纯追踪算法是一种计算方法，目的是计算移动拖拉机到达指定位置所需走过的弧 长。该方法具有简单、直观和容易实现的特点，其核屯、是确定一个合适的前视距离，该算法 的描述如图2所示。
[0047] 图2中的车体坐标系〇/ /中，点，/)为路径上的目标点，L为连接车体坐标 系原点和点P的弧段弦长，即前视距离，R为该弧段半径。、L和R的关系式为
[004引 （公式7)
[0049]
[0化日] (公式8)
[0051 ]公式8中可视为跟踪路径上目标点再车体坐标系下的横坐标。
[0化2]
(公式9)
[0053] 其中，
[0054] Pe为车体质屯、相对于跟踪路径的横向跟踪误差，在车体前进方向偏右为正，偏左 为负；
[0055] 柏为车体当前航向角度与跟踪直线目标航向角度之间的差值。
[0056] 根据简化二轮车辆模型，得到车体前轮转角和转弯半径之间的关系：
[0化7] 5 = arctan 化/R)(公式 10)
[0化引其中，S为前轮转角，E为车体轴距。
[0059] 由公式8~10得直线跟踪条件下纯追踪模型计算的转向轮偏角控制量：
[0060]
(公式 11)
[0061] 式中，E为已知，Pe和柏可依据Kalman滤波器的状态估计结算获得。
[0062] 前视距离L确定方法：
[0063] 在综合最佳时间响应的性能指标中，一般都考虑了误差和发生误差过程所需要的 时间运两个重要因素。通常的综合性能指标包括:误差平方的积分ISE、误差绝对值的积分 IAE、时间乘绝对值误差的积分ITAE、时间乘误差平方的积分ITSE等。其中，根据ITAE准则设 计的系统超调量小，阻尼适中，且具有良好的选择性，故本发明采用ITAE准则。
[0064]
(公式口）
[0065] 利用公式12得到的EITAE，即可对不同前视距离条件下得到的横向跟踪误差数据 进行对比分析，使该值减至最小的前视距离即为最优值。 【附图说明】 图1是一种低成本车辆自动驾驶系统组成示意图 图2是纯追踪模型几何解析图。
【主权项】
1.本发明由卫星定位、天线位置姿态修正、安装误差标定、路径规划和自动控制等核心 技术组成。卫星天线接收卫星信号（GPS，北斗等），经基站电台RTK技术获取厘米级位置精 度，控制器内陀螺仪和加速度进行kalman滤波融合实时，1计算拖拉机倾斜角度，使用此角 度和路径规划策略对位置信号进行姿态修正，并计算出横向偏差。2控制器内的陀螺仪和卫 星输出的航向角进行Kalman滤波融合实时计算拖拉机的航向角。3将横向偏差和航向角最 为控制系统信号输入。4通过前轮角度传感器构成闭环控制系统，采用自动控制算法模块与 电磁阀驱动拖拉机自动行使。
【文档编号】G01C21/18GK105987696SQ201610249930
【公开日】2016年10月5日
【申请日】2016年4月20日
【发明人】王成, 张瑞
【申请人】上海雷易工业自动化有限公司