四元数在惯性导航系统中的应用课件

Lecture11--Quaternion1ReviewComputingavehicle’sattitudebysolvingwhereusingPeano-Bakersolutionorusingnumericalintegration.Lecture11--Quaternion1RevieLecture11--Quaternion2QuaternionsinSINS四元数在惯性导航系统中的应用Lecture11--Quaternion2QuateLecture11--Quaternion3Outline四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解Lecture11--Quaternion3OutliLecture11--Quaternion41.0Hamilton和四元数四元数:描述刚体的转动(byHamilton)理论上的突破--1843.10.162005朝拜之旅“Hereashewalkedbyonthe16thofOctober1843,SirWilliamRowanHamiltoninaflashofgeniusdiscoveredthefundamentalformulaforquaternionmultiplicationcarvedonastoneonthebridge”在捷联惯性导航及图像处理中应用的优势Lecture11--Quaternion41.0HLecture11--Quaternion5n1.1*四元数(quaternions)定义一个有固定点的刚体通过绕该点的某个轴转过特定角度可达到任何姿态转轴的方向可以表示成一个单位矢量:则描述该转动的四元数可以表示成:四元数既反映了转动的方向又反映了转动的幅值.Lecture11--Quaternion5n1.1*Lecture11--Quaternion61.2四元数的组成四元数的表示：λ-----标量部分----矢量部分包括一个实数单位1

和三个虚数单位i,j,k

另一种表示法:,P

代表矢量部分Lecture11--Quaternion61.2四Lecture11--Quaternion7Outline四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解Lecture11--Quaternion7OutliLecture11--Quaternion82.1*加法和减法加法和减法：或简写成：Lecture11--Quaternion82.1*加Lecture11--Quaternion92.2虚数单位的乘法规则i,j,k在乘法运算中的规则:对比Hamilton的公式Lecture11--Quaternion92.2虚Lecture11--Quaternion102.3*四元数乘法或简单地表示成：Lecture11--Quaternion102.3*Lecture11--Quaternion112.3四元数乘法自定义函数function[q1]=qmul(q,m)lm=q(1);p1=q(2);p2=q(3);p3=q(4);q1=[lm-p1-p2-p3p1lm-p3p2p2p3lm-p1p3-p2p1lm]*m;>>a=[1223]';>>b=[2423]';>>q=qmul(a,b)q=-0.77960.32820.49240.2052>>Lecture11--Quaternion112.3Lecture11--Quaternion122.3*四元数乘法表示符号※四元数乘法的符号※关于交换率和结合律Lecture11--Quaternion122.3*Lecture11--Quaternion132.4*共扼和范数共扼四元数的定义------两个四元数的标量部分相同，向量部分相反q

和q*彼此互为四元数.可以证明：四元数的范数---定义成,则q

成为规范化的四元数若是规范化的Lecture11--Quaternion132.4*Lecture11--Quaternion142.5*四元数的逆和除法若则q1

和q2

彼此互为逆,写为和因为除法:没有具体意义或function[qi]=qinv(q)%inverseofquaternionqn=norm(q);q(2:4)=-q(2:4);qi=q/qn^2;Lecture11--Quaternion142.5*Lecture11--Quaternion15Outline四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解Lecture11--Quaternion15OutlLecture11--Quaternion163.1*矢量的旋转如果矢量R

相对固定坐标系旋转,并且该旋转可以用四元数q

描述，新矢量记为R’，则R

和R’

之间的变换可以表示成下述四元数运算:含义:矢量R

相对固定坐标系旋转，旋转的角度和轴向由q决定上述运算中,R

被当成一个标量部分为零的四元数，即：Lecture11--Quaternion163.1*Lecture11--Quaternion173.2*坐标系的旋转一个矢量V

相对于坐标系OXYZ

固定:从坐标系OXYZ

转动了q,得到一个新坐标系OX’Y’Z’.V

