平面运动机器人的传感器外参标定

简述

对任意两个传感器进行外参标定可以采用手眼标定算法来完成,但是,传统手眼标定算法对于运动具有一定的要求,可以证明,至少需要两个以上轴角方向不同的旋转运动才可以正确估计出外参旋转,因此,如果使用手眼标定算法标定外参,例如标定imu与激光雷达、imu与相机、多激光雷达的外参… 那么需要充分的全方位空间的旋转和平移运动,才可以成功完成标定。

但是如果机器人是处于平面运动的地面机器人呢?那么这个时候很可能很多的外参都是不可观的,准确来说,依然存在一个特殊情况,使得我们可以使用特定的算法标定出除了z轴平移外的全部参数,即其中一个传感器旋转运动时的轴角方向和它的z轴刚好平行,除此之外,也就是说如果你的传感器安装角度不满足这个条件,那么就别指望仅靠平面上运动就能完成标定。

简单来说,对于平面运动机器人的传感器外参标定,有两个结论:
1、如果其中一个传感器的旋转运动的轴角方向与其传感器坐标z轴是平行的,那么可以标定出除了z轴平移外的全部参数。
2、任意安装的两个传感器在平面运动下外参不可观。

例如,平面运动机器人中,轮速计、2d激光雷达等和车体平行安装的传感器都满足结论一的条件。下面将介绍如何通过算法求解出外参。

算法

分为两个步骤 1、先求解出旋转的pitch和roll。2、求解yaw、x,y。

step1:
首先我们知道手眼标定的旋转关系:
q R i R i + 1 ⊗ q C R = q C R ⊗ q C i C i + 1 q^{R_{i+1}}_{R_i}\otimes q^R_C=q^R_C \otimes q^{C_{i+1}}_{C_i} qRiRi+1qCR=qCRqCiCi+1其中R表示任何与车体水平安装的传感器(例如轮速计、2D激光…),C表示任意其他3D传感器如相机、3D激光雷达… 外参旋转为 q C R q^R_C qCR
由上式得:
q R i R i + 1 ⊗ q C R − q C R ⊗ q C i C i + 1 = 0 = > ( Q L ( q R i R i + 1 ) − Q R ( q C i C i + 1 ) ) q C R = 0 (1) q^{R_{i+1}}_{R_i}\otimes q^R_C-q^R_C \otimes q^{C_{i+1}}_{C_i}=0 \\ =>\begin{aligned} (Q_L(q^{R_{i+1}}_{R_i})-Q_R(q^{C_{i+1}}_{C_i}))q^R_C=0 \end{aligned}\tag{1} qRiRi+1qCRqCRqCiCi+1=0=>(QL(qRiRi+1)QR(qCiCi+1))qCR=0(1)

Q L , Q R Q_L,Q_R QL,QR即四元数的左乘、右乘矩阵。
因此转换为了一个齐次线性方程组,在给定约束 ∣ ∣ q C R ∣ ∣ = 1 ||q^R_C||=1 ∣∣qCR∣∣=1下求解该方程的最小二乘解即可解得 q C R q^R_C qCR
但是由于我们的运动被限制为平面运动,上面方程的秩为2,存在多余的零空间维度,因此无法正确求解,因此我们采用别的技巧进行处理,首先将外参旋转表示为z-y-z 欧拉角形式:
q C R = q z ( α ) q y ( β ) q z ( γ ) (2) \begin{aligned} q^R_C=q_z(\alpha)q_y(\beta)q_z(\gamma)\end{aligned}\tag{2} qCR=qz(α)qy(β)qz(γ)(2) q z ( α ) q_z(\alpha) qz(α)即绕z轴旋转 α \alpha α角度 , q y ( β ) q_y(\beta) qy(β)为绕y轴旋转 β \beta β角度。
根据轴角到四元数的转换关系 q = [ c o s α 2 , u s i n α 2 ] q=[cos\frac{\alpha}{2},usin\frac{\alpha}{2}] q=[cos2α,usin2α],可得:
q z ( α ) = [ c o s α 2 , 0 , 0 , s i n α 2 ] T q_z(\alpha)=[cos\frac{\alpha}{2},0,0,sin\frac{\alpha}{2}]^T qz(α)=[cos2α,0,0,sin2α]T q y ( β ) = [ c o s β 2 , 0 , s i n β 2 , 0 ] T q_y(\beta)=[cos\frac{\beta}{2},0,sin\frac{\beta}{2},0]^T qy(β)=[cos2β,0,sin2β,0]T

