具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

参照图1，本申请实施例中提供一种仿人机器人的冗余手臂的抛投轨迹规划方法，所述方法适用于仿人机器人的冗余手臂，所述方法包括：

S1：获取世界坐标系下的世界起点位置、世界目标点位置及世界出手点位置；

S2：获取预设加速度变化曲线，根据所述预设加速度变化曲线、所述世界起点位置、所述世界目标点位置及所述世界出手点位置生成加速阶段轨迹；

S3：根据所述加速阶段轨迹分别进行跟随阶段轨迹的获取、减速阶段轨迹的获取及轨迹拼接，得到抛投轨迹；

S4：采用预设时间间隔，对所述抛投轨迹进行离散处理，得到多个离散位置；

S5：分别对每个所述离散位置进行关节角的求解，得到多个求解结果；

S6：当存在所述求解结果为无解时，对所述世界出手点位置进行调整，重复执行所述获取预设加速轨迹曲线的步骤，直至各个所述求解结果均为有解，根据各个所述求解结果均为有解对应的各个关节角确定目标关节角集合。

本实施例通过首先根据预设加速度变化曲线、世界起点位置、世界目标点位置及世界出手点位置生成加速阶段轨迹，根据加速阶段轨迹分别进行跟随阶段轨迹的获取、减速阶段轨迹的获取及轨迹拼接，得到抛投轨迹，其次采用预设时间间隔，对抛投轨迹进行离散处理，得到多个离散位置，然后分别对每个离散位置进行关节角的求解，得到多个求解结果，最后当存在求解结果为无解时，对世界出手点位置进行调整，重复执行生成加速阶段轨迹的步骤直至各个求解结果均为有解，根据各个求解结果均为有解对应的各个关节角确定目标关节角集合，通过加速阶段轨迹、跟随阶段轨迹、减速阶段轨迹拼接得到三段式的抛投轨迹，三段式的抛投轨迹适用于仿人机器人的冗余手臂，从而实现仿人机器人的冗余手臂的抛投轨迹规划，面对不同的抛投目标具有较强的通用性；而且通过对抛投轨迹离散后针对每个离散位置进行关节角的求解，实现了对仿人机器人的冗余手臂的抛投规划的分步式解析逆解计算，提高了轨迹求解精度，提高了轨迹求解速率。

可以理解的是，本申请的所述仿人机器人的冗余手臂是七度自由度的手臂。

参照图2，所述仿人机器人的冗余手臂的七度自由度包括7个关节，是第一关节的关节角，是第二关节的关节角，是第三关节的关节角，是第四关节的关节角，是第五关节的关节角，是第六关节的关节角，是第七关节的关节角，其中，第一关节、第二关节和第三关节是位于肩部的关节，第四关节是位于肘部的关节，第五关节、第六关节和第七关节是位于手腕的关节。

可以理解的是，第一关节、第二关节、第三关节和第四关节决定仿人机器人的冗余手臂的手臂末端的位置，第五关节、第六关节和第七关节仅影响了仿人机器人的冗余手臂的手臂末端的姿态，在抛投过程中更为看重的是手臂末端的位置以及手掌开合的朝向。手臂末端，是指小臂的两端的靠近手的一端。

从所述冗余手臂到手依次包括：上臂、肘部、小臂、手腕、手。

可以理解的是，在本申请中，中的上标T表述的对向量G1的转置，G1可以是本申请中的任一向量。

对于S1，可以从所述仿人机器人的存储空间获取世界坐标系下的世界起点位置、世界目标点位置及世界出手点位置，也可以从工控机中获取世界坐标系下的世界起点位置、世界目标点位置及世界出手点位置，还可以从第三方应用系统中获取世界坐标系下的世界起点位置、世界目标点位置及世界出手点位置。

世界起点位置，是在世界坐标系下的仿人机器人的冗余手臂抛投时的起点位置。

世界目标点位置，是在世界坐标系下的仿人机器人的冗余手臂抛投时的抛投物体的目标位置。

抛投物体，是需要抛投到世界目标点位置的物体。抛投物体放在所述仿人机器人的冗余手臂的手中进行抛投。

世界出手点位置，是在世界坐标系下的仿人机器人的冗余手臂抛投时冗余手臂的手与抛投物体分离的位置。

可以理解的是，世界起点位置和世界出手点位置，都是仿人机器人的冗余手臂的手臂末端在抛投是经过的位置。

对于S2，可以从所述仿人机器人的存储空间获取预设加速度变化曲线，也可以从工控机中获取预设加速度变化曲线，还可以从第三方应用系统中获取预设加速度变化曲线。

预设加速度变化曲线是加速阶段的加速度的变化曲线。

可选的，所述预设加速度变化曲线设置为梯形曲线。从而实现先采用直线增加的加速度进行加速，然后采用固定的加速度进行加速，最后采用直线减少的加速度进行加速，简化了对冗余手臂的控制。

可以理解的是，所述预设加速度变化曲线还可以设置成其他线性，比如正态分布曲线，在此不做具体限定。

其中，先根据所述世界起点位置、所述世界目标点位置构建抛投平面和抛投坐标系，然后基于抛投平面和抛投坐标系，根据所述预设加速度变化曲线、所述世界起点位置、所述世界目标点位置及所述世界出手点位置生成加速阶段轨迹。

对于S3，根据所述加速阶段轨迹获取跟随阶段轨迹，然后根据跟随阶段轨迹获取减速阶段轨迹，最后将加速阶段轨迹、跟随阶段轨迹、减速阶段轨迹进行依次拼接，将拼接得到的轨迹作为抛投轨迹。从而得到了三段式的抛投轨迹，三段式的抛投轨迹适用于仿人机器人的冗余手臂，从而实现仿人机器人的冗余手臂的抛投轨迹规划，面对不同的抛投目标具有较强的通用性。

加速阶段轨迹，是冗余手臂为抛投做准备进行加速运动的轨迹。

跟随阶段轨迹，是冗余手臂在将抛投物体抛出之后跟随抛投物体做抛物线跟随运动的轨迹。

减速阶段轨迹，是冗余手臂在跟随阶段轨迹后做减速运动的轨迹。

可以理解的是，抛投轨迹是仿人机器人的冗余手臂的手臂末端在抛投时经过的位置数据集合。

可以理解的是，在加速阶段轨迹结束时冗余手臂将抛投物体抛出，然后跟随抛投物体做抛物线跟随运动，最后按减速阶段轨迹做减速运动直至停止。

对于S4，可以从所述仿人机器人的存储空间获取预设时间间隔，也可以从工控机中获取预设时间间隔，还可以从第三方应用系统中获取预设时间间隔。预设时间间隔，是一个具体数值。比如，预设时间间隔设置为0.01秒，在此举例不做具体限定。

