1.5　习题

习题1.1　伴随矩阵的性质

考虑向量w=（w_x，w_y，w_z）及其伴随矩阵Ad（w）。

1）证明：Ad（w）的特征值为{0，||w||i，-||w||i}，并给出一个与0相对应的特征向量，并讨论。

2）证明：Ad（w）x=w∧x为一个垂直于w和x的向量，则该三面体（w，x，w∧x）为正三面体。

3）证明：w∧x的范数是由w和x形成的平行四边形A的表面积。

习题1.2　雅可比恒等式

可将雅可比恒等式写为：

1）证明这个恒等式等价于：

2）在斜对称矩阵空间中，将李氏括号定义如下：

证明：

3）在习题1.4中，将证明在斜对称矩阵A之后的一个无穷小旋转为exp（A·dt）I+A·dt+。考虑两个斜对称矩阵A，B，并定义两个无穷小的旋转矩阵为：

计算与旋转相关的二阶泰勒展开式：

根据该结果，解释说明李氏括号[A，B]=AB-BA。

4）证明集合（R³，+，∧，·）构成一个李代数。

习题1.3　范力农公式

考虑如下情况，一个重心保持在伽利略坐标系原点的刚体，正绕轴以一个旋转向量w旋转，请给出刚体上一点x的轨迹方程。

习题1.4　罗德里格斯公式

考虑如下情况，一个重心保持在伽利略坐标系原点的刚体，正绕轴旋转，刚体上一点x的位置满足该状态方程（范力农公式）：

式中，w与该旋转轴Δ平行，||w||为刚体的回转速度（单位rad.s^-1）。

1）证明可将该状态方程写为如下形式：

并解释为什么通常将矩阵A记作w∧。

2）请给出该状态方程解的表达式。

3）由此可以推断出：绕w且旋转角为||w||的旋转矩阵R可由下式表示，并称之为罗德里格斯公式。

4）计算出A的特征值，证明w是与0特征值相对应的特征向量，并讨论。

5）R的特征值是多少？

6）利用之前的问题，请给出绕向量w=（1，0，0）旋转α角的旋转表达式。

7）用MATLAB编写一段程序，eulermat（phi，theta，psi）利用罗德里格斯公式返回欧拉矩阵。

习题1.5　罗德里格斯公式的几何逼近

考虑绕单位向量n旋转角度φ的一个旋转R_n，φ。令u为一个服从该旋转的向量，则可将其分解为如下形式：

式中，u||与n共线，u_⊥在平面P_⊥上并正交于n（见图1.9）。

1）证明罗德里格斯公式可表示为：

图1.9　向量u绕向量n的旋转
a）透视图；b）俯视图

2）利用二重向量积公式a∧（b∧c）=（a^Tc）·b-（a^Tb）·c，在元素n∧（n∧u）上证明可将罗德里格斯公式写为：

由此推断，与该线性算子R_n，φ相关的矩阵可写为：

3）相反地，给定一个旋转矩阵R_n，φ，得到旋转轴n和旋转角φ。给出的表达式，并用它来得到n和φ关于R_n，φ的方程，图1.10给出了其几何图示。

图1.10　a）为绕n转动φ角的旋转示意图；b）为与罗德里格斯菱形相对应部分的示意图

4）推导一种可找出两个旋转矩阵R_a，R_b之间的插值轨迹矩阵的方法，使得R（0）=R_a且R（1）=R_b。

5）利用由sinφ和cosφ演变而来的麦克劳林级数，证明：

有时可将其写为：

习题1.6　四元数

用W.R.Hamilton所提出的四元数来表示三维空间中的旋转（见[COR 11]）。四元数为复数的拓展，对应于标量s加上R³中的向量v。本书采用下述等效符号：

其中：i²=j²=k²=ijk=-1

1）根据这些关系，填写以下乘法表：

2）单位四元数为单位值的四元数：

考虑一种绕单位向量v旋转θ的旋转情况，由罗德里格斯公式可得，相应的旋转表示为exp（θ·v∧），对于此旋转，将四元数与之关联为：

检查此四元数是否具有单位值。

3）证明四元数及其相反数均对应于R³中的相同旋转。

4）假设旋转的合成对应于四元数的乘法，则证明：

5）考虑的欧拉旋转矩阵，对应于以α角绕单位向量v旋转。请通过三种方法给出v和α的值：几何方法（手绘）、矩阵和四元数。

习题1.7　舒勒振荡

