uwb最小二乘空间定位+python模拟

悟悟悟！

已于 2024-06-16 13:26:06 修改

阅读量1.4k

点赞数 31

文章标签： python 最小二乘法机器人算法无人机

于 2024-02-12 01:50:31 首次发布

本文链接：https://blog.csdn.net/m0_64401328/article/details/136099139

版权

本文介绍了如何在UWB定位系统中，利用最小二乘空间定位原理，通过多个基站的距离测量来计算标签的三维坐标。文中详细展示了如何构建矩阵并用Python实现最小二乘法求解，同时提到了二维模拟中的问题和解决策略。

摘要由CSDN通过智能技术生成

传统最小二乘空间定位原理

假设UWB定位系统里有n个基站。基站坐标设为 $x_{i},y_{i},z_{i})$ (i=1,2,3…),标签坐标为（x,y,z),标签到基站的距离设为 $d_{i}(i=1,2,3...)$ 可得以下关系式
$\begin{cases}\left(x-x_1\right)^2+\left(y-y_1\right)^2+\left(z-z_1\right)^2=d_1^2\\\left(x-x_2\right)^2+\left(y-y_2\right)^2+\left(z-z_2\right)^2=d_2^2\\\cdots\\\left(x-x_n\right)^2+\left(y-y_n\right)^2+\left(z-z_n\right)^2=d_n^2\end{cases}$
从第二式开始减去第一式，消去 $x^2,y^2,z^2$ 可得
$\begin{cases}({x_2}^2-{x_1}^2)+({y_2}^2-{y_1}^2)+({z_2}^2-{z_1}^2)-2(x_2-x_1)x-2(y_2-y_1)y-2(z_2-z_1)z={d_2}^2-{d_1}^2\\ ({x_3}^2-{x_1}^2)+({y_3}^2-{y_1}^2)+({z_3}^2-{z_1}^2)-2({x_3}-x_1)x-2({y_3}-y_1)y-2({z_3}-z_1)z={d_3}^2-{d_1}^2\\ \vdots\\ ({x_n}^2-{x_1}^2)+({y_n}^2-{y_1}^2)+({z_{n}}^2-{z_1}^2)-2({x_{n}}-{x_1})x-2({y_n}-{y_1})y-2({z_{n}}-z_1)z={d_n}^2-{d_1}^2\end{cases}$

化成矩阵形式为AX=B
$\left.A=2\left[\begin{matrix}x_1-x_2 & & y_1-y_2 & z_1-z_2\\ x_1-x_3 & & y_1-y_3 & z_1-yz_3\\ \vdots & & \vdots & \\ x_1-x_{n} & & y_1-y_{n} & z_1-z_{n}\end{matrix}\right.\right]$
$X=\begin{bmatrix}x\\y\\z\end{bmatrix}$
$\left.B=\left[\begin{matrix}{{d}_{2}}^2-{{d}_{1}}^2+({x_{1}}^2-{x_{2}}^2)+({y_{1}}^2-{y_{2}}^2)+({z_1}^2-{z_2}^2) & \\ {{d}_{3}}^2-{{d}_{1}}^2+({x_{1}}^2-{x_{3}}^2)+({y_{1}}^2-{y_{3}}^2)+({z_1}^2-{z_3}^2) & \\ \vdots & \\ {{d}_{n}}^2-{{d}_{1}}^2+({x_{1}}^2-{x_{n}}^2)+({y_{1}}^2-{y_{n}}^2)+({z_1}^2-{z_{n}}^2) & \end{matrix}\right.\right]$

最小二乘法推导：
在这里插入图片描述

要求X可得
$\begin{bmatrix}x\\ y\\ z\end{bmatrix}=\dfrac{1}{2}\Big(A^{T}A\Big)^{-1}A^{T}B$
要求三维坐标至少四个基站

python算法模拟：
这里先模拟二维的，使用最小二乘法求解时，会因基站z轴坐标差值小导致误差较大，相较与水平坐标，z坐标不可靠，故采用其他方法获取z值

import tkinter as tk

import numpy as np

import math

import random

import matplotlib.pyplot as plt

  

