三坐标定位器调姿运控方法、系统、调姿控制器及存储介质

摘要

本发明公开了一种三坐标定位器调姿运控方法、系统、调姿控制器及存储介质，该方法包括：解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量；解算刚体定轴转动过程的每个定位器的空间圆弧轨迹的圆心和半径，并解算圆弧角速度范围和刚体角速度范围；进行刚体复合运动速度规划并修整；按每个定位器驱动轴允许的速度范围和加速度范围调整刚体复合运动的速度和加速度，重新进行刚体复合运动速度规划并修整；按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置；根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置。本发明可满足加速度要求且有冗余驱动轴，调姿过程平稳无冲击。

权利要求

1.一种三坐标定位器调姿运控方法，应用于飞机数字化装配中，其特征在于，所述方法包括：

建立飞机大部件刚体局部坐标系和定位器局部坐标系，按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量；

根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的空间圆弧轨迹的圆心和半径，并按每个圆弧轨迹允许误差解算相应的圆弧角速度范围和刚体角速度范围；

根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度规划并修整；

根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的速度范围和加速度范围调整刚体复合运动的速度和加速度；

当调整刚体复合运动的速度和加速度后，重新进行刚体复合运动速度规划并修整；

根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置；

根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置；

所述按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量，具体包括：

解算刚体的平动移动量，如下式：

解算刚体的平动方向矢量，如下式：式中，

先绕刚体的Z轴转动A角度，再绕刚体的Y轴转动B角度，最后绕刚体的X轴转动C角度，如下式：

式中，s1＝sin(A)，c1＝cos(A)，s2＝sin(B)，c2＝cos(B)，s3＝sin(C)，c3＝cos(C)；

解算刚体定轴转动的转动角度，如下式:解算刚体定轴转动的转动轴矢量，如下式：式中，

2.根据权利要求1所述的三坐标定位器调姿运控方法，其特征在于，所述建立飞机大部件刚体局部坐标系和定位器局部坐标系，具体包括：为相对地面参照系建立全局坐标系Oxyz；

为飞机大部件建立刚体局部坐标系Olxyz，刚体局部坐标系的X、Y、Z轴向与全局坐标系的X、Y、Z轴平行，刚体局部坐标系的转动中心设为全局坐标系的原点，局部坐标系的原点调T

姿前在全局坐标系的坐标为Ol(px，py，pz) ；

为第i个定位器建立定位器局部坐标系Oixyz，定位器局部坐标系的X、Y、Z轴向是定位器的支撑点沿定位器的X、Y、Z轴驱动移动的方向，定位器局部坐标系的原点是定位器的X、Y、Z轴回零后的支撑点位置，第i个定位器局部坐标系的原点在全局坐标系的坐标为Oi(oix，oiy，T T

oiz) ，第i个定位器支撑点在全局坐标系的坐标为Pi(pix，piy，piz) 。

3.根据权利要求1所述的三坐标定位器调姿运控方法，其特征在于，所述根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的空间圆弧轨迹的圆心和半径，按每个圆弧轨迹允许误差解算相应的圆弧角速度范围和刚体角速度范围，具体包括：根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的空间圆弧轨迹的圆心和半径，如下式：

式中， 为刚体定轴转动过程的第i个定位器的空间圆弧轨迹的圆心位置，为定位器第i个支撑点调姿前在刚体局部坐标系Olxyz中的位置，为刚体定轴转动的转动轴矢量，ri为刚体定轴转动过程的第i个定位器的空间圆弧轨迹的半径：设Tinp为插补周期时间，Herr为弓高误差，Lc为插补周期输出量弦长，如下式：

2 2

Lc＝8Herrri+4Herr计算满足弓高误差Herr的弦长，如下式：式中，Lc＝VTinpo＝ωiriTinp，则若第i个定位器调姿过程按允许的弓高误差Herr，则要求刚体转动角速度ωi最大，并且弓高误差Herr不变时，半径vi越大允许的刚体转动角速度ωi越小；

求出所有定位器空间圆弧轨迹半径中的最大半径，如下式：rmax＝max(r1，r2，r3，...，ri，...，rN)插补时，插补周期时间Tinp相同，且刚体定轴转动多个圆弧角速度相同；

求出弓高误差Herr满足要求的刚体绕 转动角速度最大值，如下式：

4.根据权利要求1所述的三坐标定位器调姿运控方法，其特征在于，所述根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度规划并修整，具体包括：根据刚体角速度最大值，调整刚体定轴转动结构强度要求的最大角速度ωmax＝min(ωmax1，ωmax2)；

根据飞机大部件调姿允许的最大角加速度αmax、最大平动速度vmax、最大平动加速度amax，刚体定轴转动结构强度要求的最大角速度ωmax＝min(ωmax1，ωmax2)，调姿起始和终止的速度vs＝ve＝0，调姿起始和终止的加速度as＝ae＝0，刚体定轴转动的转动角度θe和刚体的平动移动量S，分别进行铃型加/减速的刚体复合运动速度规划计算，计算出各阶段时间、平动实际最大加速度anowmax、平动实际最大速度vnowmax、转动实际最大角加速度αnowmax和实际最大角速度ωnowmax，随后进行刚体复合运动速度规划时间修整。

5.根据权利要求4所述的三坐标定位器调姿运控方法，其特征在于，所述进行铃型加/减速的刚体复合运动速度规划计算，具体包括：根据刚体的平动移动量S、最大平动速度vmax、最大平动加速度amax和平动加加速jl，分为加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减减速阶段共七个阶段进行速度规划计算；其中：加加速的计算如下式：

式中，J＝jl；

加速度的计算如下式：

速度的计算如下式：

式中，T1＝t1，T7＝t2‑t1，T3＝t3‑t2；T4＝t4‑t3，T5＝t5‑t4，T6＝t6‑t5；T7＝t7‑t6，T1＝T3，T5＝T7；

平动移动量的计算如下式：

平动的速度规划为s＝fs1(t)，v＝fv1(t)，a＝fa1(t)，js＝J(t)；

根据铃型加/减速时T1s＝T3s，T5s＝T7s，调姿起始和终止的速度vs＝ve＝0，调姿起始和终止的加速度as＝ae＝0，以及加速阶段与减速阶段镜像对称，得到T1s＝T7s＝T5s＝T3s，T2s＝T6s；

最大速度和最大加速度在安全范围约束条件为v3≤vmax，Jt1≤amax，求出加加速阶段时间T1s、匀加速阶段时间T2s、减加速阶段时间T3s、匀速阶段时间T4s、加减速阶段时间T5s、匀减速阶段时间T6s、减减速阶段时间T7s；

计算实际最大加速度anowmax＝jlT1s，实际最大速度vnowmax＝v3，以及平动总时间Ts＝T1s+T2s+T3s+T4s+T5s+T6s+T7s；

根据刚体定轴转动的转动角度θe、最大角速度ωmax、最大角加速度αmax和转动角加加速jθ，分为加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减减速阶段共七个阶段进行速度规划的计算；其中：角加加速的计算如下式：

式中，J＝jθ；

角加速度的计算如下式：

角速度的计算如下式：

式中，T1＝t1，T2＝t2‑t1，T3＝t3‑t2；T4＝t4‑t3，T5＝t5‑t4，T6＝t6‑t5；T7＝t7‑t6，T1＝T3，T5＝T7；

转动角度的计算如下式：

转动的速度规划为jθ＝J(t)，θ＝fθ1(t)，ω＝fω1(t)，α＝fα1(t)；

根据铃型加/减速时T1θ＝T3θ，T5θ＝T7θ，建立转动角度的分段函数，并根据角加加速度J＝jθ，转动角度调姿增量θe，调姿起始和终止的角速度ωs＝ωe＝0，调姿起始和终止的角加速度αs＝αe＝0，以及加速阶段与减速阶段镜像对称，得到T1θ＝T7θ＝T5θ＝T3θ，T7θ＝T6θ；T1θ＝T7θ＝T5θ＝T3θ，T2θ＝T6θ；

最大速度和最大加速度在安全范围约束条件为ω3≤ωmax，Jt1≤αmax，求出加加速阶段时间T1θ、匀加速阶段时间T2θ、减加速阶段时间T3θ、匀速阶段时间T4θ、加减速阶段时间T5θ、匀减速阶段时间T6θ、减减速阶段时间T7θ；

计算实际最大角加速度αnowmax，实际最大角速度ωnowmax，以及转动总时间Tθ＝T1θ+T2θ+T3θ+T4θ+T5θ+T6θ+T7θ。

6.根据权利要求4所述的三坐标定位器调姿运控方法，其特征在于，所述进行刚体复合运动速度规划时间修整，具体包括：对刚体定轴转动和刚体平动分别进行铃型加/减速的速度规划两个运动时间修整为插补周期时间Tinp整数倍而且调整平动时间与转动时间相等，计算调整时间轴后的各阶段时间、刚体平动实际最大加速度anow2max、刚体平动实际最大速度vnow2max、刚体定轴转动实际最大角加速度αnow2max、刚体定轴转动实际最大角速度ωnow2max。

7.根据权利要求6所述的三坐标定位器调姿运控方法，其特征在于，所述计算调整时间轴后的各阶段时间、刚体平动实际最大加速度anow2max、刚体平动实际最大速度vnow2max、刚体定轴转动实际最大角加速度αnow2max、刚体定轴转动实际最大角速度ωnow2max，具体包括：将平动总时间Ts或转动总时间Tθ向上取整为插补周期时间Tinp的整数倍，如下式：刚体平动与刚体定轴转动同步进行，要求同时开始、同时结束，则调整平动时间与定轴转动时间为相同时间，如下式：

Tnow＝Tnows＝Tnowθ＝max(Tnows，Tnowθ)各阶段的时间按比例调整，如下：

刚体平动时间调整比例：

刚体定轴转动时间调整比例：

T1s＝T7s＝T5s＝T3s＝T1sKnows，T2s＝T6s＝T2sKnows，T4s＝T4sKnowsT1θ＝T7θ＝T5θ＝T3θ＝T1θKnowθ，T2θ＝T6θ＝T2θKnowθ，T4θ＝T4θKnowθ平动距离S和转动角度θe保持不变，Tnows和Tnowθ时间变长，每个阶段的时间变长，调整实际速度、实际加速度、实际加加速度，如下：刚体平动实际最大速度和刚体定轴转动实际最大角速度调整：刚体平动实际最大加速度和刚体定轴转动实际最大角加速度调整：刚体平动实际加加速度和刚体定轴转动实际加加速度调整：当按比例调整各阶段时间、速度、加速度、加加速度后，各阶段刚体平动移动量和刚体定轴转动角度与调整前保持不变，得到调整后的速度规划如下：刚体平动：s＝fs(t)；v＝fv(t)；a＝fa(t)；其中t∈[0，Tnow]；s∈[0，S]刚体定轴转动：θ＝fθ(t)；ω＝fω(t)；α＝fα(t)；其中t∈[0，Tnow]；θ∈[0，θe]。

8.根据权利要求1所述的三坐标定位器调姿运控方法，其特征在于，所述根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的速度范围和加速度范围调整刚体复合运动的速度和加速度，具体包括：

将每个定位器支撑点作为刚体上质点，刚体进行定轴转动和平动的同步叠加运动，利用逆向运动学位置反解，求解出每个定位器驱动轴的空间圆弧运动轨迹、速度和加速度，在定位器局部坐标系中计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的速度范围和加速度范围调整刚体复合运动的速度和加速度。

9.根据权利要求8所述的三坐标定位器调姿运控方法，其特征在于，所述将每个定位器支撑点作为刚体上质点，刚体进行定轴转动和平动的同步叠加运动，利用逆向运动学位置反解，求解出每个定位器驱动轴的空间圆弧运动轨迹、速度和加速度，在定位器局部坐标系中计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的速度范围和加速度范围调整刚体复合运动的速度和加速度，具体包括：以第i个定位器支撑点空间圆弧Ci的圆心为坐标原点建立新的局部坐标系Cixyz，局部坐标系Cixyz的X、Y、Z轴平行于定位器局部坐标系Oixyz的X、Y、Z轴；

空间圆弧运动轨迹中的圆心角为刚体旋转的角度θ＝fθ(t)；

刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移距离s＝fs(t)；

刚体平动方向矢量 其中

对应局部坐标系Cixyz的原点在定位器局部坐标系Oixyz方向矢量刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移位置分量形式如下：

T

fis(t)＝[xis(t) yis(t) zis(t)]式中，xis(t)＝uixfs(t)，yis(t)＝uiyfs(t)，zis(t)＝uizfs(t)；

刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移速度分量形式如下：

T

fiv(t)＝[xiv(t) yiv(t) ziv(t)]式中，xiv(t)＝uixfv(t)，yiv(t)＝uiyfv(t)，ziv(t)＝uizfv(t)；

定位器局部坐标系Oixyz中X、Y、Z轴平动过程的最大速度，如下式：viSxmax＝max(|xiv(t)|)＝|uix|max(fv(t))＝|uix|vnow2maxviSymax＝max(|yiv(t)|)＝|uiy|max(fv(t))＝|uiy|vnow2maxviSzmax＝max(|ziv(t)|)＝|uiz|max(fv(t))＝|uiz|vnow2max刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移加速度分量形式如下：

T

fia(t)＝[xia(t) yia(t) zia(t)]式中，xia(t)＝uixfa(t)，yia(t)＝uiyfa(t)，zia(t)＝uizfa(t)；

定位器局部坐标系Oixyz中X、Y、Z轴平动过程的最大加速度，如下式：aiSxmax＝max(|xia(t)|)＝|uix|max(fa(t))＝|uix|anow2maxaiSymax＝max(|yia(t)|)＝|uiy|max(fa(t))＝|uiy|anow2maxaiSzmax＝max(|zia(t)|)＝|uiz|max(fa(t))＝|uiz|anow2max刚体定轴转动中刚体局部坐标系Olxyz的转动轴 刚体定轴转动的转动角度θ＝fθ(t)；ω＝fω(t)；α＝fα(t)；其中t∈[0，Tnow]；θ∈[0，θe]；

调姿前第i个定位器支撑点在刚体局部坐标系Olxyz中的位置矢量第i个定位器在刚体局部坐标系Olxyz中的旋转半径 圆心坐标刚体定轴转动角度θ＝fθ(t)，刚体旋转Rw，θ，如下式：式中，sθ＝sin(θ)，cθ＝cos(θ)，vθ＝1‑cos(θ)，θ＝fθ(t)，t∈[0，Tnow]，θ∈[0，θe]；调姿过程中第i个定位器支撑点在刚体局部坐标系Olxyz中的位置矢量为 刚体转动轴在局部坐标系Cixyz中为

第i个定位器支撑点在局部坐标系Cixyz中的位置Ci(θ)为旋转角度θ函数，如下式：第i个定位器支撑点在局部坐标系Cixyz中的位置Ci(θ)的分量形式如下：T

Ci(θ)＝[xi(θ) yi(θ) zi(θ)]，xi(θ)，yi(θ)，zi(θ)Ci任意θ位置的法向方向矢量，如下式：Ci任意θ位置的法向方向矢量分量形式如下：xin(θ)，yin(θ)，zin(θ)；

