SLAM从入门到精通(三边测量法详解)

【 声明:版权所有,欢迎转载,请勿用于商业用途。 联系信箱:feixiaoxing @163.com】

        三边测量,或者说叫三角定位,是定位系统中很常见的一种测量方法。它最主要的原理就是依靠已有的三个特征坐标和半径,就可以计算出当前自己的坐标信息。听上去这么做很玄乎,其实相关的计算,用初中数学的知识就可以完全掌握。

        首先我们假设三个特征坐标分别是(x1,y1)、(x2,y2)、(x3,y3),它们的半径分别是r1、r2、r3。已知的条件就这么多,下面就要求出(x,y)的坐标了。

1、第一步,构建方程组

(x-x1)*(x-x1) + (y-y1)*(y-y1) = r1*r1
(x-x2)*(x-x2) + (y-y2)*(y-y2) = r2*r2
(x-x3)*(x-x3) + (y-y3)*(y-y3) = r3*r3

2、第二步,展开方程组

x*x - 2*x1*x + x1*x1 + y*y - 2*y1*y + y1*y1 = r1 * r1
x*x - 2*x2*x + x2*x2 + y*y - 2*y2*y + y2*y2 = r2 * r2
x*x - 2*x3*x + x3*x3 + y*y - 2*y3*y + y3*y3 = r3 * r3

3、第三步,两两相减

        针对之前生成的公式,现在可以两两相减,这样就可以得到如下公式,这是目前为止最为关键的一步,

(-2*x1 + 2*x2)*x + (-2*y1 + 2*y2)*y = r1*r1 - r2*r2 -x1*x1 + x2*x2 -y1*y1 + y2*y2
(-2*x2 + 2*x3)*x + (-2*y2 + 2*y3)*y = r2*r2 - r3*r3 -x2*x2 + x3*x3 -y2*y2 + y3*y3

4、第四步,重写表达式

        因为上面公式的内容比较多,我们可以换个符号重写一下,即,

A*x + B*y = C
D*x + E*y = F

        注意,这里的A、B、C、D、E、F分别代表上面数学表达式中的子表达式,

A = -2*x1 + 2*x2
B = -2*y1 + 2*y2
C = r1*r1 - r2*r2 -x1*x1 + x2*x2 -y1*y1 + y2*y2
D = -2*x2 + 2*x3
E = -2*y2 + 2*y3
F = r2*r2 - r3*r3 -x2*x2 + x3*x3 -y2*y2 + y3*y3

5、第五步,获取最终的x和y

        有了A/B/C/D/E/F,这样最终的x和y也就可以计算出来了,

x = (CE-FB)/(EA-BD)
y = (CD-AF)/(BD-AE)

6、python代码范例

        最后,我们给出一份python代码,大家可以灵活地将他改成自己需要的语言。最后,非常感谢https://www.101computing.net/cell-phone-trilateration-algorithm/这个链接,有兴趣的同学可以直接阅读上面的内容。

def track(x1,y1,r1,x2,y2,r2,x3,y3,r3):A = 2*x2 - 2*x1B = 2*y2 - 2*y1C = r1**2 - r2**2 - x1**2 + x2**2 - y1**2 + y2**2D = 2*x3 - 2*x2E = 2*y3 - 2*y2F = r2**2 - r3**2 - x2**2 + x3**2 - y2**2 + y3**2x = (C*E - F*B) / (E*A - B*D)y = (C*D - A*F) / (B*D - A*E)return x,y

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/118751.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Windows10系统安装telnet命令

简介 telnet命令可以测试目标服务器端口是否开通,使用命令 telnet ip地址 端口,输入命令后回车,如果进入输入状态,则表示目标服务器端口已开通,可以通过外网访问 Windows10系统安装步骤 1.打开控制面板 2.选择程序…

Hadoop3教程(三十一):(生产调优篇)异构存储

文章目录 (157)异构存储概述概述异构存储的shell操作 (158)异构存储案例实操参考文献 (157)异构存储概述 概述 异构存储,也叫做冷热数据分离。其中,经常使用的数据被叫做是热数据&…

Python Opencv实践 - 车辆统计(2)检测线绘制,车辆数量计数和显示

针对我所使用的视频,对上一节的代码进行了修改,增加了更多参数。 Python Opencv实践 - 车辆统计(1)读取视频,移除背景,做预处理_亦枫Leonlew的博客-CSDN博客示例中的图像的腐蚀、膨胀和闭运算等需要根据具…

操作系统-浅谈CPU与内存

目录 计算机的基本组成CPU内存虚拟内存内存分段内存分页 CPU与内存的交互过程高速缓存cache 所有图片均来自:小林coding 计算机的基本组成 计算机由软件和硬件组成 硬件由CPU(中央处理器)存储器(内存外存)外部设备组成。 软件由应用软件和系…

【算法】模拟退火算法(SAA,Simulated Annealing Algorithm)

模拟退火算法(SAA)简介 模拟退火算法(SAA,Simulated Annealing Algorithm)的灵感来源于工艺铸造流程中的退火处理,随着铸造温度升高,分子运动趋于无序,徐徐冷却后,分子运…

ES6初步了解Symbol的用法

