CN112977603B - 电机控制的方法和装置、农机以及计算机可读存储介质 - Google Patents

电机控制的方法和装置、农机以及计算机可读存储介质 Download PDF

Info

Publication number
CN112977603B
CN112977603B CN202110191877.3A CN202110191877A CN112977603B CN 112977603 B CN112977603 B CN 112977603B CN 202110191877 A CN202110191877 A CN 202110191877A CN 112977603 B CN112977603 B CN 112977603B
Authority
CN
China
Prior art keywords
steering angle
motor
agricultural machine
angular velocity
speed
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.)
Active
Application number
CN202110191877.3A
Other languages
English (en)
Other versions
CN112977603A (zh
Inventor
李黎黎
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Heilongjiang Huida Technology Co ltd
Original Assignee
Heilongjiang Huida Technology Development Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Heilongjiang Huida Technology Development Co ltd filed Critical Heilongjiang Huida Technology Development Co ltd
Priority to CN202110191877.3A priority Critical patent/CN112977603B/zh
Publication of CN112977603A publication Critical patent/CN112977603A/zh
Application granted granted Critical
Publication of CN112977603B publication Critical patent/CN112977603B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D5/00—Power-assisted or power-driven steering
    • B62D5/04—Power-assisted or power-driven steering electrical, e.g. using an electric servo-motor connected to, or forming part of, the steering gear
    • B62D5/0457—Power-assisted or power-driven steering electrical, e.g. using an electric servo-motor connected to, or forming part of, the steering gear characterised by control features of the drive means as such
    • B62D5/046—Controlling the motor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D15/00—Steering not otherwise provided for
    • B62D15/02—Steering position indicators ; Steering position determination; Steering aids
    • B62D15/021—Determination of steering angle

Landscapes

  • Engineering & Computer Science (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Guiding Agricultural Machines (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本申请提供了一种电机控制的方法和装置，该方法包括：获取农机的角速度和速度；根据农机的角速度、农机的速度和农机的轴距确定第一转向角度；根据电机的旋转角速度、电机的旋转角速度的输出时间间隔确定第二转向角度；第一转向角度和第二转向角度进行滤波处理，确定第三转向角度；控制电机驱动农机的前轮转动至第三转向角度。本申请实施例的电机控制的方法和装置，有助于在保证农机按照既定路线行驶，可以适用于各类农机设备中。

Description

电机控制的方法和装置、农机以及计算机可读存储介质
技术领域
本申请涉及电机领域，并且更具体地，涉及一种电机控制的方法和装置。
背景技术
现有导航系统中，一般使用电机控制车轮的转向，保证车辆按照既定的路线行驶。现有的电机控制系统包括：电机、转向传感器。电机控制系统一边使用转向传感器测量车辆前轮转向角度，一边通过电机驱动控制车轮的转向，保证车辆按照既定的路线行驶。现有的前轮转向传感器有两种：电位计转向器和陀螺仪转向传感器。上述两种传感器在使用时存在几个问题：(1)需要将转向传感器安装在前轮附近，增加成本；(2)电位计转向传感器安装时，需要许多连接件，安装工艺复杂，并且在使用中会造成机械损耗，影响测量精度，使用一段时间后，需要重新校准传感器，增加后期维护费用；(3)有一些车辆前轮附件由于结构原因不便于安装转向传感器，需要额外定制底板等辅助工具，增加安装成本和安装时间。因此如何在实现没有转向传感器的情况下，电机控制系统可以精准控制车辆前轮的转向角度，是一项亟待解决的技术问题。
发明内容
本申请提供一种电机控制的方法和装置，能够在没有转向传感器的情况下，精准控制农机的前轮转向角度，从而可以节约成本。
第一方面，提供了一种电机控制的方法，该方法包括：获取所述农机的角速度和速度；根据所述农机的角速度、所述农机的速度和所述农机的轴距确定第一转向角度；根据电机的旋转角速度、所述电机的旋转角速度的输出时间间隔确定第二转向角度；所述第一转向角度和所述第二转向角度进行滤波处理，确定第三转向角度；所述电机驱动所述农机的前轮转动至所述第三转向角度。
根据本申请实施例的电机控制的方法，通过获取农机的角速度和速度，并根据农机的轴距，可以计算第一转向角度，根据电机的旋转角速度、电机的旋转角速度的输出时间间隔确定第二转向角度；第一转向角度和第二转向角度进行滤波处理，确定第三转向角度，控制电机驱动前轮转动至第三转向角度，从而能够在没有转向传感器的情况下，可以精准控制农机的前轮转向角度，进而降低成本和维护费用，简化安装工艺。
结合第一方面，在第一方面的某些实现方式上，根据以下公式确定所述第一转向角度,
Figure GDA0003550826290000021
其中，Am表示所述第一转向角度，
Wvehicle表示所述农机的角速度，
L表示所述农机的轴距，
V表示所述农机的速度。
通过农机的角速度、速度、轴距可以得到第一转向角度，使得在没有转向传感器的情况下得到农机的转向角度。
结合第一方面，在第一方面的某些实现方式上，根据以下公式确定第二转向角度，
Ac＝ωm*dt，
其中，Ac表示所述第二转向角度，
ωm表示所述电机的旋转角速度，
dt表示所述电机的旋转角速度的输出时间间隔。
结合第一方面，在第一方面的某些实现方式上，当采用编码器测量电机的旋转角速度时，根据以下公式确定第二转向角度,
Ac＝ωm*K*dt，
其中，Ac表示所述第二转向角度，
ωm表示所述电机的旋转角速度，
K表示所述电机编码器比例，
dt表示所述电机的旋转角速度的输出时间间隔。
结合第一方面，在第一方面的某些实现方式上，所述农机的角速度可以为所述农机前轮的角速度。
结合第一方面，在第一方面的某些实现方式上，所述第一转向角度和所述第二转向角度进行滤波处理，可选地所述第一转向角度和所述转向角度进行扩展卡尔曼滤波处理，确定所述第三转向角度。
通过扩展卡尔曼滤波处理，将非线性化的方程线性化，对转向角度进行最优估计，排除测量过程中的噪声和干扰的影响。
结合第一方面，在第一方面的某些实现方式上，所述第一转向角度和所述第二转向角度进行扩展卡尔曼滤波处理，确定所述第三转向角度，包括：
确定状态转移方程：Ac＝ωm*dt或Ac＝ωm*K*dt，
确定观测方程
Figure GDA0003550826290000031
根据状态转移方程和观测方程进行扩展卡尔曼滤波处理。
将根据电机的旋转角速度、编码器比例、电机的旋转角速度的输出时间间隔得到的第二角度的计算公式作为状态转移方程，将根据农机的速度、角速度、轴距得到的第一角度的计算公式作为观测方程，通过扩展卡尔曼滤波处理得到精确的转向角度。
结合第一方面，在第一方面的某些实现方式上，获取由惯性测量单元测量的角速度；获取由全球卫星导航系统测量的速度。
惯性测量单元和全球卫星导航系统安装简单，成本低，后期维护成本低，通过惯性测量单元和全球卫星导航系统得到的农机的速度、角速度结合农机的轴距、电机的旋转角速度、电机的旋转角速度的输出时间间隔得到农机的转向角度，电机可以在没有转向传感器的情况下，精准的控制前轮转动。
第二方面，提供了一种电机控制的装置，所述装置安装在农机上，所述装置包括获取模块、处理模块、控制模块，其中，所述获取模块用于获取所述农机的角速度和所述农机的速度；所述处理模块用于根据所述农机的角速度、所述农机的速度和所述农机的轴距确定第一转向角度；所述处理模块还用于根据电机的旋转角速度、所述电机的旋转角速度的输出时间间隔确定第二转向角度；所述处理模块还用于对所述第一转向角度和所述第二转向角度进行滤波处理，确定第三转向角度；所述控制模块用于所述电机驱动所述农机的前轮转动至所述第三转向角度。
根据本申请实施例的电机控制的装置，通过获取农机的角速度和速度，并根据农机的轴距，可以计算第一转向角度，根据电机的旋转角速度、电机的旋转角速度的输出时间间隔确定第二转向角度；第一转向角度和第二转向角度进行滤波处理，确定第三转向角度，控制电机驱动前轮转动至第三转向角度，从而能够在没有转向传感器的情况下，精准控制农机的前轮转向角度，进而降低成本和维护费用，简化安装工艺。
结合第二方面，在第二方面的某些实现方式上，所述处理模块根据以下公式确定所述农机的第一转向角度，
Figure GDA0003550826290000041
其中，Am表示所述第一转向角度，
Wvehicle表示所述农机的角速度，
L表示所述农机的轴距，
V表示所述农机的速度。
结合第二方面，在第二方面的某些实现方式上，所述处理模块根据以下公式确定所述农机的第二转向角度，
Ac＝ωm*dt
其中，Ac表示所述第二转向角度，
ωm表示所述电机的旋转角速度，
dt表示所述电机的旋转角速度的输出时间间隔
结合第二方面，在第二方面的某些实现方式上，采用编码器测量所述电机的旋转角速度时，所述处理模块根据以下公式确定第二转向角度，
Ac＝ωm*K*dt
其中，Ac表示所述第二转向角度，
ωm表示所述电机的旋转角速度，
K表示所述电机编码器比例，
dt表示所述电机的旋转角速度的输出时间间隔。
结合第二方面，在第二方面的某些实现方式上，所述农机的角速度可以为所述农机前轮的角速度。
结合第二方面，在第二方面的某些实现方式上，所述处理模块还用于对所述第一转向角度和所述第二转向角度进行扩滤波处理，包括所述处理模块对所述第一转向角度和所述第二转向角度进行扩展卡尔曼滤波处理。
通过扩展卡尔曼滤波处理，将非线性化的方程线性化，对转向角度进行最优估计，排除测量过程中的噪声和干扰的影响。
结合第二方面，在第二方面的某些实现方式上，所述处理模块还用于对所述第一转向角度和所述第二转向角度进行扩展卡尔曼滤波处理，确定所述第三转向角度，包括：
所述处理模块确定状态转移方程：Ac＝ωm*dt或Ac＝ωm*K*dt；
所述处理模块确定观测方程：
Figure GDA0003550826290000051
所述处理模块根据状态转移方程和观测方程进行扩展卡尔曼滤波处理。
结合第二方面，在第二方面的某些实现方式上，所述获取模块可以获取惯性测量单元的角速度测量数据；所述获取模块可以获取全球卫星导航系统的速度测量数据。
惯性测量单元和全球卫星导航系统安装简单，成本低，后期维护成本低，通过惯性测量单元和全球卫星导航系统得到的农机的速度、角速度结合农机的轴距、电机的旋转角速度、电机的旋转角速度的输出时间间隔得到农机的转向角度，电机可以在没有转向传感器的情况下，精准的控制前轮转动。
第三方面，提供了一种农机，包括：如第二方面或第二方面的任一可能的实现方式中的电机控制的装置。
结合第三方面，在第三方面的某些实现方式上，所述农机还包括：
惯性测量单元，所述惯性测量单元用于测量农机的角速度；
全球卫星导航系统，所述全球卫星导航系统用于测量农机的速度；
显示模块，所述显示模块用于显示所述角速度、所述速度、所述第三转向角度。
第四方面，提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有程序，当所述程序运行时，实现第一方面或第一方面中任意一种可能的实现方式中的导航系统中电机控制的方法的指令。
第五方面，提供了一种包含指令的计算机程序产品，其在计算机上运行时，使得计算机执行上述第一方面或其任意可能的实现方式中的方法。
附图说明
图1是本申请实施例适用的应用场景示意图。
图2是根据本申请实施例的电机控制的方法的示意性框图。
图3是扩展卡尔曼滤波处理的方法的示意性框图。
图4是根据本申请实施例的电机控制的装置的示意性框图。
具体实施方式
下面将结合附图，对本申请实施例中的技术方案进行描述。
以下实施例中所使用的术语只是为了描述特定实施例的目的，而并非旨在作为对本申请的限制。如在本申请的说明书和所附权利要求书中所使用的那样，单数表达形式“一个”、“一种”、“所述”、“上述”、“该”和“这一”旨在也包括例如“一个或多个”这种表达形式，除非其上下文中明确地有相反指示。还应当理解，在本申请以下各实施例中，“至少一个”、“一个或多个”是指一个、两个或两个以上。术语“和/或”，用于描述关联对象的关联关系，表示可以存在三种关系；例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B的情况，其中A、B可以是单数或者复数。字符“/”一般表示前后关联对象是一种“或”的关系。
在本说明书中描述的参考“一个实施例”或“一些实施例”等意味着在本申请的一个或多个实施例中包括结合该实施例描述的特定特征、结构或特点。由此，在本说明书中的不同之处出现的语句“在一个实施例中”、“在一些实施例中”、“在其他一些实施例中”、“在另外一些实施例中”等不是必然都参考相同的实施例，而是意味着“一个或多个但不是所有的实施例”，除非是以其他方式另外特别强调。术语“包括”、“包含”、“具有”及它们的变形都意味着“包括但不限于”，除非是以其他方式另外特别强调。
应理解，本申请实施例可以应用于电机领域，包括但不限于电机控制和基于电机控制的产品，本申请实施例仅以农机电机控制为例进行说明，但不应对本申请实施例构成任何限定。
下面，结合图1对本申请实施例适用的应用场景进行说明。
图1为本申请实施例适用的应用场景。装置10在道路上行驶，例如当装置10为农机时，装置10可以在农田上行驶，例如装置10可以按照既定的路线行驶直线或者转弯、执行开沟、起垄、播种、喷药、铺膜、收货等作业活动
应理解，本申请实施例中，装置10可以为农机、卡车等车辆，本申请实施例中对车辆的类型不做限制。
为了保证农机在作业时按照既定的路线行驶，需要控制农机的转向。现有的电机控制系统包括：电机、转向传感器。电机控制系统一边使用转向传感器测量车辆前轮转向角度，一边通过电机驱动控制车轮的转向，保证车辆按照既定的路线行驶。现有的前轮转向传感器有两种：电位计转向器和陀螺仪转向传感器。上述两种传感器在使用时存在几个问题：(1)需要将转向传感器安装在前轮附近，增加成本；(2)电位计转向传感器安装时，需要许多连接件，安装工艺复杂，并且在使用中会造成机械损耗，影响测量精度，使用一段时间后，需要重新校准传感器，增加后期维护费用；(3)有一些车辆前轮附件由于结构原因不便于安装转向传感器，需要额外定制底板等辅助工具，增加安装成本和安装时间。
为了解决上述安装转向传感器导致的安装成本、维护成本、安装时间增加的问题，本申请提出了一种电机控制的方法，能够在不安装转向传感器的情况下，计算车辆前轮的转向角度，并对计算出的前轮的转向角度进行优化，从而电机驱动前轮转向，保证农机按照既定路线行驶。
图2为本申请实施例提供的一种导航系统中电机控制的方法，包括：下面详细介绍实施例中的步骤。
需要说明的是，为了便于理解，本申请实施例中装置以农机为例，但是本申请实施例对此并不作限定。
S210，农机获取农机的角速度、农机的速度。
农机在农田上行驶时，安装在农机车身上的获取模块可以获取由惯性测量单元(Inertial Measurement Unit，IMU)测量的农机的角速度，获取模块还可以获取由全球导航卫星系统(Global Navigation Satellite System，GNSS)测量的农机的速度。
IMU是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪。加速度计是敏感轴向加速度并转换成可用输出信号的传感器，陀螺仪是能够敏感运动体相对于惯性空间的运动角速度的传感器。
GNSS是利用一组卫星的伪距、星历、卫星发射时间等观测量，同时还必须知道用户钟差。GNSS是能在地球表面或近地空间的任何地点为用户提供全天候的3维坐标和速度以及时间信息的空基无线电导航定位系统。常见的GNSS包括无线通信技术和全球卫星定位系统(Navigation Satellite Timing And Ranging Global Position System，GPS)、格洛纳斯(Global Navigation Satellite System，GLONASS)、伽利略卫星导航系统(Galileosatellite navigation system，GALILEO)、北斗卫星导航系统(BeiDou NavigationSatellite System，BDS)。本申请实施例中对GNSS的类型不作限制。
惯性导航系统(Inertial Navigation System，INS)是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯性导航系统的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。
需要说明的是，本申请实施例对于IMU和GNSS的安装位置并不作限定，IMU和GNSS在保证获取数据精度的情况下，可以安装在农机的任意位置。一般来说，IMU安装在农机车身重心的位置，GNSS安装在发动机仓内部。
应理解，本申请实施例中还可以将IMU和GNSS集成为一体。
应理解，IMU和GNSS可以实时获取农机的角速度和速度，也可以定时获取农机的角速度和速度。例如，每2分钟获取一次农机的角速度和速度。本申请实施例对此并不作限定。
还应理解，当IMU和GNSS定时获取农机的角速度和速度时，农机可以设定阈值计算一段时间内的农机的角速度和速度的平均值。例如，每2秒钟获取一次农机的角速度和速度，阈值为10秒钟，则在10秒钟时共获取5次农机的角速度和速度，此时导航系统计算5次角速度和速度的平均值。
S220，农机根据农机的角速度、速度、轴距确定第一转向角度。
农机得到农机的角速度和速度后，安装在农机车身上的处理模块将农机角速度和农机速度按照公式(1)计算前轮的转向角度，即第一转向角度。
Figure GDA0003550826290000081
其中，Am表示所述第一转向角度，Wvehicle表示所述农机的角速度，L表示所述农机的轴距，V表示所述农机的速度。
应理解，第一转向角度是根据农机的角速度、农机的速度和农机的轴距计算得到的，根据公式(1)可以在没有转向传感器的情况下计算得到第一转向角度。
S230，农机根据电机的旋转角速度、电机的旋转角速度的输出时间间隔确定第二转向角度。
农机的获取模块获取电机的旋转角速度和电机的旋转角速度的输出时间间隔，农机的处理模块根据公式(2)计算第二转向角度。
Ac＝ωm*dt (2)
其中，Ac表示所述第二转向角度，ωm表示所述电机的旋转角速度，dt表示所述电机的旋转角速度的输出时间间隔。
应理解，当采用编码器测量电机的旋转角速度时，编码器产生相应的脉冲波形，电机每旋转一周，编码器产生固定的脉冲数，通过计算固定时间内的脉冲数，从而可以计算出电机的旋转角速度。此时，可以根据电机的旋转加速度以及结合电机的编码器比例和电机的旋转角速度的输出时间间隔根据公式(3)计算第二转向角度。
Ac＝ωm*K*dt (3)
其中，Ac表示所述第二转向角度，ωm表示所述电机的旋转角速度，K表示所述电机编码器比例，dt表示所述电机的旋转角速度的输出时间间隔。
应理解，本申请实施例为了便于介绍，以编码器得到电机的旋转角速度为例，但是本申请实施例对此不作限定，通过其他方式测量得到电机的旋转角速度并实施本申请实施例的，均在本申请的保护范围之内，例如通过测量电机的转速得到电机的旋转角速度。
应理解，dt可以与上述IMU、GNSS的定时测量农机的角速度和速度的时长相等或不相等。例如，dt为2秒钟，IMU、GNSS同样2秒钟测量一次农机的角速度和速度。
还应理解，当采用不同编码器测量电机的旋转角速度时，会引入不同的系数K，本申请实施例对此不作限定。
S240，农机对第一转向角度和所述第二转向角度进行滤波处理，确定第三转向角度。
滤波(Wave filtering)是将信号中特定波段频率滤除的操作，是抑制和防止干扰的一项重要措施，滤波分为经典滤波和现代滤波。常见的滤波算法有中位值滤波法、算术平均滤波法、递推平均滤波法、限幅平均滤波法、一阶滞后滤波法、卡尔曼滤波法、扩展卡尔曼滤波法。
卡尔曼滤波(Kalman filtering，KF)是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。数据滤波是去除噪声还原真实数据的一种数据处理技术，KF在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，KF是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。
扩展卡尔曼滤波(Extended Kalman filtering，EKF)，是KF的非线性版本，在状态转移方程确定的情况下，EKF已经成为了非线性系统状态估计的事实标准。
优选地，本申请实施例以扩展卡尔曼滤波为例，农机对第一转向角度和第二转向角度进行扩展卡尔曼滤波处理，确定第三转向角度。
农机根据电机的旋转角速度、电机的旋转角速度的输出时间间隔确定第二转向角度或者农机根据电机的旋转角速度、编码器的比例、电机的旋转角速度的输出时间间隔确定第二转向角度，该第二转向角度由于编码器测量误差、安装误差因素造成第二转向角度存在偏差，需要对第二转向角度进行优化处理。本申请实施例中通过EKF对第二转向角度进行优化。EKF是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波。在EKF中，根据实际情况设置状态转移方程和观测方程。确定状态转移方程和观测方程后，对状态转移方程和观测方程泰勒得到状态转移矩阵、观测矩阵、协方差矩阵，根据上述三个矩阵求解卡尔曼增益，根据卡尔曼增益求解更新的状态转移方程，最后求解更新的协方差。上述为EKF的一个计算周期，循环下去就是各个时刻扩展卡尔曼对非线性系统的处理过程。
本申请实施例中，将EKF计算过程中的状态转移方程设定为：Ac＝ωm*K*dt，将观测方程设定为：
Figure GDA0003550826290000101
由观测方程和状态转移方程可以得到状态矩阵为：X＝|Ac；K|，观测矩阵为：H＝|Am|。
图3为本申请实施例中一次EKF计算过程。
S241、初始化状态矩阵、观测矩阵、协方差矩阵P(0)。
根据状态转移方程和观测方程确定了状态矩阵和观测矩阵，根据得到的状态矩阵和观测矩阵初始化状态矩阵、观测矩阵、协方差矩阵。
S242、进行状态预测。
针对某时刻的农机的状态，根据公式(4)和公式(5)进行状态预测，即进行泰勒展开。
X(k|k-1)(0)＝X(k-1)(0)+ωm*dt*X(k-1)(1) (4)
X(k|k-1)(1)＝X(k-1)(1) (5)
S243、进行观测预测。
与S242进行状态预测相同，针对某时刻的农机的观测，根据公式(6)进行观测预测，即进行泰勒展开。
Y(k|k-1)＝X(k-1)(0) (6)
S244、一阶线性化方程，求解状态转移矩阵。
得到一阶线性化方程后，对该一阶方程求偏导后可以得到状态转移矩阵，即按照公式(7)计算得到状态转移矩阵。
Figure GDA0003550826290000111
S245、求解协方差预测矩阵P(k|k-1)。
上述步骤中，对系统结果已经更新，但是并没有更新协方差，根据上述步骤求解得到状态转移矩阵，根据公式(8)求解协方差预测矩阵。
P(k|k-1)＝Φ(k)P(k-1|k-1)ΦT(k)+Q (8)
Q表示为过程噪声协方差矩阵。
应理解，过程噪声协方差矩阵可以根据实际情况选取，本申请实施例对此不作限定。
S246、求解卡尔曼增益。
卡尔曼增益通过测量值和观测值之间的协方差最小是确定，由此求解协方差偏导为0的系数，得到的系数为卡尔曼增益。根据公式(9)可以求解得到卡尔曼增益。
K(k)＝P(k-1|k-1)HT(k)(H(k)P(k|k-1)H(k)+R) (9)
R表示为观测噪声协方差矩阵。
应理解，观测噪声协方差矩阵可以根据实际情况选取，本申请实施例对此不作限定。
S247、进行状态更新。
求解出卡尔曼增益后，根据公式(10)进行状态转移方程的更新，在本申请实施例中，可以得到第一转向角度和第二转向角度经过扩展卡尔曼滤波处理后的第三角度。
X(k)＝X(k|k-1)+K(Y(k)-Y(k|k-1)) (10)
上述计算为EKF的一个计算周期，可以得到某一时刻的农机的转向角度，循环计算可以得到各个时刻农机的转向角度。
应理解，本申请实施例中，对状态转移方程和观测方程不作限定，状态转移方程可以为：
Figure GDA0003550826290000121
观测方程可以为：Ac＝ωm*K*dt，其余计算按照上述步骤进行。
S250，电机驱动所述农机的前轮转动至所述第三转向角度。
确定第三角度后，电机根据第三角度驱动农机的前轮转动，直到前轮的转向角度达到第三转向角度。例如，农机在农田上进行喷药作业，需要直线行驶，当发现农机行驶路线偏离直线时，此时根据上述步骤S210-250可以对农机行驶路线纠偏，保证农机的直线行驶。
应理解，本申请实施例中以农机直线行驶为例只是便于说明，本申请实施例对此不作限定。
图4示出了本申请实施例的电机控制的装置的示意性框图。所述电机控制的装置用于执行前述电机控制的方法。可选地，所述电机控制的装置中各个模块可以是通过软件来实现。如图4所示，所述电机控制的装置包括：
获取模块410，用于获取所述农机的角速度和农机的速度。
获取模块410可以获取由IMU测量的农机的角速度，获取模块410还可以获取由GNSS测量的农机的速度。
需要说明的是，本申请实施例对于IMU和GNSS的安装位置并不作限定，IMU和GNSS在保证获取数据精度的情况下，可以安装在农机的任意位置。一般来说，IMU安装在农机车身重心的位置，GNSS安装在发动机仓内部。
应理解，本申请实施例中还可以将IMU和GNSS与本申请实施例中的装置集成为一体，从而IMU和GNSS构成测量模块。
应理解，IMU和GNSS可以实时获取农机的角速度和速度，也可以定时获取农机的角速度和速度。例如，每2分钟获取一次农机的角速度和速度。本申请实施例对此并不作限定。
还应理解，当IMU和GNSS定时获取农机的角速度和速度时，农机可以设定阈值计算一段时间内的农机的角速度和速度的平均值。例如，每2秒钟获取一次农机的角速度和速度，阈值为10秒钟，则在10秒钟时共获取5次农机的角速度和速度，此时导航系统计算5次角速度和速度的平均值。
处理模块420，用于根据农机的角速度、农机的速度、农机的轴距确定农机的第一转向角度，还用于根据电机的旋转角速度、所述电机的旋转角速度的输出时间间隔确定第二转向角度，还用于对第一转向角度和第二转向角度进行滤波处理，确定第三转向角度。
获取模块410得到农机的角速度和速度后，处理模块420将农机角速度和农机速度按照公式(1)计算前轮的转向角度，即第一转向角度。
获取模块410获取电机的旋转角速度和电机的旋转角速度的输出时间间隔，处理模块420根据公式(2)计算第二转向角度。
应理解，当采用编码器测量电机的旋转角速度时，编码器产生相应的脉冲波形，电机每旋转一周，编码器产生固定的脉冲数，通过计算固定时间内的脉冲数，从而可以计算出电机的旋转角速度。此时，可以根据电机的旋转加速度以及结合电机的编码器比例和电机的旋转角速度的输出时间间隔根据公式(3)计算第二转向角度。
应理解，本申请实施例为了便于介绍，以编码器得到电机的旋转角速度为例，但是本申请实施例对此不作限定，通过其他方式测量得到电机的旋转角速度并实施本申请实施例的，均在本申请的保护范围之内，例如通过测量电机的转速得到电机的旋转角速度。
应理解，dt可以与上述IMU、GNSS的定时测量农机的角速度和速度的时长相等或不相等。例如，dt为2秒钟，IMU、GNSS同样2秒钟测量一次农机的角速度和速度。
还应理解，当采用不同编码器测量电机的旋转角速度时，会引入不同的系数K，本申请实施例对此不作限定。
处理模块420对第一角度和第二角度进行滤波处理，包括中位值滤波法、算术平均滤波法、递推平均滤波法、限幅平均滤波法、一阶滞后滤波法、KF、EKF。
应理解，当采用EKF时，处理模块420将EKF计算过程中的状态转移方程设定为：Ac＝ωm*K*dt或Ac＝ωm*dt，本申请实施例以Ac＝ωm*K*dt为例，将观测方程设定为：
Figure GDA0003550826290000131
由观测方程和状态转移方程可以得到状态矩阵为：X＝|Ac；K|，观测矩阵为：H＝|Am|。
处理模块420在确定状态矩阵和观测矩阵后，执行步骤S241-S247。
控制模块430，用于控制农机的前轮转动至第三转向角度。
确定第三角度后，控制模块控制电机根据第三角度驱动农机的前轮转动，直到前轮的转向角度达到第三转向角度。例如，农机在农田上进行喷药作业，需要直线行驶，当发现农机行驶路线偏离直线时，对农机行驶路线纠偏，保证农机的直线行驶。
本申请的又一方面提供了一种计算机可读存储介质，该计算机可读存储介质中存储有指令，当该指令在计算机上运行时，使得计算机执行上述方法实施例。
本申请的又一方面提供了一种包含指令的计算机程序产品，当该计算机程序产品在计算机上运行时，使得计算机执行上述方法实施例。
本申请的又一方面提供了一种农机，该农机可以包括：上述任一申请实施例的电机控制的装置。
可选地，在本申请的一个实施例中，该农机还可以包括惯性测量单元和全球卫星导航系统，惯性测量单元用于测量该农机的角速度，全球卫星导航系统用于测量该农机的速度。
可选地，在本申请的一个实施例中，该农机还可以包括显示模块，显示模块用于显示农机的角速度、速度、第三转向角度。
应理解，在本申请的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
上述本申请实施例揭示的方法可以应用于处理器中，或者由处理器实现。处理器可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器可以是通用处理器、数字信号处理器(digital signal processor，DSP)，专用集成电路(applicationspecific integrated circuit，ASIC)，现成可编程门阵列(field programmable gatearray，FPGA)或者其他可编程逻辑器件，分立门或者晶体管逻辑器件，分立硬件组件，还可以是系统芯片(system on chip，SoC)，还可以是中央处理器(central processor unit，CPU)，还可以是网络处理器(network processor，NP)，还可以是数字信号处理电路(digital signal processor，DSP)，还可以是微控制器(micro controller unit，MCU)，还可以是可编程控制器(programmable logic device，PLD)或其他集成芯片。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本申请实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存取存储器(random access memory，RAM)、闪存、只读存储器(read-only memory，ROM)、可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器读取存储器中的指令，结合其硬件完成上述方法的步骤。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。

Claims (19)

1.一种电机控制的方法，其特征在于，所述方法应用于农机，所述方法包括：
获取所述农机的角速度和速度；
根据所述角速度、所述速度和所述农机的轴距确定第一转向角度；
根据电机的旋转角速度、所述电机的旋转角速度的输出时间间隔确定第二转向角度；
根据所述第一转向角度和所述第二转向角度进行滤波处理，确定第三转向角度；
控制所述电机驱动所述农机的前轮转动至所述第三转向角度。
2.根据权利要求1所述的方法，其特征在于，所述根据所述角速度、所述速度和所述农机的轴距确定第一转向角度，包括：
根据以下公式确定所述第一转向角度，
Figure FDA0003550826280000011
其中，Am表示所述第一转向角度，
Wvehicle表示所述角速度，
L表示所述轴距，
V表示所述速度。
3.根据权利要求1或2所述的方法，其特征在于，所述根据电机的旋转角速度、所述电机的旋转角速度的输出时间间隔确定第二转向角度，包括：
根据以下公式确定所述第二转向角度，
Ac＝ωm*dt，
其中，Ac表示所述第二转向角度，
ωm表示所述电机的旋转角速度，
dt表示所述电机的旋转角速度的输出时间间隔。
4.根据权利要求1或2所述的方法，其特征在于，所述方法还包括：
采用编码器测量所述电机的旋转角速度时，根据以下公式确定所述第二转向角度，
Ac＝ωm*K*dt，
其中，Ac表示所述第二转向角度，
ωm表示所述电机的旋转角速度，
K表示所述电机编码器比例，
dt表示所述电机的旋转角速度的输出时间间隔。
5.根据权利要求1或2所述的方法，其特征在于，所述农机的角速度为所述农机前轮的角速度。
6.根据权利要求1或2所述的方法，其特征在于，所述根据所述第一转向角度和所述第二转向角度进行滤波处理，确定第三转向角度，包括：
根据所述第一转向角度和所述第二转向角度进行扩展卡尔曼滤波处理，确定所述第三转向角度。
7.根据权利要求6所述的方法，其特征在于，所述根据第一转向角度和所述第二转向角度进行扩展卡尔曼滤波推算处理，确定所述第三转向角度，包括：
确定状态转移方程：Ac＝ωm*dt或Ac＝ωm*K*dt；
确定观测方程：
Figure FDA0003550826280000021
根据所述状态转移方程和所述观测方程进行扩展卡尔曼滤波处理。
8.根据权利要求1或2所述的方法，其特征在于，所述获取所述农机的角速度和速度，包括：
获取惯性测量单元的角速度测量数据；
获取全球卫星导航系统的速度测量数据。
9.一种电机控制的装置，其特征在于，所述装置安装在农机上，所述装置包括获取模块、处理模块、控制模块，其中，
所述获取模块用于获取所述农机的角速度和速度；
所述处理模块用于根据所述角速度、所述速度和所述农机的轴距确定第一转向角度；
所述处理模块还用于根据电机的旋转角速度和所述电机的旋转角速度的输出时间间隔确定第二转向角度；
所述处理模块还用于对所述第一转向角度和所述第二转向角度进行滤波处理，确定第三转向角度；
所述控制模块用于控制所述电机驱动所述农机的前轮转动至所述第三转向角度。
10.根据权利要求9所述的装置，其特征在于，所述处理模块用于根据所述角速度、所述速度和所述农机的轴距确定第一转向角度，包括：
根据以下公式确定所述第一角度，
Figure FDA0003550826280000031
其中，Am表示所述第一转向角度，
Wvehicle表示所述角速度，
L表示所述轴距，
V表示所述速度。
11.根据权利要求9或10所述的装置，其特征在于，所述处理模块还用于根据电机的旋转角速度和所述电机的旋转角速度的输出时间间隔确定第二转向角度，包括：
根据以下公式确定所述第二转向角度，
Ac＝ωm*dt，
其中，Ac表示所述第二转向角度，
ωm表示所述电机的旋转角速度，
dt表示所述电机的旋转角速度的输出时间间隔。
12.根据权利要求9或10所述的装置，其特征在于，所述装置还包括：
采用编码器测量所述电机的旋转角速度时，根据以下公式确定所述第二转向角度，
Ac＝ωm*K*dt，
其中，Ac表示所述第二转向角度，
ωm表示所述电机的旋转角速度，
K表示所述电机编码器比例，
dt表示所述电机的旋转角速度的输出时间间隔。
13.根据权利要求9或10所述的装置，其特征在于，所述农机的角速度为所述农机前轮的角速度。
14.根据权利要求9或10所述的装置，其特征在于，所述处理模块还用于对所述第一转向角度和所述第二转向角度进行滤波处理，确定第三转向角度，包括：
所述处理模块对所述第一转向角度和所述第二转向角度进行扩展卡尔曼滤波处理，确定第三转向角度。
15.根据权利要求14所述的装置，其特征在于，所述处理模块对所述第一转向角度和所述第二转向角度进行扩展卡尔曼滤波处理，确定第三转向角度，包括：
所述处理模块确定状态转移方程：Ac＝ωm*dt或Ac＝ωm*K*dt；
所述处理模块确定观测方程：
Figure FDA0003550826280000041
所述处理模块根据所述状态转移方程和所述观测方程进行扩展卡尔曼滤波处理。
16.根据权利要求9或10所述的装置，其特征在于，所述获取模块用于获取所述农机的角速度和速度，包括：
所述获取模块获取惯性测量单元的角速度测量数据；
所述获取模块获取全球卫星导航系统的速度测量数据。
17.一种农机，其特征在于，包括：如上述权利要求9至16中任一项所述的电机控制的装置。
18.根据权利要求17所述的农机，其特征在于，所述农机还包括：
惯性测量单元，所述惯性测量单元用于测量所述农机的角速度；
全球卫星导航系统，所述全球卫星导航系统用于测量所述农机的速度；
显示模块，所述显示模块用于显示所述角速度、所述速度、所述第三转向角度。
19.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有程序，当所述程序运行时，实现如权利要求1至8中任一项所述的方法。
CN202110191877.3A 2021-02-20 2021-02-20 电机控制的方法和装置、农机以及计算机可读存储介质 Active CN112977603B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110191877.3A CN112977603B (zh) 2021-02-20 2021-02-20 电机控制的方法和装置、农机以及计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110191877.3A CN112977603B (zh) 2021-02-20 2021-02-20 电机控制的方法和装置、农机以及计算机可读存储介质

Publications (2)

Publication Number Publication Date
CN112977603A CN112977603A (zh) 2021-06-18
CN112977603B true CN112977603B (zh) 2022-05-06

Family

ID=76393640

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110191877.3A Active CN112977603B (zh) 2021-02-20 2021-02-20 电机控制的方法和装置、农机以及计算机可读存储介质

Country Status (1)

Country Link
CN (1) CN112977603B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114162139B (zh) * 2021-12-06 2024-03-26 上海华测导航技术股份有限公司 一种车辆控制方法、控制器、系统、装置及存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103425134A (zh) * 2012-05-18 2013-12-04 宁波市鄞州麦谷农业科技有限公司 高速插秧机的卫星导航自动驾驶系统及其方法
CN109073763A (zh) * 2016-04-06 2018-12-21 株式会社久保田 测位检测装置、以及具备测位检测装置的工作机
JP2019059449A (ja) * 2017-09-28 2019-04-18 トヨタ自動車株式会社 車両運転支援装置
CN111722257A (zh) * 2020-06-29 2020-09-29 河南工业大学 一种基于gps和ins组合制导的远程救援机器人系统
JP2020175865A (ja) * 2019-04-23 2020-10-29 三菱電機株式会社 車両操舵装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103425134A (zh) * 2012-05-18 2013-12-04 宁波市鄞州麦谷农业科技有限公司 高速插秧机的卫星导航自动驾驶系统及其方法
CN109073763A (zh) * 2016-04-06 2018-12-21 株式会社久保田 测位检测装置、以及具备测位检测装置的工作机
JP2019059449A (ja) * 2017-09-28 2019-04-18 トヨタ自動車株式会社 車両運転支援装置
JP2020175865A (ja) * 2019-04-23 2020-10-29 三菱電機株式会社 車両操舵装置
CN111722257A (zh) * 2020-06-29 2020-09-29 河南工业大学 一种基于gps和ins组合制导的远程救援机器人系统

Also Published As

Publication number Publication date
CN112977603A (zh) 2021-06-18

Similar Documents

Publication Publication Date Title
CN107588769B (zh) 一种车载捷联惯导、里程计及高程计组合导航方法
KR102018450B1 (ko) 단일 안테나에 기초한 ｇｎｓｓ―ｉｎｓ 차량 자세 결정 방법
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
EP2856273B1 (en) Pose estimation
JP6094026B2 (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
CN109343095B (zh) 一种车载导航车辆组合定位装置及其组合定位方法
JP5586994B2 (ja) 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
JP2008076389A (ja) ナビゲーション・システムおよびナビゲーション方法
CN113631883B (zh) 车辆定位装置
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN110849360A (zh) 面向多机协同编队飞行的分布式相对导航方法
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
JPH095104A (ja) 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN112977603B (zh) 电机控制的方法和装置、农机以及计算机可读存储介质
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
JP5219547B2 (ja) 車載ナビゲーションシステム及びナビゲーション方法
RU2504734C1 (ru) Способ определения параметров модели погрешностей измерений акселерометров инерциальной навигационной системы по измерениям спутниковой навигации
AU2020104248A4 (en) Integrated gps and inertial navigation system for industrial robots
CN114111792A (zh) 一种车载gnss/ins/里程计组合导航方法
RU100232U1 (ru) Комплексированная навигационная система для определения координат подвижных наземных объектов
JP6981459B2 (ja) センサ誤差補正装置
JP2007155365A (ja) 方位センサの補正係数演算装置及び演算プログラム
JP2015102330A (ja) 移動情報算出装置、移動情報算出方法、移動情報算出プログラム、および移動体
JP4831441B2 (ja) 方位センサの補正係数演算装置及び演算プログラム

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
GR01 Patent grant
GR01 Patent grant
CP01 Change in the name or title of a patent holder

Address after: 150006 6th floor, Yitong building, 133 xidazhi street, Nangang District, Harbin City, Heilongjiang Province

Patentee after: Heilongjiang Huida Technology Co.,Ltd.

Address before: 150006 6th floor, Yitong building, 133 xidazhi street, Nangang District, Harbin City, Heilongjiang Province

Patentee before: HEILONGJIANG HUIDA TECHNOLOGY DEVELOPMENT Co.,Ltd.

CP01 Change in the name or title of a patent holder