人工智能与智能系统3->|人工智能与智能系统3-> 机器人学3 | 移动机器人平台人工智能与智能系统3-

机器人学的基本工具已经了解完毕，现在开始了解移动机器人，这部分包括机器人平台、导航、定位。
所谓机器人平台就是指机器人的物理结构及其驱动方式。本文将学习两种典型移动机器人平台(四旋翼和轮式车)的运动与控制，研究输入控制信号怎么控制位姿随着时间变化。

到这一部分网上的笔记突然变少 ≈ 0
但我想从这一部分开始，就更有意思了。
0207注：好难啊，机器人不愧是屠龙之术。
0212注：此文很长。已经到了可以分几篇发出去的程度...

本系列参考资料：

《Robotics, Vision and Control》
B站公开课：
1. 台湾交通大学机器人学公开课
2. Peter Corke 配套视频
3. 这本书的中文公开课(质量不算高但还算实用)【公开课】机器人学视觉控制- matlab仿真_哔哩哔哩_bilibili

00 机动性

0210附：机动性这部分物理、力学、控制概念极多，非相关专业会看着比较难受，推荐一个物理教育网站，闲暇可以玩玩：
物理概况 - 主题索引 (hypertextbook.com)
以及一个控制、力学相关的系列文章：
https://zhuanlan.zhihu.com/p/48662038

学习两种平台之前，有一个一般性的概念：机动性，可以帮助之后的理解。
所谓机动性，其实是机器人实现空间运动的相关概念集合。

位形空间(Configuration space / C-space)：
所有的可能的位置形态集合，我们记为C：

\[位置坐标\dot{q}\in \mathcal{C} \]
任务空间(task space)
机器人所有可能的位姿集合，任务空间依赖于具体应用或任务。记为T：

\[位姿\xi\in\mathcal{T} \]
自由度
确定系统在空间中的位置所需要的最小坐标数。
广义坐标
用来描述系统位形所需要的独立参数或者最小参数
欠驱动系统
系统独立控制变量个数小于系统的自由度
即输入的量要比控制的量要少的系统
欠驱动系统
我们可以举几个运动系统的例子来理解理解这几个概念
- 火车
  1. 对于火车来说，火车的位置可以用其沿轨道的距离数据来描述，即一个标量参数q来决定火车的位置；
  2. 位形空间(C空间)：即q描述的火车的位置构成的空间，q∈C；
  3. 任务空间：依赖于具体任务的意思就是说，比如火车是要沿直线导轨运动，我们只需要关心火车在一个直线上的位置，此时：
    
    \[\mathcal{T}\subset R \]
    同理，考虑火车在平面上的位置则：
    
    \[\mathcal{T}\subset R^2 \]
    三维同理。
  4. 自由度：1维，因为火车由一个独立的标量参数q来完全决定
    但联系一下C空间和task space，当火车的任务空间维数超过位形空间维数时，那么火车不能随便到达任意一个任务空间中的位置，因为它是被约束在一条导轨上的。
  5. 广义坐标：即这个q
  6. 驱动情况：全驱动系统，因为火车有一个驱动器，推动火车沿着轨道前进或者后退
- 气垫船
  1. 气垫船有两个螺旋桨，轴线平行，两个螺旋桨推力的合理形成一个向前的推动力。可以用q=(x,y,θ)来决定其位置；
  2. 位形空间，即q描述的位置构成的空间，q∈C；
    PS：
    
    \[这里的C\subset{}R^2×S \]
  3. 任务空间同上个例子的讨论
  4. 自由度，可见最小独立描述坐标数为3(x , y , θ)，自由度为3；
  5. 广义坐标：q
  6. 驱动情况：两个驱动器，比自由度少，所以为欠驱动系统。
    
    欠驱动系统会限制自由运动的能力，我们任意时刻都可以控制气垫船的前向加速度和旋转加速度，但气垫船的侧向加速度始终为0；但是我们总能通过其他的运动来代替直接侧向移动（虽然麻烦了一点）。
    但是对于这些工具来说，这满足了最常用的前进的功能要求，并且驱动器较少。
    
    如果给这个船侧向垂直于原两个螺旋桨的方向增加一个螺旋桨，那么就实现了平面上任意方向的运动，前进、侧移、旋转。这时，任务空间等效于位形空间。
