CN106184819A - 一种姿态机动自适应轨迹规划方法 - Google Patents

Abstract

一种姿态机动自适应轨迹规划方法，依次计算机动欧拉角、机动欧拉轴和欧拉轴方向转动惯量，确定加减速最大时间和加减速最小时间，根据欧拉角及执行机构能力进行自主路径规划。本发明通过地面上注的姿态机动角度指令，计算相应的机动欧拉角和欧拉轴，计算沿欧拉轴方向的转动惯量，根据执行机构的最大力矩和最大角动量能力确定对应的最大角加速度和最大角速度，通过对加减速段设计了一阶三角函数过渡过程，使控制力矩的频率与挠性附件的基频隔离，确定允许的加减速最大时间和加减速最小时间范围，从而有效的抑制挠性附件的振动。

Description

一种姿态机动自适应轨迹规划方法

技术领域

本发明涉及一种姿态机动自适应轨迹规划方法。

背景技术

随着卫星功能增强，星上携带的挠性附件面积尺寸越来越大，带来的影响是附件的挠性基频变得更低，耦合作用变大。随着卫星对平台姿态机动能力需求提高，星上配置大力矩的执行结构，如控制力矩陀螺群。这种大力矩执行机构输出力矩大，在姿态机动过程中容易激起挠性附件的振动，使得快速稳定时间变长。

发明内容

本发明提供一种姿态机动自适应轨迹规划方法。能够自主进行任意角度机动的轨迹规划，能够自主调节加减速时间和最大角加速度，并在加减速过程中设计了平滑的过度过程，能够对挠性附件的挠性振动进行有效的抑制，可以大幅缩短姿态稳定时间，简单可靠，运算量小，工程易于实现。

为了达到上述目的，本发明提供一种姿态机动自适应轨迹规划方法，包含以下步骤：

步骤S1、计算姿态机动需要转过的欧拉角；

步骤S2、计算姿态机动欧拉轴；

步骤S3、计算欧拉轴方向转动惯量；

步骤S4、确定加减速最大时间和加减速最小时间；

步骤S5、根据欧拉角及执行机构能力进行自主路径规划。

所述的步骤S1中，计算机动欧拉角包含：

φ_Euler＝2arccos(q_{a_new2a_old}(4))

其中，q_{a_old2a_new}表示机动前的对地坐标系到目标对地坐标系的姿态四元数。

所述的步骤S2中，计算机动欧拉轴包含：

$\begin{array}{c}\stackrel{\→}{L}={\left[\begin{array}{ccc}{l}_x& l_y& l_z\end{array}\right]}^T=\\ {\left[\begin{array}{ccc}{q}_{a\_new2a\_old}\left(1\right)/sin({\mathrm{\φ}}_{Euler}/2)& q_{a\_new2a\_old}\left(2\right)/sin({\mathrm{\φ}}_{Euler}/2)& q_{a\_new2a\_old}\left(3\right)/sin({\mathrm{\φ}}_{Euler}/2)\end{array}\right]}^T\end{array}$

所述的步骤S3中，沿欧拉轴方向的转动惯量为：

$J_L=l_x^2{I}_{xx}+l_y^2{I}_{yy}+l_z^2{I}_{zz}-2l_x{l}_y{I}_{xy}-2l_x{l}_z{I}_{xz}-2l_y{l}_z{I}_{yz}$

其中，I_xx,I_yy,I_zz,I_xy,I_xz,I_yz是对地坐标系下的惯量参数，形式为：

$I=\left[\begin{array}{ccc}{I}_{xx}& -I_{xy}& -I_{xz}\\ -I_{xy}& I_{yy}& -I_{yz}\\ -I_{xz}& -I_{yz}& I_{zz}\end{array}\right].$

所述的步骤4中，将加减速最大时间τ_c ¹设置为对应执行机构在机动过程中加速到最大角动量对应的时间；

将加减速最小时间τ_c2定义为：

其中，f₁为挠性附件的基频。

所述的步骤5中，设置两个边界角度：

${\mathrm{\φ}}_{c1}=2\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}{{\mathrm{\τ}}_{c1}}^2$

${\mathrm{\φ}}_{c2}=2\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}{{\mathrm{\τ}}_{c2}}^2$

其中，T_cmax为执行机构的最大输出力矩；ΔI表示允许的转动惯量拉偏阈值；

当φ_Euler>φ_c1时，系统有匀速运动，此时的路径参数为：

最大角加速度：

$a_{max}=\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}$

加减速时间：

τ＝τ_c1

匀速段运动时间：

