本文主要是介绍激光SLAM理论与实践 - 第六期 第1章 激光SLAM简介 作业与心得，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
-
了解 Linux 系统：阅读《鸟哥的 Linux 私房菜》自学前三部分内容，或利用互联网进行学习，简答以下问题；（3 分）
-
列举三个你常用的 Linux 命令，并说明他们的功能。
man指令，是manual(操作说明)的缩写，该指令用于查看其他指令的详细说明；比如$ man date，就会出现有关date的详细说明。cd指令，是change directory（改变目录）的缩写，该指令用于改变工作目录；比如$ cd /home/tx-ubuntu64，就会将当前目录变为/home/tx-ubuntu64。mkdir指令，是make directory（创建目录）的缩写，该指令用于创建新目录；比如$ mkdir abc,就会创建文件夹abc。
-
一句话简要介绍 Vim 的功能，如何在 Vim 中进行插入和删除，如何保存并退出 Vim？
- Vim 是 vi 进阶版本的文本编辑器，Vim 可以用颜色或底线等方式来显示一些特殊的信息。
- 在一般模式中，按下
i（i, I, o, O, a, A, r, R）进入编辑模式，开始编辑（包括插入和删除）文字，按下Esc即可退出编辑模式。 - 在一般模式中，按下
:wq保存并退出Vim。
-
列举两种常用的 Linux 压缩和解压缩命令。
tar指令，解压tar zxvf FileName.tar.gz，压缩tar zcvf FileName.tar.gz DirName。unzip指令与zip指令，解压unzip FileName.zip，压缩zip -q -r FileName.zip FileName。
-
-
了解 ROS：观看 ROS 免费公开课或前往 ROS 官网学习官方教程，安装好 ROS，提供运行小海龟跑的截图；（3 分）
这里需要注意ROS更新过key，最好使用英文版安装教程。
-
学习机器人姿态描述入门材料，完成坐标转换推导；（3 分）
设机器人的世界坐标为 xa, ya，其相对于世界坐标系的方向为 θa（右手坐标系）。假设机器人旁边有一物体在世界坐标系下的位姿为（xb, yb, θb）,请问：-
该物体相对于机器人的位置和朝向是什么，即该物体在当前机器人坐标系下的位姿是多少？
解：设世界坐标系为 { W } \{W\} {W} ，机器人坐标系为 { A } \{A\} {A} ，物体坐标系为 { B } \{B\} {B} ，则
B A T = W A T ⋅ B W T = A W T − 1 ⋅ B W T = [ A W R T − A W R T ⋅ A W P 0 1 ] ⋅ [ B W R B W P 0 1 ] = [ A W R T ⋅ B W R A W R T ⋅ B W P − A W R T ⋅ A W P 0 1 ] = [ A W R T ⋅ B W R A W R T ⋅ ( B W P − A W P ) 0 1 ] \begin{aligned} ^A_BT&=^A_WT\cdot^W_BT \\ &=^W_AT^{-1}\cdot^W_BT \\ &=\begin{bmatrix} ^W_AR^T & -^W_AR^T\cdot^W_AP \\ 0 & 1 \\ \end{bmatrix}\cdot\begin{bmatrix} ^W_BR & ^W_BP \\ 0 & 1 \\ \end{bmatrix} \\ &=\begin{bmatrix} ^W_AR^T\cdot^W_BR & ^W_AR^T\cdot^W_BP-^W_AR^T\cdot^W_AP \\ 0 & 1 \\ \end{bmatrix} \\ &=\begin{bmatrix} ^W_AR^T\cdot^W_BR & ^W_AR^T\cdot(^W_BP-^W_AP) \\ 0 & 1 \\ \end{bmatrix} \end{aligned} BAT=WAT⋅BWT=AWT−1⋅BWT=[AWRT0−AWRT⋅AWP1]⋅[BWR0BWP1]=[AWRT⋅BWR0AWRT⋅BWP−AWRT⋅AWP1]=[AWRT⋅BWR0AWRT⋅(BWP−AWP)1]
式中：
A W R T ⋅ B W R = [ c o s θ a − s i n θ a s i n θ a c o s θ a ] T ⋅ [ c o s θ b − s i n θ b s i n θ b c o s θ b ] = [ c o s θ a s i n θ a − s i n θ a c o s θ a ] ⋅ [ c o s θ b − s i n θ b s i n θ b c o s θ b ] = [ c o s θ a c o s θ b + s i n θ a s i n θ b − c o s θ a s i n θ b + s i n θ a c o s θ b − s i n θ a c o s θ b + c o s θ a s i n θ b s i n θ a s i n θ b + c o s θ a c o s θ b ] = [ c o s ( θ b − θ a ) − s i n ( θ b − θ a ) s i n ( θ b − θ a ) c o s ( θ b − θ a ) ] \begin{aligned} ^W_AR^T\cdot^W_BR&=\begin{bmatrix} cos\theta_a & -sin\theta_a \\ sin\theta_a & cos\theta_a \\ \end{bmatrix}^T\cdot\begin{bmatrix} cos\theta_b & -sin\theta_b \\ sin\theta_b & cos\theta_b \\ \end{bmatrix} \\ &=\begin{bmatrix} cos\theta_a & sin\theta_a \\ -sin\theta_a & cos\theta_a \\ \end{bmatrix}\cdot\begin{bmatrix} cos\theta_b & -sin\theta_b \\ sin\theta_b & cos\theta_b \\ \end{bmatrix} \\ &=\begin{bmatrix} cos\theta_acos\theta_b+sin\theta_asin\theta_b & -cos\theta_asin\theta_b+sin\theta_acos\theta_b \\ -sin\theta_acos\theta_b+cos\theta_asin\theta_b & sin\theta_asin\theta_b+cos\theta_acos\theta_b \\ \end{bmatrix} \\ &=\begin{bmatrix} cos(\theta_b-\theta_a) & -sin(\theta_b-\theta_a) \\ sin(\theta_b-\theta_a) & cos(\theta_b-\theta_a) \\ \end{bmatrix} \end{aligned} AWRT⋅BWR=[cosθasinθa−sinθacosθa]T⋅[cosθbsinθb−sinθbcosθb]=[cosθa−sinθasinθacosθa]⋅[cosθbsinθb−sinθbcosθb]=[cosθacosθb+sinθasinθb−sinθacosθb+cosθasinθb−cosθasinθb+sinθacosθbsinθasinθb+cosθacosθb]=[cos(θb−θa)sin(θb−θa)−sin(θb−θa)cos(θb−θa)]
A W R T ⋅ ( B W P − A W P ) = [ c o s θ a − s i n θ a s i n θ a c o s θ a ] T ⋅ ( [ x b y b ] − [ x a y a ] ) = [ c o s θ a s i n θ a − s i n θ a c o s θ a ] ⋅ [ x b − x a y b − y a ] = [ ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a ] \begin{aligned} ^W_AR^T\cdot(^W_BP-^W_AP)&=\begin{bmatrix} cos\theta_a & -sin\theta_a \\ sin\theta_a & cos\theta_a \\ \end{bmatrix}^T\cdot(\begin{bmatrix} x_b \\ y_b \\ \end{bmatrix}-\begin{bmatrix} x_a \\ y_a \\ \end{bmatrix}) \\ &=\begin{bmatrix} cos\theta_a & sin\theta_a \\ -sin\theta_a & cos\theta_a \\ \end{bmatrix}\cdot\begin{bmatrix} x_b-x_a \\ y_b-y_a \\ \end{bmatrix} \\ &=\begin{bmatrix} (x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a \\ -(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a \\ \end{bmatrix} \end{aligned} AWRT⋅(BWP−AWP)=[cosθasinθa−sinθacosθa]T⋅([xbyb]−[xaya])=[cosθa−sinθasinθacosθa]⋅[xb−xayb−ya]=[(xb−xa)cosθa+(yb−ya)sinθa−(xb−xa)sinθa+(yb−ya)cosθa]
代入：
B A T = [ c o s ( θ b − θ a ) − s i n ( θ b − θ a ) ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a s i n ( θ b − θ a ) c o s ( θ b − θ a ) − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a 0 0 1 ] \begin{aligned} ^A_BT&=\begin{bmatrix} cos(\theta_b-\theta_a) & -sin(\theta_b-\theta_a) & (x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a\\ sin(\theta_b-\theta_a) & cos(\theta_b-\theta_a) & -(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a\\ 0 & 0 & 1\\ \end{bmatrix} \end{aligned} BAT=⎣⎡cos(θb−θa)sin(θb−θa)0−sin(θb−θa)cos(θb−θa)0(xb−xa)cosθa+(yb−ya)sinθa−(xb−xa)sinθa+(yb−ya)cosθa1⎦⎤
位姿为： ( ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a , − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a , θ b − θ a ) ((x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a,-(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a,\theta_b-\theta_a) ((xb−xa)cosθa+(yb−ya)sinθa,−(xb−xa)sinθa+(yb−ya)cosθa,θb−θa)。 -
机器人此时朝它的正前方（机器人坐标系 X 轴）行进了 d 距离，然后又转了 θd角，请问物体此时在这一时刻机器人坐标系下的位姿是多少？
解：移动后的机器人坐标系为 { A ′ } \{A'\} {A′} ，则
B A ′ T = A A ′ T ⋅ B A T = A ′ A T − 1 ⋅ B A T = [ A ′ A R T − A ′ A R T ⋅ A ′ A P 0 1 ] ⋅ [ B A R B A P 0 1 ] = [ A ′ A R T ⋅ B A R A ′ A R T ⋅ B A P − A ′ A R T ⋅ A ′ A P 0 1 ] = [ A ′ A R T ⋅ B A R A ′ A R T ⋅ ( B A P − A ′ A P ) 0 1 ] \begin{aligned} ^{A'}_BT&=^{A'}_AT\cdot^A_BT \\ &=^A_{A'}T^{-1}\cdot^A_BT \\ &=\begin{bmatrix} ^A_{A'}R^T & -^A_{A'}R^T\cdot^A_{A'}P \\ 0 & 1 \\ \end{bmatrix}\cdot\begin{bmatrix} ^A_BR & ^A_BP \\ 0 & 1 \\ \end{bmatrix} \\ &=\begin{bmatrix} ^A_{A'}R^T\cdot^A_BR & ^A_{A'}R^T\cdot^A_BP-^A_{A'}R^T\cdot^A_{A'}P \\ 0 & 1 \\ \end{bmatrix} \\ &=\begin{bmatrix} ^A_{A'}R^T\cdot^A_BR & ^A_{A'}R^T\cdot(^A_BP-^A_{A'}P) \\ 0 & 1 \\ \end{bmatrix} \end{aligned} BA′T=AA′T⋅BAT=A′AT−1⋅BAT=[A′ART0−A′ART⋅A′AP1]⋅[BAR0BAP1]=[A′ART⋅BAR0A′ART⋅BAP−A′ART⋅A′AP1]=[A′ART⋅BAR0A′ART⋅(BAP−A′AP)1]
式中：
A ′ A R T ⋅ B A R = [ c o s θ d − s i n θ d s i n θ d c o s θ d ] T ⋅ [ c o s ( θ b − θ a ) − s i n ( θ b − θ a ) s i n ( θ b − θ a ) c o s ( θ b − θ a ) ] = [ c o s θ d s i n θ d − s i n θ d c o s θ d ] ⋅ [ c o s ( θ b − θ a ) − s i n ( θ b − θ a ) s i n ( θ b − θ a ) c o s ( θ b − θ a ) ] = [ c o s θ d c o s ( θ b − θ a ) + s i n θ d s i n ( θ b − θ a ) − c o s θ d s i n ( θ b − θ a ) + s i n θ d c o s ( θ b − θ a ) − s i n θ d c o s ( θ b − θ a ) + c o s θ d s i n ( θ b − θ a ) s i n θ d s i n ( θ b − θ a ) + c o s θ d c o s ( θ b − θ a ) ] = [ c o s ( ( θ b − θ a ) − θ d ) − s i n ( ( θ b − θ a ) − θ d ) s i n ( ( θ b − θ a ) − θ d ) c o s ( ( θ b − θ a ) − θ d ) ] = [ c o s ( θ b − θ a − θ d ) − s i n ( θ b − θ a − θ d ) s i n ( θ b − θ a − θ d ) c o s ( θ b − θ a − θ d ) ] \begin{aligned} ^A_{A'}R^T\cdot^A_BR&=\begin{bmatrix} cos\theta_d & -sin\theta_d \\ sin\theta_d & cos\theta_d \\ \end{bmatrix}^T\cdot\begin{bmatrix} cos(\theta_b-\theta_a) & -sin(\theta_b-\theta_a) \\ sin(\theta_b-\theta_a) & cos(\theta_b-\theta_a) \\ \end{bmatrix} \\ &=\begin{bmatrix} cos\theta_d & sin\theta_d \\ -sin\theta_d & cos\theta_d \\ \end{bmatrix}\cdot\begin{bmatrix} cos(\theta_b-\theta_a) & -sin(\theta_b-\theta_a) \\ sin(\theta_b-\theta_a) & cos(\theta_b-\theta_a) \\ \end{bmatrix} \\ &=\begin{bmatrix} cos\theta_dcos(\theta_b-\theta_a)+sin\theta_dsin(\theta_b-\theta_a) & -cos\theta_dsin(\theta_b-\theta_a)+sin\theta_dcos(\theta_b-\theta_a) \\ -sin\theta_dcos(\theta_b-\theta_a)+cos\theta_dsin(\theta_b-\theta_a) & sin\theta_dsin(\theta_b-\theta_a)+cos\theta_dcos(\theta_b-\theta_a) \\ \end{bmatrix} \\ &=\begin{bmatrix} cos((\theta_b-\theta_a)-\theta_d) & -sin((\theta_b-\theta_a)-\theta_d) \\ sin((\theta_b-\theta_a)-\theta_d) & cos((\theta_b-\theta_a)-\theta_d) \\ \end{bmatrix} \\ &=\begin{bmatrix} cos(\theta_b-\theta_a-\theta_d) & -sin(\theta_b-\theta_a-\theta_d) \\ sin(\theta_b-\theta_a-\theta_d) & cos(\theta_b-\theta_a-\theta_d) \\ \end{bmatrix} \\ \end{aligned} A′ART⋅BAR=[cosθdsinθd−sinθdcosθd]T⋅[cos(θb−θa)sin(θb−θa)−sin(θb−θa)cos(θb−θa)]=[cosθd−sinθdsinθdcosθd]⋅[cos(θb−θa)sin(θb−θa)−sin(θb−θa)cos(θb−θa)]=[cosθdcos(θb−θa)+sinθdsin(θb−θa)−sinθdcos(θb−θa)+cosθdsin(θb−θa)−cosθdsin(θb−θa)+sinθdcos(θb−θa)sinθdsin(θb−θa)+cosθdcos(θb−θa)]=[cos((θb−θa)−θd)sin((θb−θa)−θd)−sin((θb−θa)−θd)cos((θb−θa)−θd)]=[cos(θb−θa−θd)sin(θb−θa−θd)−sin(θb−θa−θd)cos(θb−θa−θd)]
A ′ A R T ⋅ ( B A P − A ′ A P ) = [ c o s θ d − s i n θ d s i n θ d c o s θ d ] T ⋅ ( [ ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a ] − [ d 0 ] ) = [ c o s θ d s i n θ d − s i n θ d c o s θ d ] ⋅ [ ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a − d − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a ] = [ [ ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a − d ] c o s θ d + [ − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a ] s i n θ d − [ ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a − d ] s i n θ d + [ − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a ] c o s θ d ] \begin{aligned} ^A_{A'}R^T\cdot(^A_BP-^A_{A'}P)&=\begin{bmatrix} cos\theta_d & -sin\theta_d \\ sin\theta_d & cos\theta_d \\ \end{bmatrix}^T\cdot(\begin{bmatrix} (x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a \\ -(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a \\ \end{bmatrix}-\begin{bmatrix} d \\ 0 \\ \end{bmatrix}) \\ &=\begin{bmatrix} cos\theta_d & sin\theta_d \\ -sin\theta_d & cos\theta_d \\ \end{bmatrix}\cdot\begin{bmatrix} (x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a-d \\ -(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a \\ \end{bmatrix} \\ &=\begin{bmatrix} [(x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a-d]cos\theta_d+[-(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a]sin\theta_d \\ -[(x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a-d]sin\theta_d+[-(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a]cos\theta_d \\ \end{bmatrix} \end{aligned} A′ART⋅(BAP−A′AP)=[cosθdsinθd−sinθdcosθd]T⋅([(xb−xa)cosθa+(yb−ya)sinθa−(xb−xa)sinθa+(yb−ya)cosθa]−[d0])=[cosθd−sinθdsinθdcosθd]⋅[(xb−xa)cosθa+(yb−ya)sinθa−d−(xb−xa)sinθa+(yb−ya)cosθa]=[[(xb−xa)cosθa+(yb−ya)sinθa−d]cosθd+[−(xb−xa)sinθa+(yb−ya)cosθa]sinθd−[(xb−xa)cosθa+(yb−ya)sinθa−d]sinθd+[−(xb−xa)sinθa+(yb−ya)cosθa]cosθd]
代入：
B A ′ T = [ c o s ( θ b − θ a − θ d ) − s i n ( θ b − θ a − θ d ) [ ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a − d ] c o s θ d + [ − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a ] s i n θ d s i n ( θ b − θ a − θ d ) c o s ( θ b − θ a − θ d ) − [ ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a − d ] s i n θ d + [ − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a ] c o s θ d 0 0 1 ] \begin{aligned} ^{A'}_BT&=\begin{bmatrix} cos(\theta_b-\theta_a-\theta_d) & -sin(\theta_b-\theta_a-\theta_d) & [(x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a-d]cos\theta_d+[-(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a]sin\theta_d\\ sin(\theta_b-\theta_a-\theta_d) & cos(\theta_b-\theta_a-\theta_d) & -[(x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a-d]sin\theta_d+[-(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a]cos\theta_d\\ 0 & 0 & 1\\ \end{bmatrix} \end{aligned} BA′T=⎣⎡cos(θb−θa−θd)sin(θb−θa−θd)0−sin(θb−θa−θd)cos(θb−θa−θd)0[(xb−xa)cosθa+(yb−ya)sinθa−d]cosθd+[−(xb−xa)sinθa+(yb−ya)cosθa]sinθd−[(xb−xa)cosθa+(yb−ya)sinθa−d]sinθd+[−(xb−xa)sinθa+(yb−ya)cosθa]cosθd1⎦⎤
位姿为： ( [ ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a − d ] c o s θ d + [ − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a ] s i n θ d , − [ ( x b − x a ) c o s θ a + ( y b − y a ) s i n θ a − d ] s i n θ d + [ − ( x b − x a ) s i n θ a + ( y b − y a ) c o s θ a ] c o s θ d , θ b − θ a − θ d ) ([(x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a-d]cos\theta_d+[-(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a]sin\theta_d,-[(x_b-x_a)cos\theta_a+(y_b-y_a)sin\theta_a-d]sin\theta_d+[-(x_b-x_a)sin\theta_a+(y_b-y_a)cos\theta_a]cos\theta_d,\theta_b-\theta_a-\theta_d) ([(xb−xa)cosθa+(yb−ya)sinθa−d]cosθd+[−(xb−xa)sinθa+(yb−ya)cosθa]sinθd,−[(xb−xa)cosθa+(yb−ya)sinθa−d]sinθd+[−(xb−xa)sinθa+(yb−ya)cosθa]cosθd,θb−θa−θd)。
这题需要知道转换矩阵可以是坐标系的描述，也可以表示主动变换和被动变换；这里可以发现，在二维平面中，角度直接加减就可，在上面的坐标系中也可以比较明显得看出来。
-
-
完成基础数学坐标转换的代码作业。（3 分）
这题的原理比较简单，就是已知 A O T ^O_AT AOT 、 B O T ^O_BT BOT ，求 A B T ^B_AT ABT ， A B T = O B T ⋅ A O T = B O T − 1 ⋅ A O T ^B_AT=^B_OT\cdot^O_AT=^O_BT^{-1}\cdot^O_AT ABT=OBT⋅AOT=BOT−1⋅AOT 。
// TODO 参照第一课PPT // start your code here (5~10 lines) Eigen::Matrix3d TOA; TOA << cos(A(2)), -sin(A(2)), A(0),sin(A(2)), cos(A(2)), A(1),0, 0, 1; Eigen::Matrix3d TBA = TBO * TOA; cout << TBA << std::endl; BA << TBA(0, 2),TBA(1, 2),atan2(TBA(1, 0), TBA(0, 0)); // end your code here运行结果为：
$ ./basicTransformStudy TBA: -1.83697e-16 -1 21 -1.83697e-16 10 0 1 The right answer is BA: 2 1 1.5708 Your answer is BA: 2 1 1.5708详见代码仓库。
这篇关于激光SLAM理论与实践 - 第六期 第1章 激光SLAM简介 作业与心得的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