- 直升机
  1. 简单介绍一下，4个驱动器+一个重力驱动，位姿由6个广义坐标描述q=(x,y,z,θrθpθy)，即三维空间中的位置xyz，以及滚转-俯仰-偏转的姿态角度。
  2. 【人工智能与智能系统3->|人工智能与智能系统3-> 机器人学3 | 移动机器人平台】自由度为6.是欠驱动系统。
  3. \[任务空间\mathcal{T}\subset SE(3) \]
- 固定翼
  1. 与直升机类似，驱动器（向前推进器、副翼、升降舵和方向舵）位形空间等与直升机相同。
- 汽车/坦克
  1. 两驱动器，一个前后运动、另一个改变行驶方向
  2. q = (x, y, θ)，三个维度
  3. 欠驱动
  4. 车体转弯时由于由横向摩擦力，所以不需要额外驱动器提供这个力
  5. 可以实现侧滚的轮子已经研发出来了：全向轮、麦克纳姆轮：麦克纳姆轮浅谈
补充几个概念：
1. 再次感谢这个系列对于我认知概念的指引，机动性这部分力学内容很多：
  思考(8)李括号, 完整与非完整约束的区别
2. 完整约束和非完整约束：
  1. 完整约束可以用位形q的一个方程F(q, t)来描述；
  2. 非完整约束约束方程中包含坐标对时间的导数（如运动约束），
    
    \[F(q,\dot{q},t)且不能通过积分转化为F(q,t) \]
    而且方程不可能积分成有限形式，这类约束成为非完整约束。
3. 非完整系统、完整系统：
  - 非完整系统是指典型的受非完整约束系统，比如：车辆；标志是时变控制策略。
  - 反之为完整系统。

这里这些概念的理解其实有待商榷，我没有完全在脑海里形成对这些概念的具象印象，我搜了很多，大多是运动规划方面的名词，但是并不完全对应(因为我对运动规划也并不了解)，这对我的学习造成了很大的困难。
所以我认真研究了书上的例子，对这些名词进行对应，大概好了一点。

01 轮式机器人四轮机器人的常用模型是双轮自行车模型，即两后轮固定在车体上，两前轮可以绕水平轴转动以实现车辆转向。
01-1 概念及模型
01-1-1 模型的相关概念引入模型图如下，浅灰色是车体，深色是近似的自行车两轮，位形q=(x,y,θ)∈C（SE(2)）。其中x是在世界坐标系O中x的坐标，y是O中y的坐标，θ是指车身与O坐标系x的夹角。

人工智能与智能系统3->|人工智能与智能系统3-> 机器人学3 | 移动机器人平台

文章图片
机器人学3 | 移动机器人平台_第1张图片" width="419" height="466" style="border:1px solid black; ">

根据机器人学1部分的内容，对车辆建立物体坐标系V（图中红色），γ是前轮的转向角，是前轮与Vx轴的夹角，
左上角的ICR是小车的旋转瞬心（后轮前轮的中垂线交点，转弯时可以看作圆周运动；
R1和R2分别是后轮前轮到ICR的举例
L是轮基的长度。
V中速度及角速度：
1. \[^V\dot{x}=v\\ ^v\dot{y}=0\\ \dot\theta=\frac{v}{R_1} \]
  （因为后轮与v方向一致）
2. 易知图中转向的圆心角为γ，则
  
  \[R_1 = \frac{L}{\tan{\gamma}} \]
3. 从上式可以看出，转弯半径R1会随着车辆长度(轮基L)的增加而增加，γ的大小是由机械结构限制的，R1最小时，γ最大。
4. R2>R1，前轮会比后轮多走一段路，并且两个前轮的情况也不同，所以两个前轮的转向角度也应该有所不同，可以通过阿卡曼(模型)转向机构实现：
文章图片
机器人学3 | 移动机器人平台_第2张图片" width="650" height="225" style="border:1px solid black; ">
阿克曼模型 ≠ 自行车模型
当然，两个前车轮转弯时要以不同的速度转动，所以发动机和驱动轮之间要按转一套差动齿轮箱。
坐标系O中速度及角速度：
1. \[\dot{x}=v\cos\theta\\ \dot{y}=v\sin\theta\\ \dot{\theta}=\frac{v}{L}\tan\gamma \]
2. 这就是小车的运动学模型，即只描述了小车的运动速度，没有涉及力和力矩的建模；
3. 第三个式子表达了θ的变化率，也叫转动速率，可以通过陀螺仪(上一部分的惯性导航部分)直接测量，也可以通过左右轮的角速度值推算出来；
4. \[\dot{y}\cos\theta-\dot{x}\sin\theta\equiv0 \]
  世界坐标系中y方向的表达式，是一个非完整约束(无法积分为x,y,θ表达式)