$t_y=\frac{{\mathrm{\φ}}_{Euler}-{\mathrm{\φ}}_{c1}}{a_{\max }\mathrm{\τ}}$

角加速度路径：

$a_r=\left\{\begin{array}{c}\frac{a_{max}}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}t\left)\right),t\∈(0,2\mathrm{\τ})\\ 0,t\∈(2\mathrm{\τ},2\mathrm{\τ}+t_y)\\ -\frac{a_{\max }}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}(t-t_y-2\mathrm{\τ})\left)\right),t\∈(t_y+2\mathrm{\τ},t_y+4\mathrm{\τ})\end{array}\right.$

角速度路径：

${\mathrm{\ω}}_r=\left\{\begin{array}{c}\frac{a_{max}}{2}t-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right),t\∈(0,2\mathrm{\τ})\\ a_{max}\mathrm{\τ},t\∈(2\mathrm{\τ},2\mathrm{\τ}+t_y)\\ a_{\max }\mathrm{\τ}-\[\frac{a_{max}}{2}(t-t_y-2\mathrm{\τ})-\frac{a_{\max }\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\right(t-t_y-2\mathrm{\τ}\left)\right)\],t\∈(t_y+2\mathrm{\τ},t_y+4\mathrm{\τ})\end{array}\right.$

角度路径：

当φ_c2＜φ_Euler＜φ_c1时，系统无匀速运动，此时的路径参数为：

最大角加速度：

$a_{max}=\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}$

加减速时间：

$\mathrm{\τ}=\sqrt{\frac{{\mathrm{\φ}}_{Euler}}{2a_{\max }}}$

匀速运动时间：

t_y＝0

角加速度路径：

$a_r=\left\{\begin{array}{c}\frac{a_{max}}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}t\left)\right),t\∈(0,2\mathrm{\τ})\\ -\frac{a_{\max }}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}(t-2\mathrm{\τ})\left)\right),t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角速度路径：

${\mathrm{\ω}}_r=\left\{\begin{array}{c}\frac{a_{max}}{2}t-\frac{a_{\max }\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right),t\∈(0,2\mathrm{\τ})\\ a_{max}\mathrm{\τ}-\[\frac{a_{max}}{2}(t-2\mathrm{\τ})-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\right(t-2\mathrm{\τ}\left)\right)\],t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角度路径：

当φ_Euler＜φ_c2时，系统无匀速运动，此时的路径参数为：

最大角加速度：

$a_{max}=\frac{{\mathrm{\φ}}_{Euler}}{2{{\mathrm{\τ}}_{c2}}^2}$

加减速时间：

τ＝τ_c2

匀速运动时间：

t_y＝0

角加速度路径：

$a_r=\left\{\begin{array}{c}\frac{a_{max}}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}t\left)\right),t\∈(0,2\mathrm{\τ})\\ -\frac{a_{\max }}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}(t-2\mathrm{\τ})\left)\right),t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角速度路径：

${\mathrm{\ω}}_r=\left\{\begin{array}{c}\frac{a_{max}}{2}t-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right),t\∈(0,2\mathrm{\τ})\\ a_{max}\mathrm{\τ}-\[\frac{a_{max}}{2}(t-2\mathrm{\τ})-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\right(t-2\mathrm{\τ}\left)\right)\],t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角度路径：

在得到欧拉轴和规划好的欧拉转角后，可以得到规划的四元数：

本发明通过地面上注的姿态机动角度指令，计算相应的机动欧拉角和欧拉轴，计算沿欧拉轴方向的转动惯量，根据执行机构的最大力矩和最大角动量能力确定对应的最大角加速度和最大角速度，通过对加减速段设计了一阶三角函数过渡过程，使控制力矩的频率与挠性附件的基频隔离，确定允许的加减速最大时间和加减速最小时间范围，从而有效的抑制挠性附件的振动。

附图说明

图1是本发明提供的一种姿态机动自适应轨迹规划方法的流程图。

具体实施方式

以下根据图1具体说明本发明的较佳实施例。

如图1所示，本发明提供一种姿态机动自适应轨迹规划方法，包含以下步骤：

步骤S1、计算姿态机动需要转过的欧拉角；

φ_Euler＝2arccos(q_{a_new2a_old}(4))

其中，q_{a_old2a_new}表示机动前的对地坐标系到目标对地坐标系的姿态四元数；

步骤S2、计算姿态机动的欧拉轴；