class MapApp:

    def __init__(self, master):

        self.master = master

        self.master.title("UWB Emulation")

        self.master.geometry("800x600")

  

        self.num_base_stations = 4

        self.base_stations = [(random.randint(50, 550), random.randint(50, 550)) for _ in range(self.num_base_stations)]

        self.noise_mean = 0

        self.noise_stddev = 1

  

        self.canvas = tk.Canvas(self.master, width=600, height=600, bg="white")

        self.canvas.pack(side=tk.LEFT)

  

        self.label = tk.Label(self.master, text="")

        self.label.pack()

  

        self.mouse_marker = None

        self.distance_circles = []

  

        self.create_widgets()

        # 鼠标移动时进行一次计算

        self.canvas.bind("<Motion>", self.show_mouse_position)

  

    def create_widgets(self):

        self.create_base_stations()

        self.create_noise_slider()

  

    def create_base_stations(self):

        i=0

        for station in self.base_stations:

  

            x, y = station

            self.canvas.create_oval(x - 5, y - 5, x + 5, y + 5, fill="red")

             # 在标签中显示每个基站的坐标

            station_label = tk.Label(self.master, text=f"Base Station {i+1}: ({x}, {y})")

            station_label.pack()

            i = i+1

  

    def calculate_distance(self, x, y):

        distances = [math.sqrt((x - station[0])**2 + (y - station[1])**2) + random.gauss(self.noise_mean, self.noise_stddev) for station in self.base_stations]

        return distances

  

    def draw_distance_circles(self, x, y, distances):

        for i, station in enumerate(self.base_stations):

            x0, y0 = station

            radius = distances[i]

            circle = self.canvas.create_oval(x0 - radius, y0 - radius, x0 + radius, y0 + radius, outline="blue")

            self.distance_circles.append(circle)

  
  

    #最小二乘法

  

    def uwb_LeastQusare(self, distance_vector):

        # 创建A矩阵

        x_1, y_1 = self.base_stations[0]

        a = np.array([[x_1 - station[0], y_1 - station[1]] for station in self.base_stations[1:]])

        # 创建B矩阵

        d_1 = distance_vector[0]

        # print('test')

        # print(distance_vector[1]**2 - d_1**2)

        # print(distance_vector[1]**2 - d_1**2 + (x_1**2 - self.base_stations[1][0]**2))

        # print(distance_vector[1]**2 - d_1**2 + (x_1**2 - self.base_stations[1][0]**2) + (y_1**2 - self.base_stations[1][1]**2))

        b = np.array([[distance_vector[i]**2 - d_1**2 + (x_1**2 - station[0]**2) + (y_1**2 - station[1]**2)] for i, station in enumerate(self.base_stations[1:],start=1)])

  

        X = np.dot(np.linalg.inv(np.dot(np.transpose(a), a)), np.dot(np.transpose(a), b)) * 0.5

        # print('aa:')

        # print(a)

        # print('bb:')

        # print(b)

        # print('cc')

        # print(X)

        return X;

  

  

    def show_mouse_position(self, event):

        if self.mouse_marker:

            self.canvas.delete(self.mouse_marker)

        true_val = event.x, event.y

        x, y = event.x, event.y

        distances = self.calculate_distance(x, y)

        distance_text = "\n".join([f"Base Station {i+1}: {dist:.2f}" for i, dist in enumerate(distances)])

        self.label.config(text=f"Mouse Position: ({x}, {y})\nDistances to Base Stations:\n{distance_text}")

        #最小二乘法求解

        result = self.uwb_LeastQusare(distances)

        Err_val = (true_val[0]-result[0])**2 + (true_val[1]-result[1])**2

        print('result'+'     '+str(result))

        print(Err_val)

  

        self.mouse_marker = self.canvas.create_oval(x - 3, y - 3, x + 3, y + 3, fill="green")

  

        for circle in self.distance_circles:

            self.canvas.delete(circle)

        self.distance_circles = []

  

        self.draw_distance_circles(x, y, distances)

  

    def create_noise_slider(self):

        noise_frame = tk.Frame(self.master)

        noise_frame.pack()

  

        noise_label = tk.Label(noise_frame, text="Noise Level:")

        noise_label.pack(side=tk.LEFT)

  

        noise_slider = tk.Scale(noise_frame, from_=0, to=100, resolution=0.1, orient=tk.HORIZONTAL, command=self.update_noise_level)

        noise_slider.pack(side=tk.LEFT)

  

    def update_noise_level(self, value):

        self.noise_stddev = float(value)

  

    def get_distance_values(self):

        # 获取显示在窗口上的距离值

        return self.label["text"]

  
  

def main():

    root = tk.Tk()

    app = MapApp(root)

    root.mainloop()

  

    # 获取显示在窗口上的距离值

    distance_values = app.get_distance_values()

    print("Distance Values:", distance_values)

  

if __name__ == "__main__":

    main()