背景
在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)
// }