一种物流机械臂的动力学建模方法与流程

本发明涉及动力学建模，尤其涉及一种物流机械臂的动力学建模方法。

背景技术：

1、物流机械臂是自动化仓储系统的核心执行单元，其动力学模型的精度直接决定分拣效率、定位精度与运行稳定性。高保真的动力学模型是实现高性能轨迹跟踪控制与力控柔顺作业的前提。然而，物流分拣场景具有以下典型特征，给传统动力学建模方法带来了严峻挑战：负载在秒级内从数十克到数十公斤的阶跃式突变，导致固定参数的刚体模型瞬间失配；高速启停激发的连杆柔性、关节非线性摩擦等未建模动态与负载变化强耦合，刚体模型无法描述；受成本限制，仅有的电机电流与位置传感器缺乏关节力矩传感，电流到力矩的换算存在非线性误差；此外，分拣动作由“抬升-搬运-投放”构成强周期性序列，各阶段的主导动力学效应差异显著，但这一特化结构尚未被现有建模方法有效利用。

2、现有技术的动力学建模方法各有不足：基于牛顿-欧拉法或拉格朗日法的机理建模，依赖精确物理参数与复杂的辨识实验，且难以对摩擦、柔性等机理不明确或过于复杂的动态特性精确建模；纯数据驱动方法虽非线性拟合能力强，但作为黑箱模型可能违反能量守恒等物理定律，在数据稀疏区域泛化能力差；物理信息神经网络方法通过在损失函数中融入动力学方程残差与能量约束，为解决上述问题提供了思路，但现有研究多聚焦通用工业机械臂，尚未有效利用物流分拣场景的负载突变、高速启停与强周期特性。

3、针对上述问题，本发明提出一种物流机械臂的动力学建模方法。

技术实现思路

1、本发明的目的是提供一种物流机械臂的动力学建模方法，以解决现有技术中物流机械臂动力学模型在负载频繁突变工况下精度不足、泛化能力差的问题。

2、为解决上述技术问题，本发明提供了如下技术方案：

3、一种物流机械臂的动力学建模方法，包括：

4、步骤s1：采集物流机械臂在典型分拣工况下常规分拣数据，重点采集负载切换瞬间前后时间窗口内的负载切换暂态数据，所述常规分拣数据和负载切换暂态数据共同构成物流特化数据集；

5、步骤s2：构建以拉格朗日动力学为物理先验的双层物理信息神经网络；其中第一层物理信息神经网络以关节位置和负载类别编码的拼接向量为输入，输出惯性矩阵与重力项；对于训练中未出现过的负载重量，通过对两个相邻已知负载类别编码进行线性插值，将插值后的编码向量作为第一层物理信息神经网络的输入，由第一层物理信息神经网络输出对应的惯性矩阵与重力项；第二层物理信息神经网络以关节位置和速度为输入，输出摩擦力项；

6、步骤s3：将惯性项、科里奥利力项、重力项与摩擦力项共同代入拉格朗日动力学方程，构建机械臂的完整动力学预测模型；所述动力学预测模型以关节位置、速度、加速度和负载类别编码为输入，以预测力矩为输出，其中惯性项由惯性矩阵与加速度相乘构成；所述科里奥利力项由惯性矩阵对位置的偏导数导出；

7、步骤s4：设计融合物流场景约束的联合损失函数，包括数据拟合损失、经阶段感知加权后的物理残差损失、负载突变正则项和能量一致性约束；所述阶段感知加权用于根据分拣动作所处阶段，对整条拉格朗日动力学方程残差向量施加阶段依赖的权重向量，以差异化调节各阶段、各关节残差分量在总损失中的贡献比例；所述权重向量以各元素均为1.0为基础向量，当所有权重元素取1时退化为标准物理残差损失；所述权重向量作用于残差向量外部，不改变拉格朗日动力学方程本身的结构，仅用于引导网络优先降低当前阶段残差较大的关节分量；

8、步骤s5：基于物流特化数据集和联合损失函数，对双层物理信息神经网络进行联合训练，以所述联合损失函数最小化为优化目标，通过反向传播同步更新第一层物理信息神经网络和第二层物理信息神经网络的参数，获得训练后的动力学模型；其中，所述数据拟合损失用于使预测力矩逼近实测力矩，所述加权物理残差损失用于在加权最小二乘意义上引导模型输出逼近拉格朗日动力学方程，所述负载突变正则项用于约束负载切换时刻预测力矩的变化率，所述能量一致性约束用于保证系统长期能量稳定性；

9、步骤s6：将训练后的动力学模型部署至机械臂控制器，根据实时关节位置、速度、加速度与当前负载类别编码，通过训练后的动力学预测模型计算前馈补偿力矩，所述前馈补偿力矩与所述控制器的反馈控制力矩叠加，控制机械臂的运动。

