[发明专利]考虑航天器本体姿态运动的机械臂轨迹规划方法

权利要求书

1.考虑航天器本体姿态运动的机械臂轨迹规划方法，其特征在于：包括如下步骤，

步骤一：在航天器初始状态下，通过五次多项式路径规划方法确定机械臂末端作动器的位置和姿态指向的变化轨迹；

步骤二、根据航天器中心体姿态运动需求进行姿态运动规划，同时求取本体系下原期望位置与姿态指向由于中心体姿态运动而产生的变化轨迹，对机械臂的轨迹规划进行补偿；

步骤三：将步骤一与步骤二中得到的末端点运动轨迹对应求和，采用机械臂运动学伪逆算法求解各关节角的运动轨迹，实现考虑航天器本体姿态运动情况的机械臂轨迹规划；

步骤一具体实现方法为，

在航天器初始状态下，定义初始时刻的服务航天器的中心体体坐标系f_b0与惯性系f_e重合，中心体体坐标系f_b随中心体运动而运动；根据服务航天器中心体和机械臂的初始状态，获得机械臂末端在本体系下的初始位置和初始姿态指向预设服务航天器中心体与机械臂运动速度的最大值、航天器中心体坐标系下的原期望位置与原期望姿态指向由于初始姿态指向矢量和原期望姿态指向矢量中均通过方向余弦表示且矢量的模均为1，使得初始姿态指向矢量和原期望姿态指向分别具有不完全独立的三个分量，因而各选取前两个分量作为变量，但是对应的第三个分量存在正负两种可能；为避免所述存在正负两种可能导致的不确定，将由方向余弦表示的姿态指向转化为空间中的两个方位角α_n和α_e，求取末端作动器空间方位角的一阶和二阶时间导数，再将其转化为方向余弦的一阶和二阶时间导数；定义方位角α_n代表姿态指向与本体系X_bO_bZ_b平面之间的夹角，α_e代表姿态指向在本体系X_bO_bZ_b平面上的投影与本体系O_bZ_b轴之间的夹角；

根据对方位角α_n和α_e的定义，有

其中：表示姿态指向中的第二个分量；

在基于姿态指向求解方位角α_e的过程中，为了避免分母为零产生的奇异，将方位角α_e的求解过程定义为：

其中：表示姿态指向中的第二个分量，表示姿态指向中第三个分量；

通过末端作动器初始姿态指向求得初始方位角α_{n_0}和α_{e_0}，通过末端作动器原期望姿态指向求得原期望方位角α_{n_r0}和α_{e_r0}；将末端作动器原期望位置和原期望方位角α_{n_r0}和α_{e_r0}与初始位置和初始方位角α_{n_0}和α_{e_0}做差，得到末端作动器的原位置变化量与原方位角变化量；

在五次多项式规划方法中，根据变量在任务始末状态的差值和变化率限制，求出五次多项式的待定系数，进而得到规划量关于时间二阶平滑的变化过程；将规划量θ的期望值记为θ_r，其初始状态的值记为θ₀，规定参数变化中一阶导数的最大值为二阶导数的最大值为则根据五次多项式，有：

其中，时间系数τ为当前时间t与任务时长t_f的比值；

根据五次多项式的特性与规划量变化率受限等约束条件，任务时长有如下约束：

选择同时满足公式所述条件中最小的一个作为任务所需最短时间，由此得到满足各个机械臂运动约束的最短任务时长t_f；在得到任务时长t_f之后，基于五次多项式的方法，规划量θ及其一阶和二阶时间导数为：

在机械臂轨迹规划过程中，规划量为机械臂末端作动器的位置和姿态指向按式所述的五次多项式的方法求得机械臂末端作动器位置和方位角和光滑连续的一阶和二阶时间导数；

由于机械臂末端作动器的方位角和的变化难以写成显含机械臂关节角角速度的形式，而姿态指向的轨迹则可以写成显含机械臂关节角角速度的形式，有利于机械臂关节轨迹规划；因此在通过机械臂逆运动学关系求解关节角度运动规律之前，将机械臂上末端作动器的方位角和及其轨迹转化为姿态指向的变化轨迹；根据方位角和与姿态指向之间的几何关系，有：

对姿态指向求一阶和二阶时间导数，有

其中α为方位角和组成的矩阵，Φ为方位角α矩阵与姿态指向之间的转换矩阵，具体写作：

至此，通过五次多项式方法规划得到末端作动器在笛卡尔空间下位置与姿态指向平滑连续的理想一阶和二阶时间导数矩阵和

由于姿态指向矢量中的三个分量不完全独立，令表示姿态指向矢量中的前两个分量，记为其中：

公式求得的机械臂末端作动器位置与姿态指向平滑连续的一阶和二阶时间导数矩阵和即为通过五次多项式规划方法求得的机械臂末端作动器运动到原期望位置与原期望姿态指向的轨迹。