惯性装置的基本组成之一就是测斜仪，该传感器可以测出垂直方向。传统方法是用钟摆（或铅垂线）来测量。然而在移动时，由于加速度的影响，钟摆开始振荡，因此不能再用其测量垂直方向。在此，思考设计一个钟摆，使得任何水平加速度都不会导致其振荡。考虑一种两端质量为2m的钟摆，两端与光杆的旋转轴之间的距离分别为₁和₂（见图1.11），该旋转轴在地球表面上移动。假设₁和₂远小于地球半径r。

图1.11　摆式测斜仪在地球表面移动

1）请给出该系统的状态方程。

2）假设α==0，对于该钟摆的任意水平运动，₁和₂取何值时，该钟摆保持垂直？如果令₁=1m，地球半径r=6400km，则₁的应为何值？

3）假设该钟摆由于扰动而开始振荡，则将这类振荡的周期称为舒勒周期，请计算该周期。

4）将一类高斯白噪声作为该系统的加速度输入，对其进行精确仿真。由于该系统是保守系统，因此使用欧拉积分法的效果并不好（该钟摆将获得能量），故而应该采用诸如龙格库塔法的一类高阶积分法，如下式所示：

令r=10m、₁=1m、g=10ms^-2，以便得到更简洁的表达式，并讨论该结果。

习题1.8　制动检测器

在此，研究一类包含基变换和旋转矩阵的问题。一辆车m在另一辆车之前（可将其视为一个点）。在该车上固连一个如图1.12所示的坐标系R₁：（o₁，i₁，j₁），假设R₀：（o₀，i₀，j₀）为固定的地面坐标系。

图1.12　车辆探测点m是否制动

该车装备有以下传感器：

1）几个位于后轮的里程计，以便测量后桥中心的速度；

2）一个陀螺仪，用以提供车辆的角速度和角加速度；

3）一个位于o₁处的加速度计，用以测量表示在坐标系R₁内的o₁的加速度向量（α，β）；

4）前方装有两个雷达，使车辆能够测量（间接地）点m在坐标系R₁内的坐标（a₁，b₁）和其前两阶导数和。

然而，该车并没有配备能够获取经x,y,,信息的定位系统（如全球定位系统，GPS），也没有可以测量角度θ的指南针，相关系统变量如下：

已测变量：v，，a₁，b₁，α，β。

未知变量：。

假设当测得一个变量时，也会测得其微分，但是反之不可以。例如当测得a₁，b₁后，可认为也被同时测得了，但是当测得时，并不代表也测得了θ的值。在此并不知道本车的状态方程，该问题旨在寻求与所测变量（及其导数）相关的一种条件，据此可获得前车是否制动的信息。那么即使是前车的后刹车灯看不见（雾天，无刹车灯的拖车）或故障时，也可根据这样的一个条件为后车建立一个警告信息，提示前车正在刹车。

1）通过在坐标系R₀内表示沙勒定理（o₀m=o₀o₁+o₁m），给出其基变换公式：

式中，R_θ为旋转矩阵。

2）证明为一个反对称矩阵，并给出其表达式。

3）令u为固定观测器所观测到的点m的速度向量，求出该速度向量u在坐标系R₁内的表达式u|R₁，将其表示为关于已测变量的方程。

4）在此，使用车辆上的加速度计，获得坐标系R₁内o₁的加速度向量（α，β）。在坐标系R₀内，通过求m|R₀的二阶微分，得到m的加速度a|R₀的表达式。据此，推导出其在坐标系R₁内的表达式a|R₁，并将其仅表示为关于已测变量的方程。

5）给出一个关于测量值的条件，使后车能以此探测前车是否刹车。

习题1.9　水下机器人建模

在此所要建模的机器人是水下无人扫雷潜航器Redermor（用布里多尼语说就是水中灰狗），如图1.13所示。它是一个完全自主的水下机器人，该机器人由GESMA（大西洋水下研究小组）研发，长6m、直径1m、重3800kg，并配备非常高效的推进和控制系统，用以探测海床上的水雷。

图1.13　由GESMA（大西洋水下研究小组）研发的水下无人扫雷潜航器Redermor在其发射船附近的水面上

在该机器人上方建立一个随其移动的局部坐标系R₀：（o₀，i₀，j₀，k₀），其原点o₀位于海洋表面，i₀指向北方，j₀指向东方，k₀指向地球中心。令p=（p_x，p_y，p_z）为坐标系R₀内机器人的中心点坐标。在坐标系R₀内，该水下机器人的状态变量为位姿p、切向速度v及其三个欧拉角ψ，θ，φ。系统输入为其切向加速度和三个操纵面，分别为w_x，w_y，w_z，更正式地将其表示为：