01-1-2 simulink模型 simulink的模型是很好读和理解的。
A Bicycle 模型

文章图片
机器人学3 | 移动机器人平台_第3张图片" width="650" height="200" style="border:1px solid black; ">
可见，输入为速度 v和角度 γ，接着分别是是速度限制和加速度限制、转向角限制和转向速率限制，tan(u/L)中u代表的就是这个γ，计算出了θ(输出3)，与cos和sin混合后，乘上v对时间进行积分得到x(输出1)和y(输出2)；

我们还可以用一个单独的手刹模块向积分模块输出一个常量信号，将积分器强制调回到初始条件

这就是上面运动学模型的模块计算体现。
下面我们使用这个模块实现一个变换车道的运动：
B sl_lanechange模型

文章图片
机器人学3 | 移动机器人平台_第4张图片" width="650" height="299" style="border:1px solid black; ">
可以看到最左侧的脉冲驱动发生器是先产生一个左转信号(z正值)，一段时间后右转(负值)，结合前面的小车模型图就可以料想我们就实现了一个左转变换车道后右转恢复正向。

文章图片
机器人学3 | 移动机器人平台_第5张图片" width="502" height="414" style="border:1px solid black; ">
下面我们查看一下其他值的情况：

sl_lanechange; r = sim('sl_lanechange'); Y = r.find('y'); T = r.find('t'); subplot(2,2,1); plot(T,Y(:,1)); xlabel('t'),ylabel('X'); subplot(2,2,2); plot(T,Y(:,2)); xlabel('t'),ylabel('Y'); subplot(2,2,3); plot(T,Y(:,3)); xlabel('t'),ylabel('theta'); subplot(2,2,4); plot(T,Y(:,4)); xlabel('t'),ylabel('gamma'); % 图表如下：分别是X、Y、Theta、gamma的时间变化曲线

文章图片
机器人学3 | 移动机器人平台_第6张图片" width="571" height="703" style="border:1px solid black; ">
01-2 移动到一个点
01-2-1 控制条件讨论一下轮式向平面上某个点(x1,y1)移动的问题，我们预设小车的速度大小与离目标的距离成正比。由如下等式：

\[(1).{}v_1=K_v\sqrt{(x_1-x)^2+(y_1-y)^2} \]
转向目标角使用世界坐标系中的相对角度θ1：

\[(2).\theta_1 = \arctan\frac{y_1-y}{x_1-x} \]
我们使用比例微分控制器(PID)来调整车辆的转向角(θ是小车目前与世界坐标系x轴的夹角)：

\[(3).\gamma = K_h(\theta_1\ominus\theta),K_h > 0\\ (\ominus是符合特定条件的-)\\ 作差后的角度是逆时针的 \]
理解这个PID，当Kh=1时，我们小车就将与车目标点连线平行
如果Kh更大(>1)，则会向与连线交错的方向前进。
有了相关的控制条件就可以进行仿真：
Simulink仿真

文章图片
机器人学3 | 移动机器人平台_第7张图片" width="650" height="297" style="border:1px solid black; ">
可以看到这个模型使用的bicycle模块输入是速度和转向角，输出θ，对于速度v我们采用的即为上面(1)式控制，γ角即为(2)(3)式控制，

Warning：此时运行报错，说参数范围不对
"Simulink will stop the simulation of model 'sl_drivepoint' because..."，
上网搜索了一下，发现是simulink内置的angdiff的参数顺序与我们预想正好反了，修改即可：

文章图片
机器人学3 | 移动机器人平台_第8张图片" width="650" height="322" style="border:1px solid black; ">
参考：
sl_drivepoint 模型 consecutive zero crossings

sl_drivepoint; ; xg = [5 5]; x0 = [8 5 pi/2]; r = sim('sl_drivepoint'); q = r.find('y'); %书上此处不对 plot(q(:,1), q(:,2)) % 输出三个黑图和自己画的位置变化图：

