WO2021254414A1 - 一种基于力反馈的机器人实时运动规划方法 - Google Patents

一种基于力反馈的机器人实时运动规划方法 Download PDF

Info

Publication number
WO2021254414A1
WO2021254414A1 PCT/CN2021/100482 CN2021100482W WO2021254414A1 WO 2021254414 A1 WO2021254414 A1 WO 2021254414A1 CN 2021100482 W CN2021100482 W CN 2021100482W WO 2021254414 A1 WO2021254414 A1 WO 2021254414A1
Authority
WO
WIPO (PCT)
Prior art keywords
correction amount
periodic
force
attitude
interpolation
Prior art date
Application number
PCT/CN2021/100482
Other languages
English (en)
French (fr)
Inventor
张加波
徐建萍
赵长喜
刘净瑜
乐毅
Original Assignee
北京卫星制造厂有限公司
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by 北京卫星制造厂有限公司 filed Critical 北京卫星制造厂有限公司
Publication of WO2021254414A1 publication Critical patent/WO2021254414A1/zh

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control

Definitions

  • the invention relates to a robot real-time motion planning method based on force feedback, and belongs to the technical field of control engineering.
  • Robot is a large-load, high-precision automated mechanical system, and it is increasingly used in processing and assembly fields.
  • traditional teaching is difficult to meet the accuracy requirements, time-consuming, and poor environmental adaptability.
  • the robot active compliance control system based on force feedback is composed of a robot arm, a robot arm control cabinet, a force sensor installed at the end of the robot arm, and a host computer control system. It can follow the external force for flexible movement and realize the flexibility of the end parts of the robot arm. adjust.
  • the traditional linear mapping method is used to map the external force and the period posture correction of the manipulator.
  • the posture period correction amount obtained through linear mapping is small, and the manipulator can start and run smoothly; when the external force is large, the posture period correction amount obtained is larger, which not only causes the mechanical arm to start up. Oscillation and acceleration/decelerator torque exceed the rated value and stop due to failures.
  • the force sensor has data collection fluctuations due to mechanical oscillations, and the robustness of the entire system is poor.
  • the acceleration/deceleration torque ratings of the three coordinate axes are different.
  • the external force is decomposed into the X, Y, and Z axes of the sensor coordinate system, and the force ratio of each axis is obtained through linear mapping to obtain the corresponding axis period posture correction. Then, in a certain force value area, the three axes cannot perform smooth acceleration and deceleration in accordance with the predetermined proportion.
  • the technical problem solved by the present invention is: in view of the current prior art, in the process of performing compliance force follow-up control of the manipulator by adopting the piecewise linear mapping of the force on the end of the manipulator and the periodic posture correction amount, the motion shock and the failure shutdown that are prone to occur , Relying solely on the theoretical model without considering the actual rated torque constraints of each axis of the manipulator, and proposed a real-time motion planning method for the robot based on force feedback.
  • a method of robot real-time motion planning based on force feedback the steps are as follows:
  • the six-dimensional force sensor is used for data collection. After gravity compensation and coordinate transformation, the component forces f X , f Y , f Z of the X, Y, and Z axes in the base coordinate system of the manipulator are obtained, and according to the component forces f X , f Y , f Z obtain the external force f in the base coordinate system of the manipulator;
  • step (1) According to the external force f obtained in step (1), establish the segmented mapping relationship f (f, v a , f 1 , f 2 ) between the external force and the periodic posture target correction amount in the base coordinate system of the manipulator;
  • Step (2) The resulting mapping relationship f (f, v a, f 1, f 2) determined period correction speed v a, according to step (1) the force f X score, f Y, f Z and an external The ratio of the acting force f is mapped to the X, Y, and Z axes to obtain the mapping relationship between each axis force and the periodic attitude target correction amount f 1 (f, f x , v ax ), f 2 (f, f Y , v aY ), f 3 (f, f Z , v aZ );
  • the component force f X , f Y , and f Z in step (1) calculate the periodic posture of the X, Y, and Z axes Correction amount interpolation step base v bX , v bY , v bZ ;
  • step (6) Judge the cumulative interpolation correction amount of the current periodic attitude of the three axes obtained in step (6). If the cumulative interpolation correction amount of the current periodic attitude of each axis is greater than the target correction amount of the periodic attitude of the axis, the number of interpolation steps will follow The running period gradually decreases to the real-time periodic attitude target correction amount, otherwise it gradually increases to the real-time periodic attitude target correction amount.
  • the X and Y axes of the six-dimensional force sensor coordinate system and the robotic arm tool coordinate system are respectively coincident, and the calculation method of the external force is: using the six-dimensional force sensor measured values in the base coordinate system It is obtained by subtracting the load gravity at the end of the robotic arm and the static error value of the six-dimensional force sensor.
  • mapping relationship f(f, v a , f 1 , f 2 ) is specifically:
  • k1, k2, k3 are the force position conversion scale factors
  • f 1 and f 2 are the function segment data thresholds
  • v a is the periodic correction speed
  • step (1) If the external force f is less than the data calculation threshold f 1 , then discard the value and return to step (1) to collect the next set of data, otherwise, establish force-position mapping conversion according to the mapping relationship in step (2).
  • mapping relationships f 1 (f, f x , v ax ), f 2 (f, f Y , v aY ), f 3 (f, f Z , v aZ ) are specifically:
  • the X-axis periodic attitude target correction amount is:
  • the Y-axis periodic attitude target correction amount is:
  • the Z-axis periodic attitude target correction amount is:
  • the calculation method of the interpolating step bases v bX , v bY , v bZ of the periodic attitude correction amount of the X, Y, and Z axes is specifically as follows:
  • the periodic posture correction amount interpolation step base segment function in the X-axis direction is as follows:
  • the periodic posture correction amount interpolation step base piecewise function in the Y-axis direction is as follows:
  • the periodic posture correction amount interpolation step base segment function in the Z-axis direction is as follows:
  • the calculation method of the three-axis current cycle attitude cumulative interpolation correction amount v sX , v sY , v sZ is:
  • the method for the cumulative interpolation correction amount of the current period attitude to gradually increase or decrease with the operation period is specifically as follows:
  • the real-time periodic attitude cumulative interpolation correction amount in the X-axis direction is specifically:
  • the real-time periodic attitude cumulative interpolation correction amount in the Y-axis direction is specifically:
  • the real-time periodic attitude cumulative interpolation correction amount in the Z-axis direction is specifically:
  • the invention provides a robot real-time motion planning method based on force feedback, which uses a six-dimensional force sensor to sense external forces, establishes a three-segment function of force and periodic posture target correction, and sets a force control trigger threshold. Obtain the periodic attitude target correction amount of each axis through function mapping and compare the minimum value of each axis and its external force.
  • Figure 1 is a flow chart of the motion planning method provided by the invention.
  • a real-time motion planning method for robots based on force feedback Aiming at the problem that the manipulator cannot move smoothly in the case of a large period of posture correction, it modifies the existing piecewise linear force position control method, as shown in Figure 1. The specific steps are:
  • the six-dimensional force sensor is used for data collection. After gravity compensation and coordinate transformation, the component forces f X , f Y , f Z of the X, Y, and Z axes in the base coordinate system of the manipulator are obtained, and according to the component forces f X , f Y , f Z obtain the external force f in the base coordinate system of the manipulator;
  • the six-dimensional force sensor coordinate system coincides with the X and Y axes of the robotic arm tool coordinate system, respectively, and the calculation method of the external force is: using the six-dimensional force sensor measured value in the base coordinate system to subtract the load gravity at the end of the robotic arm, After obtaining the static error value of the six-dimensional force sensor, the origin of the base coordinate system of the manipulator is generally at the foot of the manipulator, and the Z axis is vertically upward. The origin of the tool coordinate system is generally located at the end flange of the robot arm, and the Z axis is perpendicular to the outside. Both coordinate systems are factory-defined coordinate systems;
  • step (1) According to the external force f obtained in step (1), establish the segmented mapping relationship f (f, v a , f 1 , f 2 ) between the external force and the periodic posture target correction amount in the base coordinate system of the manipulator;
  • mapping relationship f(f, v a , f 1 , f 2 ) is specifically:
  • k1, k2, k3 are the force position conversion scale factors
  • f 1 and f 2 are the function segment data thresholds
  • v a is the periodic correction speed
  • step (1) If the external force f is less than the data calculation threshold f 1 , discard the value and return to step (1) to collect the next set of data; otherwise, establish the force position mapping conversion according to the mapping relationship in step (2) and enter step (3);
  • Step (2) The resulting mapping relationship f (f, v a, f 1, f 2) determined period correction speed v a, according to step (1) the force f X score, f Y, f Z and an external The ratio of the acting force f is mapped to the X, Y, and Z axes to obtain the mapping relationship between each axis force and the periodic attitude target correction amount f 1 (f, f x , v ax ), f 2 (f, f Y , v aY ), f 3 (f, f Z , v aZ ), where:
  • mapping relations f 1 (f, f x , v ax ), f 2 (f, f Y , v aY ), and f 3 (f, f Z , v aZ ) are specifically:
  • the X-axis periodic attitude target correction amount is:
  • the Y-axis periodic attitude target correction amount is:
  • the Z-axis periodic attitude target correction amount is:
  • the component force f X , f Y , and f Z in step (1) calculate the periodic posture of the X, Y, and Z axes Correction amount interpolation step base v bX , v bY , v bZ ;
  • the calculation method of the periodic attitude correction amount interpolation step base v bX , v bY , v bZ of the X, Y, and Z axes is specifically as follows:
  • the periodic posture correction amount interpolation step base segment function in the X-axis direction is as follows:
  • the periodic posture correction amount interpolation step base piecewise function in the Y-axis direction is as follows:
  • the periodic posture correction amount interpolation step base segment function in the Z-axis direction is as follows:
  • the calculation method of the three-axis current cycle attitude cumulative interpolation correction amount v sX , v sY , v sZ is:
  • step (6) Judge the cumulative interpolation correction amount of the current periodic attitude of the three axes obtained in step (6). If the cumulative interpolation correction amount of the current periodic attitude of each axis is greater than the target correction amount of the periodic attitude of the axis, the number of interpolation steps will follow The operating period gradually decreases to the real-time periodic attitude target correction amount, otherwise it gradually increases to the real-time periodic attitude target correction amount, specifically:
  • the real-time periodic attitude cumulative interpolation correction amount in the X-axis direction is specifically:
  • the real-time periodic attitude cumulative interpolation correction amount in the Y-axis direction is specifically:
  • the real-time periodic attitude cumulative interpolation correction amount in the Z-axis direction is specifically:
  • the periodical posture correction amount interpolation step base segment function in the X-axis, Y-axis, and Z-axis directions is as follows:
  • the cumulative interpolation steps Step X , Step Y , Step Z of each axis per cycle and the interpolation step base number v bX of the attitude correction amount per cycle of each axis, v bY , v bZ are respectively multiplied to obtain the current periodic attitude cumulative interpolation correction amount of each axis v sX , v sY , v sZ , in the process of fitting and approaching the periodic attitude target correction amount, first cumulative interpolation of the periodic attitude of each axis The correction amount and the corresponding periodic attitude target correction amount are judged on the magnitude relationship, and then it is judged whether the cumulative interpolation correction amount of the periodic attitude of each axis is gradually increasing or decreasing.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

