2023张江智能机器人科创赛
2023 Zhangjiang Intelligent Robot Science and
Technology Innovation Competition
答辩人：史家箫
指导老师：李跃峰
目录
CONTENTS
01
赛题
02
备赛过程
03
表盘表带设计制作
04
算法设计
05
收获总结
Part1
赛题
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
1. 赛题内容
01 传感器系统
以人体手臂为对象，
大臂和小臂分别穿戴m
和s两个惯性测量模块，
组成用于测量手臂姿态
的传感器系统。
02 时刻位姿记录
(1) t0时刻时，m和s模块
的姿态分别为mt0与st0；
(2) t1时刻时，m和s模块
的姿态分别为mt1与st1。
03 求解姿态变化量
求从t0时刻到
t1时刻姿态角
的变化量，即
θm与θs之差。
Part2
备赛过程
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
2. 备赛过程
设计表带，并激光切割
根据数学方法形成代
对imu模块LPMS-B2
成型；组装表盘表带，
码实现功能；发现并
进行激光扫描成型
根据佩戴情况微调
解决问题，优化算法
01
表盘表带
设计制作
02
03
04
三维设计可将imu模块
学习数学理论知
佩戴在大臂和小臂上的
识，分析出解决
表盘，并对其3D打印
问题的多种方法
05
算法设计
赛题
3. 表盘表带设计制作
备赛过程
表盘表带
设计制作
算法设计
收获总结
3.1 表盘建模
根据SolidWorks中的LPMS-B2
模型进行表盘的模型设计
LPMS-B2模型
表盘第一版模型
3.2 表盘3D打印
根据imu和表盘模型的适配
程度微调模型，再次3D打印
表盘第二版模型
3.3 表带设计
表盘3D打印模型
表
带
模
型
用CAXA根据腕、臂围及表盘
预留空隙设计表带长度和宽度
3.4 表带激光切割并组装
表盘表带与imu组装
imu模块佩戴在手腕上
Part4
算法设计
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.1 设计过程
理论学习
方法设计
代码编写
发现问题
卡尔曼滤波
方法一
四元数
欧拉角
改进优化
方法二
逐级输出
C++
串口编
码
找到根源
优化解决
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.2 数学理论
组成
直观理解
𝒒 𝒘, 𝒙, 𝒚, 𝒛 = 𝒒𝒘 + 𝒒𝒗= 𝒒𝒘 + 𝒒𝒙 · ,⃑ + 𝒒𝒚 · -⃑ + 𝒒𝒛 · 𝒌
𝜽
𝜽
𝒒 = 𝒄𝒐𝒔 + 𝒓 · 𝒔𝒊𝒏
𝟐
𝟐
向量的旋转
相对同一坐标系
四元数
旋转公式
相对旋转关系
相对同一坐标系
连续旋转两次
相对同一坐标系
绕向量𝒓旋转𝜽
𝑽! = 𝒒 ' 𝑽 ' 𝒒"𝟏 ， 𝑽 = 𝟎, 𝒓
𝑨 = 𝒒𝟏 % 𝑽 % 𝒒𝟏!𝟏
𝑩 = 𝒒𝟐 % 𝑽 % 𝒒𝟐!𝟏
𝑩 = (𝒒𝟐 % 𝒒𝟏!𝟏 ) % 𝑨 % (𝒒𝟐 % 𝒒𝟏!𝟏 )!𝟏
𝒒 = 𝒒𝟐 · 𝒒𝟏
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.3.1 设计算法
单自由度系统 方法一
imu与所在胳膊
相对关系不变
双自由度系统 方法二
两臂间相对旋
转关系变化量
小臂朝向
不改变
小臂旋转
f=1
相同
解析得到
夹角
imu间相对旋
转关系变化量
imu的X轴
胳膊朝向
imu间时刻相
对旋转关系
消除初始
关系
imu的X轴单
位向量
点乘得到
夹角
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.3.2 数据读取问题
数据读取
3秒之后数据才稳定
延时函数
四元数初始不是1000
且两臂毫无关联
标定坐标系
欧拉角数据合理
四元数为实际共轭
四元数取共轭
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.3.3 名词解释
标定位姿：借助参考使imu姿态相同
位姿
初始位姿：在手臂上的最初姿态
任意时刻位姿：任意时刻姿态
名
词
解
释
标定位姿
世界坐标系(world)：芯片旋转信息的相对坐标系(不同)
坐标系
自身坐标系：固定在芯片的坐标系
标定坐标系(set)：标定时芯片坐标系(同)
q：坐标系之间的相对旋转关系
变量
𝑣⃑ ：四元数形式，计算在不同坐标系的方向
angle： 夹角或夹角的变化量
自
身
坐
标
系
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.4 方法一
标
定
qA
qB
①上位机
相对标定坐标系！
qA2B_init
set
②算
法
消
除
初
始
关
系
qA
qA_new
qA2B_init
qA_new
q_relative
qB
angle
计
算
初
始
关
系
计
算
夹
角
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.4.1 上位机标定
上位机标定
qA qB 相对世界坐标系
断连后z旋转90°，再连接仅改变2°
断开连接
Z轴旋转不记录
自定义算法
赛题
4.4.2 算法标定一
表盘表带
设计制作
算法设计
收获总结
注1：qX代表的是X自身坐标系相对于测量相对坐标系的旋转四元数
注2：向量和四元数使用时均满足归一化条件
得
到
A
标
定
记
录
备赛过程
A和B实际位姿相同！
相对标定坐标系！
qA_set
𝒒𝑨 =
qB_set
𝒒𝑨_𝒔𝒆𝒕!𝟏 ) 𝒒𝑨
标定坐标系旋转
回世界坐标系时，
A就变为相对标
定坐标系！
四
元
数
赛题
4.4.3 算法标定二
算法设计
收获总结
注1：qX代表的是X自身坐标系相对于测量相对坐标系的旋转四元数
注2：向量和四元数使用时均满足归一化条件
A和B实际位姿相同！
相对世界坐标系！
qA_set
𝒒𝑨𝒘𝒐𝒓𝒍𝒅 =
qB_set
𝒒𝑨 ) 𝒒𝑨_𝒔𝒆𝒕!𝟏
𝒒𝑨𝒘𝒐𝒓𝒍𝒅
𝒒𝑽#
𝒒𝑽
=
𝒒𝑨_𝒔𝒆𝒕!𝟏 ) 𝒒𝑽 ) 𝒒𝑨_𝒔𝒆𝒕
相对标定坐标系(同)！
𝒒𝑨𝒘𝒐𝒓𝒍𝒅
𝒒𝒘
𝒒𝑽′
qA
更
新
相
对
关
系
得
到
A
向
量
坐
标
系
变
换
表盘表带
设计制作
A
标
定
记
录
备赛过程
四
元
数
赛题
4.4.4 计算初始关系一
备赛过程
表盘表带
设计制作
算法设计
收获总结
注1：qX代表的是X自身坐标系相对于测量相对坐标系的旋转四元数
注2：向量和四元数使用时均满足归一化条件
𝒒𝑨𝟐𝑩_𝒊𝒏𝒊𝒕
初
始
记
录
相对标定坐标系(同)！
相对A坐标系！
qA_init
𝒒𝑨𝟐𝑩_𝒊𝒏𝒊𝒕 =
qB_init
𝒒𝑨_𝒊𝒏𝒊𝒕/𝟏 - 𝒒𝑩_𝒊𝒏𝒊𝒕
获
得
初
始
关
系
赛题
4.4.5 计算初始关系二
初
始
记
录
向
量
坐
标
系
变
换
备赛过程
表盘表带
设计制作
算法设计
收获总结
注1：qX代表的是X自身坐标系相对于测量相对坐标系的旋转四元数
注2：向量和四元数使用时均满足归一化条件
相对标定坐标系(同)！
相对标定坐标系(同)！
qA_init
𝒒𝑨𝟐𝑩_𝒔𝒆𝒕 =
qB_init
𝒒𝑩_𝒊𝒏𝒊𝒕 - 𝒒𝑨_𝒊𝒏𝒊𝒕/𝟏
𝒒𝑨𝟐𝑩_𝒔𝒆𝒕
𝒒𝑽
𝒒𝑽# =
𝒒𝑨_𝒊𝒏𝒊𝒕!𝟏
) 𝒒𝑽 ) 𝒒𝑨_𝒊𝒏𝒊𝒕
𝒒𝑨𝟐𝑩_𝒔𝒆𝒕
𝒒𝒘
𝒒𝑽′
相对A坐标系！
𝒒𝑨𝟐𝑩_𝒊𝒏𝒊𝒕
获
得
初
始
位
姿
获
得
初
始
关
系
赛题
表盘表带
设计制作
算法设计
收获总结
注1：qX代表的是X自身坐标系相对于测量相对坐标系的旋转四元数
注2：向量和四元数使用时均满足归一化条件
4.4.6 消除初始关系
获
得
位
姿
备赛过程
相对标定坐标系(同)！
qA
qB
相对标定坐标系(同)！
𝒒𝑨_𝒏𝒆𝒘 =
𝒒𝑨 - 𝒒𝑨𝟐𝑩_𝒊𝒏𝒊𝒕
AB同样旋转，把A旋
转回标定坐标系时B的
位姿 ，让B做和A相同
的旋转，后续和B对比
消
除
初
始
关
系
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.4.7 计算夹角
获
得
相
对
旋
转
关
系
𝜽
𝜽
𝒒 = 𝒄𝒐𝒔 + 𝒓 · 𝒔𝒊𝒏
𝟐
𝟐
𝒒_𝒓𝒆𝒍𝒂𝒕𝒊𝒗𝒆
相对标定坐标系(同)！
𝒒_𝒓𝒆𝒍𝒂𝒕𝒊𝒗𝒆 =
!𝟏
𝒒𝑨_𝒏𝒆𝒘 . 𝒒𝑩
𝒒𝒘
𝒂𝒏𝒈𝒍𝒆 =
𝟐 . 𝒂𝒓𝒄𝒄𝒐𝒔(𝒂𝒃𝒔(𝒒𝒘))
计
算
夹
角
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.5 方法二
①上位机
相
同
X
标
定
qA
qB
轴
朝
向
②算
法
X
更
新
轴
朝
向
相对标定坐标系！
𝒙_𝒊𝒏𝒊𝒕
qA qB
𝒙𝑨 𝒙𝑩
𝒙𝑨 = 𝒒𝑨 % 𝒙_𝒊𝒏𝒊𝒕 % 𝒒𝑨3𝟏
𝒂𝒏𝒈𝒍𝒆 = 𝒙𝑨 ) 𝒙𝑩
计
算
夹
角
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.6 算法调试和修改
方法二测试
𝒙𝑨 = 𝒒𝑨 % 𝒙_𝒊𝒏𝒊𝒕 % 𝒒𝑨3𝟏
输出芯片X轴单位向量
期望：( 𝟏, 𝟎, 𝟎 )
实际：( 𝟎, −𝟏, 𝟎 )
上位机标定时芯片X轴
世界坐标系
后用程序标定时芯片X轴
任意时刻芯片X轴
相对旋转关系
标定坐标系
赛题
备赛过程
表盘表带
设计制作
算法设计
4.7 测试结果和展示
标定之后的四元数为1000
夹角约为0度时算得夹角
使用机械臂测试算
法准确性和精度
夹角约为90度时算得夹角
收获总结
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.8 算法比较和结论
难易程度
方法一 > 方法二
一：芯片随意摆放
优异比较
二：小臂可转动
方法一：单自由度
适用条件
算法精度
方法二：双自由度
锻炼价值
方法一 > 方法二
近似且精度较高
两者结合
可测小臂旋转角度！
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
4.9 加速度积分求位移
单位确定
三个方向平方和开方
单位为 g
自由落体和接住时
零点漂移
较稳定初始非零值
积分
采样频率为100HZ
精度
仅加速度明显精度较好
记录并与测量值做差
运算时间不干扰采样时间间隔
陀螺仪精度、稳定性不好
多线程
累积误差大
Part5
收获总结
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
5.1 收获总结
设计模型、严谨性和归零精神
动手能力
3D打印、激光切割、组装模型
PPT制作能力
收
获
总
结
卡尔曼滤波
理论知识
四元数
欧拉角
设计算法、编写代码能力、独立思考能力
工程能力
解决问题能力：分解程序输出，找到问题的根源
书写工程日志，为优化提供依据
赛题
备赛过程
表盘表带
设计制作
算法设计
收获总结
5.2 参考内容及图片来源
[1] 四元数(Quaternions) ——lxycg
参考内容
[2] 四元数和旋转(Quaternion & rotation) ——lxycg
知乎
[3] 如何通俗的理解欧拉角？之后为什么要引入四元数？——马同学
[4] 如何通俗且尽可能详细的解释卡尔曼滤？——司南牧(李韬)
B站
图片来源
[1] 四元数的可视化 ——3Blue1Brown
文章内、拍摄、iPad手绘
致 谢
感谢主办方提供的学习和竞赛的平台；
竞赛感谢母校哈尔滨工业大学提供的学习与实践的机会；
感谢李跃峰老师、郑一珍博士给予的耐心指导；
感谢一起比赛的同学们的帮助；
感谢一起比赛现场帮我调试的两位老师；
特别感谢杨雨晨同学在和我讨论过程中给予我的灵感；
感谢各位评委老师和听众！
请评委老师指正
答辩人：史家箫
指导老师：李跃峰