xint(fθ(t))，yint(fθ(t))，zint(fθ(t))；

Ci任意θ位置的法向方向矢量X、Y、Z轴分量绝对值最大值，如下式：xintmax＝max(|xint(fθ(t))|)＝max(|xin(θ)|)yintmax＝max(|yint(fθ(t))|)＝max(|yin(θ)|)zintmax＝max(|zint(fθ(t))|)＝max(|zin(θ)|)Ci任意θ位置的切向方向矢量，如下式：其中

Ci任意θ位置的切向方向矢量分量形式如下：xiτ(θ)，yiτ(θ)，ziτ(θ)；

xiτt(fθ(t))，yiτt(fθ(t))，ziτt(fθ(t))；

Ci任意θ位置的切向方向矢量X、Y、Z轴分量绝对值最大值，如下式：xiτtmax＝max(|xiτt(fθ(t))|)＝max(|xiτ(θ)|)yiτtmax＝max(|yiτt(fθ(t))|)＝max(|yiτ(θ)|)ziτtmax＝max(|ziτt(fθ(t))|)＝max(|ziτ(θ)|)第i个定位器支撑点在局部坐标系Cixyz中的线速度viτ(t)＝rifω(t)；

第i个定位器支撑点在局部坐标系Cixyz中的最大线速度max(viτ(t))＝riωnow2max；

第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动速度第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动速度分量形式如下：T

fivτ(t)＝[xivτ(t) yivτ(t) zivτ(t)]式中，xivτ(t)＝viτ(t)xiτt(fθ(t))，yivτ(t)＝viτ(t)yiτt(fθ(t))，zivτ(t)＝viτ(t)ziτt(fθ(t))；

第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动最大线速度乘以方向矢量各轴分量最大值不小于圆弧运动fivτ(t)各轴最大速度，如下：max(|xivτ(t)|)＝max(viτ(t)|xiτt(fθ(t))|)≤max(viτ(t))max(|xiτt(fθ(t))|)max(|yivτ(t)|)＝max(viτ(t)|yiτt(fθ(t))|)≤max(viτ(t))max(|yiτt(fθ(t))|)max(|zivτ(t)|)＝max(viτ(t)|ziτt(fθ(t))|)≤max(viτ(t))max(|ziτt(fθ(t))|)第i个定位器支撑点在定位器局部坐标系Oixyz中的运动速度为圆弧运动速度fivτ(t)和平动速度fiv(t)同步叠加的速度，平动速度X、Y、Z轴分量最大速度与圆弧运动速度X、Y、Z轴分量最大速度之和大于定位器实际X、Y、Z实际的最大速度，若小于各驱动轴允许的速度范围，则安全，否则降低刚体复合运动的速度，如下：vixtest≥max(viτ(t)|xiτt(fθ(t))|)+max(|xiv(t)|)≥max(|xivτ(t)+xiv(t)|)viytest≥max(viτ(t)|yiτt(fθ(t))|)+max(|yiv(t)|)≥max(|yivτ(t)+yiv(t)|)viztest≥max(viτ(t)|ziτt(fθ(t))|)+max(|ziv(t)|)≥max(|zivτ(t)+ziv(t)|)式中，xivτ(t)+xiv(t)为X轴实际速度，yivτ(t)+yiv(t)为Y轴实际速度，zivτ(t)+ziv(t)为Z轴实际速度；

vixtest＝max(viτ(t))max(|xiτt(fθ(t))|)+max(|xiv(t)|)＝riωnow2maxxiτtmax+|uix|vnow2max；

viytest＝max(viτ(t))max(|yiτt(fθ(t))|)+max(|yiv(t)|)＝riωnow2maxyiτtmax+|uiy|vnow2max；

viztest＝max(viτ(t))max(|ziτt(fθ(t))|)+max(|ziv(t)|)＝riωnow2maxziτtmax+|uiz|vnow2max；

若vixtest≤vixmax，设 否则若viytest≤viymax，设 否则若viztest≤vizmax，设 否则第i个定位器速度调节系数

第i个定位器支撑点在局部坐标系Cixyz中的切向加速度fiaτ(t)＝rifα(t)；

第i个定位器支撑点在局部坐标系Cixyz中的切向加速度最大值max(fiaτ(t))＝riαnow2max；

第i个定位器支撑点在局部坐标系Cixyz中的圆弧切向加速度矢量第i个定位器支撑点在局部坐标系Cixyz中的圆弧切向加速度分量形式如下：T

aiτ(t)＝[xiaτ(t) yiaτ(t) ziaτ(t)]式中，xiaτ(t)＝fiaτ(t)xiτt(fθ(t))，yiaτ(t)＝fiaτ(t)yiτt(fθ(t))，ziaτ(t)＝fiaτ(t)ziτt(fθ(t))；

2

第i个定位器支撑点在局部坐标系Cixyz中的向心加速度fian(t)＝rifω(t)；

第i个定位器支撑点在局部坐标系Cixyz中的向心加速度最大值max(fian(t))＝ri2

ωnow2max；

第i个定位器支撑点在局部坐标系Cixyz中的圆弧法向加速度矢量第i个定位器支撑点在局部坐标系Cixyz中的圆弧法向加速度分量形式如下：T

ain(t)＝[xian(t) yian(t) zian(t)]式中，xiaθ(t)＝fiaτ(t)xiτt(fθ(t))+fian(t)xint(fθ(t))；yiaθ(t)＝fiaτ(t)yiτt(fθ(t))+fian(t)yint(fθ(t))；ziaθ(t)＝fiaτ(t)ziτt(fθ(t))+fian(t)zint(fθ(t))；

第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动加速度aiθ(t)的X、Y、Z轴分量最大值不大于按角加速度和角速度的最大值且各轴法向分量和切向分量最大值进行计算结果，如下：

max(|xiaθ(t)|)≤max(fiaτ(t))max(|xiτt(fθ(t))|)+max(fian(t))max(|xint(fθ(t))|)max(|yiaθ(t)|)≤max(fiaτ(t))max(|yiτt(fθ(t))|)+max(fian(t))max(|yint(fθ(t))|)max(|ziaθ(t)|)≤max(fiaτ(t))max(|ziτt(fθ(t))|)+max(fian(t))max(|zint(fθ(t))|)第i个定位器支撑点在定位器局部坐标系Oixyz中的运动速度为圆弧运动加速度aiθ(t)和平动加速度fia(t)同步叠加的加速度，平动加速度X、Y、Z轴分量最大值与圆弧运动加速度X、Y、Z轴分量最大值之和大于定位器实际X、Y、Z实际的最大加速度，若小于各驱动轴允许的加速度范围，则安全，否则降低刚体复合运动的加速度和角速度，如下：aixtest≥max(|xiaθ(t)|)+max(|xia(t)|)≥max(|xiaθ(t)+xia(t)|)aiytest≥max(|yiaθ(t)|)+max(|yia(t)|)≥max(|yiaθ(t)+yia(t)|)aiztest≥max(|ziaθ(t)|)+max(|zia(t)|)≥max(|ziaθ(t)+zia(t)D式中，xiaθ(t)+xia(t)为X轴实际加速度，yiaθ(t)+yia(t)为Y轴实际加速度，ziaθ(t)+zia(t)为Z轴实际加速度；

aixtest＝max(fiaτ(t))max(|xiτt(fθ(t))|)+max(fian(t))max(|xint(fθ(t))|)+max(|xia(t)|)

2

＝riαnow2maxxiτtmax+riωnow2maxxintmax+|uix|anow2max；

aiytest＝max(fiaτ(t))max(|yiτt(fθ(t))|)+max(fian(t))max(|yint(fθ(t))|)+max(|yia(t)|)

2

＝riαnow2maxyiτtmax+riωnow2maxyintmax+|uiy|anow2max；

aiztest＝max(fiaτ(t))max(|ziτt(fθ(t))|)+max(fian(t))max(|zint(fθ(t))|)+max(|zia(t)|)

2

＝riαnow2maxziτtmax+riωnow2maxzintmax+|uiz|anow2max；

若aixtest≤aixmax，设 否则若aiytest≤aiymax，设 否则若aiztest≤aizmax，设 否则第i个定位器的加速度调节系数

若 并且 则降低刚体转动角速度，减少向心加速度，调整求出飞机调姿过程的速度调节系数最大值求出飞机调姿过程的加速度调节系数最大值根据飞机调姿过程的速度、加速度调节系数调整刚体平动和定轴转动的速度和加速度，如下：

刚体平动的最大速度和刚体定轴转动的最大角速度 ：刚体平动的最大加速度和刚体定轴转动的最大角加速度：刚体平动的加加速度和刚体定轴转动的加加速度：jl＝jl2，jθ＝jθ2。

10.根据权利要求1所述的三坐标定位器调姿运控方法，其特征在于，所述重新进行刚体复合运动速度规划并修整，具体包括：重新进行刚体平动的速度规划，如下：根据刚体平动的最大速度vmax、最大加速度amax、加加速度jl，调姿起始和终止的速度vs＝ve＝0，调姿起始和终止的加速度as＝ae＝0，刚体平动移动量S，进行铃型加/减速的刚体平动速度规划计算，计算出刚体平动的实际最大速度vnewmax和实际最大加速度anewmax；

重新计算平动运行时间TS，每阶段时间T1s，T2s，T3s，T4s，T5s，T6s，T7s；其中T1s＝T7s＝T5s＝T3s，T2s＝T6s；

速度规划时间取整，如下：

重新进行刚体定轴转动的速度规划，如下：根据刚体定轴转动的最大角速度ωmax、最大角加速度αmax、角加加速度jθ，调姿起始和终止的速度ωs＝ωe＝0，调姿起始和终止的加速度αs＝αe＝0，刚体定轴转动的角度增量θe，进行铃型加/减速的刚体定轴转动速度规划计算，计算出刚体定轴转动的实际最大角速度ωnewmax和最大角加速度αnewmax；

重新计算刚体定轴转动时间Tθ，每阶段时间为T1θ，T2θ，T3θ，T4θ，T5θ，T6θ，T7θ；其中T1θ＝T7θ＝T5θ＝T3θ，T7θ＝T6θ；

速度规划时间取整，如下：

刚体平动与刚体定轴转动同步进行，调整平动时间与定轴转动时间为相同时间，如下：Tnew＝Tnews＝Tnewθ＝max(Tnews，Tnewθ)其中各个阶段的时间按比例调整，如下：T1s＝T7s＝T5s＝T3s＝T1sKnews，T2s＝T6s＝T2sKnews，T4s＝T4sKnewsT1θ＝T7θ＝T5θ＝T3θ＝T1θKnewθ，T2θ＝T6θ＝T2θKnewθ，T4θ＝T4θKnewθ式中， 为刚体平动时间调整比例； 刚体定轴转动时间调整比例；

调整实际速度、实际加速度、实际加加速度，如下：刚体平动实际最大速度和刚体定轴转动实际角速度调整：刚体平动实际最大加速度和刚体定轴转动实际角加速度调整：刚体平动实际加加速度和刚体定轴转动实际加加速度调整：得到调整后的速度规划：

刚体平动：s＝fs3(t)；v＝fv3(t)；a＝fa3(t)；其中t∈[0，Tnew]；s∈[0，S]；

刚体定轴转动：θ＝fθ3(t)；ω＝fω3(t)；α＝fα3(t)；其中t∈[0，Tnew]；θ∈[0，θe]。

11.根据权利要求1‑10任一项所述的三坐标定位器调姿运控方法，其特征在于，所述根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置，具体包括：

根据刚体复合运动新的速度规划，按时间分割法分别进行刚体定轴转动插补和刚体平动插补，同步叠加后计算出每个定位器支撑点在全局坐标系位置，再转换到每个定位器局部坐标系，输出每个定位器三轴指令坐标位置。

12.根据权利要求11所述的三坐标定位器调姿运控方法，其特征在于，所述根据刚体复合运动新的速度规划，按时间分割法分别进行刚体定轴转动插补和刚体平动插补，同步叠加后计算出每个定位器支撑点在全局坐标系位置，再转换到每个定位器局部坐标系，输出每个定位器三轴指令坐标位置，具体包括：刚体局部坐标系Olxyz的原点调姿前在全局坐标系中位置定位器局部坐标系Oixyz的原点在全局坐标中位置第i个定位器支撑点调姿前的位置，包括在全局坐标系中的位置 在刚体局部坐标系Olxyz中位置 以及在定位器局部坐标系Oixyz中位置

第i个定位器支撑点调姿后的位置，包括在刚体局部坐标系Olxyz中的位置 在全局坐标中的位置 以及在定位器局部坐标系Oixyz中位置计算第i个定位器支撑点调姿过程的插补位置，如下：刚体定轴转动的转动角度函数为：

θ＝fθ3(t)；ω＝fω3(t)；α＝fα3(t)；t∈[0，Tnew]；θ∈[0，θe]；

刚体转动轴在刚体局部坐标系Olxyz的矢量刚体平动的移动量函数为：

s＝fs3(t)；v＝fv3(t)；a＝fa3(t)；t∈[0，Tnew]；s∈[0，S]；

刚体平动方向矢量

解算第i个定位器支撑点在定位器局部坐标系Oixyz位置函数 如下式：式中， 为第i个定位器支撑点在全局坐标系中的位置矢量，如下式：式中， 是位姿调整过程中刚体局部坐标系原点在全局坐标系的位置，刚体定轴转动与平动同步叠加时，第i个定位器支撑点在定位器局部坐标系Oixyz的位置计算公式如下：

式中， sθ＝sin(θ)，cθ＝cos(θ)，vθ＝1‑cos(θ)，θ＝fθ3(t)，t∈[0，Tnew]，θ∈[0，θe]；

第i个定位器支撑点调姿过程在定位器局部坐标系Oixyz的位置分量形式如下：式中， 为插补输出的X轴指令坐标位置， 为插补输出的Y轴指令坐标位置，为插补输出的Z轴指令坐标位置。

13.根据权利要求1‑10任一项所述的三坐标定位器调姿运控方法，其特征在于，所述根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置，具体包括：

根据每个定位器三轴指令坐标位置，进行各轴的反向间隙补偿和螺距误差补偿，再根据空间误差数据进行三轴补偿处理，输出各轴驱动器的实际位置。

14.根据权利要求13所述的三坐标定位器调姿运控方法，其特征在于，所述根据每个定位器三轴指令坐标位置，进行各轴的反向间隙补偿和螺距误差补偿，再根据空间误差数据进行三轴补偿处理，输出各轴驱动器的实际位置，具体包括：读取每个定位器的X、Y、Z轴指令坐标位置，进行各轴的反向间隙补偿和螺距误差补偿，若在单轴测量点，则直接读取反向间隙补偿数据和螺距误差补偿数据，若不在单轴测量点，则按所在单轴测量区间的两端点误差值进行线性插值，补偿所在单轴测量区间的单轴螺距误差，再加上反向间隙值；

读取每个定位器的X、Y、Z轴指令坐标位置，进行空间误差补偿，若在空间误差测量的方格顶点上，则直接读取数据进行补偿，若在某个方格内不在空间误差测量的方格顶点上，读取这个方格的八个顶点误差数据进行空间线性插值，解算出这个方格的八个顶点拟合的空间误差；

当进行各轴的反向间隙补偿和螺距误差补偿，以及空间误差补偿后，输出各轴驱动器的实际位置。

15.一种三坐标定位器调姿运控系统，应用于飞机数字化装配中，其特征在于，所述系统包括：

第一解算模块，用于建立飞机大部件刚体局部坐标系和定位器局部坐标系，按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量；

第二解算模块，用于根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的空间圆弧轨迹的圆心和半径，并按每个圆弧轨迹允许误差解算相应的圆弧角速度范围和刚体角速度范围；

第一规划与修整模块，用于根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度规划并修整；

调整模块，用于根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的速度范围和加速度范围调整刚体复合运动的速度和加速度；

第二规划与修整模块，用于当调整刚体复合运动的速度和加速度后，重新进行刚体复合运动速度规划并修整；

插补计算模块，用于根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置；

补偿模块，用于根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置；

所述按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量，具体包括：

解算刚体的平动移动量，如下式：

解算刚体的平动方向矢量，如下式：式中，

先绕刚体的Z轴转动A角度，再绕刚体的Y轴转动B角度，最后绕刚体的X轴转动C角度，如下式：

式中，s1＝sin(A)，c1＝cos(A)，s2＝sin(B)，c2＝cos(B)，s3＝sin(C)，c3＝cos(C)；

解算刚体定轴转动的转动角度，如下式：解算刚体定轴转动的转动轴矢量，如下式：式中，

16.一种调姿控制器，包括处理器以及用于存储处理器可执行程序的存储器，其特征在于，所述处理器执行存储器存储的程序时，实现权利要求1‑14任一项所述的三坐标定位器调姿运控方法。

17.一种存储介质，存储有程序，其特征在于，所述程序被处理器执行时，实现权利要求

1‑14任一项所述的三坐标定位器调姿运控方法。

说明书

三坐标定位器调姿运控方法、系统、调姿控制器及存储介质

技术领域

[0001] 本发明涉及一种三坐标定位器调姿运控方法、系统、调姿控制器及存储介质，属于飞机数字化装配的研究领域。

背景技术

[0002] 飞机装配是飞机制造过程中最重要、最复杂的环节之一。飞机部件或组件的空间定位问题是飞机装配中的共性关键问题，定位精度将直接影响飞机产品的气动外形、疲劳
寿命与可靠性等。为了能够提高飞机部件姿态调整的质量与效率，目前先进的调姿技术正
朝着数字化、自动化、柔性化的方向发展。

[0003] 飞机部件自动化调姿机构主要由多个三坐标数控定位器、工艺接口构成。工艺接头一端与飞机部件固连，另一端与定位器构成球面副或球绞副。定位器可沿三个相互垂直
的方向(X、Y、Z)运动，改变其支撑点(工艺接头球心)位置。空间3点能确定一个平面，飞机部
件由3个或3个以上的定位器支撑。多个定位器可沿各自X、Y、Z轴运动，飞机部件在三维空间
中可实现六自由度的位姿调整。3个数控定位器单元能接收9个控制轴运动指令输入，有冗
余驱动轴。大部件调姿设备有4或6或8个定位器，则一共有12或18或24个驱动轴。由多个定
位器与飞机部件组成的调姿装置是冗余驱动并联结构。这些轴协同运动，使得大部件放到
这些定位器上，可以在空间内平移和转动，并且确保运动过程中大部件或者工件不会变型。
并联结构具有高刚度、高精度、结构稳定、承载力分布均匀的优点。在调姿过程中，如何保证
所有的数控定位器支撑点按刚体运动规则协同运动是一个关键问题。

[0004] 定位器驱动布设方向最好能与全局坐标系平行，多个定位器的X、Y、Z轴向一般是平行且方向相同。但定位器安装过程测量精度不足，调整不到位，则仍有安装角度误差。

[0005] 飞机大部件是刚体，刚体运动包括刚体平动和刚体定轴转动。调姿指令数据包括X、Y、Z轴平动移动量，A、B、C转动角增量。调姿运控包括调姿指令分解、插补处理、速度规划、
误差补偿，最终输出实际位置给每个定位器的伺服驱动。

[0006] 飞机调姿装配全过程如下。

[0007] (1)部件入位：将飞机部件吊到定位器上；

[0008] (2)初始测量：通过激光跟踪仪对部件测量点位置进行测量，获取初始位置；

[0009] (3)初始调整：计算偏差发位姿调整指令，通过定位器对部件进行调整；

[0010] (4)精确测量：通过激光跟踪仪对部件测量点位置再次进行测量；

[0011] (5)精确调整：若存在偏差，则再次发出指令进行位姿调整；

[0012] (6)检测是否完成调姿目标，完成装配则下架或撤离；否则重复第(4)步、第(5)步调整好位置。

[0013] 目前国内飞机调姿的运控方法有以下几种情况：

[0014] (1)无冗余轴的定位器调姿

[0015] 无冗余驱动轴存在浮动轴或随动轴，调姿过程中浮动轴随大部件姿态位置变化可自由滑动，可直接从定位器支撑点起点移动到调姿终点位置，不必将支撑点作为刚体上质
点进行最优路径分析。六自由度飞机调姿可采用三个定位器六个主动控制轴。例如机翼调
姿装置有3个数控定位器，第一个定位器有X、Y、Z驱动轴，第2个定位器只有Y、Z驱动轴(X轴
浮动)，第3个定位器只有Z驱动轴(XY轴浮动)。

[0016] A、减少定位器的主动控制轴数，定位器有一部分轴采用浮动轴，有浮动轴的定位器调姿装置，在调姿过程中主动控制轴调整飞机部件姿态，飞机部件可拖动浮动轴滑动。

[0017] B、调姿机构力_位置混合控制系统，定位器中一部分是主动控制轴，其他冗余驱动轴作为从动轴，在调姿过程中主动控制轴调整飞机部件姿态，根据工艺接头上力传感器反
馈力大小从动轴跟随拉力方向移动。

[0018] (2)有冗余轴的的定位器调姿

[0019] 有冗余驱动轴的定位器的六自由度飞机调姿必须将飞机部件作为刚体考虑来计算，遵行刚体中任意两质点间距离始终保持不变的原则，插补算法处理较复杂。位姿调整一
般分为姿态调整和位置调整阶段，一般在时间上不重叠。姿态调整和位置调整釆用按时间
先后分别进行分步转动和平动的算法求解。每阶段运动规划采用位置‑‑时间模式，一般采
用不分段的整体三次或四次多项式进行速度规划。求解出每个定位器支撑点在空间移动的
曲线上离散点，直接输出位置坐标给定位器的驱动器。

[0020] 有冗余驱动轴6自由度调姿过程一般为以下4个阶段：

[0021] A、俯仰角、偏转角和位置均保持不变，绕自身坐标轴转动，横滚角调整至目标角度。

[0022] B、横滚角、偏转角和位置均保持不变，绕自身坐标轴转动，俯仰角调整至目标角度。

[0023] C、横滚角、俯仰角和位置均保持不变，绕自身坐标轴转动，偏转角调整至目标角度。

[0024] D、姿态保持不变，位置从初始位置平动至目标位置。

[0025] 综上所述，目前调姿运控方法现状分析：

[0026] 1)没有冗余主动驱动轴方式的定位器主动驱动轴数量少，控制简单，成本低。六自由度飞机调姿可采用三个定位器六个主动控制轴，有浮动轴的定位器调姿装置。浮动轴方
式需要飞机大部件提供拉力拖动，飞机大部件内应力较大。随动方式跟随应力变化控制从
动轴，可减少调姿过程飞机部件应力，但从动轴不能主动提供足够支撑力，调姿过程不能对
部件保型。没有冗余主动驱动轴方式主要不足是定位器数量少，支撑刚度不够，局部支撑力
较大易引起飞机局部变形。这种方式在调姿过程不能对飞机部件进行保型或支撑。由于大
飞机尺寸大、重量重，需要更多的定位器承重，并且要提供足够多支撑点形成柔性支架进行
保型，没有冗余主动驱动轴的定位器调姿方式不适合大型飞机调姿；