将(2)带入(1)可得:
q R i R i + 1 ⊗ q z ( α ) q y ( β ) q z ( γ ) − q z ( α ) q y ( β ) q z ( γ ) ⊗ q C i C i + 1 = 0 q^{R_{i+1}}_{R_i}\otimes q_z(\alpha)q_y(\beta)q_z(\gamma)\\-q_z(\alpha)q_y(\beta)q_z(\gamma)\otimes q^{C_{i+1}}_{C_i}=0 qRiRi+1qz(α)qy(β)qz(γ)qz(α)qy(β)qz(γ)qCiCi+1=0关键来了,由于传感器R的旋转轴与z轴平行,因此可将 q R i R i + 1 q^{R_{i+1}}_{R_i} qRiRi+1 q z ( α ) q_z(\alpha) qz(α)的顺序交换,可得下式:
δ = q z ( α ) ⊗ q R i R i + 1 q y ( β ) q z ( γ ) − q z ( α ) q y ( β ) q z ( γ ) ⊗ q C i C i + 1 = q z ( α ) ( q R i R i + 1 q y ( β ) q z ( γ ) − q y ( β ) q z ( γ ) q C i C i + 1 ) \delta =q_z(\alpha) \otimes q^{R_{i+1}}_{R_i} q_y(\beta)q_z(\gamma)\\-q_z(\alpha)q_y(\beta)q_z(\gamma)\otimes q^{C_{i+1}}_{C_i}\\=q_z(\alpha)(q^{R_{i+1}}_{R_i} q_y(\beta)q_z(\gamma)-q_y(\beta)q_z(\gamma)q^{C_{i+1}}_{C_i}) δ=qz(α)qRiRi+1qy(β)qz(γ)qz(α)qy(β)qz(γ)qCiCi+1=qz(α)(qRiRi+1qy(β)qz(γ)qy(β)qz(γ)qCiCi+1)
显然,
q R i R i + 1 q y ( β ) q z ( γ ) − q y ( β ) q z ( γ ) q C i C i + 1 = 0 q^{R_{i+1}}_{R_i} q_y(\beta)q_z(\gamma)-q_y(\beta)q_z(\gamma)q^{C_{i+1}}_{C_i}=0 qRiRi+1qy(β)qz(γ)qy(β)qz(γ)qCiCi+1=0
q y ( β ) q z ( γ ) q_y(\beta)q_z(\gamma) qy(β)qz(γ)记为 q Y Z q_{YZ} qYZ,
q Y Z = [ c o s ( β 2 ) c o s ( γ 2 ) s i n ( β 2 ) s i n ( γ 2 ) s i n ( β 2 ) c o s ( γ 2 ) c o s ( β 2 ) s i n ( γ 2 ) ] (3) \begin{aligned} q_{YZ}=\begin{bmatrix} cos(\frac{\beta}{2})cos(\frac{\gamma}{2})\\sin(\frac{\beta}{2})sin(\frac{\gamma}{2}) \\ sin(\frac{\beta}{2})cos(\frac{\gamma}{2})\\cos(\frac{\beta}{2})sin(\frac{\gamma}{2})\end{bmatrix}\end{aligned} \tag{3} qYZ= cos(2β)cos(2γ)sin(2β)sin(2γ)sin(2β)cos(2γ)cos(2β)sin(2γ) (3)