文章图片
机器人学3 | 移动机器人平台_第9张图片" width="650" height="369" style="border:1px solid black; ">
值得注意的是最后出现了一段直线。
01-3 跟踪一条直线
01-3-1 控制条件下面我们探讨跟踪平面上由ax+by+cz=0定义的一条线，我们会需要两个控制器，第一个控制机器人与直线的距离，以使得其与直线垂直距离最短。距离为：

\[(1).d=\frac{(a,b,c)\cdot(x,y,1)}{\sqrt{a^2+b^2}} \]

值得注意的是这个公式没有加绝对值，在上方为正，在下方为负。

用下式PID控制机器人的向目标直线运动：

\[(2). \alpha_d = -K_dd,K_d>0 \]
第二个要调整小车的位姿平行于直线，θ1是直线的斜率对应的角度：

\[(3).\alpha_h=K_h(\theta_1\ominus\theta),K_h>0\\ (4).\theta_1 = \arctan\frac{-a}{b} \]
我们可以将两个控制律综合起来：

\[(5).\gamma=-K_dd+K_h(\theta_1\ominus\theta) \]
我们没有控制速度，因为直线是无限长的，速度约束不强，只需要取一个常数即可。
01-3-2 Simulink仿真

文章图片
机器人学3 | 移动机器人平台_第10张图片" width="650" height="280" style="border:1px solid black; ">
可见速度是一个常数1，相关设置不一一解释，实现的就是上面的控制律。

sl_driveline; % 设置目标直线 L = [1 -2 4]; % 车的位置和位姿 x0 = [8 5 pi/2]; r = sim('sl_driveline');

同上个例子，角度输入顺序不对，同样需要调整一下angdiff的参数顺序，不然会反复报错。

文章图片
机器人学3 | 移动机器人平台_第11张图片" width="650" height="645" style="border:1px solid black; ">
01-4 跟踪一般路线
01-4-1 控制条件除了直线，更普遍的是更具一般性的曲线。而曲线无疑在机器人规划中具有重要意义，当我们讨论机器人导航，传感器或规划器产生的路径很有可能就是一般曲线。
如何考虑这件事情呢，可以采用纯追踪算法，即目标点沿着曲线恒速运动，而机器人向目标点运动。即在01-2移动到一个点的基础上使这个目标点移动。
小车不能超过目标点，需要一个安全距离（跟踪误差）：

\[(1).e = \sqrt{(x_1-x)^2+(y_1-y)^2}-d^* \]
意思是，当小车到达距目标点d*处时，小车速度不能超过目标点速度。
我们需要限制小车的速度：

\[(2).v_1 = K_ve+K_i\int{e}dt \]
积分项是用来确保当e趋于0时仍能提供一个有限的速度需求量v1，第二个控制律需要控制小车转向目标点：

\[(3). \alpha = K_h(\theta_1\ominus\theta),K_h>0\\ (4).\theta_1 = \arctan\frac{y-y_1}{x-x_1} \]
理想情况是保持相对静止。
01-4-2 Simulink仿真

文章图片
机器人学3 | 移动机器人平台_第12张图片" width="650" height="275" style="border:1px solid black; ">
最左侧的trajectory模块是利用插值方法的轨迹生成器；

sl_pursuit; r = sim('sl_pursuit'); % 输出：

文章图片
机器人学3 | 移动机器人平台_第13张图片" width="389" height="339" style="border:1px solid black; ">
可见红色虚线为我们的一般路径，蓝色即为小车的仿真路线。在直线部分的贴合效果很好，在拐弯的地方两者都改变运动状态，小车需要调整。
01-5 运动到某个位姿
01-5-1 控制条件如何小车运动到一个特定的位姿呢，之前我们只实现了控制小车到指定目标点，但并没有控制位姿，或者说，最终的位姿会依赖于起始位姿。
下面我们尝试控制最终的目标位姿，虽然只增加了一个条件但是需要考虑的事情复杂了一些：
回忆一下我们之前的运动控制式：

\[\dot{x}=v\cos\theta\\ \dot{y}=v\sin\theta\\ \dot{\theta}=\frac{v}{L}\tan\gamma \]
我们将其重写为：

\[\left[\begin{matrix} \dot{x}\\ \dot{y}\\ \dot{\theta} \end{matrix}\right] = \left[\begin{matrix} \cos\theta & 0\\ \sin\theta & 0\\ 0 & 1 \end{matrix}\right] \left[\begin{matrix} v \\ \dot{\gamma} \end{matrix}\right] \]
可见差别是第三行，但实际上是等价的，这是因为：