其中，对所述抛投轨迹进行离散处理，得到多个离散点，相邻两个离散点的时间差与预设时间间隔相同，将各个离散点的位置作为多个离散位置。从而为对仿人机器人的冗余手臂的抛投规划的分步式解析逆解计算提供了基础。

可以理解的是，离散位置是冗余手臂的手臂末端的位置数据。

对于S5，因为在抛投过程中更为看重的是手臂末端的位置以及手掌开合的朝向，因此在采用逆解计算对每个所述离散位置进行关节角的求解时，前五个关节（第一关节、第二关节、第三关节、第四关节、第五关节）的关节角的优先级高于后两个关节（第六关节、第七关节）的关节角，并且，受冗余手臂的结构空间限制，手腕处第六关节和第七关节的电机不论是力矩还是工作空间都较小，对抛投速度的贡献不大，因为手腕关节对末端姿态的影响会增加逆解计算的复杂度以及导致抛投规划过程的不直观，因此在抛投过程中将后两个关节（第六关节、第七关节）的关节角固定。

因此，分别针对每个离散位置，基于所述冗余手臂的第五关节不影响所述抛投物体的位置和所述冗余手臂的第六关节及第七关节在抛投过程中固定不变的规则，在保持第五关节的关节角不变的情况下对前四个关节（第一关节、第二关节、第三关节、第四关节）进行关节角求解，然后根据求解得到的关节角进行手臂末端的姿态计算，最后基于将所述关节坐标系集合的第七坐标系的Z轴方向作为手掌开合方向和所述冗余手臂的所述第六关节及所述第七关节在抛投过程中固定不变的规则，对所述冗余手臂的各个关节进行关节角计算，从而完成了一轮逆解计算。

对于S6，当存在所述求解结果为无解时，意味着当前的世界出手点位置不是合适的冗余手臂与抛投物体分离的位置，因此需要对所述世界出手点位置进行调整，调整后的所述世界出手点位置用于下一次迭代计算，重复执行步骤S2至步骤S6，直至各个所述求解结果均为有解，然后将最后一次迭代计算的各个离散位置各自对应的所有关节角作为目标关节角集合。也就是说，离散位置的数量乘以7等于目标关节角集合中的关节角数量。

可选的，所述根据各个所述求解结果均为有解对应的各个关节角确定目标关节角集合的步骤，包括：将最后一次迭代计算的每个离散位置对应的七个关节角（第一关节、第二关节、第三关节、第四关节、第五关节、第六关节和第七关节对应的关节角）作为一个关节角向量，将所有关节角向量作为目标关节角集合。

可选的，所述对所述世界出手点位置进行调整的步骤，包括：根据所述求解结果为无解的所述离散位置进行无解轨迹阶段确定，得到无解轨迹阶段集合；获取预设的出手点调整规则，根据所述无解轨迹阶段集合和所述预设的出手点调整规则对所述世界出手点位置进行调整。

无解轨迹阶段集合包括：加速阶段轨迹、跟随阶段轨迹、减速阶段轨迹中的一个或多个。

可选的，所述预设的出手点调整规则为在预设位置范围内随机选择。可以理解的是，所述预设的出手点调整规则还可以设置为其他规则，比如，当所述无解轨迹阶段集合中包括减速阶段轨迹时，意味着抛投过早，此时将所述世界出手点位置往靠近减速阶段轨迹的方向调整，在此举例不做具体限定。

所述目标关节角集合，用于对仿人机器人的冗余手臂的关节的关节角控制，以实现将抛投物体抛投到世界目标点位置。

在一个实施例中，上述根据所述预设加速度变化曲线、所述世界起点位置、所述世界目标点位置及所述世界出手点位置生成加速阶段轨迹的步骤，包括：

S21：根据所述世界起点位置和所述世界目标点位置确定抛投平面及抛投坐标系；

S22：对所述世界起点位置、所述世界目标点位置及所述世界出手点位置进行抛投坐标系转换，得到抛投起点位置、抛投目标点位置和抛投出手点位置；

S23：获取预设加速轨迹曲线，在所述抛投平面中，根据所述预设加速度变化曲线、所述预设加速轨迹曲线、所述抛投起点位置、所述抛投目标点位置和所述抛投出手点位置生成所述加速阶段轨迹。

本实施例实现了先建立抛投平面及抛投坐标系，然后基于抛投平面及抛投坐标系，根据所述预设加速度变化曲线、所述预设加速轨迹曲线生成所述加速阶段轨迹，从而为三段式的抛投轨迹的规划提供了基础。

对于S21，将重力方向、所述世界起点位置和所述世界目标点位置所在的平面作为抛投平面；然后根据抛投平面确定抛投坐标系，其中，将所述世界起点位置在抛投平面的位置作为抛投坐标系的原点，抛投坐标系的X轴与所述抛投平面平行，抛投坐标系的Y轴与所述抛投平面垂直，抛投坐标系的Z轴与重力方向相反。

其中，抛投坐标系的X轴表述为，抛投坐标系的Y轴表述为，抛投坐标系的Z轴表述为，相互计算关系为：

是所述世界目标点位置，是所述世界起点位置，是计算向量的的模长，为，中的T是转置。

其中，是抛投坐标系相对世界坐标系的齐次变换矩阵。

也就是说，齐次变换矩阵包括旋转矩阵和位置坐标。

对于S22，对所述世界起点位置进行抛投坐标系转换，得到抛投起点位置；对所述世界目标点位置进行抛投坐标系转换，得到抛投目标点位置；对所述世界出手点位置进行抛投坐标系转换，得到抛投出手点位置。因将所述世界起点位置在抛投平面的位置作为抛投坐标系的原点，所以所述抛投起点位置为（0,0,0）。

对于S23，可以从所述仿人机器人的存储空间获取预设加速轨迹曲线，也可以从工控机中获取预设加速轨迹曲线，还可以从第三方应用系统中获取预设加速轨迹曲线。

可选的，所述预设加速轨迹曲线设置为直线，从而简化了对冗余手臂的控制。可以理解的是，所述预设加速轨迹曲线还可以设置为其他线性，比如，曲线，在此不做具体限定。

其中，在所述抛投平面中，根据所述预设加速度变化曲线、所述预设加速轨迹曲线、所述抛投起点位置、所述抛投目标点位置和所述抛投出手点位置分别进行抛出角度计算、抛出速度计算及所述加速阶段轨迹生成。

在一个实施例中，上述在所述抛投平面中，根据所述预设加速度变化曲线、所述预设加速轨迹曲线、所述抛投起点位置、所述抛投目标点位置和所述抛投出手点位置生成所述加速阶段轨迹的步骤，包括：

