多关节机构轨迹规划方法、装置、电子设备及存储介质制造方法及图纸

本发明专利技术涉及一种多关节机构轨迹规划方法、装置、电子设备及存储介质；方法包括：基于关节空间轨迹规划法确定出多关节机构末端由起始点A点向终止点B点运动过程中第一关节轨迹规划数据；基于广义逆矩阵法确定出多关节机构末端由起始点A点向终止点B点沿直线运动的第二关节轨迹规划数据；根据预设的运动提前量，基于第二关节轨迹规划数据规划多关节机构各关节的轨迹；实时判断轨迹规划中各关节角度值是否超出了关节角度极限值，是，对多关节机构各关节的轨迹中的关节姿态进行优化。本发明专利技术实现多关节机构末端的直线运动，解决在运动中关节角度超限的问题；对多种初始位型均能实现优化目的，从而降低优化难度。从而降低优化难度。从而降低优化难度。

【技术实现步骤摘要】
多关节机构轨迹规划方法、装置、电子设备及存储介质


[0001]本专利技术涉及关节控制
，具体而言，涉及一种多关节机构轨迹规划方法、装置、电子设备及存储介质。

技术介绍

[0002]混凝土泵车臂架是一种冗余自由度串联的多关节机构，实现其末端由A点至B点的直线运动的关节轨迹规划，一直是行业难题。冗余自由度臂架的关节空间规划，可以实现臂架末端由A点至B点的运动，但是臂架末端的运动轨迹不是直线；基于梯度投影法，可以实现臂架末端的直线运动，但是容易造成关节角度超限，从而臂架末端不能到达B点。
[0003]因此，需要一种新的泵车臂架关节轨迹规划方法、装置、电子设备及存储介质。
[0004]在所述
技术介绍
部分公开的上述信息仅用于加强对本专利技术的背景的理解，因此它可以包括不构成对本领域普通技术人员已知的现有技术的信息。

技术实现思路

