CN114905519B - 一种自由漂浮空间机器人轨迹规划方法、机器人及介质 - Google Patents

一种自由漂浮空间机器人轨迹规划方法、机器人及介质 Download PDF

Info

Publication number
CN114905519B
CN114905519B CN202210737762.4A CN202210737762A CN114905519B CN 114905519 B CN114905519 B CN 114905519B CN 202210737762 A CN202210737762 A CN 202210737762A CN 114905519 B CN114905519 B CN 114905519B
Authority
CN
China
Prior art keywords
track
robot
matrix
error
singular value
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
Application number
CN202210737762.4A
Other languages
English (en)
Other versions
CN114905519A (zh
Inventor
王二磊
李海艳
杨煜俊
刘海涛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong Robot Innovation Center Co ltd
Guangdong University of Technology
Original Assignee
Guangdong Robot Innovation Center Co ltd
Guangdong University of Technology
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 Guangdong Robot Innovation Center Co ltd, Guangdong University of Technology filed Critical Guangdong Robot Innovation Center Co ltd
Priority to CN202210737762.4A priority Critical patent/CN114905519B/zh
Publication of CN114905519A publication Critical patent/CN114905519A/zh
Application granted granted Critical
Publication of CN114905519B publication Critical patent/CN114905519B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1628Programme controls characterised by the control loop
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1656Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

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

Abstract

本发明公开了一种自由漂浮空间机器人轨迹规划方法、机器人及介质,所述方法包括如下步骤:对机器人进行初始化,设置各项初始值;计算机器人期望轨迹与实际轨迹的轨迹误差、机器人关节速度与末端执行器的线速度和角速度的矩阵的最小奇异值估计以及该矩阵的阻尼伪逆;将轨迹误差与轨迹误差阈值、最小奇异值估计与最小奇异值估计阈值进行比较;当轨迹误差大于轨迹误差阈值,且最小奇异值估计大于最小奇异值估计阈值时,计算误差反馈补偿;根据反馈补偿,设置代价函数;根据代价函数与混沌粒子群优化算法,对轨迹进行优化求解。本发明通过结合阻尼最小二乘方法和反馈补偿来避免动力学奇异点,并逐渐减小机器人轨迹的轨迹误差。

Description

一种自由漂浮空间机器人轨迹规划方法、机器人及介质
技术领域
本发明属于机器人轨迹规划技术领域,具体涉及一种自由漂浮空间机器人轨迹规划方法、机器人及介质。
背景技术
自由漂浮空间机器人的笛卡尔轨迹规划在通过运动学逆方程求解时会碰到动力学奇异点;因此,动力学奇异点成为众多学者的研究热点。为了避免动力学奇异点,需要对自由漂浮空间机器人的关节转角、关节速度和关节加速度的轨迹进行参数化。有学者提出了一种基于可达工作空间的动力学奇异点避免方法,并将其应用于2自由度的自由漂浮空间机器人,但对高自由度的自由漂浮空间机器人来说,其达到指定空间是非常困难的。在puma型机械臂的自由漂浮空间机器人上,有学者提出了一种奇异点分离与阻尼倒数相结合的方法,通过调整自由漂浮空间机器人的初始位姿(包括关节角度和航天器姿态)来避免动力学奇异点。但这样会忽略了基座姿态干扰。
阻尼最小二乘(DLS)是处理普通机器人奇异点的常用方法,它也可以应用于自由漂浮空间机器人,但可能会导致出现轨迹误差。在自由漂浮空间机器人的可达空间内,只能通过低自由度来实现某种目的,对于特别复杂的任务,低自由度自由漂浮空间机器人无法完成;由于在空间中,所有物体都是运动着的,这对于自由漂浮空间机器人来说是一项巨大的挑战。考虑到多方面因素,误差是一个不能忽视的存在,需要把误差放到允许的范围内,需要更深的研究来解决自由漂浮空间机器人的动力学奇异点问题。
发明内容
为了克服上述技术缺陷,本发明提供了一种自由漂浮空间机器人轨迹规划方法,以解决动力学奇异点问题并减小轨迹误差。
为了解决上述问题,本发明按以下技术方案予以实现的:
一种自由漂浮空间机器人轨迹规划方法,所述方法包括如下步骤:
对机器人进行初始化,设置各项初始值;
计算机器人期望轨迹与实际轨迹的轨迹误差、机器人关节速度与末端执行器的线速度和角速度的矩阵的最小奇异值估计以及该矩阵的阻尼伪逆;
将轨迹误差与轨迹误差阈值、最小奇异值估计与最小奇异值估计阈值进行比较;
当轨迹误差大于轨迹误差阈值,且最小奇异值估计大于最小奇异值估计阈值时,计算误差反馈补偿;
根据反馈补偿,设置代价函数;
根据代价函数与混沌粒子群优化算法,对轨迹进行优化求解。
进一步的,步骤计算机器人期望轨迹与实际轨迹的轨迹误差、机器人关节速度与末端执行器的线速度和角速度的矩阵的最小奇异值估计以及该矩阵的阻尼伪逆,包括如下步骤:
建立机器人的期望轨迹矩阵与实际轨迹矩阵,计算轨迹误差;
获得关节速度与线速度和角速度的矩阵的数据,计算矩阵的最小奇异值估计及矩阵的阻尼伪逆。
进一步的,步骤建立机器人的期望轨迹矩阵与实际轨迹矩阵,计算轨迹误差,包括如下步骤:
建立机器人的期望轨迹矩阵与实际轨迹矩阵;
根据期望轨迹矩阵、实际轨迹矩阵,计算上一时刻的误差;
根据期望轨迹矩阵、实际轨迹矩阵及上一时刻的轨迹误差,计算轨迹误差。
进一步的,步骤获得关节速度与线速度和角速度的矩阵的数据,计算矩阵的最小奇异值估计及矩阵的阻尼伪逆,包括如下步骤:
获得机器人基座、机器人末端的数据,计算关节速度与线速度和角速度的矩阵;
根据矩阵,获得最小奇异值估计;
根据最小奇异值估计,计算实际阻尼因子;
根据矩阵及实际阻尼因子,获得阻尼伪逆。
进一步的,步骤当轨迹误差大于轨迹误差阈值,且最小奇异值估计大于最小奇异值估计阈值时,计算误差反馈补偿,包括如下步骤:
当轨迹误差大于轨迹误差阈值,且最小奇异值估计大于最小奇异值估计阈值时,记录补偿次数,计算每次的误差反馈补偿;
根据每次的误差反馈补偿,计算总误差补偿。
进一步的,步骤当轨迹误差大于轨迹误差阈值,且最小奇异值估计大于最小奇异值估计阈值时,计算误差反馈补偿后,还包括如下步骤:
根据反馈补偿,计算机器人某一姿态的关节角度及下一姿态的关节角度。
与现有技术相比,本发明具有以下有益效果:
本发明公开了一种自由漂浮空间机器人轨迹规划方法,通过结合阻尼最小二乘方法和反馈补偿来避免动力学奇异点,同时使得机器人轨迹的轨迹误差逐渐减小,且通过代价函数和混沌粒子群优化算法,对轨迹进行优化求解。
本发明还公开了一种机器人,包括:
处理器;
用于存储所述处理器可执行指令的存储器;
其中,所述处理器被配置为执行所述指令,以实现上述的轨迹规划方法。
本发明还公开了一种计算机可读存储介质,其为计算器可读的存储介质,其上存储有计算机程序,所述计算机程序被执行时实现上述的轨迹规划方法。
附图说明
下面结合附图对本发明的具体实施方式作进一步详细的说明,其中:
图1为实施例1所述的轨迹规划方法的流程图;
图2为实施例1所述的步骤S6的计算流程示意图;
图3为实施例2所述的机器人的结构示意图。
具体实施方式
以下结合附图对本发明的优选实施例进行说明,应当理解,此处所描述的优选实施例仅用于说明和解释本发明,并不用于限定本发明。
实施例1
如图1,本实施例公开了一种自由漂浮空间机器人轨迹规划方法,方法包括如下步骤:
S1、对机器人进行初始化,设置各项初始值:初始化机器人,设置时间t=0、0时刻机器人关节角度θ(0)=θ0、0时刻机器人关节角加速度ω0(0)=0、反馈补偿μ=0、上一时刻误差Δsum=0、反馈补偿次数k=0。
S2、计算机器人期望轨迹与实际轨迹的轨迹误差、机器人关节速度与末端执行器的线速度和角速度的矩阵的最小奇异值估计以及该矩阵的阻尼伪逆:建立机器人的期望轨迹矩阵与实际轨迹矩阵,计算轨迹误差;获得关节速度与线速度和角速度的矩阵的数据,计算矩阵的最小奇异值估计及矩阵的阻尼伪逆。
具体的,建立机器人的期望轨迹矩阵:
与机器人的实际轨迹矩阵:
Pef(τ)表示某时刻位置矢量,Φef(τ)表示某时刻欧拉角,τ=t/Tf,Tf—移动时间。
根据期望轨迹矩阵xe、实际轨迹矩阵xef(τ),计算上一时刻误差:
Δsum=xei-1)-xefi-1)
根据期望轨迹矩阵xe、实际轨迹矩阵xef(τ)及上一时刻的轨迹误差Δsum,计算轨迹误差:
其中,
τi表示从动态奇点情况到正常情况的时间阈值。
获得机器人基座雅可比矩阵Jb、机器人基座雅克比耦合矩阵Jbm,机器人末端雅克比矩阵Jm,计算关节速度与线速度和角速度的矩阵:
Jg=JbJbm+Jm
根据矩阵Jg,获得最小奇异值估计;在Jg的基础上估计一个最小值
根据最小奇异值估计,计算实际阻尼因子:
其中,λ0 2为定义阻尼因子的最大值
根据矩阵Jg及实际阻尼因子λ2,获得阻尼伪逆:
其中,En为n阶单位矩阵。
采用阻尼最小二乘方法(DLS)来避免动态奇异点,空间机器人末端执行器会偏离目标轨迹;为了弥补这一不足,在空间机器人通过奇异点后,引入反馈补偿,使机器人在一段时间后返回到所期望的轨迹。
S3、将轨迹误差Δx与轨迹误差阈值εx进行比较,将最小奇异值估计与最小奇异值估计阈值εσ进行比较;轨迹误差阈值εx用以确定实际轨迹是否偏离了期望轨迹,最小奇异值估计阈值εσ用以表示奇异点领域的边界,εx和εσ根据实际具体情况不断调整。
S4、当轨迹误差大于轨迹误差阈值Δx>εx,且最小奇异值估计大于最小奇异值估计阈值时,记录补偿次数k,计算每次的误差反馈补偿μk(τ),设
μk(τ)是μi(τ)的第k次误差,根据每次的误差反馈补偿,计算总误差补偿:
这样就完成了反馈补偿和轨迹偏差的计算。
在上述实施例中,步骤S4后,还包括如下步骤:
进行反馈补偿后,计算关节角速度:
其中,Jg*为广义雅可比矩阵Jg的阻尼伪逆;—E3为单位矩阵,NΦe为关节角角速度与欧拉角角速度的矩阵关系。
如果机器人下一步骤的步长积分为d,则可以计算出其下一姿态的关节角度:
Θ(τ+dτ)=Θ(τ)+Θ′(dτ)
通过计算优化后的关节角速度,验证优化效果。
S5、根据反馈补偿μ,设置代价函数:
Tmin=αμ+βTf+γFf
其中,α和β、γ分别为反馈补偿和移动时间,总位移量的权重因子,Ff为惩罚函数,Pi为末端位置矢量。
代价函数用以在多约束条件下找最优解,在时间允许的范围内(时间t≤移动时间Tf)尽可能的减少误差补偿。
S6、如图2,根据代价函数与混沌粒子群优化算法,对轨迹进行优化求解:在最大迭代次数下,不断迭代优化,在好的粒子旁边继续找最好的粒子,其中G为带入代价函数的适应值;混沌粒子群优化算法的相关内容请参见现有技术。
在上述实施例中,步骤S4还包括:
当误差大于误差阈值,最小奇异值估计小于估计阈值时,计算关节角速度;
当误差小于误差阈值,最小奇异值估计大于估计阈值时,计算关节角速度;
当误差小于误差阈值,最小奇异值估计小于估计阈值时,计算关节角速度;
计算关节角速度公式为:
本实施例能够逐渐减小轨迹误差,与为优化的轨迹相比,优化后的轨迹在运动时间和基座姿态扰动方面均有较好的性能;同时本实施例保证了关节角速度和位置的约束,使得机器人轨迹规划更具柔性。
本实施例通过结合阻尼最小二乘方法与反馈补偿来避免动力学奇异点,在考虑基座姿态干扰和运动时间的前提下,针对通信、观测等特定任务,设置代价函数,将轨迹规划转为多目标优化,并采用混沌粒子群优化算法进行优化求解。
实施例2
如图3,本实施例公开了一种机器人,包括:处理器;用于存储处理器可执行指令的存储器;其中,处理器被配置为执行指令,以实现实施例1所述的轨迹规划方法。
实施例3
本实施例公开了一种计算机可读存储介质,其为计算器可读的存储介质,其上存储有计算机程序,计算机程序被执行时实现实施例1中的轨迹规划方法。
可选地,该计算机可读存储介质可以包括:只读存储器(ROM,Read Only Memory)、随机存取记忆体(RAM,Random Access Memory)、固态硬盘(SSD,Solid State Drives)或光盘等。其中,随机存取记忆体可以包括电阻式随机存取记忆体(ReRAM,ResistanceRandomAccess Memory)和动态随机存取存储器(DRAM,Dynamic Random Access Memory)。
以上所述,仅是本发明的较佳实施例而已,并非对本发明作任何形式上的限制,故凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何修改、等同变化与修饰,均仍属于本发明技术方案的范围内。