S231：在所述抛投平面中，根据所述预设加速轨迹曲线、所述抛投目标点位置和所述抛投出手点位置分别进行抛出角度计算及抛出速度计算，得到目标抛出角度和目标抛出速度；

S232：获取起点速度、最大加速度、加速总时长和最大加速度用时比例；

S233：在所述抛投平面中，根据所述目标抛出角度、所述目标抛出速度、所述抛投起点位置、所述起点速度、所述最大加速度、所述加速总时长和所述最大加速度用时比例生成所述加速阶段轨迹，其中，所述抛投出手点位置作为所述加速阶段轨迹的轨迹路径的终点，所述目标抛出速度作为所述加速阶段轨迹的轨迹路径的终点的轨迹速度，所述目标抛出角度作为所述加速阶段轨迹的轨迹路径的终点的切线的角度。

本实施例先在所述抛投平面中，根据所述预设加速轨迹曲线、所述抛投目标点位置和所述抛投出手点位置分别进行抛出角度计算及抛出速度计算，然后在所述抛投平面中，根据所述目标抛出角度、所述目标抛出速度、所述抛投起点位置、所述起点速度、所述最大加速度、所述加速总时长和所述最大加速度用时比例生成所述加速阶段轨迹，从而为三段式的抛投轨迹的规划提供了基础。

对于S231，在所述抛投平面中，因为所述预设加速轨迹曲线为直线，所以根据所述抛投出手点位置进行抛出角度计算，将计算得到的抛出角度作为目标抛出角度；然后在所述抛投平面中，根据目标抛出角度、所述抛投目标点位置、所述抛投出手点位置和重力加速度进行抛出速度计算，将计算得到的抛出速度作为目标抛出速度。

可以理解的是，是对的转置计算。

其中，所述预设加速轨迹曲线为直线，所述目标抛出角度的计算公式和目标抛出速度的计算公式为：

是四象限反正切函数，是所述抛投出手点位置，为，为，是所述抛投目标点位置，是转置计算，cos是余弦函数，sin是正弦函数，g是重力加速度。

也就是说，在所述抛投平面中，因为所述预设加速轨迹曲线为直线，首先从所述抛投出手点位置中提取出Z轴方向的数值和X轴方向的数值，然后将提取出的Z轴方向的数值和X轴方向的数值输入四象限反正切函数进行反正切计算，将计算得到的数据作为目标抛出角度；将所述抛投目标点位置减去所述抛投出手点位置，得到抛投目标点与出手点直线距离；在所述抛投平面中，根据重力加速度、所述目标抛出角度和所述抛投目标点与出手点直线距离进行抛出速度计算，将计算得到的抛出速度作为目标抛出速度。

对于S232，可以从所述仿人机器人的存储空间获取起点速度、最大加速度、加速总时长和最大加速度用时比例，还可以从第三方应用系统中获取起点速度、最大加速度、加速总时长和最大加速度用时比例，也可以将起点速度、最大加速度、加速总时长和最大加速度用时比例写入实现本申请的程序中。

起点速度是一个具体数值，是冗余手臂的手臂末端在所述抛投起点位置的速度。

可选的，所述起点速度设置为0。可以理解的是，所述起点速度还可以是其他数值，在此不做具体限定。

最大加速度，是冗余手臂的手臂末端在加速阶段（也就是所述加速阶段轨迹对应的阶段）的最大的加速度。

最大加速度用时比例，是冗余手臂的手臂末端在加速阶段中从所述抛投起点位置开始加速到最大加速度所用时长占加速总时长的比例。

加速总时长，是冗余手臂的手臂末端在加速阶段做加速运动的总时长。

对于S233，因为所述预设加速轨迹曲线为直线，以及冗余手臂的手臂末端是在加速阶段轨迹结束时将抛投物体抛出，所以所述加速阶段轨迹是一条直线轨迹，所述抛投出手点位置是所述加速阶段轨迹的轨迹路径的终点，所述目标抛出速度是所述加速阶段轨迹的轨迹路径的终点的轨迹速度，所述目标抛出角度是所述加速阶段轨迹的轨迹路径的终点的切线的角度。

其中，所述轨迹路径和所述轨迹速度的计算公式为：

是所述目标抛出速度，是所述最大加速度用时比例，是所述加速总时长，是所述最大加速度，是所述起点速度，是所述抛投起点位置。

可以理解的是，是计算的，中的是所述目标抛出速度。

也就是说，将抛投目标点与出手点直线距离的模长依次进行乘以2和除以所述目标抛出速度，将计算得到的数据作为所述加速总时长；将所述最大加速度用时比例与所述加速总时长进行相乘，得到持续增加加速度阶段的时长；将所述最大加速度除以所述持续增加加速度阶段的时长，得到持续增加加速度阶段的平均加速度；将所述加速总时长减去持续增加加速度阶段的时长，得到加速度保持不变阶段的结束时长。

因所述预设加速度变化曲线设置为梯形曲线，针对小于所述持续增加加速度阶段的时长的加速阶段轨迹，根据所述起点速度、持续增加加速度阶段的平均加速度和运行时刻计算轨迹速度，根据所述抛投起点位置、所述起点速度、持续增加加速度阶段的平均加速度和运行时刻计算轨迹位置；针对大于或等于所述持续增加加速度阶段的时长，并且，小于或等于加速度保持不变阶段的结束时长的加速阶段轨迹，根据所述持续增加加速度阶段的时长、所述起点速度、持续增加加速度阶段的平均加速度和运行时刻计算轨迹速度，根据所述持续增加加速度阶段的时长、所述抛投起点位置、所述起点速度、持续增加加速度阶段的平均加速度和运行时刻计算轨迹位置；针对大于加速度保持不变阶段的结束时长，并且，小于所述加速总时长的加速阶段轨迹，根据所述加速总时长、所述持续增加加速度阶段的时长、所述起点速度、持续增加加速度阶段的平均加速度和运行时刻计算轨迹速度，根据所述加速总时长、所述持续增加加速度阶段的时长、所述抛投起点位置、所述起点速度、持续增加加速度阶段的平均加速度和运行时刻计算轨迹位置。

其中，小于所述持续增加加速度阶段的时长的加速阶段轨迹是加速度持续增加的轨迹；大于或等于所述持续增加加速度阶段的时长，并且，小于或等于加速度保持不变阶段的结束时长的轨迹，是加速度保持不变的轨迹的加速阶段轨迹；大于加速度保持不变阶段的结束时长，并且，小于所述加速总时长的加速阶段轨迹，是加速度减少的轨迹。也就是说，加速阶段轨迹分为：加速度增加、加速度保持、加速度减少不变三个子阶段。

