SLAM导航实战（二）：硬件基础

前言

本系列主要基于《机器人SLAM导航——核心技术与实战》一书进行学习总结。

由于笔者本身不是从事硬件开发设计，故对其中部分硬件设计只做简要概括

简介

本篇作者从机器人传感器、机器人主机、机器人底盘对SLAM系统中硬件部分做了简要介绍，通过了解具体硬件的基本原理，能更好地去体会上层代码的设计思路，同时也为系统实际落地提供了思路。

机器人传感器

惯性测量单元

工作原理

测量方式通常为机械方式、光学方式、微机电方式。

加速度测量通常采用MEMS；利用高速旋转陀螺具有定轴性测量角速度，激光陀螺仪利用萨格奈克效应进行角速度测量；地磁通常采用霍尔效应进行测量。

原始数据采集

• 硬件电路搭建
• 固件驱动开发
• 上位机ROS驱动程序（发布ROS /imu话题）

参数标定

检测指标：

1. 重复上电零偏影响
2. 温度对零偏影响
3. 震动对零偏影响
4. 高冲击容忍度
5. 非线性度

内参标定：

​ 三计标定通常为轴偏差项、尺度偏差项、零偏差项对系统进行标定修正

标定模型改进：

​ 除了三计自身，还包括外部因素来影响其标定效果，温度、重力、轴间敏感度、Allan方差

数据滤波

​ 常见数据滤波：均值、滑动（解决均值输出延迟）、滑动中值滤波（剔除影响平均值的数据）、RC低通、FIR、IIR

姿态融合

​ 主要涉及两个滤波算法：卡尔曼滤波、互补滤波 （注：后续专门总结一下这部分）

激光雷达

工作原理

测距原理： 三角测距（厘米级）、TOF（毫米级）

扫描原理： 单线、多线、固态、单线多自由度、面激光

性能参数

• 激光线数

• 测距频率

• 扫描频率

• 测距量程

• 扫描角度

• 距离分辨率

• 角度分辨率

  $$角度分辨率={扫描频率\times360° \over 测距频率}$$

• 使用寿命

数据处理

上位机ROS驱动程序： 发布ROS /scan话题，标准数据格式LaerScan（单线）、MultiEchoLaserScan、PointCloud2（多线）

扫描点的时间同步： 纯估计法（主要涉及ICP、VICP）；里程计辅助法（通常里程计数据需要线性插值以便与雷达数据对齐）

相机

单目相机

无畸变内参模型：K为相机内参

$$\begin{pmatrix} U \\ V \\ 1 \end{pmatrix} = {1 \over Z} \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} X \\ Y \\ 1 \end{pmatrix} = {1 \over Z}KP$$

径向畸变： 距离像素中心 $r$ 做三件泰勒近似

外参模型：T为相机在世界坐标系下的外参

$$\begin{bmatrix} U \\ V \\ 1 \end{bmatrix} =\frac{1}{Z}KP^c=\frac{1}{Z}K(RP^W + t) = \frac{1}{Z}kTP^W$$

上位机ROS驱动程序： 通常将数据发布ROS //image_raw话题中，系统自带cheese可作为查看工具一般有三种驱动方式：

• usb_cam
• gscam（仅raw数据）
• 借助opencv自制驱动

相机标定： 查看ROS功能包camera_calibration

双目相机

深度信息获取原理：

$$\frac{Z-f}{Z} = \frac{b - U_L + U_R}{b}$$

简化结果：

$$Z = \frac{f\times b}{U_L - U_R}$$

工艺原因，通常两个相机不能保持绝对平行

此时相机成像模型

$$s_LP_L=K_LP$$ $$s_RP_R = K_R(RP+t)$$

对极几何约束：P点不在两者射线时，估计P在射线$C1P、C2P$ 的公垂线的中点

RGB-D相机

测距原理：三角测距、TOF

带编码器的减速电机

基本介绍

