CN113043283A - Robot tail end external force estimation method - Google Patents

Abstract

Translated fromChinese

本发明公开了一种机器人末端外力预估方法，其技术方案要点是：包括以下步骤：Step1:建立机器人与外力交互的末端动力学模型；Step2:建立机器人动力学补偿误差模型；Step3:建立机器人末端外力预估模型；Step4:采用粒子群算法标定外力预估模型参数；本机器人末端无力传感器外力预估方法，基于所述粒子群算法离线优化出最优的机器人动力学补偿误差方差矩阵与力方差矩阵和机器人本体传感器信息，实时预估机器人末端与环境的交互力，避免了力传感器引入机器人控制系统。本发明对机器人动力学精确模型依赖性弱，优于依赖模型精确性的外力预估方法广义动量法，无需额外的繁琐的机器人动力学模型参数标定试验，提高了工作效率。

The invention discloses a method for estimating external force at the end of a robot, and the main points of the technical solution are as follows: Step 1: establish a dynamic model of the end of the interaction between the robot and the external force; Step 2: establish a dynamic compensation error model of the robot; Step 3: establish a robot Terminal external force prediction model; Step4: using particle swarm algorithm to calibrate external force prediction model parameters; this robot terminal inability sensor external force prediction method, based on the particle swarm algorithm offline optimization to optimize the robot dynamics compensation error variance matrix and force The variance matrix and the sensor information of the robot body can estimate the interaction force between the robot end and the environment in real time, avoiding the introduction of force sensors into the robot control system. The invention has weak dependence on the precise model of robot dynamics, and is superior to the generalized momentum method, which is an external force prediction method that depends on the accuracy of the model.

Description

Translated fromChinese

一种机器人末端外力预估方法A method for predicting external force at the end of a robot

技术领域technical field

本发明涉及工业机器人技术领域，特别涉及一种机器人末端外力预估方法。The invention relates to the technical field of industrial robots, in particular to a method for estimating external force at the end of a robot.

背景技术Background technique

新一代的机器人应该具有较好的环境自适应能力，能够在与环境交互过程中根据受到的力信息采用柔顺控制策略自适应调整其运行状态以满足环境约束。目前，为了实现机器人与环境交互的安全性，大都机器人借助力传感器实现柔顺控制，使机器人动态特性服从交互力特性。然而，力传感器的引入无疑增加了机器人控制系统结构难度和机器人制造成本，且降低了系统对环境的鲁棒性。因此，研究基于机器人本体传感器预估机器人与环境之间的交互力具有重大的应用价值。The new generation of robots should have good environmental adaptive ability, and can adaptively adjust its operating state according to the force information received in the process of interacting with the environment to meet the environmental constraints. At present, in order to realize the safety of the interaction between the robot and the environment, most robots use force sensors to achieve compliant control, so that the dynamic characteristics of the robot obey the interactive force characteristics. However, the introduction of force sensors undoubtedly increases the structural difficulty of the robot control system and the manufacturing cost of the robot, and reduces the robustness of the system to the environment. Therefore, it is of great application value to study the prediction of the interaction force between the robot and the environment based on the robot body sensor.

众多学者对机器人与环境之间的交互力的准确预估进行了大量研究。Cirillo etal.在机器人本体上覆盖敏感材料作为机器人的皮肤，以此直接测量交互力与接触的位置。然而，因为在机器人本体上覆盖敏感材料还不能完全被接受，因此这一方法未被广泛采用。Many scholars have done a lot of research on the accurate prediction of the interaction force between the robot and the environment. Cirillo et al. covered the robot body with a sensitive material as the robot's skin to directly measure the interaction force and contact position. However, this method has not been widely adopted because it is not yet fully acceptable to cover the robot body with sensitive materials.

另外，基于机器人关节本体传感器信息，比如关节位置、关节力矩等信息，Haddadin et al.和Smith et al.利用观测器来预估机器人与环境之间的碰撞力。Luca和Mattone提出基于机器人惯量和机器人关节角速度的广义动量方法来预估机器人碰撞力矩。Tian et al.和Lee et al.利用广义动量观测器预估机器人关节之间的摩擦力矩，但是广义动量方法依赖机器人的精确模型，其机器人精确模型不宜从实际获得。因此，业内亟需一种低成本和精确性较好的机器人末端外力预估方法。In addition, based on the sensor information of the robot joint body, such as joint position, joint torque and other information, Haddadin et al. and Smith et al. used the observer to estimate the collision force between the robot and the environment. Luca and Mattone proposed a generalized momentum method based on robot inertia and robot joint angular velocity to estimate robot collision moment. Tian et al. and Lee et al. used the generalized momentum observer to estimate the friction torque between the robot joints, but the generalized momentum method relies on the accurate model of the robot, and the accurate model of the robot should not be obtained from reality. Therefore, there is an urgent need in the industry for a low-cost and accurate method for predicting the external force at the end of the robot.

发明内容SUMMARY OF THE INVENTION

针对背景技术中提到的问题，本发明的目的是提供一种机器人末端外力预估方法，以解决背景技术中提到的问题。In view of the problems mentioned in the background art, the purpose of the present invention is to provide a method for estimating the external force of the robot end, so as to solve the problems mentioned in the background art.

本发明的上述技术目的是通过以下技术方案得以实现的：The above-mentioned technical purpose of the present invention is achieved through the following technical solutions:

一种机器人末端外力预估方法，包括以下步骤：A method for estimating external force at the end of a robot, comprising the following steps:

Step1:建立机器人与外力交互的末端动力学模型；Step1: Establish the terminal dynamics model of the interaction between the robot and the external force;

Step2:建立机器人动力学补偿误差模型；Step2: establish a robot dynamic compensation error model;

Step3:建立机器人末端外力预估模型；Step3: Establish a model for predicting the external force at the end of the robot;

Step4:采用粒子群算法标定外力预估模型参数。Step4: Use the particle swarm algorithm to calibrate the parameters of the external force prediction model.

较佳的，所述Step3中，机器人末端受到的外力F_ext包括机器人末端与环境的交互力，包括力和力矩，即：Preferably, in the Step 3, the external force F_ext received by the end of the robot includes the interaction force between the end of the robot and the environment, including force and torque, namely:

F_ext＝[f_{ext_x},f_{ext_y},f_{ext_z},τ_{ext_x},τ_{ext_y},τ_{ext_z}]^T；F_ext = [f_{ext_x} , f_{ext_y} , f_{ext_z} , τ_{ext_x} , τ_{ext_y} , τ_{ext_z} ]^T ;

其中，F_ext为基于机器人基坐标系表示机器人末端受到的外力F_ext矩阵。Among them, F_ext is the F_ext matrix representing the external force received by the robot end based on the robot base coordinate system.

较佳的，所述Step3中，机器人末端动力学模型由机器人关节空间动力学模型通过末端雅克比矩阵J_c(q)转换而来，机器人关节空间动力学模型为：Preferably, in the Step 3, the robot end dynamics model is converted from the robot joint space dynamics model through the end Jacobian matrix J_c (q), and the robot joint space dynamics model is:

其中，M_s(q)是机器人连杆惯量矩阵；

是离心力科氏力矢量；G(q)是机器人重力力矩；

为关节摩擦力矩；τ_c为机器人驱动力矩；

为F_ext在机器人关节空间的等效关节力矩；q、

分别为机器人连杆角度位置、速度和加速度。Among them, M_s (q) is the inertia matrix of the robot link;

is the centrifugal force Coriolis force vector; G(q) is the gravitational moment of the robot;

is the joint friction torque; τ_c is the driving torque of the robot;

is the equivalent joint moment of F_ext in the robot joint space; q,

are the angular position, velocity and acceleration of the robot link, respectively.

较佳的，所述Step1中，机器人与外力的末端动力学为：Preferably, in theStep 1, the end dynamics of the robot and the external force are:

其中，x为机器人末端位姿；