参照图3，公开了抛物轨迹示意图，图中是抛物坐标系的原点，也就是所述抛投起点位置，是所述抛投出手点位置和跟随阶段轨迹的开始点，是跟随阶段轨迹的结束点和减速阶段轨迹的开始点，是减速阶段轨迹的结束点，是所述抛投目标点位置。

在一个实施例中，上述分别对每个所述离散位置进行关节角的求解，得到多个求解结果的步骤，包括：

S51：采用DH参数规则，针对所述冗余手臂的手臂基座及各个关节建立局部坐标系，得到手臂基座坐标系和关节坐标系集合；

S52：获取所述冗余手臂的上臂长度、小臂长度、手心到手腕长度；

S53：从各个所述离散位置中获取所述离散位置作为目标离散位置；

S54：根据所述目标离散位置、所述手心到手腕长度确定抛投物体的中心点在所述手臂基座坐标系下的中心点初始位置；

S55：获取所述冗余手臂的第五关节角初始值、第六关节角初始值和第七关节角初始值，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度、所述中心点初始位置、所述第五关节角初始值、所述第六关节角初始值和所述第七关节角初始值，对所述冗余手臂的各个关节进行关节角计算，得到关节角计算结果集合；

S56：当所述关节角计算结果集合中不存在关节角计算结果为无解时，重复执行所述从各个所述离散位置中获取所述离散位置作为目标离散位置的步骤，直至所述关节角计算结果集合中存在所述关节角计算结果为无解或完成所述离散位置的获取；

S57：当所述关节角计算结果集合中存在所述关节角计算结果为无解时，确定存在所述关节角计算结果为无解的所述关节角计算结果集合对应的所述求解结果为无解；

S58：当所述关节角计算结果集合中不存在所述关节角计算结果为无解时，确定不存在所述关节角计算结果为无解的所述关节角计算结果集合对应的所述求解结果为有解。

本实施例先针对所述冗余手臂的手臂基座及各个关节建立局部坐标系，其次基于所述冗余手臂的第五关节不影响所述抛投物体的位置和所述冗余手臂的第六关节及第七关节在抛投过程中固定不变的规则进行关节角计算，然后根据计算得到的关节角进行在所述手臂基座坐标系下的手臂末端的姿态计算，最后基于将所述关节坐标系集合的第七坐标系的Z轴方向作为手掌开合方向和所述冗余手臂的所述第六关节及所述第七关节在抛投过程中固定不变的规则对所述冗余手臂的各个关节进行关节角计算，从而实现了采用逆解计算对每个所述离散位置进行关节角的求解，实现了对仿人机器人的冗余手臂的抛投规划的分步式解析逆解计算，提高了轨迹求解精度，提高了轨迹求解速率。

对于S51，DH参数是机器人的机械臂的参数。DH参数一般使用四个参数：X轴旋转、X轴平移、Z轴旋转和Z轴平移；X轴旋转，一般使用α表示；X轴平移，一般使用a表示；Z轴旋转，一般使用θ表示；Z轴平移，一般使用d表示。根据轴的先后顺序分为标准DH参数和改进DH参数。

其中，采用DH参数规则，针对所述冗余手臂的手臂基座及各个关节建立局部坐标系的具体步骤在此不做赘述。

因本申请的所述仿人机器人的冗余手臂是七度自由度的手臂，所以关节坐标系集合中有7个关节坐标系。

对于S52，可以从所述仿人机器人的存储空间获取所述冗余手臂的上臂长度、小臂长度、手心到手腕长度，也可以从工控机中获取所述冗余手臂的上臂长度、小臂长度、手心到手腕长度，还可以从第三方应用系统中获取所述冗余手臂的上臂长度、小臂长度、手心到手腕长度，还可以将所述冗余手臂的上臂长度、小臂长度、手心到手腕长度写入实现本申请的程序中。

上臂长度，是所述冗余手臂的上臂的长度。

小臂长度，是所述冗余手臂的小臂的长度。

手心到手腕长度，是所述冗余手臂的手的手心到手腕的长度。

对于S53，依次从各个所述离散位置中获取一个所述离散位置，将获取的所述离散位置作为目标离散位置。

对于S54，因目标离散位置是抛投坐标系下的位置，将所述目标离散位置进行所述手臂基座坐标系下的位置转换，得到基座离散位置；基座离散位置是冗余手臂的手臂末端的位置数据，而手心到手腕的长度是固定数值，因此，根据基座离散位置和手心到手腕的长度，可以得到抛投物体的中心点在所述手臂基座坐标系下的中心点初始位置。

对于S55，第五关节角初始值，是第五关节的关节角的初始值。第六关节角初始值，是第六关节的关节角的初始值。第七关节角初始值，是第七关节的关节角的初始值。

其中，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度、所述中心点初始位置、所述第五关节角初始值、所述第六关节角初始值和所述第七关节角初始值，依次进行对所述冗余手臂的第一关节、第二关节、第三关节及第四关节进行关节角计算和对所述冗余手臂的各个关节进行关节角计算，将计算得到的各个关节角作为关节角计算结果集合

对于S56，当所述关节角计算结果集合中不存在关节角计算结果为无解时，意味着在目标离散位置能计算出第一关节对应的关节角、第二关节对应的关节角、第三关节对应的关节角、第四关节对应的关节角、第五关节对应的关节角的值，重复执行步骤S53至步骤S56，直至所述关节角计算结果集合中存在所述关节角计算结果为无解或完成所述离散位置的获取。

对于S57，当所述关节角计算结果集合中存在所述关节角计算结果为无解时，意味着存在所述关节角计算结果集合对应的所述离散位置对应的所述求解结果为无解，因此确定存在所述关节角计算结果为无解的所述关节角计算结果集合对应的所述离散位置对应的所述求解结果为无解。

对于S8，当所述关节角计算结果集合中不存在所述关节角计算结果为无解时，意味着所有的所述关节角计算结果集合中关节角计算结果都是有解的，因此确定不存在所述关节角计算结果为无解的所述关节角计算结果集合对应的所述离散位置对应的所述求解结果为有解。

在一个实施例中，上述根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度、所述中心点初始位置、所述第五关节角初始值、所述第六关节角初始值和所述第七关节角初始值，对所述冗余手臂的各个关节进行关节角计算，得到关节角计算结果集合的步骤，包括：

S551：基于所述冗余手臂的第五关节不影响所述抛投物体的位置和所述冗余手臂的第六关节及第七关节在抛投过程中固定不变的规则，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度和所述中心点初始位置，对所述冗余手臂的第一关节、第二关节、第三关节及第四关节进行关节角计算，得到第一关节角待调整值、第二关节角待调整值、第三关节角待调整值和第四关节角待调整值；