$\begin{array}{c}\stackrel{\→}{L}={\left[\begin{array}{ccc}{l}_x& l_y& l_z\end{array}\right]}^T=\\ {\left[\begin{array}{ccc}{q}_{a\_new2a\_old}\left(1\right)/sin({\mathrm{\φ}}_{Euler}/2)& q_{a\_new2a\_old}\left(2\right)/sin({\mathrm{\φ}}_{Euler}/2)& q_{a\_new2a\_old}\left(3\right)/sin({\mathrm{\φ}}_{Euler}/2)\end{array}\right]}^T\end{array}$

步骤S3、计算欧拉轴方向转动惯量；

沿欧拉轴方向的转动惯量为：

$J_L=l_x^2{I}_{xx}+l_y^2{I}_{yy}+l_z^2{I}_{zz}-2l_x{l}_y{I}_{xy}-2l_x{l}_z{I}_{xz}-2l_y{l}_z{I}_{yz}$

其中，I_xx,I_yy,I_zz,I_xy,I_xz,I_yz是对地坐标系下的惯量参数，形式为：

$I=\left[\begin{array}{ccc}{I}_{xx}& -I_{xy}& -I_{xz}\\ -I_{xy}& I_{yy}& -I_{yz}\\ -I_{xz}& -I_{yz}& I_{zz}\end{array}\right]$

步骤S4、确定加减速最大时间和加减速最小时间；

将加减速最大时间τ_c1设置为对应执行机构在机动过程中加速到最大角动量对应的时间，具体根据不同的执行机构来设置；

将加减速最小时间τ_c2定义为：

其中，f₁为挠性附件的基频；

加减速时间越小，对应的控制力矩频率越高；

步骤S5、根据欧拉角及执行机构能力进行自主路径规划；

设置两个边界角度：

${\mathrm{\φ}}_{c1}=2\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}{{\mathrm{\τ}}_{c1}}^2$

${\mathrm{\φ}}_{c2}=2\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}{{\mathrm{\τ}}_{c2}}^2$

其中，T_cmax为执行机构的最大输出力矩，不同的执行机构对应不同的最大输出力矩；ΔI表示允许的转动惯量拉偏阈值，可地面通过遥控注数更改，一般取为0.1～0.3；

当φ_Euler>φ_c1时，系统有匀速运动，此时的路径参数为：

最大角加速度：

$a_{max}=\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}$

加减速时间：

τ＝τ_c1

匀速段运动时间：

$t_y=\frac{{\mathrm{\φ}}_{Euler}-{\mathrm{\φ}}_{c1}}{a_{\max }\mathrm{\τ}}$

角加速度路径：

$a_r=\left\{\begin{array}{c}\frac{a_{max}}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}t\left)\right),t\∈(0,2\mathrm{\τ})\\ 0,t\∈(2\mathrm{\τ},2\mathrm{\τ}+t_y)\\ -\frac{a_{max}}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}(t-t_y-2\mathrm{\τ})\left)\right),t\∈(t_y+2\mathrm{\τ},t_y+4\mathrm{\τ})\end{array}\right.$

角速度路径：

${\mathrm{\ω}}_r=\left\{\begin{array}{c}\frac{a_{max}}{2}t-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right),t\∈(0,2\mathrm{\τ})\\ a_{max}\mathrm{\τ},t\∈(2\mathrm{\τ},2\mathrm{\τ}+t_y)\\ a_{\max }\mathrm{\τ}-\[\frac{a_{max}}{2}(t-t_y-2\mathrm{\τ})-\frac{a_{\max }\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\right(t-t_y-2\mathrm{\τ}\left)\right)\],t\∈(t_y+2\mathrm{\τ},t_y+4\mathrm{\τ})\end{array}\right.$

角度路径：

当φ_c2＜φ_Euler＜φ_c1时，系统无匀速运动，此时的路径参数为：最大角加速度：

$a_{max}=\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}$

加减速时间：

$\mathrm{\τ}=\sqrt{\frac{{\mathrm{\φ}}_{Euler}}{2a_{\max }}}$

匀速运动时间：

t_y＝0

角加速度路径：

$a_r=\left\{\begin{array}{c}\frac{a_{max}}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}t\left)\right),t\∈(0,2\mathrm{\τ})\\ -\frac{a_{\max }}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}(t-2\mathrm{\τ})\left)\right),t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角速度路径：

${\mathrm{\ω}}_r=\left\{\begin{array}{c}\frac{a_{max}}{2}t-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right),t\∈(0,2\mathrm{\τ})\\ a_{max}\mathrm{\τ}-\[\frac{a_{max}}{2}(t-2\mathrm{\τ})-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\right(t-2\mathrm{\τ}\left)\right)\],t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角度路径：

当φ_Euler＜φ_c2时，系统无匀速运动，此时的路径参数为：