[0027] 2)目前冗余驱动轴三坐标定位器六自由度的飞机调姿一般是分步执行姿态调整后再执行位置调整，调姿时间较长；

[0028] 3)运动规划过程没有对加速度进行准确计算，飞机调姿过程有可能出现超过三坐标数控定位器的驱动电机速度/加速度允许范围或飞机大部件转动或平动的速度/加速度
允许范围，调姿运行不平稳或调姿动作缓慢，特别是在调姿启动时和调姿结束时电机容易
有冲击；

[0029] 4)调姿过程没有对支撑点位置误差和轮廓误差进行准确分析，若轮廓误差大或定位器机械定位精度、安装精度不够高，飞机调姿过程飞机大部件可能少量变形，导致受到过
大应力甚至损伤。

发明内容

[0030] 有鉴于此，本发明提供了一种三坐标定位器调姿运控方法、系统、调姿控制器及存储介质，其应用于飞机数字化装配中，可以满足加速度要求且有冗余驱动轴，不需要分步进
行转动和平动，不需要限制驱动器轴数，调姿过程平稳无冲击，可以克服现有飞机装配中三
坐标定位器调姿运控处理存在的不足之处。

[0031] 本发明的第一个目的在于提供一种三坐标定位器调姿运控方法。

[0032] 本发明的第二个目的在于提供一种三坐标定位器调姿运控系统。

[0033] 本发明的第三个目的在于提供一种调姿控制器。

[0034] 本发明的第四个目的在于提供一种存储介质。

[0035] 本发明的第一个目的可以通过采取如下技术方案达到：

[0036] 一种三坐标定位器调姿运控方法，应用于飞机数字化装配中，所述方法包括：

[0037] 建立飞机大部件刚体局部坐标系和定位器局部坐标系，按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量；

[0038] 根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的空间圆
弧轨迹的圆心和半径，并按每个圆弧轨迹允许误差解算相应的圆弧角速度范围和刚体角速
度范围；

[0039] 根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度规划并修整；

[0040] 根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的速度范围和加速
度范围调整刚体复合运动的速度和加速度；

[0041] 当调整刚体复合运动的速度和加速度后，重新进行刚体复合运动速度规划并修整；

[0042] 根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置；

[0043] 根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置。

[0044] 进一步的，所述建立飞机大部件刚体局部坐标系和定位器局部坐标系，具体包括：

[0045] 为相对地面参照系建立全局坐标系Oxyz；

[0046] 为飞机大部件建立刚体局部坐标系Olxyz，刚体局部坐标系的X、Y、Z轴向与全局坐标系的X、Y、Z轴平行，刚体局部坐标系的转动中心设为全局坐标系的原点，局部坐标系的原
T
点调姿前在全局坐标系的坐标为Ol(px，py，pz) ；

[0047] 为第i个定位器建立定位器局部坐标系Oixyz，定位器局部坐标系的X、Y、Z轴向是定位器的支撑点沿定位器的X、Y、Z轴驱动移动的方向，定位器局部坐标系的原点是定位器
的X、Y、Z轴回零后的支撑点位置，第i个定位器局部坐标系的原点在全局坐标系的坐标为Oi
T T
(oix，oiy，oiz) ，第i个定位器支撑点在全局坐标系的坐标为Pi(pix，piy，piz) 。

[0048] 进一步的，所述按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量，具体包括：

[0049] 解算刚体的平动移动量，如下式：

[0050]

[0051] 解算刚体的平动方向矢量，如下式：

[0052]

[0053] 式中，

[0054] 先绕刚体的Z轴转动A角度，再绕刚体的Y轴转动B角度，最后绕刚体的X轴转动C角度，如下式：

[0055]

[0056] 式中，s1＝sin(A)，c1＝cos(A)，s2＝sin(B)，c2＝cos(B)，s3＝sin(C)，c3＝cos(C)；

[0057] 解算刚体定轴转动的转动角度，如下式：

[0058]

[0059] 解算刚体定轴转动的转动轴矢量，如下式：

[0060]

[0061] 式中，

[0062] 进一步的，所述根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个
定位器的空间圆弧轨迹的圆心和半径，按每个圆弧轨迹允许误差解算相应的圆弧角速度范
围和刚体角速度范围，具体包括：

[0063] 根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的空间圆
弧轨迹的圆心和半径，如下式：

[0064]

[0065]

[0066] 式中， 为刚体定轴转动过程的第i个定位器的空间圆弧轨迹的圆心位置， 为定位器第i个支撑点调姿前在刚体局部坐标系Olxyz中的位置， 为刚体定轴转动的转动轴
矢量，ri为刚体定轴转动过程的第i个定位器的空间圆弧轨迹的半径：

[0067] 设Tinp为插补周期时间，Herr为弓高误差，Lc为插补周期输出量弦长，如下式：

[0068] Lc2＝8Herrri+4Herr2

[0069] 计算满足弓高误差Herr的弦长，如下式：

[0070]

[0071] 式中，Lc＝VTinpo＝ωiriTinp，则

[0072] 若第i个定位器调姿过程按允许的弓高误差Herr，则要求刚体转动角速度ωi最大，并且弓高误差Herr不变时，半径ri越大允许的刚体转动角速度ωi越小；

[0073] 求出所有定位器空间圆弧轨迹半径中的最大半径，如下式：

[0074] rmax＝max(r1,r2,r3,…,ri,…,rN)

[0075] 插补时，插补周期时间Tinp相同，且刚体定轴转动多个圆弧角速度相同；

[0076] 求出弓高误差Herr满足要求的刚体绕 转动角速度最大值，如下式：

[0077]

[0078] 进一步的，所述根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度规划并修整，
具体包括：

[0079] 根据刚体角速度最大值，调整刚体定轴转动结构强度要求的最大角速度ωmax＝min(ωmax1,ωmax2)；

[0080] 根据飞机大部件调姿允许的最大角加速度αmax、最大平动速度vmax、最大平动加速度amax，刚体定轴转动结构强度要求的最大角速度ωmax＝min(ωmax1,ωmax2)，调姿起始和终
止的速度vs＝ve＝0,，调姿起始和终止的加速度as＝ae＝0，刚体定轴转动的转动角度θe和刚
体的平动移动量S，分别进行铃型加/减速的刚体复合运动速度规划计算，计算出各阶段时
间、平动实际最大加速度anowmax、平动实际最大速度vnowmax、转动实际最大角加速度αnowmax和
实际最大角速度ωnowmax，随后进行刚体复合运动速度规划时间修整。

[0081] 进一步的，所述进行铃型加/减速的刚体复合运动速度规划计算，具体包括：

[0082] 根据刚体的平动移动量s、最大平动速度vmax、最大平动加速度amax和平动加加速jl，分为加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减减速
阶段共七个阶段进行速度规划计算；其中：

[0083] 加加速的计算如下式：

[0084]

[0085] 式中，J＝jl；

[0086] 加速度的计算如下式：

[0087]

[0088] 速度的计算如下式：

[0089]

[0090] 式中，T1＝t1,T2＝t2‑t1,T3＝t3‑t2；T4＝t4‑t3,T5＝t5‑t4,T6＝t6‑t5；T7＝t7‑t6,T1＝T3,T5＝T7；

[0091] 平动移动量的计算如下式：