S552：根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度、所述中心点初始位置、所述第一关节角待调整值、所述第二关节角待调整值、所述第三关节角待调整值、所述第四关节角待调整值、所述第五关节角初始值、所述第六关节角初始值和所述第七关节角初始值，对所述冗余手臂的各个关节进行关节角计算，得到所述关节角计算结果集合。

本实施例先基于所述冗余手臂的第五关节不影响所述抛投物体的位置和所述冗余手臂的第六关节及第七关节在抛投过程中固定不变的规则进行关节角计算，然后对所述冗余手臂的各个关节进行关节角计算，从而实现了采用逆解计算对每个所述离散位置进行关节角的求解，实现了对仿人机器人的冗余手臂的抛投规划的分步式解析逆解计算，提高了轨迹求解精度，提高了轨迹求解速率。

对于S551，基于所述冗余手臂的第五关节不影响所述抛投物体的位置和所述冗余手臂的第六关节及第七关节在抛投过程中固定不变的规则，也就是保持第五关节对应的关节角、第六关节对应的关节角和第七关节对应的关节角不变化。

其中，对所述冗余手臂的第一关节、第二关节、第三关节及第四关节进行关节角计算，也就是计算所述冗余手臂的手臂末端运动到所述目标离散位置时，第一关节对应的关节角、第二关节对应的关节角、第三关节对应的关节角及第四关节对应的关节角的值。将对第一关节的关节角计算得到的数值作为第一关节角待调整值，将对第二关节的关节角计算得到的数值作为第二关节角待调整值，将对第三关节的关节角计算得到的数值作为第三关节角待调整值，将对第四关节的关节角计算得到的数值作为第四关节角待调整值。

对于S552，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度、所述中心点初始位置、所述第一关节角待调整值、所述第二关节角待调整值、所述第三关节角待调整值、所述第四关节角待调整值、所述第五关节角初始值、所述第六关节角初始值和所述第七关节角初始值，依次进行在所述手臂基座坐标系下的手臂末端的姿态计算和对所述冗余手臂的各个关节进行关节角计算，将计算得到的各个关节角作为所述关节角计算结果集合。

在一个实施例中，上述根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度、所述中心点初始位置、所述第一关节角待调整值、所述第二关节角待调整值、所述第三关节角待调整值、所述第四关节角待调整值、所述第五关节角初始值、所述第六关节角初始值和所述第七关节角初始值，对所述冗余手臂的各个关节进行关节角计算，得到所述关节角计算结果集合的步骤，包括：

S5521：根据所述第一关节角待调整值、所述第二关节角待调整值、所述第三关节角待调整值、所述第四关节角待调整值、所述第五关节角初始值、所述第六关节角初始值和所述第七关节角初始值进行在所述手臂基座坐标系下的手臂末端的姿态计算，得到第一末端姿态；

S5522：基于将所述关节坐标系集合的第七坐标系的Z轴方向作为手掌开合方向和所述冗余手臂的所述第六关节及所述第七关节在抛投过程中固定不变的规则，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度、所述中心点初始位置和所述第一末端姿态，对所述冗余手臂的各个关节进行关节角计算，得到所述关节角计算结果集合。

本实施例首先根据计算得到的关节角进行在所述手臂基座坐标系下的手臂末端的姿态计算，然后基于将所述关节坐标系集合的第七坐标系的Z轴方向作为手掌开合方向和所述冗余手臂的所述第六关节及所述第七关节在抛投过程中固定不变的规则对所述冗余手臂的各个关节进行关节角计算，从而实现了采用逆解计算对每个所述离散位置进行关节角的求解，实现了对仿人机器人的冗余手臂的抛投规划的分步式解析逆解计算，提高了轨迹求解精度，提高了轨迹求解速率。

对于S5521，根据机器人的手臂的运动学正解，根据所述第一关节角待调整值、所述第二关节角待调整值、所述第三关节角待调整值、所述第四关节角待调整值、所述第五关节角初始值、所述第六关节角初始值和所述第七关节角初始值进行在所述手臂基座坐标系下的手臂末端的姿态计算，将计算得到的末端姿态作为第一末端姿态，从而实现将关节角映射到任务空间。

对于S5522，基于将所述关节坐标系集合的第七坐标系（第七关节对应的局部坐标系）的Z轴方向作为手掌开合方向和所述冗余手臂的所述第六关节及所述第七关节在抛投过程中固定不变的规则，也就是保持第六关节对应的关节角和第七关节对应的关节角不变化，在冗余手臂的手臂末端位于所述目标离散位置时，调整第五关节对应的关节角和计算第一关节对应的关节角、第二关节对应的关节角、第三关节对应的关节角及第四关节对应的关节角，以使手掌开合方向与抛投平面平行。

其中，对所述冗余手臂的各个关节进行关节角计算，也就是计算所述冗余手臂的手臂末端运动到所述目标离散位置时，第一关节对应的关节角、第二关节对应的关节角、第三关节对应的关节角、第四关节对应的关节角、第五关节对应的关节角的值，当能计算出关节的关节角的值时意味着该关节对应的关节角计算结果为有解，当不能计算出关节的关节角的值时意味着该关节对应的关节角计算结果为无解。

可以理解的是，每次对所述冗余手臂的各个关节进行关节角计算得到关节角计算结果集合时，关节角计算结果集合是指5个关节角计算结果，5个关节角计算结果分别对应第一关节、第一关节、第一关节、第一关节、第五关节。

在一个实施例中，上述基于所述冗余手臂的第五关节不影响所述抛投物体的位置和所述冗余手臂的第六关节及第七关节在抛投过程中固定不变的规则，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度和所述中心点初始位置，对所述冗余手臂的第一关节、第二关节、第三关节及第四关节进行关节角计算，得到第一关节角待调整值、第二关节角待调整值、第三关节角待调整值和第四关节角待调整值的步骤，包括：

S5511：根据所述中心点初始位置、所述上臂长度、所述小臂长度和所述手心到手腕长度，对所述冗余手臂的第四关节进行关节角计算，得到所述第四关节角待调整值；

S5512：基于所述冗余手臂的第五关节不影响所述抛投物体的位置、所述冗余手臂的第六关节及第七关节在抛投过程中固定不变的规则和所述冗余手臂的手臂末端的运动学方程，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度和所述中心点初始位置，确定所述关节坐标系集合中的第三坐标系相对所述手臂基座坐标系的初始旋转矩阵；

S5513：根据所述初始旋转矩阵，对所述冗余手臂的第一关节、第二关节和第三关节进行关节角计算，得到所述第一关节角待调整值、所述第二关节角待调整值和所述第三关节角待调整值。