一种基于力反馈的机器人实时运动规划方法，针对在较大周期姿态修正量下，机械臂不能平滑加/减速运动的问题，修正了现有的分段线性力位控制方法。采用六维力传感器感知外部作用力，获取坐标系各轴每周期姿态目标修正量；并通过各坐标轴稳定运行时的最大周期姿态修正量增量确定各轴每周期姿态修正量插补步幅基数，最后通过力位转换、插补逼近各轴的周期姿态目标修正量，避免了机械臂启动或停止时，运动状态的改变导致的震荡，实现机械臂力控过程中的平滑运动。

Description

一种基于力反馈的机器人实时运动规划方法
本申请要求于2020年06月18日提交中国专利局、申请号为202010560487.4、申请名称为“一种基于力反馈的机器人实时运动规划方法”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本发明涉及一种基于力反馈的机器人实时运动规划方法，属于控制工程技术领域。
背景技术
机器人是一种大承载、高精度的自动化机械系统，并越来越多的应用到加工及装配领域。但在某些加工装配领域，传统的示教难以满足精度要求、耗时长、环境适应性差。基于力反馈的机器人主动柔顺控制系统由机械臂、机械臂控制柜、安装于机械臂末端的力传感器以及上位机控制系统组成，能够跟随外部作用力进行柔性运动，实现机械臂末端部件姿态的灵活调整。
在机器人柔顺力控制研究中发现：为了实现机械臂的运行速度大小与外部作用力的大小正相关，采用传统的线性映射方法，将外部作用力与机械臂周期姿态修正量进行映射。当外部作用力较小时，通过线性映射得到的姿态周期修正量较小，机械臂可以平稳启动并运行；当外部作用力较大时，得到的姿态周期修正量较大，不仅导致机械臂启动出现震荡以及加/减速器力矩超过额定值而故障停机，同时力传感器由于机械震荡出现数据采集波动，整个系统的鲁棒性较差。
同时，三个坐标轴的加/减速力矩额定值不同。将外部作用力分解到传感器坐标系的X、Y、Z轴，得到各个轴的受力比分别通过线性映射得到对应的轴周期姿态修正量。那么，在一定的作用力取值区域，三个轴不能按照既定占 比进行运动平滑加、减速。
发明内容
本发明解决的技术问题是：针对目前现有技术中，采用机械臂末端受力与周期姿态修正量分段线性映射对机械臂进行柔顺力随动控制过程中，容易出现的运动震荡以及故障停机，单纯依赖理论模型未考虑机械臂各轴的实际额定力矩约束的问题，提出了一种基于力反馈的机器人实时运动规划方法。
本发明解决上述技术问题是通过如下技术方案予以实现的：
一种基于力反馈的机器人实时运动规划方法，步骤如下：
(1)利用六维力传感器进行数据采集，经重力补偿及坐标变换后获取机械臂基坐标系下X，Y，Z轴的分作用力f X，f Y，f Z，并根据分作用力f X，f Y，f Z获取机械臂基坐标系下的外部作用力f；
(2)根据步骤(1)所得外部作用力f建立外部作用力与机械臂基坐标系下周期姿态目标修正量分段映射关系f(f、v a、f 1、f 2)；
(3)根据步骤(2)所得映射关系f(f、v a、f 1、f 2)确定周期修正速度v a，根据步骤(1)所得分作用力f X，f Y，f Z与外部作用力f的比值，分别映射至X、Y、Z轴上，获取各轴力与周期姿态目标修正量的映射关系f 1(f、f x、v ax)、f 2(f、f Y、v aY)、f 3(f、f Z、v aZ)；
(4)确定机械臂基坐标系下，机械臂三轴各自稳定运行情况下周期姿态修正量增量Δv X，Δv Y，Δv Z，选取最小值Δv并确定对应轴的外部作用力值f A；
(5)根据步骤(4)中选定的最小值Δv及其外部作用力值f A、步骤(1)中分作用力f X，f Y，f Z计算X、Y、Z轴的周期姿态修正量插补步幅基数v bX，v bY，v bZ；
(6)根据各轴每周期累计插补步数Step X，Step Y，Step Z与步骤(5)所得周期姿态修正量插补步幅基数计算三轴当前周期姿态累积插补修正量v sX， v sY，v sZ；
(7)对步骤(6)所得三轴当前周期姿态累积插补修正量分别进行判断，若各轴当前周期姿态累积插补修正量大于该轴周期姿态目标修正量，则通过插补步数随运行周期逐步递减至实时周期姿态目标修正量，否则逐步递增至实时周期姿态目标修正量。
所述步骤(1)中，所述六维力传感器坐标系与机械臂工具坐标系X、Y轴分别重合，所述外部作用力的计算方法为：利用基坐标系下六维力传感器测量值减去机械臂末端负载重力、六维力传感器传感器静态误差值后获得。
所述步骤(2)中，映射关系f(f、v a、f 1、f 2)具体为：
Figure PCTCN2021100482-appb-000001
式中，k1，k2，k3分别为力位转换比例因子，f 1，f 2分别为函数分段数据阈值，v a为周期修正速度，其中：
若外部作用力f小于数据计算阈值f 1，则舍弃该数值并返回步骤(1)重新采集下一组数据，否则根据步骤(2)中映射关系建立力位映射转换。
所述步骤(3)中，映射关系f 1(f、f x、v ax)、f 2(f、f Y、v aY)、f 3(f、f Z、v aZ)具体为：
X轴周期姿态目标修正量为：
Figure PCTCN2021100482-appb-000002
Y轴周期姿态目标修正量为：
Figure PCTCN2021100482-appb-000003
Z轴周期姿态目标修正量为：
Figure PCTCN2021100482-appb-000004
所述步骤(5)中，X、Y、Z轴的周期姿态修正量插补步幅基数v bX，v bY，v bZ的计算方法具体为：
X轴方向的周期姿态修正量插补步幅基数分段函数如下：
Figure PCTCN2021100482-appb-000005
Y轴方向的周期姿态修正量插补步幅基数分段函数如下：
Figure PCTCN2021100482-appb-000006
Z轴方向的周期姿态修正量插补步幅基数分段函数如下：
Figure PCTCN2021100482-appb-000007
所述步骤(6)中，三轴当前周期姿态累积插补修正量v sX，v sY，v sZ的计算方法为：
X轴方向当前周期姿态累积插补修正量：v sX＝v bX*Step X
Y轴方向当前周期姿态累积插补修正量：v sY＝v bY*Step Y
Z轴方向当前周期姿态累积插补修正量：v sz＝v bz*Step Z。
所述步骤(7)中，当前周期姿态累积插补修正量随运行周期逐步递增或递减的方法具体为：
(7a)确定三轴各自当前周期姿态修正目标插补步数，具体为：
Figure PCTCN2021100482-appb-000008
(7b)于逐步递增或递减，拟合逼近周期姿态目标修正量的过程中，X轴方向的实时周期姿态累积插补修正量具体为：
Figure PCTCN2021100482-appb-000009
v sX＝Step X'*v bX
Y轴方向的实时周期姿态累积插补修正量具体为：
Figure PCTCN2021100482-appb-000010
v sY＝Step Y'*v bY
Z轴方向的实时周期姿态累积插补修正量具体为：
Figure PCTCN2021100482-appb-000011
本发明与现有技术相比的优点在于：
本发明提供的一种基于力反馈的机器人实时运动规划方法，采用六维力传感器感知外部作用力，建立了力与周期姿态目标修正量的三段分段函数，并设定力控触发阈值，通过函数映射获取各轴周期姿态目标修正量并比较获得各轴中的最小值及其外部作用力，在此基础上根据三轴的外部作用力相互关系获取各轴的周期姿态修正量插补步幅基数，并按照各自修正插补步幅进行逼近拟合加速，实现了机械臂的平滑加速、减速，能够在较大周期姿态修正量下机械臂力控过程中的平滑运动，不仅消除了力传感器因为加速度较大突变导致的数据采集噪声，同时实现了加工装配的稳定性，提高了加工装配质量。
附图说明
图1为发明提供的运动规划方法流程图；
具体实施方式
一种基于力反馈的机器人实时运动规划方法，针对在较大周期姿态修正量 情况下，机械臂不能平滑运动的问题，修正了现有的分段线性力位控制方法，如图1所示，具体步骤为：
(1)利用六维力传感器进行数据采集，经重力补偿及坐标变换后获取机械臂基坐标系下X，Y，Z轴的分作用力f X，f Y，f Z，并根据分作用力f X，f Y，f Z获取机械臂基坐标系下的外部作用力f；
其中，六维力传感器坐标系与机械臂工具坐标系X、Y轴分别重合，所述外部作用力的计算方法为：利用基坐标系下六维力传感器测量值减去机械臂末端负载重力、六维力传感器传感器静态误差值后获得，机械臂基坐标系原点一般位于机械臂足部，Z轴垂直向上。工具坐标系的原点一般位于机械臂末端法兰，Z轴垂直向外。两个坐标系均为出厂已定义坐标系；
(2)根据步骤(1)所得外部作用力f建立外部作用力与机械臂基坐标系下周期姿态目标修正量分段映射关系f(f、v a、f 1、f 2)；
其中，映射关系f(f、v a、f 1、f 2)具体为：
Figure PCTCN2021100482-appb-000012
式中，k1，k2，k3分别为力位转换比例因子，f 1，f 2分别为函数分段数据阈值，v a为周期修正速度，其中：
若外部作用力f小于数据计算阈值f 1，则舍弃该数值并返回步骤(1)重新采集下一组数据，否则根据步骤(2)中映射关系建立力位映射转换并进入步骤(3)；
(3)根据步骤(2)所得映射关系f(f、v a、f 1、f 2)确定周期修正速度v a，根据步骤(1)所得分作用力f X，f Y，f Z与外部作用力f的比值，分别映射至X、Y、Z轴上，获取各轴力与周期姿态目标修正量的映射关系f 1(f、f x、v ax)、f 2(f、f Y、v aY)、f 3(f、f Z、v aZ)，其中：
映射关系f 1(f、f x、v ax)、f 2(f、f Y、v aY)、f 3(f、f Z、v aZ)具体为：
X轴周期姿态目标修正量为：
Figure PCTCN2021100482-appb-000013
Y轴周期姿态目标修正量为：
Figure PCTCN2021100482-appb-000014
Z轴周期姿态目标修正量为：
Figure PCTCN2021100482-appb-000015
(4)确定机械臂基坐标系下，机械臂三轴各自稳定运行情况下周期姿态修正量增量Δv X，Δv Y，Δv Z，选取最小值Δv并确定对应轴的外部作用力值f A；
(5)根据步骤(4)中选定的最小值Δv及其外部作用力值f A、步骤(1)中分作用力f X，f Y，f Z计算X、Y、Z轴的周期姿态修正量插补步幅基数v bX，v bY，v bZ；
其中，X、Y、Z轴的周期姿态修正量插补步幅基数v bX，v bY，v bZ的计算方法具体为：
X轴方向的周期姿态修正量插补步幅基数分段函数如下：
Figure PCTCN2021100482-appb-000016
Y轴方向的周期姿态修正量插补步幅基数分段函数如下：
Figure PCTCN2021100482-appb-000017
Z轴方向的周期姿态修正量插补步幅基数分段函数如下：
Figure PCTCN2021100482-appb-000018
(6)根据各轴每周期累计插补步数Step X，Step Y，Step Z与步骤(5)所得周期姿态修正量插补步幅基数计算三轴当前周期姿态累积插补修正量v sX，v sY，v sZ；
三轴当前周期姿态累积插补修正量v sX，v sY，v sZ的计算方法为：
X轴方向当前周期姿态累积插补修正量：v sX＝v bX*Step X
Y轴方向当前周期姿态累积插补修正量：v sY＝v bY*Step Y
Z轴方向当前周期姿态累积插补修正量：v sz＝v bz*Step Z。
(7)对步骤(6)所得三轴当前周期姿态累积插补修正量分别进行判断，若各轴当前周期姿态累积插补修正量大于该轴周期姿态目标修正量，则通过插补步数随运行周期逐步递减至实时周期姿态目标修正量，否则逐步递增至实时周期姿态目标修正量，具体为：
(7a)确定三轴各自当前周期姿态修正目标插补步数，具体为：
Figure PCTCN2021100482-appb-000019
(7b)于逐步递增或递减，拟合逼近周期姿态目标修正量的过程中，X轴方向的实时周期姿态累积插补修正量具体为：
Figure PCTCN2021100482-appb-000020
v sX＝Step X'*v bX
Y轴方向的实时周期姿态累积插补修正量具体为：
Figure PCTCN2021100482-appb-000021
v sY＝Step Y'*v bY
Z轴方向的实时周期姿态累积插补修正量具体为：
Figure PCTCN2021100482-appb-000022
v sZ＝Step Z'*v bZ
下面结合具体实施例进行进一步说明：
对六维力传感器进行数据采集，通过重力补偿及坐标转换，获取机械臂基坐标系下X，Y，Z轴的分作用力f X，f Y，f Z，并根据分作用力f X，f Y，f Z获取机械臂基坐标系下的外部作用力f。
建立外部作用力f与机械臂基坐标系下周期姿态目标修正量分段映射关系，f(f、v a、f 1、f 2)；
计算周期修正速度v a，按照步骤(1)中获得的三个轴的作用力f X，f Y，f Z与合作用力f的比值关系，分别映射到三轴，得到三轴与周期姿态目标修正量的映射关系f 1(f、f x、v ax)、f 2(f、f Y、v aY)、f 3(f、f Z、v aZ)；
确定机械臂X，Y，Z轴各自均能稳定运行的周期姿态修正量增量Δv X，Δv Y，Δv Z，三者中的最小值Δv以及对应轴的外部作用力值f A；在本实施例中，Δv Y最小；
计算三轴的作用力大小建立三轴的周期姿态修正量插补步幅基数v bX，v bY，v bZ，其中：
X轴、Y轴、Z轴方向的周期姿态修正量插补步幅基数分段函数如下：
Figure PCTCN2021100482-appb-000023
v bY＝Δv Y
Figure PCTCN2021100482-appb-000024
在上述计算所得周期姿态修正量插补步幅基数基础上，由各轴每周期累计插补步数Step X，Step Y，Step Z与各轴每周期姿态修正量插补步幅基数v bX，v bY，v bZ分别相乘得到各轴当前周期姿态累积插补修正量v sX，v sY，v sZ，在拟合逼近周期姿态目标修正量的过程中，先对各轴周期姿态累积插补修正量与对应周期姿态目标修正量进行大小关系判断，再判断各轴周期姿态累积插补修正量是逐步递增还是递减。
尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (7)

  1. 一种基于力反馈的机器人实时运动规划方法，其特征在于步骤如下：
    (1)利用六维力传感器进行数据采集，经重力补偿及坐标变换后获取机械臂基坐标系下X，Y，Z轴的分作用力f X，f Y，f Z，并根据分作用力f X，f Y，f Z获取机械臂基坐标系下的外部作用力f；
    (2)根据步骤(1)所得外部作用力f建立外部作用力与机械臂基坐标系下周期姿态目标修正量分段映射关系f(f、v a、f 1、f 2)；
    (3)根据步骤(2)所得映射关系f(f、v a、f 1、f 2)确定周期修正速度v a，根据步骤(1)所得分作用力f X，f Y，f Z与外部作用力f的比值，分别映射至X、Y、Z轴上，获取各轴力与周期姿态目标修正量的映射关系f 1(f、f x、v ax)、f 2(f、f Y、v aY)、f 3(f、f Z、v aZ)；
    (4)确定机械臂基坐标系下，机械臂三轴各自稳定运行情况下周期姿态修正量增量Δv X，Δv Y，Δv Z，选取最小值Δv并确定对应轴的外部作用力值f A；
    (5)根据步骤(4)中选定的最小值Δv及其外部作用力值f A、步骤(1)中分作用力f X，f Y，f Z计算X、Y、Z轴的周期姿态修正量插补步幅基数v bX，v bY，v bZ；
    (6)根据各轴每周期累计插补步数Step X，Step Y，Step Z与步骤(5)所得周期姿态修正量插补步幅基数计算三轴当前周期姿态累积插补修正量v sX，v sY，v sZ；
    (7)对步骤(6)所得三轴当前周期姿态累积插补修正量分别进行判断，若各轴当前周期姿态累积插补修正量大于该轴周期姿态目标修正量，则通过插补步数随运行周期逐步递减至实时周期姿态目标修正量，否则逐步递增至实时周期姿态目标修正量。
  2. 根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法， 其特征在于：所述步骤(1)中，所述六维力传感器坐标系与机械臂工具坐标系X、Y轴分别重合，所述外部作用力的计算方法为：利用基坐标系下六维力传感器测量值减去机械臂末端负载重力、六维力传感器传感器静态误差值后获得。
  3. 根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(2)中，映射关系f(f、v a、f 1、f 2)具体为：
    Figure PCTCN2021100482-appb-100001
    式中，k1，k2，k3分别为力位转换比例因子，f 1，f 2分别为函数分段数据阈值，v a为周期修正速度，其中：
    若外部作用力f小于数据计算阈值f 1，则舍弃该数值并返回步骤(1)重新采集下一组数据，否则根据步骤(2)中映射关系建立力位映射转换。
  4. 根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(3)中，映射关系f 1(f、f x、v ax)、f 2(f、f Y、v aY)、f 3(f、f Z、v aZ)具体为：
    X轴周期姿态目标修正量为：
    Figure PCTCN2021100482-appb-100002
    Y轴周期姿态目标修正量为：
    Figure PCTCN2021100482-appb-100003
    Z轴周期姿态目标修正量为：
    Figure PCTCN2021100482-appb-100004
  5. 根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(5)中，X、Y、Z轴的周期姿态修正量插补步幅基数v bX，v bY，v bZ的计算方法具体为：
    X轴方向的周期姿态修正量插补步幅基数分段函数如下：
    Figure PCTCN2021100482-appb-100005
    Y轴方向的周期姿态修正量插补步幅基数分段函数如下：
    Figure PCTCN2021100482-appb-100006
    Z轴方向的周期姿态修正量插补步幅基数分段函数如下：
    Figure PCTCN2021100482-appb-100007
  6. 根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(6)中，三轴当前周期姿态累积插补修正量v sX，v sY，v sZ的计算方法为：
    X轴方向当前周期姿态累积插补修正量：v sX＝v bX*Step X
    Y轴方向当前周期姿态累积插补修正量：v sY＝v bY*Step Y
    Z轴方向当前周期姿态累积插补修正量：v sz＝v bz*Step Z。
  7. 根据权利要求1所述的一种基于力反馈的机器人实时运动规划方法，其特征在于：所述步骤(7)中，当前周期姿态累积插补修正量随运行周期逐步递增或递减的方法具体为：
    (7a)确定三轴各自当前周期姿态修正目标插补步数，具体为：
    Figure PCTCN2021100482-appb-100008
    (7b)于逐步递增或递减，拟合逼近周期姿态目标修正量的过程中，X轴 方向的实时周期姿态累积插补修正量具体为：
    Figure PCTCN2021100482-appb-100009
    v sX＝Step X'*v bX
    Y轴方向的实时周期姿态累积插补修正量具体为：
    Figure PCTCN2021100482-appb-100010
    v sY＝Step Y'*v bY
    Z轴方向的实时周期姿态累积插补修正量具体为：
    Figure PCTCN2021100482-appb-100011
    v sZ＝Step Z'*v bZ。