10、进一步的，所述步骤s1中，典型分拣工况包括包裹抬升动作、水平搬运动作、投放释放动作及其组合序列；所述数据采集覆盖至少三种不同重量等级的包裹负载，包括轻载、中载和重载，分别对应依次递增的包裹重量范围。

11、所述常规分拣数据包括关节位置、速度、加速度与实测力矩；所述负载切换暂态数据包括负载切换瞬间前后时间窗口内的关节位置、速度、加速度与实测力矩高频采样数据；所述物流特化数据集还包括负载类别编码和阶段标签；所述实测力矩通过电机电流经力矩常数换算得到；由于电机力矩常数受温度变化、磁路饱和及低速工况等因素影响，电流换算得到的实测力矩可能与真实力矩之间存在一定的非线性误差。

12、需要说明的是，上述实测力矩通过电机电流经力矩常数换算得到，受温度变化、磁路饱和及低速工况等因素影响，该换算值可能存在一定的非线性误差。本发明在训练中直接使用该换算值作为监督信号，并未引入额外的独立力矩传感器进行标定。为降低上述电流-力矩换算误差对模型训练精度的影响，本发明通过以下机制实现鲁棒学习：

13、（1）所述加权物理残差损失通过方程结构本身对模型施加物理一致性限制，降低模型对标签中潜在误差的过拟合风险；

14、（2）所述能量一致性约束基于能量守恒关系，提供额外的物理正则化约束，引导模型输出满足能量守恒，进一步降低模型对实测力矩标签中潜在误差的敏感性；

15、（3）所述第二层物理信息神经网络的结构化摩擦力模型中设置有可学习的低速补偿项，用于处理零速附近因电流采样噪声引起的力矩波动，避免将其错误归因于惯性参数或重力参数。

16、与现有技术中仅采集稳态或均匀采样不同，本发明重点采集负载切换瞬间前后各0.5至2秒时间窗口内的暂态高密度数据。负载切换瞬间的短暂暂态过程包含了丰富的参数突变动力学信息，是提升模型在变负载工况下泛化能力的关键。该策略使得数据集不仅包含不同负载的静态特性，更蕴含了负载切换的动态过渡特性。

17、进一步的，所述步骤s2中，所述第一层物理信息神经网络为物理参数辨识网络，用于学习惯性矩阵m(q)和重力项g(q)。所述负载类别编码为一独热向量或可学习的嵌入向量，代表当前末端负载的重量等级，与关节位置向量拼接后作为第一层物理信息神经网络的输入。

18、进一步的，所述步骤2中，为严格保证m(q,c)的物理正定性，网络输出一个下三角矩阵l(q,c)的元素值，为防止l(q,c)奇异或对角元素为零导致构造出的m(q,c)仅为半正定而丧失物理正定性，本发明对l(q,c)的对角元素进行正定性变换：设网络原始输出为，其对角元素为，则实际用于构造的下三角矩阵的对角元素取为或（其中为一个预设的极小正值，例如），非对角元素直接取的对应元素值。通过上述正定性变换，下三角矩阵l(q,c)的对角元素严格大于零，保证了l(q,c)满秩。随后按下式构造惯性矩阵：

19、

20、其中，q为关节位置向量，c为所述负载类别编码，为所述第一层物理信息神经网络输出的下三角矩阵，m(q,c)为构造得到的惯性矩阵。由于l(q,c)为对角元素严格大于零的下三角矩阵，其必然满秩，由此构造的m(q,c)满足对称正定性，符合物理惯性矩阵的数学要求。作为替代方案，也可以采用cholesky分解的变体或其他能够保证正定性的矩阵分解方式，本发明对此不作限定。

21、根据拉格朗日力学的标准推导，由所述惯性矩阵m(q,c)对关节位置q的偏导数，通过克里斯托费尔符号自动计算得出科里奥利力，确保了模型各部分之间的物理自洽性。

22、第二层物理信息神经网络（摩擦力拟合网络）：用于拟合包含库仑摩擦和粘滞摩擦的摩擦力项，其模型表示为：

23、

24、其中，q为关节位置向量，由机械臂各关节的角度值组成，维度等于机械臂的自由度数；为关节角速度向量，由机械臂各关节的旋转角速度组成，是关节位置向量对时间的一阶导数；为库仑摩擦系数向量，表征与速度方向有关、与速度大小无关的摩擦分量，在速度过零时方向发生突变；为粘滞摩擦系数向量，表征与速度大小成正比的摩擦分量，速度越大该项越大；为控制逼近陡峭程度的预设正系数，用于调节tanh函数的斜率，α值越大，tanh函数的输出越接近符号函数的阶跃形态；为双曲正切函数，以α倍的关节角速度作为输入，输出值在-1到1之间平滑过渡，用于可导地逼近不可导的符号函数，保证神经网络训练过程中的数值稳定性；为残余非线性摩擦力项，由一个全连接子网络以关节位置q和关节角速度q̇为输入而输出，用于捕捉库仑摩擦和粘滞摩擦之外的非线性摩擦特性。