本实施例实现了基于所述冗余手臂的第五关节不影响所述抛投物体的位置和所述冗余手臂的第六关节及第七关节在抛投过程中固定不变的规则，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度和所述中心点初始位置，对所述冗余手臂的第一关节、第二关节、第三关节及第四关节进行关节角计算，为进行在所述手臂基座坐标系下的手臂末端的姿态计算提供了基础。

第一关节角待调整值、所述第二关节角待调整值、所述第三关节角待调整值和所述第四关节角待调整值通过如下公式计算得到：

所述冗余手臂的手臂末端的运动学方程为：

其中，是所述中心点初始位置，是所述关节坐标系集合的第三坐标系相对所述手臂基座坐标系的初始旋转矩阵，是所述关节坐标系集合的第四坐标系相对所述关节坐标系集合的第三坐标系的所述旋转矩阵，是在所述冗余手臂的第五关节不影响所述抛投物体的位置和所述冗余手臂的第六关节及第七关节在抛投过程中固定不变时所述关节坐标系集合的第七坐标系相对所述关节坐标系集合的第四坐标系的所述旋转矩阵，是所述上臂长度，是所述小臂长度，是所述手心到手腕长度；

是所述冗余手臂的肩部与手部之间的相对位置关系，是所述冗余手臂的肘部与手部之间的相对位置关系，是所述冗余手臂的手腕与手部之间的相对位置关系，分别表述为：

其中，是所述上臂长度，是所述小臂长度，是所述手心到手腕长度；

根据所述第五关节不影响所述抛投物体的位置，确定手心到肘部长度的计算公式为：

所述冗余手臂的手臂末端的运动学方程表述为：

将等效为：

参照图4，其中，是所述冗余手臂的肘部，是所述冗余手臂的手腕，是所述冗余手臂的所述抛投物体的中心点；

参照图4，将平面与参考平面的夹角作为初始臂角，其中，所述参考平面是所述第三关节的关节角为0时的平面，是所述冗余手臂的肩部；

根据罗德里格斯公式，将所述初始臂角相对所述手臂基座坐标系的旋转矩阵的计算公式为：

其中，为，是轴的单位矢量，是的反对称矩阵，轴是所述冗余手臂的肩部与所述抛投物体的中心点的连线；

所述第三坐标系相对所述手臂基座坐标系的初始旋转矩阵计算公式为：

其中，是3*3的矩阵，是3*3的矩阵，是3*3的矩阵，是为0时所述第三坐标系相对所述手臂基座坐标系的初始旋转矩阵；

根据三角形集合关系，得到所述第四关节角待调整值的计算公式：

根据所述手臂末端的运动学方程的等式关系，得到所述第一关节角待调整值、第二关节角待调整值、第三关节角待调整值的计算公式：

其中，是的第2行第2列的值，是的第2行第2列的值，是的第2行第2列的值，是的第1行第2列的值，是的第1行第2列的值，是的第1行第2列的值，是的第3行第2列的值，是的第3行第2列的值，是的第3行第2列的值，是的第3行第3列的值，是的第3行第3列的值，是的第3行第3列的值，是的第3行第1列的值，是的第3行第1列的值，是的第3行第1列的值。

其中，所述旋转矩阵，是齐次变换矩阵的旋转矩阵。

其中，根据所述手臂末端的运动学方程的等式关系，得到所述第一关节角待调整值、第二关节角待调整值、第三关节角待调整值的计算公式，也就是根据等效手臂的运动学方程等式关系得到所述第一关节角待调整值、第二关节角待调整值、第三关节角待调整值的计算公式。

也就是说，采用公式即可计算得到所述第四关节角待调整值。

采用公式即可计算得到所述第一关节角待调整值；采用公式即可计算得到所述第二关节角待调整值；采用公式即可计算得到所述第三关节角待调整值。

在一个实施例中，上述基于将所述关节坐标系集合的第七坐标系的Z轴方向作为手掌开合方向和所述冗余手臂的所述第六关节及所述第七关节在抛投过程中固定不变的规则，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度、所述中心点初始位置和所述第一末端姿态，对所述冗余手臂的各个关节进行关节角计算，得到所述关节角计算结果集合的步骤，包括：

S55221：基于将所述第七坐标系的Z轴方向作为手掌开合方向和使所述第七坐标系的所述Z轴方向与所述抛投平面平行确定所述第五关节的调整角度；

S55222：根据所述调整角度和所述第五关节角初始值确定所述第五关节对应的关节角计算结果；

S55223：根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述上臂长度和所述手心到手腕长度，对所述冗余手臂的手腕的中心点进行在所述手臂基座坐标系下的位置计算，得到基座手腕中心位置；

S55224：根据所述基座手腕中心位置、所述上臂长度和所述小臂长度进行所述冗余手臂的关节角计算，得到所述第四关节对应的关节角计算结果；

S55225：基于所述冗余手臂的所述第六关节及所述第七关节在抛投过程中固定不变的规则和罗德里格斯公式，根据所述调整角度、所述手臂基座坐标系、所述关节坐标系集合和所述第一末端姿态，确定所述关节坐标系集合中的第三坐标系相对所述手臂基座坐标系的目标旋转矩阵；

S55226：根据所述目标旋转矩阵，对所述冗余手臂的第一关节、第二关节和第三关节进行关节角计算，得到所述第一关节对应的关节角计算结果、所述第二关节对应的关节角计算结果和所述第三关节对应的关节角计算结果；

S55227：根据所述第一关节对应的关节角计算结果、所述第二关节对应的关节角计算结果、所述第三关节对应的关节角计算结果、所述第四关节对应的关节角计算结果和所述第五关节对应的关节角计算结果，确定所述关节角计算结果集合。

本实施例实现了基于将所述关节坐标系集合的第七坐标系的Z轴方向作为手掌开合方向和所述冗余手臂的所述第六关节及所述第七关节在抛投过程中固定不变的规则，根据所述手臂基座坐标系、所述关节坐标系集合、所述上臂长度、所述小臂长度、所述手心到手腕长度、所述中心点初始位置和所述第一末端姿态，对所述冗余手臂的各个关节进行关节角计算，从而实现了对抛投轨迹离散后针对每个离散位置进行关节角的求解，实现了对仿人机器人的冗余手臂的抛投规划的分步式解析逆解计算，提高了轨迹求解精度，提高了轨迹求解速率。

基于将所述第七坐标系的Z轴方向作为手掌开合方向和使所述第七坐标系的所述Z轴方向与所述抛投平面平行，采用调整角度对所述第五关节对应的关节角进行调整；

其中，是所述手臂基座坐标系相对所述世界坐标系的旋转矩阵，是围绕所述世界坐标系的X轴的旋转矩阵，是所述冗余手臂的第五关节角初始值，是所述抛投坐标系相对所述世界坐标系的旋转矩阵的第二列向量，为，是所述第一末端姿态，在采用调整角度对所述第五关节对应的关节角进行调整之后所述第一末端姿态更新为；