PCT/CN2021/100482 2020-06-18 2021-06-17 一种基于力反馈的机器人实时运动规划方法 WO2021254414A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202010560487.4A CN111805538B (zh) 2020-06-18 2020-06-18 一种基于力反馈的机器人实时运动规划方法
CN202010560487.4 2020-06-18

Publications (1)

Publication Number Publication Date
WO2021254414A1 true WO2021254414A1 (zh) 2021-12-23

Family

ID=72845216

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/100482 WO2021254414A1 (zh) 2020-06-18 2021-06-17 一种基于力反馈的机器人实时运动规划方法

Country Status (2)

Country Link
CN (1) CN111805538B (zh)
WO (1) WO2021254414A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116512238A (zh) * 2023-01-17 2023-08-01 上海都协自动化科技有限公司 液压多轴机械臂的控制方法、装置、存储介质及电子设备

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111805538B (zh) * 2020-06-18 2022-01-04 北京卫星制造厂有限公司 一种基于力反馈的机器人实时运动规划方法
CN116175594B (zh) * 2023-04-26 2023-06-30 北京宸普豪新科技有限公司 一种机器人的机械臂控制方法、设备及机器人

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05341834A (ja) * 1992-06-09 1993-12-24 Matsushita Electric Ind Co Ltd 多関節ロボットの直接教示装置
JP2005088114A (ja) * 2003-09-16 2005-04-07 Yaskawa Electric Corp ロボットの直接教示装置
CN104440913A (zh) * 2013-09-19 2015-03-25 库卡实验仪器有限公司 手动引导地调节机器人臂姿势的方法和相应的工业机器人
CN105583824A (zh) * 2016-01-26 2016-05-18 清华大学 力控牵引和摆位的多自由度机械臂控制装置及其控制方法
CN106774181A (zh) * 2016-12-23 2017-05-31 东南大学 基于阻抗模型的高精度牵引示教机器人的速度控制方法
CN108789363A (zh) * 2018-05-25 2018-11-13 雅客智慧（北京）科技有限公司 一种基于力传感器的直接拖动示教系统及方法
CN110666799A (zh) * 2019-10-15 2020-01-10 吉林大学 一种基于重力补偿的六自由度串联机器人柔顺控制方法
CN111805538A (zh) * 2020-06-18 2020-10-23 北京卫星制造厂有限公司 一种基于力反馈的机器人实时运动规划方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH1133960A (ja) * 1997-07-17 1999-02-09 Fanuc Ltd ロボット制御方法
JP2008296310A (ja) * 2007-05-30 2008-12-11 Fanuc Ltd 加工ロボットの制御装置
CN104626168B (zh) * 2014-12-16 2016-08-24 苏州大学 基于智能算法的机器人力位柔顺控制方法
CN106584462B (zh) * 2016-12-22 2019-01-15 南京埃斯顿自动化股份有限公司 一种机器人运行速度实时调节方法
CN106475999B (zh) * 2016-12-23 2018-11-09 东南大学 刚性条件下基于阻抗模型的双臂协调的加速度控制方法
CN109968356A (zh) * 2019-03-18 2019-07-05 华南理工大学 基于嵌入式运动控制器的机械臂力反馈控制系统及方法
CN110497423B (zh) * 2019-08-22 2022-08-16 泉州装备制造研究所 一种机械手自适应加工方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05341834A (ja) * 1992-06-09 1993-12-24 Matsushita Electric Ind Co Ltd 多関節ロボットの直接教示装置
JP2005088114A (ja) * 2003-09-16 2005-04-07 Yaskawa Electric Corp ロボットの直接教示装置
CN104440913A (zh) * 2013-09-19 2015-03-25 库卡实验仪器有限公司 手动引导地调节机器人臂姿势的方法和相应的工业机器人
CN105583824A (zh) * 2016-01-26 2016-05-18 清华大学 力控牵引和摆位的多自由度机械臂控制装置及其控制方法
CN106774181A (zh) * 2016-12-23 2017-05-31 东南大学 基于阻抗模型的高精度牵引示教机器人的速度控制方法
CN108789363A (zh) * 2018-05-25 2018-11-13 雅客智慧（北京）科技有限公司 一种基于力传感器的直接拖动示教系统及方法
CN110666799A (zh) * 2019-10-15 2020-01-10 吉林大学 一种基于重力补偿的六自由度串联机器人柔顺控制方法
CN111805538A (zh) * 2020-06-18 2020-10-23 北京卫星制造厂有限公司 一种基于力反馈的机器人实时运动规划方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116512238A (zh) * 2023-01-17 2023-08-01 上海都协自动化科技有限公司 液压多轴机械臂的控制方法、装置、存储介质及电子设备
CN116512238B (zh) * 2023-01-17 2023-11-03 上海都协自动化科技有限公司 液压多轴机械臂的控制方法、装置、存储介质及电子设备