式中，u₂，u₃前面的系数v表示当该机器人在前进时，才能够向左/向右（通过u₃）或向上/向下（通过u₂）转动。请给出该系统的运动学状态模型。

习题1.10　三维机器人图形

在机器人学的仿真中，广泛用到在屏幕上绘制二维、三维机器人或物体。经典方法（OPENGL所使用的）依赖于利用一系列如下形式的仿射变换（旋转、平移和同位变换）对物体姿态的建模：

式中，n=2或3。然而，仿射函数各组成部分的处理并没有线性应用的那么简单，在齐次坐标系中，该转换的理念是将一个仿射方程的系统转化为一个线性方程的系统。首先应注意的是，可将一个形为y=Ax+b的仿射方程写为：

因此，将一个向量的齐次变换定义如下：

那么，一个如此形式的方程为：

包含三个仿射变换部分在内，并可将其写为：

一个模型就是一个有两行或三行（行数取决于该物体是处在平面上还是空间内）、n列的矩阵，n列表示可以包容该物体的一个刚体多边形的n个顶点。尤为重要的是，由该模型的两个连续点形成的所有部分的合集形成了所要表达的多边形的所有顶点。

1）考虑在齐次坐标系中，该水下机器人（或自主水下车辆（AUV））的模型如下式所示：

请在一张纸上绘制该模型的透视图。

2）该机器人的状态方程如下：

式中，（φ，θ，ψ）为三个欧拉角。该系统的输入为切向加速度u₁、俯仰角u₂和偏航角u₃。因此，其状态向量x=（p_x，p_y，p_z，v，ψ，θ，φ）。请给出一个能够在平面x-y内绘制出该机器人的三维模型及其投影的MATLAB函数。通过依次移动该机器人的六个自由度，证明如上绘图的正确性。绘制一个如图1.14所示的三维模型。

3）利用如下关系：

绘制该机器人的瞬时旋转向量。

4）用欧拉法对机器人在各种情况下的运动进行仿真。

图1.14　机器人的三维模型及其在水平面内的投影

习题1.11　机械手

如图1.15所示的史陶比尔机械手，由几个刚性手臂组成。通过一系列几何变换便可获得该机器人的末端执行器坐标。可以证明的是，用四自由度的参数化法能够表示出这些变换，而这样的参数化法有很多，同时各有优劣，其中应用最广的便是Denavit–Hartenberg参数化法。在机械手关节都是旋转关节（就像该史陶比尔机械手的关节可以转动）的情况下，经证明如图所示的参数化法或许是非常实用的，因为这可以使该机械手的绘制变得更为简单。该变换是由四个基本变换组成：①沿z轴长度为r的变换；②沿x轴长度为d的变换；③绕y轴角度为α的旋转；④角度为θ的旋转（绕z轴）。利用所绘制的这些手臂的图片和该机械手的照片，对该机器人的运动进行实际模拟。

图1.15　机械手的直接几何模型的参数化法

习题1.12　浮轮

考虑图1.16所示的轮子在空间中漂浮和旋转的情形。

图1.16　浮轮（有关此图的彩色版本，请参见www.iste.co.uk/jaulin/robotics.zip）

假设车轮是实心的，类似于质量m=1，半径ρ=1的均质圆盘。其惯性矩阵为：

1）给出该浮轮的状态方程，所选状态变量为①浮轮中心坐标p=（x，y，z），②浮轮的方向（φ，θ，ψ），③浮轮坐标系下的浮轮中心点速度向量v_r，④浮轮坐标系下的旋转向量w_r。由此可得一个12级系统。

2）当角速度向量随时间改变方向时，出现无力矩进动。请通过仿真解释说明该进动。

3）通过仿真验证角动量L=R·I·R^T·w和动能均为常数。

4）利用符号计算点x₀=（0，0，0，0，0，0，0，0，0，w_x，w_y，w_z）^T处的演化函数f的雅可比矩阵，并解释说明。

习题1.13　惯性系中的舒勒振荡

假设地球静止不转动，存在直接指向中心的重力场，重力加速度为g=9.81ms^-2。一惯性单元固定在机器人R₁中，该机器人位于半径为r的地球表面。如图1.17所示，其位置为p=（r，0，0），欧拉角速度为（φ，θ，ψ）=（0，0，0）。在该题中，机器人R₁将一直静止不动。