分解在新坐标系OX’Y’Z’中矢量V

在两个坐标系之间的坐标变换:记：则分别称为V

在两个坐标系中的映像.和Lecture11--Quaternion173.2*Lecture11--Quaternion183.3四元数和方向余弦----表示坐标系旋转,其中q

和C之间是什么关系?假设则应用四元数乘法,得到Lecture11--Quaternion183.3Lecture11--Quaternion193.3四元数和方向余弦方向余弦矩阵Lecture11--Quaternion193.3Lecture11--Quaternion203.4四元数转动变换的两种形式如果一个矢量V固定，坐标系旋转按照四元数q

进行了旋转，得到了一个新坐标系，则该矢量分别在新旧坐标系中投影表达式间的关系借助映像方式可以表示为：如果一个坐标系固定,一个矢量VE

按照四元数q相对该坐标系进行了转动，得到一个新的矢量VE’，则新旧矢量之间的关系为：Lecture11--Quaternion203.4Lecture11--Quaternion21Outline四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解Lecture11--Quaternion21OutlLecture11--Quaternion224.0*转动四元数的合成连续的多次转动可以等效成一次转动.假设四元数q1和q2

分别代表第一次和第二次坐标系旋转.q1q2则合成后的转动四元数q

可以表示成:q其中q1

和q2

的轴必须表示成映像形式.若q1

和q2

的轴都表示在原来坐标系中,则Lecture11--Quaternion224.0*Lecture11--Quaternion234.1四元数合成例子:非映像方式坐标系顺序旋转情况下四元数的合成坐标系OX’Y’Z’

相对坐标系OXYZ

多次旋转首先,绕Z

轴转过角度ψ,瞬时转轴n

和k

轴重合,则Lecture11--Quaternion234.1四Lecture11--Quaternion24第二次旋转:绕X’

转过角度θ,旋转轴n

表示为:4.1四元数合成:非映像方式对应的四元数:Lecture11--Quaternion24第二次旋Lecture11--Quaternion254.1四元数合成:映像的方式这里q1

和q2

的转动轴表示为非映像的形式,因此合成的四元数为:转动次数越多，合成后四元数的表达式会愈发复杂Lecture11--Quaternion254.1Lecture11--Quaternion264.2*四元数合成:映像的方式每次转动的瞬时转轴都以映像方式给出.对第一次转动,瞬时转轴n1

的映像方式和非映像方式相同：因此Lecture11--Quaternion264.2*Lecture11--Quaternion274.2四元数合成:映像的方式第二次转动绕着OX’

转过了θ转轴n2

沿着OX’n2

在坐标系X’Y’Z’

中的映像为：因此q2

的映像形式为Lecture11--Quaternion274.2Lecture11--Quaternion284.2四元数合成:映像的方式第三次转动绕着OY’

轴转过φ.转轴n3

沿着OY’

轴OY’

是由原来坐标系的OY

轴转动得到的，因此n3

的映像形式为:这样，四元数q3

的映像形式为Lecture11--Quaternion284.2Lecture11--Quaternion294.2四元数合成:映像的方式因为q1,q2

和q3

都表示成了映像的形式,所以合成的四元数q

的计算公式为:由q

可以进一步得到合成转动对应的方向余弦矩阵Lecture11--Quaternion294.2Lecture11--Quaternion30Outline四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解Lecture11--Quaternion30OutlLecture11--Quaternion315.0*姿态解算的四元数微分方程对SINS，如用方向余弦矩阵,微分方程为如果用四元数,其微分方程为其中q

为描述载体转动的四元数,ω

为载体相对导航参考坐标系的角速度,也表示为四元数的形式:则:或------只包含四个一阶微分方程Lecture11--Quaternion315.0*Lecture11--Quaternion325.1一阶Runge-Kutta算法假设捷联惯导的“数学平台”跟踪地理坐标系,则四元数微分方程及简化利用Runge-Kutta数值积分算法一阶Runge-Kutta算法的解,从时刻t

