CN115741718B - 一种机器人完全零力控制方法及系统 - Google Patents
一种机器人完全零力控制方法及系统 Download PDFInfo
- Publication number
- CN115741718B CN115741718B CN202211566523.3A CN202211566523A CN115741718B CN 115741718 B CN115741718 B CN 115741718B CN 202211566523 A CN202211566523 A CN 202211566523A CN 115741718 B CN115741718 B CN 115741718B
- Authority
- CN
- China
- Prior art keywords
- model
- dynamic
- robot
- joint
- moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 105
- 230000005484 gravity Effects 0.000 claims abstract description 74
- 230000005284 excitation Effects 0.000 claims abstract description 72
- 230000003068 static effect Effects 0.000 claims abstract description 62
- 230000033001 locomotion Effects 0.000 claims abstract description 42
- 238000005457 optimization Methods 0.000 claims description 40
- 239000011159 matrix material Substances 0.000 claims description 28
- 238000001914 filtration Methods 0.000 claims description 26
- 230000008569 process Effects 0.000 claims description 24
- 230000001133 acceleration Effects 0.000 claims description 18
- 238000012545 processing Methods 0.000 claims description 18
- 238000000354 decomposition reaction Methods 0.000 claims description 10
- 239000013598 vector Substances 0.000 claims description 10
- 239000006185 dispersion Substances 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 7
- 238000012886 linear function Methods 0.000 claims description 6
- 238000012887 quadratic function Methods 0.000 claims description 5
- 230000003044 adaptive effect Effects 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000012544 monitoring process Methods 0.000 claims description 4
- 238000000926 separation method Methods 0.000 claims description 4
- 230000008859 change Effects 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 76
- 238000004891 communication Methods 0.000 description 8
- 238000004590 computer program Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 4
- 238000003860 storage Methods 0.000 description 4
- 239000003638 chemical reducing agent Substances 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 230000003139 buffering effect Effects 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 201000010099 disease Diseases 0.000 description 1
- 208000037265 diseases, disorders, signs and symptoms Diseases 0.000 description 1
- 238000000802 evaporation-induced self-assembly Methods 0.000 description 1
- 239000004519 grease Substances 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000001050 lubricating effect Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Landscapes
- Feedback Control In General (AREA)
Abstract
本发明提供了一种机器人完全零力控制方法及系统，所述方法包括：得到所述机器人的动力学模型与动力学参数；得到所述机器人的最优激励轨迹，并采集运动数据；基于所述动力学模型构建初始动态动力学模型；采集所述机器人的重力参数及动摩擦力参数，以得到最终动态动力学模型；采集所述机器人的静摩擦力参数，得到最终静态动力学模型；将所述最终动态动力学模型与所述最终静态动力学模型进行结合，以得到完全零力控制模型。本发明能够不在各个关节添加额外力矩传感器的同时，克服重力及摩擦力的影响，将动力学模型细分为动态动力学模型和静态动力学模型，实现机器人在任何时间、任何姿态下的完全零力控制，且能很好适应环境变化。
Description
技术领域
本发明属于机器人控制的技术领域，具体地涉及一种机器人完全零力控制方法及系统。
背景技术
目前主流的机器人的机械臂的拖拽控制主要分为两种：一种是基于关节力传感器补偿的拖拽控制；另一种是基于机械臂动力学模型或软PID的控制方式，前者传感器无法克服重力影响，会导致机械臂在拖拽时无法抵消重力而下坠，后者无法克服摩擦力影响，在起始和环境变化时需要较大力才能推动，并且在力臂短时更加明显。
例如，公布号为CN 115042177A的机器人末端控制方法、装置、设备及存储介质的专利，其技术方案为获得机器人的末端受力向量，进行映射转换后转为位置向量，以此对机器人进行末端工作空间拖拽，但其不能分辨向前关节的力，且无法克服静摩擦力及重力项；
公布号为CN11 1300433A的基于六维力传感器的机器人拖拽示教装置的专利，其技术方案为根据末端六维力传感器受力信息感知外部用力实现拖拽示教，但只能感受到末端受力，当人手施加在向前关节时无法判断，并且成本较高。
发明内容
为了解决上述技术问题，本发明提供了一种机器人完全零力控制方法及系统，用于解决现有技术中存在的技术问题。
第一方面，该发明提供以下技术方案，一种机器人完全零力控制方法，所述方法包括：
对机器人进行动力学建模及动力学参数辨识，以得到所述机器人的动力学模型与动力学参数；
根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹，并通过二次规划算法得到所述机器人的最优激励轨迹，控制所述机器人按照所述最优激励轨迹运动并采集运动数据；
对所述运动数据进行滤波处理，使用QR分解法进行参数辨识，并基于所述动力学模型构建初始动态动力学模型；
采集所述机器人的重力参数、动摩擦力参数，并对所述重力参数与所述动摩擦力参数采用最小二乘法求解得到重力矩模型与动摩擦力模型，根据所述重力矩模型与所述动摩擦力模型，以得到最终动态动力学模型；
采集所述机器人的静摩擦力参数，并根据所述静摩擦力参数得到最终静态动力学模型；
将所述最终动态动力学模型与所述最终静态动力学模型进行结合，以得到完全零力控制模型，并根据所述完全零力控制模型，完成所述机器人的完全零力控制。
相比现有技术，本申请的有益效果为：本申请不用各个关节加装力矩传感器就能实现克服重力的稳定拖拽效果，大大节约成本，能够很好的克服动静摩擦力达到轻松拖动机械臂的效果，提高工作效率和安全性，同时模型采用自适应滤波，只需数据递推而无需缓存，运算量小，响应速度快，且适用于各种产线、工厂环境，鲁棒性强，同时机器人的机械臂在任何姿态都可静止，保证一人就能完成拖拽示教，交互性更强更友好。
较佳的，所述对机器人进行动力学建模及动力学参数辨识，以得到所述机器人的动力学模型与动力学参数的步骤包括：
对机器人进行动力学建模，以得到所述机器人的动力学模型：
τ＝τf+τd；
式中，τ为关节驱动力矩，τf为关节摩擦力矩，τd为关节动态力矩，q、 分别为关节角度、关节角速度、关节角加速度，D(q)为惯性项，为科式力及离心力项，G(q)为重力项；
将关节动态力矩τd转化为线性分离形式：
式中，P为惯性参数；
在所述动力学模型中引入Stribeck模型细化关节摩擦力矩τf：
式中，为直线运动速度，Ff为完整摩擦力，Fs为最大静摩擦力，Fc为库伦摩擦力，Bv为粘滞摩擦系数，Vs为Stribeck速度；
对所述机器人进行动力学参数辨识，以得到所述机器人的动力学参数：
pd＝[Ixx，Ixy，Iyy，Ixz，Iyz，Izz，Hx，Hy，Hz，m，Ff]；
式中，Ixx，Ixy，Iyy，Ixz，Iyz，Ixz为机器人惯量矩阵I的六个参数，Hx，Hy，Hz为质心向量，m为质量。
较佳的，所述根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹，并通过二次规划算法得到所述机器人的最优激励轨迹，控制所述机器人按照所述最优激励轨迹运动并采集运动数据的步骤包括：
根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹：
式中，qi为关节转角，qi0为关节转角常量，i表示关节的第i轴，al、bl为激励轨迹的常系数，ωf为激励轨迹的基频，ωfl表示第l阶的频率，N为阶数，t为采样时间；
通过二次规划算法并引入约束将得到最优激励轨迹的过程转化为非线性优化问题，所述非线性优化问题的目标函数为：
min f(X)；
s.t.gu(X)≤0(u＝1，2，...，p)；
hv(X)＝0(v＝1，2，...，m)；
式中，gu(X)为所述机器人的机械臂的实际角度、实际角速度、实际角加速度最大值的约束，hv(X)为在起始时间的初始值约束，f(X)为优化目标；
利用泰勒展开将所述非线性优化问题的目标函数在迭代点Xk处简化为二次函数，并将约束条件简化为线性函数，以得到二次规划问题的目标函数：
s.t.
式中，为微分算子，p、m分别为关节轴的数量，T为转置矩阵。
较佳的，在所述利用泰勒展开将非线性优化问题的目标函数在迭代点Xk处简化为二次函数，并将约束条件简化为线性函数，以得到二次规划问题的目标函数的步骤之后，所述方法还包括：
令S＝X-Xk，将所述二次规划问题的目标函数转换为变量S的目标函数：
s.t.
式中，S为当前点与迭代点之间的差值，为微分算子；
根据所述变量S的目标函数，令：
式中，Hk、C为变量S的目标函数的二次型矩形和向量，Aeq、Beq为线性等式约束，A、B为线性不等式约束；
将二次规划问题的目标函数转换为一般形式目标函数：
s.t.AS＝-B；
AeqS＝-Beq；
根据所述一般形式目标函数求解所述非线性优化问题的目标函数，以得到最优激励轨迹；
控制机器人按照所述最优激励轨迹运动并采集相应的初始数据，并对所述初始数据分别进行一次差分与二次差分，以得到运动数据。
较佳的，所述根据所述一般形式目标函数求解所述非线性优化问题的目标函数，以得到最优激励轨迹的步骤包括：
给定初始点X0、收敛精度ε，并另H0＝I，k＝0；
将非线性优化问题的目标函数在迭代点Xk处简化为二次规划问题的目标函数；
求解所述二次规划问题的目标函数，并令Sk＝S*；
在方向Sk上对所述二次规划问题的目标函数进行约束一维搜索，得到下一迭代点Xk+1；
判断Xk+1是否满足终止准则，若Xk+1满足终止准则，则取其中条件数最小的激励轨迹作为最优激励轨迹，将Xk+1作为最优解，作f(Xk+1)为目标函数的最优代价，若Xk+1不满足终止准则，则令k＝k+1，将Hk修正为Hk+1，并返回执行所述将非线性优化问题的目标函数在迭代点Xk处简化为二次规划问题的目标函数的步骤。
较佳的，所述对所述运动数据进行滤波处理，使用QR分解法进行参数辨识，并基于所述动力学模型构建初始动态动力学模型的步骤包括：
采用自适应滤波算法对所述运动数据进行滤波处理；
对于所述动力学模型，存在其中，Yr为对应的所述动力学模型的离差目标函数为：
式中，φ(P)为实测值与计算值的离差，且为正交矩阵，是上三角矩阵，m′为关节轴的数量，n′为惯性参数的数量，表示实数集合；
令c1∈Rn′，c2∈Rm′-n′，则：
令即根据 求解出惯性参数P，将惯性参数P代入所述动力学模型中，以得到初始动态动力学模型。
较佳的，所述采集所述机器人的重力参数、动摩擦力参数，并对所述重力参数与所述动摩擦力参数采用最小二乘法求解得到重力矩模型与动摩擦力模型，根据所述重力矩模型与所述动摩擦力模型，以得到最终动态动力学模型的步骤包括：
令所述机器人的关节由第一速度θ1匀速运动到第二速度θ2，再令所述机器人的关节由第二速度θ2匀速运动到第一速度θ1，以得到关节输入力矩与关节输出力矩：
式中，τm1为关节输入力矩，τm2为关节输出力矩，τg为重力矩，τf为摩擦力矩，θ为实际关节角度，为实际关节角速度；
根据所述关节输入力矩与所述关节输出力矩，得到重力矩与动摩擦力矩：
将所述动摩擦力矩与重力矩τg(θ)代入所述动力学模型中，并通过最小二乘法求解出模型参数值，以得到动摩擦力模型与重力矩模型；
将所述动摩擦力模型与所述重力矩模型代入所述初始动态动力学模型中进行部分替换，以得到最终动态动力学模型：
式中，τe为所述机器人的关节在运动时及静止克服重力时的最终动态力矩，为减除重力矩及动摩擦力矩后只与关节角速度角加速度有关的观测矩阵。
较佳的，所述采集所述机器人的静摩擦力参数，并根据所述静摩擦力参数得到最终静态动力学模型的步骤包括：
控制所述机器人进入力控模式，采集参数信息，将所述参数信息代入所述最终动态动力学模型中，求解出τe；
依次对所述机器人的各关节施加额外力矩τadd，并监测关节角速度
当大于第一预设值时，记录对应的额外力矩τadd并将其置零；
当所述机器人关节的角度大于第二预设值时，将对应的额外力矩τadd进行线性拟合，其拟合后直线的数值作为最优值，即为最终静态动力学模型F(static)。
较佳的，在所述将所述最终动态动力学模型与所述最终静态动力学模型进行结合，以得到完全零力控制模型，并根据所述完全零力控制模型，完成所述机器人的完全零力控制的步骤中，所述完全零力控制模型为：
第二方面，该发明提供以下技术方案，一种机器人完全零力控制系统，所述系统包括：
建模与辨识模块，用于对机器人进行动力学建模及动力学参数辨识，以得到所述机器人的动力学模型与动力学参数；
轨迹生成模块，用于根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹，并通过二次规划算法得到所述机器人的最优激励轨迹，控制所述机器人按照所述最优激励轨迹运动并采集运动数据；
滤波处理模块，用于对所述运动数据进行滤波处理，使用QR分解法进行参数辨识，并基于所述动力学模型构建初始动态动力学模型；
第一采集模块，用于采集所述机器人的重力参数、动摩擦力参数，并对所述重力参数与所述动摩擦力参数采用最小二乘法求解得到重力矩模型与动摩擦力模型，根据所述重力矩模型与所述动摩擦力模型，以得到最终动态动力学模型；
第二采集模块，用于采集所述机器人的静摩擦力参数，并根据所述静摩擦力参数得到最终静态动力学模型；
完全零力控制模块，用于将所述最终动态动力学模型与所述最终静态动力学模型进行结合，以得到完全零力控制模型，并根据所述完全零力控制模型，完成所述机器人的完全零力控制。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明第一实施例提供的机器人完全零力控制方法的流程图；
图2为本发明第一实施例提供的机器人完全零力控制方法中步骤S1的详细流程图；
图3为本发明第一实施例提供的机器人完全零力控制方法的中步骤S2的详细流程图一；
图4为本发明第一实施例提供的机器人完全零力控制方法的中步骤S2的详细流程图二；
图5为本发明第一实施例提供的机器人完全零力控制方法的中步骤S27的详细流程图
图6为本发明第一实施例提供的机器人完全零力控制方法的中步骤S3的详细流程图
图7为本发明第一实施例提供的机器人完全零力控制方法的中步骤S4的详细流程图
图8为本发明第一实施例提供的机器人完全零力控制方法的中步骤S5的详细流程图
图9为本发明第二实施例提供的机器人完全零力控制系统的结构框图；
图10为本发明另一实施例提供的计算机设备的硬件结构示意图。
以下将结合附图对本发明实施例作进一步说明。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明的实施例，而不能理解为对本发明的限制。
在本发明实施例的描述中，需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本发明实施例中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明实施例中的具体含义。
实施例一
如图1所示，在本发明的第一个实施例中，该发明提供以下技术方案，一种机器人完全零力控制方法，所述机器人搭载于AGV小车上，所述方法包括：
S1、对机器人进行动力学建模及动力学参数辨识，以得到所述机器人的动力学模型与动力学参数；
具体的，在本步骤中的机器人的动力学模型及动力学参数，为机器人的一般动力学模型，通过构建动力学模型以及辨识动力学参数，可便于后续构建完全零力控制模型。
如图2所示，其中，所述步骤S1包括：
S11、对机器人进行动力学建模，以得到所述机器人的动力学模型：
τ＝τf+τd；
式中，τ为关节驱动力矩，τf为关节摩擦力矩，τd为关节动态力矩，q、 分别为关节角度、关节角速度、关节角加速度，D(q)为惯性项，为科式力及离心力项，G(q)为重力项；
其中，该步骤中的动力学模型为机器人，即协作机器人的一般动力学模型，且关节驱动力矩等于关节摩擦力矩与关节动态力矩之和，而惯性项D(q)、科式力及离心力项重力项G(q)上述三者均为机器人惯性参数与关节运动参数的函数；
S12、将关节动态力矩τd转化为线性分离形式：
式中，P为惯性参数；
其中，对于动力学模型的求解以及参数辨识来说，为了寻求一种更为有效的求解方式，因此需将关节动态力矩τd中的参数q、合理区分开，因此需将关节动态力矩τd的表达式转化为线性分离形式，以便于对动力学模型进行求解；
值得说明的是，只与机器人的状态有关，而P则是与之一一对应的惯性参数，在后续步骤中，通过获取机器人的运动数据，并通过运动数据得到与τd，以此便可求解出惯性参数P
S13、在所述动力学模型中引入Stribeck模型细化关节摩擦力矩τf：
式中，为直线运动速度，Ff为完整摩擦力，Fs为最大静摩擦力，Fc为库伦摩擦力，Bv为粘滞摩擦系数，Vs为Stribeck速度；
其中，机器人动力学参数辨识一般采用线性摩擦模型，但是由于机器人关节结构极为复杂，在关节模组内部还存在减速机、电机、谐波发生器及传动机构，这些部件之间的耦合也会用到润滑油脂，因此在关节运行时各速度区间的摩擦力是不同的，低速伺服系统会出现爬行，而稳态时又有较大的静差或出现极限环震荡，因此在此步骤中，需引入Stribeck模型细化关节摩擦力矩τf；
S14、对所述机器人进行动力学参数辨识，以得到所述机器人的动力学参数：
pd＝[Ixx，Ixy，Iyy，Ixz，Iyz，Izz，Hx，Hy，Hz，m，Ff]；
式中，Ixx，Ixy，Iyy，Ixz，Iyz，Izz为机器人惯量矩阵I的六个参数，Hx，Hy，Hz为质心向量，m为质量；
其中，机器人惯量矩阵I与H包含在D(q)及内，而m包含在G(q)内；
值得说明的是，对于一般物体来说：
对于均质物体来说：
式中，mi、xi、yi、zi分别为杆件质量与长宽高在微观下的离散化表达，n为离散点数，x、y、z为杆件长宽高在宏观下的表达，ρ为杆件密度，V为杆件体积；
S2、根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹，并通过二次规划算法得到所述机器人的最优激励轨迹，控制所述机器人按照所述最优激励轨迹运动并采集运动数据；
其中，在生成机器人的激励轨迹之后，但由于实际轨迹需要满足一系列的约束，并且要能够将动力学模型中待辨识的参数很好的激发出来，因此需要采用二次规划算法，以得到其中最优的激励轨迹，即最优激励轨迹，在确定最优激励轨迹之后，给定机器人一个运动周期，在该运动周期内控制机器人按照该最优激励轨迹进行运动并采集相应的运动数据，以便于后续步骤根据该运动参数构建初始状态动力学模型。
如图3所示，其中，所述步骤S2包括：
S21、根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹：
式中，qi为关节转角，qi0为关节转角常量，i表示关节的第i轴，al、bl为激励轨迹的常系数，ωf为激励轨迹的基频，ωfl表示第l阶的频率，N为阶数，t为采样时间；
具体的，该激励轨迹采用傅里叶级数型的轨迹，且距离采用五阶傅里叶级数型的轨迹，因此在上式中，阶数N最大为5，且t为采样时间，但在离散化后其代表采样点数；
S22、通过二次规划算法并引入约束将得到最优激励轨迹的过程转化为非线性优化问题，所述非线性优化问题的目标函数为：
min f(X)；
s.t.gu(X)≤0(u＝1，2，...，p)；
hv(X)＝0(v＝1，2，...，m)；
式中，gu(X)为所述机器人的机械臂的实际角度、实际角速度、实际角加速度最大值的约束，hv(X)为在起始时间的初始值约束，f(X)为优化目标；
具体的，由于实际轨迹需要满足一系列约束，并且要能够将动力学模型中待辨识的参数很好的激发出来，避免出现病态矩阵或系数高度敏感参数，这样才能保证动力学模型的准确性，而多约束的激励轨迹优化属于非线性优化问题，因此采用序列二次规划算法求取模型条件数最小的激励轨迹，即为最优激励轨迹，因此上式非线性优化问题的目标函数即为非线性优化问题的表达式；
S23、利用泰勒展开将所述非线性优化问题的目标函数在迭代点Xk处简化为二次函数，并将约束条件简化为线性函数，以得到二次规划问题的目标函数：
s.t.
式中，为微分算子，p、m分别为关节轴的数量，T为转置矩阵。
如图4所示，其中，在步骤S23之后，所述方法还包括：
S24、令S＝X-Xk，将所述二次规划问题的目标函数转换为变量S的目标函数：
s.t.
式中，S为当前点与迭代点之间的差值，为微分算子，其中，S具体为约束条件线性化和离散化后当前点与迭代点之间的差值；
具体的，由于二次规划问题的目标函数的解只为原非线性优化问题的目标函数的近似解，其该近似解并不是原非线性优化问题的目标函数的最优解，而是在局部的更好的一个解，因此通过令S＝X-Xk，将二次规划问题的目标函数转换为关于变量S的目标函数，以便于求出原非线性优化问题的目标函数的最优解；
S25、根据所述变量S的目标函数，令：
Beq＝[h1(Xk)，h2(Xk)，...，hm(Xk)]T；
B＝[g1(Xk)，g2(Xk)，...，gp(Xk)]T；
式中，Hk、C为变量S的目标函数的二次型矩形和向量，Aeq、Beq为线性等式约束，A、B为线性不等式约束；
S26、将二次规划问题的目标函数转换为一般形式目标函数：
s.t.AS＝-B；
AeqS＝-Beq；
S27、根据所述一般形式目标函数求解所述非线性优化问题的目标函数，以得到最优激励轨迹；
具体的，通过一般形式目标函数便可求解出所述非线性优化问题的目标函数的最优解，而该最优解即为最优激励路径；
如图5所示，其中，所述步骤S27包括：
S271、给定初始点X0、收敛精度ε，并另H0＝I，k＝0；
S272、将非线性优化问题的目标函数在迭代点Xk处简化为二次规划问题的目标函数；
S273、求解所述二次规划问题的目标函数，并令Sk＝S*；
S274、在方向Sk上对所述二次规划问题的目标函数进行约束一维搜索，得到下一迭代点Xk+1；
S275、判断Xk+1是否满足终止准则，若Xk+1满足终止准则，则取其中条件数最小的激励轨迹作为最优激励轨迹，将Xk+1作为最优解，作f(Xk+1)为目标函数的最优代价，若Xk+1不满足终止准则，则令k＝k+1，将Hk修正为Hk+1，并返回执行步骤S272。
具体的，重复上述步骤，即可得到满足最优解的最优激励轨迹，同时当Xk+1满足终止准则时，即在角度、角速度、角加速度的约束下达到优化次数，取其中条件数最小的轨迹即可作为最优激励轨迹；
S28、控制机器人按照所述最优激励轨迹运动并采集相应的初始数据，并对所述初始数据分别进行一次差分与二次差分，以得到运动数据。
其中，所述初始数据包括机器人的关节角度、电流值，在初始数据分别经过一次差分与二次差分之后，便可得到关节角速度与关节角加速度，即得到的运动数据。
S3、对所述运动数据进行滤波处理，使用QR分解法进行参数辨识，并基于所述动力学模型构建初始动态动力学模型；
具体的，对于获取到的数据，由于需要对位置信息进行两次差分得到加速度，过程中会引入很大噪声，并且电机电流也存在高斯噪声，因此采用自适应滤波对数据进行处理，将滤波后的数据代入模型，由于动力学模型基于最小惯性参数集，因此其观测矩阵为满秩矩阵，通过QR分解法求得最小惯性参数集矩阵；
其中QR分解法具体为：
一个矩阵m≥n，可以被分解成A＝QR，其中是正交矩阵；是上三角矩阵。
如图6所示，其中，所述步骤S3包括：
S31、采用自适应滤波算法对所述运动数据进行滤波处理；
S32、对于所述动力学模型，存在其中，Yr为对应的所述动力学模型的离差目标函数为：
式中，φ(P)为实测值与计算值的离差，且为正交矩阵，是上三角矩阵，m′为关节轴的数量，n′为惯性参数的数量，表示实数集合；
S33、令c1∈Rn′，c2∈Rm′-n′，则：
S34、令即根据 求解出惯性参数P，将惯性参数P代入所述动力学模型中，以得到初始动态动力学模型。
具体的，当令即之后，可以避免求解动力学模型时出现条件数更大的情况，使其结果更加稳定；
值得说明的是，在得到初始动态动力学模型前，需将求解出的惯性参数P以及经过滤波处理之后的运动数据代入至动力学模型中，替换其中的数据以及惯性参数P，以得到新的动力学模型，即初始动态动力学模型。
S4、采集所述机器人的重力参数及动摩擦力参数，并对所述重力参数与所述动摩擦力参数采用最小二乘法求解得到重力矩模型与动摩擦力模型，基于所述重力矩模型与所述动摩擦力模型得到最终动态动力学模型；
具体的，在采集所述机器人的重力参数、动摩擦力参数时，固定机器人的机械臂各轴，每次只运动其中一个关节，依次使其按照与地面平行的轴线对称两端做匀速往复运动，并且使用不同的速度进行多次采样得到数据，以得到所述机器人的重力参数、动摩擦力参数。
如图7所示，其中，所述步骤S4包括：
S41、令所述机器人的关节由第一速度θ1匀速运动到第二速度θ2，再令所述机器人的关节由第二速度θ2匀速运动到第一速度θ1，以得到关节输入力矩与关节输出力矩：
式中，τm1为关节输入力矩，τm2为关节输出力矩，τg为重力矩，τf为摩擦力矩，θ为实际关节角度，为实际关节角速度；
其中，由于关节内部状态不变，且减速器精度较高，因此正反摩擦力基本相等，因此可得
S42、根据所述关节输入力矩与所述关节输出力矩，得到重力矩与动摩擦力矩：
S43、将所述动摩擦力矩与重力矩τg(θ)代入所述动力学模型中，并通过最小二乘法求解出模型参数值，以得到动摩擦力模型与重力矩模型；
具体的，在此步骤中，对于动摩擦力矩来说，先将动摩擦力矩以及经过滤波处理后的运动数据的角速度代入至动力学模型中的τf中，通过最小二乘法求解出对应的模型参数值，Ff、Fs、Fc、Bv、Vs，以此得到动摩擦力模型；
对于重力矩τg(θ)来说，在动力学模型中能够得到其与关节力矩的解耦形式，二者在数值上是相等的，即τg(θ)＝G(q)，G只是与关节角度q有关的函数，同样使用最小二乘法根据τg(θ)与关节角度q解出G(q)得到重力矩模型；
S44、将所述动摩擦力模型与所述重力矩模型代入所述初始动态动力学模型中进行部分替换，以得到最终动态动力学模型：
式中，τe为所述机器人的关节在运动时及静止克服重力时的最终动态力矩，为减除重力矩及动摩擦力矩后只与关节角速度角加速度有关的观测矩阵；
具体的，P为与动态运动相关的最小惯性参数集，即为步骤S3中求解出的P，G(q)为重力矩模型，为动摩擦力模型。
S5、采集所述机器人的静摩擦力参数，并根据所述静摩擦力参数得到最终静态动力学模型；
如图8所示，其中，所述步骤S5包括：
S51、控制所述机器人进入力控模式，采集参数信息，将所述参数信息代入所述最终动态动力学模型中，求解出τe；
其中参数信息包括各关节角度、角速度、角加速度、电流等信息，将各项参数代入所述最终动态动力学模型中，以得到τe，且此时机械臂能够克服重力项在任意姿态保持静止，即只存在静摩擦力阻碍完全零力控制；
S52、依次对所述机器人的各关节施加额外力矩τadd，并监测关节角速度
S53、当大于第一预设值时，记录对应的额外力矩τadd并将其置零；
具体的，第一预设值为0，当大于0时，即为关节克服静摩擦力开始运动，记录下此时的额外力矩τadd并将其置零；
S54、当所述机器人关节的角度大于第二预设值时，将对应的额外力矩τadd进行线性拟合，其拟合后直线的数值作为最优值，即为最终静态动力学模型F(static)；
具体的，重复步骤S52至S53，知道机器人的关节角度为180度后，将对应的额外力矩τadd进行线性拟合，其拟合后直线的数值作为最优值，即为最终静态动力学模型F(static)。
S6、将所述最终动态动力学模型与所述最终静态动力学模型进行结合，以得到完全零力控制模型，并根据所述完全零力控制模型，完成所述机器人的完全零力控制；
其中，所述完全零力控制模型为：
具体的，由于机器人静态条件下，机器人任然受重力作用，因此需要在最终静态动力学模型中加入重力项，即G(q)。
本实施例一的好处在于：本申请不用各个关节加装力矩传感器就能实现克服重力的稳定拖拽效果，大大节约成本，能够很好的克服动静摩擦力达到轻松拖动机械臂的效果，提高工作效率和安全性，同时模型采用自适应滤波，只需数据递推而无需缓存，运算量小，响应速度快，且适用于各种产线、工厂环境，鲁棒性强，同时机器人的机械臂在任何姿态都可静止，保证一人就能完成拖拽示教，交互性更强更友好。
实施例二
如图9所示，在本发明的第二个实施例提供了一种机器人完全零力控制系统，所述系统包括：
建模与辨识模块1，用于对机器人进行动力学建模及动力学参数辨识，以得到所述机器人的动力学模型与动力学参数；
轨迹生成模块2，用于根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹，并通过二次规划算法得到所述机器人的最优激励轨迹，控制所述机器人按照所述最优激励轨迹运动并采集运动数据；
滤波处理模块3，用于对所述运动数据进行滤波处理，使用QR分解法进行参数辨识，并基于所述动力学模型构建初始动态动力学模型；
第一采集模块4，用于采集所述机器人的重力参数及动摩擦力参数，并对所述重力参数与所述动摩擦力参数采用最小二乘法求解得到重力矩模型与动摩擦力模型，基于所述重力矩模型与所述动摩擦力模型得到最终动态动力学模型；
第二采集模块5，用于采集所述机器人的静摩擦力参数，并根据所述静摩擦力参数得到最终静态动力学模型；
完全零力控制模块6，用于将所述最终动态动力学模型与所述最终静态动力学模型进行结合，以得到完全零力控制模型，并根据所述完全零力控制模型，完成所述机器人的完全零力控制。
其中，所述建模与辨识模块1包括：
建模子模块，用于对机器人进行动力学建模，以得到所述机器人的动力学模型：
τ＝τf+τd；
式中，τ为关节驱动力矩，τf为关节摩擦力矩，τd为关节动态力矩，q、 分别为关节角度、关节角速度、关节角加速度，D(q)为惯性项，为科式力及离心力项，G(q)为重力项；
转化子模块，用于将关节动态力矩τd转化为线性分离形式：
式中，P为惯性参数；
细化子模块，用于在所述动力学模型中引入Stribeck模型细化关节摩擦力矩τf：
式中，为直线运动速度，Ff为完整摩擦力，Fs为最大静摩擦力，Fc为库伦摩擦力，Bv为粘滞摩擦系数，Vs为Stribeck速度；
辨识子模块，用于对所述机器人进行动力学参数辨识，以得到所述机器人的动力学参数：
pd＝[Ixx，Ixy，Iyy，Ixz，Iyz，Izz，Hx，Hy，Hz，m，Ff]；
式中，Ixx，Ixy，Iyy，Ixz，Iyz，Izz为机器人惯量矩阵I的六个参数，Hx，Hy，Hz为质心向量，m为质量。
所述轨迹生成模块2包括：
轨迹生成子模块，用于根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹：
式中，qi为关节转角，qi0为关节转角常量，i表示关节的第i轴，al、bl为激励轨迹的常系数，ωf为激励轨迹的基频，ωfl表示第l阶的频率，N为阶数，t为采样时间；
转化子模块，用于通过二次规划算法并引入约束将得到最优激励轨迹的过程转化为非线性优化问题，所述非线性优化问题的目标函数为：
minf(X)；
s.t.gu(X)≤0(u＝1，2，...，p)；
hv(X)＝0(v＝1，2，...，m)；
式中，gu(X)为所述机器人的机械臂的实际角度、实际角速度、实际角加速度最大值的约束，hv(X)为在起始时间的初始值约束，f(X)为优化目标；
简化子模块，用于利用泰勒展开将所述非线性优化问题的目标函数在迭代点Xk处简化为二次函数，并将约束条件简化为线性函数，以得到二次规划问题的目标函数：
式中，为微分算子，p、m分别为关节轴的数量，T为转置矩阵。
所述轨迹生成模块2还包括：
变量转化子模块，用于令S＝X-Xk，将所述二次规划问题的目标函数转换为变量S的目标函数：
s.t.
式中，S为当前点与迭代点之间的差值，为微分算子；
指定子模块，用于根据所述变量S的目标函数，令：
Beq＝[h1(Xk)，h2(Xk)，...，hm(Xk)]T；
B＝[g1(Xk)，g2(Xk)，...，gp(Xk)]T；
式中，Hk、C为变量S的目标函数的二次型矩形和向量，Aeq、Beq为线性等式约束，A、B为线性不等式约束；
一般形式转换子模块，用于将二次规划问题的目标函数转换为一般形式目标函数：
s.t.AS＝-B；
AeqS＝-Beq；
最优轨迹确定子模块，用于根据所述一般形式目标函数求解所述非线性优化问题的目标函数，以得到最优激励轨迹；
数据采集子模块，用于控制机器人按照所述最优激励轨迹运动并采集相应的初始数据，并对所述初始数据分别进行一次差分与二次差分，以得到运动数据。
所述最优轨迹确定子模块包括：
给定单元，用于给定初始点X0、收敛精度ε，并另H0＝I，k＝0；
简化单元，用于将非线性优化问题的目标函数在迭代点Xk处简化为二次规划问题的目标函数；
求解单元，用于求解所述二次规划问题的目标函数，并令Sk＝S*；
搜索单元，用于在方向Sk上对所述二次规划问题的目标函数进行约束一维搜索，得到下一迭代点Xk+1；
判断单元，用于判断Xk+1是否满足终止准则，若Xk+1满足终止准则，则取其中条件数最小的激励轨迹作为最优激励轨迹，将Xk+1作为最优解，作f(Xk+1)为目标函数的最优代价，若Xk+1不满足终止准则，则令k＝k+1，将Hk修正为Hk+1，并返回执行所述将非线性优化问题的目标函数在迭代点Xk处简化为二次规划问题的目标函数的步骤。
所述滤波处理模块3包括：
滤波处理子模块，用于采用自适应滤波算法对所述运动数据进行滤波处理；
离差函数确定子模块，用于对于所述动力学模型，存在其中，Yr为对应的所述动力学模型的离差目标函数为：
式中，φ(P)为实测值与计算值的离差，且为正交矩阵，是上三角矩阵，m′为关节轴的数量，n′为惯性参数的数量，表示实数集合；
换算子模块，用于令c1∈Rn′，c2∈Rm′-n′，则：
惯性参数确定子模块，用于令即根据求解出惯性参数P，将惯性参数P代入所述动力学模型中，以得到初始动态动力学模型。
所述第一采集模块4包括：
力矩获取子模块，用于令所述机器人的关节由第一速度θ1匀速运动到第二速度θ2，再令所述机器人的关节由第二速度θ2匀速运动到第一速度θ1，以得到关节输入力矩与关节输出力矩：
式中，τm1为关节输入力矩，τm2为关节输出力矩，τg为重力矩，τf为摩擦力矩，θ为实际关节角度，为实际关节角速度；
力矩确定子模块，用于根据所述关节输入力矩与所述关节输出力矩，得到重力矩与动摩擦力矩：
参数值确定子模块，用于将所述动摩擦力矩与重力矩τg(θ)代入所述动力学模型中，并通过最小二乘法求解出模型参数值，以得到动摩擦力模型与重力矩模型；
替换子模块，用于将所述动摩擦力模型与所述重力矩模型代入所述初始动态动力学模型中进行部分替换，以得到最终动态动力学模型：
式中，τe为所述机器人的关节在运动时及静止克服重力时的最终动态力矩，为减除重力矩及动摩擦力矩后只与关节角速度角加速度有关的观测矩阵。
所述第二采集模块5包括：
控制子模块，用于控制所述机器人进入力控模式，采集参数信息，将所述参数信息代入所述最终动态动力学模型中，求解出τe；
力矩施加子模块，用于依次对所述机器人的各关节施加额外力矩τadd，并监测关节角速度
归零子模块，用于当大于第一预设值时，记录对应的额外力矩τadd并将其置零；
拟合子模块，用于当所述机器人关节的角度大于第二预设值时，将对应的额外力矩τadd进行线性拟合，其拟合后直线的数值作为最优值，即为最终静态动力学模型F(static)。
所述完全零力控制模块6还用按下式确定完全零力控制模型：
在本发明的另一些实施例中，本发明实施例提供以下技术方案，一种计算机，包括存储器102、处理器101以及存储在所述存储器102上并可在所述处理器101上运行的计算机程序，所述处理器101执行所述计算机程序时实现上所述的机器人完全零力控制方法。
具体的，上述处理器101可以包括中央处理器(CPU)，或者特定集成电路(Application Specific Integrated Circuit，简称为ASIC)，或者可以被配置成实施本申请实施例的一个或多个集成电路。
其中，存储器102可以包括用于数据或指令的大容量存储器。举例来说而非限制，存储器102可包括硬盘驱动器(Hard Disk Drive，简称为HDD)、软盘驱动器、固态驱动器(Solid State Drive，简称为SSD)、闪存、光盘、磁光盘、磁带或通用串行总线(UniversalSerial Bus，简称为USB)驱动器或者两个或更多个以上这些的组合。在合适的情况下，存储器102可包括可移除或不可移除(或固定)的介质。在合适的情况下，存储器102可在数据处理装置的内部或外部。在特定实施例中，存储器102是非易失性(Non-Volatile)存储器。在特定实施例中，存储器102包括只读存储器(Read-Only Memory，简称为ROM)和随机存取存储器(Random Access Memory，简称为RAM)。在合适的情况下，该ROM可以是掩模编程的ROM、可编程ROM(Programmable Read-Only Memory，简称为PROM)、可擦除PROM(ErasableProgrammable Read-Only Memory，简称为EPROM)、电可擦除PROM(Electrically ErasableProgrammable Read-Only Memory，简称为EEPROM)、电可改写ROM(ElectricallyAlterable Read-Only Memory，简称为EAROM)或闪存(FLASH)或者两个或更多个以上这些的组合。在合适的情况下，该RAM可以是静态随机存取存储器(Static Random-AccessMemory，简称为SRAM)或动态随机存取存储器(Dynamic Random Access Memory，简称为DRAM)，其中，DRAM可以是快速页模式动态随机存取存储器(Fast Page Mode DynamicRandom Access Memory，简称为FPMDRAM)、扩展数据输出动态随机存取存储器(ExtendedDate Out Dynamic Random Access Memory，简称为EDODRAM)、同步动态随机存取内存(Synchronous Dynamic Random-Access Memory，简称SDRAM)等。
存储器102可以用来存储或者缓存需要处理和/或通信使用的各种数据文件，以及处理器101所执行的可能的计算机程序指令。
处理器101通过读取并执行存储器102中存储的计算机程序指令，以实现上述机器人完全零力控制方法。
在其中一些实施例中，计算机还可包括通信接口103和总线100。其中，如图10所示，处理器101、存储器102、通信接口103通过总线100连接并完成相互间的通信。
通信接口103用于实现本申请实施例中各模块、装置、单元和/或设备之间的通信。通信接口103还可以实现与其他部件例如：外接设备、图像/数据采集设备、数据库、外部存储以及图像/数据处理工作站等之间进行数据通信。
总线100包括硬件、软件或两者，将计算机设备的部件彼此耦接在一起。总线100包括但不限于以下至少之一：数据总线(Data Bus)、地址总线(Address Bus)、控制总线(Control Bus)、扩展总线(Expansion Bus)、局部总线(Local Bus)。举例来说而非限制，总线100可包括图形加速接口(Accelerated Graphics Port，简称为AGP)或其他图形总线、增强工业标准架构(Extended Industry Standard Architecture，简称为EISA)总线、前端总线(Front Side Bus，简称为FSB)、超传输(Hyper Transport，简称为HT)互连、工业标准架构(Industry Standard Architecture，简称为ISA)总线、无线带宽(InfiniBand)互连、低引脚数(Low Pin Count，简称为LPC)总线、存储器总线、微信道架构(Micro ChannelArchitecture，简称为MCA)总线、外围组件互连(Peripheral Component Interconnect，简称为PCI)总线、PCI-Express(PCI-X)总线、串行高级技术附件(Serial AdvancedTechnology Attachment，简称为SATA)总线、视频电子标准协会局部(Video ElectronicsStandards Association Local Bus，简称为VLB)总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下，总线100可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线，但本申请考虑任何合适的总线或互连。
该计算机可以基于获取到机器人完全零力控制系统，执行本申请的机器人完全零力控制方法，从而实现机器人的完全零力控制。
在本发明的再一些实施例中，结合上述的机器人完全零力控制方法，本发明实施例提供以下技术方案，一种可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述的机器人完全零力控制。
本领域技术人员可以理解，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或它们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。
Claims (8)
1.一种机器人完全零力控制方法，其特征在于，所述方法包括：
对机器人进行动力学建模及动力学参数辨识，以得到所述机器人的动力学模型与动力学参数；
根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹，并通过二次规划算法得到所述机器人的最优激励轨迹，控制所述机器人按照所述最优激励轨迹运动并采集运动数据；
对所述运动数据进行滤波处理，使用QR分解法进行参数辨识，并基于所述动力学模型构建初始动态动力学模型；
采集所述机器人的重力参数及动摩擦力参数，并对所述重力参数与所述动摩擦力参数采用最小二乘法求解得到重力矩模型与动摩擦力模型，基于所述重力矩模型与所述动摩擦力模型得到最终动态动力学模型；
采集所述机器人的静摩擦力参数，并根据所述静摩擦力参数得到最终静态动力学模型；
将所述最终动态动力学模型与所述最终静态动力学模型进行结合，以得到完全零力控制模型，并根据所述完全零力控制模型，完成所述机器人的完全零力控制；
所述对机器人进行动力学建模及动力学参数辨识，以得到所述机器人的动力学模型与动力学参数的步骤包括：
对机器人进行动力学建模，以得到所述机器人的动力学模型：
；
；
式中，为关节驱动力矩，为关节摩擦力矩，为关节动态力矩，、、分别为关节角度、关节角速度、关节角加速度，为惯性项，为科式力及离心力项，为重力项；
将关节动态力矩转化为线性分离形式：
；
式中，为惯性参数；
在所述动力学模型中引入Stribeck模型细化关节摩擦力矩：
；
式中，为直线运动速度，为完整摩擦力，为最大静摩擦力，为库伦摩擦力，为粘滞摩擦系数，为Stribeck速度；
对所述机器人进行动力学参数辨识，以得到所述机器人的动力学参数：
；
式中，为机器人惯量矩阵的六个参数，为质心向量，为质量；
所述根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹，并通过二次规划算法得到所述机器人的最优激励轨迹，控制所述机器人按照所述最优激励轨迹运动并采集运动数据的步骤包括：
根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹：
；
式中，为关节转角，为关节转角常量，表示关节的第轴，、为激励轨迹的常系数，为激励轨迹的基频，表示第阶的频率，为阶数，为采样时间；
通过二次规划算法并引入约束将得到最优激励轨迹的过程转化为非线性优化问题，所述非线性优化问题的目标函数为：
；；；
式中，为所述机器人的机械臂的实际角度、实际角速度、实际角加速度最大值的约束，为在起始时间的初始值约束，为优化目标；
利用泰勒展开将所述非线性优化问题的目标函数在迭代点处简化为二次函数，并将约束条件简化为线性函数，以得到二次规划问题的目标函数：
；；；
式中，为微分算子，、分别为关节轴的数量，为转置矩阵。
2.根据权利要求1所述的机器人完全零力控制方法，其特征在于，在所述利用泰勒展开将非线性优化问题的目标函数在迭代点处简化为二次函数，并将约束条件简化为线性函数，以得到二次规划问题的目标函数的步骤之后，还包括：
令，将所述二次规划问题的目标函数转换为变量的目标函数：
；；；
式中，为当前点与迭代点之间的差值，为微分算子；
根据所述变量S的目标函数，令：
；；；；；；
式中，、为变量S的目标函数的二次型矩形和向量，、为线性等式约束，、为线性不等式约束；
将二次规划问题的目标函数转换为一般形式目标函数：
；；；
根据所述一般形式目标函数求解所述非线性优化问题的目标函数，以得到最优激励轨迹；
控制机器人按照所述最优激励轨迹运动并采集相应的初始数据，并对所述初始数据分别进行一次差分与二次差分，以得到运动数据。
3.根据权利要求2所述的机器人完全零力控制方法，其特征在于，所述根据所述一般形式目标函数求解所述非线性优化问题的目标函数，以得到最优激励轨迹的步骤包括：
给定初始点、收敛精度，并另，=0；
将非线性优化问题的目标函数在迭代点处简化为二次规划问题的目标函数；
求解所述二次规划问题的目标函数，并令；
在方向上对所述二次规划问题的目标函数进行约束一维搜索，得到下一迭代点；
判断是否满足终止准则，若满足终止准则，则取其中条件数最小的激励轨迹作为最优激励轨迹，将作为最优解，作为目标函数的最优代价，若不满足终止准则，则令，将修正为，并返回执行所述将非线性优化问题的目标函数在迭代点处简化为二次规划问题的目标函数的步骤。
4.根据权利要求1所述的机器人完全零力控制方法，其特征在于，所述对所述运动数据进行滤波处理，使用QR分解法进行参数辨识，并基于所述动力学模型构建初始动态动力学模型的步骤包括：
采用自适应滤波算法对所述运动数据进行滤波处理；
对于所述动力学模型，存在，其中，为，对应的所述动力学模型的离差目标函数为：
；
式中，为实测值与计算值的离差，且为正交矩阵，，是上三角矩阵,为关节轴的数量，为惯性参数的数量，表示实数集合；
令，则：
；
令=0，即，，根据，，求解出惯性参数，将惯性参数代入所述动力学模型中，以得到初始动态动力学模型。
5.根据权利要求1所述的机器人完全零力控制方法，其特征在于，所述采集所述机器人的重力参数及动摩擦力参数，并对所述重力参数与所述动摩擦力参数采用最小二乘法求解得到重力矩模型与动摩擦力模型，基于所述重力矩模型与所述动摩擦力模型得到最终动态动力学模型的步骤包括：
令所述机器人的关节由第一速度匀速运动到第二速度，再令所述机器人的关节由第二速度匀速运动到第一速度，以得到关节输入力矩与关节输出力矩：
；
；
；
式中，为关节输入力矩，为关节输出力矩，为重力矩，为摩擦力矩，为实际关节角度，为实际关节角速度；
根据所述关节输入力矩与所述关节输出力矩，得到重力矩与动摩擦力矩：
；
；
将所述动摩擦力矩 与重力矩代入所述动力学模型中，并通过最小二乘法求解出模型参数值，以得到动摩擦力模型与重力矩模型；
将所述动摩擦力模型与所述重力矩模型代入所述初始动态动力学模型中进行部分替换，以得到最终动态动力学模型：
；
式中，为所述机器人的关节在运动时及静止克服重力时的最终动态力矩，为减除重力矩及动摩擦力矩后只与关节角速度、角加速度有关的观测矩阵，为动摩擦力模型。
6.根据权利要求5所述的机器人完全零力控制方法，其特征在于，所述采集所述机器人的静摩擦力参数，并根据所述静摩擦力参数得到最终静态动力学模型的步骤包括：
控制所述机器人进入力控模式，采集参数信息，将所述参数信息代入所述最终动态动力学模型中，求解出；
依次对所述机器人的各关节施加额外力矩，并监测关节角速度；
当大于第一预设值时，记录对应的额外力矩并将其置零；
当所述机器人关节的角度大于第二预设值时，将对应的额外力矩进行线性拟合，其拟合后直线的数值作为最优值，即为最终静态动力学模型。
7.根据权利要求6所述的机器人完全零力控制方法，其特征在于，在所述将所述最终动态动力学模型与所述最终静态动力学模型进行结合，以得到完全零力控制模型，并根据所述完全零力控制模型，完成所述机器人的完全零力控制的步骤中，所述完全零力控制模型为：
式中，为重力项，为最终静态动力学模型，为减除重力矩及动摩擦力矩后只与关节角速度、角加速度有关的观测矩阵，为动摩擦力模型。
8.一种机器人完全零力控制系统，所述系统采用如权利要求7所述的机器人完全零力控制方法，其特征在于，所述系统包括：
建模与辨识模块，用于对机器人进行动力学建模及动力学参数辨识，以得到所述机器人的动力学模型与动力学参数；
轨迹生成模块，用于根据所述动力学模型与所述动力学参数生成所述机器人的激励轨迹，并通过二次规划算法得到所述机器人的最优激励轨迹，控制所述机器人按照所述最优激励轨迹运动并采集运动数据；
滤波处理模块，用于对所述运动数据进行滤波处理，使用QR分解法进行参数辨识，并基于所述动力学模型构建初始动态动力学模型；
第一采集模块，用于采集所述机器人的重力参数、动摩擦力参数，并对所述重力参数与所述动摩擦力参数采用最小二乘法求解得到重力矩模型与动摩擦力模型，根据所述重力矩模型与所述动摩擦力模型，以得到最终动态动力学模型；
第二采集模块，用于采集所述机器人的静摩擦力参数，并根据所述静摩擦力参数得到最终静态动力学模型；
完全零力控制模块，用于将所述最终动态动力学模型与所述最终静态动力学模型进行结合，以得到完全零力控制模型，并根据所述完全零力控制模型，完成所述机器人的完全零力控制。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211566523.3A CN115741718B (zh) | 2022-12-07 | 2022-12-07 | 一种机器人完全零力控制方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211566523.3A CN115741718B (zh) | 2022-12-07 | 2022-12-07 | 一种机器人完全零力控制方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115741718A CN115741718A (zh) | 2023-03-07 |
| CN115741718B true CN115741718B (zh) | 2024-06-07 |
Family
ID=85344242
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211566523.3A Active CN115741718B (zh) | 2022-12-07 | 2022-12-07 | 一种机器人完全零力控制方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115741718B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116442240B (zh) * | 2023-05-26 | 2023-11-14 | 中山大学 | 一种基于高通滤波解耦的机器人零力控制方法及装置 |
| CN116442246B (zh) * | 2023-06-14 | 2023-08-29 | 广州东焊智能装备有限公司 | 一种应用于机器人的姿态平衡控制方法 |
| CN116810802B (zh) * | 2023-08-28 | 2024-01-26 | 江苏云幕智造科技有限公司 | 偏置机械臂离散点轨迹平滑规划方法、系统及存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105479459A (zh) * | 2015-12-29 | 2016-04-13 | 深圳市汇川技术股份有限公司 | 机器人零力控制方法以及系统 |
| CN109403950A (zh) * | 2018-12-05 | 2019-03-01 | 北京卫星制造厂有限公司 | 一种星壤钻取驱动与控制系统仿真建模方法 |
| CN110531707A (zh) * | 2019-09-16 | 2019-12-03 | 无锡信捷电气股份有限公司 | Scara机器人的摩擦模型改进以及动力学参数辨识方法 |
| WO2021238049A1 (zh) * | 2020-05-28 | 2021-12-02 | 杭州键嘉机器人有限公司 | 机械臂的多负载自适应重力补偿方法、装置及控制设备 |
| CN115070768A (zh) * | 2022-07-08 | 2022-09-20 | 江西省智能产业技术创新研究院 | 机器人恒定力控及负载自辨识方法、系统、介质及计算机 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8924021B2 (en) * | 2006-04-27 | 2014-12-30 | Honda Motor Co., Ltd. | Control of robots from human motion descriptors |
-
2022
- 2022-12-07 CN CN202211566523.3A patent/CN115741718B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105479459A (zh) * | 2015-12-29 | 2016-04-13 | 深圳市汇川技术股份有限公司 | 机器人零力控制方法以及系统 |
| CN109403950A (zh) * | 2018-12-05 | 2019-03-01 | 北京卫星制造厂有限公司 | 一种星壤钻取驱动与控制系统仿真建模方法 |
| CN110531707A (zh) * | 2019-09-16 | 2019-12-03 | 无锡信捷电气股份有限公司 | Scara机器人的摩擦模型改进以及动力学参数辨识方法 |
| WO2021238049A1 (zh) * | 2020-05-28 | 2021-12-02 | 杭州键嘉机器人有限公司 | 机械臂的多负载自适应重力补偿方法、装置及控制设备 |
| CN115070768A (zh) * | 2022-07-08 | 2022-09-20 | 江西省智能产业技术创新研究院 | 机器人恒定力控及负载自辨识方法、系统、介质及计算机 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115741718A (zh) | 2023-03-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115741718B (zh) | 一种机器人完全零力控制方法及系统 | |
| CN113078861B (zh) | 一种永磁同步电机滑模控制方法、系统、介质及应用 | |
| CN111660307A (zh) | 一种机器人操作高辅精度的虚拟夹具控制方法及系统 | |
| JP2022513416A (ja) | 機械の動作を制御する装置及び方法、並びに記憶媒体 | |
| CN108365787A (zh) | 一种基于内模控制的永磁同步电机调速系统及其设计方法 | |
| CN114800519B (zh) | 一种考虑摩擦的六自由度工业机器人动力学参数辨识方法 | |
| CN110569767B (zh) | 运动参数识别方法、系统、设备及计算机可读存储介质 | |
| Lei et al. | Adaptive neural nonsingular terminal sliding mode control for MEMS gyroscope based on dynamic surface controller | |
| CN115741697A (zh) | 一种机械臂关节的优化力矩反馈方法、系统及设备 | |
| CN116619383A (zh) | 基于确定学习的机械臂pid控制方法及系统 | |
| Li et al. | Vibration suppression of ball-screw drive system based on flexible dynamics model | |
| CN116466589B (zh) | 一种基于自抗扰模型的路径跟踪控制方法、装置、存储介质及电子设备 | |
| Yin et al. | Overshoot Reduction Inspired Recurrent RBF Neural Network Controller Design for PMSM | |
| CN104734591B (zh) | 汽车电动转向电机磁场定向控制的串级系统稳定调速方法 | |
| Ke et al. | Tangential velocity tracking-based task coordinate frame approach for contouring control of biaxial motion systems | |
| Song et al. | Flexible joint parameters identification method based on improved tracking differentiator and adaptive differential evolution | |
| CN116729407B (zh) | 车辆横向速度检测方法、电子设备及存储介质 | |
| Zhao et al. | Accelerated Adaptive Backstepping Control of the Chaotic MEMS Gyroscope by Using the Type-2 Sequential FNN | |
| CN116015119B (zh) | 永磁同步电机电流控制方法及装置、存储介质及电子设备 | |
| CN113816265B (zh) | 一种基于人工神经网络的多模态桥吊摆动抑制控制方法 | |
| CN117590754B (zh) | 一种机器人系统的智能学习输出调控方法 | |
| CN116909136B (zh) | 基于确定学习的2-dof直升机滑模控制方法及系统 | |
| CN112395916B (zh) | 目标的运动状态信息确定方法、确定装置及电子设备 | |
| All-bail et al. | Preview control of trajectory tracking for servo system based on optimal preview control method | |
| Yan et al. | Weighted Multiple Model Adaptive Dynamic Surface Control for a Flexible-Joint Manipulator |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |