一种无人机自主充电导航方法、装置、设备及介质与流程

1.本发明属于电力技术领域，具体涉及一种无人机自主充电导航方法、装置、设备及介质。


背景技术：

2.随着无人机技术的发展，对无人机的自主性和独立性的要求越来越高，在工业和家庭服务等应用场景下要求无人机能够长时间的自主工作。传统的人工干预进行充电的方式不仅阻碍了无人机的智能化，而且耗费了人力资源，因此让无人机在无人工干预下安全、快速、高效地实现自主充电是实现无人机智能化的一项关键技术。


技术实现要素：

3.本发明的目的在于提供一种无人机自主充电导航方法、装置、设备及介质，以解决现有无人机充电方法需人为干预，无法自主进行充电的技术问题。
4.为实现上述目的，本发明采用如下技术方案予以实现：
5.第一方面，一种无人机自主充电导航方法，包括以下步骤：
6.扫描环境信息，获得激光雷达数据；
7.从激光雷达数据中识别充电桩路标，建立坐标系；
8.通过icp法处理坐标系得到位姿增益；
9.获取位姿增益对应的里程计信息；
10.根据里程计信息和位姿增益采用扩展卡尔曼滤波法进行融合得到估算位姿；
11.将估算位姿作为pid控制法的输入控制无人机运动到目标充电位置。
12.本发明的进一步改进在于：从激光雷达数据中识别充电桩路标前，对激光雷达数据进行聚类处理，所述聚类处理具体包括以下步骤：
13.对激光雷达数据进行聚类处理，找出一帧激光雷达数据中的断点，将激光雷达数据分成若干簇；
14.将各簇中的点连接成线，采用分割-合并算法，以聚类处理的分组点集s作为输入，分为n个子集{s1,...,sn}；
15.每个点集s中的所有点用最小二乘法拟合成一条直线，使每个点集s中的所有点到以此点集拟合的直线的距离不超过第一预设值。
16.本发明的进一步改进在于：所述将激光雷达分成若干簇时，根据激光雷达数据中相邻点的欧式距离d与第一预设值threshold之间的大小关系进行分类。
17.本发明的进一步改进在于：所述拟合成一条直线时，具体包括以下步骤：
18.a1、将点集s作为输入；
19.a2、取点集s中的收尾两点p1和p2，得到初始化端点集合e＝{p1,pn}；
20.a3、采用最小二乘法将(pi,p
i+1
)之间的所有点拟合直线l，计算点pi和点p
i+1
之间的点到直线l的距离dm；
21.a4、判断dm中的最大值dk与第一预设值之间的大小关系，若dk大于第一预设值，则在初始化端点集合e中插入点pk，e＝{...,pi,pk,p
i+1
,...}，再重复a3分割(pi,pk)和(pk,p
i+1
)区间点，直至dk小于等于一预设值。
22.本发明的进一步改进在于：所述通过icp法处理坐标系得到位姿增益时，具体包括以下步骤：
23.获取路标模板数据；
24.根据路标模板数据和激光雷达数据进行扫描匹配，得到路标模板数据和激光雷达数据的位姿变换关系；
25.根据路标模板数据和激光雷达数据的位姿变换关系得到在坐标系下的位姿增益。
26.本发明的进一步改进在于：所述路标模板数据为激光雷达在一个预设参考位姿处扫描充电桩路标得到的理想数据。
27.本发明的进一步改进在于：所述根据里程计信息和位姿增益采用扩展卡尔曼滤波法进行融合得到估算位姿时，包括位姿预测和位姿更新，所述位姿预测通过计算运动模型的雅可比矩阵，更新无人机位姿状态的协方差矩阵得到观测信息；所述位姿更新通过计算观测模型的雅可比矩阵，结合协方差可得到卡尔曼增益的表达形式，根据卡尔曼增益和观测信息得到位姿估计。
28.第二方面，一种无人机自主充电导航装置，包括：
29.激光雷达：用于扫描环境信息，获得激光雷达数据；
30.路标识别模块：用于从激光雷达数据中识别充电桩，建立坐标系；
31.位姿增益计算模块：用于通过icp法处理坐标系得到位姿增益；
32.里程计：用于获取位姿增益对应的里程计信息；
33.估算位姿模块：用于根据里程计信息和位姿增益采用扩展卡尔曼滤波法进行融合得到估算位姿；
34.控制模块：用于将估算位姿作为pid控制法的输入完成导航。
35.第三方面，一种计算机设备，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，其特征在于，所述处理器执行计算机程序时实现上述的一种无人机自主充电导航方法。
36.第四方面，一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现上述的一种无人机自主充电导航方法。
37.与现有技术相比，本发明至少包括以下有益效果：
38.1、本发明通过应用路标识别和定位导航技术，代替了传统的人工充电的方式，极大的降低了人力资源的占用，降低了无人机充电成本，提高了充电效率；
39.2、本发明通过对激光雷达数据进行聚类处理保证同一簇中的激光扫描点属于环境中的同一连续物体，增加路标识别的准确度；
40.3、本发明采用的算法步骤简洁整个充电过程实现相对容易，导航快速准确，充电效率大大提高。
附图说明
41.构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示
意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
42.在附图中：
43.图1为本发明一种无人机自主充电导航方法的流程图；
44.图2为本发明一种无人机自主充电导航方法中icp法第一示意图；
45.图3为本发明一种无人机自主充电导航方法中icp法第二示意图；
46.图4为本发明一种无人机自主充电导航方法中pid导航控制法逻辑框图；
47.图5为本发明一种无人机自主充电导航装置的结构框图。
具体实施方式
48.下面将参考附图并结合实施例来详细说明本发明。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
49.以下详细说明均是示例性的说明，旨在对本发明提供进一步的详细说明。除非另有指明，本发明所采用的所有技术术语与本发明所属领域的一般技术人员的通常理解的含义相同。本发明所使用的术语仅是为了描述具体实施方式，而并非意图限制根据本发明的示例性实施方式。
50.实施例1
51.一种无人机自主充电导航方法，如图1所示，包括以下步骤：
52.s1、扫描环境信息，获得激光雷达数据；
53.无人机电量低于预设电量时，无人机进行充电，在行进到充电处的过程中会通过激光雷达扫描外界环境信息。
54.s2、从激光雷达数据中识别充电桩，建立坐标系；
55.充电桩路标为人工路标，放置在无人机工作点附近，无人机携带的激光雷达扫描的数据中识别出充电桩，以识别的等腰三角形充电桩的两条腰的交点作为局部坐标系的原点，建立直角坐标系。
56.在采集到一帧激光雷达数据之后首先是从激光数据中识别充电桩路标，并提取出属于充电桩路标的激光数据。对扫描到的激光雷达数据做聚类处理，找出一帧数据中的断点。如果相邻点之间间隔大于等于第一预设值，则认为这两点属于不同的簇，于是，即可将激光雷达数据分成不同的簇。对同一簇的点递归地进行直线拟合处理，直线拟合采用最小二乘方法，将同一簇中的点拟合成一条条线段。充电桩路标的形状、边长、相邻边之间的夹角都为已知数据，在得到一条条线段后进行找段的操作，段长和夹角是作为搜索线段的两个条件，其中段长与充电桩参考数据相比需＜10cm，夹角需要在
±
10
°
的容许范围内。符合条件的段可能有多条，取距离激光雷达最近的铰合点连接的段作为最终路标，完成人工路标充电桩的识别。
57.在s2中从激光雷达数据中识别充电桩路标前，对激光雷达数据进行聚类处理，聚类处理具体包括以下步骤：
58.s21、对激光雷达数据进行聚类处理，判断激光雷达数据中相邻点的欧式距离d是否超出第一预设值threshold，找出一帧激光雷达数据中的断点，可将激光雷达数据分成若干簇；
59.通过s21中的步骤保证同一簇中的激光扫描点属于环境中的同一连续物体，增加
路标识别的准确度；
60.s22、将各簇中的点连接成线，采用分割-合并算法，以聚类处理的分组点集s作为输入，分为n个子集{s1,...,sn}；
61.s23、每个点集中的所有点用最小二乘法拟合成一条直线，保证每个点集中的所有点到以此点集拟合的直线的距离不超过第一预设值；
62.在s23中拟合成一条直线时，具体包括以下步骤：
63.s231、将点集s作为输入；
64.s232、取点集s中的收尾两点p1和p2，得到初始化端点集合e＝{p1,pn}；
65.s233、采用最小二乘法将(pi,p
i+1
)之间的所有点拟合直线l，计算点pi和点p
i+1
之间的点到直线l的距离dm；
66.s234、判断dm中的最大值dk与第一预设值之间的大小关系，若dk大于threshold，则在初始化端点集合e中插入点pk，e＝{...,pi,pk,p
i+1
,...}，再重复步骤s233分割(pi,pk)和(pk,p
i+1
)区间点，直至dk小于等于threshold。
67.作为本发明的示例，本方案中设定第一预设值为20cm。
68.本方案中所采用的充电桩路标的形状一般为等腰三角形，边长为30cm，相邻边之间的夹角为120
°
。
69.s3、通过icp法处理坐标系得到位姿增益；如图2-3所示；
70.s31、获取路标模板数据；
71.路标模板数据为icp匹配算法的标准，路标模板数据通过激光雷达在一个人为设定的参考位姿处扫描人工路标得到的理想数据，可提前离线计算保存该数据；
72.s32、根据路标模板数据和激光雷达数据进行扫描匹配，得到路标模板数据和激光雷达数据的位姿变换关系；
73.坐标系x
lolyl
的原点固连在人工路标(即等腰三角形充电桩)上两腰的交点；
74.坐标系x
lolyl
下的位姿q＝(x,y,θ)＝(t,θ)。对应的其次变换矩阵表示为：
[0075][0076]
其中，r(θ)为正交旋转矩阵，参考位姿的齐次变换矩阵记为无人机当前位姿的齐次变换矩阵记为当前位姿相对于参考位姿的齐次变换矩阵记为路标模板数据与路标扫描数据的位姿变换关系记为它们之间满足：
[0077][0078][0079]
s33、根据路标模板数据和激光雷达数据的位姿变换关系得到在坐标系下的位姿增益；
[0080]
坐标系下的位姿增益：
[0081]
[0082]
其中，参考位姿是人为选定的，所以是已知量。
[0083]
s4、获取位姿增益对应的里程计信息；
[0084]
里程计信息为从无人机开始寻找充电桩到位姿增益发生时行驶的里程。
[0085]
s5、根据里程计信息和位姿增益采用扩展卡尔曼滤波法进行融合得到估算位姿；
[0086]
通过扩展卡尔曼滤波器对位姿增益和里程计信息进行融合，估计无人机位于充电桩建立的坐标系下的局部位姿，主要分为位姿预测和位姿更新两部分。位姿预测部分主要计算运动模型的雅可比矩阵，更新无人机位姿状态估计的协方差矩阵得到观测信息；位姿更新部分主要计算观测模型的雅可比矩阵，结合协方差可得到卡尔曼增益的表达形式，最后利用卡尔曼增益和观测信息得到位姿估计。
[0087]
s6、如图4所示，将估算位姿作为pid控制法的输入控制无人机运动到目标充电位置。
[0088]
pid控制器的规律为：
[0089][0090]
其中，k
p
为比例系数，ti为积分时间常数，td为微分时间常数，e(t)为pid控制器的输入，u(t)为pid控制器的输出。
[0091]
pid控制将系统给出的给定值(此处为无人机的局部位姿)和测量出的实际值之间的偏差反馈，利用比例p、积分i和微分d运算纠正偏差，控制无人机精确运动到目标位置进行充电。
[0092]
实施例2
[0093]
如图5所示，一种无人机自主充电导航方装置，基于上述的无人机自主充电导航方法，包括：
[0094]
激光雷达：用于扫描环境信息，获得激光雷达数据；
[0095]
路标识别模块：用于从激光雷达数据中识别充电桩路标，建立坐标系；
[0096]
位姿增益计算模块：用于通过icp法处理坐标系得到位姿增益；
[0097]
里程计：用于获取位姿增益对应的里程计信息；
[0098]
估算位姿模块：用于根据里程计信息和位姿增益采用扩展卡尔曼滤波法进行融合得到估算位姿；
[0099]
控制模块：用于将估算位姿作为pid控制法的输入完成导航。
[0100]
实施例3
[0101]
一种计算机设备，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现实施例1的一种无人机自主充电导航方法。
[0102]
实施例4
[0103]
一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现实施例1中的一种无人机自主充电导航方法。
[0104]
由技术常识可知，本发明可以通过其它的不脱离其精神实质或必要特征的实施方案来实现。因此，上述公开的实施方案，就各方面而言，都只是举例说明，并不是仅有的。所有在本发明范围内或在等同于本发明的范围内的改变均被本发明包含。
[0105]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序
产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0106]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0107]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0108]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0109]
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。