基于人类示教学习的路径规划方法、系统、设备及介质

1.本发明涉及一种基于人类示教学习的路径规划方法、系统、设备及介质，属于家禽养殖装备自动化技术领域。


背景技术：

2.中国是农业大国，规模化笼养家禽养殖技术和舍内农用技术装备的持续发展催生了舍内自主移动机器人的诞生，舍内自主移动机器人目前应用在舍内巡检、消杀喷洒和打扫清污等作业方面，而舍内自主移动机器人的作业均离不开作业路径的规划，其中，作业路径的规划策略包括全局路径规划策略与遇到障碍物时所采取的局部路径规划策略。
3.目前，在笼养家禽养殖场的路径规划大多沿用家用或工业用室内机器人的路径规划方法，但存在三种缺陷，具体如下：
4.1、采用激光雷达或视觉传感器建立栅格地图，在上位机中进行全局和局部路径规划，但搭载导航级别的激光雷达成本较高，而且鸡笼上部或底部镂空、粉尘积聚并漂浮在空气中而导致的能见度降低会影响建图效果，导致运行稳定性欠佳，实际导航效果并不完全适用笼养家禽养殖场的实际环境。
5.2、采用舍内布置磁条的路径规划方式，除了需要对禽舍环境进行改造之外，还可能受到鸡笼金属材质的影响，导致导航精度降低，并且需要人工定时维护，当重新规划路径时，需要重新铺设磁条；此外，移动机器人只能沿着磁条行进作业，若磁条上出现障碍物，则无法实现智能避让。
6.3、采用室内定位技术，例如：二维码导航、rfid、uwb等进行导航，前期部署成本高，且定位精度与部署位置、密度有关，需要投入较大成本且后期维护成本高，无法实现快速部署。


技术实现要素：