\[\dot\gamma = \frac{v}{R_1}=\frac{v}{L}\tan\gamma\\ ∴\dot\gamma=\dot\theta \]
这是个很容易理解但很重要的结果。

文章图片
机器人学3 | 移动机器人平台_第14张图片" width="470" height="471" style="border:1px solid black; ">
我们根据上图再对变量做一下变换(极坐标)：

\[\rho=\sqrt{\Delta_x^2+\Delta_y^2}\\ \alpha = \arctan\frac{\Delta_y}{\Delta_x}-\theta\\ \beta=-\theta-\alpha \]

θ是车身与世界坐标系X轴的夹角
α是小车后轮(速度方向)与ρ的夹角
γ是前轮转向角
β是始末两车中心连线与X轴的夹角，加负号是因为方向

带入后我们得到：

\[\left[\begin{matrix} \dot{\rho}\\ \dot{\alpha}\\ \dot{\beta} \end{matrix}\right] = \left[\begin{matrix} -\cos\alpha & 0\\ \frac{\sin\alpha}{\rho} & -1\\ -\frac{\sin\alpha}{\rho} & 0 \end{matrix}\right] \cdot \left[\begin{matrix} {v}\\ \dot{\gamma} \end{matrix}\right]\\ 若\alpha\in[-\frac{\pi}{2},\frac{\pi}{2}] \]

我们来简单理解一下：
第一行，即速度v在ρ分方向上投影的速度vcosα，负号表示向左转；
第三行，也很好理解，设想一个微分的情况，即绕上图G坐标系原点G做一个微小的旋转，线速度为vsinα，半径来不及改变故为ρ；利用角速度公式即可得；
第二行，利用上面的 β=-α-θ ，而：

\[\dot{\theta}=\dot{\gamma} \]

假设目标在小车前方，则采用下面的线性控制方法：

\[v = K_\rho\rho\\ \dot{\gamma} = K_\alpha\alpha+K_\beta\beta \]
Kρρ和Kρα控制小车沿一条直线向目标运动（ρ->0，α->0），而Kρβ用来转动该直线使得β->0。再次代入，闭环控制系统为：

\[\left[\begin{matrix} \dot{\rho}\\ \dot{\alpha}\\ \dot{\beta} \end{matrix}\right] = \left[\begin{matrix} -k_\rho\rho\cos\alpha\\ k_\rho\sin\alpha-k_\alpha\alpha-k_\beta\beta\\ -k_\rho\sin\alpha \end{matrix}\right] \\ 稳定条件为：k_\rho>0,k_\beta<0,k_\alpha-k_\rho>0\\ PS:书上右侧矩阵第一行少\rho,是默认为1了 \]
当目标在机器人后方，只需把控制算法中的v和γ符号变为负号，v不改变符号，取决于α初值。
到这里为止还不够，因为我们讨论的还是到(0, 0)处任意位姿的控制律，而要想得到其他任一点(x, y)的控制律，坐标变换即可：

\[\Delta{x}=x-x_1\\ \Delta{y}=y-y_1\\ \theta'=\theta\\ \beta=\beta'+\theta* \]

前两行容易理解
第三行代表θ没有改变
第四行意思是，最终的β角度由两部分组成，其一是转过的角度，其二是目标位姿与世界坐标系X轴的夹角。

01-5-2 Simulink仿真

文章图片
机器人学3 | 移动机器人平台_第15张图片" width="650" height="170" style="border:1px solid black; ">
这个图还是挺好看的，值得注意的是：

右侧偏上的desired position中为x1，y1，0，与下方x，y，theta作差，输出Δx、Δy、theta进入后面to_polar中转化为极坐标；
to_polar输出ρ、α、β’
to_polar中direction，会输出一个1和-1，来调整相关值的符号；
xg(3)是θ*，与左侧来的β' 相加，得到β；
左侧的系数就比较好读了。

sl_drivepose; % 目标位姿 xg = [5 5 pi/2]; % 初始位姿 x0 = [8 5 pi/2]; r = sim('sl_drivepose'); y = r.find('y'); plot(y(:,1),y(:,2)); % 输出：

文章图片
机器人学3 | 移动机器人平台_第16张图片" width="650" height="628" style="border:1px solid black; ">
01-6 阶段总结
总结一下上面四个模型，都是通过控制v和γ来控制小车运动。

