一种实现路径自动记忆及重现的两轮平衡车自主巡逻方法与流程

本发明涉及一种平衡车路径记忆及重现方法，尤其是涉及一种实现路径自动记忆及重现的两轮平衡车自主巡逻方法。

背景技术：

近年来，两轮平衡车因其运动灵活、智能控制、操作简单、节省能源、绿色环保等特点在现代交通工具中应用越来越广泛，如用于日常代步工具、警务人员治安巡查、广告宣传、辅助拍摄等。然而，除了其载人功能外，两轮平衡车作为机器人家族中的一员，在其自动行走、姿态调节等基本功能的基础上搭载某些传感器后即可在很多场合下替代巡逻机器人完成很多任务，如电站巡检、小区安保、日常巡逻、环境监测、军事侦察等。

对于具备自主巡逻功能的平衡车而言，其中一个重要功能是将平衡车行进过的路径信息记录保存下来，这样在不同应用场景下只需要控制平衡车巡逻一次，之后平衡车可根据该次行进过程中记忆的数据重现该路径，自主完成巡逻。对于搭载里程计的两轮平衡车来说，记忆路径信息时是通过预设定的时间段(如每10ms)记录左右轮的里程、速度等信息，当巡逻路径距离较长时或需要存储的路径条数较多的时候，数据量巨大，会占据大量的储存空间，而实际上平衡车在行进过程中记录的很多数据都是冗余数据，对于重现路径并非必需，因此有必要提供一种两轮平衡车的路径记忆及重现方法，该方法可大量节省平衡车记忆路径时存储的数据量。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种数据存储量小、可靠性高的实现路径自动记忆及重现的两轮平衡车自主巡逻方法。

本发明的目的可以通过以下技术方案来实现：

一种实现路径自动记忆及重现的两轮平衡车自主巡逻方法，所述的平衡车带有控制器及里程计量装置，所述的方法包括：

控制器初始化后，选择巡逻模式，若为路径记忆模式，则平衡车从起点开始行驶，里程计量装置分别对左轮和右轮的里程进行数据采样，控制器根据里程采样数据生成路径记录数据，并写入外部存储器，直到行驶结束，若为路径重现模式，则控制器从外部存储器读取路径记录数据，使平衡车按路径行驶。

路径记录数据包括顺序保存的多组计算数据，每组计算数据由某个采样时刻的左轮里程S_Li和右轮里程S_Ri计算得到，每组计算数据包括：该采样时刻的左轮里程差△S_Li、左轮速度V_Li和左右轮速度差△V_LRi，其中，i代表该采样时刻的序号，左轮里程差△S_Li＝S_Li—S_Li-1，S_Li为该采样时刻的左轮里程，S_Li-1为上一组被保存的计算数据所在采样时刻的左轮里程，左轮速度V_Li＝(S_Li-S_Li-1)/T，T为采样间隔，左右轮速度差ΔV_LRi＝V_Li-V_Ri＝(S_Li-S_Li-1)/T-(S_Ri-S_Ri-1)/T，V_Ri为右轮速度，其计算原理与左轮速度V_Li相同。

路径记忆模式中，控制器实时计算每组计算数据，并判断该组计算数据被保存或被舍弃，其中，起点或终点的计算数据被保存，对非起点或终点的计算数据的判断规则为：从上一组计算数据所在的采样时刻到本组计算数据采样时刻之间，若平衡车的运动为直线运动(包括匀速直线运动和非匀速直线运动)或圆周运动(包括匀速圆周运动和曲率不变的非匀速圆周运动)或暂停行驶，则舍弃本组计算数据，否则保存本组计算数据。

所述的路径记忆模式中，控制器实时计算每组计算数据，并判断该组计算数据被保存或被舍弃，其中，起点的计算数据被保存，对非起点的计算数据判断过程包括以下步骤：

S11，计算左右轮速度差变化值△△V_LRi，△△V_LRi＝△V_LRi—△V_LRi-1，若△△V_LRi＝0，则进入步骤S12，否则进入步骤S13；

S12，判断该组计算数据是否对应路径终点，若是，则保存该组计算数据，否则舍弃该组计算数据；