于是,这个问题转换为一个求解 q Y Z q_{YZ} qYZ的最小二乘问题:
q Y Z = arg min ⁡ q Y Z ∣ ∣ M q Y Z ∣ ∣ 2 M = [ L ( q R i R i + 1 ) − R ( q C i C i + 1 ) . . L ( q R 0 R 1 ) − R ( q C 0 C 1 ) ] q_{YZ}=\argmin \limits_{q_{YZ}}||Mq_{YZ}||^2\\ M=\begin{bmatrix} L(q^{R_{i+1}}_{R_i})-R(q^{C_{i+1}}_{C_i}) \\ .\\.\\L(q^{R_{1}}_{R_0})-R(q^{C_{1}}_{C_0})\end{bmatrix} qYZ=qYZargmin∣∣MqYZ2M= L(qRiRi+1)R(qCiCi+1)..L(qR0R1)R(qC0C1)
可以证明,这个矩阵M的秩为2,证明过程见文献【2】,因此,上述最小二乘解的零空间维度为2,因此我们需要继续寻找约束,根据(3)式可知: q Y Z 1 q Y Z 2 = q Y Z 3 q Y Z 4 q_{YZ1}q_{YZ2}=q_{YZ3}q_{YZ4} qYZ1qYZ2=qYZ3qYZ4另外表示旋转的四元数都是单位四元数: ∣ ∣ q Y Z ∣ ∣ 2 = 1 ||q_{YZ}||_2=1 ∣∣qYZ2=1对于上述齐次线性最小二乘问题,其解为 M T M M^TM MTM最小特征值对应的特征向量,因为M的秩为2,因此有两个为0的最小特征值,这两个0特征值对应的两个线性无关的特征向量记为 t 1 , t 2 t_1,t_2 t1,t2,它们构成了零空间的一组基(实对称矩阵必能对角化,有n个相同的特征值必然存在n个不相关的特征向量),因此解可以表示为: q Y Z = a t 1 + b t 2 q_{YZ}=at_1+bt_2 qYZ=at1+bt2根据上面给出的两个约束即可以求解出 a , b a,b a,b
需要注意的是,由于噪声影响, M T M M^TM MTM最终是满秩的,并不会有两个真正为0的特征值,因此只需要取2个最小的特征值对应的特征向量即可。

step2求解yaw,x,y

根据手眼标定,有关系:
( R ( q R i R i + 1 ) − I ) p C R − R Z ( α ) R ( q Y Z ) p C i C i + 1 + p R i R i + 1 = 0 (R(q^{R_{i+1}}_{R_i})-I)p^R_C-R_Z(\alpha)R(q_{YZ})p^{C_{i+1}}_{C_i}+p^{R_{i+1}}_{R_i}=0 (R(qRiRi+1)I)pCRRZ(α)R(qYZ)pCiCi+1+pRiRi+1=0其中, q Y Z q_{YZ} qYZ即step1求解的结果, R Z ( α ) R_Z(\alpha) RZ(α)即外参旋转待求的一部分, p C R p^R_C pCR为待求外参平移。
由于外参平移中,z发生变化时传感器的运动观测保持不变 ,因此z方向的平移是不可观的,因此将外参平移z设为0,并丢弃上式第3行,
R ( q Y Z ) p C i C i + 1 R(q_{YZ})p^{C_{i+1}}_{C_i} R(qYZ)pCiCi+1即将传感器C的运动投影到运动平面,令: R ( q Y Z ) p C i C i + 1 = [ p i 1 p i 2 p i 3 ] R(q_{YZ})p^{C_{i+1}}_{C_i}=\begin{bmatrix}p_{i_1}\\p_{i_2}\\p_{i_3}\end{bmatrix} R(qYZ)pCiCi+1= pi1pi2pi3
带入上式并忽略第3行,可得: ( R ( q R i R i + 1 ) − I ) 2 × 2 [ p x p y ] − [ c o s α − s i n α s i n α c o s α ] [ p i 1 p i 2 ] + p R i R i + 1 = 0 (R(q^{R_{i+1}}_{R_i})-I)_{2\times2}\begin{bmatrix}p_x\\p_y\end{bmatrix}-\\\begin{bmatrix}cos\alpha&-sin\alpha \\ sin\alpha & cos\alpha \end{bmatrix}\begin{bmatrix} p_{i_1}\\ p_{i_2}\end{bmatrix}+p^{R_{i+1}}_{R_i}=0 (R(qRiRi+1)I)2×2[pxpy][cosαsinαsinαcosα][pi1pi2]+pRiRi+1=0
使用最小二乘法即可解得 p x , p y , α p_x,p_y,\alpha px,py,α,具体细节见参考文献【2】。