到t+T:Lecture11--Quaternion325.1一Lecture11--Quaternion335.1二阶Runge-Kutta算法其二阶Runge-Kutta算法的解为:对于四元数微分方程:Lecture11--Quaternion335.1Lecture11--Quaternion345.1*四阶Runge-Kutta算法对于四元数微分方程Lecture11--Quaternion345.1*Lecture11--Quaternion35对于5.2*角增量算法其Peano-Baker解为:其中或写成迭代的形式:Lecture11--Quaternion35对于5.Lecture11--Quaternion365.2角增量算法一阶增量算法:Lecture11--Quaternion365.2Lecture11--Quaternion375.2二阶到四阶增量算法2nd:3rd:4th:Lecture11--Quaternion375.2Lecture11--Quaternion38ThanksLecture11--Quaternion38ThanLecture11--Quaternion39ReviewComputingavehicle’sattitudebysolvingwhereusingPeano-Bakersolutionorusingnumericalintegration.Lecture11--Quaternion1RevieLecture11--Quaternion40QuaternionsinSINS四元数在惯性导航系统中的应用Lecture11--Quaternion2QuateLecture11--Quaternion41Outline四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解Lecture11--Quaternion3OutliLecture11--Quaternion421.0Hamilton和四元数四元数:描述刚体的转动(byHamilton)理论上的突破--1843.10.162005朝拜之旅“Hereashewalkedbyonthe16thofOctober1843,SirWilliamRowanHamiltoninaflashofgeniusdiscoveredthefundamentalformulaforquaternionmultiplicationcarvedonastoneonthebridge”在捷联惯性导航及图像处理中应用的优势Lecture11--Quaternion41.0HLecture11--Quaternion43n1.1*四元数(quaternions)定义一个有固定点的刚体通过绕该点的某个轴转过特定角度可达到任何姿态转轴的方向可以表示成一个单位矢量:则描述该转动的四元数可以表示成:四元数既反映了转动的方向又反映了转动的幅值.Lecture11--Quaternion5n1.1*Lecture11--Quaternion441.2四元数的组成四元数的表示：λ-----标量部分----矢量部分包括一个实数单位1

和三个虚数单位i,j,k

另一种表示法:,P

代表矢量部分Lecture11--Quaternion61.2四Lecture11--Quaternion45Outline四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解Lecture11--Quaternion7OutliLecture11--Quaternion462.1*加法和减法加法和减法：或简写成：Lecture11--Quaternion82.1*加Lecture11--Quaternion472.2虚数单位的乘法规则i,j,k在乘法运算中的规则:对比Hamilton的公式Lecture11--Quaternion92.2虚Lecture11--Quaternion482.3*四元数乘法或简单地表示成：Lecture11--Quaternion102.3*Lecture11--Quaternion492.3四元数乘法自定义函数function[q1]=qmul(q,m)lm=q(1);p1=q(2);p2=q(3);p3=q(4);q1=[lm-p1-p2-p3p1lm-p3p2p2p3lm-p1p3-p2p1lm]*m;>>a=[1223]';>>b=[2423]';>>q=qmul(a,b)q=-0.77960.32820.49240.2052>>Lecture11--Quaternion112.3Lecture11--Quaternion502.3*四元数乘法表示符号※四元数乘法的符号※关于交换率和结合律Lecture11--Quaternion122.3*Lecture11--Quaternion512.4*共扼和范数共扼四元数的定义------两个四元数的标量部分相同，向量部分相反q

和q*彼此互为四元数.可以证明：四元数的范数---定义成,则q

成为规范化的四元数若是规范化的Lecture11--Quaternion132.4*Lecture11--Quaternion522.5*四元数的逆和除法若则q1

和q2