7.有鉴于此，本发明提供了一种基于人类示教学习的路径规划方法、系统、计算机设备及存储介质，其通过遥控器或手柄，操控移动机器人从设定的出发点无碰撞地运动到目标终点，以此作为一次示教演示过程，并对一次示教演示过程中移动机器人的运动数据进行采样；将多次示教采样的数据输入单一自由度位移的动态运动基元模型进行训练，使移动机器人能够复现人工示教轨迹并进行规划路径作业，实现移动机器人在非结构鸡舍内的快速路径规划和作业。
8.本发明的第一个目的在于提供一种基于人类示教学习的路径规划方法。
9.本发明的第二个目的在于提供一种基于人类示教学习的路径规划系统。
10.本发明的第三个目的在于提供一种计算机设备。
11.本发明的第四个目的在于提供一种存储介质。
12.本发明的第一个目的可以通过采取如下技术方案达到：
13.一种基于人类示教学习的路径规划方法，所述方法包括：
14.获取同一起点和终点的多条示教轨迹；
15.识别示教轨迹中的关键动作点，并根据所述关键动作点，对示教轨迹进行分割，得到多条子示教轨迹；
16.在时间序列上，对两两示教轨迹的轨迹点进行对齐；
17.以关键动作点为节点，根据单一自由度位移的动态运动基元模型，对前后节点之间的子示教轨迹进行学习，生成拟合示教轨迹的复现轨迹。
18.进一步的，所述示教轨迹是通过多通道遥控器对移动机器人进行控制而获得；
19.所述多通道遥控器上设置有操纵器；
20.所述多通道遥控器包括在垂直方向上操纵器控制移动机器人移动所对应的第一通道和在水平方向上操纵器控制移动机器人移动所对应的第二通道；
21.所述通过多通道遥控器对移动机器人进行控制，具体包括：
22.根据操纵器的控制幅度大小，对所述第一通道和所述第二通道的信道值进行混合换算，输出移动机器人左右电机各自的转速控制值；
23.将所述转速控制值换算成转速pwm值，实现对移动机器人的控制。
24.进一步的，所述操纵器为自复位摇杆；
25.所述识别示教轨迹中的关键动作点，具体包括；
26.在示教过程中，若n1＞end_action_fb＞m1和n2＞end_action_lr＞m2，则判断移动机器人结束大角度动作，以及得到结束时间节点；
27.根据结束时间节点，将移动机器人的坐标位置锁存在第一数组的对应顺序位中，并将移动机器人的航向角锁存在第二数组的对应顺序位中，由此识别得到关键动作点；
28.所述end_action_fb和end_action_lr，如下式：
29.end_action_fb＝fb_signal/fb_zero
30.end_action_lr＝lr_signal/lr_zero
31.其中，fb_signal表示所述第一通道的信道值，lr_signal表示所述第二通道的信道值，fb_zero表示当摇杆处于自复位位置或仅在垂直方向上有一个方向上的位移时，所述第一通道的对应信道值，lr_zero表示当摇杆处于自复位位置或仅在水平方向上有一个方向上的位移时，所述第二通道的对应信道值，n1表示第一设定阈值，m1表示第二设定阈值，n2表示第三设定阈值，m2表示第四设定阈值。
32.进一步的，所述在时间序列上，对两两示教轨迹的轨迹点进行对齐，包括：
33.选定一条示教轨迹作为参考轨迹；
34.根据动态时间规整算法，对参考轨迹的轨迹点与其余每条示教轨迹的轨迹点进行对齐，确定示教轨迹之间的最短距离评估相似度，以及对不同示教轨迹的轨迹点之间进行时间步对齐。
35.进一步的，所述根据单一自由度位移的动态运动基元模型，对前后节点之间的子示教轨迹进行学习，生成拟合示教轨迹的复现轨迹，具体包括：
36.设定所述动态运动基元模型的初始参数和定义用于逼近示教轨迹运动的强迫项的非线性函数；
37.根据所述初始参数和所述动态运动基元模型，得到单一自由度示教轨迹中的强迫项；
38.将所述子示教轨迹的运动数据集代入所述强迫项并结合所述动态运动基元模型，生成目标逼近函数；
39.根据目标逼近函数，采用局部加权回归，求解最佳权重；
40.根据最佳权重，使所述非线性函数近似逼近所述强迫项，以生成拟合子示教轨迹的复现轨迹，进而生成拟合示教轨迹的复现轨迹。
41.进一步的，所述初始参数包括起始点的横坐标取值、目标点的横坐标取值、起始点的纵坐标取值和目标点的纵坐标取值。
42.进一步的，在所述生成拟合示教轨迹的复现轨迹之后，还包括：
43.控制移动机器人沿着拟合示教轨迹的复现轨迹进行作业；
44.当任意时刻移动机器人和障碍物之间的距离小于第五设定阈值时，则对该时刻移动机器人的轨迹点和移动机器人的目标点以根节点的形式进行扩展，以分别构建第一随机树和第二随机树；
45.根据所述轨迹点和所述目标点的距离、障碍物坐标与形状信息和所述轨迹点的坐标信息，以及利用所述动态运动基元模型生成的复现轨迹点，为第一随机树和第二随机树导向并生成修正节点；
46.求解两棵随机树中距离修正节点最近的第一节点；
47.从所述第一节点出发，向修正节点延伸扩展步长，得到第二节点；
48.若检测到第二节点与障碍物不碰撞，则将第二节点添加到第一节点所在的随机树中；
49.重复上述操作，直至第一随机树和第二随机树各自的第二节点之间的距离小于第六设定阈值，进而生成避障路径。
50.本发明的第二个目的可以通过采取如下技术方案达到：
51.一种基于人类示教学习的路径规划系统，所述系统包括：
52.获取单元，用于获取同一起点和终点的多条示教轨迹；
53.识别与分割单元，用于识别示教轨迹中的关键动作点，并根据所述关键动作点，对示教轨迹进行分割，得到多条子示教轨迹；
54.对齐单元，用于在时间序列上，对两两示教轨迹的轨迹点进行对齐；
55.复现单元，用于以关键动作点为节点，根据单一自由度位移的动态运动基元模型，对前后节点之间的子示教轨迹进行学习，生成拟合示教轨迹的复现轨迹。
56.本发明的第三个目的可以通过采取如下技术方案达到：
57.一种计算机设备，包括处理器以及用于存储处理器可执行程序的存储器，所述处理器执行存储器存储的程序时，实现上述的路径规划方法。
58.本发明的第四个目的可以通过采取如下技术方案达到：
59.一种存储介质，存储有程序，所述程序被处理器执行时，实现上述的路径规划方法。
60.本发明相对于现有技术具有如下的有益效果：
61.1、本发明提供的基于人类示教学习的路径规划方法基于示教学习思想，利用dmp模型对人工示教轨迹进行编码学习，即完成了“示教－训练－复现”这一流程的路径规划，为传统路径规划方法中比较依赖环境建模且实时性不佳等问题提供了新的解决思路；
62.2、本发明提供的基于人类示教学习的路径规划方法采用动态时间规整算法对示教数据进行预处理，即将每轮示教中由于人为操作、传感器干扰和时间不一致产生的数据进行预先的优化处理，以提高复现精度；
63.3、本发明提供的基于人类示教学习的路径规划方法采用识别大角度动作为关键动作点的方式，并根据关键动作点，将示教轨迹分割成一系列子示教轨迹，然后通过dmp模型对一系列子示教轨迹进行学习，降低了在规模化鸡舍中示教轨迹过长对整体复现效果的不佳影响；
64.4、本发明提供的基于人类示教学习的路径规划方法在面对笼养家禽养殖场中的复杂动态作业环境时，通过dmp模型生成的复现轨迹点，不断地为bi-rrt算法提供趋近于复现轨迹的双向导向信息，其中，bi-rrt算法具有局部修正轨迹的能力，对动态变化的场景具有较好的适应性，可以使绕行障碍物后的轨迹和避障前的复现轨迹依旧保持形状相似。
附图说明
65.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
66.图1为本发明实施例1的路径规划导航系统的结构图。
67.图2为本发明实施例1的基于人类示教学习的路径规划方法的简要流程图。
68.图3为本发明实施例1的基于人类示教学习的路径规划方法的具体流程图。
69.图4为本发明实施例1的人工示教部分的流程图。
70.图5为本发明实施例1的复现轨迹部分的流程图。
71.图6为本发明实施例1的预处理和训练过程的流程图。
72.图7为本发明实施例1的复现轨迹及避障轨迹的示意图。
73.图8为本发明实施例1的bi-rrt算法扩展树生长的示意图。
74.图9为本发明实施例2的基于人类示教学习的路径规划系统的结构框图。
75.图10为本发明实施例3的计算机设备的结构框图。
76.其中，1-上层控制单元，2-底层控制单元，3-遥控器，4-姿态传感器，5-传感器装置，6-电机控制器，7-lcd液晶屏幕，8-电机，9-增量式编码器。
具体实施方式
77.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
78.本说明书和权利要求书中，术语“第一”、“第二”等是用于区别类似的对象，而不用于描述指定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便本技术的实施例能够以除了在这里图示或描述的那些以外的顺序实施，且“第一”、“第二”所区别的对象通常为一类，并不限定对象的个数，例如第一对象可以是一个，也可以是多个。
此外，说明书以及权利要求中“和”、“与”表示所连接对象的至少其中之一，字符“/”一般表示前后关联对象是一种“或”的关系。
79.本说明书中，移动机器人或简称为机器人；驱动电机或简称为电机；移动车体或简称为车体；多通道遥控器为十通道遥控器，或者简称为遥控器；自复位摇杆或简称为摇杆；控制机器人前进/后退的通道或简称为fb通道/第一通道，控制机器人左转/右转的通道或简称为lr通道/第二通道，控制机器人运行状态的通道或简称为os通道，控制机器人状态初始化的通道或简称为oz通道；dmp模型即是单一自由度位移的动态运动基元模型；rrt指的是快速搜索随机树算法(rapidly-exploring random tree)；bi-rrt算法指的是双向快速扩展随机数算法。
80.实施例1：
81.如图1所示，本实施例提供了一种路径规划导航系统，该系统包括上层控制单元、底层控制单元、姿态传感器、传感器装置和电机控制器；其中：上层控制单元、底层控制单元、姿态传感器、传感器装置、电机控制器安装在移动机器人上；上层控制单元、姿态传感器、传感器装置、电机控制器与底层控制单元连接。
82.本实施例中，通过十通道遥控器对机器人进行控制，遥控器涉及四个信号通道，包括控制机器人前进/后退的通道、控制机器人左转/右转的通道、控制机器人运行状态的通道和控制机器人初始化状态的通道；其中：控制机器人前进/后退的通道和控制机器人左转/右转的通道通过自复位摇杆进行控制，控制机器人运行状态的通道通过三段式自锁拨杆开关进行控制，控制机器人初始化状态的通道通过二段式自复位拨杆开关进行控制；遥控器与接收机配对使用，该接收器与底层控制单元的uart串口电连接，进而通过sbus协议，以dma直接存储器访问的形式将遥控器各通道的信道值传入底层控制单元中，由此底层控制单元对遥控器各通道的信道值进行识别和监测。
83.具体地，底层控制单元通过分析控制机器人运行状态的通道的信道值，将机器人设置为三种运行状态，如下式：
[0084][0085]
其中，os表示控制机器人运行状态的通道的信道值。
[0086]
实际应用中，接收机的输出信号可以是ppm信号、sbus信号或pwm信号。
[0087]
本实施例中，机器人为移动车体，车体内部安装有两个左右驱动电机，两个电机的输出轴分别与对应的驱动轮连接，两个电机的输出轴还与增量式编码器连接；其中：底层控制单元通过can总线与与左右电机对应连接的电机控制器电连接，进而通过canopen通讯协议，以帧报文的形式向左右电机发送速度指令；增量式编码器通过信号线与电机控制器连接，因此在底层控制单元中，可以读取连接can总线上相应地址的电机控制器的增量式编码器的脉冲反馈信号，并在相应程序中运算，即可得到车体实时的里程信息。
[0088]
例如：在can总线上0组1号标记为左电机，0组2号标记为右电机，于是便可以通过给对应地址的电驱控制器发can报文，控制对应的电机的运动，左右两轮为独立控制，通过左右轮差速来调整车体的移动方向，于是底层控制单元便可通过can总线，实现将所述上层
控制单元运算得到的控制信号发给所述底层控制单元，底层控制单元以can的通讯协议的形式发给电驱控制器，来控制驱动电机的转速大小，以此来完成复现过程。
[0089]
底层控制单元和电机控制器形成控制车体移动和转向的底层驱动机构，该底层驱动机构还包括本领域人员熟知的其他驱动部件，这里不再赘述。
[0090]
本实施例中，姿态传感器通过串口以dma直接存储器访问的形式将机器人的实时航向角传入底层控制单元中；传感器装置为五路超声波传感器，五路超声波传感器布置在车体上，并通过rs458通讯接口以轮询的形式将四周的实时距离数据传入底层控制单元中。障碍物位置坐标信息求解联合五路超声波距离传感器的扫描方向和距离障碍物的大小信息，运算得出前方障碍物的坐标信息。碰撞检测采用安装在车体上的五路超声波传感器检测四周的障碍物距离车体的距离大小，为避免车体与障碍物直接接触，设定与障碍物的碰撞裕度为0.3m。
[0091]
本实施例中，底层控制单元与上层控制单元之间通过uart串口进行通信；其中：底层控制单元采用arm微控制器，芯片型号为stm32f04；上层控制单元采用windows10操作系统，在matlab中编程，以实现数据处理、模型训练和控制下发。
[0092]
路径规划导航系统还包括lcd液晶屏幕，lcd液晶屏幕通过spi协议驱动，用于显示机器人的实时运行状态并进行相关文字提示。
[0093]
值得注意的是，在示教演示过程中，采用卡尔曼滤波(kf)消除因惯性器件在车体长时间运行而引发的环境退化问题，以获取精确的位置信息，具体步骤如下：
[0094]
s11、预测：
[0095][0096]
s12、更新：
[0097]
s13、计算后验概率的分布：
[0098][0099][0100]
上述所得的既是修正之后的距离信息；
[0101]
其中，k表示卡尔曼增益，表示k时刻系统的状态变量，uk表示k时刻系统的控制变量；b表示系统输入控制矩阵，表示系统k-1时刻的估计状态；ak和ck分布为系统状态转移矩阵和测量矩阵；r和qk分布为过程噪声矩阵和测量噪声矩阵；表示上一次协方差矩阵，表示当前状态协方差矩阵；zk表示k时刻传感器的测量向量，表示系统k时刻的估计状态。
[0102]
如图2～图6所示，本实施例提供了一种基于人类示教学习的路径规划方法，基于上述路径规划导航系统实现，并运用于笼养家禽养殖场，该方法包括以下步骤：
[0103]
s201、获取同一起点和终点的多条示教轨迹。
[0104]
s2011、整机上电，并通过遥控器，将三段式自锁拨杆开关设置为“遥控示教模式”，即将机器人的运行状态设置为人工示教状态。
[0105]
s2012、在遥控示教模式下，利用遥控器控制机器人从起点无碰撞地运动到终点。
[0106]
步骤s2012中，以频率50hz采集姿态传感器和增量式编码器的数据，具体为：通过
姿态传感器采集车体的加速度通过增量式编码器采集车体的位移x和速度υ。
[0107]
步骤s2012中，还包括：采用经卡尔曼滤波修正的距离值，并利用航迹推算法预测车体在二维笼养家禽养殖场坐标系下的实时位置坐标，并以频率50hz上传至上层控制单元，用于验证下述dmp模型生成复现轨迹的准确性；其中：该实时位置坐标的形式为：坐标向量p＝(x，y)，x，y表示机器人当前所在位置的横、纵坐标值。
[0108]
值得注意的是，在复现过程中，对机器人的实时位置坐标进行定时采样，在二维笛卡尔坐标系平面上生成复现过程的轨迹点图，与示教过程采样位置坐标后生成的示教过程的轨迹点图进行比对，进而判定本次训练完毕后的模型输出生成的复现轨迹精准度是否达标，用于辅助人为决定是否增加训练轮次或重新开始模型训练。
[0109]
本实施例中，设定人工示教次数为三次，程序在判定结束一次人工示教时会相应判定是否完成三次人工示教，若否，则通过可视化界面提示“请人为控制移动机器人回到初始点，进行下一次训练。”；若是，则通过可视化界面提示“请人为控制移动机器人回到示教初始点，开始复现。”，此时，将三段式自锁拨杆开关设置为“复现模式”。此外，当人为控制一次示教过程结束之后，即在选择结束示教之后，控制机器人回到初始点的过程并不会进行数据采样，以区分需要记录的轨迹和无需记录的轨迹。
[0110]
遥控器包括在垂直方向上操纵器控制移动机器人移动所对应的第一通道和在水平方向上操纵器控制移动机器人移动所对应的第二通道，其中，遥控器上设置有操纵器，操纵器为自复位摇杆。
[0111]
本实施例中，通过多通道遥控器对移动机器人进行控制，具体为：通过遥控器以混合控制(mix)方式对机器人的左右电机的转速进行控制。
[0112]
通过遥控器以混合控制(mix)方式对机器人的左右电机的转速进行控制，具体步骤如下：
[0113]
s21、设定摇杆垂直方向移动对应前进/后退通道(fb通道，在垂直方向上操纵器控制移动机器人移动所对应的第一通道为fb通道)，某一摇杆位置下的对应信道值为fb_signal，设定摇杆水平方向移动对应左转/右转通道(lr通道，在水平方向上操纵器控制移动机器人移动所对应的第二通道为lr通道)，某一摇杆位置下的对应信道值为lr_signal；当摇杆处于自复位位置或仅在一个方向上有位移时，底层控制单元接收到fb通道的信道值为fb_zero、lr通道的信道值为lr_zero，设定摇杆的自复位位置对应零转速位置。
[0114]
s22、根据多通道遥控器的操纵器控制幅度大小，对所述第一通道和所述第二通道的信道值进行混合换算，输出移动机器人左右电机各自的转速控制值，即根据摇杆的控制幅度大小，将fb通道与lr通道的信道值经底层控制单元中混合换算后作为左右电机的实际转速。
[0115]
步骤s22，如下式：
[0116]
rpm_left_motor＝[(fb_signal-fb_zero)-(lr_signal-lr_zero)]-fb_zero，
[0117]
rpm_right_motor＝[(fb_signal-fb_zero) (lr_signal-lr_zero)] lr_zero
[0118]
其中，rpm_left_motor和rpm_right_motor分别为当前摇杆的操控状态下，对应两个通道的信道值所对应的左右电机转速。
[0119]
s23、将所述转速控制值换算成转速pwm值，如下式：
[0120]
pwm_left_motor＝(rpm_left_motor-fb_zero)
×kl
[0121]
pwm_right_motor＝(rpm_right_motor-lr_zero)
×
kr[0122]
其中，pwm_left_motor和pwm_right_motor为对应左右电机转速的pwm值，k
l
和kr为左、右电机的控制系数，用于改变pwm输出信号的正负与最大幅值。
[0123]
s202、识别示教轨迹中的关键动作点，并根据所述关键动作点，对示教轨迹进行分割，得到多条子示教轨迹。
[0124]
在人工示教过程中，航向角变化阈值、横坐标变化阈值和纵坐标变化阈值，用于判定机器人是否开始进行一次性大角度动作；具体地，在判定当前时刻机器人的航向角与上一时刻机器人的航向角的差值是否超过对应的设定变化阈值的同时，判定横、纵坐标的变化值是否分别低于对应的设定变化阈值，若是，则表明机器人在全局坐标系下的坐标位置未产生大幅变化的情况下，开始进行一次性大角度动作。
[0125]
本实施例中，设定航向角变化阈值为45
°
，设定横坐标变化阈值和纵坐标变化阈值为30cm；在判定当前时刻机器人的航向角与上一时刻机器人的航向角的差值是否超过45
°
的同时，判定横、纵坐标的变化值是否分别低于30cm，若是，则表明机器人在全局坐标系下的坐标位置未产生大幅变化的情况下，开始进行一次性大角度动作，即关键动作，通过识别该动作结束的时刻，将该动作结束的时刻的对应点设置为关键动作点；利用开辟的二维坐标数组node_coord[][](第一数组)和一维角度数组node_angle_bias[](第二数组)，分别记录关键动作完成时刻机器人的位置坐标和航向角。
[0126]
获得关键动作完成时刻的大体思路为：依据人类操作习惯，当人们试图完成大角度动作时，通常lr通道的信道值变化较大，而fb通道的信道值则在fb_zero上下波动。
[0127]
本实施例中，获得关键动作完成时刻以及结合记录关键动作完成时刻的机器人的位置坐标和航向角的过程，即识别示教轨迹中的关键动作点，具体如下：
[0128]
示教过程中，若n1＞end_action_fb＞m1以及n2＞end_action_lr＞m2，则判定移动机器人结束大角度动作并得到结束的时间节点。
[0129]
具体地，n1为1700，m1为1250；n2为1050，m2为950。
[0130]
根据所述时间节点，将移动机器人的坐标位置锁存在第一数组对应顺序位中，并将移动机器人的航向角锁存在第二数组对应顺序位中，由此识别得到关键动作点。
[0131]
本实施例中，end_action_fb和end_action_lr，如下式：
[0132]
end_action_fb＝fb_signal/fb_zero
[0133]
end_action_lr＝lr_signal/lr_zero
[0134]
其中，fb_signal表示所述第一通道的信道值，lr_signal表示所述第二通道的信道值，fb_zero表示当摇杆处于自复位位置或者仅在垂直方向有一个方向上的位移时，所述第一通道的对应信道值，lr_zero表示当摇杆处于自复位位置或者仅在水平方向有一个方向上的位移时，所述第二通道的对应信道值，n1表示第一设定阈值，m1表示第二设定阈值，n2表示第三设定阈值，m2表示第四设定阈值。
[0135]
根据所述关键动作点，对示教轨迹进行分割，得到多条子示教轨迹，具体为：利用关键动作点对示教轨迹进行分割，以简化轨迹，即根据示教轨迹中机器人动作，将示教轨迹分为一系列子轨迹，例如：直线－右转，或直线－右转－直线，然后对单个子示教轨迹分别进行优化，降低优化成本，参考步骤s203。
[0136]
s203、在时间序列上，对两两示教轨迹的轨迹点进行对齐。
[0137]
在完成所有轮次的示教过程之后，选定一条示教轨迹作为参考轨迹；根据动态时间规整算法，对参考轨迹的轨迹点与其余每条示教轨迹的轨迹点进行对齐，确定示教轨迹之间的最短距离评估相似度，以及对不同示教轨迹的轨迹点之间进行时间步对齐，即预处理优化，具体过程如下：
[0138]
设两条轨迹序列a＝{a1，a2，
…
，ai，
…
，a
la
}和b＝{b1，b2，
…
，bj，
…
，b
lb
}，其中，ai、bj分别表示两条轨迹序列中的第i和第j个采样点，ai和bj为某一时刻采样到的坐标值，形式为坐标向量ai＝(xi，yi)和bj＝(xj，yj)，la、lb表示两条轨迹序列的长度；计算a轨迹中第i个采样点与b轨迹中第j个采样点之间的欧式距离，获得一个序列距离矩阵，其中矩阵行对应a轨迹序列，矩阵列对应b轨迹序列，矩阵元素为对应行列中a序列和b序列点到点的欧式距离，通过求解轨迹序列a与轨迹序列b之间的最短路径dab＝dc(la，lb)，将两个轨迹在某些时点上压缩，实现两个轨迹之间“距离”最小。同理，多条示教轨迹采取两两分别对齐的方法，最终将多条轨迹在时间步上对齐，如下式：
[0139][0140]
d(ωk)＝(a(ik)-b(jk))2[0141]
dc(i，j)＝d(ai，bj) min[dc(qi，qj)]
[0142]
其中，ωk＝(ik，jk)是由两个轨迹序列次号构成的匹配坐标，有w＝{ω1，ω2，
…
，ωk}；d(ωk)指a序列中第ik个特征点与b序列中的第jk个特征点之间的欧式距离。
[0143]
本实施例中，(qi，qj)属于从(1，1)到(i，j)之间存在的特定路径的所有点的集合，通过两个步骤实现，具体如下：
[0144]
s31、计算a轨迹序列与b轨迹序列各个点之间的距离矩阵。
[0145]
a轨迹为第一条示教轨迹，也为参考轨迹。
[0146]
s32、寻找一条从矩阵左上角到右下角的路径，使得路径上的元素和最小，求解所对应的规整函数，利用规整函数实现对轨迹的延伸和缩短。
[0147]
s204、以关键动作点为节点，根据单一自由度位移的动态运动基元模型，对前后节点之间的子示教轨迹进行学习，生成拟合示教轨迹的复现轨迹。
[0148]
s2041、建立单一自由度位移x的动态运动基元模型，如下式：
[0149][0150][0151][0152]
其中，k
l
、d
l
、α
l
表示模型的参数；ξg和x0表示该段子轨迹的目标位置和初始位置；表示系统的位移、速度和加速度；τ表示时间伸缩因子；f表示决定响应形状的项，称为强迫项。
[0153]
s2042、设定所述动态运动基元模型的初始参数和定义用于逼近示教轨迹运动的强迫项的非线性函数。
[0154]
本实施例中，初始参数包括机器人运动的起始点x0和目标位置ξg，这里起始点x0为起始节点坐标的横坐标取值，目标位置ξg为该节点下一个节点的横坐标取值，k
l
、d
l
、α
l
是模
型的参数，时间常数τ。
[0155]
本实施例中，定义用于逼近示教轨迹运动的强迫项的非线性函数f(s)，如下式：
[0156][0157]
其中，ψi(s)表示高斯径向基核函数，i表示第i个高斯径向基核函数ψi(s)，其取值范围1到n，n表示高斯径向基核函数的个数；
[0158]
所述ψi(s)，如下式：
[0159][0160]
其中，表示所述ψi(s)的中心位于ci。
[0161]
s2043、根据所述初始参数和所述动态运动基元模型，得到单一自由度示教轨迹中的强迫项。
[0162]
本实施例中，单一自由度示教轨迹中的强迫项，如下式：
[0163][0164]
s2044、将所述子示教轨迹的运动数据集代入所述强迫项并结合所述动态运动基元模型，生成目标逼近函数。
[0165]
将子示教轨迹的运动数据集代入步骤s2043的强迫项公式并结合所述动态运动基元模型的表达式，生成目标逼近函数，如下式：
[0166][0167]
s2045、根据目标逼近函数，采用局部加权回归，求解最佳权重。
[0168]
采用局部加权回归求解最佳权重ωi，通过局部加权回归最小化，如下式：
[0169][0170]
单一自由度位移y，参考步骤s2041～s2045不再赘述。
[0171]
s2046、根据最佳权重，使所述非线性函数近似逼近所述强迫项，以生成拟合子示教轨迹的复现轨迹，进而生成拟合示教轨迹的复现轨迹。
[0172]
当j取得最小值时的ωi即为最佳权重值，根据得到的最佳权重值，使非线性函数f(s)近似逼近示教轨迹的运动强迫项f，从而生成拟合子示教轨迹的复现轨迹，进而生成拟合示教轨迹的复现轨迹。
[0173]
值得注意的是，以长示教轨迹分割后的子轨迹作为一个训练单元，对在该训练单元记录的运动数据集输入dmp模型进行的单独训练，包括对该段子轨迹利用dmp模型分别在单个运动方向上进行学习，先将数据集分解至x轴方向和y轴方向，获得在某一方向上的示教运动数据，并先后在x轴方向和y轴方向上进行训练，模型最终输出移动机器人在x方向和y方向上的运动数据，包括位移、速度与加速度，通过将在两个方向上的运动数据融合在二维鸡舍平面上生成移动机器人的复现轨迹；当执行复现过程时，设定子轨迹的起始点和目标点，根据模型输出的运动数据基于移动机器人的运动学模型，将模型输出的在x轴和y轴
上的位移、速度和加速度数据转换为对应时刻的左、右电机的转速，由底层控制单元完成机器人运动的控制，即实现移动机器人对该段子轨迹的复现；完成对该段子轨迹的复现后，立即将下一个关键动作节点设为目标点，开始完成下一段子轨迹的复现，依次完成全部子轨迹的复现，体现分段复现的思想。
[0174]
本实施例中，在生成拟合示教轨迹的复现轨迹之后，还包括：
[0175]
s41、控制移动机器人沿着拟合示教轨迹的复现轨迹进行作业。
[0176]
如图7所示，在上位控制单元完成对示教轨迹训练并生成复现轨迹后，并且通过可视化界面给予提示，则人为控制移动机器人回到示教起始点，通过遥控器对机器人运动状态切换，切换至复现模式，以当前机器人所在位置为起始位置，读取node_coord[][]中第一个关键动作节点的横、纵坐标值为目标点，由dmp模型输出的x轴方向上的学习后的位移、速度、加速度与y轴方向上学习后的位移、速度与加速度，经移动机器人运动学模型分析，转换为从起始点到目标点过程中的左右电机的转速，以30hz的频率由上层控制单元下发至底层控制单元，经由底层控制单元与电机控制器的can总线分别下发至对应左右电机的电机控制器，控制移动机器人从起始点移动到第一关键动作节点，完成子轨迹1后，控制运动速度减为0，然后读取node_angle_bias[]数组中第一关键动作节点对应的航偏角大小，通过姿态传感器对车体的转向的闭环控制完成大角度转向后，顺序执行对子轨迹2的复现，依此类推
……
完成全部的轨迹复现。
[0177]
在复现过程中，若有障碍物覆盖了复现轨迹，则需要进行避障，即为局部轨迹的修正。
[0178]
在dmp模型生成复现轨迹的基础上，利用bi-rrt算法对有障碍物的情况下的轨迹进行修正，由dmp模型生成复现轨迹点来控制bi-rrt算法的方向和步长。具体过程参考步骤s42-步骤s46和图8。
[0179]
s42、当任意时刻移动机器人和障碍物之间的距离小于第五设定阈值时，则对该时刻移动机器人的轨迹点(ξ
t
)和移动机器人的目标点(ξ
goal
)以根节点的形式进行扩展，以分别构建第一随机树(t1)和第二随机树(t2)。
[0180]
步骤s42中，第五设定阈值为0.5m。
[0181]
s43、根据所述轨迹点和所述目标点的距离(δξ
t 1
)、障碍物坐标与形状信息和所述轨迹点(ξ
t
)的坐标信息，以及利用所述动态运动基元模型生成的复现轨迹点，为第一随机树和第二随机树导向并生成修正节点(ξ
rand
)。
[0182]
s44、求解两棵随机树中距离修正节点最近的第一节点(ξ
near
)。
[0183]
s45、从所述第一节点出发，向修正节点延伸扩展步长(pr)，得到第二节点(ξ
new
)。
[0184]
步骤s45之后，还包括：若检测到第二节点与障碍物碰撞，则舍弃该节点。
[0185]
s46、若检测到第二节点与障碍物不碰撞，则将第二节点添加到第一节点所在的随机树中。
[0186]
步骤s46中，第一节点是第二节点的父节点。
[0187]
重复步骤s42～步骤s46，第一随机树和第二随机树在dmp模型生成的复现轨迹点的导向上不断向对方扩展，并设定目标导向的最大扩展步长不超过随机扩展步长，直至第一随机树和第二随机树各自的第二节点之间的距离小于第六设定阈值，即认为两棵树相遇，从而生成避障路径，其中第六设定阈值为机器人的纵向长度。
[0188]
值得注意的是，通过bi-rrt算法对复现过程中的出现占据复现轨迹生成点的未知障碍物的情况的局部轨迹修正；通过bi-rrt算法寻找避障路径，从起始点和目标点同时生成两棵快速扩展随机树来搜索避障路径，并利用由训练完毕的dmp模型生成的复现轨迹对两棵树的生成方向进行引导，使得bi-rrt算法搜寻的节点在复现轨迹附近生成，减小避障修正路径对复现轨迹的偏移量，且可以快速回到复现轨迹上。
[0189]
本领域技术人员可以理解，实现上述实施例的方法中的全部或部分步骤可以通过程序来指令相关的硬件来完成，相应的程序可以存储于计算机可读存储介质中。
[0190]
应当注意，尽管在附图中以特定顺序描述了上述实施例的方法操作，但是这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。相反，描绘的步骤可以改变执行顺序。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。
[0191]
实施例2：
[0192]
如图9所示，本实施例提供了一种基于人类示教学习的路径规划系统，该系统包括获取单元901、识别与分割单元902、对齐单元903和复现单元904，各个单元的具体功能如下：
[0193]
获取单元901，用于获取同一起点和终点的多条示教轨迹；
[0194]
识别与分割单元902，用于识别示教轨迹中的关键动作点，并根据所述关键动作点，对示教轨迹进行分割，得到多条子示教轨迹；
[0195]
对齐单元903，用于在时间序列上，对两两示教轨迹的轨迹点进行对齐；
[0196]
复现单元904，用于以关键动作点为节点，根据单一自由度位移的动态运动基元模型，对前后节点之间的子示教轨迹进行学习，生成拟合示教轨迹的复现轨迹。
[0197]
实施例3：
[0198]
如图10所示，本实施例提供了一种计算机设备，其包括通过系统总线1001连接的处理器1002、存储器、输入装置1003、显示装置1004和网络接口1005。其中，处理器1002用于提供计算和控制能力，存储器包括非易失性存储介质1006和内存储器1007，该非易失性存储介质1006存储有操作系统、计算机程序和数据库，该内存储器1007为非易失性存储介质1006中的操作系统和计算机程序的运行提供环境，计算机程序被处理器1002执行时，实现上述实施例1的路径规划方法，如下：
[0199]
获取同一起点和终点的多条示教轨迹；
[0200]
识别示教轨迹中的关键动作点，并根据所述关键动作点，对示教轨迹进行分割，得到多条子示教轨迹；
[0201]
在时间序列上，对两两示教轨迹的轨迹点进行对齐；
[0202]
以关键动作点为节点，根据单一自由度位移的动态运动基元模型，对前后节点之间的子示教轨迹进行学习，生成拟合示教轨迹的复现轨迹。
[0203]
实施例4：
[0204]
本实施例提供一种存储介质，该存储介质为计算机可读存储介质，其存储有计算机程序，所述计算机程序被处理器执行时，实现上述实施例1的路径规划方法，如下：
[0205]
获取同一起点和终点的多条示教轨迹；
[0206]
识别示教轨迹中的关键动作点，并根据所述关键动作点，对示教轨迹进行分割，得
到多条子示教轨迹；
[0207]
在时间序列上，对两两示教轨迹的轨迹点进行对齐；
[0208]
以关键动作点为节点，根据单一自由度位移的动态运动基元模型，对前后节点之间的子示教轨迹进行学习，生成拟合示教轨迹的复现轨迹。
[0209]
需要说明的是，本实施例的计算机可读存储介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑磁盘只读存储器(cd-rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。
[0210]
在本实施例中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本实施例中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读存储介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读存储介质上包含的计算机程序可以用任何适当的介质传输，包括但不限于：电线、光缆、rf(射频)等等，或者上述的任意合适的组合。
[0211]
上述计算机可读存储介质可以以一种或多种程序设计语言或其组合来编写用于执行本实施例的计算机程序，上述程序设计语言包括面向对象的程序设计语言—诸如java、python、c ，还包括常规的过程式程序设计语言—诸如c语言或类似的程序设计语言。程序可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络，包括局域网(lan)或广域网(wan)连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
[0212]
综上所述，本发明通过遥控器或手柄，操控移动机器人从设定的出发点无碰撞地运动到目标终点，以此作为一次示教演示过程，并对一次示教演示过程中移动机器人的运动数据进行采样；将多次示教采样的数据输入单一自由度位移的动态运动基元模型进行训练，使移动机器人能够复现人工示教轨迹并进行规划路径作业，实现移动机器人在非结构鸡舍内的快速路径规划和作业。
[0213]
以上所述，仅为本发明专利较佳的实施例，但本发明专利的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明专利所公开的范围内，根据本发明专利的技术方案及其发明构思加以等同替换或改变，都属于本发明专利的保护范围。