航向校正方法、系统、自移动装置、可读存储介质与流程

1.本发明涉及了自动控制技术领域，尤其涉及一种航向校正方法、系统、自移动装置及可读存储介质。


背景技术：

2.近年来，自移动装置逐渐成为了我们生活中不可缺少的一部分，其中，智能吸尘器普遍地运用于家庭室内环境的清洁，而自动割草机则运用于花园中草坪的维护。为了避免自动割草机作业时对花园中草坪造成损害，需要自动割草机可进行有规划地割草，而获得准确的航向是自动割草机能够进行有规划割草的基础。
3.自动割草机一般通过uwb(ultrawideband，超宽带)技术或惯性导航技术以获得航向，其中，uwb无法直接获得自动割草机的航向，故通常采用带反射标的激光测角方法来进行辅助，然而这种方法易受干扰、易被遮挡，从而易于产生航向误差；通过惯性导航技术获得航向，其中，陀螺仪是一种常见的选择，然而陀螺仪由于零位漂移和白噪声，会积累航向误差。以此，如何进行航向校正，以降低甚至消除自动割草机的航向误差，成为了亟待解决的问题。
4.现有的航向校正方法主要包含以下两种，具体为：1、通过惯性导航系统获得航向，并利用uwb进行辅助校正；2、利用惯性导航系统获得航向，并通过地磁来进行航向校正。
5.但是上述两种航向校正方法存在以下问题：地磁场容易受到诸如汽车、铁磁材料以及交直流切换等因素的干扰，从而通过地磁进行航向校正，有时候会产生较大的误差，使得自动割草机偏离预设的航向；uwb仅在自动割草机直线移动时，才能获得准确的航向，从而对惯性导航系统获得的航向进行校正，而当自动割草机曲线移动时，uwb获得的航向仍是自动割草机直线移动的航向，以此，此时uwb获得的航向与自动割草机的准确航向偏差较大，若此时利用uwb获得的航向进行校正，反而会使得自动割草机偏离预设的航向。


技术实现要素：