最大角加速度：

$a_{max}=\frac{{\mathrm{\φ}}_{Euler}}{2{{\mathrm{\τ}}_{c2}}^2}$

加减速时间：

τ＝τ_c2

匀速运动时间：

t_y＝0

角加速度路径：

$a_r=\left\{\begin{array}{c}\frac{a_{max}}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}t\left)\right),t\∈(0,2\mathrm{\τ})\\ -\frac{a_{\max }}{2}(1-cos(\frac{\mathrm{\π}}{\mathrm{\τ}}(t-2\mathrm{\τ})\left)\right),t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角速度路径：

${\mathrm{\ω}}_r=\left\{\begin{array}{c}\frac{a_{max}}{2}t-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right),t\∈(0,2\mathrm{\τ})\\ a_{max}\mathrm{\τ}-\[\frac{a_{max}}{2}(t-2\mathrm{\τ})-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\right(t-2\mathrm{\τ}\left)\right)\],t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角度路径：

在得到欧拉轴和规划好的欧拉转角后，可以得到规划的四元数：

本发明通过地面上注的姿态四元数qa_old2a_new，计算相应的机动欧拉角和欧拉轴，计算沿欧拉轴方向的转动惯量，根据执行机构的最大力矩和最大角动量能力确定对应的最大角加速度和最大角速度，通过对加减速段设计了一阶三角函数过渡过程，使控制力矩的频率与挠性附件的基频隔离，确定允许的加减速最大时间和加减速最小时间范围，从而有效的抑制挠性附件的振动。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (6)

1.一种姿态机动自适应轨迹规划方法，其特征在于，包含以下步骤：

步骤S1、计算姿态机动需要转过的欧拉角；

步骤S2、计算姿态机动欧拉轴；

步骤S3、计算欧拉轴方向转动惯量；

步骤S4、确定加减速最大时间和加减速最小时间；

步骤S5、根据欧拉角及执行机构能力进行自主路径规划。

2.如权利要求1所述的姿态机动自适应轨迹规划方法，其特征在于，所述的步骤S1中，计算机动欧拉角包含：

φ_Euler＝2arccos(q_{a_new2a_old}(4))

其中，q_{a_old2a_new}表示机动前的对地坐标系到目标对地坐标系的姿态四元数。

3.如权利要求2所述的姿态机动自适应轨迹规划方法，其特征在于，所述的步骤S2中，计算机动欧拉轴包含：

$\begin{array}{c}\stackrel{\→}{L}={\left[\begin{array}{ccc}{l}_x& l_y& l_z\end{array}\right]}^T=\\ {\left[\begin{array}{ccc}{q}_{a\_new2a\_old}\left(1\right)/\sin ({\mathrm{\φ}}_{Euler}/2)& q_{a\_new2a\_old}\left(2\right)/\sin ({\mathrm{\φ}}_{Euler}/2)& q_{a\_new2a\_old}\left(3\right)/\sin ({\mathrm{\φ}}_{Euler}/2)\end{array}\right]}^T\end{array}.$

4.如权利要求3所述的姿态机动自适应轨迹规划方法，其特征在于，所述的步骤S3中，沿欧拉轴方向的转动惯量为：

$J_L=l_x^2{I}_{xx}+l_y^2{I}_{yy}+l_z^2{I}_{zz}-2l_x{l}_y{I}_{xy}-2l_x{l}_z{I}_{xz}-2l_y{l}_z{I}_{yz}$

其中，I_xx,I_yy,I_zz,I_xy,I_xz,I_yz是对地坐标系下的惯量参数，形式为：

$I=\left[\begin{array}{ccc}{I}_{xx}& -I_{xy}& -I_{xz}\\ -I_{xy}& I_{yy}& -I_{yz}\\ -I_{xz}& -I_{yz}& I_{zz}\end{array}\right].$

5.如权利要求4所述的姿态机动自适应轨迹规划方法，其特征在于，所述的步骤4中，将加减速最大时间τ_c1设置为对应执行机构在机动过程中加速到最大角动量对应的时间；

将加减速最小时间τ_c2定义为：

其中，f₁为挠性附件的基频。

6.如权利要求5所述的姿态机动自适应轨迹规划方法，其特征在于，所述的步骤5中，设置两个边界角度：

${\mathrm{\φ}}_{c1}=2\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}{{\mathrm{\τ}}_{c1}}^2$

${\mathrm{\φ}}_{c2}=2\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}{{\mathrm{\τ}}_{c2}}^2$

其中，T_cmax为执行机构的最大输出力矩；ΔI表示允许的转动惯量拉偏阈值；