Claims (7)

1.一种自由漂浮空间机器人轨迹规划方法,其特征在于,所述方法包括如下步骤:
对机器人进行初始化,设置各项初始值;
计算机器人期望轨迹与实际轨迹的轨迹误差、机器人关节速度与末端执行器的线速度和角速度的矩阵的最小奇异值估计以及该矩阵的阻尼伪逆;
将轨迹误差与轨迹误差阈值、最小奇异值估计与最小奇异值估计阈值进行比较;
当轨迹误差大于轨迹误差阈值,且最小奇异值估计大于最小奇异值估计阈值时,计算误差反馈补偿;
根据反馈补偿,设置代价函数;
根据代价函数与混沌粒子群优化算法,对轨迹进行优化求解;
步骤当轨迹误差大于轨迹误差阈值,且最小奇异值估计大于最小奇异值估计阈值时,计算误差反馈补偿,包括如下步骤:
当轨迹误差大于轨迹误差阈值,且最小奇异值估计大于最小奇异值估计阈值时,记录补偿次数,计算每次的误差反馈补偿;
根据每次的误差反馈补偿,计算总误差补偿。
2.根据权利要求1所述的轨迹规划方法,其特征在于,步骤计算机器人期望轨迹与实际轨迹的轨迹误差、机器人关节速度与末端执行器的线速度和角速度的矩阵的最小奇异值估计以及该矩阵的阻尼伪逆,包括如下步骤:
建立机器人的期望轨迹矩阵与实际轨迹矩阵,计算轨迹误差;
获得机器人关节速度与末端执行器的线速度和角速度的矩阵的数据,计算该矩阵的最小奇异值估计及该矩阵的阻尼伪逆。
3.根据权利要求2所述的轨迹规划方法,其特征在于,步骤建立机器人的期望轨迹矩阵与实际轨迹矩阵,计算轨迹误差,包括如下步骤:
建立机器人的期望轨迹矩阵与实际轨迹矩阵;
根据期望轨迹矩阵、实际轨迹矩阵,计算上一时刻的误差;
根据期望轨迹矩阵、实际轨迹矩阵及上一时刻的轨迹误差,计算轨迹误差。
4.根据权利要求2所述的轨迹规划方法,其特征在于,步骤机器人关节速度与末端执行器的线速度和角速度的矩阵的数据,计算该矩阵的最小奇异值估计及该矩阵的阻尼伪逆,包括如下步骤:
获得机器人基座、机器人末端的数据,计算机器人关节速度与末端执行器的线速度和角速度的矩阵;
根据机器人关节速度与末端执行器的线速度和角速度的矩阵,获得最小奇异值估计;
根据最小奇异值估计,计算实际阻尼因子;
根据机器人关节速度与末端执行器的线速度和角速度的矩阵及实际阻尼因子,获得阻尼伪逆。
5.根据权利要求1所述的轨迹规划方法,其特征在于,步骤当轨迹误差大于轨迹误差阈值,且最小奇异值估计大于最小奇异值估计阈值时,计算误差反馈补偿后,还包括如下步骤:
根据反馈补偿,计算机器人某一姿态的关节角度及下一姿态的关节角度。
6.一种机器人,其特征在于,包括:
处理器;
用于存储所述处理器可执行指令的存储器;
其中,所述处理器被配置为执行所述指令,以实现如权利要求1-5中任一项所述的轨迹规划方法。
7.一种计算机可读存储介质,其特征在于,其为计算器可读的存储介质,其上存储有计算机程序,所述计算机程序被执行时实现如权利要求1-5中任一项所述的轨迹规划方法。
CN202210737762.4A 2022-06-27 2022-06-27 一种自由漂浮空间机器人轨迹规划方法、机器人及介质 Active CN114905519B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210737762.4A CN114905519B (zh) 2022-06-27 2022-06-27 一种自由漂浮空间机器人轨迹规划方法、机器人及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210737762.4A CN114905519B (zh) 2022-06-27 2022-06-27 一种自由漂浮空间机器人轨迹规划方法、机器人及介质

