[发明专利]多关节机构轨迹规划方法、装置、电子设备及存储介质

权利要求书

1.一种多关节机构轨迹规划方法，其特征在于，包括：

根据在起始点A和终止点B的多关节机构的各关节姿态，基于关节空间轨迹规划法确定出多关节机构末端由起始点A向终止点B运动过程中第一关节轨迹规划数据；

根据多关节机构末端从起始点A至终止点B运动的末端速度，基于雅可比矩阵的广义逆矩阵法确定出多关节机构的末端由起始点A向终止点B点沿直线运动的第二关节轨迹规划数据；

根据预设的运动提前量，基于第二关节轨迹规划数据规划多关节机构各关节的轨迹，控制关节运动；实时判断所述多关节机构各关节的轨迹中各关节角度值是否超出了关节角度极限值，是，则结合第一关节轨迹规划数据，对所述多关节机构各关节的轨迹中的关节姿态进行优化；

所述预设的运动提前量为多关节机构末端移动距离提前量L或为多关节机构末端移动时间提前量T。

2.根据权利要求1所述的多关节机构轨迹规划方法，其特征在于，

当所述预设的运动提前量为多关节机构末端移动距离提前量L，对所述多关节机构各关节的轨迹中的关节姿态进行优化，包括：

当多关节机构末端运动到时间点t₁时，规划的第二关节轨迹规划数据已经达到时间点t₁+T_L，得到多关节机构的关节角Θ_i(t₁+T_L)；T_L为规划的多关节机构末端从时间点t₁起移动距离L所经历的时间；i＝1,…,N；N为多关节机构的关节数量；

实时检测规划的关节角Θ_i(t₁+T_L)是否超出了对应关节的极限值；是，则结合第一关节轨迹规划数据，基于梯度投影法优化多关节机构的关节角速度

对多关节机构角速度进行积分，确定出优化后的多关节机构末端由A点向B点沿直线运动的关节轨迹{Θ_yi(t)}，并下发至下位机控制关节运动。

3.根据权利要求2所述的多关节机构轨迹规划方法，其特征在于，所述梯度投影法得到的多关节机构优化角速度

式中，为R^N×1的向量，J⁺为雅可比矩阵广义逆，为R^N×3的矩阵，为多关节机构末端在笛卡尔空间中的X、Y、Z的运动速度；k为放大系数，I为单位阵，H(Θ)为优化函数，为梯度，x为多关节机构末端在X轴上的移动量，x的范围为2L；x₁为时间点t₁对应的多关节机构末端在笛卡尔空间中X轴的坐标。

4.根据权利要求3所述的多关节机构轨迹规划方法，其特征在于，优化函数

式中，Θ_i(t₁+T_L)为基于广义逆矩阵法规划得到的关节i的关节角度；θ_i(t₁+T_L)为关节空间轨迹规划法得到的关节i在t₁+T_L时刻的关节角度；θ_imax为关节i的角度范围最大值；θ_imin为关节i的角度范围最小值；函数abs()返回变量的绝对值；函数max()返回两个变量的最大值。