彼此互为逆,写为和因为除法:没有具体意义或function[qi]=qinv(q)%inverseofquaternionqn=norm(q);q(2:4)=-q(2:4);qi=q/qn^2;Lecture11--Quaternion142.5*Lecture11--Quaternion53Outline四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解Lecture11--Quaternion15OutlLecture11--Quaternion543.1*矢量的旋转如果矢量R

相对固定坐标系旋转,并且该旋转可以用四元数q

描述，新矢量记为R’，则R

和R’

之间的变换可以表示成下述四元数运算:含义:矢量R

相对固定坐标系旋转，旋转的角度和轴向由q决定上述运算中,R

被当成一个标量部分为零的四元数，即：Lecture11--Quaternion163.1*Lecture11--Quaternion553.2*坐标系的旋转一个矢量V

相对于坐标系OXYZ

固定:从坐标系OXYZ

转动了q,得到一个新坐标系OX’Y’Z’.V

分解在新坐标系OX’Y’Z’中矢量V

在两个坐标系之间的坐标变换:记：则分别称为V

在两个坐标系中的映像.和Lecture11--Quaternion173.2*Lecture11--Quaternion563.3四元数和方向余弦----表示坐标系旋转,其中q

和C之间是什么关系?假设则应用四元数乘法,得到Lecture11--Quaternion183.3Lecture11--Quaternion573.3四元数和方向余弦方向余弦矩阵Lecture11--Quaternion193.3Lecture11--Quaternion583.4四元数转动变换的两种形式如果一个矢量V固定，坐标系旋转按照四元数q

进行了旋转，得到了一个新坐标系，则该矢量分别在新旧坐标系中投影表达式间的关系借助映像方式可以表示为：如果一个坐标系固定,一个矢量VE

按照四元数q相对该坐标系进行了转动，得到一个新的矢量VE’，则新旧矢量之间的关系为：Lecture11--Quaternion203.4Lecture11--Quaternion59Outline四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解Lecture11--Quaternion21OutlLecture11--Quaternion604.0*转动四元数的合成连续的多次转动可以等效成一次转动.假设四元数q1和q2

分别代表第一次和第二次坐标系旋转.q1q2则合成后的转动四元数q

可以表示成:q其中q1

和q2

的轴必须表示成映像形式.若q1

和q2

的轴都表示在原来坐标系中,则Lecture11--Quaternion224.0*Lecture11--Quaternion614.1四元数合成例子:非映像方式坐标系顺序旋转情况下四元数的合成坐标系OX’Y’Z’

相对坐标系OXYZ

多次旋转首先,绕Z

轴转过角度ψ,瞬时转轴n

和k

轴重合,则Lecture11--Quaternion234.1四Lecture11--Quaternion62第二次旋转:绕X’

转过角度θ,旋转轴n

表示为:4.1四元数合成:非映像方式对应的四元数:Lecture11--Quaternion24第二次旋Lecture11--Quaternion634.1四元数合成:映像的方式这里q1

和q2

的转动轴表示为非映像的形式,因此合成的四元数为:转动次数越多，合成后四元数的表达式会愈发复杂Lecture11--Quaternion254.1Lecture11--Quaternion644.2*四元数合成:映像的方式每次转动的瞬时转轴都以映像方式给出.对第一次转动,瞬时转轴n1

的映像方式和非映像方式相同：因此Lecture11--Quaternion264.2*Lecture11--Quaternion654.2四元数合成:映像的方式第二次转动绕着OX’

转过了θ转轴n2

沿着OX’n2

在坐标系X’Y’Z’

中的映像为：因此q2

的映像形式为Lecture11--Quaternion274.2Lecture11--Quaternion664.2四元数合成:映像的方式第三次转动绕着OY’

轴转过φ.转轴n3

沿着OY’

轴OY’

是由原来坐标系的OY

轴转动得到的，因此n3

的映像形式为:这样，四元数q3

的映像形式为Lecture11--Quaternion284.2Lecture11--Quaternion674.2四元数合成:映像的方式因为q1,q2

和q3

都表示成