本来笔者一直调不好，已经准备放弃，没想到最后发现是转动惯量参数写错了，不得不说SimScape还是非常强大的。
上一篇详细介绍了如何构建并导入模型：
MATLAB Simmechanics/Simscape四旋翼无人机控制仿真（1） SolidWorks模型的导入.
这篇博客将从导入到Simulink的SolidWorks模型开始，对这个模型进行调节，为旋翼机添加动力，为后面的控制设计做准备。
因为参考了许多别人的模型，最后我的MATLAB升级为了2019a。
模型详见：
https://github.com/mkb9559/Simscape-Quadrotor
MATLAB Simmechanics/Simscape四旋翼无人机控制仿真（3） 无人机控制器设计.
MATLAB Simmechanics/Simscape四旋翼无人机控制仿真（4） 串级姿态控制.
Demo视频.
最近更新了第二代模型，可以参考：
MATLAB Simmechanics/Simscape四旋翼无人机控制仿真（5） F250旋翼仿真搭建.
上一篇博客已经将模型导入到了Simulink，如上图所示。
旋翼机飞行时，螺旋桨旋转，会产生升力
T
T
T，同时对机身产生反扭力矩
M
M
M，其与转速平方呈线性关系，这里转速单位一般是rpm/min：
T
=
k
t
Ω
2
T{=}{{k}_{t}}{{\Omega }^{2}}
T=ktΩ2
M
=
k
q
Ω
2
M={{k}_{q}}{{\Omega }^{2}}
M=kqΩ2因此，配置以4号电机为例，如下图所示，
给这个螺旋桨一个输入（实际上是力矩），这个转速按照如上公式关系，生成一个升力和反扭力矩，因为笔者对空气动力不了解，因此这里的系数随便给了两个值都是1。笔者参考过很多旋翼仿真模型，这种是比较好的，优点是在输入稳定的情况下，转速能快速稳定，这对仿真性能有很大影响。下图是恒定输入下的转速变化，可以看到转速的稳定连0.1s都不需要，这样的方法是很快的。
还有两种种配置方法，一是不添加反扭力矩，为给转轴关节添加阻尼，使转速稳定；二是不添加反扭力矩，设计电机转速控制器。但是这两种方法的致命缺点是会使得旋翼机的偏航控制通变成一阶系统，和实际的二阶系统不符，如果使用PID控制，那么是没有什么区别的，甚至这样更加简单。
为每个螺旋桨按照如上配置，旋翼机的动力就基本配置完成，注意，四旋翼的螺旋桨旋转是对角方向相同，相邻方向相反的。
这里介绍X型布局的推力分配，这实际上是无人机控制的底层，控制关系如下：
u
1
=
k
t
(
Ω
1
2
+
Ω
2
2
+
Ω
3
2
+
Ω
4
2
)
{{u}_{1}}={{k}_{t}}\left( \Omega _{1}^{2}+\Omega _{2}^{2}+\Omega _{3}^{2}+\Omega _{4}^{2} \right)
u1=kt(Ω12+Ω22+Ω32+Ω42)
u
2
=
k
t
l
(
Ω
1
2
+
Ω
2
2
−
Ω
3
2
−
Ω
4
2
)
{{u}_{2}}={{k}_{t}}l\left( \Omega _{1}^{2}+\Omega _{2}^{2}-\Omega _{3}^{2}-\Omega _{4}^{2} \right)
u2=ktl(Ω12+Ω22−Ω32−Ω42)
u
3
=
k
t
l
(
Ω
1
2
+
Ω
4
2
−
Ω
2
2
−
Ω
3
2
)
{{u}_{3}}={{k}_{t}}l\left( \Omega _{1}^{2}+\Omega _{4}^{2}-\Omega _{2}^{2}-\Omega _{3}^{2} \right)
u3=ktl(Ω12+Ω42−Ω22−Ω32)
u
4
=
k
q
(
Ω
1
2
+
Ω
3
2
−
Ω
2
2
−
Ω
4
2
)
{{u}_{4}}={{k}_{q}}\left( \Omega _{1}^{2}+\Omega _{3}^{2}-\Omega _{2}^{2}-\Omega _{4}^{2} \right)
u4=kq(Ω12+Ω32−Ω22−Ω42)
u
1
{{u}_{1}}
u1到
u
4
{{u}_{4}}
u4就是无人机控制里面一般使用的控制输入，
l
{{l}}
l是旋翼升力所用于无人机产生力矩的力臂，X布局的是
2
2
\frac{\sqrt{2}}{2}
22
机臂长，他们的关系如上，可以通过Matlab反解出
Ω
1
2
{\Omega_{1}^{2}}
Ω12到
Ω
4
2
{\Omega_{4}^{2}}
Ω42，这里就不打公式了，模型里面有。
到此，Simulink模型的调节基本结束，无人机已经拥有了动力特性，在调节无人机控制的时候一般先调节姿态控制，再调节位置控制，因此再调节姿态控制的时候使用一个球转动关节与世界坐标系固连，并将重力配置为0，给定各个电机一定的转速，可以查看无人机的姿态变化：
如果将这个球转动关节换为6自由度移动连接，并且恢复重力，就可以实现无人机在世界坐标系上移动了
由于姿态使用了欧拉角表示，因此会出现万向节死锁的问题，这是使用欧拉角的必然缺陷，本模型使用的旋转顺序为ZYX，因此再偏航角邻近±90度的时候会出现万向节死锁，如果使用了这个时候的欧拉角做控制，那么必然会崩溃。关于万向节死锁可以参看其他博客或者下面这个视频：
https://v.qq.com/x/cover/e055516g79w/e055516g79w.html
解决方法有两种：第一是更换欧拉角旋转顺序，可以使万向节死锁不发生在航角邻近±90度，但是还是会在其他角度出现；第二种是直接使用四元数，不转化为欧拉角。
