本发明涉及一种冗余度空间机械臂轨迹规划方法，属于空间机械臂容错轨迹规划。
背景技术：
1、空间站的大量在轨运行任务依赖操作能力强、运动灵活性高、环境适应性强的空间机械臂，这对空间机械臂的可靠性和安全性提出了很高的要求，因此通常要求空间机械臂具有一定的容错能力。在实际航天应用中，许多空间机械臂被设计成冗余度空间机械臂，以增加冗余自由度的方式使其具有容错能力，如ssrms、era、中国空间站机械臂等均为7自由度机械臂。因此，对冗余度空间机械臂的轨迹规划方法进行改进，以取得优良的运动能力，对进一步提升空间机械臂在轨服役可靠性具有重要意义。
2、对冗余度空间机械臂进行轨迹规划时需依赖其逆运动学，而求解其逆运动学需要已知自运动变量，因此，冗余度空间机械臂轨迹规划的核心是规划自运动变量的轨迹。目前已有基于容错构型群组的冗余度空间机械臂轨迹规划方法，先在容错构型群组中规划自运动变量的轨迹，然后基于逆运动学求解其他关节的轨迹。但是现有容错构型群组仅由代表机械臂构型的点构成，而没有包含速度约束信息，容易导致冗余度空间机械臂关节速度超限，不利于空间机械臂的安全运行。因此，亟需在构建容错构型群组时考虑速度约束信息，以提升空间机械臂运行安全性。
技术实现思路
1、有鉴于此，本发明提供了一种基于容错构型群组的冗余度空间机械臂轨迹规划方法，依据退化可操作度和关节极限指标筛选末端轨迹对应的构型群组，构建出容错构型群组，求解所有关节速度不超限时关节1的速度区间，并将其融入容错构型群组中，最后基于该包含速度约束信息的容错构型群组进行轨迹规划。
2、本发明提供了一种基于容错构型群组的冗余度空间机械臂轨迹规划方法，包括：
3、步骤s1：构建冗余度空间机械臂退化可操作度、关节极限指标；
4、步骤s2：构建关节1与其他关节的速度关系，依据人为设定的关节安全速度区间，得到所有关节速度不超限时关节1的速度区间；
5、步骤s3：求解空间机械臂所有末端轨迹点对应的构型群组，依据所述冗余度空间机械臂退化可操作度、关节极限指标，筛选构型群组中的构型，并构建冗余度空间机械臂容错构型群组；
6、步骤s4：依据所述冗余度空间机械臂容错构型群组和所述所有关节速度不超限时关节1的速度区间，添加速度约束信息，构建包含速度约束信息的冗余度空间机械臂容错构型群组；
7、步骤s5：依据所述包含速度约束信息的冗余度空间机械臂容错构型群组，规划关节1的轨迹，依据所述关节1的轨迹求解其他关节的轨迹，完成轨迹规划。
8、所述步骤s1包括：
9、将冗余度空间机械臂雅可比矩阵j的第i列置零得到冗余度空间机械臂退化雅可比矩阵ij，根据退化雅可比矩阵ij构造冗余度空间机械臂退化可操作度和关节极限指标：
10、
11、式中，j为冗余度空间机械臂雅可比矩阵，ij为冗余度空间机械臂退化雅可比矩阵，ijt表示退化雅可比矩阵ij的转置矩阵，det(ijijt)表示矩阵ijijt的行列式，iw1为冗余度空间机械臂退化可操作度，w2是关节极限指标，θi是第i个关节当前的关节角度，θimax、θimin分别是第i个关节的关节角度上极限、下极限，n是冗余度空间机械臂关节空间维数。
12、所述步骤s2包括：
13、建立冗余度空间机械臂关节速度与末端速度的映射关系：
14、
15、式中，ve为冗余度空间机械臂的末端速度，为冗余度空间机械臂所有关节的速度，是第i个关节的速度；
16、根据冗余度空间机械臂关节速度与末端速度的映射关系，得到关节1与其他关节的速度关系：
17、
18、式中，为其他关节的速度，为关节1的速度，jcol_1为冗余度空间机械臂雅可比矩阵j的第1列，为冗余度空间机械臂雅可比矩阵j去掉第1列后形成的矩阵，m为操作空间维数；
19、依据人为设定的关节安全速度区间分别将代入关节1与其他关节的速度关系式中，得到n-1组关节1的速度区间其中，分别表示由人为设定的关节安全速度区间的最小值、最大值，分别表示关节i速度不超限时关节1的最小速度、最大速度；
20、将n-1组关节1的速度区间取交集，得到所有关节速度不超限时关节1的速度区间其中，分别表示所有关节速度不超限时关节1速度的最小值、最大值。
21、所述步骤s3包括：
22、求解空间机械臂所有末端轨迹点对应的构型群组：
23、w＝{s|f(s)＝xej,j＝1,2,…,nstep}
24、式中，w表示所有末端轨迹点对应的构型群组，s表示单个机械臂构型，f表示冗余度空间机械臂正运动学映射关系，xej表示单个末端轨迹点，是任务执行步数，tf是任务总执行时间，dt是控制系统时间步长；
25、依据所述冗余度空间机械臂退化可操作度、关节极限指标，筛选构型群组中的构型，构建冗余度空间机械臂容错构型群组：
26、wi＝{s|f(s)＝xej,iw1(s)>[iw1],w2(s)<[w2],s∈w,
27、i∈1,2,…,n,j＝1,2,…,nstep}
28、式中，wi是冗余度空间机械臂容错构型群组，[iw1]和[w2]分别为退化可操作度阈值与关节极限指标阈值。
29、所述步骤s4包括：
30、根据所述所有关节速度不超限时关节1的速度区间获得关节轨迹曲线的切向范围为[β1,β2]，其中β1、β2的表达式为：
31、
32、对所述冗余度空间机械臂容错构型群组中的所有构型点求解关节轨迹曲线的切向范围[β1,β2]，给所有构型点添加2个箭头，使2个箭头与水平方向夹角分别为β1、β2，构建出包含速度约束信息的冗余度空间机械臂容错构型群组。
33、所述步骤s5包括：
34、在所述包含速度约束信息的冗余度空间机械臂容错构型群组中从初始构型点开始，从左至右规划出一条连续的曲线，并调整曲线形状，使曲线切向始终位于2个箭头之间，得到关节1的轨迹，并求解其他关节的轨迹。
35、由以上技术方案可以看出，本发明具有以下有益效果：
36、本发明的技术方案中，根据冗余度空间机械臂逆运动学求解所有末端轨迹点对应的构型群组，使空间机械臂能跟踪末端轨迹；构建退化可操作度与关节极限指标，并在构型群组中筛选出满足指标阈值的构型，构成容错构型群组，能使空间机械臂具有容错能力且关节角度不超限；求解所有关节速度不超限时关节1的速度区间，将其作为速度约束信息添加到容错构型群组中，构成包含速度约束信息的冗余度空间机械臂容错构型群组，能使空间机械臂所有关节速度不超限。上述有益效果能提高空间机械臂运行的安全性、可靠性，更符合空间任务的需求。所提基于容错构型群组的冗余度空间机械臂轨迹规划方法能够为系统轨迹规划方法的设计提供依据。
1.一种基于容错构型群组的冗余度空间机械臂轨迹规划方法，其特征在于，所述方法包括：
2.根据权利要求1所述的方法，其特征在于，所述步骤s1包括：
3.根据权利要求1所述的方法，其特征在于，所述步骤s2包括：
4.根据权利要求1所述的方法，其特征在于，所述步骤s3包括：
5.根据权利要求1所述的方法，其特征在于，所述步骤s4包括：
6.根据权利要求1所述的方法，其特征在于，所述步骤s5包括：
