参考引用
- 张虎,机器人SLAM导航核心技术与实战[M]. 机械工业出版社,2022.
- 本博客未详尽之处可自行查阅上述书籍
惯性测量单元 (Inertial Measurement Unit,IMU) 是用来测量惯性物理量的设备
航空航天、无人机、自动驾驶汽车、机器人、智能穿戴等领域广泛使用 IMU 进行运动测量和状态估计,一般从 IMU 设备采集到的原始数据都存在较大的误差,所以需要对测量到的原始数据进行标定和滤波
纯估计法
里程计辅助法
可以先利用 IMU 或轮式里程计进行初步校正,再利用 ICP 迭代修正
特点
上位机 ROS 驱动程序
单目相机无法测量物体点的深度信息,用两个单目相机组成的双目相机就可测量深度信息,也把双目相机叫深度相机
双目相机虽然能测量深度信息,但是需要事先找到同一物体点在左右相机中成像点对,也就是要先匹配。匹配过程很容易受到光照强度等环境因素干扰,在没有特征的环境中匹配会失效,深度信息将无法测量
RGB-D 相机是主动测量深度的传感器,受环境的干扰会小一些。RGB-D 相机一般有 3 个镜头:中间的镜头是普通的摄像头,采集彩色图像,另外两个镜头分别用来发射红外光和接收红外光
有时候机器人上只安装了 RGB-D 相机,而没有激光雷达。如果要运行激光 SLAM,可以将 RGB-D 相机的深度图转换成激光雷达扫描图,就可以运行激光 SLAM 了。转换用的功能包是 depthimage_to_laserscan
由于 RGB-D 相机的视野角度小,就激光 SLAM 建图效果看,没有 360 度扫描的激光雷达建的地图好。所以,通常不用 RGB-D 相机建图,而只做避障用
PID 控制
通信协议
$ ssh 主机用户名@主机ip
两轮差速模型可以说是最简单的底盘模型,在底盘的左右两边平行安装两个由电机驱动的动力轮,考虑到至少需要 3 点才能稳定支撑,底盘上还需要安装用于支撑的万向轮
s R = φ ⋅ ( ρ + d ) = V R ⋅ t s L = φ ⋅ ρ = V L ⋅ t \begin{aligned}s_\mathrm{R}&=\varphi\cdot(\rho+d)=V_\mathrm{R}\cdot t\\s_\mathrm{L}&=\varphi\cdot\rho=V_\mathrm{L}\cdot t\end{aligned} sRsL=φ⋅(ρ+d)=VR⋅t=φ⋅ρ=VL⋅t
φ = s R − s L d = V R − V L d ⋅ t s = φ ⋅ ( ρ + d 2 ) = φ ⋅ ρ + φ ⋅ d 2 = s L + s R − s L 2 = s R + s L 2 = V R + V L 2 ⋅ t \begin{aligned} &\varphi=\frac{s_{\mathrm{R}}-s_{\mathrm{L}}}{d}=\frac{V_{\mathrm{R}}-V_{\mathrm{L}}}{d}\cdot t \\ &s=\varphi\cdot\left(\begin{array}{c}\rho+\frac{d}{2}\\\end{array}\right)=\varphi\cdot\rho+\varphi\cdot\frac{d}{2}=s_{L}+\frac{s_{R}-s_{L}}{2}=\frac{s_{R}+s_{L}}{2}=\frac{V_{R}+V_{L}}{2}\cdot t \end{aligned} φ=dsR−sL=dVR−VL⋅ts=φ⋅(ρ+2d)=φ⋅ρ+φ⋅2d=sL+2sR−sL=2sR+sL=2VR+VL⋅t
[ v x ω z ] = [ 1 2 1 2 − 1 d 1 d ] [ V L V R ] \begin{bmatrix}v_x\\\omega_z\end{bmatrix}=\begin{bmatrix}\dfrac{1}{2}&\dfrac{1}{2}\\-\dfrac{1}{d}&\dfrac{1}{d}\end{bmatrix}\begin{bmatrix}V_\mathrm{L}\\V_\mathrm{R}\end{bmatrix} [vxωz]= 21−d121d1 [VLVR]
[ V L V R ] = [ 1 − d 2 1 d 2 ] [ v x ω z ] \left[\begin{matrix}V_{\mathrm{L}}\\V_{\mathrm{R}}\end{matrix}\right]=\left[\begin{matrix}1&-\dfrac{d}{2}\\1&\dfrac{d}{2}\end{matrix}\right]\left[\begin{matrix}v_{x}\\\omega_{z}\end{matrix}\right] [VLVR]= 11−2d2d [vxωz]
底盘在运动过程中,轮式编码器可以反馈每个轮子的转速,经过前向运动学公式解算就可以得到底盘的整体速度 v x v_x vx 和 ω z \omega_z ωz。基于 v x v_x vx 和 ω z \omega_z ωz,就可以由前一时刻底盘的位姿 P k − 1 \boldsymbol{P}_{k-1} Pk−1 推算出当前时刻底盘的位姿 P k \boldsymbol{P}_{k} Pk,这个推算过程也叫 “航迹推演算法”,由于底盘不是严格的质点,所以底盘的位姿代表的是底盘速度瞬心在世界坐标系的位姿
利用轮式编码器反馈,经过航迹推演算法,得到底盘当前时刻位姿,整个过程就是机器人技术中常说的轮式里程计
下面具体分析一下轮式里程计的解算过程,如下图所示
机器人学中通常采用右手坐标系,底盘的正前方为 x 轴正方向、底盘的正左方为 y 轴正方向、底盘的正上方为 z 轴正方向、航向角 θ \theta θ 以 x 轴为 0 度角并沿逆时针方向增大
一般在底盘上电时刻,在底盘的位姿处建立里程计坐标系 (odom),随时间推移底盘的实时位姿 P 0 , P 1 , P 2 , P 3 , ⋯ , P n P_0, P_1, P_2, P_3,\cdots, P_n P0,P1,P2,P3,⋯,Pn 连接起来就形成了底盘的航行轨迹。同时,底盘自身的坐标系 (robot) 建立在其速度瞬心处
建立起 robot 坐标系中的速度量 v x v_x vx 和 ω z \omega_z ωz 与 odom 坐标系中的底盘位姿 P k \boldsymbol{P}_{k} Pk 之间的关系,就能解算出轮式里程计。在二维平面中移动的底盘,可以用 P = [ X , Y , θ ] T \boldsymbol{P}=[X,Y,\theta]^\mathrm{T} P=[X,Y,θ]T 表示底盘的坐标和航向(即底盘的位姿),那么位姿 P \boldsymbol{P} P 满足下式所示微分方程
P ˙ = [ X ˙ Y ˙ θ ˙ ] = [ cos θ − s i n θ 0 sin θ cos θ 0 0 0 1 ] [ v x v y ω z ] \dot{\boldsymbol{P}}=\left[\begin{array}{c}\dot{X}\\\dot{Y}\\\dot{\theta}\end{array}\right]=\left[\begin{array}{ccc}\cos\theta&-sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{array}\right]\left[\begin{array}{c}v_x\\v_y\\\omega_z\end{array}\right] P˙= X˙Y˙θ˙ = cosθsinθ0−sinθcosθ0001 vxvyωz
将 P ˙ \dot{\boldsymbol{P}} P˙ 在时间 t 上进行积分,就可以求出底盘实时的位姿,也就是里程计信息。实际情况下,需要在离散时间域上进行计算,由于底盘相邻两位姿相隔时间 Δ t \Delta{t} Δt 很小,积分运算可以用下式的累加运算来替代
P k = P k − 1 + P ˙ k − 1 ⋅ Δ t \boldsymbol{P}_k=\boldsymbol{P}_{k-1}+\dot{\boldsymbol{P}}_{k-1}\cdot\Delta t Pk=Pk−1+P˙k−1⋅Δt
由于里程计提供底盘在 odom 坐标系中的位姿,而 odom 坐标系与底盘上电启动时所处的地方有关,也就是说里程计中的底盘位姿是一个相对值。另外,里程计随时间的推移存在较大的累积误差,所以一般都是取不同时刻之间的里程计差值并应用于其他算法。由于轮式里程计只能提供底盘的局部位姿,并且存在较大累积误差,因此需要结合地图及 SLAM 重定位技术来获取全局位姿
[ V L V R ] = [ 1 − c 2 1 c 2 ] [ v c x ω e ] \begin{bmatrix}V_\mathrm{L}\\V_\mathrm{R}\end{bmatrix}=\begin{bmatrix}1&-\dfrac{c}{2}\\1&\dfrac{c}{2}\end{bmatrix}\begin{bmatrix}v_\mathrm{cx}\\\omega_\mathrm{e}\end{bmatrix} [VLVR]= 11−2c2c [vcxωe]
[ V L V R ] = [ 1 − d 2 1 d 2 ] [ v b a c k ω b a c k ] \begin{bmatrix}V_{\mathrm{L}}\\V_{\mathrm{R}}\end{bmatrix}=\begin{bmatrix}1&-\dfrac{d}{2}\\1&\dfrac{d}{2}\end{bmatrix}\begin{bmatrix}v_{\mathrm{back}}\\\omega_{\mathrm{back}}\end{bmatrix} [VLVR]= 11−2d2d [vbackωback]
在阿克曼底盘中,除了需要求左、右后轮的速度外,还需要求出前轮的转向角。前轮的转向角可以用两个前轮中轴位置的平均转向角 δ \delta δ 表示
tan δ = l R \text{tan}\delta=\frac lR tanδ=Rl
其中
R = v b a c k ω b a c k R=\frac{v_{\mathrm{back}}}{\omega_{\mathrm{back}}} R=ωbackvback
从而求出
δ = arctan ( l ⋅ ω b a c k v b a c k ) \delta=\arctan\left(\frac{l\cdot\omega_{\mathrm{back}}}{v_{\mathrm{back}}}\right) δ=arctan(vbackl⋅ωback)
求出的转向角 δ \delta δ 不是最终控制量。还要将转向角 δ \delta δ 映射成舵机的输出舵量 Angle,才能最终实现底盘转向。而转向角 δ \delta δ 与舵量 Angle 之间的映射关系与舵机及转向悬臂具体安装位置有关系,通常都是通过实验标定法获取到转向角 δ \delta δ 与舵量 Angle 之间的映射表 δ \delta δ-Angle
[ V 1 V 2 V 3 V 4 ] = [ 1 − 1 − ( a + b ) 1 1 ( a + b ) 1 1 − ( a + b ) 1 − 1 ( a + b ) ] [ v x v y ω ] \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\\\omega\end{bmatrix} V1V2V3V4 = 1111−111−1−(a+b)(a+b)−(a+b)(a+b) vxvyω
将机器人放置到未知环境,前面这种上帝视角般的先验信息将不复存在,机器人将陷入一种进退两难的局面,即所谓的 “先有鸡还是先有蛋” 的问题。如果没有全局地图信息机器人位姿将无法求解,没有机器人位姿,地图又将如何求解呢?
- CMU-自主探索导航系统(TARE & FAR Planner)学习-All in one
# 安装 openslam-gmapping 和 slam-gmapping 功能包及其依赖
$ sudo apt install ros-melodic-openslam-gmapping ros-melodic-gmapping
# 卸载 openslam-gmapping 和 slam-gmapping 功能包,但保留其依赖
$ sudo apt remove ros-melodic-openslam-gmapping ros-melodic-gmapping
# 创建工作空间
$ mkdir -p gmapping_ws/src
$ cd gmapping_ws/src
# 源码下载
$ git clone https://github.com/ros-perception/slam_gmapping.git
$ cd slam_gmapping
$ git clone https://github.com/ros-perception/openslam_gmapping.git
$ cd ~/gmapping_ws/
# 编译安装
$ catkin_make -DCATKIN_WHITELIST_PACKAGES="openslam_gmapping"
$ catkin_make -DCATKIN_WHITELIST_PACKAGES="gmapping"
$ roslaunch gmapping slam_gmapping_pr2.launch
# bag 下载地址 http://download.ros.org/data/amcl/?C=D;O=A
$ rosbag play basic_localization_stage_indexed.bag
# 在 rviz 中订阅地图话题 /map 即可看到地图
$ rviz
# 录制数据集
$ rosbag record -O test.bag /scan /tf /tf_static
# 启动建图(记得打开 use_sim_time 参数)
$ roslaunch gmapping slam_gmapping.launch
# 播放数据集
$ rosbag play test.bag
cartographer 核心库实现建图算法的具体过程,也就是局部建图、闭环检测和全局建图的实现,下图为谷歌官方给出的流程框图。激光雷达数据、轮式里程计数据、IMU 数据和外部位姿辅助数据为整个算法的输入:其中激光雷达为必需数据,而轮式里程计、IMU 和外部位姿辅助(即 GPS 和已知信标)为可选数据
LOAM 的核心思想:将 SLAM 问题拆分成独立的定位和建图分别来处理
$ sudo apt install cmake
$ sudo apt install libgoogle-glog-dev libgflags-dev
$ sudo apt install libatlas-base-dev
$ sudo apt-get install libeigen3-dev
$ sudo apt-get install libsuitesparse-dev
$ git clone https://github.com/ceres-solver/ceres-solver.git
$ cd ceres-solver
$ mkdir build
$ cd build
$ cmake ..
$ make
$ make test
$ sudo make install
$ mkdir -p a_loam_ws/src
$ cd ~/a_loam_ws/src
$ git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git
$ cd ../
$ catkin_make
$ cd ~/a_loam_ws
$ source devel/setup.bash
$ roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch
$ cd bag
$ rosbag play nsh_indoor_outdoor.bag
# nsh_indoor_outdoor.bag 下载地址
# 链接: https://pan.baidu.com/s/1zpS7rfLzVmyErVDlLdNP4g?pwd=tvfb 提取码: tvfb
实时定位可以分为被动定位和主动定位两种
- 被动定位依赖外部人工信标:如 GPS、UWB、RFID、二维码等
- 主动定位不依赖外部人工信标:以 SLAM 为代表
Dijkstra 算法
假如 P(A, F) 是 A 到 F 的最短路径,D 是该最短路径上的某个点,那么 P(A, D) 必定也是 A 到 D 的最短路径
P ( A , F ) ⏟ arg min = P ( A , D ) ⏟ arg min + P ( D , F ) \underbrace{P(A,F)}_{\arg\min}=\underbrace{P(A,D)}_{\arg\min}+P(D,F) argmin P(A,F)=argmin P(A,D)+P(D,F)
Dijkstra 算法是按照广度优先(先在当前搜索节点上横向生长出子节点,然后从所有叶子节点中选择最优节点继续同样的生长方式)进行搜索,就是先遍历父节点周围的子节点,然后选择一个符合条件的子节点继续
Dijkstra 算法包含两个嵌套的循环,时间复杂度为 O ( n 2 ) O(n^2) O(n2)
由上图搜索过程来看,从扩展出的叶子节点(浅灰色节点)中挑选出下一个搜索节点时,仅考虑了源节点到这些叶子节点的信息
A* 算法
以 PRM、RRT 等为代表的基于采样的路径搜索算法是将机器人所处的连续空间用随机采样离散化,然后在离散采样点上进行路径搜索
PRM 算法
第 1 步:随机采样。即在给定的地图上抛撒一定数量的随机点,利用这些随机点对地图中的连续空间进行离散化
第 2 步:移除无效采样点,也就是将落在障碍物上的采样点删除
第 3 步:连接,也就是按照最近邻规则将采样点与周围相邻点进行连接
第 4 步:移除无效连接,也就是将横穿障碍物的连接删除,这样就构建出了所谓的 PRM 路线图
第 5 步:添加导航任务的源节点和目标节点,也就是将源节点和目标节点与 PRM 路线图相连
PRM 在构建出路线图后仍然采用 A* 搜索路径,PRM 在路径规划问题上是不完备的,当采样点较少时可能规划不出路径。由于采样点不能覆盖所有情况,所以 PRM 规划出的路径也不是最优的
RRT 算法
虽然 PRM 利用采样方式对连续空间进行离散化可以大大缩小搜索范围,但最后路径依然是在图结构上进行搜索
1、全局路径规划和局部路径规划统称为路径规划
2、局部路径规划和轨迹规划统称为运动规划
3、轨迹规划和轨迹跟踪统称为运动控制,轨迹规划为轨迹跟踪器提供参考轨迹,而轨迹跟踪器生成动作量实现执行器的最终操控
机器人在世界坐标系的状态量为 q = [ x , y , θ ] q=[x,y,\theta] q=[x,y,θ],其中 x 和 y 为机器人的位置坐标, θ \theta θ 为机器人的航向角
通常选择参考轨迹中离机器人最近的点 q r e f = [ x r e f , y r e f , θ r e f ] q_{ref} =[x_{ref},y_{ref},\theta_{ref}] qref=[xref,yref,θref] 作为追踪目标,以 q q q 和 q r e f q_{ref} qref 与之间的误差为反馈进行运动控制。最简单的方式就是:用距离误差调节两轮差速机器人的线速度,以航向角误差调节角速度
这里采用离散位置型 PID 控制算法,离散其实是指 PID 算法中的积分运算与微分运算分别用累加运算与差分运算替代,位置型是指 PID 算法的计算结果直接为控制量。利用两轮差速机器人运动学模型将 PID 算法得到的线速度量和角速度量转换成每个电机的速度控制量来实现最终控制
其中 D e ( t ) = ( x − x r e f ) 2 + ( y − y r e f ) 2 D_e(t)=\sqrt{(x-x_{\mathrm{ref}})^2+(y-y_{\mathrm{ref}})^2} De(t)=(x−xref)2+(y−yref)2
ω ( t ) = k p ′ ⋅ θ e ( t ) + k i ′ ⋅ ∑ t θ e ( t ) + k d ′ ⋅ ( θ e ( t ) − θ e ( t − 1 ) ) + ω r e f \omega(t)=k_{p}^{\prime}\cdot\theta_{e}(t)+k_{i}^{\prime}\cdot\sum_{t}\theta_{e}(t)+k_{d}^{\prime}\cdot(\theta_{e}(t)-\theta_{e}(t-1))+\omega_{\mathrm{ref}} ω(t)=kp′⋅θe(t)+ki′⋅t∑θe(t)+kd′⋅(θe(t)−θe(t−1))+ωref
其中 θ e ( t ) = θ − θ r e f \theta_e(t)=\theta-\theta_\mathrm{ref} θe(t)=θ−θref
u ( k ) = arg min u ( k ) J \boldsymbol{u}(k)=\arg\min_{\boldsymbol{u}(k)}J u(k)=argu(k)minJ
ROS学习7:ROS机器人导航仿真
AMCL
Costmap
ros-navigation 系统框架