[0092]

[0093] 平动的速度规划为s＝fs1(t)，v＝fv1(t)，a＝fa1(t)，js＝J(t)；

[0094] 根据铃型加/减速时T1s＝T3s,T5s＝T7s，调姿起始和终止的速度vs＝ve＝0，调姿起始和终止的加速度as＝ae＝0，以及加速阶段与减速阶段镜像对称，得到T1s＝T7s＝T5s＝T3s，
T2s＝T6s；

[0095] 最大速度和最大加速度在安全范围约束条件为v3≤vmax，Jt1≤amax，求出加加速阶段时间T1s、匀加速阶段时间T2s、减加速阶段时间T3s、匀速阶段时间T4s、加减速阶段时间T5s、
匀减速阶段时间T6s、减减速阶段时间T7s；

[0096] 计算实际最大加速度anowmax＝jlT1s，实际最大速度vnowmax＝v3，以及平动总时间Ts＝T1s+T2s+T3s+T4s+T5s+T6s+T7s；

[0097] 根据刚体定轴转动的转动角度θe、最大角速度ωmax、最大角加速度αmax和转动角加加速jθ，分为加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减
减速阶段共七个阶段进行速度规划的计算；其中：

[0098] 角加加速的计算如下式：

[0099]

[0100] 式中，J＝jθ；

[0101] 角加速度的计算如下式：

[0102]

[0103] 角速度的计算如下式：

[0104]

[0105] 式中，T1＝t1,T2＝t2‑t1,T3＝t3‑t2；T4＝t4‑t3,T5＝t5‑t4,T6＝t6‑t5；T7＝t7‑t6,T1＝T3,T5＝T7；

[0106] 转动角度的计算如下式：

[0107]

[0108] 转动的速度规划为jθ＝J(t)，θ＝fθ1(t)，ω＝fω1(t)，α＝fα1(t)；

[0109] 根据铃型加/减速时T1θ＝T3θ,T5θ＝T7θ，建立转动角度的分段函数，并根据角加加速度J＝jθ，转动角度调姿增量θe，调姿起始和终止的角速度ωs＝ωe＝0，调姿起始和终止的
角加速度αs＝αe＝0，以及加速阶段与减速阶段镜像对称，得到T1θ＝T7θ＝T5θ＝T3θ，T2θ＝T6θ；
T1θ＝T7θ＝T5θ＝T3θ，T2θ＝T6θ；

[0110] 最大速度和最大加速度在安全范围约束条件为ω3≤ωmax，Jt1≤αmax，求出加加速阶段时间T1θ、匀加速阶段时间T2θ、减加速阶段时间T3θ、匀速阶段时间T4θ、加减速阶段时间
T5θ、匀减速阶段时间T6θ、减减速阶段时间T7θ；

[0111] 计算实际最大角加速度αnowmax，实际最大角速度ωnowmax，以及转动总时间Tθ＝T1θ+T2θ+T3θ+T4θ+T5θ+T6θ+T7θ。

[0112] 进一步的，所述进行刚体复合运动速度规划时间修整，具体包括：

[0113] 对刚体定轴转动和刚体平动分别进行铃型加/减速的速度规划两个运动时间修整为插补周期时间Tinp整数倍而且调整平动时间与转动时间相等，计算调整时间轴后的各阶
段时间、刚体平动实际最大加速度anow2max、刚体平动实际最大速度vnow2max、刚体定轴转动实
际最大角加速度αnow2max、刚体定轴转动实际最大角速度ωnow2max。

[0114] 进一步的，所述计算调整时间轴后的各阶段时间、刚体平动实际最大加速度anow2max、刚体平动实际最大速度vnow2max、刚体定轴转动实际最大角加速度αnow2max、刚体定轴
转动实际最大角速度ωnow2max，具体包括：

[0115] 将平动总时间Ts或转动总时间Tθ向上取整为插补周期时间Tinp的整数倍，如下式：

[0116]

[0117] 刚体平动与刚体定轴转动同步进行，要求同时开始、同时结束，则调整平动时间与定轴转动时间为相同时间，如下式：

[0118] Tnow＝Tnows＝Tnowθ＝max(Tnows,Tnowθ)

[0119] 各阶段的时间按比例调整，如下：

[0120] 刚体平动时间调整比例：

[0121] 刚体定轴转动时间调整比例：

[0122] T1s＝T7s＝T5s＝T3s＝T1sKnows，T2s＝T6s＝T2sKnows，T4s＝T4sKnows

[0123] T1θ＝T7θ＝T5θ＝T3θ＝T1θKnowθ，T2θ＝T6θ＝T2θKnowθ，T4θ＝T4θKnowθ

[0124] 平动距离S和转动角度θe保持不变，Tnows和Tnowθ时间变长，每个阶段的时间变长，调整实际速度、实际加速度、实际加加速度，如下：

[0125] 刚体平动实际最大速度和刚体定轴转动实际最大角速度调整：

[0126] 刚体平动实际最大加速度和刚体定轴转动实际最大角加速度调整：

[0127] 刚体平动实际加加速度和刚体定轴转动实际加加速度调整：

[0128] 当按比例调整各阶段时间、速度、加速度、加加速度后，各阶段刚体平动移动量和刚体定轴转动角度与调整前保持不变，得到调整后的速度规划如下：

[0129] 刚体平动：s＝fs(t)；v＝fv(t)；a＝fa(t)；其中t∈[0,Tnow]；s∈[0,S]

[0130] 刚体定轴转动：θ＝fθ(t)；ω＝fω(t)；α＝fα(t)；其中t∈[0,Tnow]；θ∈[0,θe]。

[0131] 进一步的，所述根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的
速度范围和加速度范围调整刚体复合运动的速度和加速度，具体包括：

[0132] 将每个定位器支撑点作为刚体上质点，刚体进行定轴转动和平动的同步叠加运动，利用逆向运动学位置反解，求解出每个定位器驱动轴的空间圆弧运动轨迹、速度和加速
度，在定位器局部坐标系中计算每个定位器驱动轴上的最大速度和最大加速度，并按每个
定位器驱动轴允许的速度范围和加速度范围调整刚体复合运动的速度和加速度。

[0133] 进一步的，所述将每个定位器支撑点作为刚体上质点，刚体进行定轴转动和平动的同步叠加运动，利用逆向运动学位置反解，求解出每个定位器驱动轴的空间圆弧运动轨
迹、速度和加速度，在定位器局部坐标系中计算每个定位器驱动轴上的最大速度和最大加
速度，并按每个定位器驱动轴允许的速度范围和加速度范围调整刚体复合运动的速度和加
速度，具体包括：

[0134] 以第i个定位器支撑点空间圆弧Ci的圆心为坐标原点建立新的局部坐标系Cixyz，局部坐标系Cixyz的X、Y、Z轴平行于定位器局部坐标系Oixyz的X、Y、Z轴；

[0135] 空间圆弧运动轨迹中的圆心角为刚体旋转的角度θ＝fθ(t)；

[0136] 刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移距离s＝fs(t)；

[0137] 刚体平动方向矢量 其中

[0138] 对应局部坐标系Cixyz的原点在定位器局部坐标系Oixyz方向矢量

[0139] 刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移位置分量形式如下：

[0140] fis(t)＝[xis(t) yis(t) zis(t)]T

[0141] 式中，xis(t)＝uixfs(t)，yis(t)＝uiyfs(t)，zis(t)＝uizfs(t)；

[0142] 刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移速度分量形式如下：

[0143] fiv(t)＝[xiv(t) yiv(t) ziv(t)]T

[0144] 式中，xiv(t)＝uixfv(t)，yiv(t)＝uiyfv(t)，ziv(t)＝uizfv(t)；

[0145] 定位器局部坐标系Oixyz中X、Y、Z轴平动过程的最大速度，如下式：

[0146] viSxmax＝max(|xiv(t)|)＝|uix|max(fv(t))＝|uix|vnow2max

[0147] viSymax＝max(|yiv(t)|)＝|uiy|max(fv(t))＝|uiy|vnow2max

[0148] viSzmax＝max(|ziv(t)|)＝|uiz|max(fv(t))＝|uiz|vnow2max

[0149] 刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移加速度分量形式如下：

[0150] fia(t)＝[xia(t) yia(t) zia(t)]T

[0151] 式中，xia(t)＝uixfa(t)，yia(t)＝uiyfa(t)，zia(t)＝uizfa(t)；

[0152] 定位器局部坐标系Oixyz中X、Y、Z轴平动过程的最大加速度，如下式：

[0153] aiSxmax＝max(|xia(t)|)＝|uix|max(fa(t))＝|uix|anow2max

[0154] aiSymax＝max(|yia(t)|)＝|uiy|max(fa(t))＝|uiy|anow2max

[0155] aiSzmax＝max(|zia(t)|)＝|uiz|max(fa(t))＝|uiz|anow2max。

[0156] 进一步的，所述按每个定位器驱动轴允许的速度范围和加速度范围调整刚体复合运动的速度和加速度，具体包括：

[0157] 刚体定轴转动中刚体局部坐标系Olxyz的转动轴 刚体定轴转动的转动角度θ＝fθ(t)；ω＝fω(t)；α＝fα(t)；其中t∈[0,Tnow]；θ∈[0,θe]；

[0158] 调姿前第i个定位器支撑点在刚体局部坐标系Olxyz中的位置矢量

[0159] 第i个定位器在刚体局部坐标系Olxyz中的旋转半径 圆心坐标

[0160] 刚体定轴转动角度θ＝fθ(t)，刚体旋转Rw,θ，如下式：

[0161]

[0162] 式中，sθ＝sin(θ)，cθ＝cos(θ),vθ＝1‑cos(θ)，θ＝fθ(t)，t∈[0,Tnow]，θ∈[0,θe]；调姿过程中第i个定位器支撑点在刚体局部坐标系Olxyz中的位置矢量为 刚体转动
轴在局部坐标系Cixyz中为

[0163] 第i个定位器支撑点在局部坐标系Cixyz中的位置Ci(θ)为旋转角度θ函数，如下式：

[0164]

[0165]

[0166] 第i个定位器支撑点在局部坐标系Cixyz中的位置Ci(θ)的分量形式如下：

[0167] Ci(θ)＝[xi(θ) yi(θ) zi(θ)]T，xi(θ)，yi(θ)，zi(θ)

[0168] Ci任意θ位置的法向方向矢量，如下式：

[0169]

[0170]

[0171] Ci任意θ位置的法向方向矢量分量形式如下：

[0172] xin(θ)，yin(θ)，zin(θ)；

[0173] xint(fθ(t))，yint(fθ(t))，zint(fθ(t))；

[0174] Ci任意θ位置的法向方向矢量X、Y、Z轴分量绝对值最大值，如下式：

[0175] xintmax＝max(|xint(fθ(t))|)＝max(|xin(θ)|)

