背景
在AMR物流机器人的实际使用场景中,要快速确定AMR距离哪个坐标点最近,可以使用空间数据结构和相应的算法,比如k-d树(k-dimensional tree,K:K维空间,D:dimension,KD-Tree即K维度树)作为空间数据结构,它可以高效地搜索多维数据空间。对于二维场景,k-d树即为二维树。以下是详细步骤:
-
构建k-d树: 首先,将所有二维码地图上的坐标点插入到k-d树中。在构建k-d树时,每个节点存储一个坐标点,通过轮流比较x轴和y轴坐标将点分配到左子树或右子树。这样可以在O(log n)时间内找到任何给定点的最近邻。
KD树是一种数据结构,可以用于高效地解决k近邻搜索问题。在构建KD树时,可以使用不同的分割准则和分割顺序,因此可能会产生不同的KD树。
常用的分割准则包括中位数分割、方差分割、最大值-最小值分割等等,分割顺序可以是轮流选择维度进行分割,也可以根据某种规则选择优先分割哪些维度。不同的分割准则和分割顺序会导致生成的KD树结构不同,但它们的本质都是相同的,都是二叉树结构。因此,KD树的生成方式不是唯一的,但都遵循相同的原理。
-
查询最近的坐标点: 当AMR定位完成后,它将有自己的坐标。此时,可以使用k-d树的最近邻搜索算法来找到距离AMR当前位置最近的坐标点。算法首先从根节点开始搜索,沿着k-d树向下移动,直到找到包含目标点的叶子节点。然后,算法向上回溯,检查子树中是否有更接近的点。如果有,将最近点更新为该点。整个过程的时间复杂度为O(log n)。
-
数据结构和算法实现: 为了实现k-d树和最近邻搜索算法,可以使用现有的库,如Python的SciPy库,Go的go-kdtree。这些库提供了实现k-d树及相关算法的函数。
KD-Tree的构建
简述过程,例如有以下二维坐标点:
A: (6, 2) B: (4, 1) C: (9, 6) D: (2, 4) E: (5, 7)
则k-d树构建算法如下:
-
选择根节点:从x轴(depth = 0)开始,找到中位数点作为根节点。在这个例子中,E(5, 7)是中位数点,将E设为根节点。使用某个坐标值的中位数作为根节点可以在一定程度避免Tree发生偏斜,左侧的点是D(2, 4)和B(4, 1),右侧的点是A(6, 2)和C(9, 6)。
E (5, 7)
-
递归构建左子树:
现在我们在y轴上找到左侧点的中位数。对点按y坐标排序:B(4, 1), D(2, 4)。中位数是点D(2, 4),因此它将成为左子树的根节点。
E (5, 7) | D (2, 4)
点B(4, 1)位于D的右侧,所以它成为D的右子节点。
E (5, 7) | D (2, 4) \ B (4, 1)
-
构建右子树
现在我们在y轴上找到右侧点的中位数。对点按y坐标排序:A(6, 2), C(9, 6)。中位数是点A(6, 2),因此它将成为右子树的根节点。
E (5, 7) / \ D (2, 4) A (6, 2)
点C(9, 6)位于A的右侧,所以它成为A的右子节点。
E (5, 7) / \ D (2, 4) A (6, 2) \ C (9, 6)
-
停止条件:递归过程将继续,直到每个子数据集中的点数达到预定义的阈值,或者无法进一步划分为止。这时,子树将成为叶子节点,存储在该叶子节点中的数据点的列表。
应用这个算法,我们得到以下k-d树:
E (5, 7)
/ \
D (2, 4) A (6, 2)
\ \
B (4, 1) C (9, 6)
结果不唯一,A点和C点y值与中位数距离相等,也会有:
E: [5 7]
D: [2 4]
B: [4 1]
C: [9 6]
A: [6 2]
AMR快速收敛到最近点
假设在这个场景中,有A: (6, 2) B: (4, 1) C: (9, 6) D: (2, 4) E: (5, 7)
五个地图点,AMR坐标为(8, 4),则确定最近地图点的过程如下:
- 从根节点(E)开始,我们检查目标点与当前节点的X坐标(因为当前节点的深度为0,也就是第一个坐标轴):
- 目标点的X坐标为8,E节点的X坐标为5,所以目标点在E节点的右子树。
- 继续前进到右子树的A节点(深度为1,按Y轴比较坐标):
- 目标点的Y坐标为4,A节点的Y坐标为2,所以目标点在A节点的右子树。
- 继续前进到右子树的C节点(深度为2,按X轴比较坐标):
- C节点没有子节点,所以搜索到达叶子节点。
- 开始回溯搜索路径,计算节点到目标点的距离:
- 目标点与C节点的距离:sqrt((9-8)^2 + (6-4)^2) = 2.24
- 将最近距离设置为2.24,最近节点设置为C。
- 回溯到A节点:
- 计算目标点与A节点的距离:sqrt((6-8)^2 + (2-4)^2) = 2.83
- 当前最近距离为2.24(C节点),2.83 > 2.24,因此A节点不会更新最近距离和最近节点。
- 回溯到根节点E:
- 计算目标点与E节点的距离:sqrt((5-8)^2 + (7-4)^2) = 4.24
- 当前最近距离为2.24(C节点),4.24 > 2.24,因此E节点不会更新最近距离和最近节点。
- 由于目标点沿X轴与E节点的距离(8 – 5 = 3)大于当前最近距离(2.24),我们不需要检查E节点的左子树(D节点)
- 假如也检查下左子树D?
- 进入左子树的D节点(深度为1,按Y轴比较坐标):
- 目标点的Y坐标为4,D节点的Y坐标为4,所以我们需要检查D节点的两个子树。
- 首先检查D节点的左子树(B节点):
- 计算目标点与B节点的距离:sqrt((4-8)^2 + (1-4)^2) = 5
- 当前最近距离为2.24(C节点),5 > 2.24,因此B节点不会更新最近距离和最近节点。
- 现在回溯到D节点,并检查D节点的右子树(不存在)。
- 计算目标点与D节点的距离:sqrt((2-8)^2 + (4-4)^2) = 6
- 当前最近距离为2.24(C节点),6 > 2.24,因此D节点不会更新最近距离和最近节点。
- 回溯到根节点E,完成搜索。
找到了距离目标点(8, 4)最近的节点是C(9, 6),距离为2.24。则AMR最近点为C点。
KDTree的Python和Go代码实现
Python代码示例
class Node:
def __init__(self, point, left=None, right=None):
self.point = point
self.left = left
self.right = right
def build_kdtree(points, depth=0):
if not points:
return None
k = len(points[0])
axis = depth % k
points.sort(key=lambda x: x[axis])
median = len(points) // 2
return Node(
points[median],
left=build_kdtree(points[:median], depth + 1),
right=build_kdtree(points[median + 1:], depth + 1)
)
def print_kdtree(node, depth=0):
if node:
print(depth * " " + str(node.point))
print_kdtree(node.left, depth + 1)
print_kdtree(node.right, depth + 1)
points = [
(6, 2),
(4, 1),
(9, 6),
(2, 4),
(5, 7)
]
kdtree = build_kdtree(points)
print_kdtree(kdtree)
在这个实现中,我们首先定义了一个名为Node
的类来表示k-d树的每个节点。该类包含一个表示节点的坐标点的属性point
,以及两个分别表示左子树和右子树的属性left
和right
。
然后,我们定义了一个名为build_kdtree
的递归函数,用于构建k-d树。这个函数接受一个坐标点列表points
和当前深度depth
作为输入。首先,我们检查points
是否为空。如果是,则返回None
。接下来,我们根据当前深度选择划分轴,然后将点集按照当前划分轴的坐标值进行排序。然后,我们找到排序后点集的中位数点,并使用它来创建一个新的Node
实例。我们递归地为中位数点左侧的点集构建左子树,为中位数点右侧的点集构建右子树。
最后,我们定义了一个名为print_kdtree
的函数,用于以树状结构打印k-d树。这个函数接受一个Node
实例和当前深度depth
作为输入。我们首先打印当前节点的坐标点,然后递归地打印左子树和右子树。
在主程序中,我们使用给定的二维坐标点列表调用build_kdtree
函数,构建k-d树。然后,我们使用print_kdtree
函数打印k-d树的结构。
scipy工具实现
from scipy.spatial import KDTree
# 定义二维坐标点数据
points = [(2, 3), (5, 7), (8, 1), (4, 5)]
# 构建k-d树
kdtree = KDTree(points)
# 查询最近的坐标点
agv_position = (8, 4)
nearest_distance, nearest_index = kdtree.query(agv_position)
nearest_point = points[nearest_index]
print("最近的坐标点:", nearest_point)
也有blog比较scipy 和sklearn的运算速度,结果是sklearn 更好,可以自己尝试
Go代码示例
Go语言KDTree实现:
package main
import (
"fmt"
"math"
"sort"
)
type Point struct {
name string
coordinates []int
}
type KDNode struct {
point *Point
axis int
left *KDNode
right *KDNode
}
func buildKDTree(points []*Point, depth int) *KDNode {
// 当点集为空时,返回nil
if len(points) == 0 {
return nil
}
// 计算当前深度的轴
axis := depth % len(points[0].coordinates)
// 根据当前轴对点集进行排序
sort.Slice(points, func(i, j int) bool {
return points[i].coordinates[axis] < points[j].coordinates[axis]
})
// 选择中位数点作为根节点
median := len(points) / 2
// 递归地构建左子树和右子树
left := buildKDTree(points[:median], depth+1)
right := buildKDTree(points[median+1:], depth+1)
return &KDNode{
point: points[median],
axis: axis,
left: left,
right: right,
}
}
func euclideanDistance(p1, p2 []int) float64 {
sum := 0.0
for i := 0; i < len(p1); i++ {
diff := float64(p1[i] - p2[i])
sum += diff * diff
}
return math.Sqrt(sum)
}
func findNearestNeighbor(node *KDNode, target *Point, depth int, best *Point) *Point {
if node == nil {
return best
}
axis := depth % len(target.coordinates)
currentDist := euclideanDistance(node.point.coordinates, target.coordinates)
bestDist := euclideanDistance(best.coordinates, target.coordinates)
if currentDist < bestDist {
best = node.point
}
var next, other *KDNode
if target.coordinates[axis] < node.point.coordinates[axis] {
next, other = node.left, node.right
} else {
next, other = node.right, node.left
}
best = findNearestNeighbor(next, target, depth+1, best)
if math.Abs(float64(target.coordinates[axis]-node.point.coordinates[axis])) < bestDist {
best = findNearestNeighbor(other, target, depth+1, best)
}
return best
}
func printKDTree(node *KDNode, depth int) {
if node == nil {
return
}
indent := ""
for i := 0; i < depth; i++ {
indent += " "
}
fmt.Printf("%s%s: %v\n", indent, node.point.name, node.point.coordinates)
printKDTree(node.left, depth+1)
printKDTree(node.right, depth+1)
}
func main() {
points := []*Point{
{name: "A", coordinates: []int{6, 2}},
{name: "B", coordinates: []int{4, 1}},
{name: "C", coordinates: []int{9, 6}},
{name: "D", coordinates: []int{2, 4}},
{name: "E", coordinates: []int{5, 7}},
}
kdTree := buildKDTree(points, 0)
printKDTree(kdTree, 0)
target := &Point{name: "T", coordinates: []int{8, 4}}
fmt.Printf("\nSearching nearestneighbor for point %s: %v\n", target.name, target.coordinates)
nearest := findNearestNeighbor(kdTree, target, 0, points[0])
fmt.Printf("Nearest neighbor: %s: %v\n", nearest.name, nearest.coordinates)
}
go-kdtree包示例
go get -u github.com/hongshibao/go-kdtree
package kdTreePak
import (
"fmt"
kdtree "github.com/hongshibao/go-kdtree"
)
type EuclideanPoint struct {
kdtree.Point
name string
coordinates []float64
}
func (p *EuclideanPoint) Dim() int {
return len(p.coordinates)
}
func (p *EuclideanPoint) GetValue(dim int) float64 {
return p.coordinates[dim]
}
func (p *EuclideanPoint) Distance(other kdtree.Point) float64 {
var ret float64
for i := 0; i < p.Dim(); i++ {
tmp := p.GetValue(i) - other.GetValue(i)
ret += tmp * tmp
}
return ret
}
func (p *EuclideanPoint) PlaneDistance(val float64, dim int) float64 {
tmp := p.GetValue(dim) - val
return tmp * tmp
}
func NewEuclideanPoint(name string,vals ...float64) *EuclideanPoint {
ret := &EuclideanPoint{name: name}
for _, val := range vals {
ret.coordinates = append(ret.coordinates, val)
}
return ret
}
func FindNearestNeighbor(target *EuclideanPoint, pointsInfo map[string][]float64) *EuclideanPoint {
points := make([]kdtree.Point, 0)
for name, coords := range pointsInfo {
point := NewEuclideanPoint(name, coords...)
points = append(points, point)
}
tree := kdtree.NewKDTree(points)
nearest := tree.KNN(target, 1)
nearestPoint := nearest[0].(*EuclideanPoint)
fmt.Printf("Nearest neighbor: %s %v\n", nearestPoint.name, nearestPoint.coordinates)
return nearestPoint
}
// func FindNearestNeighbor() *EuclideanPoint {
// p1 := NewEuclideanPoint("A", 6, 2)
// p2 := NewEuclideanPoint("B", 4, 1)
// p3 := NewEuclideanPoint("C", 9, 6)
// p4 := NewEuclideanPoint("D", 2, 4)
// p5 := NewEuclideanPoint("E", 5, 7)
// points := make([]kdtree.Point, 0)
// points = append(points, p1)
// points = append(points, p2)
// points = append(points, p3)
// points = append(points, p4)
// points = append(points, p5)
// tree := kdtree.NewKDTree(points)
// target := NewEuclideanPoint("T", 8, 4)
// nearest := tree.KNN(target, 1)
// nearestPoint := nearest[0].(*EuclideanPoint)
// fmt.Printf("Nearest neighbor: %s %v\n", nearestPoint.name, nearestPoint.coordinates)
// return nearestPoint
// }
package kdTreePak_test
import (
"testing"
kdTreePak "kdtree/KDTree_pak"
)
func TestFindNearestNeighbor(t *testing.T) {
target := kdTreePak.NewEuclideanPoint("T", 8, 4)
pointsInfo := map[string][]float64{
"A": {6, 2},
"B": {4, 1},
"C": {9, 6},
"D": {2, 4},
"E": {5, 7},
}
kdTreePak.FindNearestNeighbor(target, pointsInfo)
}
// func TestFindNearestNeighbor(t *testing.T) {
// result := kdTreePak.FindNearestNeighbor()
// t.Log(result)
// }
发表回复