摘要: 首先给出刚体被控对象的微分方程,然后对四元数微分方程线性化求出合适的PD控制参数,然后详细分析了误差四元数的概念和性质,并提出四元数和旋转矩阵的等价性,然后简要介绍了非对角转动惯量矩阵的一些特点,最后分别仿真验证了调节问题、跟踪问题和误差四元数,附录中给出了使用拉塞尔不变性原理证明PD控制稳定性的过程。
被控对象建模
首先列出被控对象的微分方程。在此之前先提一下坐标系,常用的就两个,一个是固定于地面的世界系,另一个是固定在无人机(或称刚体)上的本体系。被控对象的微分方程如下
w
⃗
˙
=
J
−
1
(
M
⃗
−
w
⃗
×
J
w
⃗
)
Q
˙
=
1
2
Q
∘
[
0
,
w
⃗
]
(1)
\begin{aligned} & \dot{\vec w}=J^{-1}(\vec M-\vec w\times J\vec w) \\ & \dot Q=\frac 12 Q\circ [0,\vec w] \end{aligned} \tag{1}
w˙=J−1(M−w×Jw)Q˙=21Q∘[0,w](1)
上面两式分别称作刚体姿态动力学方程和运动学方程。第一行的动力学方程中,
J
J
J 是刚体在本体系下的转动惯量(注意不是世界系下,除了四元数是本体系相对于世界系以外,其它变量都是在本体系下描述),
w
⃗
\vec w
w 是刚体在本体系下的角速度向量,
M
⃗
\vec M
M 是刚体在本体系下所受的力矩向量。第二行的运动学方程中,
Q
Q
Q 是刚体在世界系下的姿态四元数,
∘
\circ
∘ 表示四元数乘法。这两个微分方程就描述了刚体从力矩到姿态的变化过程。这两个方程的进一步细节分析可以看我之前的推导笔记
姿态运动学:角速度变化时四元数和旋转矩阵微分方程的证明
姿态动力学:刚体姿态动力学推导与进动现象仿真
实际上(1)式或(2)式是航空航天中使用最广泛的建模公式,其它建模方法,像旋转矩阵、欧拉角、李代数、罗德里格斯参数什么的都不够简洁,可以看 为何三维旋转只有三个自由度却用四元数来表示?-找不到服务器的回答 -知乎 中我和其他人的回答。
线性化设计控制律
控制问题分为调节问题和跟踪问题。调节问题指的是,没有外部输入,设计一个状态反馈使系统稳定到平衡点;跟踪问题就是常见的使系统状态跟踪指令输入。严格的定义参考一下专业书。
设计PD控制律
M
⃗
=
−
k
p
ϵ
⃗
−
k
d
w
⃗
\vec M=-k_p\vec\epsilon-k_d\vec w
M=−kpϵ−kdw
使用拉塞尔不变集原理可以证明,四元数PD控制中取任意大于0的PD参数都可以让刚体从任何状态回到初始位置。我看完了证明过程仍然觉得这很不可思议,但前提是控制律也得是连续的,而实际中控制器都是离散的。证明过程贴到最后面。
尽管任意PD参数都可以保证稳定,但控制效果就不一定能保证了,下面计算合适的PD参数。把四元数微分方程拆开,四元数的标量和向量部分分别记作
η
\eta
η 和
υ
⃗
\vec\upsilon
υ,于是被控对象可以写作
η
˙
=
−
1
2
ϵ
⃗
T
w
⃗
ϵ
⃗
˙
=
1
2
(
η
w
⃗
+
ϵ
⃗
×
w
⃗
)
=
1
2
(
η
I
3
+
ϵ
⃗
×
)
w
⃗
w
⃗
˙
=
J
−
1
(
M
⃗
−
w
⃗
×
J
w
⃗
)
(2)
\begin{aligned} & \dot\eta=-\frac 12\vec\epsilon^\text T\vec w \\ & \dot{\vec\epsilon} = \frac 12(\eta\vec w+\vec\epsilon\times\vec w) =\frac 12(\eta I_3+\vec\epsilon^\times)\vec w \\ & \dot{\vec w}=J^{-1}(\vec M-\vec w\times J\vec w) \\ \end{aligned} \tag{2}
η˙=−21ϵTwϵ˙=21(ηw+ϵ×w)=21(ηI3+ϵ×)ww˙=J−1(M−w×Jw)(2)
平衡点为
w
⃗
=
0
⃗
,
Q
=
[
1
,
0
,
0
,
0
]
\vec w=\vec 0,Q=[1,0,0,0]
w=0,Q=[1,0,0,0],
η
=
1
,
ϵ
=
0
\eta=1,\epsilon=0
η=1,ϵ=0,在平衡点附近线性化
η
˙
≈
0
,
η
≈
1
ϵ
⃗
˙
≈
1
2
w
⃗
\begin{aligned} & \dot\eta\approx 0,\eta\approx 1 \\ & \dot{\vec\epsilon} \approx \frac 12\vec w \\ \end{aligned}
η˙≈0,η≈1ϵ˙≈21w
因为
ϵ
˙
\dot\epsilon
ϵ˙ 中不含输入力矩,所以对
ϵ
˙
\dot\epsilon
ϵ˙ 再次求导并忽略高阶项
ϵ
⃗
¨
≈
1
2
J
−
1
(
−
w
⃗
×
J
w
⃗
−
k
p
ϵ
⃗
−
k
d
w
⃗
)
≈
1
2
J
−
1
(
−
k
p
ϵ
⃗
−
k
d
2
ϵ
⃗
˙
)
=
−
k
d
J
−
1
ϵ
⃗
˙
−
k
p
2
J
−
1
ϵ
⃗
\begin{aligned} \ddot{\vec\epsilon} \approx& \frac 12 J^{-1} (-\vec w\times J\vec w-k_p\vec\epsilon-k_d\vec w) \\ \approx& \frac 12 J^{-1}(-k_p\vec\epsilon-k_d2\dot{\vec\epsilon}) \\ =& -k_d J^{-1}\dot{\vec\epsilon} -\frac{k_p}2 J^{-1}\vec\epsilon \end{aligned}
ϵ¨≈≈=21J−1(−w×Jw−kpϵ−kdw)21J−1(−kpϵ−kd2ϵ˙)−kdJ−1ϵ˙−2kpJ−1ϵ
如果
J
J
J 为对角阵,那么向量形式可以简单地拆开成三个轴分别设计参数
ϵ
¨
x
=
−
k
d
x
J
x
ϵ
˙
x
−
k
p
x
2
J
x
ϵ
x
ϵ
¨
y
=
−
k
d
y
J
y
ϵ
˙
y
−
k
p
y
2
J
y
ϵ
y
ϵ
¨
z
=
−
k
d
z
J
z
ϵ
˙
z
−
k
p
z
2
J
z
ϵ
z
\begin{aligned} &\ddot\epsilon_x=-\frac{k_{dx}}{J_x}\dot\epsilon_x-\frac{k_{px}}{2J_x}\epsilon_x \\ &\ddot\epsilon_y=-\frac{k_{dy}}{J_y}\dot\epsilon_y-\frac{k_{py}}{2J_y}\epsilon_y \\ &\ddot\epsilon_z=-\frac{k_{dz}}{J_z}\dot\epsilon_z-\frac{k_{pz}}{2J_z}\epsilon_z \\ \end{aligned}
ϵ¨x=−Jxkdxϵ˙x−2Jxkpxϵxϵ¨y=−Jykdyϵ˙y−2Jykpyϵyϵ¨z=−Jzkdzϵ˙z−2Jzkpzϵz
即可根据极点配置法分别确定3轴PD参数。
跟踪问题
前面设计了调节问题的控制律参数,下面研究跟踪问题。跟踪问题需要引入误差四元数的概念,可以看下面这篇论文
E. Fresk and G. Nikolakopoulos, “Full quaternion based attitude control for a quadrotor,” 2013 European Control Conference (ECC), Zurich, Switzerland, 2013, pp. 3864-3869, doi: 10.23919/ECC.2013.6669617.
定义误差四元数为
Q
e
=
Q
0
∘
Q
−
1
Q_e=Q_0\circ Q^{-1}
Qe=Q0∘Q−1(这个好像不对,等我后面确认一下),其中
Q
0
Q_0
Q0 为期望姿态四元数,
Q
Q
Q 是当前姿态四元数,然后利用四元数的性质,四元数的虚部是旋转轴和旋转角度(具体见 如何形象地理解四元数?-知乎 或其他类似博客),利用姿态误差和角速度设计PD控制
M
⃗
=
k
p
[
q
1
q
2
q
3
]
−
k
d
[
w
x
w
y
w
z
]
(3)
\vec M=k_p \begin{bmatrix} q_1 \\ q_2 \\ q_3 \end{bmatrix} - k_d \begin{bmatrix} w_x \\ w_y \\ w_z \end{bmatrix} \tag{3}
M=kp
q1q2q3
−kd
wxwywz
(3)
其中
w
x
,
w
y
,
w
z
w_x, w_y, w_z
wx,wy,wz 为三轴角速度分量,
q
1
,
q
2
,
q
3
q_1, q_2, q_3
q1,q2,q3 为误差四元数的虚部。根据转动惯量矩阵的实际值,三个轴的PD参数
k
p
,
k
d
k_p,k_d
kp,kd 可以不同。
跟踪问题中误差四元数的原理
再深究一下误差四元数的定义,误差四元数为什么是
Q
e
=
Q
0
∘
Q
−
1
Q_e=Q_0\circ Q^{-1}
Qe=Q0∘Q−1
而不是下面这3种?
Q
e
=
Q
∘
Q
0
−
1
Q
e
=
Q
−
1
∘
Q
0
Q
e
=
Q
0
−
1
∘
Q
(4)
\begin{aligned} & Q_e=Q\circ Q_0^{-1} \\ & Q_e=Q^{-1}\circ Q_0 \\ & Q_e=Q_0^{-1}\circ Q \\ \end{aligned} \tag{4}
Qe=Q∘Q0−1Qe=Q−1∘Q0Qe=Q0−1∘Q(4)
定义函数
R
(
Q
)
R(Q)
R(Q) 表示把一个四元数转换成旋转矩阵,有下面一些性质
R
(
Q
−
1
)
=
[
R
(
Q
)
]
−
1
R
(
Q
1
)
R
(
Q
2
)
=
R
(
Q
1
∘
Q
2
)
R(Q^{-1})=[R(Q)]^{-1} \\ R(Q_1)R(Q_2)=R(Q_1\circ Q_2)
R(Q−1)=[R(Q)]−1R(Q1)R(Q2)=R(Q1∘Q2)
这个式子的证明见 证明四元数乘法与旋转矩阵乘法等价
这里先补充一下旋转矩阵的物理含义。首先定义两个姿态,零姿态指的是旋转矩阵为单位阵的初始姿态,与刚体固连的本体系与世界系重合;当前姿态指的是刚体转过一个角度后的姿态本体系与世界系不重合。
- 旋转,对向量 r ⃗ \vec r r 左乘旋转矩阵可以将 r ⃗ \vec r r 从一个位置旋转到另一个位置,例如将世界系的x轴左乘旋转矩阵就与本体系的x轴重合;
- 坐标系变换,对向量 r ⃗ \vec r r 左乘旋转矩阵可以将 r ⃗ \vec r r 从本体系转换到世界系;
- 姿态描述,一个刚体的姿态旋转矩阵为 R R R 的含义是,与刚体固连的本体系的XYZ三个坐标轴在世界系下的坐标分别对应旋转矩阵的3列元素。
这3条含义可能很多人都不完全知道,知道这些以后就会发现旋转矩阵的作用非常大。
问题就成了,怎样把当前的姿态旋转矩阵旋转到期望旋转矩阵。前面在线性化的时候把平衡点选到了
Q
=
[
1
,
0
,
0
,
0
]
Q=[1,0,0,0]
Q=[1,0,0,0] 处(后面称这个姿态为"零姿态"),如果期望姿态不在这里,那就在期望姿态处新建一个坐标系,称作期望系,使期望姿态为零姿态,然后误差旋转矩阵就指的是当前姿态在期望系下的描述。此时,期望系到世界系的旋转矩阵为
R
0
R_0
R0,本体系到世界系的旋转矩阵为
R
R
R,那么本体系到期望系的旋转矩阵就是
R
0
−
1
R
R_0^{-1}R
R0−1R,意思是,一个在本体系下描述的向量
r
⃗
\vec r
r 左乘误差旋转矩阵后就成为在期望系下描述的向量
R
0
−
1
R
r
⃗
R_0^{-1}R\vec r
R0−1Rr。因为误差四元数式(3)中自带负号,相当于旋转矩阵求逆,所以等价的误差旋转矩阵为
R
e
=
R
−
1
R
0
R_e=R^{-1}R_0
Re=R−1R0,对应的误差四元数为
Q
e
=
Q
−
1
∘
Q
0
Q_e=Q^{-1}\circ Q_0
Qe=Q−1∘Q0
除此以外还有另外一种理解方式。已知,对一个旋转矩阵左乘旋转矩阵相当于把刚体绕世界系旋转,右乘相当于绕本体系旋转(具体解释可以看我之前写的笔记 自用的四元数、欧拉角、旋转矩阵笔记 的最后一节“xyz固定角和zyx欧拉角相等的直观理解”)而力矩是在本体系下描述的,所以需要右乘,将本体系相对于世界系的旋转矩阵
R
R
R 右乘误差旋转矩阵
R
e
R_e
Re 得到期望旋转矩阵
R
0
R_0
R0,于是
R
R
e
=
R
0
RR_e=R_0
RRe=R0,
R
e
=
R
−
1
R
0
R_e=R^{-1}R_0
Re=R−1R0,也可以得到等价的误差四元数。
至于常见的误差四元数为什么是
R
e
=
R
0
R
−
1
R_e=R_0R^{-1}
Re=R0R−1 的形式,原因也很简单,如果受到的力矩是在世界系下描述的,那就把刚体左乘旋转矩阵,误差旋转矩阵就变成了
R
e
R
=
R
0
R_eR=R_0
ReR=R0。
实际仿真中分别试了4种情况,分别为正确的误差四元数和式(4)中表示的另外3种情况,发现都能达到期望姿态,但是正确的误差四元数的过渡过程是最快的。仿真代码和结果见下文“跟踪问题仿真”一节。
大概查了一些论文,4种误差四元数表示在各种论文中查到了3种,上一节 Fresk 的论文记作第一种,式(4)中的论文分别记作2,3,4种。
下面两篇论文的式(10)和(34)使用的是第2种:
Bani Younes, Ahmad & Turner, James & Mortari, D. & Junkins, John. (2012). A Survey of Attitude Error Representations. AIAA/AAS Astrodynamics Specialist Conference 2012. 10.2514/6.2012-4422.
Bani Younes, J. Turner, M. Majji, and J. Junkins. An investigation of state feedback gain sensitivity calculations.
Number AIAA-2010-8274, Toronto, Ontario, Canada, 5 August 2010. Presented to AIAA/AAS Astrodynamics Specialist Conf.
下面论文的式(27)使用的是第3种(新必应因为某些原因搜出来了日语文献,不过也能大概看懂):
Takahiro Sasaki and Takashi Shimomura. Attitude Control of a Spacecraft with RWs and a Study on the Error Quaternion: ―Convergent Range and Control Performance with Incorrect Denition of Error. 誤った定義を用いたときの収束範囲と制御性能. 2015. 10.5687/ISCIE.28.127
非对角矩阵
转动惯量矩阵是正定矩阵,证明见 Positive Definiteness of the Moment of Inertia Tensor 当转动惯量矩阵不是对角阵时,需要取线性变换转换成对角阵。取
α
⃗
=
C
ϵ
⃗
\vec\alpha=C\vec\epsilon
α=Cϵ,于是
ϵ
⃗
¨
≈
−
k
d
J
−
1
ϵ
⃗
˙
−
k
p
2
J
−
1
ϵ
⃗
\ddot{\vec\epsilon} \approx -k_d J^{-1}\dot{\vec\epsilon} -\frac{k_p}2 J^{-1}\vec\epsilon
ϵ¨≈−kdJ−1ϵ˙−2kpJ−1ϵ
变为
C
−
1
α
⃗
¨
≈
−
k
d
J
−
1
C
−
1
α
⃗
˙
−
k
p
2
J
−
1
C
−
1
α
⃗
α
⃗
¨
≈
−
k
d
C
J
−
1
C
−
1
α
⃗
˙
−
k
p
2
C
J
−
1
C
−
1
α
⃗
C^{-1}\ddot{\vec\alpha} \approx -k_d J^{-1}C^{-1}\dot{\vec\alpha} -\frac{k_p}2 J^{-1}C^{-1}\vec\alpha \\ \ddot{\vec\alpha} \approx -k_d CJ^{-1}C^{-1}\dot{\vec\alpha} -\frac{k_p}2 CJ^{-1}C^{-1}\vec\alpha \\
C−1α¨≈−kdJ−1C−1α˙−2kpJ−1C−1αα¨≈−kdCJ−1C−1α˙−2kpCJ−1C−1α
取合同变换矩阵
C
C
C 使
C
J
−
1
C
−
1
CJ^{-1}C^{-1}
CJ−1C−1 为对角阵。合同变换结果不唯一,
C
C
C 的取值既影响
k
p
,
k
d
k_p,k_d
kp,kd 参数,也通过
α
⃗
\vec\alpha
α 影响
ϵ
⃗
\vec\epsilon
ϵ 的稳定性。感觉
C
C
C 的取值类似于选择合适的传递函数零点,虽然不影响稳定性,但影响收敛速度。
合同变换方法见 请问求二次型的标准型的三种方法?
调节问题仿真
被控对象如下。其中 quaternions.py
的代码见 自用的四元数、欧拉角、旋转矩阵转换代码。
这个被控对象也在后面的仿真中使用。
# rigidbody.py
# 四阶龙格库塔法建立刚体的运动学和动力学模型
import numpy as np
from quaternions import *
# 默认控制律,没有期望姿态
def Control_Law(state):
Qv, W = state[1:4], state[4:7]
return -np.array([2, 20, 6])*Qv -np.array([2, 20, 6])*W
# 刚体
# J: 刚体的转动惯量矩阵
# initEuler: 刚体的初始欧拉角
# initOmega: 刚体的初始角速度向量
class RigidBody:
def __init__(self, J, initEuler, initOmega):
self._J = J
e1 = initEuler
w1 = initOmega
self._t = 0
self._Jinv = np.linalg.inv(self._J)
q1 = Euler_To_Quaternion(e1)
self._states = np.concatenate((q1, w1), dtype=float)
self.ctrlLaw = Control_Law # 默认控制律
def Simulate_OneStep(self):
h = 0.01
K1 = self._ODE4Function(self._t, self._states)
K2 = self._ODE4Function(self._t+h/2, self._states + h/2*K1)
K3 = self._ODE4Function(self._t+h/2, self._states + h/2*K2)
K4 = self._ODE4Function(self._t+h, self._states + h*K3)
dx = h/6*(K1 + 2*K2 + 2*K3 + K4)
self._states += dx
def Get_State(self):
return self._states
def Get_QuaternionVec(self):
return Quaternion_to_Euler(self._states[0:4])
def _ODE4Function(self, t, x):
Qs, Qv, W = x[0], x[1:4], x[4:7]
dQs = -0.5*np.dot(Qv, W)
dQv = 0.5*(Qs*W + np.cross(Qv, W))
torque = self.ctrlLaw(x)
dW = self._Jinv @ (torque - np.cross(W, self._J @ W))[0]
return np.concatenate((np.array([dQs]), dQv, np.array(dW)[0]))
主程序代码如下。
# main.py
import numpy as np
import matplotlib.pyplot as plt
from rigidbody import RigidBody
J = np.matrix([
[1, 0, 0],
[0, 10, 0],
[0, 0, 3],
])
usv1 = RigidBody(J, np.array([1, -0.2, 0.3]), np.array([-0.1, 0.2, -3]))
t = 0
plottime, ploteuler = [], []
plottime.append(t)
ploteuler.append(usv1.Get_QuaternionVec())
while 1:
for n in range(10):
usv1.Simulate_OneStep()
t += 0.1
plottime.append(t)
ploteuler.append(usv1.Get_QuaternionVec())
if t > 10:
break
pass
print(ploteuler[-1])
plt.plot(plottime, ploteuler)
plt.legend(['roll', 'pitch', 'yaw'])
plt.show()
仿真结果如下
跟踪问题仿真
期望姿态角分别为 − 1 , 0.5 , − 0.2 -1, 0.5, -0.2 −1,0.5,−0.2。
import numpy as np
import matplotlib.pyplot as plt
from quaternions import *
from rigidbody import RigidBody
def Control_Law(state):
Qcurrent, omega = state[0:4], state[4:7]
Qerr = Quaternion_Product(Quaternion_Inverse(Qcurrent), Qtarget) # +
# Qerr = Quaternion_Product(Quaternion_Inverse(Qtarget), Qcurrent) # -
# Qerr = Quaternion_Product(Qtarget, Quaternion_Inverse(Qcurrent)) # x
# Qerr = Quaternion_Product(Qcurrent, Quaternion_Inverse(Qtarget)) # x
torque = np.array([2, 20, 6])*Qerr[1:4] -np.array([2, 20, 6])*omega
return torque
J = np.matrix([
[1, 0, 0],
[0, 10, 0],
[0, 0, 3],
])
Qtarget = Euler_To_Quaternion(np.array([-1, 0.5, -0.2]))
usv1 = RigidBody(J, np.array([0, 0, 0]), np.array([-0.1, 0.2, -3]))
usv1.ctrlLaw = Control_Law
t = 0
plottime, ploteuler = [], []
plottime.append(t)
ploteuler.append(usv1.Get_QuaternionVec())
while 1:
for n in range(10):
usv1.Simulate_OneStep()
t += 0.1
plottime.append(t)
ploteuler.append(usv1.Get_QuaternionVec())
if t > 20:
break
pass
print(ploteuler[-1])
plt.plot(plottime, ploteuler)
plt.legend(['roll', 'pitch', 'yaw'])
plt.show()
其中控制律代码中列出了4种误差四元数,后面分别有+-xx
的注释,+
表示控制律为
torque = np.array([2, 20, 6])*Qerr[1:4] -np.array([2, 20, 6])*omega
-
表示控制律为
torque = -np.array([2, 20, 6])*Qerr[1:4] -np.array([2, 20, 6])*omega
x
表示结果不正确,但也能逐渐过渡到期望姿态。标+-
两种情况的仿真结果相同,如下图所示
标xx
的两种情况分别为
并且同样的原理,改了控制律中-np.array([2, 20, 6])
前面的正负号后,上面两种结果交换顺序后完全相同。
附录
这一节讲下面这本书的第24章里介绍的方法,使用拉塞尔不变集原理证明PD控制稳定性。
De Ruiter. Spacecraft dynamics and control: an introduction. 2013.
拉塞尔不变集原理
一个多输入多输出非线性系统
x
⃗
˙
=
f
⃗
(
x
⃗
)
\dot{\vec x}=\vec f(\vec x)
x˙=f(x)
定义
x
x
x 的解为集合
D
D
D,再定义一个
x
x
x 的有界闭集
Ω
\Omega
Ω 为当
t
≥
0
t\geq 0
t≥0 时,
x
(
t
)
∈
Ω
x(t)\in\Omega
x(t)∈Ω。令
V
(
x
⃗
)
:
D
→
R
V(\vec x):D\to\mathbb R
V(x):D→R 为一个连续可微函数并满足
V
˙
≤
0
\dot V\leq 0
V˙≤0,定义集合
E
E
E 为
V
˙
(
x
⃗
)
=
0
\dot V(\vec x)=0
V˙(x)=0 时
x
⃗
\vec x
x 的集合。令
M
M
M 为
E
E
E 中的最大不变集,那么当
t
→
∞
t\to\infty
t→∞ 时,
x
⃗
(
t
)
→
M
\vec x(t)\to M
x(t)→M。
运用拉塞尔不变集原理的步骤为
- 构造能量函数V,可以不是正定函数。
- 求能量函数的时间一阶导数,判断是否 V ˙ ( x ) ≤ 0 \dot V(x)\leq 0 V˙(x)≤0。如果不满足,需要重新构造。
- 令 V ˙ = 0 \dot V=0 V˙=0,求子集 E E E。
- 判断子集 E E E 中的不变集,找出最大的不变集 M M M。
关于拉塞尔不变集更详细的解释建议参考khaill的《非线性系统》。
拉塞尔不变集原理解读(包含径向无界性的解读)
关于LaSalle不变集定理的一个问题
非线性与自适应控制(三)LaSalle Invariance Principle
关于开集、闭集、紧集可以参考
【最优化】几何概念概述
小白拓补学|4. 究竟什么是紧集(compact set)?
证明PD控制稳定性
重写(2)式如下
η
˙
=
−
1
2
ϵ
⃗
T
w
⃗
ϵ
⃗
˙
=
1
2
(
η
w
⃗
+
ϵ
⃗
×
w
⃗
)
=
1
2
(
η
I
3
+
ϵ
⃗
×
)
w
⃗
w
⃗
˙
=
J
−
1
(
M
⃗
−
w
⃗
×
J
w
⃗
)
(2)
\begin{aligned} & \dot\eta=-\frac 12\vec\epsilon^\text T\vec w \\ & \dot{\vec\epsilon} = \frac 12(\eta\vec w+\vec\epsilon\times\vec w) =\frac 12(\eta I_3+\vec\epsilon^\times)\vec w \\ & \dot{\vec w}=J^{-1}(\vec M-\vec w\times J\vec w) \\ \end{aligned} \tag{2}
η˙=−21ϵTwϵ˙=21(ηw+ϵ×w)=21(ηI3+ϵ×)ww˙=J−1(M−w×Jw)(2)
设计控制律
M
⃗
=
−
k
p
ϵ
⃗
−
k
d
w
⃗
\vec M=-k_p\vec\epsilon-k_d\vec w
M=−kpϵ−kdw
代入姿态动力学方程得到
w
⃗
˙
=
J
−
1
(
−
w
⃗
×
J
w
⃗
−
k
p
ϵ
⃗
−
k
d
w
⃗
)
\dot{\vec w}=J^{-1}(-\vec w\times J\vec w-k_p\vec\epsilon-k_d\vec w)
w˙=J−1(−w×Jw−kpϵ−kdw)
考虑能量函数
V
(
η
,
ϵ
⃗
,
w
⃗
)
=
1
2
w
⃗
T
J
w
⃗
−
2
k
p
η
V(\eta,\vec\epsilon,\vec w)=\frac 12\vec w^\text TJ\vec w-2k_p\eta
V(η,ϵ,w)=21wTJw−2kpη
求导
V
˙
=
w
⃗
T
J
w
⃗
˙
−
2
k
p
η
˙
V
˙
=
w
⃗
T
(
−
w
⃗
×
J
w
⃗
−
k
p
ϵ
⃗
−
k
d
w
⃗
)
+
k
p
ϵ
⃗
T
w
⃗
=
−
k
d
w
⃗
T
w
⃗
≤
0
\begin{aligned} \dot V =& \vec w^\text TJ\dot{\vec w}-2k_p\dot\eta \\ \dot V =& \vec w^\text T(-\vec w\times J\vec w-k_p\vec\epsilon-k_d\vec w) +k_p\vec\epsilon^\text T\vec w \\ =& -k_d\vec w^\text T\vec w \leq 0 \end{aligned}
V˙=V˙==wTJw˙−2kpη˙wT(−w×Jw−kpϵ−kdw)+kpϵTw−kdwTw≤0
然后求不变集。因为单位四元数满足
∣
η
∣
≤
1
,
∥
ϵ
⃗
∥
≤
1
|\eta|\leq 1,\|\vec\epsilon\|\leq 1
∣η∣≤1,∥ϵ∥≤1
所以
1
2
w
⃗
T
J
w
⃗
−
2
k
p
≤
V
(
η
,
ϵ
⃗
,
w
⃗
)
\frac 12\vec w^\text TJ\vec w-2k_p\leq V(\eta,\vec\epsilon,\vec w)
21wTJw−2kp≤V(η,ϵ,w)
假设转动惯量矩阵的3个主惯量轴满足
J
1
≤
J
2
≤
J
3
J_1\leq J_2\leq J_3
J1≤J2≤J3
于是
1
2
(
J
1
w
x
2
+
J
2
w
y
2
+
J
3
w
z
2
)
−
2
k
p
≤
V
(
η
,
ϵ
⃗
,
w
⃗
)
\frac 12(J_1w_x^2+J_2w_y^2+J_3w_z^2)-2k_p\leq V(\eta,\vec\epsilon,\vec w)
21(J1wx2+J2wy2+J3wz2)−2kp≤V(η,ϵ,w)
1
2
J
1
∥
w
⃗
∥
2
=
1
2
(
J
1
w
x
2
+
J
1
w
y
2
+
J
1
w
z
2
)
≤
1
2
(
J
1
w
x
2
+
J
2
w
y
2
+
J
3
w
z
2
)
≤
V
(
η
,
ϵ
⃗
,
w
⃗
)
+
2
k
p
\frac 12J_1\|\vec w\|^2 = \frac 12(J_1w_x^2+J_1w_y^2+J_1w_z^2) \\ \leq \frac 12(J_1w_x^2+J_2w_y^2+J_3w_z^2) \leq V(\eta,\vec\epsilon,\vec w)+2k_p \\
21J1∥w∥2=21(J1wx2+J1wy2+J1wz2)≤21(J1wx2+J2wy2+J3wz2)≤V(η,ϵ,w)+2kp
∥
w
⃗
∥
≤
2
V
+
4
k
p
J
1
≤
2
V
0
+
4
k
p
J
1
\|\vec w\|\leq\sqrt\frac{2V+4k_p}{J_1}\leq\sqrt\frac{2V_0+4k_p}{J_1}
∥w∥≤J12V+4kp≤J12V0+4kp
其中
V
0
=
V
(
η
(
0
)
,
ϵ
⃗
(
0
)
,
w
⃗
(
0
)
)
V_0=V(\eta(0),\vec\epsilon(0),\vec w(0))
V0=V(η(0),ϵ(0),w(0)),于是
Ω
=
{
(
η
,
ϵ
⃗
,
w
⃗
)
∣
∣
η
∣
≤
1
,
∥
ϵ
⃗
∥
≤
1
,
∥
w
⃗
∥
≤
2
V
0
+
4
k
p
J
1
}
\Omega=\Big\{(\eta,\vec\epsilon,\vec w)\Big| |\eta|\leq 1,\|\vec\epsilon\|\leq 1, \|\vec w\|\leq\sqrt\frac{2V_0+4k_p}{J_1}\Big\}
Ω={(η,ϵ,w)
∣η∣≤1,∥ϵ∥≤1,∥w∥≤J12V0+4kp}
Ω
\Omega
Ω 中的子集
E
E
E 为
V
˙
=
0
\dot V=0
V˙=0 时
x
⃗
\vec x
x 的集合,即
w
⃗
=
0
\vec w=0
w=0
E
=
{
(
η
,
ϵ
⃗
,
w
⃗
)
∈
Ω
∣
∣
η
∣
≤
1
,
∥
ϵ
⃗
∥
≤
1
,
w
⃗
=
0
}
E=\Big\{(\eta,\vec\epsilon,\vec w)\in\Omega\Big| |\eta|\leq 1,\|\vec\epsilon\|\leq 1,\vec w=0\Big\}
E={(η,ϵ,w)∈Ω
∣η∣≤1,∥ϵ∥≤1,w=0}
下面求最大不变集,将
w
⃗
=
0
\vec w=0
w=0 代入被控对象式(A.1)得
η
˙
=
0
ϵ
⃗
˙
=
0
w
⃗
˙
=
J
−
1
M
⃗
=
−
k
p
J
−
1
ϵ
⃗
\begin{aligned} & \dot\eta=0 \\ & \dot{\vec\epsilon}=0 \\ & \dot{\vec w}=J^{-1}\vec M=-k_pJ^{-1}\vec\epsilon \\ \end{aligned}
η˙=0ϵ˙=0w˙=J−1M=−kpJ−1ϵ
因为
w
⃗
=
0
\vec w=0
w=0,所以
w
⃗
˙
=
0
,
ϵ
⃗
=
0
\dot{\vec w}=0,\vec\epsilon=0
w˙=0,ϵ=0,由单位四元数的性质得
η
=
±
1
\eta=\pm 1
η=±1,所以最大不变集
M
=
{
(
η
,
ϵ
⃗
,
w
⃗
)
∈
Ω
∣
∣
η
∣
=
±
1
,
∥
ϵ
⃗
∥
=
0
,
w
⃗
=
0
}
M=\Big\{(\eta,\vec\epsilon,\vec w)\in\Omega\Big| |\eta|=\pm 1,\|\vec\epsilon\|=0,\vec w=0\Big\}
M={(η,ϵ,w)∈Ω
∣η∣=±1,∥ϵ∥=0,w=0}
这就是刚体最后稳定的状态。
前面的推导中假设了转动惯量矩阵是对角阵,如果不是对角阵的话,实际的系统的转动惯量矩阵一定是正定矩阵,那么可以经过线性变换转换成正定的对角阵,也可以类似得到
w
⃗
\vec w
w 的范围。具体细节有待进一步推导。