本发明属于但不限于机器人动力学参数辨识领域，尤其涉及一种几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法及系统。
背景技术：
1、激励轨迹规划是动力学参数辨识流程中的一部分，其主要作用是激发参数的动态特性以提高动力学参数的辨识精度。为充分激发参数的动态特性，机械臂往往需要在大范围空间内进行激励运动。当机械臂工作空间内存在障碍物时，运行激励轨迹容易使机械臂和障碍物发生干涉碰撞，同时激励轨迹本身也需要满足一定的物理约束，如首尾平滑约束、位置速度加速度极限约束。如何在规划激励轨迹时同时满足物理约束与障碍物的几何约束对于动力学参数辨识具有重要意义。
2、以往的激励轨迹规划中，通常不直接考虑障碍物的限制作用，而是在无障碍物情况下仅考虑物理极限约束和物理边界约束完成激励轨迹优化，再考虑障碍物通过仿真软件进行轨迹仿真筛选无干涉轨迹，且仿真时通常只考虑地面作为障碍物。对于工作空间内存在多障碍物、大障碍物影响的机械臂，以上方法往往无法直接得到无干涉的激励轨迹。因此，针对工作空间内的障碍物进行建模，考虑激励轨迹下机械臂与障碍物的干涉情况对轨迹施加几何约束，得到几何物理混合约束的激励轨迹，对于实际工作环境中存在障碍物的机械臂的动力学参数辨识具有重要意义。
3、现有技术在激励轨迹规划方面存在的技术问题主要包括以下几点：
4、1)缺乏直接考虑障碍物约束的方法：在以往的激励轨迹规划中，通常是在无障碍物的情况下进行规划，然后再通过仿真软件来筛选无干涉的轨迹。这种方法没有直接在规划过程中考虑障碍物的存在，导致效率较低，且无法得到完全无干涉的轨迹。
5、2)对多障碍物、大障碍物处理不足：当工作空间内存在多个障碍物或大型障碍物时，现有的规划方法往往难以直接得到无干涉的激励轨迹。这增加了在实际应用中的难度和风险。
6、3)缺乏几何物理混合约束的处理方法：现有的轨迹规划方法通常只考虑物理极限约束和物理边界约束，如首尾平滑约束、位置速度加速度极限等，而未将障碍物的几何约束纳入规划过程中。这导致在实际操作中需要频繁调整轨迹以满足所有约束条件，降低了工作效率。
7、产业应用上带来的技术问题。
8、1.自动化制造业中的机械臂应用
9、问题描述：在自动化制造业中，机械臂需要在复杂的工作环境中执行多种任务，如焊接、装配和搬运。现有的轨迹规划方法难以在规划过程中直接考虑障碍物，导致规划效率低，无法保证机械臂在实际操作中的安全性。
10、影响：
11、生产效率降低：需要反复调整轨迹以避免碰撞，增加了生产周期。
12、安全风险增加：未能充分考虑障碍物，容易导致机械臂与设备、工件碰撞，造成设备损坏和生产事故。
13、维护成本上升：频繁的轨迹调整和碰撞风险增加了设备维护和更换的成本。
14、2.医疗领域中的手术机器人应用
15、问题描述：在微创手术中，手术机器人需要在患者体内精确操作，避开重要器官和血管。现有的轨迹规划方法无法在规划过程中直接考虑体内复杂的几何约束，导致手术机器人操作效率低下。
16、影响：
17、手术时间延长：需要频繁调整机器人的轨迹以避开障碍物，延长了手术时间，增加了患者的风险。
18、手术精度降低：轨迹调整过程中影响手术精度，增加了手术失败的风险。
19、操作难度增加：医生需要不断监控并调整机器人的操作轨迹，增加了操作的复杂性和难度。
技术实现思路
1、针对存在障碍物的机械臂运行激励轨迹易干涉的问题，本发明提供了一种将障碍物考虑进激励轨迹规划中的方法，通过简化的障碍物与不同姿态机械臂的几何形状计算激励轨迹的干涉度量，考虑干涉度量与观测矩阵条件数结合物理约束进行优化得到几何物理混合约束的激励轨迹。
2、本发明是这样实现的，一种几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法，该方法包括：
3、(1)根据机械臂结构和选取机械臂上的特征点pm和特征半径rm，在机械臂dh坐标系中表示为npm，结合dh坐标系变换可以得到特征点在世界坐标系下的坐标0pm：
4、
5、其中：t表示时间，npm表示机械臂的第m个特征点在的坐标系{n}下的描述，表示坐标系{0}到坐标系{n}的齐次变换矩阵，0pm表示机械臂的第m个特征点在的坐标系{0}下的描述；
6、特征点固连在关节n上，即npm为固定值，通过机械臂结构选取；
7、(2)将机械臂工作空间内的障碍物向外延伸简化为长方体，延伸后该长方体边与坐标系{0}坐标轴平行，简化后的第j个长方体信息记录为oj，在机械臂的世界坐标系中进行描述，则有：
8、
9、其中：表示障碍物oj在坐标系{0}的x轴投影的最大值和最小值，表示障碍物oj在坐标系{0}的y轴投影的最大值和最小值，表示障碍物oj在坐标系{0}的z轴投影的最大值和最小值；
10、(3)根据机械臂姿态计算特征点处的特征圆和简化后障碍物的距离。特征点坐标记为：
11、
12、其中：0xm，0ym，0zm为时间t下机械臂特征点pm在{0}坐标系下的坐标。
13、特征点pm距离简化后障碍物oj的距离lmj通过如下方式计算，首先计算点与障碍物投影的距离：
14、
15、其中：lxmj表示特征点pm与障碍物oj在{0}坐标系x轴上投影的距离，lymj表示其在y轴上投影的距离，lzmj表示其在z轴上投影的距离；
16、然后分计算点到简化后障碍物的距离：
17、
18、其中：lmj表示特征点pm与障碍物oj之间的距离，若特征点在障碍物内部则lmj为负值，表示发生干涉，在外部则根据投影距离进行计算，简化了距离的计算过程；
19、于是特征圆和障碍物的距离表示为：lmj-rm；
20、(4)选取安全距离设计干涉函数g(x)，g(x)应满足单调不增，且在距离大于安全距离时为0，作为进一步优选的，具体g(x)选取如下：
21、
22、其中：ds为安全距离，x为机械臂与障碍物之间距离，g(x)在x小于零时随x减小快速增加，在x处于安全距离与0之间时较为缓慢增加，在x大于ds时为零，g(x)越大表示干涉情况越严重，后续g(x)作为目标函数进行优化时，有利于避免干涉的情况发生；
23、(5)将激励轨迹θ(t)按照等时间间隔δt离散得到不同姿态θi，激励轨迹采用有限傅里叶级数：
24、
25、其中：θk为关节k的位置，t是时间，ωf为傅里叶级数的基础频率，h为有限傅里叶级数的阶数，和分别称为关节k的第i阶正弦和余弦项傅里叶系数，θk,0称为位置偏置项；
26、将上述激励轨迹按照如下方式离散：
27、
28、θi＝[θ1(iδt),θ2(iδt),θ3(iδt),θ4(iδt),θ5(iδt),θ6(iδt)]，
29、其中：θi表示离散后第i个位姿的六个关节角，t为有限傅里叶级数周期，δt为选取的离散时间间隔；
30、(6)计算不同姿态下姿态变换矩阵
31、
32、其中，的计算方式为：
33、
34、其中：dn，an，αn为dh参数，关节距离dn定义为从xn-1轴到xn轴的距离，沿zn-1轴的指向为正；连杆长度an定义为从zn-1轴到zn轴的距离，沿xn轴的指向为正；连杆扭角αn定义为从zn-1轴到zn轴的转角，绕xn轴的正向转动为正，且规定αn∈(-π,π]；
35、(7)由(3)计算姿态i(时间iδt时)下所有特征点p与障碍物o之间的干涉距离，得到姿态θi下机械臂与障碍物的干涉距离l的简化计算方法：
36、
37、其中lmj表示时间iδt时，第m个特征点与第j个障碍物之间的距离，rm表示第m个特征点处特征圆半径，min{·}为求最小值函数，li表示机械臂上所有特征圆和简化后障碍物距离的最小值，可以代表该姿态下机械臂与障碍物的干涉情况；
38、(8)将(7)中计算后的li带入设计的干涉函数g(x)中计算所有姿态干涉函数值g(li)，以其最大值或平均值计算该轨迹的干涉函数，优选的，一种轨迹的干涉函数计算方法为：
39、
40、其中：g(γ)为轨迹的干涉函数，γ∈r(2nh+n)×1为待确定的激励轨迹系数，g(l(t))为时间t下机械臂与障碍物的干涉函数，max{·}为最大值函数。
41、(9)以轨迹干涉函数为目标函数，在物理边界约束与物理极限约束下优化求解无干涉轨迹：
42、
43、其中：待确定的激励轨迹系数记为γ∈r(2nh+n)×1，n表示关节总数，θk,max和θk,min分别表示关节k容许的最大和最小运动范围，和分别为速度和加速度关于时间的函数，和分别表示关节k的速度和加速度极限；
44、(10)多次重复步骤(9)计算得到多个无干涉轨迹组成无干涉轨迹集；
45、(11)以激励轨迹条件数和轨迹干涉函数之和作为目标函数，以无干涉轨迹集为初始数据集，在物理边界约束与物理极限约束下采用启发式优化算法优化计算无干涉激励轨迹：
46、
47、其中：hb表示关节动力学模型中动力学参数对应的观测矩阵，cond(·)函数表示条件数计算函数；
48、发生干涉时g(l)的数量级相比cond(hb)略大，因此优化出的轨迹优先为无干涉轨迹；无干涉时g(l)较小或为0，优化轨迹可以充分发挥激励轨迹的激发特性。
49、进一步，所述步骤(1)中，特征点选取在机械臂的关节中心位置与机械臂连杆发生转折的中心位置，对应的特征半径略大于特征点处机械臂横截面外接圆半径。
50、进一步，所述步骤(2)中，长方体为障碍物的外接长方体，其边长平行于世界坐标系{0}。
51、进一步，所述步骤(5)中，时间间隔δt＝0.01t。
52、本发明另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行所述几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法的步骤。
53、本发明另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行所述几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法的步骤。
54、本发明另一目的在于提供一种用于几何与物理混合约束下的机器人动力学辨识激励轨迹规划系统，包括以下模块：
55、特征点选择模块：用于根据机械臂结构选取机械臂上的特征点和特征半径，并通过dh坐标系变换得到特征点在世界坐标系下的坐标；
56、障碍物简化模块：用于将机械臂工作空间内的障碍物向外延伸简化为长方体，并在世界坐标系中描述简化后的障碍物信息；
57、距离计算模块：用于根据机械臂姿态计算特征点处的特征圆和简化后障碍物的距离；
58、干涉函数设计模块：用于选取安全距离并设计干涉函数，确保干涉函数满足单调不增且在距离大于安全距离时为0。
59、进一步，还包括：
60、激励轨迹离散模块：用于将激励轨迹按照等时间间隔离散得到不同姿态，激励轨迹采用有限傅里叶级数表示；
61、姿态变换计算模块：用于计算不同姿态下的姿态变换矩阵。
62、进一步，还包括：
63、干涉距离计算模块：用于计算所有特征点与障碍物之间的干涉距离，得到机械臂与障碍物的干涉距离；
64、干涉函数计算模块：用于将计算后的干涉距离带入干涉函数中计算所有姿态的干涉函数值，以最大值或平均值计算该轨迹的干涉函数。
65、进一步，还包括：
66、轨迹优化模块：用于以轨迹干涉函数为目标函数，在物理边界约束和物理极限约束下优化求解无干涉轨迹，并多次重复计算得到多个无干涉轨迹组成无干涉轨迹集；
67、启发式优化模块：用于以激励轨迹条件数和轨迹干涉函数之和作为目标函数，以无干涉轨迹集为初始数据集，在物理边界约束和物理极限约束下采用启发式优化算法优化计算无干涉激励轨迹。
68、结合上述的技术方案和解决的技术问题，本发明所要保护的技术方案所具备的优点及积极效果为：
69、第一、本发明将机械臂简化为多个球体，工作空间内障碍物简化为长方体，能够在反应实际情况下机械臂和障碍物特征的同时简化对于干涉的计算量。
70、本发明采用以下步骤对于球体与长方体干涉距离进行计算：首先通过球心特征点和长方体投影的距离实现点到长方体距离的计算，进一步计算球和长方体的距离，避免了对于球和长方体在三维空间位置的大量分类计算。
71、由于对于观测矩阵的条件数优化计算耗时长，因此本发明首先在不考虑观测矩阵的条件数情况下优化求解多条无干涉轨迹作为初始数据集，再考虑条件数和干涉函数进行二次优化求解，相比于直接优化求解无干涉低条件数的激励轨迹，缩短了优化求解的时间。
72、本发明将几何约束通过将轨迹干涉度量值作为目标函数一部分，结合物理约束实现了几何物理混合约束下的激励轨迹规划。
73、第二，本发明的技术方案填补了国内外业内技术空白：
74、以往的激励轨迹规划为直接考虑障碍物的几何约束，对于复杂工况下的激励轨迹规划缺乏有效手段，本发明提供的几何物理混合约束下的机械臂激励轨迹规划方法填补了机械臂激励轨迹规划时无法直接考虑障碍物约束的空白。
75、本发明的技术方案解决了人们一直渴望解决、但始终未能获得成功的技术难题：
76、对于复杂的机械臂及其工作环境，如何对其运动过程中的干涉情况进行简化描述一直是人们渴望解决、但始终未能获得成功的技术难题，本发明通过简化机械臂和障碍物的几何特征，进行时间离散下点到面的碰撞计算，实现了对实际工况的简化建模及干涉情况的数学度量计算。
77、第三，现有技术的技术问题和显著技术进步。
78、现有技术的技术问题：
79、轨迹规划复杂度高：现有技术中，机械臂轨迹规划在复杂环境中计算量大，难以实时计算出无干涉的安全轨迹。
80、安全性不足：传统方法在复杂环境中容易出现机械臂与障碍物碰撞的情况，存在安全隐患。
81、优化效率低：现有方法在轨迹优化过程中效率较低，难以在物理约束和几何约束下找到最优解。
82、显著技术进步：
83、简化距离计算：通过将障碍物简化为长方体，极大地减少了距离计算的复杂度，提高了轨迹规划的效率。
84、高效干涉评估：设计单调不增的干涉函数，能够快速评估特征点与障碍物之间的干涉情况，量化干涉程度。
85、优化求解效率高：通过启发式优化算法，在物理边界和物理极限约束下高效求解无干涉轨迹，显著提高了轨迹优化的效率。
86、综合优化目标：在最终优化过程中，综合考虑激励轨迹条件数和轨迹干涉函数，确保了轨迹的安全性和激励效果。
87、安全性增强：多次重复计算无干涉轨迹，构建无干涉轨迹集，从中选择最优轨迹，确保机械臂在复杂环境中的安全运行。
88、本发明通过系统化的参数、算法和数学模型，解决了现有技术中的轨迹规划复杂度高、安全性不足和优化效率低等问题。采用几何与物理混合约束的方法，实现了机械臂在复杂环境中的高效、安全和精确的轨迹规划，达到了显著的技术进步。
1.一种几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法，其特征在于，该方法包括：
2.根据权利要求1所述的几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法，其特征在于：
3.根据权利要求1所述的几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法，其特征在于：
4.根据权利要求1所述的几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法，其特征在于：
5.根据权利要求1所述的几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法，其特征在于：
6.根据权利要求1所述的几何与物理混合约束下的机器人动力学辨识激励轨迹规划方法，其特征在于：
7.一种用于几何与物理混合约束下的机器人动力学辨识激励轨迹规划系统，其特征在于，包括以下模块：
8.根据权利要求7所述的系统，其特征在于，还包括：
9.根据权利要求8所述的系统，其特征在于，还包括：
10.根据权利要求8所述的系统，其特征在于，还包括：