[0005]有鉴于此，本专利技术提供一种多关节机构轨迹规划方法、装置、电子设备及存储介质，可应用于混凝土泵车臂架，实现关节末端的直线运动，解决在运动中关节角度超限的问题。
[0006]本专利技术的其他特性和优点将通过下面的详细描述变得显然，或部分地通过本专利技术的实践而习得。
[0007]根据本专利技术的一方面公开了一种多关节机构轨迹规划方法，包括：
[0008]根据作业的起始点A和终止点B的多关节机构各关节姿态，基于关节空间轨迹规划法确定出多关节机构末端由起始点A点向终止点B点运动过程中第一关节轨迹规划数据；
[0009]根据多关节机构末端从起始点A至终止点B运动的末端速度，基于广义逆矩阵法确定出多关节机构末端由起始点A向终止点B点沿直线运动的第二关节轨迹规划数据；
[0010]根据预设的运动提前量，基于第二关节轨迹规划数据规划多关节机构各关节的轨迹，控制关节运动；实时判断所述多关节机构各关节的轨迹中各关节角度值是否超出了关节角度极限值，是，则结合第一关节轨迹规划数据，对所述多关节机构各关节的轨迹中的关节姿态进行优化。
[0011]进一步地，所述预设的运动提前量为多关节机构末端移动距离提前量L。
[0012]进一步地，对所述多关节机构各关节的轨迹中的关节姿态进行优化，包括：
[0013]当多关节机构末端运动到时间点t1时，规划的第二关节轨迹规划数据已经达到时间点t1+T
L
，得到多关节机构的关节角Θ
i
(t1+T
L
)；T
L
为规划的多关节机构末端从时间点t1起移动距离L所经历的时间；i＝1,
…
,N；N为多关节机构关节的数量；
[0014]实时检测规划的关节角Θ
i
(t1+T
L
)是否超出了对应关节的极限值；是，则结合第一关节轨迹规划数据，基于梯度投影法优化多关节机构的关节角速度
[0015]对多关节机构角速度进行积分，确定出优化后的多关节机构末端由A点向B点沿直线运动的关节轨迹{Θ
yi
(t)}，并下发至下位机控制关节运动。
[0016]进一步地，所述梯度投影法得到的多关节机构优化角速度进一步地，所述梯度投影法得到的多关节机构优化角速度
[0017]式中，为R
N
×1的向量，J
+
为雅可比矩阵广义逆，为R
N
×3的矩阵，的矩阵，为多关节机构末端在笛卡尔空间中的X、Y、Z的运动速度；k为放大系数，I为单位阵，H(Θ)为优化函数，
▽
H(Θ)为梯度，x为多关节机构末端在X轴上的移动量，x的范围为2L；x1为时间点t1对应的多关节机构末端在笛卡尔空间中X轴的坐标。
[0018]进一步地，优化函数
[0019][0020]式中，Θ
i
(t1+T
L
)为基于广义逆矩阵法规划得到的关节i的关节角度；θ
i
(t1+T
L
)为关节空间轨迹规划法得到的关节i在t1+T
L
时刻的关节角度；θ
imax
为关节i的角度范围最大值；θ
imin
为关节i的角度范围最小值；函数abs()返回变量的绝对值；函数max()返回两个变量的最大值。
[0021]进一步地，所述预设的运动提前量为多关节机构末端移动时间提前量T。
[0022]进一步地，对所述多关节机构各关节的轨迹中的关节姿态进行优化，包括：
[0023]当多关节机构末端运动到时间点t1时，规划的第二关节轨迹规划数据已经达到时间点t1+T，得到多关节机构的关节角Θ
i
(t1+T)；T为规划的多关节机构末端移动时间提前量；i＝1,
…
,N；N为多关节机构关节的数量；
[0024]实时检测规划的关节角Θ
i
(t1+T)是否超出了各关节的极限值；是，则结合第一关节轨迹规划数据，基于梯度投影法优化多关节机构的关节角速度
[0025]对多关节机构角速度进行积分，确定出优化后的多关节机构末端由A点向B点沿直线运动的关节轨迹{Θ
yi
(t)}，并下发至下位机控制关节运动。
[0026]进一步地，所述梯度投影法得到的多关节机构优化角速度进一步地，所述梯度投影法得到的多关节机构优化角速度
[0027]式中，为R
N
×1的向量，J
+
为雅可比矩阵广义逆，为R
N
×3的矩阵，的矩阵，为多关节机构末端在笛卡尔空间中的X、Y、Z的运动速度；k为放大系数，I为单位阵，H(Θ)为优化函数，
▽
H(Θ)为梯度。
[0028]进一步地，优化函数
[0029][0030]式中，Θ
i
(t1+T)为基于广义逆矩阵法规划得到的关节i的关节角度；θ
i
(t1+T)为关节空间轨迹规划法得到的关节i在t1+T时刻的关节角度；θ
imax
为关节i的角度范围最大值；θ
imin
为关节i的角度范围最小值；函数abs()返回变量的绝对值，函数max()返回两个变量的最大值。
[0031]进一步地，所述放大系数k的取值，包括；
[0032]确定多关节机构中的某一个关节j为优化对象；
[0033]基于梯度投影法优化的关节优化速度qd_h＝(I
‑
J+J)
▽
H(Θ)；qd_h为R
N
×1的向量；则关节j的关节优化速度为qd_h的第j分量qd_h(j)；
[0034]放大系数k取值为式中，δt为优化时间参数；θ
j
(t)为关节空间轨迹规划法得到的关节j的关节角，Θ
j
(t)为基于广义逆矩阵法规划的关节j的关节角。
[0035]进一步地，当多关节机构中含有反关节运动的关节时，所述优化对象为反关节本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种多关节机构轨迹规划方法，其特征在于，包括：根据在起始点A和终止点B的多关节机构的各关节姿态，基于关节空间轨迹规划法确定出多关节机构末端由起始点A向终止点B运动过程中第一关节轨迹规划数据；根据多关节机构末端从起始点A至终止点B运动的末端速度，基于广义逆矩阵法确定出多关节机构的末端由起始点A向终止点B点沿直线运动的第二关节轨迹规划数据；根据预设的运动提前量，基于第二关节轨迹规划数据规划多关节机构各关节的轨迹，控制关节运动；实时判断所述多关节机构各关节的轨迹中各关节角度值是否超出了关节角度极限值，是，则结合第一关节轨迹规划数据，对所述多关节机构各关节的轨迹中的关节姿态进行优化。2.根据权利要求1所述的多关节机构轨迹规划方法，其特征在于，所述预设的运动提前量为多关节机构末端移动距离提前量L。3.根据权利要求2所述的多关节机构轨迹规划方法，其特征在于，对所述多关节机构各关节的轨迹中的关节姿态进行优化，包括：当多关节机构末端运动到时间点t1时，规划的第二关节轨迹规划数据已经达到时间点t1+T
L
，得到多关节机构的关节角Θ
i
(t1+T
L
)；T
L
为规划的多关节机构末端从时间点t1起移动距离L所经历的时间；i＝1,
…
,N；N为多关节机构的关节数量；实时检测规划的关节角Θ
i
(t1+T
L
)是否超出了对应关节的极限值；是，则结合第一关节轨迹规划数据，基于梯度投影法优化多关节机构的关节角速度对多关节机构角速度进行积分，确定出优化后的多关节机构末端由A点向B点沿直线运动的关节轨迹{Θ
yi
(t)}，并下发至下位机控制关节运动。4.根据权利要求3所述的多关节机构轨迹规划方法，其特征在于，所述梯度投影法得到的多关节机构优化角速度的多关节机构优化角速度式中，为R
N
×1的向量，J
+
为雅可比矩阵广义逆，为R
N
×3的矩阵，的矩阵，为多关节机构末端在笛卡尔空间中的X、Y、Z的运动速度；k为放大系数，I为单位阵，H(Θ)为优化函数，
▽
H(Θ)为梯度，x为多关节机构末端在X轴上的移动量，x的范围为2L；x1为时间点t1对应的多关节机构末端在笛卡尔空间中X轴的坐标。5.根据权利要求4所述的多关节机构轨迹规划方法，其特征在于，优化函数式中，Θ
i
(t1+T
L
)为基于广义逆矩阵法规划得到的关节i的关节角度；θ
i
(t1+T
L
)为关节空间轨迹规划法得到的关节i在t1+T
L
时刻的关节角度；θ
imax
为关节i的角度范围最大值；θ
imin
为关节i的角度范围最小值；函数abs()返回变量的绝对值；函数max()返回两个变量的最大值。6.根据权利要求1所述的多关节机构轨迹规划方法，其特征在于，所述预设的运动提前
量为多关节机构末端移动时间提前量T。7.根据权利要求6所述的多关节机构轨迹规划方法，其特征在于，对所述多关节机构各关节的轨迹中的关节姿态进行优化，包括：当多关节机构末端运动到时间点t1时，规划的第二关节轨迹规划数据已经达到时间点t1+T，得到多关节机构的关节角Θ
i
(t1+T)；T为规划的多关节机构末端移动时间提前量；i＝1,
…
,N；N为多关节机构关节的数量；实时检测规划的关节角Θ
i
(t1+T)是否超出了各关节的极限值；是，则结合第一关节轨迹规划数据，基于梯度投影法优化多关节机构的关节角速度对多关节机构角速度进行积分，确定出优化后的...

【专利技术属性】
技术研发人员：皮皓杰，付玲，杨召，刘延斌，于晓颖，
申请(专利权)人：中联重科股份有限公司，
类型：发明
国别省市：