图1.17　机器人内的惯性单元是静止不动的

1）请给出由R₁的惯性单元所得到和w_r的值。

2）在此，通过在不同位置的R₂中初始化该机器人诱导惯性单元，并找到一个轨迹使得惯性单元始终固定于R₁。更准确地说，找到一个R₂的移动p（t）=（x（t），y（t），z（t）），使得惯性单元能精确地感知R₁的输入,W_r。为此，假设在t=0时刻，对于R₂，有（φ，θ，ψ）=（0，0，0）。为简化起见，还可假设R₂保持在平面y=0上，请在上述情形下，简化R₂的状态方程。

3）请在p（0）=（r，0，1）^T的初始化条件下，给出R₂的一个轨迹模拟，并解释。

4）在对应于R₁的状态向量处线性化R₂的状态方程。计算特征值并解释说明。

5）假设惯性单元没有完全初始化，针对相应的误差传播能得出什么结论？

习题1.14　控制用李氏括号

考虑Rⁿ中的两个分别对应于状态方程=f(x)和=g(x)的向量场f和g，将这两个向量场之间的李氏括号定义为

可以证明带有李氏括号的向量场集是李代数，该证明并不困难，但较为烦琐，在此不做要求。

1）考虑在线性向量场f（x）=A·x，g（x）=B·x中，计算[f，g]（x）。

2）考虑如下系统：

建议采用以下循环序列：

式中，δ是无穷小的时间周期δ。用{（1，0），（0，1），（-1，0），（0，-1）}表示该周期性序列。

证明：

3）请问应该采用哪一个周期序列来跟踪v·[f，g]？首先考虑v≥0的情况，然后考虑v≤0的情况。

4）请给出一个控制器，使系统变为：

式中，a=（a₁，a₂，a₃）为新输入向量。

5）考虑一个由以下状态方程描述的Dubins车：

式中，u₁为汽车的速度，θ为其方向，（x，y）为其中心坐标。使用李氏括号，为系统增加一个新输入，即新的控制方向。

6）设计一个仿真模拟来检查控制器的行为好坏。为此，取足够小的δ，使其与二阶泰勒近似一致，但对于采样时间dt，δ较大。例如，取，初始状态向量取为x（0）=（0，0，1）。

7）设计一个可以使汽车上升的控制器，这样汽车就可以上升了。考虑同样的问题，分别是汽车在哪里向下，向右和向左。

习题1.15　跟踪赤道

考虑一个移动体绕半径为ρ_E的地球垂直轴以的旋转速率转动的情形。

1）在图1.18所示各坐标系下，在p点绘制一个绕静止物体旋转的地球。

（1）R₀为固定在地球中心的惯性坐标系。

（2）R₁是地球坐标系，与地球以旋转速度转动。

（3）R₂是导航坐标系，以自我为中心，指向天空。

（4）R₃机器人以自我为中心的坐标系。

针对该仿真，取=0.2rad/s，ρ_E=10m。

2）给出该物体的运动状态方程，状态变量取（p，R₃，v₃），其中p为物体在R₀中的位置；R₃为物体的方向；v₃表示在R₃中的，从R₀所得到的物体的速度。选择输入为w₃，即物体和a₃的旋转向量。所测得的加速度均表示在R₃中，请给出地球自转和物体在空间中运动的仿真模拟，重力由给出。

3）现在假设这颗行星是由水构成的，它的身体是一个水下机器人，配备有执行器（如螺旋桨）在水中移动。重力由给出。旋转行星的水通过摩擦力（切向和旋转）来拖动机器人。此外，与周围流体具有相同密度的机器人，会受到向上的阿基米德浮力。请给出描述该机器人运动的动力学模型，输入为表示在R₃中的旋转加速度u_w3和切向加速度u_a3。这些加速度由于执行器的作用，在机器人上产生力和力矩。请给出一个模拟仿真来说明所有这些效果。

4）该机器人的行为就像一个3D Dubins小车模型。更准确地说，它配备了一个可以推动机器人前进但不能控制速度的螺旋桨。取u_a3=（1，0，0）^T，假设机器人可以使用方向舵、喷射泵或惯性轮来选择其旋转向量。设计一个控制器让机器人沿着赤道向东走，并进行仿真验证。

图1.18　不同坐标系中的地球（已将帧的原点进行了更大程度的可视化移动。有关此图的彩色版本，请参见www.iste.co.uk/jaulin/robotics.zip）