Also Published As

Publication number Publication date
CN111805538A (zh) 2020-10-23
CN111805538B (zh) 2022-01-04

Similar Documents

Publication Publication Date Title
WO2021254414A1 (zh) 一种基于力反馈的机器人实时运动规划方法
US9221170B2 (en) Method and apparatus for controlling a robotic device via wearable sensors
WO2018188276A1 (zh) 一种六自由度机器人末端空间曲线轨迹的误差建模方法
WO2021128787A1 (zh) 一种定位的方法及装置
Wilson et al. Relative end-effector control using cartesian position based visual servoing
CN108839027B (zh) 基于激光测距传感器的机器人自动对准控制方法
US11179855B2 (en) Acceleration compensation method for humanoid robot and apparatus and humanoid robot using the same
CN111624941A (zh) 一种面向未知环境的六自由度机器人力控方法
US11465281B2 (en) Dynamic planning controller
CN111037542B (zh) 一种逆动力学控制机器人直线加工的轨迹误差补偿方法
CN109591019B (zh) 一种无确定性定位特征物体的空间精确定位方法
WO2022241806A1 (zh) 一种基于强化学习的双机器人力/位多元数据驱动方法
CN110561424A (zh) 基于多传感器混合滤波器的在线机器人运动学校准方法
WO1992012473A1 (en) Method of correcting deflection of robot
WO2023168849A1 (zh) 机械臂动作捕捉方法、介质、电子设备及系统
WO2023000946A1 (zh) 一种机器人系统的控制单元、机器人系统及机器人系统的控制方法
CN111515928B (zh) 机械臂运动控制系统
CN110370271A (zh) 工业串联机器人的关节传动比误差校准方法
US20220371186A1 (en) Dual-robot position/force multivariate-data-driven method using reinforcement learning
JPH0768480A (ja) マニピュレータの関節角度制御方法
CN111331604A (zh) 一种基于机器视觉的阀门旋拧柔顺作业方法
CN115502979A (zh) 一种机械臂力矩主动柔顺精确控制方法及系统
KR20220109930A (ko) 강인 외력 관측기를 이용한 다관절 로봇의 센서리스 접촉력 추정 방법
CN109397293B (zh) 一种基于移动机器人的地面水平误差建模及补偿方法
CN116494250A (zh) 基于速度补偿的机械臂控制方法、控制器、介质及系统

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21825815

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21825815

Country of ref document: EP

Kind code of ref document: A1