Among them, x is the robot end pose;

其中，Λ_s(q)为操作空间惯量矩阵；

为操作空间中的摩擦力；

为操作空间中的科氏力；F_g(q)为操作空间中的重力；F_c为操作空间中的关节驱动力；

为机器人末端雅可比矩阵的加权广义逆矩阵；Among them, Λ_s (q) is the operation space inertia matrix;

is the frictional force in the operating space;

is the Coriolis force in the operating space; F_g (q) is the gravity in the operating space; F_c is the joint driving force in the operating space;

is the weighted generalized inverse of the Jacobian matrix at the end of the robot;

当机器人雅可比矩阵处于奇异状态或者接近奇异状态时，

可采用阻尼最小二乘法避开奇异状态，保证机器人关节角速度连续。When the robot Jacobian matrix is in a singular state or close to a singular state,

The damped least squares method can be used to avoid the singular state and ensure the continuous angular velocity of the robot joints.

较佳的，所述Step2中，机器人动力学补偿误差模型用于分析机器人末端动力学补偿误差因素对外力预估的影响；Preferably, in the Step 2, the robot dynamics compensation error model is used to analyze the influence of the robot end dynamics compensation error factors on the external force estimation;

令

分别为

F_g(q)动力学前馈补偿项，在外力作用下所述机器人驱动力矩F_c可以表示为：make

respectively

F_g (q) dynamic feedforward compensation term, the robot driving torque F_c under the action of external force can be expressed as:

其中，F_res为包含F_ext的残余有效力。Among them,_Fres is the residual effective force including F_ext .

较佳的，所述Step2中，由于现实中不能完全精确掌握机器人动力学模型参数，前馈补偿项相对实际存有误差，机器人动力学补偿误差e_dyn为：Preferably, in the Step 2, since the parameters of the robot dynamics model cannot be completely and accurately grasped in reality, the feedforward compensation term has an error relative to the actual, and the robot dynamics compensation error_edyn is:

e_dyn＝e_ΛU+e_f+e_g；_edyn =e_ΛU +e_f +e_g ;

其中，

in,

其中e_ΛU为操作空间惯性力和科氏力的补偿误差；e_f为操作空间摩擦力的补偿误差where e_ΛU is the compensation error of the inertial force and Coriolis force in the operating space; e_f is the compensation error of the frictional force in the operating space

；e_g为操作空间重力的补偿误差；; e_g is the compensation error of the operating space gravity;

基于所述机器人的末端动力学和所述机器人的动力学补偿误差，得到机器人动力学补偿误差对机器人外力预估影响的表达式，即：Based on the terminal dynamics of the robot and the dynamic compensation error of the robot, the expression of the influence of the robot dynamic compensation error on the prediction of the external force of the robot is obtained, namely:

F_ext＝e_dyn-F_res；F_ext =_edyn -F_res ;

其中，F_ext为需要预估的外力；e_dyn为动力学补偿误差；F_res为参与有效力；Among them, F_ext is the external force that needs to be estimated;_edyn is the dynamic compensation error;_Fres is the effective participation force;

所述机器人的末端外力预估模型是用于在无力传感器条件下，基于机器人本体传感器信号包括电机角度位置、电机角速度和电机扭矩，实时预估机器人末端与环境发生接触的力。The robot end external force prediction model is used to estimate the force of the robot end contacting the environment in real time based on the robot body sensor signal including the motor angular position, the motor angular velocity and the motor torque under the condition of a powerless sensor.

较佳的，所述机器人动力学补偿误差e_dyn和所述外力F_ext在机器人与环境交互过程中不能时刻保持常值，设定所述机器人动力学补偿误差e_dyn和所述外力F_ext服从正态分布的随机变量，两者相互独立；Preferably, the robot dynamics compensation error_edyn and the external force F_ext cannot keep constant values at all times during the interaction between the robot and the environment, and the robot dynamics compensation error_edyn and the external force F_ext are set to obey. A normally distributed random variable, the two are independent of each other;

设定机器人动力学补偿误差期望E[e_dyn]＝0，力期望E[F_ext]＝0，机器人动力学补偿误差方差Var[e_dyn]＝Ω_edyn，力方差Var[F_ext]＝Ω_Fext，且e_dyn与F_ext的协方差为零，即

Set robot dynamics compensation error expectation E[_{ed dyn} ]=0, force expectation E[F_ext ]=0, robot dynamics compensation error variance Var[ed_dyn ]=Ω_edyn , force variance Var[F_ext ]=Ω_Fext , and the covariance of_edyn and_Fext is zero, that is

基于所述力方差Ω_Fext，机器人动力学补偿误差方差Ω_edyn和残余有效力F_res，建立所述机器人末端外力预估模型，为：Based on the force variance Ω_Fext , the robot dynamic compensation error variance Ω_edyn and the residual effective force F_res , the robot end external force prediction model is established, which is:

其中，

为所述外力F_ext的预估值。in,

is the estimated value of the external force F_ext .

较佳的，若所述机器人动力模型存在偏差d_fe，服从正态分布，其期望E[d_fe]＝Ν_bia，方差Var[d_fe]＝Ω_edyn，则所述机器人末端外力预估模型预估出的外力存在偏差F_bia，即：Preferably, if the robot dynamic model has a deviation d_fe and obeys a normal distribution, and its expectation is E[d_fe ]=N_bia and variance Var[d_fe ]=Ω_edyn , then the robot end external force prediction model The estimated external force has a deviation F_bia , namely:

F_bia＝Ω_Fext(Ω_Fext+Ω_edyn)^-1N_bia。F_bia =Ω_Fext (Ω_Fext +Ω_edyn )⁻¹ N_bia .

较佳的，所述外力预估模型参数包括所述机器人动力学补偿误差方差Ω_edyn矩阵和力方差Ω_Fext矩阵，利用粒子群算法标定外力预估模型参数，步骤为：Preferably, the external force prediction model parameters include the robot dynamics compensation error variance Ω_edyn matrix and the force variance Ω_Fext matrix, and the particle swarm algorithm is used to calibrate the external force prediction model parameters, and the steps are:

A.建立机器人操作空间零力控制模型

A. Establish a zero-force control model of the robot operating space

B.人手低速拖动机器人末端执行器，实时采集机器人电机角度θ_i(t)、电机角速度

电机扭矩τ_m,i(t)和传感器力信息F_ext(t)，采用粒子群优化算法离线优化Ω_Fext和Ω_edyn，使得基于机器人本体传感器信息，所述机器人末端外力预估模型能够准确预估外力F_ext；B. The human hand drags the robot end effector at low speed, and collects the robot motor angle θ_i (t) and motor angular velocity in real time

Motor torque τ_m,i (t) and sensor force information F_ext (t), particle swarm optimization algorithm is used to optimize Ω_Fext and Ω_{edyn offline} , so that based on the sensor information of the robot body, the robot end external force prediction model can accurately predict. Estimate the external force F_ext ;

所述机器人操作空间零力控制模型是由关节空间机器人零力控制模型通过末端雅可比矩阵转换得到，并对机器人重力和关节摩擦力进行补偿；The zero-force control model of the robot operation space is obtained by transforming the zero-force control model of the joint space robot through the end Jacobian matrix, and compensates the robot gravity and joint friction;

所述机器人连杆角度q_i(t)与角速度

和所述机器人驱动力矩τ_c,i(t)是由所述机器人电机角度θ_i(t)与角速度

和所述电机扭矩τ_m,i(t)通过伺服系统减速比因子N_ratio,i转换计算得到，即：The robot link angle_qi (t) and angular velocity

and the robot driving torque τ_c,i (t) is determined by the robot motor angle θ_i (t) and the angular velocity

And the motor torque τ_m,i (t) is calculated by the reduction ratio factor N_ratio,i of the servo system, namely:

q_i(t)＝θ_i(t)/N_ratio,i；q_i (t)=θ_i (t)/N_ratio,i ;