[0176] yintmax＝max(|yint(fθ(t))|）＝max(|yin(θ)|)

[0177] zintmax＝max(|zint(fθ(t))|)＝max(|zin(θ)|)

[0178] Ci任意θ位置的切向方向矢量，如下式：

[0179] 其中

[0180]

[0181] Ci任意θ位置的切向方向矢量分量形式如下：

[0182] xiτ(θ)，yiτ(θ)，ziτ(θ)；

[0183] xiτt(fθ(t))，yiτt(fθ(t))，ziτt(fθ(t))；

[0184] Ci任意θ位置的切向方向矢量X、Y、Z轴分量绝对值最大值，如下式：

[0185] xiτtmax＝max(|xiτt(fθ(t))|)＝max(|xiτ(θ)|)

[0186] yiτtmax＝max(|yiτt(fθ(t))|)＝max(|yiτ(θ)|)

[0187] ziτtmax＝max(|ziτt(fθ(t))|)＝max(|ziτ(θ)|)

[0188] 第i个定位器支撑点在局部坐标系Cixyz中的线速度viτ(t)＝rifω(t)；

[0189] 第i个定位器支撑点在局部坐标系Cixyz中的最大线速度max(viτ(t))＝riωnow2max；

[0190] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动速度

[0191] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动速度分量形式如下：

[0192] fivτ(t)＝[xivτ(t) yivτ(t) zivτ(t)]T

[0193] 式中，xivτ(t)＝viτ(t)xiτt(fθ(t))，yivτ(t)＝viτ(t)yiτt(fθ(t))，zivτ(t)＝viτ(t)ziτt(fθ(t))；

[0194] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动最大线速度乘以方向矢量各轴分量最大值不小于圆弧运动fivτ(t)各轴最大速度，如下：

[0195] max(|xivτ(t)|)＝max(viτ(t)|xiτt(fθ(t))|)≤max(viτ(t))max(|xiτt(fθ(t))|)

[0196] max(|yivτ(t)|)＝max(viτ(t)|yiτt(fθ(t))|)≤max(viτ(t))max(|yiτt(fθ(t))|)

[0197] max(|zivτ(t)|)＝max(viτ(t)|ziτt(fθ(t))|)≤max(viτ(t))max(|ziτt(fθ(t))|)

[0198] 第i个定位器支撑点在定位器局部坐标系Oixyz中的运动速度为圆弧运动速度fivτ(t)和平动速度fiv(t)同步叠加的速度，平动速度X、Y、Z轴分量最大速度与圆弧运动速度X、
Y、Z轴分量最大速度之和大于定位器实际X、Y、Z实际的最大速度，若小于各驱动轴允许的速
度范围，则安全，否则降低刚体复合运动的速度，如下：

[0199] vixtest≥max(viτ(t)|xiτt(fθ(t))|)+max(|xiv(t)|)≥max(|xivτ(t)+xiv(t)|)

[0200] viytest≥max(viτ(t)|yiτt(fθ(t))|)+max(|yiv(t)|)≥max(|yivτ(t)+yiv(t)|)

[0201] viztest≥max(viτ(t)|ziτt(fθ(t))|)+max(|ziv(t)|)≥max(|zivτ(t)+ziv(t)|)

[0202] 式中，xivτ(t)+xiv(t)为X轴实际速度，yivτ(t)+yiv(t)为Y轴实际速度，zivτ(t)+ziv(t)为Z轴实际速度；

[0203] vixtest＝max(viτ(t))max(|xiτt(fθ(t))|)+max(|xiv(t)|)＝riωnow2maxxiτtmax+|uix|vnow2max；

[0204] viytest＝max(viτ(t))max(|yiτt(fθ(t))|)+max(|yiv(t)|)＝riωnow2maxyiτtmax+|uiy|vnow2max；

[0205] viztest＝max(viτ(t))max(|ziτt(fθ(t))|)+max(|ziv(t)|)＝riωnow2maxziτtmax+|uiz|vnow2max；

[0206] 若vixtest≤vixmax，设 否则

[0207] 若viytest≤viymax，设 否则

[0208] 若viztest≤vizmax，设 否则

[0209] 第i个定位器速度调节系数

[0210] 第i个定位器支撑点在局部坐标系Cixyz中的切向加速度fiaτ(t)＝rifα(t)；

[0211] 第i个定位器支撑点在局部坐标系Cixyz中的切向加速度最大值max(fiaτ(t))＝riαnow2max；

[0212] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧切向加速度矢量

[0213] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧切向加速度分量形式如下：

[0214] aiτ(t)＝[xiaτ(t) yiaτ(t) ziaτ(t)]T

[0215] 式中，xiaτ(t)＝fiaτ(t)xiτt(fθ(t))，yiaτ(t)＝fiaτ(t)yiτt(fθ(t))，ziaτ(t)＝fiaτ(t)ziτt(fθ(t))；

[0216] 第i个定位器支撑点在局部坐标系Cixyz中的向心加速度fian(t)＝rifω2(t)；

[0217] 第i个定位器支撑点在局部坐标系Cixyz中的向心加速度最大值max(fian(t))＝ri2
ωnow2max；

[0218] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧法向加速度矢量

[0219] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧法向加速度分量形式如下：

[0220] ain(t)＝[xian(t) yian(t) zian(t)]T

[0221] 式中，xiaθ(t)＝fiaτ(t)xiτt(fθ(t))+fian(t)xint(fθ(t))；yiaθ(t)＝fiaτ(t)yiτt(fθ(t))+fian(t)yint(fθ(t))；ziaθ(t)＝fiaτ(t)ziτt(fθ(t))+fian(t)zint(fθ(t))；

[0222] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动加速度aiθ(t)的X、Y、Z轴分量最大值不大于按角加速度和角速度的最大值且各轴法向分量和切向分量最大值进行计
算结果，如下：

[0223] max(|xiaθ(t)|)≤max(fiaτ(t))max(|xiτt(fθ(t))|)+max(fian(t))max(|xint(fθ(t))|)

[0224] max(|yiaθ(t)|)≤max(fiaτ(t))max(|yiτt(fθ(t))|)+max(fian(t))max(|yint(fθ(t))|)

[0225] max(|ziaθ(t)|)≤max(fiaτ(t))max(|ziτt(fθ(t))|)+max(fian(t))max(|zint(fθ(t))|)

[0226] 第i个定位器支撑点在定位器局部坐标系Oixyz中的运动速度为圆弧运动加速度aiθ(t)和平动加速度fia(t)同步叠加的加速度，平动加速度X、Y、Z轴分量最大值与圆弧运动
加速度X、Y、Z轴分量最大值之和大于定位器实际X、Y、Z实际的最大加速度，若小于各驱动轴
允许的加速度范围，则安全，否则降低刚体复合运动的加速度和角速度，如下：

[0227] aixtest≥max(|xiaθ(t)|)+max(|xia(t)|)≥max(|xiaθ(t)+xia(t)|)

[0228] aiytest≥max(|yiaθ(t)|)+max(|yia(t)|)≥max(|yiaθ(t)+yia(t)|)

[0229] aiztest≥max(|ziaθ(t)|)+max(|zia(t)|)≥max(|ziaθ(t)+zia(t)|)

[0230] 式中，xiaθ(t)+xia(t)为X轴实际加速度，yiaθ(t)+yia(t)为Y轴实际加速度，ziaθ(t)+zia(t)为Z轴实际加速度；

[0231] aixtest＝max(fiaτ(t))max(|xiτt(fθ(t))|)+max(fian(t))max(|xint(fθ(t))|)+max(|xia(t)|)

[0232]      ＝riαnow2maxxiτtmax+riωnow2max2xintmax+|uix|anow2max；

[0233] aiytest＝max(fiaτ(t))max(|yiτt(fθ(t))|)+max(fian(t))max(|yint(fθ(t))|)+max(|yia(t)|)

[0234]      ＝riαnow2maxyiτtmax+riωnow2max2yintmax+|uiy|anow2max；

[0235] aiztest＝max(fiaτ(t))max(|ziτt(fθ(t))|)+max(fian(t))max(|zint(fθ(t))|)+max(|zia(t)|)

[0236]      ＝riαnow2maxziτtmax+riωnow2max2zintmax+|uiz|anow2max；

[0237] 若aixtest≤aixmax，设 否则

[0238] 若aiytest≤aiymax，设 否则

[0239] 若aiztest≤aizmax，设 否则

[0240] 第i个定位器的加速度调节系数

[0241] 若 并且 则降低刚体转动角速度，减少向心加速度，调整

[0242] 求出飞机调姿过程的速度调节系数最大值

[0243] 求出飞机调姿过程的加速度调节系数最大值

[0244] 根据飞机调姿过程的速度、加速度调节系数调整刚体平动和定轴转动的速度和加速度，如下：

[0245] 刚体平动的最大速度和刚体定轴转动的最大角速度：

[0246] 刚体平动的最大加速度和刚体定轴转动的最大角加速度：

[0247] 刚体平动的加加速度和刚体定轴转动的加加速度：jl＝jl2，jθ＝jθ2。

[0248] 进一步的，所述重新进行刚体复合运动速度规划并修整，具体包括：

[0249] 重新进行刚体平动的速度规划，如下：

[0250] 根据刚体平动的最大速度vmax、最大加速度amax、加加速度jl，调姿起始和终止的速度vs＝ve＝0，调姿起始和终止的加速度as＝ae＝0，刚体平动移动量S，进行铃型加/减速的
刚体平动速度规划计算，计算出刚体平动的实际最大速度vnewmax和实际最大加速度anewmax；

[0251] 重新计算平动运行时间Ts，每阶段时间T1s,T2s,T3s,T4s,T5s,T6s,T7s；其中T1s＝T7s＝T5s＝T3s，T2s＝T6s；

[0252] 速度规划时间取整，如下：

[0253]

[0254] 重新进行刚体定轴转动的速度规划，如下：

[0255] 根据刚体定轴转动的最大角速度ωmax、最大角加速度αmax、角加加速度jθ，调姿起始和终止的速度ωs＝ωe＝0，调姿起始和终止的加速度αs＝αe＝0，刚体定轴转动的角度增
量θe，进行铃型加/减速的刚体定轴转动速度规划计算，计算出刚体定轴转动的实际最大角
速度ωnewmax和最大角加速度αnewmax；

[0256] 重新计算刚体定轴转动时间Tθ，每阶段时间为T1θ,T2θ,T3θ,T4θ,T5θ,T6θ,T7θ；其中T1θ＝T7θ＝T5θ＝T3θ，T2θ＝T6θ；

[0257] 速度规划时间取整，如下：

[0258]

[0259] 刚体平动与刚体定轴转动同步进行，调整平动时间与定轴转动时间为相同时间，如下：

[0260] Tnew＝Tnews＝Tnewθ＝max(Tnews,Tnewθ)

[0261] 其中各个阶段的时间按比例调整，如下：

[0262] T1s＝T7s＝T5s＝T3s＝T1sKnews，T2s＝T6s＝T2sKnews，T4s＝T4sKnews

[0263] T1θ＝T7θ＝T5θ＝T3θ＝T1θKnewθ，T2θ＝T6θ＝T2θKnewθ，T4θ＝T4θKnewθ

[0264] 式中， 为刚体平动时间调整比例； 刚体定轴转动时间调整比例；

[0265] 调整实际速度、实际加速度、实际加加速度，如下：

[0266] 刚体平动实际最大速度和刚体定轴转动实际角速度调整：

[0267] 刚体平动实际最大加速度和刚体定轴转动实际 角加速度调整：

[0268] 刚体平动实际加加速度和刚体定轴转动实际加加速度调整：

[0269] 得到调整后的速度规划：

[0270] 刚体平动：s＝fs3(t)；v＝fv3(t)；a＝fa3(t)；其中t∈[0,Tnew]；s∈[0,S]；

[0271] 刚体定轴转动：θ＝fθ3(t)；ω＝fω3(t)；α＝fα3(t)；其中t∈[0,Tnew]；θ∈[0,θe]。

[0272] 进一步的，所述根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置，具体包括：

[0273] 根据刚体复合运动新的速度规划，按时间分割法分别进行刚体定轴转动插补和刚体平动插补，同步叠加后计算出每个定位器支撑点在全局坐标系位置，再转换到每个定位
器局部坐标系，输出每个定位器三轴指令坐标位置。

[0274] 进一步的，所述根据刚体复合运动新的速度规划，按时间分割法分别进行刚体定轴转动插补和刚体平动插补，同步叠加后计算出每个定位器支撑点在全局坐标系位置，再
转换到每个定位器局部坐标系，输出每个定位器三轴指令坐标位置，具体包括：

[0275] 刚体局部坐标系Olxyz的原点调姿前在全局坐标系中位置

[0276] 定位器局部坐标系Oixyz的原点在全局坐标中位置

[0277] 第i个定位器支撑点调姿前的位置，包括在全局坐标系中的位置在刚体局部坐标系Olxyz中位置 以及在定位器局部坐标系Oixyz
中位置

[0278] 第i个定位器支撑点调姿后的位置，包括在刚体局部坐标系Olxyz中的位置在全局坐标中的位置 以及在定位器局部坐标系
Oixyz中位置

[0279] 计算第i个定位器支撑点调姿过程的插补位置，如下：

[0280] 刚体定轴转动的转动角度函数为：

[0281] θ＝fθ3(t)；ω＝fω3(t)；α＝fα3(t)；t∈[0,Tnew]；θ∈[0,θe]；

[0282] 刚体转动轴在刚体局部坐标系Olxyz的矢量

[0283] 刚体平动的移动量函数为：

[0284] s＝fs3(t)；v＝fv3(t)；a＝fa3(t)；t∈[0,Tnew]；s∈[0,S]；

[0285] 刚体平动方向矢量

[0286] 解算第i个定位器支撑点在定位器局部坐标系Oixyz位置函数 如下式：

[0287]

[0288] 式中， 为第i个定位器支撑点在全局坐标系中的位置矢量，如下式：

[0289]

[0290] 式中， 是位姿调整过程中刚体局部坐标系原点在全局坐标系的位置，

[0291] 刚体定轴转动与平动同步叠加时，第i个定位器支撑点在定位器局部坐标系Oixyz的位置计算公式如下：

[0292]

[0293] 式中， sθ＝sin(θ)，cθ＝cos(θ),vθ＝1‑cos(θ)，θ＝fθ3(t)，t∈[0,Tnew]，θ∈[0,θe]；

[0294] 第i个定位器支撑点调姿过程在定位器局部坐标系Oixyz的位置分量形式如下：

[0295]

[0296] 式中， 为插补输出的X轴指令坐标位置， 为插补输出的Y轴指令坐标位置， 为插补输出的Z轴指令坐标位置。

[0297] 进一步的，所述根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置，具体包括：

[0298] 根据每个定位器三轴指令坐标位置，进行各轴的反向间隙补偿和螺距误差补偿，再根据空间误差数据进行三轴补偿处理，输出各轴驱动器的实际位置。

[0299] 进一步的，所述根据每个定位器三轴指令坐标位置，进行各轴的反向间隙补偿和螺距误差补偿，再根据空间误差数据进行三轴补偿处理，输出各轴驱动器的实际位置，具体
包括：

[0300] 读取每个定位器的X、Y、Z轴指令坐标位置，进行各轴的反向间隙补偿和螺距误差补偿，若在单轴测量点，则直接读取反向间隙补偿数据和螺距误差补偿数据，若不在单轴测
量点，则按所在单轴测量区间的两端点误差值进行线性插值，补偿所在单轴测量区间的单
轴螺距误差，再加上反向间隙值；

[0301] 读取每个定位器的X、Y、Z轴指令坐标位置，进行空间误差补偿，若在空间误差测量的方格顶点上，则直接读取数据进行补偿，若在某个方格内不在空间误差测量的方格顶点
上，读取这个方格的八个顶点误差数据进行空间线性插值，解算出这个方格的八个顶点拟
合的空间误差；

[0302] 当进行各轴的反向间隙补偿和螺距误差补偿，以及空间误差补偿后，输出各轴驱动器的实际位置。

[0303] 本发明的第二个目的可以通过采取如下技术方案达到：

[0304] 一种三坐标定位器调姿运控系统，应用于飞机数字化装配中，所述系统包括：

[0305] 第一解算模块，用于建立飞机大部件刚体局部坐标系和定位器局部坐标系，按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢
量；

[0306] 第二解算模块，用于根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的
每个定位器的空间圆弧轨迹的圆心和半径，并按每个圆弧轨迹允许误差解算相应的圆弧角
速度范围和刚体角速度范围；

[0307] 第一规划与修整模块，用于根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度规
划并修整；

[0308] 调整模块，用于根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的
速度范围和加速度范围调整刚体复合运动的速度和加速度；

[0309] 第二规划与修整模块，用于当调整刚体复合运动的速度和加速度后，重新进行刚体复合运动速度规划并修整；

[0310] 插补计算模块，用于根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置；

[0311] 补偿模块，用于根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置。

[0312] 本发明的第三个目的可以通过采取如下技术方案达到：

[0313] 一种调姿控制器，包括处理器以及用于存储处理器可执行程序的存储器，所述处理器执行存储器存储的程序时，实现上述的三坐标定位器调姿运控方法。

[0314] 本发明的第四个目的可以通过采取如下技术方案达到：

[0315] 一种存储介质，存储有程序，所述程序被处理器执行时，实现上述的三坐标定位器调姿运控方法。

[0316] 本发明相对于现有技术具有如下的有益效果：

[0317] 1、本发明的飞机大部件六自由度调姿过程是同时执行刚体平动和刚体定轴转动的刚体复合运动，可缩短调姿时间；调姿插补计算对冗余驱动轴数没有限制，将每个定位器
支撑点作为刚体上的质点来解算，这样在调姿过程满足刚体上任何两点距离保持不变的要
求，减少飞机部件的内应力。可用于有冗余驱动轴的多定位器飞机调姿，满足大飞机调姿过
程保型的需求。

[0318] 2、本发明的使用者只需输入飞机部件六自由度的调姿指令，按设定的调姿参数，其调姿插补算法自动根据飞机部件当前位置和调姿指令计算出满足飞机大部件要求的最
大平动速度、最大平动加速度、最大转动角速度、最大角加速度，并满足每个定位器驱动轴
要求的最大速度和最大加速度进行速度规划，针对速度规划时间不是插补周期时间整数倍
的情况增加了修整处理，使飞机调姿全过程速度平稳无冲击，使飞机大部件调姿过程运行
平稳。不需反复尝试即可获得满意效果。

[0319] 3、本发明在飞机大部件调姿插补过程的定位器支撑点移动过程误差进行了限制和补偿，减少飞机大部件调姿过程的内应力和变形，提高装配质量，主要对刚体转动过程圆
弧轮廓误差进行分析，定位器X、Y、Z轴向与全局坐标系X、Y、Z轴不平行情况进行坐标转动处
理，并对定位器的定位误差进行空间补尝。

附图说明

[0320] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本
发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以
根据这些附图示出的结构获得其他的附图。

[0321] 图1为本发明实施例1的三坐标定位器调姿运控方法流程图。

[0322] 图2为本发明实施例2的三坐标定位器调姿运控系统的结构框图。

[0323] 图3为本发明实施例3的调姿控制器的结构框图。

具体实施方式

[0324] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是
本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人
员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0325] 在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各步骤描述成顺序的处理，但是式中的许多
步骤可以被并行地、并发地或者同时实施。此外，各步骤的顺序可以被重新安排。当其操作
完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以
对应于方法、函数、规程、子例程、子程序等等。