Publications (2)

Publication Number Publication Date
CN114905519A CN114905519A (zh) 2022-08-16
CN114905519B true CN114905519B (zh) 2024-01-23

Family

ID=82772296

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210737762.4A Active CN114905519B (zh) 2022-06-27 2022-06-27 一种自由漂浮空间机器人轨迹规划方法、机器人及介质

Country Status (1)

Country Link
CN (1) CN114905519B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5159249A (en) * 1989-05-16 1992-10-27 Dalila Megherbi Method and apparatus for controlling robot motion at and near singularities and for robot mechanical design
CN105700527A (zh) * 2016-01-26 2016-06-22 哈尔滨工业大学 一种平面冗余度机器人避障及避奇异的路径规划方法
FR3045847A1 (fr) * 2015-12-17 2017-06-23 Centre Nat D'etudes Spatiales (Cnes) Procede de commande d'un systeme redondant

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5159249A (en) * 1989-05-16 1992-10-27 Dalila Megherbi Method and apparatus for controlling robot motion at and near singularities and for robot mechanical design
FR3045847A1 (fr) * 2015-12-17 2017-06-23 Centre Nat D'etudes Spatiales (Cnes) Procede de commande d'un systeme redondant
CN105700527A (zh) * 2016-01-26 2016-06-22 哈尔滨工业大学 一种平面冗余度机器人避障及避奇异的路径规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"空间机器人目标捕获的路径规划与实验研究";徐文福;《中国博士学位论文全文数据库信息科技辑》;第3-4章 *
用阻尼伪逆法控制冗余度机器人的一种新方案;黄磊光 李耀通;《自动化学报》;第24卷(第3期);315-322 *