τ_c,i(t)＝τ_m,i(t)N_ratio,i，i＝1,…,n；τ_c,i (t)=τ_m,i (t)N_ratio,i , i=1,...,n;

其中，n为机器人关节自由度。Among them, n is the degree of freedom of the robot joint.

较佳的，所述粒子群算法能够避免算法早熟及陷入局部最优值，其中，所述粒子群算法的适应度函数定义为：Preferably, the particle swarm algorithm can prevent the algorithm from being premature and falling into a local optimum, wherein the fitness function of the particle swarm algorithm is defined as:

其中，

为外力预估误差、

为外力预估矩阵因子的最大奇异值、z₁和z₂分别为优化权重值，且z₁+z₂＝1，z₁＞0，z₂＞0。in,

Estimate error for external force,

The maximum singular value of the external force prediction matrix factor, z₁ and z₂ are the optimization weight values respectively, and z₁ +z₂ =1, z₁ >0, z₂ >0.

综上所述，本发明主要具有以下有益效果：To sum up, the present invention mainly has the following beneficial effects:

本机器人末端无力传感器外力预估方法，基于所述粒子群算法离线优化出最优的机器人动力学补偿误差方差矩阵与力方差矩阵和机器人本体传感器信息，实时预估机器人末端与环境的交互力，避免了力传感器引入机器人控制系统，克服了其给机器人系统带来的相关缺陷。另一方面，本发明对机器人动力学精确模型依赖性弱，优于依赖模型精确性的外力预估方法广义动量法，无需额外的繁琐的机器人动力学模型参数标定试验，提高了工作效率。The method for predicting the external force of the robot end incapacitating sensor, based on the particle swarm algorithm, optimizes the optimal robot dynamics compensation error variance matrix, force variance matrix and robot body sensor information offline, and estimates the interaction force between the robot end and the environment in real time. The introduction of the force sensor into the robot control system is avoided, and the related defects brought by the force sensor to the robot system are overcome. On the other hand, the present invention has weak dependence on the precise model of robot dynamics, and is superior to the generalized momentum method, which is an external force prediction method that depends on the accuracy of the model, and does not require additional and cumbersome robot dynamics model parameter calibration tests, thereby improving work efficiency.

附图说明Description of drawings

图1为本发明的方法流程图；Fig. 1 is the method flow chart of the present invention;

图2为本发明的外力预估模型方差关系图；Fig. 2 is the external force estimation model variance relation diagram of the present invention;

图3为本发明中机器人零力控制框图。FIG. 3 is a block diagram of the zero-force control of the robot in the present invention.

具体实施方式Detailed ways

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

实施例1Example 1

参考图1至图3，一种机器人末端外力预估方法，包括以下步骤：1 to 3, a method for estimating external force at the end of a robot includes the following steps:

包括以下步骤：Include the following steps:

Step1:建立机器人与外力交互的末端动力学模型；Step1: Establish the terminal dynamics model of the interaction between the robot and the external force;

Step2:建立机器人动力学补偿误差模型；Step2: establish a robot dynamic compensation error model;

Step3:建立机器人末端外力预估模型；Step3: Establish a model for predicting the external force at the end of the robot;

Step4:采用粒子群算法标定外力预估模型参数。Step4: Use the particle swarm algorithm to calibrate the parameters of the external force prediction model.

较佳的，所述Step3中，机器人末端受到的外力F_ext包括机器人末端与环境的交互力，包括力和力矩，即：Preferably, in the Step 3, the external force F_ext received by the end of the robot includes the interaction force between the end of the robot and the environment, including force and torque, namely:

F_ext＝[f_{ext_x},f_{ext_y},f_{ext_z},τ_{ext_x},τ_{ext_y},τ_{ext_z}]^T；F_ext = [f_{ext_x} , f_{ext_y} , f_{ext_z} , τ_{ext_x} , τ_{ext_y} , τ_{ext_z} ]^T ;

其中，F_ext为基于机器人基坐标系表示机器人末端受到的外力F_ext矩阵。Among them, F_ext is the F_ext matrix representing the external force received by the robot end based on the robot base coordinate system.

较佳的，所述Step3中，机器人末端动力学模型由机器人关节空间动力学模型通过末端雅克比矩阵J_c(q)转换而来，机器人关节空间动力学模型为：Preferably, in the Step 3, the robot end dynamics model is converted from the robot joint space dynamics model through the end Jacobian matrix J_c (q), and the robot joint space dynamics model is:

其中，M_s(q)是机器人连杆惯量矩阵；

是离心力科氏力矢量；G(q)是机器人重力力矩；

为关节摩擦力矩；τ_c为机器人驱动力矩；

为F_ext在机器人关节空间的等效关节力矩；q、

分别为机器人连杆角度位置、速度和加速度。Among them, M_s (q) is the inertia matrix of the robot link;

is the centrifugal force Coriolis force vector; G(q) is the gravitational moment of the robot;

is the joint friction torque; τ_c is the driving torque of the robot;

is the equivalent joint moment of F_ext in the robot joint space; q,

are the angular position, velocity and acceleration of the robot link, respectively.

较佳的，所述Step1中，机器人与外力的末端动力学为：Preferably, in theStep 1, the end dynamics of the robot and the external force are:

其中，_x为机器人末端位姿；

Among them,_x is the robot end pose;

其中，Λ_s(q)为操作空间惯量矩阵；

为操作空间中的摩擦力；

为操作空间中的科氏力；F_g(q)为操作空间中的重力；F_c为操作空间中的关节驱动力；

为机器人末端雅可比矩阵的加权广义逆矩阵；Among them, Λ_s (q) is the operation space inertia matrix;

is the frictional force in the operating space;

is the Coriolis force in the operating space; F_g (q) is the gravity in the operating space; F_c is the joint driving force in the operating space;

is the weighted generalized inverse of the Jacobian matrix at the end of the robot;

当机器人雅可比矩阵处于奇异状态或者接近奇异状态时，

可采用阻尼最小二乘法避开奇异状态，保证机器人关节角速度连续。When the robot Jacobian matrix is in a singular state or close to a singular state,

The damped least squares method can be used to avoid the singular state and ensure the continuous angular velocity of the robot joints.

较佳的，所述Step2中，机器人动力学补偿误差模型用于分析机器人末端动力学补偿误差因素对外力预估的影响；Preferably, in the Step 2, the robot dynamics compensation error model is used to analyze the influence of the robot end dynamics compensation error factors on the external force estimation;

令

分别为

F_g(q)动力学前馈补偿项，在外力作用下所述机器人驱动力矩F_c可以表示为：make

respectively

F_g (q) dynamic feedforward compensation term, the robot driving torque F_c under the action of external force can be expressed as:

其中，F_res为包含F_ext的残余有效力。Among them,_Fres is the residual effective force including F_ext .

较佳的，所述Step2中，由于现实中不能完全精确掌握机器人动力学模型参数，前馈补偿项相对实际存有误差，机器人动力学补偿误差e_dyn为：Preferably, in the Step 2, since the parameters of the robot dynamics model cannot be completely and accurately grasped in reality, the feedforward compensation term has an error relative to the actual, and the robot dynamics compensation error_edyn is:

ed_yn＝e_ΛU+e_f+e_g；ed_yn =e_ΛU +e_f +e_g ;

其中，

in,

其中e_ΛU为操作空间惯性力和科氏力的补偿误差；e_f为操作空间摩擦力的补偿误差where e_ΛU is the compensation error of the inertial force and Coriolis force in the operating space; e_f is the compensation error of the frictional force in the operating space

；e_g为操作空间重力的补偿误差；; e_g is the compensation error of the operating space gravity;

基于所述机器人的末端动力学和所述机器人的动力学补偿误差，得到机器人动力学补偿误差对机器人外力预估影响的表达式，即：Based on the terminal dynamics of the robot and the dynamic compensation error of the robot, the expression of the influence of the robot dynamic compensation error on the prediction of the external force of the robot is obtained, namely:

F_ext＝e_dyn-F_res；F_ext =_edyn -F_res ;

其中，F_ext为需要预估的外力；e_dyn为动力学补偿误差；F_res为参与有效力；Among them, F_ext is the external force that needs to be estimated;_edyn is the dynamic compensation error;_Fres is the effective participation force;

所述机器人的末端外力预估模型是用于在无力传感器条件下，基于机器人本体传感器信号包括电机角度位置、电机角速度和电机扭矩，实时预估机器人末端与环境发生接触的力。The robot end external force prediction model is used to estimate the force of the robot end contacting the environment in real time based on the robot body sensor signal including the motor angular position, the motor angular velocity and the motor torque under the condition of a powerless sensor.

较佳的，所述机器人动力学补偿误差e_dyn和所述外力F_ext在机器人与环境交互过程中不能时刻保持常值，设定所述机器人动力学补偿误差e_dyn和所述外力F_ext服从正态分布的随机变量，两者相互独立；Preferably, the robot dynamics compensation error_edyn and the external force F_ext cannot keep constant values at all times during the interaction between the robot and the environment, and the robot dynamics compensation error_edyn and the external force F_ext are set to obey. A normally distributed random variable, the two are independent of each other;

设定机器人动力学补偿误差期望E[e_dyn]＝0，力期望E[F_ext]＝0，机器人动力学补偿误差方差Var[e_dyn]＝Ω_edyn，力方差Var[F_ext]＝Ω_Fext，且e_dyn与F_ext的协方差为零，即

Set robot dynamics compensation error expectation E[_{ed dyn} ]=0, force expectation E[F_ext ]=0, robot dynamics compensation error variance Var[ed_dyn ]=Ω_edyn , force variance Var[F_ext ]=Ω_Fext , and the covariance of_edyn and_Fext is zero, that is

基于所述力方差Ω_Fext，机器人动力学补偿误差方差Ω_edyn和残余有效力F_res，建立所述机器人末端外力预估模型，为：Based on the force variance Ω_Fext , the robot dynamic compensation error variance Ω_edyn and the residual effective force F_res , the robot end external force prediction model is established, which is:

其中，

为所述外力F_ext的预估值。in,

is the estimated value of the external force F_ext .

较佳的，若所述机器人动力模型存在偏差d_fe，服从正态分布，其期望E[d_fe]＝Ν_bia，方差Var[d_fe]＝Ω_edyn，则所述机器人末端外力预估模型预估出的外力存在偏差F_bia，即：Preferably, if the robot dynamic model has a deviation d_fe and obeys a normal distribution, and its expectation is E[d_fe ]=N_bia and variance Var[d_fe ]=Ω_edyn , then the robot end external force prediction model The estimated external force has a deviation F_bia , namely:

F_bia＝Ω_Fext(Ω_Fext+Ω_edyn)^-1N_bia。F_bia =Ω_Fext (Ω_Fext +Ω_edyn )⁻¹ N_bia .

较佳的，所述外力预估模型参数包括所述机器人动力学补偿误差方差Ω_edyn矩阵和力方差Ω_Fext矩阵，利用粒子群算法标定外力预估模型参数，步骤为：Preferably, the external force prediction model parameters include the robot dynamics compensation error variance Ω_edyn matrix and the force variance Ω_Fext matrix, and the particle swarm algorithm is used to calibrate the external force prediction model parameters, and the steps are:

A.建立机器人操作空间零力控制模型

A. Establish a zero-force control model of the robot operating space

B.人手低速拖动机器人末端执行器，实时采集机器人电机角度θ_i(t)、电机角速度

电机扭矩τ_m,i(t)和传感器力信息F_ext(t)，采用粒子群优化算法离线优化Ω_Fext和Ω_edyn，使得基于机器人本体传感器信息，所述机器人末端外力预估模型能够准确预估外力F_ext；B. The human hand drags the robot end effector at low speed, and collects the robot motor angle θ_i (t) and motor angular velocity in real time

Motor torque τ_m,i (t) and sensor force information F_ext (t), particle swarm optimization algorithm is used to optimize Ω_Fext and Ω_{edyn offline} , so that based on the sensor information of the robot body, the robot end external force prediction model can accurately predict. Estimate the external force F_ext ;

所述机器人操作空间零力控制模型是由关节空间机器人零力控制模型通过末端雅可比矩阵转换得到，并对机器人重力和关节摩擦力进行补偿；The zero-force control model of the robot operation space is obtained by transforming the zero-force control model of the joint space robot through the end Jacobian matrix, and compensates the robot gravity and joint friction;

所述机器人连杆角度q_i(t)与角速度

和所述机器人驱动力矩τ_c,i(t)是由所述机器人电机角度θ_i(t)与角速度

和所述电机扭矩τ_m,i(t)通过伺服系统减速比因子N_ratio,i转换计算得到，即：The robot link angle_qi (t) and angular velocity

and the robot driving torque τ_c,i (t) is determined by the robot motor angle θ_i (t) and the angular velocity

And the motor torque τ_m,i (t) is calculated by the reduction ratio factor N_ratio,i of the servo system, namely:

q_i(t)＝θ_i(t)/N_ratio,i；q_i (t)=θ_i (t)/N_ratio,i ;

τ_c,i(t)＝τ_m,i(t)N_ratio,i，i＝1,…,n；τ_c,i (t)=τ_m,i (t)N_ratio,i , i=1,...,n;

其中，n为机器人关节自由度。Among them, n is the degree of freedom of the robot joint.

较佳的，所述粒子群算法能够避免算法早熟及陷入局部最优值，其中，所述粒子群算法的适应度函数定义为：Preferably, the particle swarm algorithm can prevent the algorithm from being premature and falling into a local optimum, wherein the fitness function of the particle swarm algorithm is defined as:

其中，

为外力预估误差、

为外力预估矩阵因子的最大奇异值、z₁和z₂分别为优化权重值，且z₁+z₂＝1，z₁＞0，z₂＞0。in,

Estimate error for external force,

The maximum singular value of the external force prediction matrix factor, z₁ and z₂ are the optimization weight values respectively, and z₁ +z₂ =1, z₁ >0, z₂ >0.

参考图1至图3，本机器人末端无力传感器外力预估方法，基于粒子群算法离线优化出最优的机器人动力学补偿误差方差矩阵与力方差矩阵和机器人本体传感器信息，实时预估机器人末端与环境的交互力，避免了力传感器引入机器人控制系统，克服了其给机器人系统带来的相关缺陷。另一方面，本发明对机器人动力学精确模型依赖性弱，优于依赖模型精确性的外力预估方法广义动量法，无需额外的繁琐的机器人动力学模型参数标定试验，提高了工作效率。Referring to Figure 1 to Figure 3, this method for predicting the external force of the robot end inability sensor, based on the particle swarm algorithm offline optimization, optimizes the optimal robot dynamic compensation error variance matrix and force variance matrix and the robot body sensor information, and estimates the robot end and the robot body sensor information in real time. The interactive force of the environment avoids the introduction of force sensors into the robot control system, and overcomes the related defects that it brings to the robot system. On the other hand, the present invention has weak dependence on the precise model of robot dynamics, and is superior to the generalized momentum method, which is an external force prediction method that depends on the accuracy of the model, and does not require additional and cumbersome robot dynamics model parameter calibration tests, thereby improving work efficiency.

实施例2Example 2

机器人包括机器人本体、示教把手和六维力传感器；本实施例包括以下方案：The robot includes a robot body, a teaching handle and a six-dimensional force sensor; this embodiment includes the following solutions:

一、建立机器人与外力交互的末端动力学模型：1. Establish the terminal dynamics model of the interaction between the robot and the external force:

机器人所受的外力主要为机器人与环境的交互力，包括力和力矩，即：The external force on the robot is mainly the interaction force between the robot and the environment, including force and torque, namely:

F_ext＝[f_{ext_x},f_{ext_y},f_{ext_z},τ_{ext_x},τ_{ext_y},τ_{ext_z}]^T；F_ext = [f_{ext_x} , f_{ext_y} , f_{ext_z} , τ_{ext_x} , τ_{ext_y} , τ_{ext_z} ]^T ;

其中，F_ext是在机器人基坐标系表示的，在本实施例中，F_ext真值由力传感器检测得到，且服从正态分布。Wherein, F_ext is represented in the robot base coordinate system. In this embodiment, the true value of F_ext is detected by a force sensor and obeys a normal distribution.

机器人关节空间动力学方程可以表示为：The dynamic equation of robot joint space can be expressed as:

其中，M_s(q)是机器人连杆惯量矩阵；

是离心力科氏力矢量；G(q)是重力力矩；

为关节摩擦力矩；τ_c为机器人驱动力矩；J_c(q)为机器人末端雅克比矩阵；

为F_ext在机器人关节空间的等效关节力矩；q、

分别为机器人连杆角度位置、速度和加速度。Among them, M_s (q) is the inertia matrix of the robot link;

is the centrifugal force Coriolis force vector; G(q) is the gravitational moment;

is the joint friction torque; τ_c is the driving torque of the robot; J_c (q) is the Jacobian matrix of the robot end;

is the equivalent joint moment of F_ext in the robot joint space; q,

are the angular position, velocity and acceleration of the robot link, respectively.

机器人与环境交互力的末端动力学表示为：The terminal dynamics of the interaction force between the robot and the environment is expressed as:

其中，x为机器人末端位姿，另外，当机器人雅可比矩阵处于奇异状态或者接近奇异状态时，可采用阻尼最小二乘法避开奇异状态，保证机器人关节角速度的连续。Among them, x is the robot end pose. In addition, when the robot Jacobian matrix is in a singular state or close to a singular state, the damped least squares method can be used to avoid the singular state to ensure the continuity of the robot joint angular velocity.

二、建立机器人动力学补偿误差模型：2. Establish a robot dynamic compensation error model:

令

分别为

F_g(q)动力学前馈补偿项，在外力作用下机器人驱动力矩F_c可以表示为：make

respectively

F_g (q) dynamic feedforward compensation term, the robot driving torque F_c under the action of external force can be expressed as:

其中，F_res为包含F_ext的残余有效力。Among them,_Fres is the residual effective force including F_ext .

由于现实中不能完全精确掌握机器人动力学模型参数，前馈补偿项相对实际存有误差，机器人动力学补偿误差e_dyn为：Since the parameters of the robot dynamics model cannot be completely and accurately grasped in reality, the feedforward compensation term has errors relative to the actual situation. The robot dynamics compensation error_edyn is:

e_dyn＝e_ΛU+e_f+e_g；_edyn =e_ΛU +e_f +e_g ;

基于机器人末端动力学和机器人动力学补偿误差，得到机器人动力学补偿误差对机器人外力预估影响的表达式，即：Based on the robot end dynamics and the robot dynamics compensation error, the expression of the influence of the robot dynamics compensation error on the prediction of the robot external force is obtained, namely:

F_ext＝e_dyn-F_res；F_ext =_edyn -F_res ;

需要注意的是，在忽略e_dyn的条件下，F_ext＝-F_res。It should be noted that, under the condition of ignoring_edyn , F_ext =-F_res .

三、建立机器人末端外力预估模型：3. Establish a model for predicting the external force at the end of the robot:

由于机器人动力学补偿误差e_dyn和外力F_ext在机器人与环境交互过程中不能时刻保持常值，因此设定机器人动力学补偿误差e_dyn和外力F_ext服从正态分布的随机变量，两者相互独立。设定机器人动力学补偿误差期望E[e_dyn]＝0，力期望E[F_ext]＝0，机器人动力学补偿误差方差Var[e_dyn]＝Ω_edyn，力方差Var[F_ext]＝Ω_Fext，且e_dyn与F_ext的协方差为零，即

Since the robot dynamic compensation error_edyn and the external force F_ext cannot keep constant values at all times during the interaction between the robot and the environment, the robot dynamic compensation error_{ed dyn} and the external force F_ext are set to obey the random variables of normal distribution, and the two interact with each other. independent. Set robot dynamics compensation error expectation E[_{ed dyn} ]=0, force expectation E[F_ext ]=0, robot dynamics compensation error variance Var[ed_dyn ]=Ω_edyn , force variance Var[F_ext ]=Ω_Fext , and the covariance of_edyn and_Fext is zero, that is

设真值F_ext与预估值

之间的误差为

寻求一标定矩阵Ψ使得方差矩阵Var[ΔF_ext]元素之和最小，即

则ΔF_ext重新定义为：Let the true value F_ext and the estimated value

The error between is

Find a calibration matrix Ψ to minimize the sum of the elements of the variance matrix Var[ΔF_ext ], that is

Then ΔF_ext is redefined as:

ΔF_ext＝F_ext-ΨF_res；ΔF_ext =F_ext −ΨF_res ;

则Var[ΔF_ext]表示为：Then Var[ΔF_ext ] is expressed as:

Var[ΔF_ext]＝E[(F_ext-ΨF_res)(F_ext-ΨF_res)^T]＝Ω_Fext+Ω_FextΨ^T+ΨΩ_Fext+ΨΩ_FextΨ^T+ΨΩ_edynΨ^T；Var[ΔF_ext ]＝E[(F_ext -ΨF_res )(F_ext -ΨF_res )^T ]＝Ω_Fext +Ω_Fext Ψ^T +ΨΩ_Fext +ΨΩ_Fext Ψ^T +ΨΩ_edyn Ψ^T ;

则

but

令

则标定矩阵为：make

Then the calibration matrix is:

Ψ＝-Ω_Fext(Ω_Fext+Ω_edyn)^-1。Ψ=-Ω_Fext (Ω_Fext +Ω_edyn )^-1 .

基于力方差Ω_Fext，机器人动力学补偿误差方差Ω_edyn和残余有效力F_res，建立机器人末端外力预估模型，为：Based on the force variance Ω_Fext , the robot dynamic compensation error variance Ω_edyn and the residual effective force F_res , the robot end external force prediction model is established, which is:

从图2和图3中可以看出，相对Ω_Fext而言，Ψ对Ω_edyn较为敏感，随着Ω_edyn元素值的增大，

的标定矩阵Ψ逐渐丧失对残余有效力F_res进行外力预估的有效性。当Ω_edyn＝C≠0，C为定常值，

相对Ω_Fext变化缓慢，且当Ω_edyn＝0，机器人前馈控制能够准确地对状态变量进行补偿，则Ψ＝-I，

若Ω_Fext＝I，此时，F_ext服从标准的正态分布。It can be seen from Figure 2 and Figure 3 that Ψ is more sensitive to Ω_edyn than Ω_Fext . As the element value of Ω_edyn increases,

The calibration matrix Ψ gradually loses its effectiveness in predicting the residual effective force_Fres . When Ω_edyn =C≠0, C is a constant value,

Relative Ω_Fext changes slowly, and when Ω_edyn = 0, the robot feedforward control can accurately compensate the state variables, then Ψ = -I,

If Ω_Fext =I, at this time,_Fext obeys the standard normal distribution.

若机器人模型误差的期望E[e_dyn]≠0，则会造成最终的

不能近似的等于真值F_ext，其期望值为：If the expected error of the robot model E[e_dyn ]≠0, it will cause the final

is not approximately equal to the true value F_ext , and its expected value is:

E[e_dyn]＝E[F_res]；E[_edyn ]=E[_Fres ];