S13，判断是否成立，若不成立，则保存该组计算数据，并将临时存储单元中的V_L、△V_LR值更新为当前时刻的V_Li、ΔV_LRi，若成立，则返回步骤S12，其中，V_L、△V_LR分别为上一组被保存的计算数据中的左轮速度和左右轮速度差，存储在临时存储单元中，判断过程中，当△V_LRi＝0时，约定当△V_LR＝0时，约定

路径重现模式中，控制器从外部存储器读取计算数据，并根据计算数据与平衡车实际的左轮速度值得到平衡车实际的右轮速度值，两组计算数据之间，平衡车的左右轮速度差保持不变，直到重现下一组计算数据。

路径记忆模式中，路径记录数据被写入外部存储器的过程包括：控制器按顺序将各组计算数据保存在预设写缓存区中，写缓存区的数据打包发送至控制器的串口，通过串口写入外部存储器文件中。

路径记忆模式中，写缓存区设置多个，路径记录数据以循环方式写入写缓存区，具体包括以下步骤：

S21，路径记录数据中的计算数据按顺序保存在第一个写缓存区，当第一个写缓存区写满时，将数据打包发送至串口，并清掉第一个写缓存区中的数据；

S22，后续各组计算数据继续按顺序写入下一个写缓存区中，写缓存区写满后，将数据打包发送至串口，并清除该缓存区中的数据，若该写缓存区为最后一个写缓存区，则返回步骤S21，否则返回步骤S22，直到最后一组计算数据被写入。

路径重现模式中，控制器通过串口按顺序读取外部存储器中的计算数据，并按顺序存储至读缓存区中，同时，平衡车根据读缓存区中的计算数据进行路径重现，完成巡逻过程。

路径重现模式中，读缓存区设置多个，数量与写缓存区相同，路径记录数据以循环方式写入读缓存区，具体包括以下步骤：

S31，控制器与外部存储器建立连接，选择外部存储器中的对应文件；

S32，控制器通过串口按顺序读取对应文件中的部分计算数据，数据量根据单个读缓存区的大小决定，并打包存储至第一个读缓存区中，同时平衡车根据读缓存区中的计算数据进行路径重现，当前缓存区写满且对应的路径重现完成后，清除该缓存区数据；

S33，后续各组计算数据继续按顺序打包写入下一个读缓存区中，该读缓存区写满且对应的路径重现完成后，清除该读缓存区中的数据，若该读缓存区为最后一个读缓存区，则返回步骤S32，否则返回步骤S33，直到最后一组计算数据被重现。

与现有技术相比，本发明具有以下优点：

(1)路径记忆模式中，通过对左右轮里程进行数据采样，即可产生可实现路径重现的路径记录数据，存储在外部存储器中，路径重现模式中，通过读取外部存储器中的数据，可指导平衡车按路径行驶，外部存储可记录大量数据，可靠性高，可记录多条标准路径信息，路径重现时从外部存储中选择相应数据文件即可实现按照选择的路径自动巡逻。

(2)路径记录数据包括顺序保存的多组计算数据，每组计算数据由某个采样时刻的左轮里程S_Li和右轮里程S_Ri计算得到，包括左轮里程差、左轮速度和左右轮速度差，通过这三个数据，可唯一的得到当前平衡车的运动状态以及两组计算数据之间平衡车走过的里程，路径重现时，计算量小，有利于提高巡逻过程的流畅性。

(3)路径记忆模式中，从上一组计算数据所在的采样时刻到本组计算数据采样时刻之间，若平衡车的运动为直线运动或圆周运动或暂停行驶，则舍弃本组计算数据，否则保存本组计算数据，即舍弃了直线运动或圆周运动的中间段数据以及暂停时的数据，克服了路径信息局部重复性大造成的巨大数据量的问题，只记录有效数据，舍弃了冗余路径信息，操作方便，稳定性好，实时性高。

(4)路径重现模式中，两组计算数据之间，只需平衡车的左右轮速度差与左轮速度之比保持不变，直到重现下一组计算数据，即可使平衡车按照标准路径行走，对左右轮的速度无绝对限制。