行星减速箱：相同减速比体积更小

编码器：霍尔、光电、碳刷

通信协议

rosserial是专门开发与外部数据串口通信的功能包

整体控制系统图如下：

通常通过/odom、/cmd_vel两个话题分别发布里程计数据以及，速度控制信息。

---

机器人主机

树莓派

建议安装Ubuntu MATE，同时给定SWAP空间充当运行内存

实用设置

永久开启ssh

sudo systemctl enable ssh

USB外设绑定

创建绑定rules文件前的序号越大代表优先级越低，一般将优先级设定小一定，创建文件/etc/udev/rules.d/99-appz99-usb-serial.rules

# appz99
KERNELS=="1_1.3",ATTRS{idProduct}=="7523",ATTRS{idVendor}=="LA86",symlink+="appz99",MODE="0777"

# lidar
KERNELS=="1_1.4",ATTRS{idProduct}=="ea60",ATTRS{idVendor}=="10c4",symlink+="lidar",MODE="0777"

(上面代码中1_1.3为**1-1.3，1.4同理)

KERNELS、idProduct、idVendor可以唯一确定设别

插入新设备用如下指令查看对应KERNELS、idProduct、idVendor

udevadm info -a -p $(udevadm info -q path -n <devpath>)

（注：替换成新插入的设备号）

查看是否绑定成功

ll /dev |grep ttyUSB

（注：此处ttyUSB为事例）

ROS节点开机自启动

sudo apt install ros-melodic-robot-upstart
rosddep install robot_upstart

# 将需要自启动的文件节点写入demo1的launch文件中，并用如下指令进行装载
roscore
rosrun robot_upstart install example/launch/demo1.launch --job myrobot --logdir ~/.ros/myrobot.log

# 启动任务
sudo systemctl daemon-reload && sudo systemctl start myrobot
# 重启任务
sudo systemctl restart myrobot
#停止任务
sudo systemctl stop myrobot

# 删除启动任务
roscore
rosrun robot_upstart uninstall myrobot

（注：myrobot为自启动任务的别名）

分布式通信

需要在网络下指定唯一MASTER

ssh本质：命令传递，所以启动不了机器人本地rviz

---

机器人底盘

底盘运动学模型

两轮差速模型

前向运动学： 根据上图建模可得

$$\begin{bmatrix} v_x \\ \omega_z \end{bmatrix} =\begin{bmatrix} 1/2 & 1/2\\ -1/d & 1/d \end{bmatrix} \begin{bmatrix} V_L \\ V_R \end{bmatrix}$$

逆向运动学： 正向运动学求逆即可

$$\begin{bmatrix} V_L \\ V_R \end{bmatrix} = \begin{bmatrix} 1 & -d/2\\ 1 & d/2 \end{bmatrix} \begin{bmatrix} v_x \\ \omega_z \end{bmatrix}$$

轨迹推演：两轮差速不发生侧向滑动 $v_y = 0$

$$\dot{P} = \begin{bmatrix} \dot{X} \\ \dot{Y} \\ \dot{\theta } \end{bmatrix} =\begin{bmatrix} \cos \theta & - \sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega_z \end{bmatrix}$$

四轮差速模型

前向运动学： 根据上图建模可得

$$\begin{bmatrix} v_{cx} \\ \omega_c \end{bmatrix} =\begin{bmatrix} 1/2 & 1/2\\ -1/c & 1/c \end{bmatrix} \begin{bmatrix} V_L \\ V_R \end{bmatrix}$$

逆向运动学： 正向运动学求逆即可

$$\begin{bmatrix} V_L \\ V_R \end{bmatrix} = \begin{bmatrix} 1 & -c/2\\ 1 & c/2 \end{bmatrix} \begin{bmatrix} v_{cx} \\ \omega_c \end{bmatrix}$$