移动到一个点
追踪一个固定位置的点，到达目的地速度必须为零，没到达目标点时需要控制γ使得小车转向目标点，但不能控制小车最终的位姿，最终位姿依赖于初始位姿。
跟踪一条直线
γ的设置要使小车不断接近目标直线，并且小车到达直线时小车平行于直线，由于直线无限长，小车到直线上时仍有速度，所以不控制速度。
跟踪一般路径
追踪一个沿固定路径运动的目标点，v仍受距离控制，不同的是当小车运动到安全距离值时，小车会调整自己的速度使自己不超过目标点。
运动到一个位姿
将运动学模型改写，并写成极坐标形式，采用线性控制使得小车沿着一条直线向目标运动，并不断调整这条线的角度。

02 飞行机器人

讲道理这部分只是很简单的建模介绍，关于无人机飞控算法，有很多书专门讲这个，我们先了解一下控制过程即可，可能会有点繁琐和困难。先放两个教程链接：

多旋翼飞行器设计与控制北航

MATLAB-官方视频四轴无人机的仿真与控制

飞行机器人与地面机器人的一些技术参数不同，飞行机器人有6个自由度，位形空间q∈SE(3)，且是由动力驱动的，所以运动模型中必须包含力和力矩，而不仅仅是对于速度和角度的限制，这就要建立动力学模型了。
飞行机器人最简单的模型就是四旋翼飞行器。
02-1 动力学分析与建模
02-1-1 建立坐标系首先我们需要建立两个坐标系，世界坐标系O和机体坐标系B，机体坐标系B固联在飞行器上，原点与质心重合，横轴OX正方向指向1号电机，纵轴OY正方向指向4号电机，OZ垂直于OXY平面，正方向垂直OXY向下。
02-1-2 四旋翼模型

文章图片
机器人学3 | 移动机器人平台_第17张图片" width="473" height="403" style="border:1px solid black; ">
图中显示了4个旋翼以及推力矢量Ti和旋转方向，旋翼1、3逆时针(从上往下看)，旋翼2、4顺时针旋转。
02-1-3 复习欧拉角主要是复习三角度表示法中的翻滚-俯仰-偏航角（卡尔丹角）：

\[R = R_x(\theta_r)R_y(\theta_p)R_z(\theta_y) \]

翻滚角Φ，表示机体坐标系绕OX轴旋转的角度，如果从飞行器尾部顺纵轴向前看，如果OZ轴位于铅垂面的右侧，则Φ为正；
俯仰角θ，表示机体坐标系绕OY轴旋转的角度，旋转后飞行器横轴指向水平面上方为正；
偏航角ψ，表示机体绕OZ轴旋转的角度，是飞行器纵轴OY在水平面的初始投影与OX轴的夹角，垂直地面看，如果OX转至投影是逆时针，则为正；

02-1-4 质心运动学模型设旋翼转速为ω，产生一个向上的推力向量，指向z轴负方向：

\[(1).T_i = b\omega_i^2,i=1,2,3,4 \]

其中b>0，是升力常数，取决于空气密度、旋翼叶片半径立方、叶片弦长；

该飞行器在世界坐标系中的移动动力学方程：

\[(2).m\dot{v}= \left[\begin{matrix} 0\\ 0\\ mg \end{matrix}\right] - {}^oR_B \left[\begin{matrix} 0\\ 0\\ T \end{matrix}\right] \]

解释：

v是飞行器在世界坐标系中的速度；

g是重力加速度；

m是飞行器的总质量；

T是向上的总推力；

oRB这个矩阵是将后面的向量转化到世界坐标系下；

简单讲就是重力减去推力，总推力T本在机体坐标系下，将其转化到世界坐标系。

\[(3).{^oR_B}=R_xR_yR_z= \left[\begin{matrix} \cos\psi\cos\phi & \cos\psi\sin\theta\sin\phi & \cos\psi\sin\theta\cos\theta+\sin\psi\sin\phi\\ \sin\psi\cos\theta & \sin\psi\sin\theta\sin\theta & \sin\psi\sin\theta\cos\phi-\sin\phi\cos\psi\\ -\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi \end{matrix}\right] \]
综合(1)(2)(3)可得质心运动模型：