(5)巡逻模式中，写缓存区设置多个，路径记录数据以循环方式写入写缓存区；路径重现模式中，读缓存区设置多个，数量与写缓存区相同，路径记录数据以循环方式写入读缓存区。缓存区的开辟，有利于提高路径记忆和路径重现的流畅性。

(6)硬件实现简单，只需在左轮和右轮分别搭载一部里程计即可实现，成本低。

附图说明

图1为本实施例方法的流程框图；

图2为本实施例方法在路径记忆模式下，用于判定是否舍弃当前时刻数据的流程图；

图3为本实施例方法路径记忆模式下将数据存入预设写缓存区的流程图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例

本实施例的两轮平衡自主巡逻车控制系统内核采用Cortex M3内核的ARM芯片STM32，在平衡车的左轮和右轮分别搭载一部里程计，分别记录平衡车行进过程中左轮和右轮的里程累计值。平衡车开机后，按照一个确定的采样时间间隔T(如T＝10ms)分别采集左右轮里程累计值S_Li和S_Ri，其中，i表示第i个采样时间序号。

本发明包括两个部分，其一是两轮平衡自主巡逻车按照设定的格式自动记忆巡逻路径，并将路径信息按照一定的格式和顺序写入外部存储中的方法，其二是两轮平衡车重现路径，即平衡车按照外部存储中存储的路径进行巡逻的方法。

需要记录的采样时刻的数据记录格式为：

左轮里程差△S_Li：左轮速度值V_Li：左右轮速度差△V_LRi

其中，左轮里程差△S_Li由当前左轮里程计里程值与上一记录点时刻的左轮里程计里程值之差，当前时刻的左轮速度值由左轮里程值和采样时间间隔计算得到，即V_Li＝(S_Li-S_Li-1)/T，当前时刻的左右轮速度差△V_i指左轮速度值与右轮速度值的差值，即ΔV_LRi＝V_Li-V_Ri＝(S_Li-S_Li-1)/T-(S_Ri-S_Ri-1)/T。左轮里程差、左轮速度值和左右轮速度差均为64位整型值，即long类型值。

参阅图1所示，为本发明两轮平衡自主巡逻车自动记忆路径以及重现路径的方法流程框图。两轮平衡车初始化后，根据模式参数mode的取值判断按照何种方式巡逻，如mode＝0，则开启路径记忆模式巡逻，如mode＝1，则开启路径重现模式巡逻。

路径记忆模式下两轮平衡车的巡逻过程，其包含下列步骤：

首先，平衡车从起始点开始巡逻，左右轮里程累计初始值分别为S_L0和S_R0，左轮里程差初始值设为ΔS_L0＝0。平衡车开始行进后，按照一个确定的采样时间间隔T分别采集左右轮里程累计值，假设采集时间序列号为i(i＝1,2,3,…)，行进过程中左右轮里程累计值分别记为S_Li和S_Ri，根据前述计算方法可得到左轮里程差△S_Li、左轮速度值V_Li和左右轮速度差△V_LRi。

如平衡车在某段时间内为直线运动(包括匀速直线运动和非匀速直线运动)或转弯运动(包括匀速转弯运动和转弯曲率为定值的非匀速转弯运动)，转弯运动即圆周运动，则可以只记录该段时间的起始时刻和结束时刻的数据值，中间时段的数据均可以舍弃。此外，对于平衡车在巡逻过程中在某点暂时停车的时段的数据也可以舍弃。

平衡车巡逻路径的起始点时刻和终止点时刻的数据值总是要记录下来，平衡车到达起始点时，系统给出一个标志信号f_start，按格式记录该起始点时刻数据。到达终止点时，系统给出一个标志信号f_end，按格式记录该终止点时刻数据。正常情况下起始点和终止点的数据值△S_Li、V_Li和△V_LRi均为零。