ES6中为我们新增了一个原始数据类型Symbol,让我为大家介绍一下吧! Symbol它表示是独一无二的值 Symbol要如何创建 第一种创建方式: let sy Symbol()第二种创建方式: let sy Symbol.for()具体独一无二在哪呢?它们的地…

开源思维导图白板工具

https://okso.app https://drawio.com https://tldraw.com https://excalidraw.com

18 Transformer 的动态流程

博客配套视频链接: https://space.bilibili.com/383551518?spm_id_from333.1007.0.0 b 站直接看 配套 github 链接:https://github.com/nickchen121/Pre-training-language-model 配套博客链接:https://www.cnblogs.com/nickchen121/p/15105048.html 机…

npm install 报node-sass command failed

一、前言 最近在前端项目Vue项目install时会出现node-sass command failed的错误,原因是NodeJS和node-sass的版本不对应导致的,本文将给出解决方案。 二、解决方案 以下是NodeJS和node-sass版本的对照关系:

C++前缀和算法的应用:摘水果 原理源码测试用例

本文涉及的基础知识点 C算法:前缀和、前缀乘积、前缀异或的原理、源码及测试用例 包括课程视频 题目 在一个无限的 x 坐标轴上,有许多水果分布在其中某些位置。给你一个二维整数数组 fruits ,其中 fruits[i] [positioni, amounti] 表示共…

1221. 四平方和--(暴力,二分)

题目&#xff1a; 1221. 四平方和 - AcWing题库 思路1&#xff1a;暴力 暴力枚举 1.枚举顺序为从a到c&#xff0c;依次增大。 2.tn-a*a-b*b-c*c&#xff0c;求得dsqrt(t) 3.判断求出的d是否成立。d要求&#xff1a;d*dt&&d>c #include<iostream> #include&…

pytorch实战---IMDB情感分析

&#x1f497;&#x1f497;&#x1f497;欢迎来到我的博客&#xff0c;你将找到有关如何使用技术解决问题的文章&#xff0c;也会找到某个技术的学习路线。无论你是何种职业&#xff0c;我都希望我的博客对你有所帮助。最后不要忘记订阅我的博客以获取最新文章&#xff0c;也欢…

原始航片匀色调色方法

使用PhotoRC 2.0软件&#xff0c;对原始航片进行批量匀色&#xff0c;可以自动处理和人机交互&#xff0c;保留exif信息。 软件下载链接&#xff1a; https://pan.baidu.com/s/1Jj4cMpq8xzYvSa1hhozH-g?pwdndfm 提取码&#xff1a;ndfm

知识点滴 - Email地址不区分大小写

电子邮件地址本身对字符大小写不敏感。这意味着实际的电子邮件地址&#xff0c;如 "exampleemail.com"&#xff0c;并不区分字母的大小写。无论你输入的是大写字母还是小写字母&#xff0c;它仍然会到达同一个电子邮件账户。例如&#xff0c;如果您的电子邮件地址是 …

Linux 用户必备的 Git 图形化工具

Git 是一个免费的开源分布式版本控制系统&#xff0c;用于软件开发和其他几个版本控制任务。它旨在根据速度、效率和数据完整性来处理从小到大的项目。 Linux 用户主要可以通过命令行管理 Git&#xff0c;但是&#xff0c;有几个图形用户界面 (GUI) Git 客户端可以促进在 Linux…

DevOps持续集成-Jenkins(3)

文章目录 DevOpsDevOps概述Jenkins实战3&#xff1a;实战1和实战2的加强版&#xff08;新增SonarQube和Harbor&#xff09;⭐环境准备⭐项目架构图对比Jenkins实战1和实战2&#xff0c;新增内容有哪些&#xff1f;SonarQube教程采用Docker安装SonarQube &#xff08;在Jenkins所…

linux-防火墙

目录 一、防火墙概念 1.软件防火墙 2.iptables默认规则 3.iptables的五链 4.iptables动作 5.四表五链 6.iptables实例 一、防火墙概念 linux下防火墙一般分为软件防火墙、硬件防火墙 硬件防火墙&#xff1a;在硬件的级别实现防火墙过滤功能&#xff0c;性能高&#xf…

centos服务器搭建安装Gitlab教程使用教程

1、更新服务器&#xff1a; sudo yum update -y && sudo yum upgrade -y 2、下载Gitlab的RPM包 https://packages.gitlab.com/gitlab/gitlab-cece表示开源el表示centos 选64位el8对应CentOS8 本教程以centos8为例&#xff0c;在服务器中&#xff0c;下载centos8的…

基于Android 10系统的瑞芯微RK3399K烧写镜像实录

基于Android 10系统的瑞芯微RK3399K烧写镜像实录 1. 前言2. 官网及相关资料3. 烧写固件所需软件4. 直接烧写单一固件步骤5. 固件文件6. Windows下烧写准备6.1 安装 RK USB 驱动6.2 连接设备 7. 烧写固件7.1 烧写统一固件 update.img7.2 烧写分区映像 8. Linux下烧写8.1 upgrade…

STM32 HAL库串口使用printf

STM32 HAL库串口使用printf 背景配置说明在usart.h中添加在usart.c中添加在工程中选中微库&#xff1a; 测试 背景 在我们使用CubeMX生成好STM32 HAL库工程之后&#xff0c;我们想使用printf函数来打印一些信息&#xff0c;配置如下&#xff1a; 配置说明 在usart.h中添加 …