\[(4).\ddot{x}=\sum_{i=1}^4b\omega_i^2(\cos\psi\sin\theta\cos\theta+\sin\psi\sin\phi)/m\\ \ddot{y} = \sum_{i=1}^4\omega_i^2(\sin\psi\sin\theta\cos\phi-\sin\phi\cos\psi)/m\\ \ddot{z} = \sum_{i=1}^4\omega_i^2(\cos\theta\cos\phi)/m-g \]
02-1-5 机体角运动模型上面是竖直方向上的约束，还需要讨论各个旋转角的动力约束。每对旋翼之间的推力差异就会导致飞行器旋转。

x轴上产生的转矩即横滚力矩

\[(5).\mathcal{T}_x=dT_4-dT_2\\ \Downarrow\\ (6).\mathcal{T}_x=db(\omega_2^2-\omega_4^2) \]
y轴产生俯仰力矩

\[(7).\mathcal{T}_y=db(\omega_1^2-\omega_3^2) \]
d是电机到飞行器质心的距离
对于z轴，考虑电机施加给每个旋翼的转矩，会有一个与之方向相反的空气阻力转矩：

\[(8).Q_i = k\omega_i^2 \]
这个转矩作用是使整个机体产生绕Z轴的转动，方向与旋翼方向相反，可以得出Z轴总转矩为：

\[(9).\mathcal{T}_z=Q_1-Q_2+Q_3-Q_4=k(\omega_1^2+\omega_3^2-\omega_2^2-\omega_4^2) \]
可见调整旋翼转速即可实现偏航力矩。
k与升力常数影响因素相同。
旋转加速度 ( 角加速度 )：

\[(10).\mathcal{J}\dot{\omega}=-\omega×\mathcal{J}\omega+\Gamma \]
1. ω 是角速度向量 (3×1)
2. Γ=(τx, τy, τz)T是作用在机体各个方向上的力矩
3. J 是3×3的机体惯性矩阵，因为四旋翼是对称的刚体：
  
  \[(11).J= \left[\begin{matrix} J_x & 0 & 0\\ 0 & J_y & 0\\ 0 & 0 & J_z \end{matrix}\right] \]

02-1-6 总的运动模型整合02-1-4和02-1-5，我们可以得到四旋翼的运动模型：

\[(12). \left[\begin{matrix} T\\ \tau_x\\ \tau_y\\ \tau_z \end{matrix}\right] = \left[\begin{matrix} T\\ \Gamma \end{matrix}\right] = \left[\begin{matrix} -b & -b & -b & -b\\ 0 & -db & 0 & db \\ db & 0 & -db & 0 \\ k & -k & k & -k \end{matrix}\right] \left[\begin{matrix} \omega_1^2\\ \omega_2^2\\ \omega_3^2\\ \omega_4^2 \end{matrix}\right] = A \left[\begin{matrix} \omega_1^2\\ \omega_2^2\\ \omega_3^2\\ \omega_4^2 \end{matrix}\right] \]
所有力的力矩都是旋翼转速的函数，如果d>0，则A是满秩的，可以通过求逆得到:

\[(13). \left[\begin{matrix} \omega_1^2\\ \omega_2^2\\ \omega_3^2\\ \omega_4^2 \end{matrix}\right] = A^{-1} \left[\begin{matrix} T\\ \tau_x\\ \tau_y\\ \tau_z \end{matrix}\right] \]
这样就可以求出对机体施加特定的力和力矩所需要的各旋翼转速。
02-1-7 附：旋转角加速度的计算最后补充一下旋转角加速度的计算：
由

\[(10).\mathcal{J}\dot{\omega}=-\omega×\mathcal{J}\omega+\Gamma\\ (11).J= \left[\begin{matrix} J_x & 0 & 0\\ 0 & J_y & 0\\ 0 & 0 & J_z \end{matrix}\right]\\ (13). \Gamma = \left[\begin{matrix} \tau_x\\ \tau_y\\ \tau_z \end{matrix}\right] = \left[\begin{matrix} dT_4-dT_2\\ dT_3-dT_1\\ k(\omega_1^2+\omega_3^2-\omega_2^2-\omega_4^2) \end{matrix}\right] \]
知：

