CN112214014A - 农机自动驾驶控制方法及系统 - Google Patents
农机自动驾驶控制方法及系统 Download PDFInfo
- Publication number
- CN112214014A CN112214014A CN202010920453.1A CN202010920453A CN112214014A CN 112214014 A CN112214014 A CN 112214014A CN 202010920453 A CN202010920453 A CN 202010920453A CN 112214014 A CN112214014 A CN 112214014A
- Authority
- CN
- China
- Prior art keywords
- information
- agricultural machine
- agricultural
- agricultural machinery
- positioning information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000001914 filtration Methods 0.000 claims abstract description 19
- 230000008859 change Effects 0.000 claims abstract description 13
- 238000004422 calculation algorithm Methods 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000003062 neural network model Methods 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 8
- 230000004927 fusion Effects 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 4
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 230000004069 differentiation Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 2
- 230000008569 process Effects 0.000 abstract description 11
- 238000010586 diagram Methods 0.000 description 17
- 238000003860 storage Methods 0.000 description 12
- 238000001514 detection method Methods 0.000 description 10
- 238000004590 computer program Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 238000004891 communication Methods 0.000 description 5
- 230000003287 optical effect Effects 0.000 description 4
- 238000005259 measurement Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 239000004065 semiconductor Substances 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000012271 agricultural production Methods 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 239000002689 soil Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Guiding Agricultural Machines (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种农机自动驾驶方法和系统，该方法包括：分别从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取农机的第二定位信息；对所述第一定位信息和所述第二定位信息进行滤波和求和处理，确定农机位姿信息；基于农机目标位置方位信息、变换后的农机当前位姿信息和所述农机导向轮的转角值，计算控制信息并发送至电液比例换向阀，以使得所述电液比例换向阀驱动农机液压导向轮改变转角值，以对所述农机进行自动驾驶控制。该方法能够将速度因素考虑到路径跟踪控制过程中，有效提高农机导航自动驾驶系统的抗干扰能力和鲁棒性，进一步提高了土地利用率和作业效率。
Description
技术领域
本发明涉及农机导航技术领域，尤其涉及一种农机自动驾驶控制方法及系统。
背景技术
随着农业现代化的快速发展，土地规模化经营逐步成型，对智能化农机装备的需求也愈加迫切，其中，农机导航自动驾驶系统是智能化农机装备的重要组成部分，是实现精准农业操作的前提与关键，其能够根据各种导航传感器信息，引导农机按照预定的作业路线实现精确跟踪行驶作业。为了大幅度提高农业作业质量和土地利用率，对农机导航自动驾驶控制的研究显得尤为重要。
目前，农机导航自动驾驶关键在于农机导航定位和路径规划，相关技术的农机导航定位大多采用组合导航定位的方式，即将两种或两种以上不同导航系统以适当的方式组合起来，并采用信息融合算法得到定位信息，例如将全球定位系统(GPS)和捷联惯性导航系统(SINS)相融合，并采用基于动力学模型的农机跟踪控制算法对农机进行跟踪控制。
然而，农机属于低速车辆，相关技术中在路径跟踪控制过程中未考虑速度因素的影响，且大部分研究在建模或者算法设计时，将速度作为定值，很少探究速度在不同范围或不稳定状态下对路径跟踪控制的影响，并且基于运动学模型的路径跟踪算法受车辆动力学特征变化影响较大，导致农机路径跟踪控制准确度较低，且稳定性较差。
发明内容
有鉴于此，本发明提供一种农机自动驾驶控制方法及系统，至少部分解决现有技术中存在的问题。
第一方面，本申请提供了一种农机自动驾驶控制方法，该方法包括：
分别从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取农机的第二定位信息；
对第一定位信息和第二定位信息进行滤波和求和处理，确定农机位姿信息；
基于农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息并发送至电液比例换向阀，以使得电液比例换向阀驱动农机液压导向轮改变转角值，以对农机进行自动驾驶控制。
在其中一个实施例中，分别从捷联惯性导航系统获取农机的第一定位信息，包括：
获取农机的加速度信息和角速度信息；
根据加速度信息和所述角速度信息进行导航解算得到农机的第一定位信息。
在其中一个实施例中，从北斗卫星导航系统获取农机的第二定位信息，包括：
获取农机的第一经纬度信息；
判断北斗卫星导航系统信号是否正常；
当信号正常时，对第一经纬度信息进行导航解算得到第二定位信息；
当信号失效时，将第一定位信息输入预先构建的神经网络模型，得到第二经纬度信息；
对第二经纬度信息进行导航解算得到农机的第二定位信息。
在其中一个实施例中，对第一定位信息和第二定位信息进行滤波和求和处理，确定农机位姿信息，包括：
采用滤波算法对第一定位信息和第二定位信息进行滤波处理，得到误差估计值；
将误差估计值与第一定位信息求和处理，确定农机位姿信息。
在其中一个实施例中，基于农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息，包括：
从农机位姿信息中确定天线点坐标位置；
将天线点坐标位置变换处理为农具中心坐标位置，确定农机的作业线；
将农具中心坐标位置按照作业线方向进行平移和旋转变换，确定变换后的农机当前位姿信息；
基于作业线所确定的农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息。
在其中一个实施例中，变换后的农机当前位姿信息包括农机的实时方位和实时位置坐标，基于农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息，包括：
基于农机实时位置坐标、农机实时方位和根据作业线确定的农机目标位置坐标、农机目标方位计算农机的位置偏差和方位偏差；
将农机的位置偏差和方位偏差采用PID、预测与模糊融合控制算法确定农机的导向轮目标转角；
从角度传感器获取农机的导向轮实时转角；
基于导向轮目标转角和导向轮实时转角，计算转角偏差；
采用PID控制算法对转角偏差进行计算确定控制信息。
第二方面，本申请提供了一种农机自动驾驶控制系统，该系统：控制器、角度传感器、北斗卫星导航系统、捷联惯性导航系统、电液比例换向阀，所述控制器分别与角度传感器、北斗卫星导航系统、捷联惯性导航系统、所述电液比例换向阀电连接；
捷联惯性导航系统用于检测农机的第一定位信息；北斗卫星导航系统用于检测农机的第二定位信息；角度传感器用于检测农机导向轮的转角值；控制器用于将北斗卫星导航系统发送的第二定位信息和捷联惯性导航系统发送的第一定位信息进行融合处理，确定农机位姿信息，并根据农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息并发送至电液比例换向阀；电液比例换向阀用于根据控制信息，驱动农机液压导向轮改变转角值，以对农机进行自动驾驶控制。
本申请实施例提供的农机自动驾驶控制方法和系统，分别从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取农机的第二定位信息，对第一定位信息和第二定位信息进行滤波和求和处理，确定农机位姿信息，基于农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息并发送至电液比例换向阀，以对农机进行自动驾驶控制。该方案能够在从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取第二定位信息时，将农机的速度因素考虑到路径跟踪控制过程中，并通过对第一定位信息和第二定位信息进行滤波和求和处理，使得确定农机的定位信息更加准确，进而结合农机的当前定位信息和导向轮的转角值，使得精准的确定出控制信息，实现对农机精确地自动驾驶控制，很大程度上提高了导航自动驾驶系统的抗干扰能力和鲁棒性，进一步提高了土地利用率和作业效率。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍。显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1为本申请实施例提供的农机自动驾驶控制方法的流程示意图；
图2为本申请实施例提供的农机自动驾驶控制方法的流程示意图；
图3为本申请实施例提供的农机自动驾驶控制系统的结构示意图；
图4为本申请实施例提供的农机自动驾驶控制系统的结构示意图；
图5为本申请实施例提供的农机自动驾驶控制系统的结构示意图；
图6为本申请实施例提供的检测点与对应的AB线距离的关系示意图；
图7为本申请实施例提供的检测点与对应的AB线距离的关系示意图；
图8为本申请实施例提供的检测点序号和轨迹线1与轨迹线2相对间距的关系示意图；
图9为本申请实施例提供的检测点序号和轨迹线1与轨迹线2相对间距的关系示意图；
图10为本申请实施例提供的一种计算机设备的结构示意图。
附图标记说明：
101-控制器；102-角度传感器；103-北斗卫星导航系统；104-捷联惯性导航系统；105-电液比例换向阀；106-液压转向器；107-导向轮；108-显示模块；1031-基站；1032-车载系统；1033-基站测量天线；1034-基站接收机；1035-基站发送电台；1036-基站发送电台天线；1037-车载接收电台天线；1038-车载接收电台；1039-车载接收机；1041-三轴加速度计；1042-三轴陀螺仪；1043-三轴磁力计；1044-车载测量主天线；1045-车载测量副天线。
具体实施方式
下面结合附图对本发明实施例进行详细描述。
需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合；并且，基于本公开中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。
需要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本公开，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。
可以理解，相关技术的农机导航定位大多数采用组合导航定位的方式，将两种或两种以上的不同导航系统以适当的方式组合起来，但是很少采用北斗卫星导航系统，并采用基于动力学模型的农机跟踪控制算法对农机进行跟踪控制。然而，农机属于低速车辆，相关技术中在路径跟踪控制过程中未考虑速度因素的影响，且大部分研究在建模或算法设计时，都将速度考虑为定值，很少探究速度在不同范围或不稳定状态下对路径跟踪控制的影响，并且基于运动学模型的路径跟踪算法受车辆动力学特征变化影响较大，导致农机路径跟踪控制准确度较低，且稳定性较差。
基于上述缺陷，本申请提供了一种农机自动驾驶控制方法，与相关技术相比，该方案能够在从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取第二定位信息时，将农机的速度因素考虑到路径跟踪控制过程中，并通过对第一定位信息和第二定位信息进行滤波和求和处理，使得确定的农机位姿信息更加准确，进而结合农机目标位置方位信息、变换后的农机当前位姿信息和导向轮的转角值，使得精准的确定出控制信息，实现对农机精确地自动驾驶控制，很大程度上提高了导航自动驾驶系统的抗干扰能力和鲁棒性，进一步提高了土地利用率和作业效率。
图1所示为本申请实施例提供的农机自动驾驶方法的流程示意图，该方法应用于农机自动驾驶装置，该装置可以是终端设备，如图1所示，该方法包括：
S101、分别从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取农机的第二定位信息。
具体的，上述捷联惯性导航系统(Strap-down Inertial Navigation System，SINS)是把惯性仪表直接固连在载体上，用计算机来完成导航平台功能的惯性导航系统，是一种不依赖于外部信息，也不向外部辐射能量的自主式导航系统。
北斗卫星导航系统：(BeiDou Navigation Satellite System，BDS)由空间段、地面段和用户段三部分组成，可在全球范围内全天候、全天时为各类用户提供高精度、高可靠定位、导航、授时服务，具短报文通信能力，并具备区域导航、定位和授时能力，定位精度为分米、厘米级别。
在从SINS中获取第一定位信息时，可以先获取农机的加速度和角速度信息，然后根据农机的加速度和角速度进行导航解算，得到农机的第一定位信息，该第一定位信息为从SINS中获取的农机的定位信息，包括农机位置、速度和姿态，其中，农机的姿态可以包括农机的方位角、横滚角和俯仰角等。
从SINS中获取农机的第一定位信息后，可以通过BDS接收到农机的第一经纬度信息，然后判断BDS信号是否正常，当信号正常时，对第一经纬度信息进行导航解算，得到农机的第二定位信息，该第二定位信息为从BDS中获取的农机的定位信息，包括农机位置、速度和姿态。
将农机的第一定位信息作为模型输入，第二经纬度信息作为模型的输出，构建神经网络模型，该第一定位信息包括农机的方位角、横滚角和俯仰角，第二经纬度信息包括农机的经度增量和纬度增量，该神经网络模型可以是非线性自适应回归NARX神经网络模型。其中，在BDS信号处于正常情况下，构建的NARX神经网络模型通过SINS和BDS的定位数据更新训练模型参数，当BDS信号处于异常情况下，NARX神经网络模型根据SINS和BDS的历史高精度数据，通过代入在BDS正常情况下训练的模型参数，通过该NARX神经网络模型计算得到第二经纬度信息，该第二经纬度信息为通过预先构建的神经网络模型输出的经度增量和纬度增量，然后获取农机上一时刻的经纬度信息，将第二经纬度信息与上一时刻的经纬度信息进行求和处理，得到农机当前时刻的经纬度信息，对当前时刻的经纬度信息进行导航解算，从而得到农机的第二定位信息。
S102、对第一定位信息和第二定位信息进行滤波和求和处理，确定农机位姿信息。
本步骤中，在从SINS获取农机的第一定位信息和从BDS获取农机的第二定位信息后，采用滤波算法对第一定位信息和第二定位信息进行滤波处理，得到误差估计值，并将误差估计值与第一定位信息进行求和处理，从而确定出农机位姿信息。
可选的，可以采用高斯-克吕格投影方法将农机定位信息换算为主天线点的高斯平面坐标，该高斯平面坐标为农机位姿信息。还可以采用扩展卡尔曼滤波器对第一定位信息和第二定位信息进行滤波处理，得到误差估计值，然后将误差估计值与第一定位信息求和处理，得到农机位姿信息。该农机位姿信息为高精度的农机位置、速度和姿态信息。
S103、基于农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息并发送至电液比例换向阀，以使得电液比例换向阀驱动农机液压导向轮改变转角值，以对农机进行自动驾驶控制。
具体的，在确定农机作业线的基础上，获得农机目标位置方位信息和变换后的农机当前位姿信息，再通过角度传感器获取农机导向轮的转角值，从而计算出控制信息。
可选的，作为一种实施方式，图2为本申请实施例提供的计算控制信息方法的流程示意图，如图2所示，该方法包括：
S201、从农机位姿信息中确定天线点坐标位置。
S202、将天线点坐标位置变换处理为农具中心坐标位置，确定农机的作业线。
S203、将农具中心坐标位置按照作业线方向进行平移和旋转变换，确定变换后的农机当前位姿信息。
S204、基于作业线确定的农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息。
具体的，可以根据主天线与农具底部中心的几何位置，以及农机姿态信息，将一主天线点高斯平面坐标表示的农机位置变换为农具底部中心高斯平面坐标表示的农机位置，该农机位置为农机中心坐标位置，然后确定农机的作业线，该作业线为AB线，并将高斯平面坐标系原点平移至A点，再旋转至AB线方向，从而得到农机的当前位姿信息，即得到农机在自动驾驶控制中可以直接应用的新平面坐标系和农机位置姿态坐标。
上述农机的当前位姿信息包括农机的实时方位和实时位置坐标，可以基于农机实时位置坐标、农机实时方位和根据作业线确定的农机目标位置坐标、农机目标方位计算农机的位置偏差和方位偏差，并将农机的位置偏差和方位偏差采用PID、预测与模糊融合控制算法确定农机的导向轮目标转角，然后从角度传感器获取农机的导向轮实时转角，基于农机的导向轮目标转角和导向轮实时转角，计算转角偏差，并采用PID控制算法对转角偏差进行计算处理，得到控制信息，并将控制信息发送至电液比例换向阀，使得电液比例换向阀驱动农机液压导向轮改变转角值，以对农机进行自动驾驶控制。
在进行农机自动驾驶控制中，对农机路径跟踪控制的目标为：农机的位置偏差和方位偏差均为零，即农机位置处于作业线上且农机的行驶方向与作业线方向保持一致。其中，位置偏差可以经过变换处理为方位偏差，因此，农机路径跟踪控制目标可以表示为农机的方位偏差为零。
农机在任意一个采集或控制周期内，能够通过农机方位偏差近似预测出所需的导向轮目标转角αr。根据导向轮目标转角和角度传感器测得的导向轮实时转角计算转角偏差，然后对转角偏差进行运算得到最终控制信号u。综合考虑上述内容，构建PID形式的控制规律，表述为：
u＝KP1*(α-αr)
上述公式中，L为农机的轴距；
为农机的实时方位角；
为农机的目标方位角；
为农机的方位角增量；y为农机位置的横向坐标；V为农机瞬时速度；T为周期；KP1、KP2、KA、KB、KD为控制参数。
在实际作业中，综合考虑田块土质状况、农机自身特性与比例换向阀性能等因素，对控制参数KP1、KP2、KA、KB、KD进行整定，使控制精度达到作业要求。
请参见图3所示，从SINS获取农机的加速度信息和角速度信息，然后将加速度和角速度进行导航解算，得到农机的第一定位信息，该第一定位信息包括农机位置、速度和姿态，并通过BDS接收机接收BDS发送的第一经纬度信息，然后判断BDS信号是否处于正常状态，当BDS信号处于正常状态时，将第一经纬度信息进行导航解算，得到农机的第二定位信息，该第二定位信息包括农机位置、速度和姿态，当BDS信号处于异常状态，即信号失效时，通过将第一定位信息输入预先构建的NARX神经网络模型中，从而输出第二经纬度信息，然后获取农机上一时刻的经纬度信息，将第二经纬度信息与上一时刻的经纬度信息进行求和处理，得到农机当前时刻的经纬度信息，对当前时刻的经纬度信息进行导航解算，从而得到农机的第二定位信息。
在获取到第一定位信息和第二定位信息后，采用扩展卡尔曼滤波器对第一定位信息和第二定位信息进行滤波处理，得到误差估计值，然后将误差估计值与第一定位信息求和处理，得到农机位姿信息。从农机位姿信息中确定天线点坐标，将天线点坐标换算处理为农具中心坐标，并确定农机作业线(AB线)，按照作业线方向，对农具中心坐标系进行平移和旋转，即将高斯平面坐标系原点平移至A点，再旋转至AB线方向，从而得到变换后的农机当前位姿信息。该农机的当前位姿信息包括农机的实时方位和实时位置，基于农机实时位置坐标、农机实时方位和根据作业线确定的农机目标位置坐标、农机目标方位计算农机的位置偏差和方位偏差，并将农机的位置偏差和方位偏差采用PID、预测与模糊融合控制算法确定农机的导向轮目标转角，然后从角度传感器获取农机的导向轮实时转角，基于农机的导向轮目标转角和导向轮实时转角，计算转角偏差，并采用PID控制算法对转角偏差进行计算处理，得到控制信息，并将控制信息发送至电液比例换向阀，使得电液比例换向阀驱动农机液压导向轮改变转角值，以对农机进行自动驾驶控制。
本申请实施例提供的农机自动驾驶控制方法，通过分别从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取农机的第二定位信息，并对第一定位信息和所述第二定位信息进行滤波和求和处理，确定农机位姿信息，然后基于农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息并发送至电液比例换向阀，以使得电液比例换向阀驱动农机液压导向轮改变转角值，以对农机进行自动驾驶控制。该方案能够在从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取第二定位信息时，将农机的速度因素考虑到路径跟踪控制过程中，并通过对第一定位信息和第二定位信息进行滤波和求和处理，使得确定农机的位姿信息更加准确，进而结合农机目标位置方位信息、变换后的农机当前位姿信息和导向轮的转角值，使得精准的确定出控制信息，实现对农机精确地自动驾驶控制，很大程度上提高了导航自动驾驶系统的抗干扰能力和鲁棒性，进一步提高了土地利用率和作业效率。
应当注意，尽管在附图中以特定顺序描述了本发明方法的操作，但是，这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。相反，流程图中描绘的步骤可以改变执行顺序。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。
另一方面，图4为本申请实施例提供的农机自动驾驶控制系统的结构示意图。该系统包括：控制器101、角度传感器102、北斗卫星导航系统103、捷联惯性导航系统104、电液比例换向阀105，控制器101分别与角度传感器102、北斗卫星导航系统103、捷联惯性导航系统104、电液比例换向阀105电连接；电液比例换向阀105通过液压转向器106与导向轮107的一端连接，导向轮107的另一端与角度传感器102连接。
该捷联惯性导航系统104用于检测农机的第一定位信息；北斗卫星导航系统103用于检测农机的第二定位信息；角度传感器102用于检测农机导向轮的转角值；控制器101用于将北斗卫星导航系统103发送的第二定位信息和捷联惯性导航系统104发送的第一定位信息进行滤波和求和处理，确定农机位姿信息，并根据农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息并发送至电液比例换向阀105；电液比例换向阀105用于根据控制信息，控制液压转向器106驱动农机液压导向轮107改变转角值，以对农机进行自动驾驶控制。
进一步地，请参见图5所示，北斗卫星导航系统103包括基站1031和车载系统1032，该基站1031包括基站测量天线1033、与基站测量天线1033连接的基站接收机1034、与基站接收机1034连接的基站发送电台1035、与基站发送电台1035连接的基站发送电台天线1036；车载系统1032包括车载接收电台天线1037、与车载接收电台天线1037连接的车载接收电台1038、与车载接收电台1038连接的车载接收机1039、与车载接收机1039连接的车载测量主天线1044、与车载接收机1039连接的车载测量副天线1045。
基站发送电台1035通过基站发送电台天线1036发送农机的实时动态位置信息；车载接收电台1038接收基站发送电台1035发送的农机的实时动态位置信息，车载接收机1039基于实时动态位置信息、通过车载测量主天线1044、车载测量副天线1045得到农机的第二定位信息。
捷联惯性导航系统104包括三轴加速度计1041、三轴陀螺仪1042、三轴磁力计1043，捷联惯性导航系统104通过三轴加速度计1041、三轴陀螺仪1042、三轴磁力计1043得到农机的第一定位信息。
进一步地，请继续参见图4所示，该系统还包括显示模块108，显示模块108与控制器101电连接；该显示模块108用于显示农机运行状态，标定农机结构参数，记录跟踪控制参数。
可选的，该显示模块可以是液晶显示屏。
本实施例提供的农机自动驾驶控制系统，能够在从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取第二定位信息时，将农机的速度因素考虑到路径跟踪控制过程中，并通过对第一定位信息和第二定位信息进行滤波和求和处理，使得确定农机的位姿信息更加准确，进而结合农机目标位置方位信息、变换后的农机当前位姿信息和导向轮的转角值，使得精准的确定出控制信息，实现对农机精确地自动驾驶控制，很大程度上提高了导航自动驾驶系统的抗干扰能力和鲁棒性，进一步提高了土地利用率和作业效率。
经过试验，当基站至移动接收机距离为5km，农机的工作速度分别为1.8km/h和9km/h，各检测点与农机的AB线的距离为100cm，检测点为50个，可以得到如下表一和表二数据以及图6-图7所示的示意图,该数据包括农机的直线度精度S，农机的直线度精度可以通过如下公式计算：
其中，xi为各检测点与AB线实测距离；N为检测点数量；
为农机的所有实测距离平均值。
表一：
表二：
当基站至移动接收机距离为5km，农机的工作速度分别为1.8km/h和9km/h，衔接行间距理论距离为100cm，检测点为50个，可以得到如下表三和表四数据以及图8-图9所示的示意图,该数据包括衔接行间距精度S′，该衔接行间距精度可以通过如下公式计算：
其中，hi为轨迹线1与轨迹线2的相对间距；N为检测点数量；
为衔接行间距平均值。
表三：
表四：
可以发现，本申请提供的农机自动驾驶控制方法和系统，通过大量田间测试后，能够实现对农机驾驶的精确控制，直线作业横向偏差和交接行误差均不大于2.5cm，停车“起步弯”误差不大于5cm，作业速度范围为0.6～15km/h，有效提高了土地利用率和作业效率，能够在农业生产领域进行更好的应用推广。
下面参考图10，图10为本申请实施例的终端设备或服务器的计算机系统的结构示意图。
如图10所示，计算机系统1000包括中央处理单元(CPU)1001，其可以根据存储在只读存储器(ROM)1002中的程序或者从存储部分1008加载到随机访问存储器(RAM)1003中的程序而执行各种适当的动作和处理。在RAM1003中，还存储有系统1000操作所需的各种程序和数据。CPU1001、ROM1002以及RAM1003通过总线1004彼此相连。输入/输出(I/O)接口1005也连接至总线1004。
以下部件连接至I/O接口1005：包括键盘、鼠标等的输入部分1006；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分1007；包括硬盘等的存储部分1008；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分1009。通信部分1009经由诸如因特网的网络执行通信处理。驱动器1010也根据需要连接至I/O接口1005。可拆卸介质1011，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器1010上，以便于从其上读出的计算机程序根据需要被安装入存储部分1008。
特别地，根据本申请的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本申请的实施例包括一种计算机程序产品，其包括承载在机器可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分1009从网络上被下载和安装，和/或从可拆卸介质1011被安装。在该计算机程序被中央处理单元(CPU)1001执行时，执行本申请的系统中限定的上述功能。
需要说明的是，本申请所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本申请各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，前述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本申请实施例中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中。
作为另一方面，本申请还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施例中描述的电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中的。上述计算机可读存储介质存储有一个或者多个程序，当上述前述程序被一个或者一个以上的处理器用来执行描述于本申请的农机自动驾驶控制方法：
分别从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取农机的第二定位信息；
对所述第一定位信息和所述第二定位信息进行滤波和求和处理，确定农机位姿信息；
基于所述农机目标位置方位信息、变换后的农机当前位姿信息和所述农机导向轮的转角值，计算控制信息并发送至电液比例换向阀，以使得所述电液比例换向阀驱动农机液压导向轮改变转角值，以对所述农机进行自动驾驶控制。
综上所述，本申请实施例提供的农机自动驾驶控制方法和系统，分别从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取农机的第二定位信息，对第一定位信息和第二定位信息进行滤波和求和处理，确定农机位姿信息，基于农机目标位置方位信息、变换后的农机当前位姿信息和农机导向轮的转角值，计算控制信息并发送至电液比例换向阀，以对农机进行自动驾驶控制。该方案能够在从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取第二定位信息时，将农机的速度因素考虑到路径跟踪控制过程中，并通过对第一定位信息和第二定位信息进行滤波和求和处理，使得确定农机的位姿信息更加准确，进而结合农机目标位置方位信息、变换后的农机当前位姿信息和导向轮的转角值，使得精准的确定出控制信息，实现对农机精确地自动驾驶控制，很大程度上提高了导航自动驾驶系统的抗干扰能力和鲁棒性，进一步提高了土地利用率和作业效率。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。
Claims (10)
1.一种农机自动驾驶控制方法，其特征在于，该方法包括：
分别从捷联惯性导航系统获取农机的第一定位信息和从北斗卫星导航系统获取农机的第二定位信息；
对所述第一定位信息和所述第二定位信息进行滤波和求和处理，确定农机位姿信息；
基于所述农机目标位置方位信息、变换后的农机当前位姿信息和所述农机导向轮的转角值，计算控制信息并发送至电液比例换向阀，以使得所述电液比例换向阀驱动农机液压导向轮改变转角值，以对所述农机进行自动驾驶控制。
2.根据权利要求1所述的农机自动驾驶方法，其特征在于，从捷联惯性导航系统获取农机的第一定位信息，包括：
获取所述农机的加速度信息和角速度信息；
根据所述加速度信息和所述角速度信息进行导航解算，得到所述农机的第一定位信息。
3.根据权利要求1所述的农机自动驾驶方法，其特征在于，从北斗卫星导航系统获取农机的第二定位信息，包括：
获取所述农机的第一经纬度信息；
判断所述北斗卫星导航系统信号是否正常；
当所述信号处于正常状态时，对所述第一经纬度信息进行导航解算，得到所述农机的第二定位信息；
当所述信号处于异常状态时，将所述第一定位信息输入预先构建的神经网络模型，并对输出得到的第二经纬度信息进行导航解算得到所述农机的第二定位信息。
4.根据权利要求1所述的农机自动驾驶方法，其特征在于，对所述第一定位信息和所述第二定位信息进行滤波和求和处理，确定农机位姿信息，包括：
采用滤波算法对所述第一定位信息和所述第二定位信息进行滤波处理，得到误差估计值；
将所述误差估计值与所述第一定位信息求和处理，确定农机位姿信息。
5.根据权利要求1所述的农机自动驾驶方法，其特征在于，基于所述农机目标位置方位信息、变换后的农机当前位姿信息和所述农机导向轮的转角值，计算控制信息，包括：
从所述农机位姿信息中确定天线点坐标位置；
将所述天线点坐标位置变换处理为农具中心坐标位置，确定农机的作业线；
将所述农具中心坐标位置按照所述作业线方向进行平移和旋转变换，确定变换后的农机当前位姿信息；
基于所述作业线确定的农机目标位置方位信息、所述变换后的农机当前位姿信息和所述农机导向轮的转角值，计算控制信息。
6.根据权利要求5所述的农机自动驾驶方法，其特征在于，所述变换后的农机当前位姿信息包括农机的实时方位和实时位置坐标，基于所述农机目标位置方位信息、变换后的农机当前位姿信息和所述农机导向轮的转角值，计算控制信息，包括：
基于农机实时位置坐标、农机实时方位和根据作业线确定的农机目标位置坐标、农机目标方位计算农机的位置偏差和方位偏差；
将所述农机的位置偏差和方位偏差采用PID、预测与模糊融合控制算法确定所述农机的导向轮目标转角；
从所述角度传感器获取所述农机的导向轮实时转角；
基于所述导向轮目标转角和所述导向轮实时转角，计算转角偏差；
采用PID控制算法对所述转角偏差进行计算处理，得到控制信息。
7.一种农机自动驾驶控制系统，其特征在于，该系统包括：控制器、角度传感器、北斗卫星导航系统、捷联惯性导航系统、电液比例换向阀，所述控制器分别与所述角度传感器、所述北斗卫星导航系统、所述捷联惯性导航系统、所述电液比例换向阀电连接；所述电液比例换向阀通过液压转向器与导向轮的一端连接，所述导向轮的另一端与所述角度传感器连接；
所述捷联惯性导航系统用于检测所述农机的第一定位信息；所述北斗卫星导航系统用于检测农机的第二定位信息；所述角度传感器用于检测农机导向轮的转角值；所述控制器用于将所述北斗卫星导航系统发送的第二定位信息和所述捷联惯性导航系统发送的第一定位信息进行滤波和求和处理，确定农机位姿信息，并根据所述农机目标位置方位信息、变换后的农机当前位姿信息和所述农机导向轮的转角值，计算控制信息并发送至所述电液比例换向阀；所述电液比例换向阀用于根据所述控制信息，控制液压转向器驱动所述农机液压导向轮改变转角值，以对所述农机进行自动驾驶控制。
8.根据权利要求7所述的系统，其特征在于，所述北斗卫星导航系统包括基站和车载系统，所述基站包括基站测量天线、与所述基站测量天线连接的基站接收机、与所述基站接收机连接的基站发送电台、与所述基站发送电台连接的基站发送电台天线；所述车载系统包括车载接收电台天线、与所述车载接收电台天线连接的车载接收电台、与所述车载接收电台连接的车载接收机、与所述车载接收机连接的车载测量主天线、与所述车载接收机连接的车载测量副天线；
所述基站发送电台通过所述基站发送电台天线发送农机的实时动态位置信息；所述车载接收电台接收所述基站发送电台发送的所述农机的实时动态位置信息，所述车载接收机基于所述实时动态位置信息、通过所述车载测量主天线、所述车载测量副天线得到所述农机的第二定位信息。
9.根据权利要求7所述的系统，其特征在于，所述捷联惯性导航系统包括三轴加速度计、三轴陀螺仪、三轴磁力计，所述捷联惯性导航系统通过所述三轴加速度计、所述三轴陀螺仪、所述三轴磁力计得到所述农机的第一定位信息。
10.根据权利要求7所述的系统，其特征在于，该系统还包括显示模块，所述显示模块与所述控制器电连接；所述显示模块用于显示农机运行状态，标定农机结构参数，记录跟踪控制参数。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010920453.1A CN112214014A (zh) | 2020-09-04 | 2020-09-04 | 农机自动驾驶控制方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010920453.1A CN112214014A (zh) | 2020-09-04 | 2020-09-04 | 农机自动驾驶控制方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112214014A true CN112214014A (zh) | 2021-01-12 |
Family
ID=74048826
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010920453.1A Pending CN112214014A (zh) | 2020-09-04 | 2020-09-04 | 农机自动驾驶控制方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112214014A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117075485A (zh) * | 2023-10-18 | 2023-11-17 | 舜泰汽车有限公司 | 一种农机自动驾驶导航设备 |
| CN117115759A (zh) * | 2023-04-12 | 2023-11-24 | 盐城工学院 | 一种基于类别引导的路侧交通目标检测系统及方法 |
| CN117572477A (zh) * | 2023-11-16 | 2024-02-20 | 深圳浪尖智造新技术有限公司 | 农机自主导航方法 |
| CN117723091A (zh) * | 2024-02-07 | 2024-03-19 | 农业农村部南京农业机械化研究所 | 一种农机的停机起步误差检测方法及装置 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101833334A (zh) * | 2010-02-09 | 2010-09-15 | 北京农业信息技术研究中心 | 拖拉机自动导航控制系统及其方法 |
| CN107643088A (zh) * | 2017-08-10 | 2018-01-30 | 中国科学院深圳先进技术研究院 | 无人机导航方法、装置、无人机及存储介质 |
| CN107942663A (zh) * | 2017-11-21 | 2018-04-20 | 山东省计算中心（国家超级计算济南中心） | 基于模糊pid算法的农机自动转向控制方法 |
| CN109291989A (zh) * | 2018-10-24 | 2019-02-01 | 山东农业大学 | 一种用于农业机械的电动方向盘及自动驾驶方法 |
| AU2020101525A4 (en) * | 2020-07-28 | 2020-09-03 | Xi`an University of Architecture and Technology | Inertial basis combined navigation method for indirect grid frame in polar zone |
-
2020
- 2020-09-04 CN CN202010920453.1A patent/CN112214014A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101833334A (zh) * | 2010-02-09 | 2010-09-15 | 北京农业信息技术研究中心 | 拖拉机自动导航控制系统及其方法 |
| CN107643088A (zh) * | 2017-08-10 | 2018-01-30 | 中国科学院深圳先进技术研究院 | 无人机导航方法、装置、无人机及存储介质 |
| CN107942663A (zh) * | 2017-11-21 | 2018-04-20 | 山东省计算中心（国家超级计算济南中心） | 基于模糊pid算法的农机自动转向控制方法 |
| CN109291989A (zh) * | 2018-10-24 | 2019-02-01 | 山东农业大学 | 一种用于农业机械的电动方向盘及自动驾驶方法 |
| AU2020101525A4 (en) * | 2020-07-28 | 2020-09-03 | Xi`an University of Architecture and Technology | Inertial basis combined navigation method for indirect grid frame in polar zone |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117115759A (zh) * | 2023-04-12 | 2023-11-24 | 盐城工学院 | 一种基于类别引导的路侧交通目标检测系统及方法 |
| CN117115759B (zh) * | 2023-04-12 | 2024-04-09 | 盐城工学院 | 一种基于类别引导的路侧交通目标检测系统及方法 |
| CN117075485A (zh) * | 2023-10-18 | 2023-11-17 | 舜泰汽车有限公司 | 一种农机自动驾驶导航设备 |
| CN117572477A (zh) * | 2023-11-16 | 2024-02-20 | 深圳浪尖智造新技术有限公司 | 农机自主导航方法 |
| CN117723091A (zh) * | 2024-02-07 | 2024-03-19 | 农业农村部南京农业机械化研究所 | 一种农机的停机起步误差检测方法及装置 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112214014A (zh) | 农机自动驾驶控制方法及系统 | |
| CN107132563B (zh) | 一种里程计结合双天线差分gnss的组合导航方法 | |
| CN111142091B (zh) | 一种融合车载信息的自动驾驶系统激光雷达在线标定方法 | |
| CN111536972B (zh) | 一种基于里程计刻度系数修正的车载dr导航方法 | |
| CN101201255A (zh) | 基于智能导航算法的车辆组合导航系统 | |
| CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
| CN102680002B (zh) | 汽车用微机械陀螺零点电压的在线标定方法 | |
| CN114179825B (zh) | 多传感器融合获取量测值置信度方法及自动驾驶车辆 | |
| CN110940344B (zh) | 一种用于自动驾驶的低成本传感器组合定位方法 | |
| CN108036792A (zh) | 一种用于移动机器人的里程计与测量位姿的数据融合方法 | |
| CN112596089B (zh) | 融合定位方法、装置、电子设备及存储介质 | |
| US11946746B2 (en) | Method for satellite-based detection of a vehicle location by means of a motion and location sensor | |
| CN106525043A (zh) | 一种智能路线规划方法 | |
| CN109656240A (zh) | 一种车辆路径跟踪控制装置、方法和车辆 | |
| EP4345421A2 (en) | Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle | |
| CN115166791A (zh) | 一种智能驾驶车辆双gnss天线航向角标定方法及装置 | |
| CN105093249A (zh) | 一种惯导导航装置 | |
| CN110057356A (zh) | 一种隧道内车辆定位方法及装置 | |
| CN111912403B (zh) | 一种叉车的定位方法及叉车 | |
| CN116974285A (zh) | 一种无人农机路径跟随控制方法、装置与电子设备 | |
| CN107588773B (zh) | 一种基于航位推算与测距信息的多节点协同导航方法 | |
| CN112611377A (zh) | 一种用于小车室外导航的状态预测方法、装置及存储介质 | |
| CN113900132B (zh) | 定位方法、装置、车载设备、终端设备以及定位系统 | |
| CN116412818B (zh) | 定位方法、装置、设备、计算机可读存储介质及程序产品 | |
| CN117687414B (zh) | 一种草坪修护机器人定位定向方法及系统 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210112 |
|
| RJ01 | Rejection of invention patent application after publication |