[0326] 实施例1：

[0327] 如图1所示，本实施例提供了一种三坐标定位器调姿运控方法，该方法应用于飞机数字化装配中大部件对接的调姿控制器，可以满足加速度要求且有冗余驱动轴，能够支撑
飞机大部件进行六自由度调姿控制，其包括以下步骤：

[0328] S101、建立飞机大部件刚体局部坐标系和定位器局部坐标系，按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量。

[0329] A、建立飞机大部件刚体局部坐标系和定位器局部坐标系。

[0330] 描述调姿前当前飞机大部件和每个定位器位置，建立如下坐标：

[0331] 为相对地面参照系建立全局坐标系Oxyz；

[0332] 为飞机大部件建立刚体局部坐标系Olxyz，刚体局部坐标系的X、Y、Z轴向与全局坐标系的X、Y、Z轴平行，刚体局部坐标系的转动中心设为全局坐标系的原点，局部坐标系的原
T
点调姿前在全局坐标系的坐标为Ol(px，py，pz) ；

[0333] 为第i个定位器建立定位器局部坐标系Oixyz，定位器局部坐标系的X、Y、Z轴向是定位器的支撑点沿定位器的X、Y、Z轴驱动移动的方向(即X、Y、Z轴伺服驱动电机驱动移动的
方向)，该方向有可能与全局坐标系的X、Y、Z轴不平行，已知定位器局部坐标系与全局坐标
系的X、Y、Z轴不平行姿态调整欧拉角或旋转矩阵Ri，定位器局部坐标系的原点是定位器的
X、Y、Z轴回零后的支撑点位置，第i个定位器局部坐标系的原点在全局坐标系的坐标为Oi
T T
(oix，oiy，oiz) ，第i个定位器支撑点在全局坐标系的坐标为Pi(pix，piy，piz) 。

[0334] B、按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量。

[0335] 具体地，飞机六自由度位姿调节的复合运动分解为刚体平动和定轴转动同步叠加，位置调整的平动路径解算其平动方向矢量和移动量，姿态调整指令多个欧拉角转动过
程转换为绕刚体欧拉轴(即转动轴)旋转一次完成，解算出刚体定轴转动(绕刚体转动中心
转动)的转动角度和转动轴矢量。

[0336] 调姿指令为飞机六自由度调姿(调整位姿)指令，一般类似P_Alignment(x，y，z，A，B，C)，其中x，y，z，A，B，C一般都是相对调姿前位置平移增量和旋转角度增量，x，y，z是飞机
大部件沿全局坐标系X、Y、Z轴向刚体平动移动增量，A，B，C是飞机大部件分别沿刚体坐标轴
的旋转角度增量。

[0337] 解算刚体的平动移动量，如下式：

[0338]

[0339] 解算刚体的平动方向矢量，如下式：

[0340]

[0341] 式中，

[0342] 平动结束后刚体局部坐标系Olxyz的原点在全局坐标系Oxyz的坐标为Ole(px+x，py+T
y，pz+z) 。

[0343] 可以按常用情况描述从当前姿态绕旋转中心，先绕刚体的Z轴转动A角度，再绕刚体的Y轴转动B角度，最后绕刚体的X轴转动C角度，或可以用其他方式旋转。不同欧拉角旋转
可转换为相应旋转矩阵R，如下式：

[0344] R＝RA·RB·RC

[0345]

[0346] 先绕刚体的Z轴转动A角度，再绕刚体的Y轴转动B角度，最后绕刚体的X轴转动C角度，如下式：

[0347]

[0348] 式中，s1＝sin(A)，c1＝cos(A)，s2＝sin(B)，c2＝cos(B)，s3＝sin(C)，c3＝cos(C)；

[0349] 可以转换刚体定轴旋转为绕 轴转动：

[0350] 解算刚体定轴转动的转动角度，如下式：

[0351]

[0352] 解算刚体定轴转动的转动轴矢量，如下式：

[0353]

[0354] 式中，

[0355] S102、根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的
空间圆弧轨迹的圆心和半径，并按每个圆弧轨迹允许误差解算相应的圆弧角速度范围和刚
体角速度范围。

[0356] 刚体局部坐标系Olxyz的原点调姿前在全局坐标系中的位置

[0357] 第i个定位器局部坐标系Oixyz原点在全局坐标系中放入位置

[0358] 第i个定位器支撑点调姿前在全局坐标系中的位置

[0359] 第i个定位器支撑点调姿前在刚体局部坐标系Olxyz中的位置

[0360] 第i个定位器支撑点在定位器局部坐标系Oixyz中的位置

[0361] 根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的空间圆
弧轨迹的圆心和半径，如下式：

[0362]

[0363]

[0364] 式中， 为刚体定轴转动过程的第i个定位器的空间圆弧轨迹的圆心位置， 为刚体定轴转动的转动轴矢量，ri为刚体定轴转动过程的第i个定位器的空间圆弧轨迹的半
径。

[0365] 类似计算，求出所有定位器的空间圆弧半径r1,r2,r3,…,ri,…,rN。

[0366] 采用时间分割法每次圆弧插补输出圆弧上一个位置坐标，实际轨迹用弦代替弧，存在弓高误差(即圆弧轨迹误差)。其弓高可认为是飞机大部件调姿过程的变形量。若弓高
误差不大，由于调姿装置存在少量间隙，调姿过程安全；否则出现弓高误差太大会引起飞机
部件变形，这种情况需要避免。

[0367] 设Tinp为插补周期时间，Herr为弓高误差，Lc为插补周期输出量弦长，如下式：

[0368] Lc2＝8Herrri+4Herr2

[0369] 计算满足弓高误差Herr的弦长，如下式：

[0370]

[0371] 式中，Lc＝VTinpo＝ωiriTinp，则

[0372] 若第i个定位器调姿过程按允许的弓高误差Herr，则要求刚体转动角速度ωi最大，并且弓高误差Herr不变时，半径ri越大允许的刚体转动角速度ωi越小；

[0373] 求出所有定位器空间圆弧轨迹半径中的最大半径，如下式：

[0374] rmax＝max(r1,r2,r3,…,ri,…,rN)

[0375] 插补时，插补周期时间Tinp相同，且刚体定轴转动多个圆弧角速度相同；

[0376] 求出弓高误差Herr满足要求的刚体绕 转动角速度最大值，如下式：

[0377]

[0378] S103、根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度规划并修整。

[0379] S1031、根据刚体角速度最大值，调整刚体定轴转动结构强度要求的最大角速度ωmax＝min(ωmax1,ωmax2)。

[0380] S1032、根据飞机大部件调姿允许的最大角加速度αmax、最大平动速度vmax、最大平动加速度amax，再考虑弓高误差要求的ωmax1调整刚体定轴转动结构强度要求的最大角速度
ωmax＝min(ωmax1,ωmax2)，调姿起始和终止的速度vs＝ve＝0,，调姿起始和终止的加速度as
＝ae＝0，刚体定轴转动的转动角度θe和刚体的平动移动量S，分别进行铃型加/减速的刚体
复合运动速度规划计算，计算出各阶段时间、平动实际最大加速度anowmax、平动实际最大速
度vnowmax、转动实际最大角加速度αnowmax和实际最大角速度ωnowmax，随后进行刚体复合运动
速度规划时间修整。

[0381] A、进行铃型加/减速的刚体复合运动速度规划计算。

[0382] 刚体绕定轴转动角度：θ＝fθ1(t)；t∈[0,Tθ]；θ∈[0,θe]；

[0383] 刚体平动距离：s＝fs1(t)；t∈[0,Ts]；s∈[0,S]；

[0384] 按铃型加/减速速度规划，分别进行平移增量和角度增量铃型加/减速的速度规划计算，如下：

[0385] 根据刚体的平动移动量S、最大平动速度vmax、最大平动加速度amax和平动加加速jl，分为加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减减速
阶段共七个阶段进行速度规划计算；其中：

[0386] 加加速的计算如下式：

[0387]

[0388] 式中，J＝jl；

[0389] 加速度的计算如下式：

[0390]

[0391] 速度的计算如下式：

[0392]

[0393] 式中，T1＝t1,T2＝t2‑t1,T3＝t3‑t2；T4＝t4‑t3,T5＝t5‑t4,T6＝t6‑t5；T7＝t7‑t6,T1＝T3,T5＝T7；

[0394] 平动移动量的计算如下式：

[0395]

[0396] 平动的速度规划为s＝fs1(t)，v＝fv1(t)，a＝fa1(t)，js＝J(t)；

[0397] 根据铃型加/减速时T1s＝T3s,T5s＝T7s，调姿起始和终止的速度vs＝ve＝0，调姿起始和终止的加速度as＝ae＝0，以及加速阶段与减速阶段镜像对称，得到T1s＝T7s＝T5s＝T3s，
T2s＝T6s。

[0398] 最大速度和最大加速度在安全范围约束条件为v3≤vmax，Jt1≤amax，求出加加速阶段时间T1s、匀加速阶段时间T2s、减加速阶段时间T3s、匀速阶段时间T4s、加减速阶段时间T5s、
匀减速阶段时间T6s、减减速阶段时间T7s。
2

[0399] 按上述速度规划公式计算，若一般情况 当vmax≤JT1时没有匀加速和匀减速段，则出现T2s＝T6s＝0；当平动距离 时，没有匀速段，则出现
T4s＝0；当平动距离 时，没有匀速段、匀加速和匀减速段，则出现T4s＝0，T2s＝T6s＝
0。

[0400] 计算实际最大加速度anowmax＝jlT1s，实际最大速度vnowmax＝v3，以及平动总时间Ts＝T1s+T2s+T3s+T4s+T5s+T6s+T7s。

[0401] 刚体定轴转动增量的速度规划与平移增量的速度规划类似。根据刚体定轴转动的转动角度θe、最大角速度ωmax、最大角加速度αmax和转动角加加速jθ，分为加加速阶段、匀加
速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段、减减速阶段共七个阶段进行速度
规划的计算；其中：

[0402] 角加加速的计算如下式：

[0403]

[0404] 式中，J＝jθ；

[0405] 角加速度的计算如下式：

[0406]

[0407] 角速度的计算如下式：

[0408]

[0409] 式中，T1＝t1,T2＝t2‑t1,T3＝t3‑t2；T4＝t4‑t3,T5＝t5‑t4,T6＝t6‑t5；T7＝t7‑t6,T1＝T3,T5＝T7；

[0410] 转动角度的计算如下式：

[0411]

[0412] 类似于平动的速度规划，转动的速度规划为jθ＝J(t)，θ＝fθ1(t)，ω＝fω1(t)，α＝fα1(t)。

[0413] 根据铃型加/减速时T1θ＝T3θ,T5θ＝T7θ，建立转动角度的分段函数，并根据角加加速度J＝jθ，转动角度调姿增量θe，调姿起始和终止的角速度ωs＝ωe＝0，调姿起始和终止的
角加速度αs＝αe＝0，以及加速阶段与减速阶段镜像对称，得到T1θ＝T7θ＝T5θ＝T3θ，T2θ＝T6θ；
T1θ＝T7θ＝T5θ＝T3θ，T2θ＝T6θ。

[0414] 最大速度和最大加速度在安全范围约束条件为ω3≤ωmax，Jt1≤αmax，求出加加速阶段时间T1θ、匀加速阶段时间T2θ、减加速阶段时间T3θ、匀速阶段时间T4θ、加减速阶段时间
T5θ、匀减速阶段时间T6θ、减减速阶段时间T7θ。
2

[0415] 按上述速度规划公式计算，若一般情况 当ωmax≤JT1时，没有匀加速和匀减速段，则出现T2θ＝T6θ＝0；当 时，没有匀速段，则出现T4θ＝
0；当 时，没有匀速段、匀加速和匀减速段，则出现T4θ＝0，T2θ＝T6θ＝0。

[0416] 计算实际最大角加速度αnowmax，实际最大角速度ωnowmax，以及转动总时间Tθ＝T1θ+T2θ+T3θ+T4θ+T5θ+T6θ+T7θ。

[0417] B、进行刚体复合运动速度规划时间修整。

[0418] 具体地，对刚体定轴转动和刚体平动分别进行铃型加/减速的速度规划两个运动时间修整为插补周期时间Tinp整数倍而且调整平动时间与转动时间相等，计算调整时间轴
后的各阶段时间、刚体平动实际最大加速度anow2max、刚体平动实际最大速度vnow2max、刚体定
轴转动实际最大角加速度αnow2max、刚体定轴转动实际最大角速度ωnow2max。

[0419] 计算调整时间轴后的各阶段时间、刚体平动实际最大加速度anow2max、刚体平动实际最大速度vnow2max、刚体定轴转动实际最大角加速度αnow2max、刚体定轴转动实际最大角速度
ωnow2max，具体包括：

[0420] 飞机调姿速度和加速度始终要平滑变化避免冲击，若速度规划时间中的平动总时间Ts或转动总时间Tθ不是插补周期时间Tinp的整数倍，则实际运行时间与速度规划时间不
同，导致最后一个插补周期实际运行速度低于规划速度值，因此需要将平动总时间Ts或转
动总时间Tθ向上取整为插补周期时间Tinp的整数倍，如下式：

[0421]

[0422] 刚体平动与刚体定轴转动同步进行，要求同时开始、同时结束，则调整平动时间与定轴转动时间为相同时间，如下式：

[0423] Tnow＝Tnows＝Tnowθ＝max(Tnows,Tnowθ)

[0424] 各阶段的时间按比例调整，如下：

[0425] 刚体平动时间调整比例：

[0426] 刚体定轴转动时间调整比例：

[0427] T1s＝T7s＝T5s＝T3s＝T1sKnows，T2s＝T6s＝T2sKnows，T4s＝T4sKnows

[0428] T1θ＝T7θ＝T5θ＝T3θ＝T1θKnowθ，T2θ＝T6θ＝T2θKnowθ，T4θ＝T4θKnowθ

[0429] 平动距离S和转动角度θe保持不变，Tnows和Tnowθ时间变长，每个阶段的时间变长，调整实际速度、实际加速度、实际加加速度，如下：

[0430] 刚体平动实际最大速度和刚体定轴转动实际最大角速度调整：

[0431] 刚体平动实际最大加速度和刚体定轴转动实际最大角加速度调整：

[0432] 刚体平动实际加加速度和刚体定轴转动实际加加速度调整：

[0433] 当按比例调整各阶段时间、速度、加速度、加加速度后，各阶段刚体平动移动量和刚体定轴转动角度与调整前保持不变，由于Knows≥1，Knowθ≥1，刚体运动实际最大速度与最
大加速度降低了，仍在要求范围内，得到调整后的速度规划如下：

[0434] 刚体平动：s＝fs(t)；v＝fv(t)；a＝fa(t)；其中t∈[0,Tnow]；s∈[0,S]；

[0435] 刚体定轴转动：θ＝fθ(t)；ω＝fω(t)；α＝fα(t)；其中t∈[0,Tnow]；θ∈[0,θe]。

[0436] S104、根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的速度范围和
加速度范围调整刚体复合运动的速度和加速度。