\[(14). \left[\begin{matrix} J_x & 0 & 0\\ 0 & J_y & 0\\ 0 & 0 & J_z \end{matrix}\right] \left[\begin{matrix} \dot{\omega_x}\\ \dot{\omega_y}\\ \dot{\omega_z} \end{matrix}\right] = \left[\begin{matrix} -\omega_x\\ -\omega_y\\ -\omega_z \end{matrix}\right] × \left[\begin{matrix} J_x & 0 & 0\\ 0 & J_y & 0\\ 0 & 0 & J_z \end{matrix}\right] \left[\begin{matrix} \omega_x\\ \omega_y\\ \omega_z \end{matrix}\right] + \left[\begin{matrix} dT_4-dT_2\\ dT_3-dT_1\\ k(\omega_1^2+\omega_3^2-\omega_2^2-\omega_4^2) \end{matrix}\right] \]
化简后即可得到：

\[(15).\dot{\omega_x}=\frac{d(T_4-T_2)+(J_z-J_y)\omega_y\omega_z}{J_x}\\ \dot{\omega_t}=\frac{d(T_3-T_1)+(J_x-J_z)\omega_x\omega_z}{J_y}\\ \dot{\omega_z}=\frac{k(\omega_1^2+\omega_3^2-\omega_2^2-\omega_4^2)+(J_y-J_z)\omega_x\omega_y}{J_z}\\ \]
02-2 控制条件
我们用一种嵌套式的控制结构来控制飞行器的运动，我们以绕y轴俯仰以及沿x轴平移为例说明。

具体控制思路为：

我们根据期望位置与实际位置的差异，控制转速，因为根据前面的动力学分析可以知道，转速同时影响俯仰和位移的加速度；

根据1的控制结果，我们控制俯仰力矩和X方向平移的力，使飞行器运动。

02-2-1 绕Y轴俯仰力矩最内层用一个PID计算机体的(绕y轴的)俯仰力矩：

\[\tau_y = K_p(\theta_p^*-\theta_p)+K_d(\dot\theta_p^*-\dot\theta_p) \]

θ*p是目标俯仰角，θp是实际角度，实际角度θp是通过惯导系统估算得出的。

计算主要是基于目标俯仰角与实际角度之间的误差，Kp和Kd的确定可以先根据近似动力学模型用经典控制设计方法给出初始值，再进行修正，来达到最佳控制效果。
02-2-2 沿X方向位移为了研究运动变化的方便，我们在机体上连接一个坐标系{V}，它与变化前的{B}相同，在变化中保持不变。
要使机体沿X轴方向位移，我们要使机头下俯(也即绕Y轴旋转一个小角度)，来产生一个向X方向的分力fx：

\[f=R_y(\theta_p) \left[\begin{matrix} 0\\ 0\\ T \end{matrix}\right] = \left[\begin{matrix} T\sin\theta_p\\ 0\\ T\cos\theta_p \end{matrix}\right]\\ f_x=T\sin\theta_p\approx{}T\theta_p\quad(1) \]
我们采用比例控制法控制这个方向的加速度：

\[f_x^*=mK_f(^Vv_x^*-{}^Vv_x)\quad (2) \]
联立(1)(2)，得：

\[\theta_p^*=\frac{m}{T}K_f(^Vv_x^*-{}^Vv_x) \]
即为为达预定前进速度所需要的俯仰角。上式中的实际飞行器速度可以有惯导或GPS接收器估算得到。
竖直方向上有重力和推力总和相等：mg=T。我们在仿真中需要这个条件来约束我们的转速。
02-2-3 速度控制接下来考虑最外层的控制律，即根据实际位置与目标位置的PID来控制速度v和ω。来实现上面所说的内层的俯仰和位移。
假设p表示机器人位置，v1是预期速度，则：

\[v_1 = K_p(p_1-p)\quad(p\in{}R^2) \]
我们将这个v1转化到坐标系V中：

\[^Vv={}^VR_0(\theta_y)v={}^0R_V^T(\theta_y)v\\ \Downarrow\\ \left[\begin{matrix} ^Vv_x \\ ^Vv_y \end{matrix}\right] ={} ^0R_V^T(\theta_y) \left[\begin{matrix} v_x \\ v_y \end{matrix}\right] \]
02-4 Simulink仿真

文章图片
机器人学3 | 移动机器人平台_第18张图片" width="650" height="251" style="border:1px solid black; ">
这个图具体实现还有一点复杂，但公式就是前面02-1和02-2推导的公式。教材上没有将其中的一些模块封装，而此图已经封装完成，每个模块的功能都标注出来了。看着也比较清晰。