25、进一步的，所述步骤3中，对于自由度机械臂，所述改进的拉格朗日动力学方程表示为：

26、

27、其中：为关节位置向量，其各元素分别对应各关节的角度值；为关节速度向量，是对时间的一阶导数；为关节加速度向量，是对时间的二阶导数；为负载类别编码，用于标识当前末端负载的重量等级，是本发明的核心输入之一，使模型能够根据负载变化自适应调整动力学参数；为对称正定的惯性矩阵，表征各关节惯量及其耦合关系，随负载类别编码变化，即不同负载下具有不同的惯量特性；为科里奥利力项，表征速度耦合产生的非线性力，随负载类别编码变化；为重力项，表征各关节所受重力矩，随负载类别编码变化，负载越重力矩越大；为摩擦力项，表征关节运动过程中的摩擦阻力；为预测力矩，即模型最终输出的各关节驱动力矩。

28、进一步的，所述步骤4中，设计融合物流场景约束的联合损失函数，包括数据拟合损失、经阶段感知加权后的物理残差损失、负载突变正则项和能量一致性约束。

29、（一）数据拟合损失

30、数据拟合损失用于约束所述动力学预测模型输出的预测力矩与实测力矩之间的偏差，以所述预测力矩与所述实测力矩之间的均方误差表示。在负载切换时刻附近的暂态数据被赋予高于稳态数据的权重系数，具体公式为：

31、

32、其中，为批次样本总数，为样本索引，为第个样本的实测力矩，为第个样本的预测力矩，为第i个样本的权重系数；当第i样本的采样时刻位于负载切换时刻前后的预设时间窗口内时，取大于1的预设值，否则=1。

33、（二）经阶段感知加权后的物理残差损失

34、物理残差损失用于将拉格朗日动力学方程作为物理先验嵌入神经网络训练过程。将第一层物理信息神经网络输出的惯性矩阵和重力项、由所述惯性矩阵导出的科里奥利力项，以及第二层物理信息神经网络输出的摩擦力项，共同代入拉格朗日动力学方程左侧，计算得到方程左侧各项之和与实测力矩之间的偏差。在此基础上，根据样本所处阶段，对完整拉格朗日动力学方程残差向量的各关节分量进行加权，以调节不同阶段样本及不同关节残差对物理残差损失的贡献比例，得到加权物理残差损失：

35、

36、其中，为批次样本总数，为样本索引；为第个样本的阶段标签；为对应于阶段的权重向量，其各元素分别对应于个关节的偏差量加权；分别为第个样本的惯性项、科里奥利力项、重力项和摩擦力项的模型预测值；为第个样本的实测力矩；表示逐元素乘法运算。所述权重向量以各元素均为1.0为基础向量，其各元素分别对应于机械臂各关节的残差分量加权系数。抬升阶段以克服重力为主，重力项对多个关节均有显著影响，故权重向量各元素整体取值大于其他阶段；搬运阶段以高速水平运动为主，主要涉及大臂、小臂等关节的高速运动，故搬运阶段权重向量中对应于大臂、小臂等主要运动关节的元素取值进一步增大；投放阶段以低速精确对准为主，主要涉及腕部等末端关节的精细调节和克服摩擦，故投放阶段权重向量中对应于末端关节的元素取值进一步增大。各阶段权重向量的具体数值根据各关节在该阶段动力学效应的重要程度预设。

37、（三）负载突变正则项

38、负载突变正则项用于在负载切换时刻约束预测力矩的变化率，迫使模型学习平滑且物理合理的过渡动力学。通过监测电机电流变化率或末端力传感器信号自动检测负载切换时刻集合，并在该时间窗口内对模型预测力矩的一阶时间差分施加范数惩罚：

39、

40、其中，为预设的惩罚系数，为负载切换时刻的集合，为所述集合中切换时刻的总数，为所述动力学预测模型在时刻预测的力矩向量，为一个采样周期。

41、（四）能量一致性约束

42、能量一致性约束用于约束系统动能变化率与外力做功功率、耗散功率之和的守恒关系，以保证模型在长时间预测下的能量稳定性。需要说明的是，该约束仅在负载类别保持不变且末端执行器未与包裹发生接触冲击的时段内成立；在负载切换瞬间前后各预设时间窗口内，包裹与机械臂之间的接触力会向系统输入或带走能量，该能量无法由电机力矩、摩擦耗散和重力做功三项完全解释，因此该时段内的样本不参与能量一致性约束的计算。基于此，能量一致性约束定义为：

43、