在内存开辟2个数据存储单元，分别临时存储V_L、△V_LR，这2个数据的初始值为起始点时刻的值，当某一时刻的数据判定为记录后，将V_L和△V_LR的值更新为当前时刻值，否则保持不变。判断某时刻点数据是否记录时，先暂存该时刻点数据V_Li、△V_LRi，结合临时存储值V_L、△V_LR来判断，如果当前时刻点相对上一记录时刻是匀速或非匀速直线运动时，△V_LRi＝0，△△V_LRi＝|△V_LRi-△V_LR|＝0；如果当前时刻点相对上一记录时刻是匀速转弯运动时，△△V_LRi＝|△V_LRi-△V_LR|＝0；而对于非匀速转弯运动，当前时刻点相对上一记录时刻的△V_LR和△△V_LRi均不为零，此时需要结合转弯曲率来判断，对于曲率为定值的非匀速转弯运动，中间时段的数据值也可以舍弃。

设某段转弯路径中的曲率半径为ρ，由曲率半径公式：

$\mathrm{\ρ}=\frac{V}{w}=\frac{(V_L+V_R)/2}{(V_L-V_R)/R}$

其中，V为平衡车质心线速度，w为平衡车质心角速度，V_L为平衡车左轮线速度，V_R为平衡车右轮线速度，ΔV_LR＝V_L-V_R，R为平衡车左轮与右轮两轮轴间距。

如果当前时刻点相对上一记录时刻是曲率不变的非匀速转弯运动时，ρ_i＝ρ，即：

${\mathrm{\ρ}}_i=\frac{(V_{Li}+V_{Ri})/2}{(V_{Li}-V_{Ri})/R}=\mathrm{\ρ}=\frac{(V_L+V_R)/2}{(V_L-V_R)/R}$

化简后得到因此可以根据左轮速度与左右轮速度差的比值是否发生变化来判断其是否是曲率不变的非匀速转弯运动。

如果平衡车在巡逻过程中在某点暂时停车，那么暂停时段的数据也可以舍弃，此时△V_LRi＝0，△△V_LRi＝0。

根据上述推导，参阅附图2所示，判断当前数据是否舍弃的流程如下：

首先判断平衡车是否到达起始点，如果当前时刻到达起始点，则记录该时刻的数据值，并将该时刻的V_L、△V_LR值存储在内存临时存储单元中；如果不是起始点，则暂存该点的V_Li、△V_LRi值，并结合临时存储单元中的V_L、△V_LR值判断，当△△V_LRi≠0不成立时，接着判断该点是否为平衡车巡逻路径的终止点，如果是，则当前时刻的数据值判定为记录，如果不是终止点，则不记录当前时刻数据；当△△V_LRi！＝0成立时，接着判断是否成立，如果不成立，则当前时刻的数据值判定为记录，并用当前时刻值更新临时存储单元中的V_L、△V_LR值；如果成立，则转向判断该点是否为终止点的节点。需要说明的是，判断这个条件时，存在两种特殊情况，一种是当△V_LRi＝0且△V_LR≠0，对应平衡车从直线运动变为转弯运动的转折点；另一种是△V_LRi≠0且△V_LR＝0，对应平衡车从转弯运动变为直线运动的转折点。这两个转折点时刻的数据均应记录下来，但此时这个计算式没有意义，因此在此做一个约定：当△V_LRi＝0时，约定当△V_LR＝0时，约定

按照此判断准则，直线运动和转弯运动只记录该段运动的起始时刻和结束时刻的数据值，其他时刻的数据值均需要记录，所有需记录的数据按照前述记录格式记录下来，并按顺序写入STM32的预设写缓存区中。预设写缓存区大小和个数由采样时间间隔、存储数据格式、串口传输速率、外部存储写入速率等参数共同决定。以2个预设写缓存区为例，请参考附图3所示，STM32中开辟两个大小相等的预设写缓存区分A、B，初始化时设置缓存区标志信号flag＝0，需要写入数据时，首先写入缓存区A，当缓存区A写满时，将flag信号置为1，将缓存区A的数据打包发送至串口，同时清除缓存区A中的数据，ARM通过串口接收数据并写入外部的SD卡文件中。而后续数据将陆续写入缓存区B，同理，当缓存区B写满时，将flag信号置为0，将缓存区B的数据打包发送至串口，同时清除缓存区B中的数据，ARM通过串口接收数据并继续写入外部的SD卡文件中。所有需记录的数据按照此方法循环写入缓存区A、B中并最终保存在外部SD的文件中。外部SD卡的容量由存储数据格式、记忆路径总距离、记忆路径最大数量等参数共同决定。