6.为了克服自移动装置在进行曲线移动时无法准确地进行航向校正的缺陷，本发明实施例提供了一种航向校正方法，用于自移动装置，所述自移动装置包括惯性导航系统和无线电探测系统，其特征在于，所述方法包括：
7.检测所述自移动装置从第一位置移动至第二位置的移动状态，所述移动状态包括曲线移动、直线移动其中之一；
8.当所述移动状态为所述曲线移动时，根据第一航向角校正值和第二航向角获得所述自移动装置到达所述第二位置时的航向角；
9.其中，
10.所述第一航向角校正值根据所述无线电探测系统测量的所述自移动装置到达所述第二位置时的第一航向角和所述惯性导航系统测量的所述自移动装置从所述第一位置移动至所述第二位置的累计航向变化值获得；所述第二航向角根据所述自移动装置的初始
航向角和所述惯性导航系统测量的所述自移动装置从初始位置移动至所述第二位置的累计航向变化值获得。
11.进一步地，当所述移动状态为所述直线移动时，根据所述第一航向角和所述第二航向角获得所述自移动装置到达所述第二位置时的航向角。
12.进一步地，所述初始航向角通过以下方式获得：
13.当所述自移动装置以充电站作为所述初始位置出发时，所述初始航向角为0。
14.进一步地，所述初始航向角通过以下方式获得：
15.当所述自移动装置以所述充电站之外的位置作为所述初始位置出发时，以所述自移动装置从所述初始位置移动预设距离后所到达的位置时，所述无线电探测系统测量的航向角为所述初始航向角。
16.进一步地，所述自移动装置到达所述第二位置时的航向角通过如下表达式获得：
17.φ
j
＝n1·
a1 n2·
a218.其中，φ
j
为所述自移动装置到达所述第二位置时的航向角，a1为所述第一航向角校正值，a2为所述第二航向角，n1、n2均为正数，并且如果所述自移动装置从所述初始位置移动至所述第二位置时的累计时间达到预设时间时，n1大于n2；如果所述自移动装置从所述初始位置移动至所述第二位置时的累计时间未达到所述预设时间时，n1小于n2。
19.进一步地，所述“检测所述自移动装置从第一位置移动至第二位置的移动状态”包括：根据预设的工作计划获得所述自移动装置从第一位置移动至第二位置的移动状态，其中，所述预设的工作计划包含所述自移动装置的位置与移动状态的对应关系。
20.进一步地，所述第一航向角校正值等于所述第一航向角与二分之一的所述惯性导航系统测量的所述自移动装置从所述第一位置移动至所述第二位置的累计航向变化值之和。
21.为了克服自移动装置在进行曲线移动时无法准确地进行航向校正的缺陷，本发明实施例提供了一种自移动装置的控制系统，其特征在于，所述控制系统包含：
22.检测模块，配置为检测所述自移动装置从第一位置移动至第二位置的移动状态，所述移动状态包括曲线移动、直线移动其中之一；
23.获取模块，配置为根据无线电探测系统测量的所述自移动装置到达所述第二位置时的第一航向角和惯性导航系统测量的所述自移动装置从所述第一位置移动至所述第二位置的累计航向变化值获得第一航向角校正值，根据所述自移动装置的初始航向角和所述惯性导航系统测量的所述自移动装置从初始位置移动至所述第二位置的累计航向变化值获得第二航向角；
24.计算模块，配置为当所述移动状态为所述曲线移动时，根据所述第一航向角校正值和所述第二航向角获得所述自移动装置到达所述第二位置时的航向角。
25.为了克服自移动装置在进行曲线移动时无法准确地进行航向校正的缺陷，本发明实施例提供了一种自移动装置，其特征在于，所述自移动装置还包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现上述任一所述的航向校正方法。
26.为了克服自移动装置在进行曲线移动时无法准确地进行航向校正的缺陷，本发明实施例提供了一种可读存储介质，其特征在于，所述计算机程序被处理器执行时实现上述
任一项所述航向校正方法。
27.本发明的有益效果如下：
28.在自移动装置处于曲线移动状态时，虽然无线电探测系统检测到的航向角是自移动装置直线移动时的航向角，并不能直接地用于航向校正，但是利用惯性导航系统检测到的航向变化值，可对无线电探测系统检测到的航向角进行校正，从而可获得自移动装置曲线移动的第一航向校正值，根据所述惯性导航系统获得的第二航向角及所述第一航向校正值，则可以获得较为准确所述自移动装置到达所述第二位置时的航向角。通过本发明的航向校正方法，自移动装置在曲线移动时，也可以通过无线电探测系统及惯性导航系统的结合以准确地进行航向校正。
29.为让本发明的上述和其他目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附图式，作详细说明如下。
附图说明
30.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
31.图1是本发明实施例中的自移动装置、充电站及超宽带基站示意图；
32.图2是本发明实施例中的自移动装置曲线移动时航向校正的示意图；
33.图3是本发明实施例中的自移动装置的航向校正方法的流程图。
具体实施方式
34.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
35.在本发明的描述中，需要说明的是，术语“上”、“下”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或隐含地包括一个或者更多个该特征。
36.为达到上述目的，本发明提供了一种自移动装置，在优选的实施例中，所述自移动装置为自动割草机1，当然，在其他实施例中，所述自移动装置也可以是扫地机器人，或其他自动设备，相关内容这里不再赘述。
37.请具体参考图1，本发明所述的自动割草机1包含主动轮11，通常，所述主动轮11设有两个，且两个所述主动轮11相对于所述自动割草机1的中心线对称设置。所述主动轮11由电机(未图示)驱动，通过所述主动轮11，所述自动割草机1可进行移动作业，在本实施例中，所述电机为带减速箱和霍尔传感器的无刷电机，以此，通过控制两个所述主动轮11的速度
和方向便可以实现所述自动割草机1的前进、后退、转弯及圆弧等行驶动作。进一步地，所述自动割草机1还包含万向轮12，在本实施例中，所述万向轮12设于所述主动轮11的前方，通过所述万向轮12，所述自动割草机1可更为平衡稳定地进行行走。
38.所述自动割草机1还包含惯性导航系统，通过所述惯性导航系统，在所述自动割草机1的工作过程中，所述自动割草机1可实时获得航向及位置，从而可便于自动割草机1按照规划的路径及工作模式进行作业。在本实施例中，所述惯性导航系统具体包含陀螺仪15，所述陀螺仪15具体可以是一轴，或者三轴陀螺仪。
39.进一步地，所述自动割草机1还包含无线电探测系统，通过所述无线电探测系统，在所述自动割草机1的工作过程中，所述自动割草机1也可实时获得航向及位置，通过所述无线电探测系统，可对所述惯性导航系统获得的航向进行校正，从而使得所述自动割草机1能够稳定地按照预设航向进行作业，提高了工作效率。在本实施例中，所述无线电探测系统具体通过超宽带技术实现所述自动割草机1的定位，具体来说，所述自动割草机1上设有超宽带标签14，对应地，草地上设有超宽带基站31(32,33)，所述自动割草机1还包含控制模块13，所述控制模块13通过串口或者i2c等方式与所述超宽带标签14和陀螺仪15进行数据接收或交换。需要注意的是，设有3个所述超宽带基站仅是一种实施例，在其他实施例中，所述超宽带基站也可以是设置4个或根据草地的面积进行具体设置。当然，还需要注意的是，所述无线电探测系统具体通过超宽带技术进行工作仅一种优选的实施例，在其他实施例中，所述无线电探测系统也可以是通过gps或dgps进行工作。
40.进一步地，在本实施例中，所述自动割草机1上仅设有一个超宽带标签14，以此，可降低成本，当然，在其他实施例中，所述自动割草机1上也可以是设有两个所述超宽带标签，且所述两个所述超宽带标签，其中一个位于另一个正前方，以此，在所述自动割草机1开始进行作业时，可以直接获得所述两个所述超宽带标签的绝对坐标，并以此计算获得初始航向，更为方便。
41.所述自动割草机1还包含充电模块(未图示)，当所述自动割草机1的电量低于预设值时，其会返回充电站2进行充电。当然，需要注意的是，在本实施例中，当所述超宽带基站的电量较低时，所述自动割草机1可以移动至所述超宽带基站处，并与所述超宽带基站对接，从而对所述超宽带基站进行充电，以此，用户无需经常为所述超宽带基站换电池，提高了用户体验。
42.本发明还提供了一种航向校正方法，所述航向校正方法用于自移动装置。
43.在本实施例中，所述航向校正方法用于所述自动割草机1，在所述自动割草机1按照工作计划进行了一段时间的作业后，由于草地的工作环境的影响及惯性导航系统积累的误差的影响，所述自动割草机1通过所述惯性导航系统获得的航向与准确的航向存在偏差，故需要进行航向校正，以使得所述自动割草机1继续按照准确的航向进行作业。
44.请具体参考图3，所述航向校正方法包含：
45.s1：检测所述自移动装置从第一位置移动至第二位置的移动状态，所述移动状态包括曲线移动、直线移动其中之一；
46.在本实施例中，在所述自动割草机1进行作业前，由用户设置工作计划或所述自动割草机1读取存储于本地存储器或网络服务器的已设置完成的工作计划，所述自动割草机1根据预设的工作计划进行作业。具体地，所述预设的工作计划包含所述自移动装置的位置
与移动状态的对应关系，以此，所述自动割草机1可根据预设的工作计划获得从所述第一位置移动至所述第二位置的移动状态，即：当所述自动割草机1移动至所述第一位置或者移动至所述第一位置与所述第二位置之间时，获取所述工作计划，并根据所述工作计划，获得在所述第一位置及所述第二位置间的具体移动状态。需要注意的是，在其他实施例中，所述移动状态也可以是用户通过移动终端发出，即：用户在观察到所述自动割草机1偏离了预定的前进方向，可通过移动终端发出指令以控制所述自动割草机的移动状态。当然，所述移动状态可以是通过所述惯性导航系统获得的，即：所述自动割草机1通过所述惯性导航系统，实现获取航向数据，根据航向数据判断当前的具体移动状态。
47.s2:当所述移动状态为曲线移动时，根据第一航向角校正值和第二航向角获得所述自移动装置到达所述第二位置时的航向角；
48.其中，所述第一航向角校正值根据所述无线电探测系统测量的自移动装置到达所述第二位置时的第一航向角和所述惯性导航系统测量的自移动装置从所述第一位置移动至所述第二位置的累计航向变化值获得。所述第二航向角根据自移动装置的初始航向角和所述惯性导航系统测量的自移动装置从初始位置移动至所述第二位置的累计航向变化值获得。
49.具体来说，当所述自动割草机1以曲线移动的移动状态由所述第一位置移动到所述第二位置时，由于草坪环境、超宽带系统的精度及惯性导航系统的精度等的影响，所述自动割草机1的航向角可能会产生误差，从而需要对所述自动割草机的航向角进行校正，以获得更为准确的航向角。
50.所述第二航向角根据自移动装置的初始航向角和所述惯性导航系统测量的自移动装置从初始位置移动至所述第二位置的累计航向变化值获得。在本实施例中，所述航向为所述自动割草机1的当前运动方向与x轴正方向的夹角，所述充电站2通常设置于x轴上。以此，当所述自动割草机1以所述充电站2作为初始位置出发时，所述初始航向角为0。当所述自动割草机1位于所述第二位置时，通过所述惯性导航系统可获得所述自动割草机1从初始位置移动至所述第二位置的累计航向变化值，将所述自动割草机1从初始位置移动至所述第二位置的累计航向变化值与所述初始航向角进行叠加，可获得所述自动割草机1的所述第二航向角。在所述自动割草机1按照预设的工作计划进行作业的过程中，所述惯性导航系统始终处于工作状态，并可实时获取所述自动割草机1的航向角，然而由于工作环境及陀螺仪精度的影响，所述惯性导航系统获得的所述第二航向角通常存在一定的误差，尤其是所述自动割草机1进行了长时间的工作后，所述第二航向角的误差逐渐累积，若不进行校正，则所述自动割草机1会偏离预定的移动方向，从而无法进行正常的作业。故，通常可通过所述超宽带系统对所述惯性导航系统获得的航向角进行校正，但是，当所述自动割草机1的移动状态为曲线移动时，所述超宽带系统获得的航向角仍是直线移动时的航向角，若以此航向角对所述惯性导航系统获得的航向角进行校正，则会使得自动割草机1偏离预定的移动方向，无法正常的作业。
51.以此，在本发明的优选实施例中，根据所述无线电探测系统测量的自移动装置到达所述第二位置时的第一航向角和所述惯性导航系统测量的自移动装置从所述第一位置移动至所述第二位置的累计航向变化值可获得第一航向角校正值。以此，通过所述第一航向角校正值，可对所述第二航向角进行校正，从而获得了所述自动割草机1处于所述第二位
置时的更为准确的航向，并提高了所述自动割草机1定位的准确性。
52.具体来说，所述第一航向角校正值等于所述第一航向角与二分之一的所述惯性导航系统测量的所述自移动装置从所述第一位置移动至所述第二位置的累计航向变化值之和。请具体参阅图2，所述自动割草机1从初始位置(未图示)移动至图示的a位置(即：所述第一位置)，所述自动割草机1根据所述预设的工作计划，获得从所述a位置移动到b位置(即：所述第二位置)的移动状态为曲线移动。所述自动割草机1在a位置的移动方向为图示的v1方向，所述航向角为所述自动割草机1的移动方向与x轴正方向的夹角，则当所述自动割草机1处于所述a位置时，其航向角设为a点航向角，即为图示的φ2。所述自动割草机1移动至图示b位置时，所述自动割草机1的移动方向为图示的v2方向，其航向角为所述第一航向角校正值，即为图示的φ4，图中所示的c点即为所述自动割草机1由a位置运动到b位置形成的圆弧的圆心。当所述自动割草机1以曲线移动的移动状态行驶至图示的b位置时，所述超宽带系统检测到的所述第一航向角为图示的φ3，而惯性导航系统检测到的所述自动割草机1从所述a位置移动至所述b位置的累计航向变化值为φ
4-φ2。由图2可知，φ4＝∠c φ2；以此，可得从而可得所述第一航向角校正值等于所述第一航向角与二分之一的所述惯性导航系统测量的所述自移动装置从所述第一位置移动至所述第二位置的累计航向变化值之和。
53.综上所述，当所述自动割草机1以曲线移动的移动状态由a移动至b时，通过超宽带系统获得的所述第一航向角是所述自动割草机1以直线移动的移动状态由a移动至b时获得的航向角，故，若直接用所述第一航向角对所述惯性导航系统获得的所述第二航向角进行校正，则明显会使得所述校正航向偏离了正确的航向。以此，在本实施例中，通过将所述超宽带系统直接获得的所述第一航向角与所述惯性导航系统检测到的所述自移动装置从所述第一位置移动至所述第二位置的累计航向变化值进行结合，获得了所述自动割草机1执行曲线移动时的所述第一航向角校正值，然后，再通过所述第一航向角校正值，对所述惯性导航系统获得的所述第二航向角进行校正，则可获得更为准确的所述自移动装置到达所述第二位置时的航向角。
54.进一步地，所述自移动装置到达所述第二位置时的航向角通过如下表达式获得：
55.φ
j
＝n1·
a1 n2·
a256.其中，φ
j
为所述自移动装置到达所述第二位置时的航向角，a1为所述第一航向角校正值，a2为所述第二航向角，n1、n2均为正数，并且如果所述自移动装置从所述初始位置移动至所述第二位置时的时间达到预设时间时，n1大于n2；如果所述自移动装置从所述初始位置移动至所述第二位置时的时间未达到预设时间时，n1小于n2。
57.具体来说，在本实施例中，若所述自移动装置从所述初始位置移动至所述第二位置时的累计时间未达到预设时间，则说明所述自动割草机1行驶时间较短，则此时所述惯性导航系统检测获得的第二航向角较为准确，从而将所述第二航向角的权重提高，并降低综合利用超宽带系统及惯性导航系统获得的所述第一航向角校正值的权重，以此，可提高所述自移动装置到达所述第二位置时的航向角的准确度。相反地，若所述自移动装置从所述初始位置移动至所述第二位置时的累计时间达到所述预设时间，则说明所述自动割草机1的行驶时间较长，此时综合利用超宽带系统及惯性导航系统获得的所述第一航向角校正值
较为准确，从而降低所述惯性导航系统检测到的第二航向角的权重，并提高综合利用超宽带系统及惯性导航系统获得的所述第一航向角校正值的权重，以此，可提高所述自移动装置到达所述第二位置时的航向角的准确度。当然，在其他实施例中，所述自移动装置到达所述第二位置时的航向角可以是通过卡尔曼滤波技术获得，而不局限于通过上述算法获得。
58.进一步地，在其他实施例中，当自移动装置以充电站之外的位置作为初始位置出发时，以自移动装置从初始位置移动预设距离后所到达的位置时所述无线电探测系统测量的航向角为所述初始航向角。具体地，当所述自动割草机1已离开所述充电站2，则控制所述自动割草机1从初始位置以直线移动至预设距离，并获取超宽度系统检测到的即时航向，以此航向为初始航向角。需要注意的是，若所述自动割草机1设有两个所述超宽度标签14，则所述自动割草机1也可以是获得所述两个所述超宽带标签14的绝对坐标，并计算获得初始航向角。
59.进一步地，所述航向校正方法还包含：
60.当检测的移动状态为直线移动时，根据所述第一航向角和所述第二航向角获得自移动装置到达所述第二位置时的航向角。
61.具体来说，当所述自动割草机1由所述第一位置移动至所述第二位置的移动状态为直线移动时，则所述超宽带系统可直接获得第一航向角，而根据初始航向角及所述惯性导航系统测量的自移动装置从初始位置移动至所述第二位置的累计航向变化值可获得第二航向角。根据所述第一航向角和所述第二航向角可获得所述自动割草机1到达所述第二位置时的航向角。以此，当所述自动割草机1直线移动时，可通过超宽带系统获得的航向对所述惯性导航系统获得的航向进行校正，从而提高了所述自动割草机1到达所述第二位置时的航向角的准确性。
62.进一步地，所述自移动装置到达所述第二位置时的航向角通过如下表达式获得：
63.φ
j
＝n1·
a3 n2·
a264.其中，φ
j
为所述自移动装置到达所述第二位置时的航向角，a3为所述第一航向角，a2为所述第二航向角，n1、n2均为正数，并且如果所述自移动装置从所述初始位置移动至所述第二位置时的时间达到预设时间时，n1大于n2；如果所述自移动装置从所述初始位置移动至所述第二位置时的时间未达到预设时间时，n1小于n2。
65.在本实施例中，若所述自移动装置从所述初始位置移动至所述第二位置时的累计时间未达到预设时间时，则说明所述自动割草机1行驶时间较短，则此时所述惯性导航系统检测获得的第二航向角较为准确，从而将所述第二航向角的权重提高，并降低利用超宽带系统获得的所述第一航向角的权重，以此，可提高所述自移动装置到达所述第二位置时的航向角的准确度。相反地，若所述自移动装置从所述初始位置移动至所述第二位置时的累计时间达到所述预设时间时，则说明所述自动割草机1的行驶时间较长，此时利用超宽带系统所述第一航向角较为准确，从而降低所述惯性导航系统检测到的第二航向角的权重，并提高利用超宽带系统获得的所述第一航向角的权重，以此，可提高所述自移动装置到达所述第二位置时的航向角的准确度。当然，在其他实施例中，所述自移动装置到达所述第二位置时的航向角可以是通过卡尔曼滤波技术获得，而不局限于通过上述算法获得。
66.在本发明的另一实施例中，还提供了一种自移动装置的控制系统，其特征在于，所述控制系统包含：
67.检测模块，配置为检测所述自移动装置从第一位置移动至第二位置的移动状态，所述移动状态包括曲线移动、直线移动其中之一；
68.获取模块，配置为根据所述无线电探测系统测量的自移动装置到达所述第二位置时的第一航向角和所述惯性导航系统测量的自移动装置从所述第一位置移动至所述第二位置的累计航向变化值获得所述第一航向角校正值，根据自移动装置的初始航向角和所述惯性导航系统测量的自移动装置从初始位置移动至所述第二位置的累计航向变化值获得所述第二航向角；
69.计算模块，配置为当所述移动状态为曲线移动时，根据第一航向角校正值和第二航向角获得所述自移动装置到达所述第二位置时的航向角。
70.进一步地，所述控制系统还包含计时模块，当所述自动割草机1开始进行作业时，所述计时模块开始记录行驶时间；在所述自动割草机1的工作过程中，所述计时模块始终在进行行驶时间的记录，同时进行所述行驶时间的累加。
71.在本发明的另一实施例中，还提供了一种自移动装置，所述自移动装置还包括：
72.一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现上述的航向校正方法。关于航向校正方法，这里不再赘述。
73.在本发明的另一实施例中，还提供了一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述航向校正方法。关于航向校正方法，这里不再赘述。
74.本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。