44、其中，该损失仅对负载类别编码保持恒定且采样时刻不处于任何负载切换事件前后预设时间窗口内的样本计算；为系统的总动能，为系统动能对时间的一阶导数，通过自动微分技术精确计算；为关节速度向量，为其转置；为关节驱动力矩；为摩擦力项；为重力项。

45、（五）总损失函数

46、总损失函数为上述各项的加权和：

47、

48、其中，为数据拟合损失，为加权物理残差损失，为负载突变正则项，为能量一致性约束，、、、分别对应其权重系数。

49、优选地，所述权重系数、、、采用基于任务不确定性的策略在网络训练过程中自适应动态调整，以平衡各损失项的量级和收敛速度。

50、通过上述设计，物流场景的“变负载”特性与“动作特化”特性在数据层、模型层与优化层系统性嵌入且协同增效。

51、通过上述数据拟合损失、加权物理残差损失、负载突变正则项与能量一致性约束的联合优化，即使在实测力矩（电流-力矩换算值）存在一定非线性误差的条件下，本发明通过物理残差和能量守恒提供的物理一致性引导，降低模型对电流-力矩换算误差的敏感度，在仅依赖电机电流信号的条件下实现鲁棒的动力学参数学习，实现对机械臂动力学参数的鲁棒辨识。

52、所述步骤s5中，使用步骤s1构建的所述物流特化数据集，以步骤s4设计的所述联合损失函数为优化目标，对步骤s2构建的所述双层物理信息神经网络进行端到端的联合训练。训练过程中，将所述物流特化数据集中的样本按批次随机采样，依次输入所述第一层物理信息神经网络和所述第二层物理信息神经网络进行前向传播：所述第一层物理信息神经网络以关节位置和负载类别编码的拼接向量为输入，输出惯性矩阵与重力项，并由所述惯性矩阵自动导出科里奥利力项；所述第二层物理信息神经网络以关节位置和速度为输入，输出摩擦力项。将上述三项代入拉格朗日动力学方程左侧，计算得到预测力矩，进而计算所述联合损失函数值。采用自适应矩估计优化器，结合余弦退火学习率调度策略，以最小化所述联合损失函数为目标，通过反向传播算法同步更新所述第一层物理信息神经网络和所述第二层物理信息神经网络的全部可训练参数，迭代执行上述过程直至所述联合损失函数收敛，获得训练后的动力学模型。优选地，所述权重系数采用基于任务不确定性的策略在网络训练过程中自适应动态调整，以平衡各损失项的量级和收敛速度。

53、所述步骤6中，将训练收敛的所述动力学模型导出并部署至物流机械臂的实时控制器中。在线运行时，所述控制器通过编码器读取实时关节位置、速度、加速度，并根据视觉系统或人工输入的包裹重量信息生成对应的负载类别编码，一同输入训练后的所述动力学模型进行前向推理，所述动力学模型根据当前负载类别编码从参数空间中调用或插值出对应的惯性矩阵与重力项，并结合当前关节位置和速度计算摩擦力项，代入拉格朗日动力学方程左侧，输出预测力矩作为前馈补偿力矩。所述前馈补偿力矩与所述控制器根据期望轨迹与实际轨迹之间的偏差实时计算的反馈控制力矩相叠加，生成最终执行力矩并下发至机械臂驱动器，以实现机械臂的运动控制。

54、与现有技术相比，本发明的有益技术效果为：

55、本发明在物流场景建模精度取得突破性提升，通过实验发现，仅在数据采集时覆盖变负载工况或仅在模型中添加物理约束均无法有效提升负载切换暂态的精度，本发明的核心贡献在于发现了负载切换暂态数据采集策略与负载突变正则项之间、分拣动作阶段划分与阶段依赖的残差加权之间的协同增效作用，通过将它们系统性耦合，模型能够“理解”负载突变过渡动力学和各阶段主导物理效应。

56、本发明在仅依赖电机电流与编码器信号的条件下实现了高精度力矩预测，显著降低了物流机械臂的力矩预测误差，为无需额外扭矩传感器的低成本方案提供了可行的技术路径。

57、本发明实现物理一致性与数据驱动能力深度融合，泛化能力极强，结构化网络设计保证了惯性矩阵的物理正定性，能量一致性约束确保了长期预测的稳定性，这使得模型既具备对复杂非线性摩擦的强大拟合能力，又在未充分训练的负载和工作空间数据稀疏区保持物理合理性，对未见过的中间负载重量表现出优异的平滑泛化能力。

58、本发明具备单一模型的工况自适应能力，通过在模型中引入负载类别编码，并利用负载突变正则项学习平滑的过渡动力学，单一模型即可连续、平滑地适配从轻载到重载的整个包裹重量范围，避免了为不同负载单独建模、切换模型带来的控制不连续与繁复工作。