reference:

[1] 标定系列一 | 机器人手眼标定的基础理论分析 https://zhuanlan.zhihu.com/p/93183788?utm_psn=1706381723099791360
[2] An Analytical Least-Squares Solution to the Odometer-Camera Extrinsic Calibration Problem

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

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

相关文章

54基于matlab的包络谱分析

基于matlab的包络谱分析,目标信号→希尔伯特变换→得到解析信号→求解析信号的模→得到包络信号→傅里叶变换→得到Hilbert包络谱,包络谱分析能够有效地将这种低频冲击信号进行解调提取。程序已调通,可直接运行。 54matlab包络谱分析信号解调…

​LeetCode解法汇总187. 重复的DNA序列

目录链接: 力扣编程题-解法汇总_分享记录-CSDN博客 GitHub同步刷题项目: https://github.com/September26/java-algorithms 原题链接:力扣(LeetCode)官网 - 全球极客挚爱的技术成长平台 描述: DNA序列 由…

轻量日志管理方案-[EFK]

使用FileBeat进行日志文件的数据收集,并发送到ES进行存储,最后Kibana进行查看展示; 这个应该是最简单,轻量的日志收集方案了。 最总方案为:FileBeatESKibana ; 【Kibana过于强大,感觉可以无限扩展】 文章目…

SpringBoot 全局日期时间格式转化

一、添加注解 在属性上加上注解,对日期进行格式化 import com.fasterxml.jackson.annotation.JsonFormat;JsonFormat(pattern "yyyy-MM-dd HH:mm:ss") private LocalDateTime createTime;二、全局配置 a. 在 WebMvcConfiguration 中扩展 SpringMVC 的消息…

msvcp140_CODECVT_IDS.dll丢失怎么办?msvcp140_CODECVT_IDS.dll丢失5个解决办法详解

首先,我要讲述一下我是如何遇到这个问题的。那时候,我正在打开一个电脑的应用程序,使用软件(ps)进行编程。在打开软件时候,突然发现程序无法正常启动,弹出了一个错误提示框,显示msvc…

Hive 知识点八股文记录 ——(一)特性

Hive通俗的特性 结构化数据文件变为数据库表sql查询功能sql语句转化为MR运行建立在hadoop的数据仓库基础架构使用hadoop的HDFS存储文件实时性较差(应用于海量数据)存储、计算能力容易拓展(源于Hadoop) 支持这些特性的架构 CLI&…

Delphi 12 重返雅典 (RAD Studio 12)

RAD Studio 12 的新功能: 以最新的平台版本为目标! RAD Studio 12 提供对 iOS 17(仅适用于 Delphi)、Android 14 和 macOS Sonoma 的官方支持。RAD Studio 12 还支持 Ubuntu 22 LTS 和 Windows Server 2022。 Delphi 源代码的多…

广州华锐互动:VR互动实训内容编辑器助力教育创新升级

随着科技的飞速发展,教育领域也正在经历一场深刻的变革。其中,虚拟现实(VR)技术为教学活动提供了前所未有的便利和可能性。在诸多的VR应用中,VR互动实训内容编辑器无疑是最具潜力和创新性的一种。广州华锐互动开发的这款编辑器以其独特的功能…

设计模式-解释器模式(Interpreter)

设计模式-解释器模式(Interpreter) 一、解释器模式概述1.1 什么是解释器模式1.2 简单实现解释器模式1.3 使用解释器模式的注意事项 二、解释器模式的用途三、解释器模式实现方式3.1 基于递归下降实现解释器模式3.2 基于LL(1)文法实现解释器模式3.3 基于A…

Java编写简易rabbitmq生产者与消费者

一、前言 开发时经常与其它系统用rabbitmq对接&#xff0c;当需要自测时&#xff0c;还是自己写rabbitmq生产者、消费者自测方便些。 下面总结下不用框架、使用java编写简易rabbitmq的方法。 二、代码 1.导入jar包 (1)如果是maven&#xff0c;那就用 <dependency>&…