[0437] 具体地，将每个定位器支撑点作为刚体上质点，刚体进行定轴转动和平动的同步叠加运动，利用逆向运动学位置反解，求解出每个定位器驱动轴的空间圆弧运动轨迹、速度
和加速度，在定位器局部坐标系中计算每个定位器驱动轴上的最大速度和最大加速度，并
按每个定位器驱动轴允许的速度范围和加速度范围(即每个定位器驱动轴上的驱动电机允
许的速度范围和加速度范围)调整刚体复合运动的速度和加速度；已知第i个定位器的X、Y、
Z轴安全运行的各轴最大速度vixmax,viymax,vizmax，最大加速度aixmax,aiymax,aizmax，实现过程如
下：

[0438] 以第i个定位器支撑点空间圆弧Ci的圆心为坐标原点建立新的局部坐标系Cixyz，局部坐标系Cixyz的X、Y、Z轴平行于定位器局部坐标系Oixyz的X、Y、Z轴；

[0439] 空间圆弧运动轨迹中的圆心角为刚体旋转的角度θ＝fθ(t)；

[0440] 刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移距离s＝fs(t)；

[0441] 刚体平动方向矢量 其中

[0442] 对应局部坐标系Cixyz的原点在定位器局部坐标系Oixyz方向矢量

[0443] 刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移位置分量形式如下：

[0444] fis(t)＝[xis(t) yis(t) zis(t)]T

[0445] 式中，xis(t)＝uixfs(t)，yis(t)＝uiyfs(t)，zis(t)＝uizfs(t)；

[0446] 刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移速度分量形式如下：

[0447] fiv(t)＝[xiv(t) yiv(t) ziv(t)]T

[0448] 式中，xiv(t)＝uixfv(t)，yiv(t)＝uiyfv(t0，ziv(t)＝uizfv(t)；

[0449] 定位器局部坐标系Oixyz中X、Y、Z轴平动过程的最大速度，如下式：

[0450] viSxmax＝max(|xiv(t)|)＝|uix|max(fv(t))＝|uix|vnow2max

[0451] viSymax＝max(|yiv(t)|)＝|uiy|max(fv(t))＝|uiy|vnow2max

[0452] viSzmax＝max(|ziv(t)|)＝|uiz|max(fv(t))＝|uiz|vnow2max

[0453] 刚体平动过程中局部坐标系Cixyz的原点在定位器局部坐标系Oixyz中的平移加速度分量形式如下：

[0454] fia(t)＝[xia(t) yia(t) zia(t)]T

[0455] 式中，xia(t)＝uixfa(t)，yia(t)＝uiyfa(t)，zia(t)＝uizfa(t)；

[0456] 定位器局部坐标系Oixyz中X、Y、Z轴平动过程的最大加速度，如下式：

[0457] aiSxmax＝max(|xia(t)|)＝|uix|max(fa(t))＝|uix|anow2max

[0458] aiSymax＝max(|yia(t)|)＝|uiy|max(fa(t))＝|uiy|anow2max

[0459] aiSzmax＝max(|zia(t)|)＝|uiz|max(fa(t))＝|uiz|anow2max

[0460] 刚体定轴转动中刚体局部坐标系Olxyz的转动轴 刚体定轴转动的转动角度θ＝fθ(t)；ω＝fω(t)；α＝fα(t)；其中t∈[0,Tnow]；θ∈[0,θe]；

[0461] 调姿前第i个定位器支撑点在刚体局部坐标系Olxyz中的位置矢量

[0462] 第i个定位器在刚体局部坐标系Olxyz中的旋转半径 圆心坐标

[0463] 刚体定轴转动角度θ＝fθ(t)，刚体旋转Rw,θ，如下式：

[0464]

[0465] 式中，sθ＝sin(θ)，cθ＝cos(θ),vθ＝1‑cos(θ)，θ＝fθ(t(，t∈[0,Tnow]，θ∈[0,θe]；

[0466] 调姿过程中第i个定位器支撑点在刚体局部坐标系Oixyz中的位置矢量为

[0467] 刚体转动轴在局部坐标系Cixyz中为

[0468] 第i个定位器支撑点在局部坐标系Cixyz中的位置Ci(θ)为旋转角度θ函数，如下式：

[0469]

[0470]

[0471] 第i个定位器支撑点在局部坐标系Cixyz中的位置Ci(θ)的分量形式如下：

[0472] Ci(θ)＝[xi(θ) yi(θ) zi(θ)]T，xi(θ)，yi(θ)，zi(θ)

[0473] Ci任意θ位置的法向方向矢量，如下式：

[0474]

[0475]

[0476] Ci任意θ位置的法向方向矢量分量形式如下：

[0477]

[0478]

[0479] Ci任意θ位置的法向方向矢量X、Y、Z轴分量绝对值最大值，如下式：

[0480] xintmax＝max(|xint(fθ(t))|)＝max(|xin(θ)|)

[0481] yintmax＝max(|yint(fθ(t))|)＝max(|yin(θ)|)

[0482] zintmax＝max(|zint(fθ(t))|)＝max(|zin(θ)|)

[0483] 由于飞机调姿一次旋转角度θe小，xin(θ),yin(θ),zin(θ),θ∈[0,θe]这些是闭区间连续函数，存在最值，一般在区间端点和函数极值点，求解出xintmax，yintmax，zintmax。

[0484] Ci任意θ位置的切向方向矢量，如下式：

[0485] 其中

[0486]

[0487] Ci任意θ位置的切向方向矢量分量形式如下：

[0488]

[0489]

[0490] Ci任意θ位置的切向方向矢量X、Y、Z轴分量绝对值最大值，如下式：

[0491] xiτtmax＝max(|xiτt(fθ(t))|)＝max(|xiτ(θ)|)

[0492] yiτtmax＝max(|yiτt(fθ(t))|)＝max(|yiτ(θ)|)

[0493] ziτtmax＝max(|ziτt(fθ(t))|)＝max(|ziτ(θ)|)

[0494] 由于飞机调姿一次旋转角度θe小，xiτ(θ),yiτ(θ),ziτ(θ),θ∈[0,θe]这些是闭区间连续函数，存在最值，一般在区间端点和函数极值点。求解出xiτtmax，yiτtmax，ziτtmax。

[0495] 第i个定位器支撑点在局部坐标系Cixyz中的线速度viτ(t)＝rifω(t)；

[0496] 第i个定位器支撑点在局部坐标系Cixyz中的最大线速度max(viτ(t))＝riωnow2max；

[0497] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动速度

[0498] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动速度分量形式如下：

[0499] fivτ(t)＝[xivτ(t) yivτ(t) zivτ(t)]T

[0500] 式中，xivτ(t)＝viτ(t)xiτt(fθ(t))，yivτ(t)＝viτ(t)yiτt(fθ(t))，zivτ(t)＝viτ(t)ziτt(fθ(t))；

[0501] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧运动中最大线速度时刻与各轴方向矢量分量达到最大值时刻可能不重叠，最大线速度乘以方向矢量各轴分量最大值不小
于圆弧运动fivτ(t)各轴最大速度，如下：

[0502] max(|xivτ(t)|)＝max(viτ(t)|xiτt(fθ(t))|)≤max(viτ(t))max(|xiτt(fθ(t))|)

[0503] max(|yivτ(t)|)＝max(viτ(t)|yiτt(fθ(t))|)≤max(viτ(t))max(|yiτt(fθ(t))|)

[0504] max(|zivτ(t)|)＝max(viτ(t)|ziτt(fθ(t))|)≤max(viτ(t))max(|ziτt(fθ(t))|)

[0505] 第i个定位器支撑点在定位器局部坐标系Oixyz中的运动速度为圆弧运动速度fivτ(t)和平动速度fiv(t)同步叠加的速度，由于平动速度X、Y、Z轴分量达到最大速度时刻与圆
弧运动速度X、Y、Z轴分量达到最大速度时刻可能不重叠，平动速度X、Y、Z轴分量最大速度与
圆弧运动速度X、Y、Z轴分量最大速度之和大于定位器实际X、Y、Z实际的最大速度，若小于各
驱动轴允许的速度范围，则安全，否则降低刚体复合运动的速度，如下：

[0506] vixtest≥max(viτ(t)|xiτt(fθ(t))|)+max(|xiv(t)|)≥max(|xivτ(t)+xiv(t)|)

[0507] viytest≥max(viτ(t)|yiτt(fθ(t))|)+max(|yiv(t)|)≥max(|yivτ(t)+yiv(t)|)

[0508] viztest≥max(viτ(t)|ziτt(fθ(t))|)+max(|ziv(t)|)≥max(|zivτ(t)+ziv(t)|)

[0509] 式中，xivτ(t)+xiv(t)为X轴实际速度，yivτ(t)+yiv(t)为Y轴实际速度，zivτ(t)+ziv(t)为Z轴实际速度。

[0510] vixtest＝max(viτ(t))max(|xiτt(fθ(t))|)+max(|xiv(t)|)＝riωnow2maxxiτtmax+|uix|vnow2max；

[0511] viytest＝max(viτ(t))max(|yiτt(fθ(t))|)+max(|yiv(t)|)＝riωnow2maxyiτtmax+|uiy|vnow2max；

[0512] viztest＝max(viτ(t))max(|ziτt(fθ(t))|)+max(|ziv(t)|)＝riωnow2maxziτtmax+|uiz|vnow2max；

[0513] 若vixtest≤vixmax，设 否则

[0514] 若viytest≤viymax，设 否则

[0515] 若viztest≤vizmax，设 否则

[0516] 第i个定位器速度调节系数

[0517] 第i个定位器支撑点在局部坐标系Cixyz中的切向加速度fiaτ(t)＝rifα(t)；

[0518] 第i个定位器支撑点在局部坐标系Cixyz中的切向加速度最大值max(fiaτ(t))＝riαnow2max；

[0519] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧切向加速度矢量

[0520] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧切向加速度分量形式如下：

[0521] aiτ(t)＝[xiaτ(t) yiaτ(t) ziaτ(t)]T

[0522] 式中，xiaτ(t)＝fiaτ(t)xiττ(fθ(t))，yiaτ(t)＝fiaτ(t)yiτt(fθ(t))，ziaτ(t)＝fiaτ(t)ziτt(fθ(t))；

[0523] 第i个定位器支撑点在局部坐标系Cixyz中的向心加速度fian(t)＝rifω2(t)；

[0524] 第i个定位器支撑点在局部坐标系Cixyz中的向心加速度最大值max(fian(t))＝ri2
ωnow2max；

[0525] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧法向加速度矢量

[0526] 第i个定位器支撑点在局部坐标系Cixyz中的圆弧法向加速度分量形式如下：

[0527] ain(t)＝[xian(t) yian(t) zian(t)]T

[0528] 式中，xiaθ(t)＝fiaτ(t)xiτt(fθ(t))+fian(t)xint(fθ(t))；yiaθ(t)＝fiaτ(t)yiτt(fθ(t))+fian(t)yint(fθ(t))；ziaθ(t)＝fiaτ(t)ziτt(fθ(t))+fian(t)zint(fθ(t))；

[0529] 第i个定位器支撑点在局部坐标系Cixyz中的中圆弧运动角加速度和角速度可能不能同时达到最大，并且X、Y、Z轴法向分量和切向分量可能不能同时达到最大，因此圆弧运
动加速度aiθ(t)的X、Y、Z轴分量最大值不大于按角加速度和角速度的最大值且各轴法向分
量和切向分量最大值进行计算结果，如下：

[0530] max(|xiaθ(t)|)≤max(fiaτ(t))max(|xiτt(fθ(t))|)+max(fian(t))max(|xint(fθ(t))|)

[0531] max(|yiaθ(t)|)≤max(fiaτ(t))max(|yiτt(fθ(t))|)+max(fian(t))max(|yint(fθ(t))|)

[0532] max(|ziaθ(t)|)≤max(fiaτ(t))max(|ziτt(fθ(t))|)+max(fian(t))max(|zint(fθ(t))|)

[0533] 第i个定位器支撑点在定位器局部坐标系Oixyz中的运动速度为圆弧运动加速度aiθ(t)和平动加速度fia(t)同步叠加的加速度，由于平动加速度X、Y、Z轴分量达到最大速度
时刻与圆弧运动加速度X、Y、Z轴分量达到最大速度时刻可能不重叠，平动加速度X、Y、Z轴分
量最大值与圆弧运动加速度X、Y、Z轴分量最大值之和大于定位器实际X、Y、Z实际的最大加
速度，若小于各驱动轴允许的加速度范围，则安全，否则降低刚体复合运动的加速度和角速
度，如下：

[0534] aixtest≥max(|xiaθ(t)|)+max(|xia(t)|)≥max(|xiaθ(t)+xia(t)|)

[0535] aiytest≥max(|yiaθ(t)|)+max(|yia(t)|)≥max(|yiaθ(t)+yia(t)|)

[0536] aiztest≥max(|ziaθ(t)|)+max(|zia(t)|)≥max(|ziaθ(t)+zia(t)|)

[0537] 式中，xiaθ(t)+xia(t)为X轴实际加速度，yiaθ(t)+yia(t)为Y轴实际加速度，ziaθ(t)+zia(t)为Z轴实际加速度；

[0538] aixtest＝max(fiaτ(t))max(|xiτt(fθ(t))|)+max(fian(t))max(|xint(fθ(t))|)+max(|xia(t)|)

[0539]      ＝riαnow2maxxiτtmax+riωnow2max2xintmax+|uix|anow2max；

[0540] aiytest＝max(fiaτ(t))max(|yiτt(fθ(t))|)+max(fian(t))max(|yint(fθ(t))|)+max(|yia(t)|)

[0541]      ＝riαnow2maxyiτtmax+riωnow2max2yintmax+|uiy|anow2max；

[0542] aiztest＝max(fiaτ(t))max(|ziτt(fθ(t))|)+max(fian(t))max(|zint(fθ(t))|)+max(|zia(t)|)

[0543]       ＝riαnow2maxziτtmax+riωnow2max2zintmax+|uiz|now2max；

[0544] 若aixtest≤aixmax，设 否则

[0545] 若aiytest≤aiymax，设 否则

[0546] 若aiztest≤aizmax，设 否则

[0547] 第i个定位器的加速度调节系数

[0548] 若 并且 则降低刚体转动角速度，减少向心加速度，调整

[0549] 求出飞机调姿过程的速度调节系数最大值

[0550] 求出飞机调姿过程的加速度调节系数最大值

[0551] 根据飞机调姿过程的速度、加速度调节系数调整刚体平动和定轴转动的速度和加速度，如下：

[0552] 刚体平动的最大速度和刚体定轴转动的最大角速度：

[0553] 刚体平动的最大加速度和刚体定轴转动的最大角加速度：

[0554] 刚体平动的加加速度和刚体定轴转动的加加速度：jl＝jl2，jθ＝jθ2。

[0555] S105、重新进行刚体复合运动速度规划并修整。

[0556] 重新进行刚体平动的速度规划，如下：

[0557] 根据刚体平动的最大速度vmax、最大加速度amax、加加速度jl，调姿起始和终止的速度vs＝ve＝0，调姿起始和终止的加速度as＝ae＝0，步骤S101求出的刚体平动移动量S，按照
步骤S103中的速度规划计算方法进行铃型加/减速的刚体平动速度规划计算，计算出刚体
平动的实际最大速度vnewmax和实际最大加速度anewmax。

[0558] 重新计算平动运行时间Ts，每阶段时间T1s,T2s,T3s,T4s,T5s,T6s,T7s；其中T1s＝T7s＝T5s＝T3s，T2s＝T6s；

[0559] 速度规划时间取整，如下：

[0560]

[0561] 重新进行刚体定轴转动的速度规划，如下：

[0562] 根据刚体定轴转动的最大角速度ωmax、最大角加速度αmax、角加加速度jθ，调姿起始和终止的速度ωs＝ωe＝0，调姿起始和终止的加速度αs＝αe＝0，步骤S101求出的刚体定
轴转动的角度增量θe，按照步骤S103中的速度规划计算方法进行铃型加/减速的刚体定轴
转动速度规划计算，计算出刚体定轴转动的实际最大角速度ωnewmax和实际最大角加速度
αnewmax；

[0563] 重新计算刚体定轴转动时间Tθ，每阶段时间为T1θ,T2θ,T3θ,T4θ,T5θ,T6θ,T7θ；其中T1θ＝T7θ＝T5θ＝T3θ，T2θ＝T6θ；

[0564] 速度规划时间取整，如下：

[0565]

[0566] 刚体平动与刚体定轴转动同步进行，调整平动时间与定轴转动时间为相同时间，如下：

[0567] Tnew＝Tnews＝Tnewθ＝max(Tnews,Tnewθ)

[0568] 其中各个阶段的时间按比例调整，如下：

[0569] T1s＝T7s＝T5s＝T3s＝T1sKnews，T2s＝T6s＝T2sKnews，T4s＝T4sKnews

[0570] T1θ＝T7θ＝T5θ＝T3θ＝T1θKnewθ，T2θ＝T6θ＝T2θKnewθ，T4θ＝T4θKnewθ

[0571] 式中， 为刚体平动时间调整比例； 刚体定轴转动时间调整比例；

[0572] 调整实际速度、实际加速度、实际加加速度，如下：

[0573] 刚体平动实际最大速度和刚体定轴转动实际角速度调整：

[0574] 刚体平动实际最大加速度和刚体定轴转动实际 角加速度调整：

[0575] 刚体平动实际加加速度和刚体定轴转动实际加加速度调整：

[0576] 得到调整后的速度规划：

[0577] 刚体平动：s＝fs3(t)；v＝fv3(t)；a＝fa3(t)；其中t∈[0,Tnew]；s∈[0,S]；

[0578] 刚体定轴转动：θ＝fθ3(t)；ω＝fω3(t)；α＝fα3(t)；其中t∈[0,Tnew]；θ∈[0,θe]。

[0579] 这样进行时间调整后速度规划函数，不仅满足刚体定轴转动和平动同步进行，而且满足调姿过程弓高误差的角速度范围限制条件、刚体调姿过程的飞机大部件的速度和加
速度范围限制条件、定位器驱动轴的速度和加速度范围限制条件。

[0580] S106、根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置。

[0581] 具体地，根据刚体复合运动新的速度规划，按时间分割法分别进行刚体定轴转动插补和刚体平动插补，同步叠加后计算出每个定位器支撑点在全局坐标系位置，再转换到
每个定位器局部坐标系，输出每个定位器三轴指令坐标位置，实现过程如下：

[0582] 刚体局部坐标系Olxyz的原点调姿前在全局坐标系中位置

[0583] 定位器局部坐标系Oixyz的原点在全局坐标中位置

[0584] 第i个定位器支撑点调姿前的位置，包括在全局坐标系中的位置在刚体局部坐标系Olxyz中位置 以及在定位器局部坐标系Oixyz
中位置

[0585] 第i个定位器支撑点调姿后的位置，包括在刚体局部坐标系Olxyz中的位置在全局坐标中的位置 以及在定位器局部坐标系
Oixyz中位置

[0586] 计算第i个定位器支撑点调姿过程的插补位置，如下：

[0587] 刚体定轴转动的转动角度函数为：

[0588] θ＝fθ3(t)；ω＝fω3(t)；α＝fα3(t)；t∈[0,Tnew]；θ∈[0,θe]；

[0589] 刚体转动轴在刚体局部坐标系Olxyz的矢量

[0590] 刚体平动的移动量函数为：

[0591] s＝fs3(t)；v＝fv3(t)；a＝fa3(t)；t∈[0,Tnew]；s∈[0,S]；

[0592] 刚体平动方向矢量

[0593] 解算第i个定位器支撑点在定位器局部坐标系Oixyz位置函数 如下式：

[0594]

[0595] 式中， 为第i个定位器支撑点在全局坐标系中的位置矢量，如下式：

[0596]

[0597] 式中， 是位姿调整过程中刚体局部坐标系原点在全局坐标系的位置，

[0598] 刚体定轴转动与平动同步叠加时，第i个定位器支撑点在定位器局部坐标系Oixyz的位置计算公式如下：

[0599]

[0600] 式中， sθ＝sin(θ)，cθ＝cos(θ),vθ＝1‑cos(θ)，θ＝fθ3(t)，t∈[0,Tnew]，θ∈[0,θe]；

[0601] 第i个定位器支撑点调姿过程在定位器局部坐标系Oixyz的位置分量形式如下：

[0602]

[0603] 式中， 为插补输出的X轴指令坐标位置， 为插补输出的Y轴指令坐标位置， 为插补输出的Z轴指令坐标位置。

[0604] 如上依次解算出所有定位器的X、Y、Z在调姿过程的指令位置坐标值；式中Rw,θ和fs3(t)是刚体定轴转动角度和刚体平动量相关，是所有定位器公共数据，故在每个插补周期周
期中只需要计算一次。式中 与定位器相关，但与插补时间t无关，每次调姿过程只
需要计算一次，不必每插补周期重复计算。

[0605] S107、根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置。

[0606] 由于定位器三个坐标轴向不垂直存在垂直度误差，每个轴导轨加工误差导致直线度误差，每轴两导轨存在不平行和存在弯曲曲率、挠率变化的情况，除了单轴的反向间隙补
偿和螺补还需要测量补偿螺补间补后的实际定位器支撑点的空间定位误差，进行补偿。

[0607] 位姿调整前预先建立误差补偿表。双向螺补可以不设置反向间隙补偿表。单向螺补方式要进行反向间隙补偿和螺补。

[0608] 先在进行定位器局部坐标系的X、Y、Z单轴反向间隙测量和螺补误差测量，建立单轴反向间隙补偿数据表、单轴螺补数据表。有了单轴反向间隙补偿数据、单轴螺补数据，可
实现螺补和反向间补功能。先开启螺补和间补功能准备进行空间误差测量。再利用飞机调
姿设备中在线测量的激光跟踪仪等设备测量出定位器支撑点在定位器局部坐标系移动过
程中空间位置误差。

[0609] 本实施例的测量方式是将定位器支撑点有效移动空间分成若干个小的方格，定位器控制器给出移动指令给定位器驱动电机使支撑点到达方格的顶点，激光跟踪仪等设备直
接读取实际空间位置，测量出其空间位置误差并记录在三维误差数据表。一般测量点不要
太多，否则测量时间会较长，存储的数据量较大。

[0610] 因此，步骤S107具体包括：根据每个定位器三轴指令坐标位置，进行各轴的反向间隙补偿和螺距误差补偿，再根据空间误差数据进行三轴补偿处理，输出各轴驱动器的实际
位置，实现过程如下：

[0611] 1)读取每个定位器的X、Y、Z轴指令坐标位置，进行各轴的反向间隙补偿和螺距误差补偿，若在单轴测量点，则直接读取反向间隙补偿数据和螺距误差补偿数据，若不在单轴
测量点，则按所在单轴测量区间的两端点误差值进行线性插值，补偿所在单轴测量区间的
单轴螺距误差，再加上反向间隙值。

[0612] 第i个驱动器在插补输出坐标 时各轴反向间隙补偿和螺距误差补偿。

[0613] X轴在 位置对应螺补和间补叠加值；

[0614] Y轴在 位置对应螺补和间补叠加值；

[0615] Z轴在 位置对应螺补和间补叠加值。

[0616] 2)读取每个定位器的X、Y、Z轴指令坐标位置，进行空间误差补偿，若在空间误差测量的方格顶点上，则直接读取数据进行补偿，若在某个方格内不在空间误差测量的方格顶
点上，读取这个方格的八个顶点误差数据进行空间线性插值，解算出这个方格的八个顶点
拟合的空间误差。

[0617] 第i个驱动器在插补输出坐标 时各轴对应三维空间补偿值

[0618] 插补输出坐标 时X轴空间误差补偿量；

[0619] 插补输出坐标 时Y轴空间误差补偿量；

[0620] 插补输出坐标 时Z轴空间误差补偿量。

[0621] 3)当进行各轴的反向间隙补偿和螺距误差补偿，以及空间误差补偿后，输出各轴驱动器的实际位置。