所述冗余手臂的所述手腕的中心点在所述手臂基座坐标系下的位置作为基座手腕中心位置，基座手腕中心位置的计算公式为：

是在采用调整角度对所述第五关节对应的关节角进行调整之后所述第三坐标系相对所述手臂基座坐标系的目标旋转矩阵，是在采用调整角度对所述第五关节对应的关节角进行调整之后所述第四坐标系相对所述第三坐标系的旋转矩阵；

在采用调整角度对所述第五关节对应的关节角进行调整之后所述初始臂角更新为调整后的臂角，根据罗德里格斯公式，将所述调整后的目标臂角相对所述手臂基座坐标系的旋转矩阵为：

其中，是3*3的矩阵，是3*3的矩阵，是3*3的矩阵，是在采用调整角度对所述第五关节对应的关节角进行调整之后为0时所述第三坐标系相对所述手臂基座坐标系的旋转矩阵, 是轴的单位矢量，是的反对称矩阵，轴是所述冗余手臂的肩部与所述冗余手臂的手腕的连线；

根据三角形集合关系，得到计算公式：

其中，上标T是转置计算；

根据所述手臂末端的运动学方程的等式关系，得到所述第一关节对应的关节角计算结果、所述第二关节对应的关节角计算结果、所述第三关节对应的关节角计算结果的计算公式：

其中，是的第2行第2列的值，是的第2行第2列的值，是的第2行第2列的值，是的第1行第2列的值，是的第1行第2列的值，是的第1行第2列的值，是的第3行第2列的值，是的第3行第2列的值，是的第3行第2列的值，是的第3行第3列的值，是的第3行第3列的值，是的第3行第3列的值，是的第3行第1列的值，是的第3行第1列的值，是的第3行第1列的值。

其中，采用调整角度对所述第五关节对应的关节角进行调整，以使手掌开合方向与抛投平面平行。

其中，采用公式即可计算得到所述第四关节对应的关节角计算结果；采用公式即可计算得到所述第一关节对应的关节角计算结果；采用公式即可计算得到所述第二关节对应的关节角计算结果；采用公式即可计算得到所述第三关节对应的关节角计算结果。

其中，因所述冗余手臂的所述第六关节及所述第七关节在抛投过程中固定不变，因此可以将所述第六关节角初始值作为所述第六关节对应的关节角计算结果，可以将所述第七关节角初始值作为所述第七关节对应的关节角计算结果，最后将所述第一关节对应的关节角计算结果、所述第二关节对应的关节角计算结果、所述第三关节对应的关节角计算结果、所述第四关节对应的关节角计算结果、所述第五关节对应的关节角计算结果、所述第六关节对应的关节角计算结果和所述第七关节对应的关节角计算结果，作为所述关节角计算结果集合。

在一个实施例中，上述根据所述加速阶段轨迹分别进行跟随阶段轨迹的获取、减速阶段轨迹的获取及轨迹拼接，得到抛投轨迹的步骤，包括：

S31：获取所述冗余手臂的手指开合延时数据；

S32：根据所述手指开合延时数据和所述加速阶段轨迹获取所述跟随阶段轨迹；

S33：获取所述冗余手臂的末端最大加速度；

S34：根据所述末端最大加速度和所述跟随阶段轨迹获取所述减速阶段轨迹；

S35：将所述加速阶段轨迹、所述跟随阶段轨迹和所述减速阶段轨迹进行依次拼接，得到所述抛投轨迹。

本实施例根据所述手指开合延时数据和所述加速阶段轨迹获取所述跟随阶段轨迹，根据所述末端最大加速度和所述跟随阶段轨迹获取所述减速阶段轨迹，最后将所述加速阶段轨迹、所述跟随阶段轨迹和所述减速阶段轨迹进行依次拼接，实现了三段式的抛投轨迹，三段式的抛投轨迹适用于仿人机器人的冗余手臂，从而实现仿人机器人的冗余手臂的抛投轨迹规划，面对不同的抛投目标具有较强的通用性。

对于S31，可以从所述仿人机器人的存储空间获取所述冗余手臂的手指开合延时数据，也可以从工控机中获取所述冗余手臂的手指开合延时数据，还可以从第三方应用系统中获取所述冗余手臂的手指开合延时数据，还可以将所述冗余手臂的手指开合延时数据写入实现本申请的程序中。

手指开合延时数据，是仿人机器人的冗余手臂从手指开合指令发出到仿人机器人的冗余手臂执行手指开合的时间差数据。

对于S32，将所述加速阶段轨迹的终点数据作为所述跟随阶段轨迹的起点数据，根据与所述手指开合延时数据正比的关系，确定所述跟随阶段轨迹。

对于S33，可以从所述仿人机器人的存储空间获取所述冗余手臂的末端最大加速度，也可以从工控机中获取所述冗余手臂的末端最大加速度，还可以从第三方应用系统中获取所述冗余手臂的末端最大加速度，还可以将所述冗余手臂的末端最大加速度写入实现本申请的程序中。

所述冗余手臂的末端最大加速度，是仿人机器人的冗余手臂的手臂末端可以执行的加速度的最大值。

对于S34，将所述跟随阶段轨迹的终点数据作为所述减速阶段轨迹的起点数据，根据与所述末端最大加速度的反比关系，确定所述减速阶段轨迹。

可选的，所述减速阶段轨迹采用直线，从而简化了控制操作。

可选的，所述减速阶段轨迹的直线与所述跟随阶段轨迹的终点的切线位于同一直线上。

对于S35，将所述加速阶段轨迹、所述跟随阶段轨迹和所述减速阶段轨迹进行依次拼接，将拼接得到的轨迹作为所述抛投轨迹。因此，所述抛投轨迹依次包括三段轨迹，分别是：所述加速阶段轨迹、所述跟随阶段轨迹和所述减速阶段轨迹。

参照图5，本申请还提出了一种仿人机器人的冗余手臂的抛投轨迹规划装置，所述装置适用于仿人机器人的冗余手臂，所述装置包括：

数据获取模块100，用于获取世界坐标系下的世界起点位置、世界目标点位置及世界出手点位置；

加速阶段轨迹确定模块200，用于获取预设加速度变化曲线，根据所述预设加速度变化曲线、所述世界起点位置、所述世界目标点位置及所述世界出手点位置生成加速阶段轨迹；

抛投轨迹确定模块300，用于根据所述加速阶段轨迹分别进行跟随阶段轨迹的获取、减速阶段轨迹的获取及轨迹拼接，得到抛投轨迹；

