本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，属于多体系统轨迹规划领域。本发明具体实现方法如下：首先在航天器初始状态下，通过五次多项式路径规划方法确定机械臂末端的位置和姿态指向的变化轨迹；根据航天器中心体姿态运动的规划，求取航天器本体坐标系下原期望位置与指向由于中心体姿态运动而产生的变化量，以此对机械臂的运动规划进行补偿；将机械臂末端的原轨迹规划与补偿规划对应求和，记为最终机械臂末端点轨迹，通过运动学伪逆算法对机械臂各关节角的运动轨迹进行规划，进而实现考虑航天器本体姿态运动的机械臂轨迹规划；本发明具有减轻星载计算机实时计算压力，提高轨迹规划效率等优点。
1.考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：包括如下步骤，步骤一：在航天器初始状态下，通过五次多项式路径规划方法确定机械臂末端作动器的位置和姿态指向的变化轨迹；
步骤二、根据航天器中心体姿态运动需求进行姿态运动规划，同时求取本体系下原期望位置 与姿态指向 由于中心体姿态运动而产生的变化轨迹，对机械臂的轨迹规划进行补偿；
步骤三：将步骤一与步骤二中得到的末端点运动轨迹对应求和，采用机械臂运动学伪逆算法求解各关节角的运动轨迹，实现考虑航天器本体姿态运动情况的机械臂轨迹规划；
在航天器初始状态下，定义初始时刻的服务航天器的中心体体坐标系fb0与惯性系fe重合，中心体体坐标系fb随中心体运动而运动；根据服务航天器中心体和机械臂的初始状态，获得机械臂末端在本体系下的初始位置 和初始姿态指向 预设服务航天器中心体与机械臂运动速度的最大值、航天器中心体坐标系下的原期望位置 与原期望姿态指向由于初始姿态指向 矢量和原期望姿态指向 矢量中均通过方向余弦表示且矢量的模均为1，使得初始姿态指向 矢量和原期望姿态指向 分别具有不完全独立的三个分量，因而各选取前两个分量作为变量，但是对应的第三个分量存在正负两种可能；为避免所述存在正负两种可能导致的不确定，将由方向余弦表示的姿态指向转化为空间中的两个方位角αn和αe，求取末端作动器空间方位角的一阶和二阶时间导数，再将其转化为方向余弦的一阶和二阶时间导数；定义方位角αn代表姿态指向 与本体系XbObZb平面之间的夹角，αe代表姿态指向 在本体系XbObZb平面上的投影与本体系ObZb轴之间的夹角；
在基于姿态指向 求解方位角αe的过程中，为了避免分母为零产生的奇异，将方位角αe的求解过程定义为：其中： 表示姿态指向 中的第二个分量， 表示姿态指向 中第三个分量；
通过末端作动器初始姿态指向 求得初始方位角αn_0和αe_0，通过末端作动器原期望姿态指向 求得原期望方位角αn_r0和αe_r0；将末端作动器原期望位置 和原期望方位角αn_r0和αe_r0与初始位置 和初始方位角αn_0和αe_0做差，得到末端作动器的原位置变化量与原方位角变化量；
在五次多项式规划方法中，根据变量在任务始末状态的差值和变化率限制，求出五次多项式的待定系数，进而得到规划量关于时间二阶平滑的变化过程；将规划量θ的期望值记为θr，其初始状态的值记为θ0，规定参数变化中一阶导数的最大值为 二阶导数的最大值为 则根据五次多项式，有：其中， 时间系数τ为当前时间t与任务时长tf的比值；
根据五次多项式的特性与规划量变化率受限等约束条件，任务时长有如下约束：选择同时满足公式所述条件中最小的一个作为任务所需最短时间，由此得到满足各个机械臂运动约束的最短任务时长tf；在得到任务时长tf之后，基于五次多项式的方法，规划量θ及其一阶和二阶时间导数为：在机械臂轨迹规划过程中，规划量为机械臂末端作动器的位置 和姿态指向 按式所述的五次多项式的方法求得机械臂末端作动器位置 和方位角 和 光滑连续的一阶和二阶时间导数；
由于机械臂末端作动器的方位角 和 的变化难以写成显含机械臂关节角角速度的形式，而姿态指向 的轨迹则可以写成显含机械臂关节角角速度的形式，有利于机械臂关节轨迹规划；因此在通过机械臂逆运动学关系求解关节角度运动规律之前，将机械臂上末端作动器的方位角 和 及其轨迹转化为姿态指向 的变化轨迹；根据方位角 和与姿态指向 之间的几何关系，有：对姿态指向 求一阶和二阶时间导数，有
其中α为方位角 和 组成的矩阵，Φ为方位角α矩阵与姿态指向 之间的转换矩阵，具体写作：至此，通过五次多项式方法规划得到末端作动器在笛卡尔空间下位置 与姿态指向平滑连续的理想一阶和二阶时间导数矩阵 和由于姿态指向 矢量中的三个分量不完全独立，令 表示姿态指向 矢量中的前两个分量，记为 其中：公式求得的机械臂末端作动器位置 与姿态指向 平滑连续的一阶和二阶时间导数矩阵 和 即为通过五次多项式规划方法求得的机械臂末端作动器运动到原期望位置 与原期望姿态指向 的轨迹。
2.如权利要求1所述的考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：还包括步骤四，将步骤三中规划的机械臂各关节角轨迹 和 传递给控制器，通过控制器实现对机械臂的运动控制。
3.如权利要求1所述的考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：步骤二具体实现方法为，
航天器中心体中某一期望位置 用其所在的惯性空间位置矢量 与航天器中心体质心所在的惯性空间位置矢量R之间的相对位置表示，并且从惯性系转换到航天器本体系下的转换过程为：其中：Abe表示惯性系到航天器本体系的坐标转换矩阵， 表示航天器中心体中某一期望位置 在惯性系下表示；
根据坐标系旋转关系，坐标转换矩阵Abe的变化率与航天器中心体转动角速度ωb之间的关系为：则原期望点位置的相对运动一阶导数和二阶导数表示为：
惯性空间下的某一姿态指向efe转换到航天器本体系的过程为：
efb＝Abeefe (0.11)根据坐标转换矩阵变化率与坐标系转动速度之间的关系，航天器本体系下该姿态指向efb的一阶和二阶时间导数为：其中：Abe表示惯性系到航天器本体系的坐标转换矩阵， 表示航天器中心体转动角速度ωb的叉乘矩阵你，具体写作：由此得到航天器中心体在姿态运动的情况下，惯性空间中某一期望位置 和某一姿态指向efb在航天器中心体坐标系下变化的一阶和二阶时间导数，将其记为：其中： 表示姿态指向矢量efb中的前两个分量；
式中求得的 和 为惯性空间中某一位置与姿态指向由于航天器中心体姿态运动而产生的变化轨迹，依据所述变化轨迹对机械臂的轨迹规划进行补偿。
4.如权利要求3所述的考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：步骤三具体实现方法为，
将步骤一中得到的在航天器初始状态下机械臂末端点的轨迹 和 与步骤二中得到的航天器中心体姿态运动而产生的补偿量 和 对应求和，记为末端点轨迹和 作为机械臂关节轨迹规划的输入：基于末端作动器的位置和姿态指向与机械臂系统之间的运动学关系，通过对雅可比矩阵伪逆求解的方式，将机械臂在笛卡尔空间中的运动转化为关节构型空间下的运动；
将笛卡尔空间中末端作动器的位置与姿态指向矩阵记为 将各节机械
臂关节角记为η，则位姿矩阵与关节角一阶导数之间具有如下关系：
根据式，依据所求得的末端点轨迹 和 和已知的机械臂运动学关系，反向求解机械臂关节角的运动轨迹；在求解过程中对雅可比矩阵进行伪逆运算，求解的运算公式如下：其中
通过公式获得机械臂各关节角的理想运动轨迹 和 使机械臂能够在航天器中心体进行姿态运动的情况下达到惯性空间中的原定期望位置和姿态指向，进而实现考虑航天器本体姿态运动的机械臂轨迹规划。
5.如权利要求4所述的考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：为改善雅可比矩阵伪逆运算的过程中容易出现奇异的情况，在运算中加入鲁棒调节项，以牺牲精度为代价一定程度上规避奇异；鲁棒调节项中的影响调节权重的因子记为λ，λ的值越大，对雅克比矩阵的调节作用就越大，同时也意味着带来的误差较大；调整后的伪逆运算表达式中 为：其中In×n表示单位矩阵，其维数与规划的末端点轨迹矩阵 和 的行数相同。
考虑航天器本体姿态运动的机械臂轨迹规划方法
技术领域
[0001] 本发明涉及一种考虑航天器姿态运动的机械臂轨迹规划方法，属于多体系统轨迹规划领域。
背景技术
[0002] 携带机械臂的航天器系统在执行在轨操作过程中，控制目标通常为机械臂末端作动器的位置和姿态指向，而航天器系统模型中可以直接控制的物理量为航天器中心体的位置与姿态和各节机械臂关节的转角。因此，为了使末端作动器在空间中以某种姿态指向某期望位置，需要通过轨迹规划算法对机械臂的运动进行规划。在机械臂轨迹规划的相关研究中，出于工程测量的实际条件，机械臂的位置和姿态指向通常都是在航天器中心体坐标系中描述。但是实际任务中航天器中心体可能存在一定的姿态运动，因而当期望到达的空间位置和姿态指向不变的情况下，相对于航天器中心体，原期望位置和姿态指向也会发生变化。因此，在航天器中心体存在姿态运动的情况下，机械臂的轨迹规划中需要考虑航天器中心体的运动影响并加入相应的运动补偿。但是在现有的相关文献中，大多数研究只提及基于星载敏感器对目标的测量进行机械臂的实时运动补偿，这种方法运算量较大，对星载计算机实时运算能力提出了较高的要求，较少有文献对这种机械臂运动补偿进行开环规划。
发明内容
[0003] 本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，要解决的问题是：机械臂上末端作动器的期望位置和指向在惯性空间中固定不变时，依据航天器中心体姿态运动信息补偿机械臂末端作动器的轨迹规划，使机械臂末端作动器能够在航天器中心体有姿态运动的情况下达到惯性空间中的原定期望位置和指向，实现考虑航天器本体姿态运动的机械臂轨迹规划，具有规划效率高的优点。
[0004] 本发明目的是通过下述技术方案实现的。
[0005] 本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，首先在航天器初始状态下，通过五次多项式路径规划方法确定机械臂上末端作动器的位置和姿态指向的变化轨迹。根据航天器中心体姿态运动的规划，求取航天器本体坐标系下原期望位置与指向由于中心体姿态运动而产生的变化量，以此对机械臂的运动规划进行补偿。将机械臂末端的原轨迹规划与补偿规划对应求和，记为最终机械臂末端点轨迹，通过运动学伪逆算法获得机械臂各关节角的运动轨迹，进而实现考虑航天器本体姿态运动的机械臂轨迹规划。该方法具有减轻实时计算压力，提高轨迹规划效率等优点。
[0006] 本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，包括如下步骤：
[0007] 步骤一：在航天器初始状态下，通过五次多项式路径规划方法确定机械臂上末端作动器的位置和姿态指向的变化轨迹。
[0008] 在航天器初始状态下，定义初始时刻的服务航天器的中心体体坐标系fb0与惯性系fe重合，中心体体坐标系fb随中心体运动而运动。根据服务航天器中心体和机械臂的初始状态，获得机械臂末端作动器在本体系下的初始位置 和初始姿态指向 预设服务航天器中心体与机械臂运动速度的最大值、航天器中心体坐标系下的原期望位置 与原期望姿态指向 由于初始姿态指向 矢量和原期望姿态指向 矢量中分别具有不完全独立的三个分量，初始姿态指向 矢量和原期望姿态指向 矢量均通过方向余弦表示，矢量的模均为1，因而各选取前两个分量作为控制量，但是对应的第三个分量存在正负两种可能。为避免所述存在正负两种可能导致的不确定，将由方向余弦表示的姿态指向转化为空间中的两个方位角αn和αe，求取末端作动器空间方位角的一阶和二阶时间导数，再将其转化为方向余弦的一阶和二阶时间导数。定义方位角αn代表姿态指向 与本体系XbObZb平面之间的夹角，αe代表姿态指向 在本体系XbObZb平面上的投影与本体系 ObZb轴之间的夹角。
[0009] 根据对方位角αn和αe的定义，有
[0010]
[0011] 其中： 表示姿态指向 中的第二个分量。
[0012] 在基于姿态指向 求解方位角αe的过程中，为了避免分母为零产生的奇异，将方位角αe的求解过程定义为：
[0013]
[0014] 其中： 表示姿态指向 中的第二个分量， 表示姿态指向 中第三个分量。
[0015] 通过末端作动器初始姿态指向 求得初始方位角αn_0和αe_0，通过末端作动器原期望姿态指向 求得原期望方位角αn_r0和αe_r0。将末端作动器原期望位置 和原期望方位角αn_r0和αe_r0与初始位置 和初始方位角αn_0和αe_0做差，得到末端作动器的原位置变化量与原方位角变化量。
[0016] 在五次多项式规划方法中，根据变量在任务始末状态的差值和变化率限制，求出五次多项式的待定系数，进而得到规划量关于时间二阶平滑的变化过程。将规划量θ的期望值记为θr，其初始状态的值记为θ0，规定参数变化中一阶导数的最大值为 二阶导数的最大值为 则根据五次多项式，有：
[0017]
[0018] 其中， 时间系数τ为当前时间t与任务时长tf的比值。
[0019] 根据五次多项式的特性与规划量变化率受限等约束条件，任务时长有如下约束：
[0020]
[0021] 选择同时满足公式所述条件中最小的一个作为任务所需最短时间，由此得到满足各个机械臂运动约束的最短任务时长tf。在得到任务时长tf之后，基于五次多项式的方法，规划量θ及其一阶和二阶时间导数为：
[0022]
[0023] 在机械臂轨迹规划过程中，规划量为机械臂末端作动器的位置 和姿态指向按式所述的五次多项式的方法求得机械臂末端作动器位置 和方位角 和 光滑连续的一阶和二阶时间导数。
[0024] 由于机械臂末端作动器的方位角 和 的变化难以写成显含机械臂关节角角速度的形式，而姿态指向 的轨迹则可以写成显含机械臂关节角角速度的形式，有利于机械臂关节轨迹规划。因此在通过机械臂逆运动学关系求解关节角度运动规律之前，将机械臂上末端作动器的方位角 和 及其轨迹转化为姿态指向 的变化轨迹。根据方位角和 与姿态指向 之间的几何关系，有：
[0025]
[0026] 对姿态指向 求一阶和二阶时间导数，有
[0027]
[0028] 其中α为方位角 和 组成的矩阵，Φ为方位角α矩阵与姿态指向 之间的转换矩阵，具体写作：
[0029]
[0030] 至此，通过五次多项式方法规划得到末端作动器在笛卡尔空间下位置 与姿态指向 平滑连续的理想一阶和二阶时间导数矩阵 和
[0031]
[0032] 由于姿态指向 矢量中的三个分量不完全独立，令 表示姿态指向 矢量中的前两个分量，记为 其中：
[0033]
[0034] 公式求得的机械臂末端作动器位置 与姿态指向 平滑连续的一阶和二阶时间导数矩阵 和 即为通过五次多项式规划方法求得的机械臂末端作动器运动到原期望位置 与原期望姿态指向 的轨迹。
[0035] 步骤二、根据航天器中心体姿态运动需求进行姿态运动规划，同时求取本体系下原期望位置 与姿态指向 由于中心体姿态运动而产生的变化轨迹，对机械臂的轨迹规划进行补偿。
[0036] 航天器中心体中某一期望位置 用其所在的惯性空间位置矢量 与航天器中心体质心所在的惯性空间位置矢量R之间的相对位置表示，并且从惯性系转换到航天器本体系下的转换过程为：
[0037]
[0038] 其中：Abe表示惯性系到航天器本体系的坐标转换矩阵， 表示航天器中心体中某一期望位置 在惯性系下表示。
[0039] 根据坐标系旋转关系，坐标转换矩阵Abe的变化率与航天器中心体转动角速度ωb之间的关系为：
[0040]
[0041] 则原期望点位置的相对运动一阶导数和二阶导数表示为：
[0042]
[0043] 惯性空间下的某一姿态指向efe转换到航天器本体系的过程为：
[0044] efb＝Abeefe (1.11)
[0045] 根据坐标转换矩阵变化率与坐标系转动速度之间的关系，航天器本体系下该姿态fb指向e 的一阶和二阶时间导数为：
[0046]
[0047] 其中：Abe表示惯性系到航天器本体系的坐标转换矩阵， 表示航天器中心体转动角速度ωb的叉乘矩阵，具体写作：
[0048]
[0049] 由此得到航天器中心体在姿态运动的情况下，惯性空间中某一期望位置 和某一姿态指向efb在航天器中心体坐标系下变化的一阶和二阶时间导数，将其记为：
[0050]
[0051] 其中： 表示姿态指向矢量efb中的前两个分量。
[0052] 式中求得的 和 为惯性空间中某一位置与姿态指向由于航天器中心体姿态运动而产生的变化轨迹，依据所述变化轨迹对机械臂的轨迹规划进行补偿。
[0053] 步骤三：将步骤一与步骤二中得到的末端点运动轨迹对应求和，采用机械臂运动学伪逆算法求解各关节角的运动轨迹，实现考虑航天器本体姿态运动情况的机械臂轨迹规划。
[0054] 将步骤一中得到的在航天器初始状态下机械臂末端点的轨迹 和 与步骤二中得到的航天器中心体姿态运动而产生的补偿量 和 对应求和，记为末端点轨迹 和 作为机械臂关节轨迹规划的输入：
[0055]
[0056] 基于末端作动器的位置和姿态指向与机械臂系统之间的运动学关系，通过对雅可比矩阵伪逆求解的方式，将机械臂在笛卡尔空间中的运动转化为关节构型空间下的运动。
[0057] 将笛卡尔空间中末端作动器的位置与姿态指向矩阵记为 将各节机械臂关节角记为η，则位姿矩阵与关节角一阶导数之间具有如下关系：
[0058]
[0059] 其中J(η)为机械臂关节角的雅可比矩阵。
[0060] 根据式，依据所求得的末端点轨迹 和 和已知的机械臂运动学关系，反向求解机械臂关节角的运动轨迹。在求解过程中对雅可比矩阵进行伪逆运算，求解的运算公式如下：
[0061]
[0062] 其中
[0063] 通过公式获得机械臂各关节角的理想运动轨迹 和 使机械臂能够在航天器中心体进行姿态运动的情况下达到惯性空间中的原定期望位置和姿态指向，进而实现考虑航天器本体姿态运动的机械臂轨迹规划。
[0064] 还包括步骤四：将步骤三中规划的机械臂各关节角轨迹 和 传递给控制器，通过控制器实现对机械臂的运动控制。
[0065] 作为优选，为改善雅可比矩阵伪逆运算的过程中容易出现奇异的情况，在运算中加入鲁棒调节项，以牺牲精度为代价一定程度上规避奇异。鲁棒调节项中的影响调节权重的因子记为λ，λ的值越大，对雅克比矩阵的调节作用就越大，同时也意味着带来的误差较大。调整后的伪逆运算表达式中 为：
[0066]
[0067] 其中In×n表示单位矩阵，其维数与规划的末端点轨迹矩阵 和 的行数相同。
[0068] 有益效果：
[0069] 1、本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，在规划航天器中心体姿态运动的同时求取本体系下原惯性空间期望位置与姿态指向的变化，对机械臂的运动规划进行补偿，使得机械臂在中心体进行姿态运动时仍能到达惯性空间中原定期望位置与姿态指向。
[0070] 2、本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，将航天器中心体姿态运动的作用影响考虑到机械臂的开环运动规划中，不必依靠期望位置与姿态指向的测量信息实时规划机械臂的运动，进而在有航天器姿态运动需求情况下降低机械臂轨迹规划的计算需求，减轻机械臂在轨实时轨迹规划的计算压力，提高规划效率。
[0071] 3、本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，分别在两个传统求解方法中做了一点补充。其一，在通过姿态指向 求解方位角αe时，为了避免分母为零产生的奇异，在传统方位角的求取方法中增加一个判断，当姿态指向 中第一和第三个分量同时为0时定义方位角αe为0。其二，为降低机械臂关节角雅克比矩阵伪逆求解发生奇异的可能性，在伪逆求解过程中加入鲁棒调节项，以牺牲精度为代价一定程度上规避奇异。
附图说明
[0072] 图1为本发明公开的考虑航天器本体姿态运动的机械臂轨迹规划方法的流程示意图。
[0073] 图2为末端作动器姿态指向矢量与方位角示意图。
[0074] 图3(a)为x轴位置追踪结果图。
[0075] 图3(b)为y轴位置追踪结果图。
[0076] 图3(c)为z轴位置追踪结果图。
[0077] 图4(a)为机械臂末端点位置与期望位置之间的差值图。
[0078] 图4(b)为机械臂末端点姿态指向与期望姿态指向之间的差值图。
[0079] 图5为机械臂轨迹规划过程中，机械臂关节角雅可比矩阵的行列式值在是否补充鲁棒调节项的对比。
具体实施方式
[0080] 为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。
[0081] 实施例1：
[0082] 本实施例公开对一个五节臂杆组成的机械臂进行轨迹规划，针对有航天器中心体姿态运动任务情况，对机械臂末端位置及姿态指向进行轨迹规划与仿真。如图1所示，本实施例公开的考虑航天器本体姿态运动的机械臂轨迹规划方法，可通过下述步骤来完成：
[0083] 步骤一：在航天器初始状态下，通过五次多项式路径规划方法确定机械臂上末端作动器的位置和姿态指向的变化轨迹。
[0084] 设航天器及其携带的机械臂相关参数的初值如表1所示。
[0085] 表1航天器中心体与机械臂初始参数
[0086]
[0087] 定义方位角αn代表姿态指向 与本体系XbObZb平面之间的夹角，αe代表姿态指向在本体系XbObZb平面上的投影与本体系ObZb轴之间的夹角。根据对方位角αn和αe的定义，有
[0088]
[0089] 其中： 表示姿态指向 中的第二个分量。
[0090] 在基于姿态指向 求解方位角αe的过程中，为了避免分母为零产生的奇异，将方位角αe的求解过程定义为：
[0091]
[0092] 其中： 表示姿态指向 中的第二个分量， 表示姿态指向 中第三个分量。
[0093] 根据表1所示数据，通过航天器运动学关系得到机械臂末端在航天器本体系下的初始位置 与方位角 和 组成的矩阵方位角矩阵α0为：
[0094]
[0095] α0＝[14.94 95.14]Tdeg
[0096] 中心体姿态、机械臂末端位置与姿态指向在始末状态间的差值如表2所示。
[0097] 表2始末状态参数差值
[0098]
[0099] 在五次多项式规划方法中，根据变量在任务始末状态的差值和变化率限制，求出五次多项式的待定系数，进而得到规划量关于时间二阶平滑的变化过程。将规划量θ的期望值记为θr，其初始状态的值记为θ0，规定参数变化中一阶导数的最大值为 二阶导数的最大值为 则根据五次多项式，有：
[0100]
[0101] 其中， 时间系数τ为当前时间t与任务时长tf的比值。
[0102] 根据五次多项式的特性与变量在任务始末状态下变化率等约束条件，对任务时长有如下约束：
[0103]
[0104] 代入机械臂关节运动等约束条件，得到同时满足航天器中心体运动和各个机械臂运动约束的最短任务时长tf为57.5s。
[0105] 采用五次多项式的方法，对机械臂末端位置 和方位角αn、αe和航天器中心体姿态运动进行规划，规划量及其一阶和二阶导数表示为：
[0106]
[0107] 根据方位角αn和αe与姿态指向 之间的几何关系，有：
[0108]
[0109] 对姿态指向 求一阶和二阶时间导数，有
[0110]
[0111]
[0112] 其中，Φ为方位角α矩阵与姿态指向 之间的转换矩阵，具体写作：
[0113]
[0114] 至此，通过五次多项式方法规划得到末端作动器在笛卡尔空间下位置 与姿态指向 平滑连续的理想一阶和二阶时间导数矩阵 和
[0115]
[0116] 由于姿态指向 矢量中的三个分量不完全独立，令 表示姿态指向 矢量中的前两个分量，记为 其中：
[0117]
[0118] 求得的机械臂末端作动器位置 与姿态指向 平滑连续的一阶和二阶时间导数矩阵 和 即为通过五次多项式规划方法求得的机械臂末端作动器运动到原期望位置 与原期望姿态指向 的轨迹。如图3所示图例为“p1”的实线，为仅进行原空间固定位置而不补偿追踪情况的结果。
[0119] 步骤二：根据航天器中心体姿态运动需求进行姿态运动规划，同时求取本体系下原期望位置 与姿态指向 由于中心体姿态运动而产生的变化轨迹，对机械臂的轨迹规划进行补偿。
[0120] 航天器中心体中某一期望位置 用其所在的惯性空间位置矢量 与航天器中心体质心所在的惯性空间位置矢量R之间的相对位置表示，并且从惯性系转换到航天器本体系下的转换过程为：
[0121]
[0122] 其中：Abe表示惯性系到航天器本体系的坐标转换矩阵， 表示航天器中心体中某一期望位置 在惯性系下表示。
[0123] 根据坐标系旋转关系，坐标转换矩阵Abe的变化率与航天器中心体转动角速度ωb之间的关系为：
[0124]
[0125] 则原期望点位置的相对运动一阶导数和二阶导数表示为：
[0126]
[0127]
[0128] 惯性空间下的某一姿态指向efe转换到航天器本体系的过程为：
[0129] efb＝Abeefe
[0130] 根据坐标转换矩阵变化率与坐标系转动速度之间的关系，航天器本体系下该姿态fb指向e 的一阶和二阶时间导数为：
[0131]
[0132]
[0133] 其中：Abe表示惯性系到航天器本体系的坐标转换矩阵，ωb表示航天器中心体转动角速度，其上方的波浪号为叉乘矩阵标记， 具体写作：
[0134]
[0135] 由此得到航天器中心体在姿态运动的情况下，惯性空间中某一期望位置 和某一姿态指向efb在航天器中心体坐标系下变化的一阶和二阶时间导数，将其记为：
[0136]
[0137] 其中： 表示姿态指向矢量efb中的前两个分量。
[0138] 和 为惯性空间中某一位置与姿态指向由于航天器中心体姿态运动而产生的变化轨迹，依据所述变化轨迹对机械臂的轨迹规划进行补偿。
[0139] 步骤三：将步骤一与步骤二中得到的末端点运动轨迹对应求和，采用机械臂运动学伪逆算法求解各关节角的运动轨迹，实现考虑航天器本体姿态运动情况的机械臂轨迹规划。
[0140] 将步骤一中得到的在航天器初始状态下机械臂末端点的轨迹 和 与步骤二中得到的航天器中心体姿态运动而产生的补偿量 和 对应求和，记为末端点轨迹 和 作为机械臂关节轨迹规划的输入：
[0141]
[0142]
[0143] 基于末端作动器的位置和姿态指向与机械臂系统之间的运动学关系，通过对雅可比矩阵伪逆求解的方式，将机械臂在笛卡尔空间中的运动转化为关节构型空间下的运动。将笛卡尔空间中末端作动器的位置与姿态指向矩阵记为 将各节机械臂
关节角记为 则位姿矩阵与关节角一阶导数之间具有如下关
系：
[0144]
[0145] 其中J(η)为机械臂关节角的雅可比矩阵：
[0146]
[0147] 式中Abj为第j节机械臂坐标系到本体系的坐标转换矩阵，Γj为第j节机械臂转轴方向矩阵，ejt为第j节机械臂与末端作动器之间姿态指向矢量，rjt为第j节机械臂质心到末端作动器质心的矢量。
[0148] 为了使机械臂自由度数目满足位置与姿态指向的规划需求，使机械臂末端关节具有三个自由度。为了确定末端作动器的姿态指向，定义末端作动器相对上一节机械臂的转动角速度与其关节角度之间的关系与航天器姿态运动学中的“3-1-2”旋转方式下角速度和姿态角之间的关系相同。末端作动器坐标系f5与上一节机械臂坐标系f4之间转换矩阵为A45＝(Ay(θ(2))Ax(θ(1))Az(θ(3)))T，θ(k)表示姿态角θ第k个分量。
[0149] 依据所求得的末端点轨迹 和 和已知的机械臂运动学关系，反向求解机械臂关节角的运动轨迹。在求解过程中对雅可比矩阵进行伪逆运算，求解的运算公式如下：
[0150]
[0151]
[0152] 其中
[0153] 得到的机械臂各关节角的理想运动轨迹 和 能够实现考虑航天器本体姿态运动的机械臂轨迹规划，使机械臂能够在航天器中心体进行姿态运动的情况下达到惯性空间中的原定期望位置和姿态指向。如图3(a)、图3(b)与图3(c)所示图例为“p12”的虚线，为依据中心体姿态运动进行机械臂运动补偿的结果，大幅度减小了不进行补偿规划时与期望目标之间的差值，提高了规划精度。如图4(a)和图4(b)所示为考虑航天器本体姿态运动的机械臂轨迹规划与期望位置与姿态指向之间的差值，最终收敛至零，说明能够达期望值。
[0154] 还包括步骤四：将步骤三中规划的机械臂各关节角轨迹 和 传递给控制器，通过控制器实现对机械臂的运动控制。
[0155] 作为优选，为改善雅可比矩阵伪逆运算的过程中容易出现奇异的情况，在运算中加入鲁棒调节项，以牺牲精度为代价一定程度上规避奇异。鲁棒调节项中的影响调节权重的因子记为λ，λ的值越大，对雅克比矩阵的调节作用就越大，同时也意味着带来的误差较大。修改后的伪逆运算表达式中 为：
[0156]
[0157] 式中I5×5表示五维的单位矩阵。
[0158] 本实施例中取调节因子λ＝10-3。如图5所示为是否添加鲁棒调节项对雅可比矩阵行列式值的影响，越接近零越容易产生奇异。
[0159] 在上述机械臂的轨迹规划方法下，机械臂末端能够在航天器中心体进行一定姿态运动的情况下到达惯性空间中某一固定的期望位置与姿态指向，不必依靠期望位置与姿态指向的测量信息实时规划机械臂的运动，减轻机械臂在轨实时轨迹规划的计算压力，提高规划效率。
[0160] 以上所述的具体描述，对发明的目的、技术方案和有益效果进行进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