设偏差d_fe是在机器人模型不准确条件下发生，E[d_fe]＝Ν_bia，Var[d_fe]＝Ω_edyn。因此，此时的外力预估误差ΔF′_ext相对E[e_dyn]＝0情形时，变为：Let the deviation d_fe occur under the condition that the robot model is inaccurate, E[d_fe ]=Ν_bia , Var[d_fe ]=Ω_edyn . Therefore, the estimated external force error_ΔF ′_ext at this time becomes:

ΔF′_ext＝F_ext-Ψ(d_fe-F_ext)＝(I+Ψ)F_ext-Ψd_fe；ΔF′_ext =F_ext -Ψ(d_fe -F_ext )=(I+Ψ)F_ext -Ψd_fe ;

则相应的方差Var[ΔF′_ext]为：Then the corresponding variance Var[ΔF′_ext ] is:

Var[ΔF′_ext]＝(I+Ψ)Ω_Fext(I+Ψ)^T-ΨΩ_edynΨ^T；Var[ΔF′_ext ]=(I+Ψ)Ω_Fext (I+Ψ)^T -ΨΩ_edyn Ψ^T ;

带入标定矩阵Ψ，得：Bringing in the calibration matrix Ψ, we get:

基于外力F_ext和与模型误差相关的力误差期望E[d_fe]＝N_bia，ΔF′_ext新的偏差F_bia可以表示为：Based on the external force F_ext and the force error expectation E[d_fe ]=N_bia related to the model error, the new deviation F_bia of ΔF′_ext can be expressed as:

F_bia＝E[ΔF′_ext]＝Ω_Fext(Ω_Fext+Ω_edyn)^-1N_bia；F_bia =E[ΔF′_ext ]=Ω_Fext (Ω_Fext +Ω_edyn )⁻¹ N_bia ;

若Ω_Fext＝I，Ω_edyn＝0，机器人动力学模型和模型偏差能够准确获得，则F_bia＝N_bia，即外力预估偏差的期望F_bia等于机器人模型偏差相对应的等效力的期望N_bia。If Ω_Fext =I, Ω_edyn =0, the robot dynamics model and the model deviation can be obtained accurately, then F_bia =N_bia , that is, the expectation F_bia of the external force prediction deviation is equal to the equivalent force expectation N corresponding to the robot model deviation_bia .

四、采用粒子群算法标定外力预估模型参数：4. Use the particle swarm algorithm to calibrate the parameters of the external force prediction model:

建立机器人操作空间零力控制模型，为：The zero-force control model of the robot operating space is established as:

其机器人零力控制框图如图3所示。对机器人重力和关节摩擦力进行了补偿，机器人处于零力状态下，外力能够轻松推动机器人。Its robot zero-force control block diagram is shown in Figure 3. The robot's gravity and joint friction are compensated, and the robot is in a zero-force state, and external forces can easily push the robot.

在人手拖动情况下，使机器人尽可能遍历各种构型配置，同时实时采集六维力传感器数据反馈信息F_ext，关节角度信息q，关节角速度信息

和关节扭矩信息τ_c。In the case of human hand dragging, make the robot traverse various configurations as much as possible, and simultaneously collect the six-dimensional force sensor data feedback information F_ext , joint angle information q, and joint angular velocity information in real time

and joint torque information τ_c .

其中，机器人连杆角度q_i(t)与角速度

和机器人驱动力矩τ_c,i(t)是由机器人电机角度θ_i(t)与角速度

和电机扭矩τ_m,i(t)通过伺服系统减速比因子N_ratio,i转换计算得到，即：Among them, the robot link angle_qi (t) and the angular velocity

and the robot driving torque τ_c,i (t) is determined by the robot motor angle θ_i (t) and the angular velocity

and the motor torque τ_m,i (t) are converted and calculated by the servo system reduction ratio factor N_ratio,i , namely:

q_i(t)＝θ_i(t)/N_ratio,i；q_i (t)=θ_i (t)/N_ratio,i ;

τ_c,i(t)＝τ_m,i(t)N_ratio,i，i＝1,…,7。τ_c,i (t)=τ_m,i (t)N_ratio,i , i=1, . . . , 7.

其中粒子群算法适应度函数建立：Among them, the fitness function of particle swarm optimization is established:

寻求最优的Ω_Fext和Ω_edyn，使得在同一个时刻

与F_def的差值的模最小，即：Find the optimal Ω_Fext and Ω_edyn such that at the same time

The modulo of the difference with F_def is the smallest, that is:

另外，机器人模型偏差等效力期望N_bia可能存在，但是在实际中，N_bia是未知的，因此若直接优化外力预估偏差的期望F_bia是不可行的。然而，由于矩阵奇异值分解能表征矩阵与向量之间的特征变化程度，因此定义Α＝Ω_Fext(Ω_Fext+Ω_edyn)^-1，Α＝U_ΑS_ΑV_Α，U_Α∈R^n×n，S_Α∈R^n×n，V_Α∈R^n×n分别为Α的左奇异向量矩阵，奇异值矩阵和右奇异向量矩阵，其中

为最大奇异值

相对应的特征变化方向。若

幅值越大，则相对应F_bia输出的笛卡尔方向越容易受N_bia影响。因此，有必要限制Α的最大奇异值

以此在N_bia的作用下优化Α，保持其最优的各向同性，即：In addition, the expected force N_bia such as robot model deviation may exist, but in practice, N_bia is unknown, so it is not feasible to directly optimize the expected F_bia of external force prediction deviation. However, since the matrix singular value decomposition can characterize the degree of feature change between the matrix and the vector, we define Α=Ω_Fext (Ω_Fext +Ω_edyn )^-1 , Α=U_Α S_Α V_Α , U_Α ∈R^n×n , S_Α ∈R^n×n , V_Α ∈R^n×n are the left singular vector matrix, singular value matrix and right singular vector matrix of Α, respectively, where

is the largest singular value

Corresponding feature change direction. like

The larger the amplitude, the more easily the Cartesian direction of the output corresponding to F_bia is affected by N_bia . Therefore, it is necessary to limit the maximum singular value of A

In this way, A is optimized under the action of N_bia to maintain its optimal isotropy, namely:

综上，结合上述两种优化目标，得出一种综合性指标作为粒子群算法的适应度函数，即：To sum up, combining the above two optimization objectives, a comprehensive index is obtained as the fitness function of particle swarm optimization, namely:

其中，

z₁和z₂分别为优化权重值，z₁＝0.5，z₂＝0.5。in,

z₁ and z₂ are optimization weight values respectively, z₁ =0.5, z₂ =0.5.

其中粒子群算法迭代更新：The particle swarm algorithm iteratively updates:

在粒子群算法中，第i个粒子的位置

和速度

依据粒子自身最优位置

和全局最优位置

在适应度函数f的比较下，自动迭代更新，其更新公式为：In particle swarm optimization, the position of the i-th particle

and speed

According to the optimal position of the particle itself

and the global optimal position

Under the comparison of the fitness function f, it is automatically updated iteratively, and its update formula is:

其中，T_pso为最大迭代次数；D_pso为最大维度数；N_pso为最大粒子数；

分别为第i个粒子在第t次迭代第d维度的位置和速度；

分别为第i个粒子自身最优位置的第d维度位置和全局最优粒子在第d维度的位置；c₁和c₂分别为算法学习因子，常取c₁＝c₂＝2；r₁和r₂为[0,1]的正态分布随机变量；ω_pso为算法惯量权重因子。Among them, T_pso is the maximum number of iterations; D_pso is the maximum number of dimensions; N_pso is the maximum number of particles;

are the position and velocity of the ith particle in the d-th dimension of the t-th iteration, respectively;

are the d-th dimension position of the optimal position of the i-th particle itself and the position of the global optimal particle in the d-th dimension; c₁ and c₂ are the algorithm learning factors, usually c₁ =c₂ =2; r₁ and r₂ are normally distributed random variables in [0,1]; ω_pso is the algorithm inertia weight factor.

另外，采用混沌映射增加种群初始化的多样性，即：In addition, the chaotic map is used to increase the diversity of population initialization, namely:

c_i∈[-1,0)∪(0,1]；

c_i ∈[-1,0)∪(0,1];

其中，

为第i个混沌序列第d维度混沌变量。in,

is the d-th dimension chaotic variable of the i-th chaotic sequence.

混沌对种群初始化后，需要将混沌序列转换为粒子群在初始化时刻的粒子位置，即：After the chaos initializes the population, it is necessary to convert the chaotic sequence into the particle position of the particle swarm at the time of initialization, namely:

其中，

和

分别为第d维度粒子位置搜索的下边界和上边界。in,

and

are the lower and upper boundaries of the particle position search in the d-th dimension, respectively.

为了有效平衡算法全局开发和局部搜索能力，粒子群算法采用指数惯量权重方法，迭代更新惯量权重ω_pso，即：In order to effectively balance the global development and local search capabilities of the algorithm, the particle swarm algorithm adopts the exponential inertia weight method to iteratively update the inertia weight ω_pso , namely:

其中，

分别最大、最小惯量权重因子。in,

Maximum and minimum inertia weighting factors, respectively.

粒子群算法采用种群适应度值方差来判断算法早熟性，增加扰动机制，以此跳出算法局部极小值点。适应度值方差表示为：The particle swarm algorithm uses the variance of the population fitness value to judge the prematurity of the algorithm, and adds a disturbance mechanism to jump out of the local minimum point of the algorithm. The fitness value variance is expressed as:

式中，

是当前迭代种群适应度平均值；f_n＝max(1,max(|f_i-f_avg|))为种群适应度归一化值。In the formula,

is the average fitness of the current iteration population; f_n =max(1,max(|f_i -f_avg |)) is the normalized value of the population fitness.

当ρ²≤[ρ²]，则算法已经陷入局部最优值，全局扰动机制生效。基于混沌映射重新混沌映射产生N_r个粒子，N_r≤N，代入已陷入早熟状态的粒子群中，重新迭代更新优化。When ρ² ≤[ρ² ], the algorithm has fallen into the local optimal value, and the global disturbance mechanism takes effect. Re-chaotic mapping based on chaotic mapping generates N_r particles, N_r ≤N, which are substituted into the particle swarm that has fallen into the precocious state, and iteratively updates and optimizes.

为了离线优化出最优的机器人动力学补偿误差方差Ω_edyn矩阵和力方差Ω_Fext矩阵，可执行多次外力预估模型参数标定试验。基于标定出的机器人动力学补偿误差方差Ω_edyn矩阵和力方差Ω_Fext矩阵，机器人能够在后续与环境交互过程中基于机器人本体传感器信号实时预估其与环境交互的外力F_ext。In order to optimize the optimal robot dynamics compensation error variance Ω_edyn matrix and force variance Ω_Fext matrix offline, multiple external force prediction model parameter calibration experiments can be performed. Based on the calibrated robot dynamic compensation error variance Ω_edyn matrix and force variance Ω_Fext matrix, the robot can estimate the external force F_ext interacting with the environment in real time based on the robot body sensor signal in the subsequent interaction process with the environment.

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。Although embodiments of the present invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, and substitutions can be made in these embodiments without departing from the principle and spirit of the invention and modifications, the scope of the present invention is defined by the appended claims and their equivalents.

Claims (10)

Translated fromChinese

1.一种机器人末端外力预估方法，其特征在于：包括以下步骤：1. a robot end external force estimation method is characterized in that: comprise the following steps:Step1:建立机器人与外力交互的末端动力学模型；Step1: Establish the terminal dynamics model of the interaction between the robot and the external force;Step2:建立机器人动力学补偿误差模型；Step2: establish a robot dynamic compensation error model;Step3:建立机器人末端外力预估模型；Step3: Establish a model for predicting the external force at the end of the robot;Step4:采用粒子群算法标定外力预估模型参数。Step4: Use the particle swarm algorithm to calibrate the parameters of the external force prediction model.2.根据权利要求1所述的一种机器人末端外力预估方法，其特征在于：所述Step3中，机器人末端受到的外力F_ext包括机器人末端与环境的交互力，包括力和力矩，即：2. a kind of robot end external force estimation method according to claim 1, is characterized in that: in described Step3, the external force F_ext that robot end receives comprises the interaction force of robot end and environment, comprises force and moment, namely:F_ext＝[f_{ext_x},f_{ext_y},f_{ext_z},τ_{ext_x},τ_{ext_y},τ_{ext_z}]^T；F_ext = [f_{ext_x} , f_{ext_y} , f_{ext_z} , τ_{ext_x} , τ_{ext_y} , τ_{ext_z} ]^T ;其中，F_ext为基于机器人基坐标系表示机器人末端受到的外力F_ext矩阵。Among them, F_ext is the F_ext matrix representing the external force received by the robot end based on the robot base coordinate system.3.根据权利要求1所述的一种机器人末端外力预估方法，其特征在于：所述Step3中，机器人末端动力学模型由机器人关节空间动力学模型通过末端雅克比矩阵J_c(q)转换而来，机器人关节空间动力学模型为：3. a kind of robot end external force estimation method according to claim 1, is characterized in that: in described Step3, robot end dynamics model is converted by end Jacobian matrix J_c (q) by robot joint space dynamics model Then, the robot joint space dynamics model is:

其中，M_s(q)是机器人连杆惯量矩阵；

是离心力科氏力矢量；G(q)是机器人重力力矩；

为关节摩擦力矩；τ_c为机器人驱动力矩；

为F_ext在机器人关节空间的等效关节力矩；q、

分别为机器人连杆角度位置、速度和加速度。Among them, M_s (q) is the inertia matrix of the robot link;

is the centrifugal force Coriolis force vector; G(q) is the gravitational moment of the robot;

is the joint friction torque; τ_c is the driving torque of the robot;

is the equivalent joint moment of F_ext in the robot joint space; q,

are the angular position, velocity and acceleration of the robot link, respectively.4.根据权利要求1所述的一种机器人末端外力预估方法，其特征在于：所述Step1中，机器人与外力的末端动力学为：4. a kind of robot terminal external force estimation method according to claim 1, is characterized in that: in described Step1, the terminal dynamics of robot and external force is:

其中，x为机器人末端位姿；

Among them, x is the robot end pose;

其中，Λ_s(q)为操作空间惯量矩阵；

为操作空间中的摩擦力；

为操作空间中的科氏力；F_g(q)为操作空间中的重力；F_c为操作空间中的关节驱动力；

为机器人末端雅可比矩阵的加权广义逆矩阵；Among them, Λ_s (q) is the operation space inertia matrix;

is the frictional force in the operating space;

is the Coriolis force in the operating space; F_g (q) is the gravity in the operating space; F_c is the joint driving force in the operating space;

is the weighted generalized inverse of the Jacobian matrix at the end of the robot;当机器人雅可比矩阵处于奇异状态或者接近奇异状态时，

可采用阻尼最小二乘法避开奇异状态，保证机器人关节角速度连续。When the robot Jacobian matrix is in a singular state or close to a singular state,

The damped least squares method can be used to avoid the singular state and ensure the continuous angular velocity of the robot joints.5.根据权利要求1所述的一种机器人末端外力预估方法，其特征在于：所述Step2中，机器人动力学补偿误差模型用于分析机器人末端动力学补偿误差因素对外力预估的影响；5. a kind of robot end external force estimation method according to claim 1, is characterized in that: in described Step2, the robot dynamics compensation error model is used to analyze the influence of robot end dynamics compensation error factor to the external force estimation;令

分别为

F_g(q)动力学前馈补偿项，在外力作用下所述机器人驱动力矩F_c可以表示为：make

respectively

F_g (q) dynamic feedforward compensation term, the robot driving torque F_c under the action of external force can be expressed as:

其中，F_res为包含F_ext的残余有效力。Among them,_Fres is the residual effective force including F_ext .6.根据权利要求1所述的一种机器人末端外力预估方法，其特征在于：所述Step2中，由于现实中不能完全精确掌握机器人动力学模型参数，前馈补偿项相对实际存有误差，机器人动力学补偿误差e_dyn为：6. a kind of robot end external force estimation method according to claim 1, is characterized in that: in described Step2, because the robot dynamics model parameter cannot be completely and accurately grasped in reality, the feedforward compensation term has an error relative to reality, The robot dynamic compensation error_edyn is:e_dyn＝e_ΛU+e_f+e_g；_edyn =e_ΛU +e_f +e_g ;其中，

in,

其中e_ΛU为操作空间惯性力和科氏力的补偿误差；e_f为操作空间摩擦力的补偿误差；e_g为操作空间重力的补偿误差；where e_ΛU is the compensation error of the inertial force and Coriolis force in the operating space; e_f is the compensation error of the friction force in the operating space; e_g is the compensation error of the gravity in the operating space;基于所述机器人的末端动力学和所述机器人的动力学补偿误差，得到机器人动力学补偿误差对机器人外力预估影响的表达式，即：Based on the terminal dynamics of the robot and the dynamic compensation error of the robot, the expression of the influence of the robot dynamic compensation error on the prediction of the external force of the robot is obtained, namely:F_ext＝e_dyn-F_res；F_ext =_edyn -F_res ;其中，F_ext为需要预估的外力；e_dyn为动力学补偿误差；F_res为参与有效力；Among them, F_ext is the external force that needs to be estimated;_edyn is the dynamic compensation error;_Fres is the effective participation force;所述机器人的末端外力预估模型是用于在无力传感器条件下，基于机器人本体传感器信号包括电机角度位置、电机角速度和电机扭矩，实时预估机器人末端与环境发生接触的力。The robot end external force prediction model is used to estimate the force of the robot end contacting the environment in real time based on the robot body sensor signal including the motor angular position, the motor angular velocity and the motor torque under the condition of a powerless sensor.7.根据权利要求6所述的一种机器人末端外力预估方法，其特征在于：所述机器人动力学补偿误差e_dyn和所述外力F_ext在机器人与环境交互过程中不能时刻保持常值，设定所述机器人动力学补偿误差e_dyn和所述外力F_ext服从正态分布的随机变量，两者相互独立；7. a kind of robot end external force estimation method according to claim 6, is characterized in that: described robot dynamics compensation error_edyn and described external force F_ext cannot keep constant value all the time in robot and environment interaction process, Setting the robot dynamic compensation error_edyn and the external force F_ext to random variables that obey a normal distribution, and the two are independent of each other;设定机器人动力学补偿误差期望E[e_dyn]＝0，力期望E[F_ext]＝0，机器人动力学补偿误差方差Var[e_dyn]＝Ω_edyn，力方差Var[F_ext]＝Ω_Fext，且e_dyn与F_ext的协方差为零，即

Set robot dynamic compensation error expectation E[_{ed dyn} ]=0, force expectation E[F_ext ]=0, robot dynamic compensation error variance Var[ed_dyn ]=Ω_edyn , force variance Var[F_ext ]=Ω_Fext , and the covariance of_edyn and_Fext is zero, that is

基于所述力方差Ω_Fext，机器人动力学补偿误差方差Ω_edyn和残余有效力F_res，建立所述机器人末端外力预估模型，为：Based on the force variance Ω_Fext , the robot dynamic compensation error variance Ω_edyn and the residual effective force F_res , the robot end external force prediction model is established, which is:

其中，

为所述外力F_ext的预估值。in,

is the estimated value of the external force F_ext .8.根据权利要求7所述的一种机器人末端外力预估方法，其特征在于：若所述机器人动力模型存在偏差d_fe，服从正态分布，其期望E[d_fe]＝Ν_bia，方差Var[d_fe]＝Ω_edyn，则所述机器人末端外力预估模型预估出的外力存在偏差F_bia，即：8. The method for estimating external force at the end of a robot according to claim 7, characterized in that: if the robot dynamic model has a deviation d_fe , and obeys a normal distribution, its expectation is E[d_fe ]=N_bia , the variance Var[d_fe ]=Ω_edyn , then the external force estimated by the robot end external force prediction model has a deviation F_bia , namely:F_bia＝Ω_Fext(Ω_Fext+Ω_edyn)^-1N_bia。F_bia =Ω_Fext (Ω_Fext +Ω_edyn )⁻¹ N_bia .9.根据权利要求1所述的一种机器人末端外力预估方法，其特征在于：所述外力预估模型参数包括所述机器人动力学补偿误差方差Ω_edyn矩阵和力方差Ω_Fext矩阵，利用粒子群算法标定外力预估模型参数，步骤为：9. a kind of robot end external force estimation method according to claim 1, is characterized in that: described external force estimation model parameter comprises described robot dynamics compensation error variance Ω_edyn matrix and force variance Ω_Fext matrix, utilizes particle. The swarm algorithm calibrates the parameters of the external force prediction model. The steps are:A.建立机器人操作空间零力控制模型

A. Establish a zero-force control model of the robot operating space

B.人手低速拖动机器人末端执行器，实时采集机器人电机角度θ_i(t)、电机角速度

电机扭矩τ_m,i(t)和传感器力信息F_ext(t)，采用粒子群优化算法离线优化Ω_Fext和Ω_edyn，使得基于机器人本体传感器信息，所述机器人末端外力预估模型能够准确预估外力F_ext；B. The human hand drags the robot end effector at low speed, and collects the robot motor angle θ_i (t) and motor angular velocity in real time

Motor torque τ_m,i (t) and sensor force information F_ext (t), particle swarm optimization algorithm is used to optimize Ω_Fext and Ω_{edyn offline} , so that based on the sensor information of the robot body, the robot end external force prediction model can accurately predict. Estimate the external force F_ext ;所述机器人操作空间零力控制模型是由关节空间机器人零力控制模型通过末端雅可比矩阵转换得到，并对机器人重力和关节摩擦力进行补偿；The zero-force control model of the robot operation space is obtained by transforming the zero-force control model of the joint space robot through the end Jacobian matrix, and compensates the robot gravity and joint friction;所述机器人连杆角度q_i(t)与角速度

和所述机器人驱动力矩τ_c,i(t)是由所述机器人电机角度θ_i(t)与角速度

和所述电机扭矩τ_m,i(t)通过伺服系统减速比因子N_ratio,i转换计算得到，即：The robot link angle_qi (t) and angular velocity

and the robot driving torque τ_c,i (t) is determined by the robot motor angle θ_i (t) and the angular velocity

And the motor torque τ_m,i (t) is calculated by the reduction ratio factor N_ratio,i of the servo system, namely:q_i(t)＝θ_i(t)/N_ratio,i；q_i (t)=θ_i (t)/N_ratio,i ;

τ_c,i(t)＝τ_m,i(t)N_ratio,i，i＝1,…,n；τ_c,i (t)=τ_m,i (t)N_ratio,i , i=1,...,n;其中，n为机器人关节自由度。Among them, n is the degree of freedom of the robot joint.10.根据权利要求9所述的一种机器人末端外力预估方法，其特征在于：所述粒子群算法能够避免算法早熟及陷入局部最优值，其中，所述粒子群算法的适应度函数定义为：10 . The method for estimating external force at the end of a robot according to claim 9 , wherein the particle swarm algorithm can avoid algorithm prematurity and falling into a local optimum, wherein the fitness function of the particle swarm algorithm defines the for:

其中，

为外力预估误差、

为外力预估矩阵因子的最大奇异值、z₁和z₂分别为优化权重值，且z₁+z₂＝1，z₁＞0，z₂＞0。in,

Estimate error for external force,

The maximum singular value of the external force prediction matrix factor, z₁ and z₂ are the optimization weight values, respectively, and z₁ +z₂ =1, z₁ >0, and z₂ >0.

Images