Also Published As

Publication number Publication date
CN114905519A (zh) 2022-08-16

Similar Documents

Publication Publication Date Title
US11845186B2 (en) Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same
CN107490965B (zh) 一种空间自由漂浮机械臂的多约束轨迹规划方法
JP4508164B2 (ja) 多関節ロボット及びその制御プログラム
US8924021B2 (en) Control of robots from human motion descriptors
US8396595B2 (en) Real-time self collision and obstacle avoidance using weighting matrix
CN109623812B (zh) 考虑航天器本体姿态运动的机械臂轨迹规划方法
Chen Reconfiguration of a parallel kinematic manipulator for the maximum dynamic load-carrying capacity
Chung et al. Torque optimizing control with singularity-robustness for kinematically redundant robots
CN111123943A (zh) 一种基于伪逆约束的超冗余机器人轨迹规划方法及系统
CN112327892A (zh) 一种auv误差受限的抗扰控制方法
CN112192573A (zh) 基于反演法的不确定性机器人自适应神经网络控制方法
CN114147720B (zh) 一种多自由度机械臂的逆运动学通用求解方法及装置
Liu et al. Trajectory planning and coordination control of a space robot for detumbling a flexible tumbling target in post-capture phase
CN114905519B (zh) 一种自由漂浮空间机器人轨迹规划方法、机器人及介质
CN109108964B (zh) 一种基于自适应动态规划Nash博弈的空间机械臂协调控制方法
US11717962B2 (en) Method for controlling movement of a robot
Sagara et al. Performance comparison of control methods using a dual-arm underwater robot-Computed torque based control and resolved acceleration control for UVMS
CN116237950A (zh) 基于分段运动规划策略的机器人末端精确控制方法及设备
CN113927603B (zh) 机械臂拖动控制方法、装置、计算机设备以及存储介质
JPH0852674A (ja) マニピュレータの位置姿勢決定方法
CN114800527A (zh) 一种移动作业机械臂末端施力控制方法及系统
Safeena et al. Adaptive Super Twisting Control of Stewart Platform Based on Super Twisting Observer
Ding et al. Chat-PM: A class of composite hybrid aerial/terrestrial precise manipulator
Zhang et al. Motion planning of underwater vehicle-manipulator system with joint limit
Liu et al. Predefined-Time Trajectory Planning for a Dual-Arm Free-Floating Space Robot

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
GR01 Patent grant