当φ_Euler>φ_c1时，系统有匀速运动，此时的路径参数为：

最大角加速度：

$a_{max}=\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}$

加减速时间：

τ＝τ_c1

匀速段运动时间：

$t_y=\frac{{\mathrm{\φ}}_{Euler}-{\mathrm{\φ}}_{c1}}{a_{max}\mathrm{\τ}}$

角加速度路径：

$a_r=\left\{\begin{array}{c}\frac{a_{max}}{2}(1-cos\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right)),t\∈(0,2\mathrm{\τ})\\ 0,t\∈(2\mathrm{\τ},2\mathrm{\τ}+t_y)\\ -\frac{a_{\max }}{2}(1-cos\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\left(t-t_y-2\mathrm{\τ}\right)\right)),t\∈(t_y+2\mathrm{\τ},t_y+4\mathrm{\τ})\end{array}\right.$

角速度路径：

${\mathrm{\ω}}_r=\left\{\begin{array}{c}\frac{a_{max}}{2}t-\frac{a_{max}\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right),t\∈(0,2\mathrm{\τ})\\ a_{max}\mathrm{\τ},t\∈(2\mathrm{\τ},2\mathrm{\τ}+t_y)\\ a_{\max }\mathrm{\τ}-\[\frac{a_{max}}{2}(t-t_y-2\mathrm{\τ})-\frac{a_{\max }\mathrm{\τ}}{2\mathrm{\π}}sin\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\left(t-t_y-2\mathrm{\τ}\right)\right)\],t\∈(t_y+2\mathrm{\τ},t_y+4\mathrm{\τ})\end{array}\right.$

角度路径：

当φ_c2＜φ_Euler＜φ_c1时，系统无匀速运动，此时的路径参数为：最大角加速度：

$a_{max}=\frac{T_{cmax}}{(1+\mathrm{\Δ}I)J_L}$

加减速时间：

$\mathrm{\τ}=\sqrt{\frac{{\mathrm{\φ}}_{Euler}}{2a_{max}}}$

匀速运动时间：

t_y＝0

角加速度路径：

$a_r=\left\{\begin{array}{c}\frac{a_{max}}{2}(1-cos\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right)),t\∈(0,2\mathrm{\τ})\\ -\frac{a_{\max }}{2}(1-cos\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\left(t-2\mathrm{\τ}\right)\right)),t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角速度路径：

${\mathrm{\ω}}_r=\left\{\begin{array}{c}\frac{a_{\max }}{2}t-\frac{a_{\max }\mathrm{\τ}}{2\mathrm{\π}}\sin \left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right),t\∈(0,2\mathrm{\τ})\\ a_{\max }\mathrm{\τ}-\[\frac{a_{\max }}{2}(t-2\mathrm{\τ})-\frac{a_{\max }\mathrm{\τ}}{2\mathrm{\π}}\sin \left(\frac{\mathrm{\π}}{\mathrm{\τ}}\right(t-2\mathrm{\τ}\left)\right)\],t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角度路径：

当φ_Euler＜φ_c2时，系统无匀速运动，此时的路径参数为：

最大角加速度：

$a_{max}=\frac{{\mathrm{\φ}}_{Euler}}{2{{\mathrm{\τ}}_{c2}}^2}$

加减速时间：

τ＝τ_c2

匀速运动时间：

t_y＝0

角加速度路径：

$a_r=\left\{\begin{array}{c}\frac{a_{max}}{2}(1-cos\left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right)),t\∈(0,2\mathrm{\τ})\\ -\frac{a_{\max }}{2}(1-cos\left(\frac{\mathrm{\π}}{\mathrm{\τ}}\left(t-2\mathrm{\τ}\right)\right)),t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角速度路径：

${\mathrm{\ω}}_r=\left\{\begin{array}{c}\frac{a_{\max }}{2}t-\frac{a_{\max }\mathrm{\τ}}{2\mathrm{\π}}\sin \left(\frac{\mathrm{\π}}{\mathrm{\τ}}t\right),t\∈(0,2\mathrm{\τ})\\ a_{\max }\mathrm{\τ}-\[\frac{a_{\max }}{2}(t-2\mathrm{\τ})-\frac{a_{\max }\mathrm{\τ}}{2\mathrm{\π}}\sin \left(\frac{\mathrm{\π}}{\mathrm{\τ}}\right(t-2\mathrm{\τ}\left)\right)\],t\∈(2\mathrm{\τ},4\mathrm{\τ})\end{array}\right.$

角度路径：

在得到欧拉轴和规划好的欧拉转角后，可以得到规划的四元数：