当平衡车到达终点后，结束本次巡逻路径记忆，将本次连续路径存储在SD卡内的数据文件命名为Road_N，其中N代表记录下来的第N条巡逻路径(N＝1,2,3,…)，所有路径文件储存于外部存储器中，完成了路径自动记忆功能。外部存储器中的所有巡逻路径均可用于两轮平衡自主巡逻车在路径重现时使用。

路径重现模式下两轮平衡车的巡逻过程，其包含下列步骤。

首先，平衡车选择外部存储器中某条路径Road_N，准备按该条路径巡逻行进。在STM32中预设多个读缓存区，其个数与大小与前述写缓存区相同。仍以2个读缓存区为例，预设的读缓存区分为缓存区C和缓存区D，其大小均与缓存区A大小相同。

ARM按存储顺序逆向读取文件Road_N中部分数据，数据量由缓存区C(或缓存区D)的大小决定。与路径记忆过程中的数据存储过程恰好相反，先将数据打包通过串口发送至STM32，并将其存储至读缓存区C中，存储完毕后，ARM继续按存储顺序逆向读取文件Road_N中剩下的数据，将数据通过串口发送至STM32，并将其存储至读缓存区D中，同时STM32读取缓存区C中的数据包，并按照里程对比方式控制平衡车的方向及路程。当这段路径重现完毕后，清除缓存区C中的数据，后续数据又按照此方法存储入缓存区C中，同时STM32读取缓存区D中的数据包，并按照里程对比方式控制平衡车的方向及路程。按照此方法循环交替读取缓存区C和缓存区D中的数据包，并使平衡车按照这些数据重现循环路径。

在读取读缓存区中的数据时，根据数据特点判断某些时段是否为直线运动或转弯运动，STM32按照里程对比方式控制平衡车方向及路程。

如相邻两个记录点中前一个记录点的△V_LR＝0，则该段运动为直线运动，STM32按照里程对比方式控制平衡车按照该段数据直线行走，即根据后一记录点记录数据中的左轮里程差△S_Li值确定小车直线行走的距离。

如相邻两个记录点中前一个记录点的△V_LR≠0，则该段运动为转弯运动，STM32按照里程对比方式控制平衡车行进的具体方法如下。

设预定标准路径(即某条自动记忆的路径)中该段转弯运动的曲率半径为ρ，由曲率半径公式：

$\mathrm{\ρ}=\frac{V}{w}=\frac{(V_L+V_R)/2}{(V_L-V_R)/R}$

其中，V为平衡车质心线速度，w为平衡车质心角速度，V_L为平衡车左轮线速度，V_R为平衡车右轮线速度，ΔV_LR＝V_L-V_R，R为平衡车左轮与右轮两轮轴间距。

为保证平衡车在重现路径中按照标准路径行进，必须保证其也以固定曲率ρ匀速转弯，设其曲率为ρ'，即需要满足：

ρ'＝ρ

即：

$\frac{({V_L}^{\′}+{V_R}^{\′})/2}{({V_L}^{\′}-{V_R}^{\′})/R}=\frac{(V_L+V_R)/2}{(V_L-V_R)/R}$

其中，V_L'为平衡车重现路径时左轮线速度，V_R'为平衡车重现路径时右轮线速度，V_L'＝V_R'+ΔV_LR'。

化简后得到：

$\frac{{V_R}^{\′}}{{V_L}^{\′}}=\frac{V_R}{V_L}$

根据前一记录点记录的左轮速度值V_L和左右轮速度差△V_LR可以得到左轮速度V_L和右轮速度V_R，在重现路径时，可设定平衡车左轮线速度V_L'值，则可根据上式计算得到平衡车右轮线速度V_R'。结合后一数据点的左轮里程差△S_L值，按照这三个参数控制平衡车行进△S_L即可完成平衡车在重现路径中按照标准路径完成这段转弯运动。

当文件Road_N中所有数据均陆续读取到缓存区中，平衡车也按照缓存区中所有记录的数据依次序完成了行进，即平衡车到达终点后，完成了对外部存储中第N条记忆路径的重现。