[0622] 第i个驱动器t时刻插补坐标 多种误差补偿计算后，t时刻输出给每个伺服驱动轴位置指令：

[0623] X伺服驱动器补偿后的位置；

[0624] Y伺服驱动器补偿后的位置；

[0625] Z伺服驱动器补偿后的位置。

[0626] 依次对所有驱动器进行误差补偿计算。最后将所有驱动轴的补偿后的位置指令在插补周期输出时刻统一发送给各个伺服驱动器，这样实现空间位置补偿，并有效提高每个
定位器支撑点的空间位置精度。

[0627] 本领域技术人员可以理解，实现上述实施例的方法中的全部或部分步骤可以通过程序来指令相关的硬件来完成，相应的程序可以存储于计算机可读存储介质中。

[0628] 实施例2：

[0629] 如图2所示，本实施例提供了一种三坐标定位器调姿运控系统，应用于飞机数字化装配中，该系统包括第一解算模块201、第二解算模块202、第一规划与修整模块203、调整模
块204、第二规划与修整模块205、插补计算模块206和补偿模块207，各个模块的具体功能如
下：

[0630] 第一解算模块201，用于建立飞机大部件刚体局部坐标系和定位器局部坐标系，按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴
矢量。

[0631] 第二解算模块202，用于根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程
的每个定位器的空间圆弧轨迹的圆心和半径，并按每个圆弧轨迹允许误差解算相应的圆弧
角速度范围和刚体角速度范围。

[0632] 第一规划与修整模块203，用于根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度
规划并修整。

[0633] 调整模块204，用于根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许
的速度范围和加速度范围调整刚体复合运动的速度和加速度。

[0634] 第二规划与修整模块205，用于当调整刚体复合运动的速度和加速度后，重新进行刚体复合运动速度规划并修整。

[0635] 插补计算模块206，用于根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置。

[0636] 补偿模块207，用于根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置。

[0637] 本实施例中各个模块的具体实现可以参见上述实施例1，在此不再一一赘述；需要说明的是，本实施例提供的系统仅以上述各功能模块的划分进行举例说明，在实际应用中，
可以根据需要而将上述功能分配由不同的功能模块完成，即将内部结构划分成不同的功能
模块，以完成以上描述的全部或者部分功能。

[0638] 实施例3：

[0639] 如图3所示，本实施例提供了一种调姿控制器，其包括处理器301、存储器302和传输单元303，该处理器301用于提供计算和控制能力，该存储器302存储有计算机程序，处理
器301执行存储器302存储的计算机程序时，实现上述实施例1的三坐标定位器调姿运控方
法，如下：

[0640] 建立飞机大部件刚体局部坐标系和定位器局部坐标系，按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量；

[0641] 根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的空间圆
弧轨迹的圆心和半径，并按每个圆弧轨迹允许误差解算相应的圆弧角速度范围和刚体角速
度范围；

[0642] 根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度规划并修整；

[0643] 根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的速度范围和加速
度范围调整刚体复合运动的速度和加速度；

[0644] 当调整刚体复合运动的速度和加速度后，重新进行刚体复合运动速度规划并修整；

[0645] 根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置；

[0646] 根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置。

[0647] 实施例4：

[0648] 本实施例提供了一种存储介质，该存储介质为计算机可读存储介质，其存储有计算机程序，计算机程序被处理器执行时，实现上述实施例1的三坐标定位器调姿运控方法，
如下：

[0649] 建立飞机大部件刚体局部坐标系和定位器局部坐标系，按调姿指令解算刚体的平动移动量和平动方向矢量，以及刚体定轴转动的转动角度和转动轴矢量；

[0650] 根据刚体定轴转动的转动轴矢量、刚体转动中心位置和每个定位器支撑点当前位置，将每个定位器支撑点作为刚体上质点，解算刚体定轴转动过程的每个定位器的空间圆
弧轨迹的圆心和半径，并按每个圆弧轨迹允许误差解算相应的圆弧角速度范围和刚体角速
度范围；

[0651] 根据刚体角速度范围，以及飞机大部件调姿允许的转动角速度范围、角加速度范围、平动速度范围和平动加速度范围，进行刚体复合运动速度规划并修整；

[0652] 根据刚体复合运动的速度规划，将每个定位器支撑点作为刚体上质点，计算每个定位器驱动轴上的最大速度和最大加速度，并按每个定位器驱动轴允许的速度范围和加速
度范围调整刚体复合运动的速度和加速度；

[0653] 当调整刚体复合运动的速度和加速度后，重新进行刚体复合运动速度规划并修整；

[0654] 根据刚体复合运动新的速度规划，按时间分割法进行插补计算，输出每个定位器三轴指令坐标位置；

[0655] 根据每个定位器三轴指令坐标位置和空间误差数据进行空间补偿，输出各轴驱动器的实际位置。

[0656] 需要说明的是，本实施例的计算机可读存储介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但
不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。
计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、
便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存
储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD‑ROM)、光存储器件、磁存储器
件、或者上述的任意合适的组合。在本实施例中，计算机可读存储介质可以是任何包含或存
储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而
在本实施例中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信
号，式中承载了计算机可读的程序。这种传播的数据信号可以采用多种形式，包括但不限于
电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存
储介质以外的任何计算机可读存储介质，该计算机可读信号介质可以发送、传播或者传输
用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读存储介质
上包含的计算机程序可以用任何适当的介质传输，包括但不限于：电线、光缆、RF(射频)等
等，或者上述的任意合适的组合。

[0657] 附图中的流程图和框图，图示了按照上述各个实施例的方法、系统和调姿控制器的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个
模块、程序段或代码的一部分，该模块、程序段或代码的一部分包含一个或多个用于实现规
定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能
也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本
并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，
框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定
的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合
来实现。描述于上述实施例中所涉及到的模块可以通过软件的方式实现，也可以通过硬件
的方式来实现。

[0658] 以上描述仅为本发明的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，上述实施例中所涉及的公开范围，并不限于上述技术特征的特定组合而成的
技术方案，同时也应涵盖在不脱离上述公开构思的情况下，由上述技术特征或其等同特征
进行任意组合而形成的其它技术方案。例如上述特征与上述实施例公开的(但不限于)具有
类似功能的技术特征进行互相替换而形成的技术方案。

[0659] 本领域技术人员应当理解，本发明不限于上述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然
通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在
不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的
权利要求范围决定。