离散位置确定模块400，用于采用预设时间间隔，对所述抛投轨迹进行离散处理，得到多个离散位置；

求解结果确定模块500，用于分别对每个所述离散位置进行关节角的求解，得到多个求解结果；

目标关节角集合确定模块600，用于当存在所述求解结果为无解时，对所述世界出手点位置进行调整，重复执行所述获取预设加速轨迹曲线的步骤，直至各个所述求解结果均为有解，根据各个所述求解结果均为有解对应的各个关节角确定目标关节角集合。

本实施例通过首先根据预设加速度变化曲线、世界起点位置、世界目标点位置及世界出手点位置生成加速阶段轨迹，根据加速阶段轨迹分别进行跟随阶段轨迹的获取、减速阶段轨迹的获取及轨迹拼接，得到抛投轨迹，其次采用预设时间间隔，对抛投轨迹进行离散处理，得到多个离散位置，然后分别对每个离散位置进行关节角的求解，得到多个求解结果，最后当存在求解结果为无解时，对世界出手点位置进行调整，重复执行生成加速阶段轨迹的步骤直至各个求解结果均为有解，根据各个求解结果均为有解对应的各个关节角确定目标关节角集合，通过加速阶段轨迹、跟随阶段轨迹、减速阶段轨迹拼接得到三段式的抛投轨迹，三段式的抛投轨迹适用于仿人机器人的冗余手臂，从而实现仿人机器人的冗余手臂的抛投轨迹规划，面对不同的抛投目标具有较强的通用性；而且通过对抛投轨迹离散后针对每个离散位置进行关节角的求解，实现了对仿人机器人的冗余手臂的抛投规划的分步式解析逆解计算，提高了轨迹求解精度，提高了轨迹求解速率。

参照图6，本申请实施例中还提供一种计算机设备，该计算机设备可以是服务器，其内部结构可以如图6所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设计的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于储存仿人机器人的冗余手臂的抛投轨迹规划方法等数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种仿人机器人的冗余手臂的抛投轨迹规划方法。所述仿人机器人的冗余手臂的抛投轨迹规划方法，所述方法适用于仿人机器人的冗余手臂，所述方法包括：获取世界坐标系下的世界起点位置、世界目标点位置及世界出手点位置；获取预设加速度变化曲线，根据所述预设加速度变化曲线、所述世界起点位置、所述世界目标点位置及所述世界出手点位置生成加速阶段轨迹；根据所述加速阶段轨迹分别进行跟随阶段轨迹的获取、减速阶段轨迹的获取及轨迹拼接，得到抛投轨迹；采用预设时间间隔，对所述抛投轨迹进行离散处理，得到多个离散位置；分别对每个所述离散位置进行关节角的求解，得到多个求解结果；当存在所述求解结果为无解时，对所述世界出手点位置进行调整，重复执行所述获取预设加速轨迹曲线的步骤，直至各个所述求解结果均为有解，根据各个所述求解结果均为有解对应的各个关节角确定目标关节角集合。

本实施例通过首先根据预设加速度变化曲线、世界起点位置、世界目标点位置及世界出手点位置生成加速阶段轨迹，根据加速阶段轨迹分别进行跟随阶段轨迹的获取、减速阶段轨迹的获取及轨迹拼接，得到抛投轨迹，其次采用预设时间间隔，对抛投轨迹进行离散处理，得到多个离散位置，然后分别对每个离散位置进行关节角的求解，得到多个求解结果，最后当存在求解结果为无解时，对世界出手点位置进行调整，重复执行生成加速阶段轨迹的步骤直至各个求解结果均为有解，根据各个求解结果均为有解对应的各个关节角确定目标关节角集合，通过加速阶段轨迹、跟随阶段轨迹、减速阶段轨迹拼接得到三段式的抛投轨迹，三段式的抛投轨迹适用于仿人机器人的冗余手臂，从而实现仿人机器人的冗余手臂的抛投轨迹规划，面对不同的抛投目标具有较强的通用性；而且通过对抛投轨迹离散后针对每个离散位置进行关节角的求解，实现了对仿人机器人的冗余手臂的抛投规划的分步式解析逆解计算，提高了轨迹求解精度，提高了轨迹求解速率。

本申请一实施例还提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现一种仿人机器人的冗余手臂的抛投轨迹规划方法，包括步骤：所述方法适用于仿人机器人的冗余手臂，所述方法包括：获取世界坐标系下的世界起点位置、世界目标点位置及世界出手点位置；获取预设加速度变化曲线，根据所述预设加速度变化曲线、所述世界起点位置、所述世界目标点位置及所述世界出手点位置生成加速阶段轨迹；根据所述加速阶段轨迹分别进行跟随阶段轨迹的获取、减速阶段轨迹的获取及轨迹拼接，得到抛投轨迹；采用预设时间间隔，对所述抛投轨迹进行离散处理，得到多个离散位置；分别对每个所述离散位置进行关节角的求解，得到多个求解结果；当存在所述求解结果为无解时，对所述世界出手点位置进行调整，重复执行所述获取预设加速轨迹曲线的步骤，直至各个所述求解结果均为有解，根据各个所述求解结果均为有解对应的各个关节角确定目标关节角集合。

上述执行的仿人机器人的冗余手臂的抛投轨迹规划方法，本实施例通过首先根据预设加速度变化曲线、世界起点位置、世界目标点位置及世界出手点位置生成加速阶段轨迹，根据加速阶段轨迹分别进行跟随阶段轨迹的获取、减速阶段轨迹的获取及轨迹拼接，得到抛投轨迹，其次采用预设时间间隔，对抛投轨迹进行离散处理，得到多个离散位置，然后分别对每个离散位置进行关节角的求解，得到多个求解结果，最后当存在求解结果为无解时，对世界出手点位置进行调整，重复执行生成加速阶段轨迹的步骤直至各个求解结果均为有解，根据各个求解结果均为有解对应的各个关节角确定目标关节角集合，通过加速阶段轨迹、跟随阶段轨迹、减速阶段轨迹拼接得到三段式的抛投轨迹，三段式的抛投轨迹适用于仿人机器人的冗余手臂，从而实现仿人机器人的冗余手臂的抛投轨迹规划，面对不同的抛投目标具有较强的通用性；而且通过对抛投轨迹离散后针对每个离散位置进行关节角的求解，实现了对仿人机器人的冗余手臂的抛投规划的分步式解析逆解计算，提高了轨迹求解精度，提高了轨迹求解速率。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的和实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可以包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双速据率SDRAM（SSRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink）DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、装置、物品或者方法不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、装置、物品或者方法所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、装置、物品或者方法中还存在另外的相同要素。

以上所述仅为本申请的优选实施例，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。