一种地面机器人集群协同定位技术的制作方法

1.本发明涉及机器人定位技术领域，特别是地面机器人集群的定位技术领域。


背景技术：

2.随着科技的快速发展，地面机器人已广泛应用到工业、军事等多个领域，但单机器人能处理的任务量小，工作效率低，任务量很有限不能满足实际工业或军事等领域中的任务处理需求，而多个单机器人之间又受限于缺乏协同运动，无法承担更为复杂的任务，因此，机器人集群化协作是以后的发展趋势。
3.目前，机器人集群协同的定位技术在本领域中也有一些技术公开，技术方法各有不同，各种方法在机器人之间的信息融合、定位精度、控制效果、编队队形保持和控制等方面的技术效果的体现也不同，各有存在优缺点，但如何达到机器人之间的信息来源多源化，信息融合程度更高，定位精度更高，控制效果更好，编队队形保持和控制更加精确与稳定是本领域技术人员对该技术不断深入研究开发的方向，有鉴于此，遂有本案产生。


技术实现要素：

4.本发明的目的在于提供一种信息来源多源化、信息融合程度更高、定位精度更高、控制效果更好、编队队形保持和控制更加精确与稳定的一种地面机器人集群协同定位技术。
5.为实现上述目的，本发明的技术方案是：一种地面机器人集群协同定位技术，地面机器人包括一个领导机器人和若干个追随机器人，各追随机器人有各自的编号，所述领导机器人与追随机器人或追随机器人与追随机器人之间通过无线数据传输，所述领导机器人配置有360
°
多线激光雷达，所述追随机器人配置有激光雷达，
6.所述领导机器人与追随机器人的编队集群协同定位方法是这样的,所有机器人在出发前以领导机器人作为最前方的机器人进行初始编队，建立出各追随机器人相对领导机器人的位置，建立出各追随机器人自身附近固定方位的固定编号的追随机器人或领导机器人并作为路标，出发行进中，所述领导机器人通过360
°
多线激光雷达扫描实现自身在全局地图中的定位和传感观测到各追随机器人的位置，并采用置信度来描绘每一个追随机器人的位置；各追随机器人通过激光雷达观测初始建立的自身附近固定方位的固定编号的追随机器人或领导机器人路标，通过扩展卡尔曼滤波定位实现自身的定位，并根据领导机器人采用置信度描绘出来的自己的位置结合来调整自身在全局地图中的位置。
7.所述领导机器人与追随机器人还配置有gnss设备，在出发行进中，所述领导机器人还通过gnss设备进行定位，所述追随机器人还通过gnss设备进行定位，并可与其扩展卡尔曼滤波定位进行定位数据融合。
8.所述追随机器人配置的激光雷达为180
°
单线激光雷达，其在编队集群协同定位方法中观测自身附近固定的几个编号的追随机器人作为路标具体观测的是编队正前方左右180
°
范围内固定的几个编号的追随机器人。
9.所述初始编队为领导机器人一个在最前方，所有追随机器人在领导机器人后方排列队形，出发行进中，领导机器人与追随机器人的运动计算方法过程是这样的；
10.整个队伍t时刻的运动方程公式如下
11.t时刻的整个队伍的位置：
12.从t-1时刻推到t时刻的整个队伍的位置：
13.计算所有机器人的线速度和角速度：
14.上述公式中f(x
t
，y
t
，θ
t
)表示t时刻的整个队伍的位置，由整个队伍所有机器人的位置(x，y，θ)的均值表示出，其中分别包含t时刻在全局地图中的xy轴坐标x
t
和y
t
以及方向角θ
t
，t时刻的队伍运动状态量表示为u
t
＝(v
t
，ω
t
)
t
，v
t
表示t时刻的队伍速度，ω
t
表示t时刻的队伍角速度；θ1与θr表示两侧的追随机器人分别与领导机器人构成的方向角，v
i，l
与v
i，r
分别表示两侧追随机器人的速度，t表示t时刻，n表示所有机器人个数，i表示机器人编号，x
a，t
、y
a，t
表示所有机器人a时刻的位置，x
b，t
、y
b，t
表示所有机器人b时刻的位置；
15.所有机器人都拥有全局地图，每个机器人的概率分布独立影响队伍的位置，将机器人理解为马尔可夫过程，领导机器人计算每个追随机器人置信度可以由下列公式表示，
[0016][0017]
其中，x
i，t
，u
i，t
及z
i，t
依次分别表示第i个机器人t时刻的位姿、控制量及观测量，m表示地图，bel(x
1，t
，x
2，t
…
x
n，t
)表示置信度，1-n表示机器人编号；
[0018]
每个追随机器人的卡尔曼滤波定位计算过程公式如下，
[0019]
各追随机器人的转移方程：
[0020]
单个追随机器人的状态向量为x
i，t
＝[x
i，t
，y
i，t
，v
i，t
，ω
i，t
，θ
i，t
]
t
，
[0021]
其中，x
i，t
，y
i，t
表示第i个机器人t时刻的xy轴坐标，v
i，t
表示第i个机器人t时刻的速度，ω
i，t
表示第i个机器人t时刻的角速度，θ
i，t
表示第i个机器人t时刻的朝向，
[0022]
各追随机器人的观测方程：x
i，t
＝x
i，obj，t
tf
i，x，t
，
[0023]
其中，系统为依次对位姿、速度、角速度和朝向的估计， [x
i，o，t y
i，o，t v
i，o，t ω
i，o，t θ
i，o，t
]为各追随机器人对依次对位姿、速度、角速度和朝向的观测量，x
i，obj，t
表示各追随机器人的状态观测量，tf
i，x，t
为进行tf变换；各追随机器人的观测由置信度给出和通过自身激光雷达观测得到，协方差通过置信度给出；
[0024]
各追随机器人定位调整位置的运动计算方程如下：
[0025]
运动学方程：
[0026]
其中，表示系统对t时刻机器人状态向量的估计，x
t-1
表示t-1时刻机器人状态向量，u
t
表示t时刻的队伍运动状态量，
[0027]
协防差转移
[0028][0029]
其中，j
t-1
为雅可比矩阵，p
t-1/t-1
为t-1时刻的协方差通过置信度给出，q
t
为 t时刻的置信度，
[0030]
实际观测量
[0031]
其中，z
t
为t时刻领导机器人对追随机器人的观测量，为t时刻追随机器人自身的观测量估计变换，
[0032]
观测协方差
[0033]
增益
[0034]
其中，h
t
为转移矩阵，p
t/t-1
为协方差转移，r
t
为噪声，
[0035]
更新方程：
[0036]
其中，x
t
为t时刻机器人状态向量，为系统对t时刻机器人状态向量的估计，
[0037]
更新协方差：p
t/t
＝(i-k
tht
)p
t/t-1
，
[0038]
其中，i为单位阵，
[0039]
观测雅可比矩阵：
[0040]
通过采用上述技术方案，本发明的有益效果是：上述技术方案为了实现多机器人定位导航，将所有机器人分出了一个领导机器人，其余为追随机器人，机器人间可以通过无线数据传输、传感器观测数据，领导机器人通过自身配置的大范围360
°
多线激光雷达观测进行自身定位和所有追随机器人的位置并采用置信度描绘每一个机器人的位置，将追随机器人之间或与领导机器人相互之间作为已知位置的路标，追随机器人通过自身配置的激光雷达采用扩展卡尔曼滤波定位与gnss设备传感融合，可防止激光扫描定位丢失缺少定位数据，结合置信度描绘的位置采用来调整自身的位置，从而实现多机器人集群协同定位。
[0041]
综上，本发明的技术方法中追随机器人按设定好的编队追随领导机器人的方法是通过领导机器人的预测结合追随机器人自身的观测定位来进行修正回行进中应该所在的位置，提高编队整体定位进度，该技术方法相较于单机器人，具有信息来源多源化、信息融合程度更高、定位精度更高、控制效果更好等优势，在编队队形保持和控制上更加精确与稳定，从而实现本发明的上述目的。
具体实施方式
[0042]
为了进一步解释本发明的技术方案，下面通过具体实施例来对本发明进行详细阐述。
[0043]
一种地面机器人集群协同定位技术，地面机器人包括一个领导机器人和若干个追随机器人，各追随机器人有各自的编号，所述领导机器人与追随机器人或追随机器人与追随机器人之间通过无线数据传输，所述领导机器人配置有 360
°
多线激光雷达，能够实现360
°
大范围扫描观测，以达到其领导执行任务功能作用，所述领导机器人还配置有gnss(全球导航卫星系统)设备，所述追随机器人配置有激光雷达和gnss设备，追随机器人的激光雷达由于其是追随于领导机器人的，因此根据本发明的技术方法可选用180
°
单线激光雷达。
[0044]
本发明实施例的所述领导机器人与追随机器人的编队集群协同定位方法是这样的：
[0045]
所有机器人拥有全局地图，在出发前以领导机器人作为最前方的机器人进行初始编队，比如所有追随机器人在领导机器人后方逐行排列成三角形队形或其他形状的队形，并建立出各追随机器人相对领导机器人的位置，还有，建立出各追随机器人自身附近固定方位的固定编号的追随机器人或领导机器人并作为路标，如本实施例中追随机器人采用的是180
°
单线激光雷达，那么其附近固定方位的固定编号的追随机器人可以是编队正前方左右180
°
范围内固定的几个编号的追随机器人，对于在编队较前位置的追随机器人也可包含领导机器人在内。
[0046]
出发行进中，整个队伍t时刻的运动计算方程公式如下
[0047]
t时刻的整个队伍的位置：
[0048]
从t-1时刻推到t时刻的整个队伍的位置：
[0049]
计算所有机器人的线速度和角速度：
[0050]
上述公式中f(x
t
，y
t
，θ
t
)表示t时刻的整个队伍的位置，由整个队伍所有机器人的位置(x，y，θ)的均值表示出，其中分别包含t时刻在全局地图中的xy轴坐标x
t
和y
t
以及方向角θ
t
，t时刻的队伍运动状态量表示为u
t
＝(v
t
，ω
t
)
t
，v
t
表示t时刻的队伍速度，ω
t
表示t时刻的队伍角速度；θ1与θr表示两侧的追随机器人分别与领导机器人构成的方向角，v
i，l
与v
i，r
分别表示两侧追随机器人的速度，t表示t时刻，n表示所有机器人个数，i表示机器人编号，x
a，t
、y
a，t
表示所有机器人a时刻的位置，x
b，t
、y
b，t
表示所有机器人b时刻的位置。
[0051]
出发行进中，所述领导机器人通过360
°
多线激光雷达扫描和通过gnss设备进行定位实现自身在全局地图中的定位和传感观测到各追随机器人的位置，并采用置信度来描绘每一个追随机器人的位置，将数据传输给追随机器人。每个机器人的概率分布独立影响队伍的位置，将机器人理解为马尔可夫过程，领导机器人计算每个追随机器人置信度可以由下列公式表示，
[0052][0053]
其中，x
i，t
，u
i，t
及z
i，t
依次分别表示第i个机器人t时刻的位姿、控制量及观测量，m表示地图，bel(x
1，t
，x
2，t
…
x
n，t
)表示置信度，1-n表示机器人编号。
[0054]
各追随机器人通过激光雷达观测初始建立的自身附近固定方位的固定编号的追随机器人或领导机器人路标，通过扩展卡尔曼滤波定位和gnss设备对传感融合实现自身在全局地图中的定位，并根据领导机器人采用置信度描绘出来的自己的位置结合来调整自身在全局地图中的位置。
[0055]
每个追随机器人的卡尔曼滤波定位计算过程公式如下，
[0056]
各追随机器人的转移方程：
[0057]
单个追随机器人的状态向量为x
i，t
＝[x
i，t
，y
i，t
，v
i，t
，ω
i，t
，θ
i，t
]
t
，
[0058]
其中，x
i，t
，y
i，t
表示第i个机器人t时刻的xy轴坐标，v
i，t
表示第i个机器人t时刻的速度，ω
i，t
表示第i个机器人t时刻的角速度，θ
i，t
表示第i个机器人t时刻的朝向，
[0059]
各追随机器人的观测方程：x
i，t
＝x
i，obj，t
tf
i，x，t
，
[0060]
其中，系统为依次对位姿、速度、角速度和朝向的估计， [x
i，o，t y
i，o，t v
i，o，t ω
i，o，t θ
i，o，t
]为各追随机器人对依次对位姿、速度、角速度和朝向的观测量，x
i，obj，t
表示各追随机器人的状态观测量，tf
i，x，t
为进行tf变换；各追随机器人的观测由置信度给出和通过自身激光雷达观测得到，协方差通过置信度给出；
[0061]
各追随机器人定位调整位置的运动计算方程如下：
[0062]
运动学方程：
[0063]
其中，表示系统对t时刻机器人状态向量的估计，x
t-1
表示t-1时刻机器人状态向量，u
t
表示t时刻的队伍运动状态量，
[0064]
协防差转移
[0065][0066]
其中，j
t-1
为雅可比矩阵，p
t-1/t-1
为t-1时刻的协方差通过置信度给出，q
t
为 t时刻的置信度，
[0067]
实际观测量
[0068]
其中，z
t
为t时刻领导机器人对追随机器人的观测量，为t时刻追随机器人自身的观测量估计变换，
[0069]
观测协方差
[0070]
增益
[0071]
其中，h
t
为转移矩阵，p
t/t-1
为协方差转移，r
t
为噪声，
[0072]
更新方程：
[0073]
其中，x
t
为t时刻机器人状态向量，为系统对t时刻机器人状态向量的估计，
[0074]
更新协方差：p
t/t
＝(i-k
tht
)p
t/t-1
，
[0075]
其中，i为单位阵，
[0076]
观测雅可比矩阵：
[0077]
在不同时刻根据不同的观测，采用不同的h。整体方案从全局更新到局部更新，形成了一套闭环。系统更加稳定，可靠。
[0078]
上述实施例并非限定本发明的产品形态和式样，任何所属技术领域的普通技术人员对其所做的适当变化或修饰，皆应视为不脱离本发明的专利范畴。