百度智能云正式上线Python SDK版本并全面开源!

文章目录 1. SDK的优势2. 千帆SDK&#xff1a;快速落地LLM应用3. 如何快速上手千帆SDK3.1 SDK快速启动3.2 SDK进阶指引3.3 通过Langchain接入千帆SDK 4. 开源社区 百度智能云千帆大模型平台再次升级&#xff01;在原有API基础上&#xff0c;百度智能云正式上线Python SDK&#…

直播会议一体机安卓主板_5G智能会议一体机双屏异显设计

5G直播会议一体机主板是专门为支持音视频输入输出而设计的&#xff0c;内置有安卓13系统&#xff0c;可兼容多种直播和会议软件。该产品可广泛应用于智能会议一体机、便携式直播设备、录播导播、无人机直播以及视频传输等多个领域。 这款主板采用了国产6纳米旗舰芯片紫光展锐T8…

虚幻C++基础 day4

虚幻中的UI 虚幻中的比较常用的UI&#xff1a;Widget Blueprint又称UMG虚幻中的两种布局&#xff1a; 网格布局锚布局 创建Widget Blueprint 网格布局 有点类似Qt中的网格布局&#xff0c;将UI面板进行行列切分Horizontal Box&#xff1a;水平分布Vertical Box&#xff1a;…

机器学习---多分类SVM、支持向量机分类

1. 多分类SVM 1.1 基本思想 Grammer-singer多分类支持向量机的出发点是直接用超平面把样本空间划分成M个区域&#xff0c;其 中每个区域对应一个类别的输入。如下例&#xff0c;用从原点出发的M条射线把平面分成M个区域&#xff0c;下图画 出了M3的情形&#xff1a; 1.2 问题…

【Spring之底层核心架构概念解析】

文章目录 一、BeanDefinition二、BeanDefinitionReader2.1、AnnotatedBeanDefinitionReader2.2、XmlBeanDefinitionReader 五、ClassPathBeanDefinitionScanner六、BeanFactory七、ApplicationContext7.1、AnnotationConfigApplicationContext7.2、ClassPathXmlApplicationCont…

2023年11月PHP测试覆盖率解决方案

【题记&#xff1a;最近进行了ExcelBDD PHP版的开发&#xff0c;查阅了大量资料&#xff0c;发现PHP测试覆盖率解决方案存在不同的历史版本&#xff0c;让我花费了蛮多时间&#xff0c;为了避免后人浪费时间&#xff0c;整理本文&#xff0c;而且网上没有给出Azure DevOps里面P…

jQuery HTML/CSS 参考文档

jQuery HTML/CSS 参考文档 文章目录 应用样式 示例属性方法示例 jQuery HTML/CSS 参考文档 应用样式 addClass( classes ) 方法可用于将定义好的样式表应用于所有匹配的元素上。可以通过空格分隔指定多个类。 示例 以下是一个简单示例&#xff0c;设置了para标签 <p&g…

超详细!Linux内核内存规整详解

1.前言 伙伴系统作为内核最基础的物理页内存分配器&#xff0c;具有高效、实现逻辑简介等优点&#xff0c;其原理页也尽可能降低内存外部碎片产生&#xff0c;但依然无法杜绝碎片问题。外部碎片带来的最大影响就是内存足够&#xff0c;但是却无法满足内存分配需求&#xff0c;如…

docker创建并访问本地前端

docker创建并访问本地前端&#xff0c;直接上命令&#xff1a; 安装nginx镜像&#xff1a; docker pull nginx 查看已安装的nginx&#xff1a; docker images 创建DockerFile文件&#xff0c;直接在当前文件夹种创建 touch Dockerfile 在Dockerfile写入内容&#xff1a; F…

【仿真动画】人机协作机器人自动化产线仿真动画欣赏

人机协作机器人自动化产线仿真动画 动画部分思维导图 机器人自动化产线仿真动画是利用三维动画技术对机器人自动化产线进行仿真模拟&#xff0c;以直观、形象的方式展示产线的运行情况。它具有以下作用&#xff1a; 提高沟通效率 机器人自动化产线的设计、实施和维护涉及多个部…