一种三维电子罗盘的标定方法技术领域
本发明涉及对三维电子罗盘的标定，特别涉及内置有磁传感器的利用地磁场进行定位的装置。
背景技术
三维电子罗盘利用内置的磁传感器通过感应地球磁场来确定航向，三维电子罗盘同时内置有倾角传感器，通过倾角补偿，使三维电子罗盘在倾斜情况下仍能准确判别方向。三维电子罗盘体积小，指示方向准确，输出电信号，可以通过各种接口集成到其他电子系统中，使用方便，现已广泛应用于海陆空航行、惯性导航、自主机器人和地质勘测等领域。
由于三维电子罗盘内置的磁传感器三轴并非完全正交，倾角传感器的测量轴也存在非正交问题，并且由于安装等问题，磁传感器坐标系和倾角传感器坐标系对应的各个轴并不严格平行，使得倾角补偿不准确，磁传感器和三维电子罗盘外壳坐标系也并非完全重合，致使测量的航向角存在误差。所以解决这几个问题是提高三维电子罗盘精度的关键。
现有的校准三维电子罗盘的方法有归一归零法、最小二乘法和拟椭圆法等，但是这些方法均没有从最基本的结构入手，而只是用线性拟合的方法校准结果，而三维电子罗盘存在正交问题和坐标系不重合问题时，输出结果是非线性的，用以上方法均不准确。还存在使用无磁的方法校准磁传感器的，在磁屏蔽房中进行实验，或者使用三维线圈将环境磁场抵消，在人为产生一个大小及角度已知的磁场来对传感器进行标定，但是这种方法对仪器条件要求苛刻，需要昂贵且体积大的磁屏蔽房和三维亥姆霍兹线圈。
在专利CN 101393022A中，发明者在环境磁场存在的情况下利用磁传感器在特定位置的输出值对其本身进行标定，不需要昂贵的磁屏蔽仪器和磁产生仪器。然而，这种标定方法需要对磁传感器的22个方位角输出值进行测量，过于复杂，且这22个位置中，有45度、135度、225度和315度的空间位置出现，对于外形为三维电子罗盘外壳的磁传感器而言，空间位置容易实现的是90度整数倍的 位置，而对其它位置的实现比较困难，并且带入误差较大，因而这种标定方法所能达到的精度只能是3～5度，而对于现在测量精度已经达到0.5度的磁传感器而言，显然是不合适的。另外，这种标定方法基于磁传感器输出特性为线性的假定，因而不能对存在正交误差的三维电子罗盘进行高精度标定。
在专利CN200910117170.7中，发明者同样在周围磁场存在的情况下进行标定，并通过在一维旋转平台水平方向360度旋转以及三维电子罗盘90度整数倍翻转的弱磁方向传感器空间位置变化所对应的传感器输出变化，确定弱磁方向传感器敏感方向大地坐标系和三维电子罗盘外壳坐标系中的空间位置，同样不需要苛刻的实验条件，并且方法简单易行。但是该专利所用的旋转平台只能在水平面内旋转，因而在确定磁倾角时存在较大误差，且并未对磁传感器和倾角传感器联合使用确定标定方法，因此难以做到高精度标定。
发明内容
本发明目的是针对现有的校准三维电子罗盘的线性的标定方法不准确的缺点，提出一种利用地磁场对三维电子罗盘进行标定的方法，该方法使用一个三维无磁旋转平台，利用三维电子罗盘内置的磁传感器和倾角传感器在不同空间方向的输出值变化对三维电子罗盘进行标定。
本发明为实现其目的所采取的技术方案：一种三维电子罗盘的标定方法，该方法的实现装置包括一个三维无磁旋转平台和一个内部置有三个磁传感器和一个倾角传感器的三维电子罗盘，所述三维电子罗盘放置于所述三维无磁旋转平台的中央位置，通过所述三维无磁旋转平台使所述三维电子罗盘在绕电子罗盘基准坐标系、大地坐标系的坐标轴各轴旋转过程中，磁传感器以及倾角传感器在地磁场及重力场作用下输出变化，确定磁传感器和倾角传感器敏感方向矢量、三维电子罗盘基准坐标系之间的空间位置关系，获得磁传感器和倾角传感器的输出特性，获得任意地磁场下三维电子罗盘输出与三维电子罗盘基准坐标系与大地坐标系之间的关系，确定方位角度，实现对三维电子罗盘高精度标定。
绕空间轴精密旋转由一个三维无磁旋转平台提供，其三个选择轴组成标准的右手三维直角坐标系 轴沿重力方向向下， 轴和 轴为相互正交的两轴，并且由 轴和 轴组成的平面可以调节至水平状态；
三维电子罗盘外壳具有三维基准直角坐标系e:(e1 e2 e3)，e1轴为三维电子罗盘的指示方向，e2轴在水平面上与e1轴正交；
三维电子罗盘内置有三个磁传感器，其敏感方向构成磁传感器坐标系ε:(ε1 ε2 ε3)；
三维电子罗盘内置有二维倾角传感器，其二个敏感方向以及由右手关系决定的第三个轴组成倾角传感器坐标系ξ:(ξ1 ξ2 ξ3)，ξ1轴和ξ2轴分别为测量倾斜角和测量翻滚角的两个敏感方向；
地磁场和重力场构成大地坐标系ζ:(ζ1 ζ2 ζ3)，其三轴分别指向磁北方向、磁东方向和重力方向；
规定当逆向面对坐标轴时，逆时针方向为正向，所有的角度保持在区间[0,2π)；
将三维无磁旋转平台坐标系的 面调节为水平状态，将三维电子罗盘外壳的e1轴和e2轴分别紧靠三维无磁旋转平台的 轴和 轴上并固定，以 轴为旋转轴，在三维无磁旋转平台沿顺时针方向转动一周的过程中，当磁传感器ε1轴输出最大值时，将此位置的三维无磁旋转平台转动角度记为θ01；然后，以e1为旋转轴将其正方向翻转180°，再次使三维无磁旋转平台绕其 轴旋转一周，获得使磁传感器ε1轴获得最大值时的转动角度，记为θ01′；
三维无磁旋转平台坐标系的 轴在初始位置时与大地坐标系的ζ1轴的夹角记为 使三维无磁旋转平台从上述的初始位置绕 轴顺时针旋转角度 得到新的基准位置，在该基准位置下使三维无磁旋转平台绕其 轴带动三维电子罗盘逆时针转动，当磁传感器ε1轴取得极大值时，得到当前三维无磁旋转平台转动的角度θ02；在新的基准位置下，将三维电子罗盘沿其外壳的e1轴将其翻转180°，再次使三维无磁旋转平台绕其 轴旋转一周，再次获得使磁传感器的ε1轴获得最大值时的转动角度，记为θ02′；
在新的基准位置下，将三维无磁旋转平台绕其 轴带动三维电子罗盘顺时针转动一个角度，此角度为磁倾角∠ζ3B的余角β，然后，使三维无磁旋转平台绕其 轴带动三维电子罗盘旋转一周，转角为θ时，磁传感器ε1轴输出量V1的函数表达式为：V1=f1(B‖)=f1(B0cosθ)。
通过翻转三维电子罗盘，将其三维电子罗盘外壳坐标系的不同面放置在三维无磁旋转平台上，使磁传感器的ε2轴和ε3轴分别代替ε1轴进行上述的操作；
将三维无磁旋转平台坐标系的 面调节为水平状态，获得倾角传感器的两个输出量 再将三维电子罗盘沿e1轴逆时针翻转90度，则得到倾角传感器的另外两个输出量
进行实际测量时，假设新磁场大小为B1，获得磁传感器三个轴的原始输出大小，记为： V 11 V 12 V 13 . ]]>
三维无磁旋转平台坐标系的 轴在初始位置时与大地坐标系的ζ1轴的夹角 和磁传感器ε1轴在罗盘坐标系的e1e2面内的投影方向与罗盘坐标系的e1轴的夹角∠e1ε1‖分别为： ∠ e 11 ϵ 1 | | = θ 01 ′ - θ 01 2 . ]]>
磁传感器ε1轴在罗盘坐标系的e1e3面内的投影分量与e1轴的夹角∠e1ε1⊥，和地磁场矢量与竖直方向的夹角∠ζ3B（即磁倾角）分别为：
∠ e 1 ϵ 1 ⊥ = θ 02 ′ - θ 02 2 , ]]> ∠ ζ 3 B = θ 02 ′ + θ 02 - 3 π 2 . ]]>
磁传感器ε1轴在三维电子罗盘外壳坐标系内的坐标表达式为：
e 1 e 2 e 3 a 1 a 2 a 3 = e 1 e 2 e 3 ( 1 + tan 2 ∠ e 1 ϵ 1 ⊥ + tan 2 ∠ e 1 ϵ 1 | | ) - 1 / 2 ( 1 + tan 2 ∠ e 1 ϵ 1 ⊥ + tan 2 ∠ e 1 ϵ 1 | | ) - 1 / 2 tan ∠ e 1 ϵ 1 | | - ( 1 + tan 2 ∠ e 1 ϵ 1 ⊥ + tan 2 ∠ e 1 ϵ 1 | | ) - 1 / 2 tan ∠ e 1 ϵ 1 ⊥ ]]>
分别计算出磁传感器ε2轴和ε3轴在外壳坐标系内的矢量表达式，就可以得到磁传感器坐标系与外壳坐标系的转换关系表达式为：
ϵ 1 ϵ 2 ϵ 3 = e 1 e 2 e 3 a 1 b 1 c 1 a 2 b 2 c 2 a 3 b 3 c 3 = e 1 e 2 e 3 M eϵ ]]>
其中，转换矩阵记为Meε，通过实验得到磁传感器ε2轴和ε3轴的数字输出V1和V2的函数表达式后，可得磁传感器器的输出函数为：
V 1 V 2 V 3 = f 1 ( B | | ) f 2 ( B | | ) f 3 ( B | | ) ]]>
设倾角传感器与e2e3面夹角为 则有：
从而可解出 进而可得到倾角传感器坐标系与外壳坐标系之间的换算关系：ξ=eMeξ。其中：
利用实验时获取的 V 1 V 2 V 3 = f 1 ( B | | ) f 2 ( B | | ) f 3 ( B | | ) ]]>和测量时三维电子罗盘输出的 V 11 V 12 V 13 , ]]>获
得磁传感器的三个轴上的磁场分量大小，记为：
B 1 = B 0 ϵ 1 ϵ 2 ϵ 3 a 11 a 12 a 13 = B 0 e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 c 1 c 2 c 3 a 11 a 12 a 13 = B 0 e 1 e 2 e 3 d 1 d 2 d 3 ]]>
此时的总磁场大小为： 在大地坐标系中，此时的磁场表达式为：
B 1 = B 1 ζ 1 ζ 2 ζ 3 sin β 2 0 cos β 2 = d 1 2 + d 2 2 + d 3 2 B 0 ζ 1 ζ 2 ζ 3 sin β 2 0 cos β 2 ]]>
设定相对于地磁场的偏转角、倾斜角和翻滚角分别为(r,s,t)，其中，r=∠ζ1e1‖ s = π 2 - ∠ ζ 3 e 1 . ]]>
由：
( - sin s ) 2 + ( cos s sin t ) 2 + ( cos s cos t ) 2 = 1 ⇒ ]]>
cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 ( N - 1 ) T cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 = 1 ]]>
可计算出cosβ2，其中，
N = M eξ 11 M eξ 12 0 M eξ 21 M eξ 22 0 M eξ 31 M eξ 32 d 3 ′ ]]>
进而由：
- sin s cos s sin t cos s cos t = cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 ⇒ ]]>
sin s = cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 i 1 ⇒ s , sin s , cos s ]]>
tan t = cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 i 2 cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 i 3 ⇒ t , sin t , cos t ]]>
计算出s和t，再由：
cos r cos s cos r sin s sin t - sin r cos t cos r sin s cos t + sin r sin t sin r cos s sin r sin s sin t + cos r cos t sin r sin s cos t - cos r sin t d 1 ′ d 2 ′ d 3 ′ = sin β 2 0 ⇒ ]]>
cot r = sin t d 3 ′ - cos td 2 ′ cos s d 1 ′ + sin s sin t d 2 ′ + sin s cos td 3 ′ ]]>
sin r = sin β 2 cot r cos s d 1 ′ + cot r sin s sin t d 2 ′ - cos t d 2 ′ + cot r sin s cos t d 3 ′ + sin t d 3 ′ ]]>
计算出r，至此所有角度都计算出来。
三维电子罗盘的标定方法，能够校准三维电子罗盘等内置有三轴磁传感器的装置，其内置的磁传感器包括各向异性磁阻传感器、霍尔效应磁传感器、磁感传感器、磁通门磁传感器等方向敏感且量程与地磁场相仿或者小于地磁场的磁传感器。
本发明的有益效果：用于校准三维电子罗盘的标定方法所用的仪器只需要一个可测旋转角度的三维无磁旋转平台，能够利用地磁场本身对三维电子罗盘进行标定，不需要磁屏蔽房和三维亥姆霍兹线圈。
用于校准三维电子罗盘的标定方法只需要在一定时间内标定空间的周围磁场稳定均匀，所以在普通的实验室或房间内就可进行标定，不需要到野外等苛刻的环境中，允许标定空间周围有铁磁物质干扰。
用于校准三维电子罗盘的标定方法，使用地磁场进行标定实验，而地磁场在短时间内稳定可靠，因此可以为精度达0.5°甚至更高的三维电子罗盘进行标定。
用于校准三维电子罗盘的标定方法包括实验部分和数据处理部分，实验操作部分，只要获取数据即可，时间较短，对环境要求低；数据处理部分，只需对实验过程中采集的数据进行离线处理，不需三维电子罗盘和三维无磁旋转平台参与，简捷高效。
用于校准三维电子罗盘的标定方法操作简单方便，精度较高，平台简单，对机械加工要求低。
附图说明
图1为三维电子罗盘水平放置在三维无磁旋转平台的示意图。
图2为三维无磁旋转平台带动三维电子罗盘绕竖直轴旋转并获取相应角度的示意图。
图3为三维无磁旋转平台带动三维电子罗盘绕 轴旋转并获取相应角度的示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明：
图1为三维电子罗盘2放置示意图。
放置的平台为三维无磁旋转平台1，其坐标系为 其中 轴为沿重力方向向下，三维无磁旋转平台1可绕 轴、 轴和 轴分别进行360°精密旋转，并且旋转角度可通过光电编码器等装置精确输出，其 面可以调节为水平状态，定义逆时针为正方向。三维电子罗盘2（其外壳坐标系为e:(e1 e2 e3)）内置有三个磁传感器3（其坐标系为ε:(ε1 ε2 ε3)）和一个倾角传感器4（其坐标系为ξ:(ξ1 ξ2 ξ3)），三维电子罗盘2放置在三维无磁旋转平台1上，其e1轴与 轴对准。
先将三维无磁旋转平台1的 面调节为水平状态，然后将三维电子罗盘2放置在三维无磁旋转平台1的 面上，获得倾角传感器4的两个输出量，即相对于三维电子罗盘2坐标系e1e2面的两个夹角 再将三维电子罗盘2沿e1轴逆时针翻转90°，则得到倾角传感器4与三维电子罗盘2坐标系e3e1面的两个夹角
设倾角传感器4与三维电子罗盘2坐标系e2e3面的两个夹角为 则有：
联立这两个方程可解出 便可得到倾角传感器4坐标系与三维电 子罗盘2坐标系之间的换算关系：ξ=eMeξ。其中：
图2为三维无磁旋转平台1绕 轴进行旋转实验时水平面内各角度的示意图。
和 分别为三维无磁旋转平台1坐标系在水平面上的两个分量，e1为三维电子罗盘2初始放置时其e1轴的矢量，e1′为三维电子罗盘2绕e1轴进行180°翻转后其e1轴的矢量，ε1‖为三维电子罗盘2初始放置时磁传感器3ε1轴在水平面上的投影矢量，ε1‖′为三维电子罗盘2绕e1轴进行180°翻转后磁传感器3ε1轴在水平面上的投影矢量，ζ1为大地坐标系ζ:(ζ1 ζ2 ζ3)在磁北方向上的分量， 为 轴在初始位置时与大地坐标系的ζ1轴的夹角，∠e1ε1‖为磁传感器3ε1轴在三维电子罗盘2坐标系的e1e2面内的投影方向与其e1轴的夹角。实验时，先将三维无磁旋转平台1的 面调节为水平状态，并将三维电子罗盘2按照图1的要求固定在三维无磁旋转平台1上，然后转动三维无磁旋转平台1使其 轴大体对正北方向对准，作为初始位置，接下来顺时针转动三维无磁旋转平台1一周，与此同时使用计算机或其他上位机接收磁传感器3ε1轴的数据和三维无磁旋转平台1的转动角度数据。由于地磁场矢量B位于大地坐标系的ζ1ζ3面内，当磁传感器3ε1轴处于ζ1ζ3面内，即与ζ1轴重合时，磁传感器3ε1轴与地磁场矢量B的夹角最小，此时磁传感器3ε1轴输出极大值，获得磁传感器3ε1轴输出的最大值相对应的转动角度，记为θ01，将三维电子罗盘2绕e1轴进行180°翻转后，再次进行一次实验，再次获得磁传感器3ε1轴输出的极大值相对应的转动角度，记为θ01′， 由图2可以看出： 由此可计算出：
图3为三维无磁旋转平台1绕 轴进行旋转实验时 面内各角度的示意图。
ζ2和ζ3分别为大地坐标系ζ:(ζ1 ζ2 ζ3)在竖直面ζ2ζ3上的分量，另外一个分量ζ1朝向我们，B为地磁场矢量，e1为三维电子罗盘2在基准位置下e1轴的矢量，e1′为三维电子罗盘2从基准位置绕e1轴进行180°翻转后其e1轴的矢量，ε1⊥为三维电子罗盘2在基准位置下磁传感器3ε1轴在竖直面ζ2ζ3上的投影矢量，ε1⊥′为三维电子罗盘2绕e1轴进行180°翻转后磁传感器3ε1轴在竖直面ζ2ζ3上的投影矢量，∠ζ3B为磁倾角，即地磁场矢量B与竖直方向ζ3的夹角，∠e1ε1⊥为磁传感器3ε1轴在三维电子罗盘2罗盘2坐标系的e1e3面内的投影分量与e1轴的夹角。实验时，先将三维无磁旋转平台1调到初始位置，再使其绕 轴顺时针旋转角度 得到新的基准位置，此时可以认定大地坐标系、三维电子罗盘2坐标系和三维无磁旋转平台坐标系完全重合，使三维无磁旋转平台1绕其 轴旋转（也是绕ζ2轴旋转），以带动三维电子罗盘2逆时针转动一周，与此同时使用计算机或其他上位机接收磁传感器3ε1轴的数据和三维无磁旋转平台1的转动角度数据。当磁传感器3ε1轴地磁场矢量B重合时，输出为最大值，获取此时转动的角度θ02；同理，在此基准位置下，将三维电子罗盘2沿其e1轴将其翻转180°，再次使三维无磁旋转平台1绕其 轴旋转一周，再次获得使磁传感器3ε1轴获得最大值时的转动角度θ02′。由图3可以看出：
θ 02 + ∠ e 1 ϵ 1 ⊥ = 3 π 2 + ∠ ζ 3 B θ 02 ′ - ∠ e 1 ϵ 1 ⊥ = 3 π 2 + ∠ ζ 3 B , ]]>由此可计算出： ∠ e 1 ϵ 1 ⊥ = θ 02 ′ - θ 02 2 ∠ ζ 3 B = θ 02 ′ + θ 02 - 3 π 2 . ]]>
计算出∠e1ε1‖和∠e1ε1⊥后，就可以计算出磁传感器3ε1轴在三维电子罗盘2坐标系内的矢量表达式： e 1 e 2 e 3 a 1 a 2 a 3 ]]>
其中：
a 1 a 2 a 3 = ( 1 + tan 2 ∠ e 1 ϵ 1 ⊥ + tan 2 ∠ e 1 ϵ 1 | | ) - 1 / 2 ( 1 + tan 2 ∠ e 1 ϵ 1 ⊥ + tan 2 ∠ e 1 ϵ 1 | | ) - 1 / 2 tan ∠ e 1 ϵ 1 | | - ( 1 + tan 2 ∠ e 1 ϵ 1 ⊥ + tan 2 ∠ e 1 ϵ 1 | | ) - 1 / 2 tan ∠ e 1 ϵ 1 ⊥ ]]>
通过90°翻转三维电子罗盘2，分别使磁传感器3ε2轴和ε3轴代替磁传感器3ε1轴重复上述的操作和计算，同理就可以分别计算出磁敏传感器ε2轴和ε3轴在三维电子罗盘2坐标系内的矢量表达式，便可以得到磁传感器3坐标系与三维电子罗盘2坐标系的转换关系表达式：
ϵ 1 ϵ 2 ϵ 3 = e 1 e 2 e 3 a 1 b 1 c 1 a 2 b 2 c 2 a 3 b 3 c 3 ↔ ϵ = M eϵ ]]>
其中，转换矩阵记为Meε，通过实验得到磁传感器3ε2轴和ε3轴的数字输出V1和V2的函数表达式后，可得磁传感器3器的输出函数为：
V 1 V 2 V 3 = f 1 ( B | | ) f 2 ( B | | ) f 3 ( B | | ) ]]>
设倾角传感器4与e2e3面夹角为 则有：
从而可解出 进而可得到倾角传感器4坐标系与外壳坐标系之间 的换算关系：ξ=eMeξ。其中：
利用实验时获取的 V 1 V 2 V 3 = f 1 ( B | | ) f 2 ( B | | ) f 3 ( B | | ) ]]>和测量时三维电子罗盘2输出的 V 11 V 12 V 13 , ]]>获得磁传感器3的三个轴上的磁场分量大小，记为：
B 1 = B 0 ϵ 1 ϵ 2 ϵ 3 a 11 a 12 a 13 = B 0 e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 c 1 c 2 c 3 a 11 a 12 a 13 = B 0 e 1 e 2 e 3 d 1 d 2 d 3 ]]>
此时的总磁场大小为： 在大地坐标系中，此时的磁场表达式为：
B 1 = B 1 ζ 1 ζ 2 ζ 3 sin β 2 0 cos β 2 = d 1 2 + d 2 2 + d 3 2 B 0 ζ 1 ζ 2 ζ 3 sin β 2 0 cos β 2 ]]>
设定相对于地磁场的偏转角、倾斜角和翻滚角分别为(r,s,t)，其中，r=∠ζ1e1‖ s = π 2 - ∠ ζ 3 e 1 . ]]>
由：
( - sin s ) 2 + ( cos s sin t ) 2 + ( cos s cos t ) 2 = 1 ⇒ ]]>
cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 ( N - 1 ) T cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 = 1 ]]>
可计算出cosβ2，其中，
N = M eξ 11 M eξ 12 0 M eξ 21 M eξ 22 0 M eξ 31 M eξ 32 d 3 ′ ]]>
进而由：
- sin s cos s sin t cos s cos t = cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 ⇒ ]]>
sin s = cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 i 1 ⇒ s , sin s , cos s ]]>
tan t = cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 i 2 cos ∠ ζ 3 ξ 1 cos ∠ ζ 3 ξ 2 cos β 2 N - 1 i 3 ⇒ t , sin t , cos t ]]>
计算出s和t，再由：
cos r cos s cos r sin s sin t - sin r cos t cos r sin s cos t + sin r sin t sin r cos s sin r sin s sin t + cos r cos t sin r sin s cos t - cos r sin t d 1 ′ d 2 ′ d 3 ′ = sin β 2 0 ⇒ ]]>
cot r = sin t d 3 ′ - cos td 2 ′ cos s d 1 ′ + sin s sin t d 2 ′ + sin s cos td 3 ′ ]]>
sin r = sin β 2 cot r cos s d 1 ′ + cot r sin s sin t d 2 ′ - cos t d 2 ′ + cot r sin s cos t d 3 ′ + sin t d 3 ′ ]]>
计算出r，至此所有角度都计算出来。