航迹推演： 不能存在太严重的侧向滑动是上式成立的前提条件，若考虑侧向滑动，以COM建立右手坐标系，ICR的位置以$(x_{ICR}, y_{ICR})$表示，则有

$$v_{cy}-\omega_cd_{cx}=0 \\ v_cy+\omega_cx_{ICR}=0$$

侧向滑动速度与$x_{ICR}$ 有关

自转顺滑问题： 游中国比赛中关于四轮模型一直卡顿问题的解释

如何让四轮模型顺滑自转？

本质上四轮模型是可以等效为两轮差速模型（两轮差速可以很好地实现自转），上下两对轮子各自的瞬心如果无限接近，那么四轮模型就逼近两轮模型，那么车体侧向滑动就更小，本身自转也就更加顺滑。

阿克曼模型

梯形四连杆：如上图a）所示，四连杆构成的等腰梯形是等腰三角型ABE的一部分

转向角设计： 过弯速度要求越高，则外轮转向角要越大（注：圆周运动）

前向运动学： 根据上图建模可得

$$\begin{bmatrix} v_{back} \\ \omega_{back} \end{bmatrix} =\begin{bmatrix} 1/2 & 1/2\\ -1/d & 1/d \end{bmatrix} \begin{bmatrix} V_L \\ V_R \end{bmatrix}$$

逆向运动学： 正向运动学求逆即可

$$\begin{bmatrix} V_L \\ V_R \end{bmatrix} = \begin{bmatrix} 1 & -d/2\\ 1 & d/2 \end{bmatrix} \begin{bmatrix} v_{back} \\ \omega_{back} \end{bmatrix}$$

中轴平均转向角

$$\tan \delta =\frac{l}{R} \\ R = \frac{v_{back}}{\omega_{back} } \\ \delta = \tan^{-1} (\frac{l \cdot \omega_{back} }{v_{back}} )$$

轨迹推演：无法原地自转，只能弧线逼近，ROS阿克曼社区

全向模型

各个模型的约束条件

麦轮的限制

• 45°存在缝隙，运动过程车体会震动
• 双90°行走落地点成锯齿状
• 但90°目前最好的麦轮

前向运动学： 根据上图建模可得

$$\begin{bmatrix} v_x \\ v_y \\ w \end{bmatrix} =\frac{1}{4} \begin{bmatrix} 1 & 1 & 1 & 1 \\ -1 & 1 & 1 & -1\\ -\frac{1}{a+b} & \frac{1}{a+b} & -\frac{1}{a+b} & \frac{1}{a+b}\\ \end{bmatrix} \begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix}$$

逆向运动学：正向运动学求逆即可

$$\begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix} =\begin{bmatrix} 1 & -1 & -(a+b)\\ 1 & 1 & (a+b)\\ 1 & 1 & -(a+b)\\ 1 & -1 & (a+b) \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ w \end{bmatrix}$$

航迹推演： 任意方向都可运动

$$\dot{P} = \begin{bmatrix} \dot{X} \\ \dot{Y} \\ \dot{\theta } \end{bmatrix} =\begin{bmatrix} \cos \theta & - \sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega_z \end{bmatrix}$$

其他模型

• 双足
• 四足
• 多足

底盘性能指标

• 载重能力
• 动力性能

$$\Gamma = \frac{P}{w}=\frac{1000\dot{P} }{2\pi \frac{\dot{n} }{60} } \\ F = \frac{\Gamma \cdot k}{R} \\ V = 2 \pi R\frac{\dot{n} }{k}$$

动力$F$ 与减速箱的参数 $k$ 成正比、与轮子半径 $R$ 成反比，轮子线速度$v$ 与减速箱参数 $k$ 成反比、与轮子半径$R$ 成正比。

• 控制精度
• 里程计精度

机器人底盘搭建

1. 底盘运动学模型选择
2. 传感器选择
3. 主机选择
4. 底盘硬件系统搭建
5. 底盘软件系统搭建

SLAM导航关系图