CN114019798B - 机器人轨迹跟踪控制方法、磁性医疗机器人及存储介质 - Google Patents
机器人轨迹跟踪控制方法、磁性医疗机器人及存储介质 Download PDFInfo
- Publication number
- CN114019798B CN114019798B CN202111295113.5A CN202111295113A CN114019798B CN 114019798 B CN114019798 B CN 114019798B CN 202111295113 A CN202111295113 A CN 202111295113A CN 114019798 B CN114019798 B CN 114019798B
- Authority
- CN
- China
- Prior art keywords
- robot
- coordinate
- actual
- iteration
- motion parameters
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 68
- 238000005457 optimization Methods 0.000 claims abstract description 31
- 239000011159 matrix material Substances 0.000 claims description 21
- 238000004590 computer program Methods 0.000 claims description 5
- 238000005516 engineering process Methods 0.000 description 7
- 230000000694 effects Effects 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000012360 testing method Methods 0.000 description 3
- 238000009795 derivation Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000003814 drug Substances 0.000 description 1
- 229940079593 drug Drugs 0.000 description 1
- 230000005684 electric field Effects 0.000 description 1
- 239000013604 expression vector Substances 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 244000005700 microbiome Species 0.000 description 1
- 238000002324 minimally invasive surgery Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
- 238000002626 targeted therapy Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 238000004260 weight control Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本申请涉及机器人轨迹跟踪控制领域,公开了一种机器人轨迹跟踪控制方法、磁性医疗机器人及存储介质。该方法包括:获取机器人基于实际运动参数的实际运动轨迹坐标以及参考轨迹坐标;计算实际轨迹坐标和参考轨迹坐标的坐标误差;以坐标误差的最小化作为迭代目标,对机器人的实际运动参数进行优化迭代得到优化运动参数;其中,每次迭代的初始值均采用上一次迭代的优化值;以优化运动参数控制机器人运动。通过上述方式,本申请能够提高机器人的控制精度与稳定性。
Description
技术领域
本申请涉及机器人轨迹跟踪控制领域,特别是涉及一种机器人轨迹跟踪控制方法、磁性医疗机器人及存储介质。
背景技术
随着信息化、工业化不断融合,以机器人科技为代表的智能产业蓬勃兴起,成为当代科技创新的一个重要标志。医疗与机器人结合也成为科技创新中的一个重要板块。医疗机器人的范围非常广泛,用于医疗全阶段的机器人或者机器人化设备都可以叫做医疗机器人。医疗机器人目前大致可以分为手术机器人、康复机器人、医用服务机器人和智能设备等。
其中,在靶向治疗与微创手术中经常要用到一种微型机器人。随着微纳米加工技术的发展,微型机器人也在不断优化,目前人们通常叫它们为人造微纳机器人。受自然界微生物***的启发,人造微纳机器人近些年得到了广泛的关注与研究,通过电场、磁场、光场等手段可以有效地驱动这些微纳机器人,在无创手术、靶向药物运输和生物传感检测等领域具有广泛的应用。在多种操控方案中,磁控驱动可以无线式精确操纵微纳机器人,通过改变外部磁场的梯度和方向,会对磁控微纳机器人施加力和力矩,进而使其沿着期望的轨迹运动。
在对磁性微纳机器人进行实时轨迹跟踪控制的问题中,大多采用基于几何方法的轨迹跟踪中的纯跟踪方法(Pure-Pursuit)和Stanley方法。Pure-Pursuit方法通过计算从当前位置到某个目标位置的距离误差的比例控制器。当前探测距离过大时,跟踪性能较差,易偏离轨迹;而Stanley方法考虑了角度误差和距离误差,但在非连续路径上表现不佳。对于基于模型预测优化控制的方法,其优化求解的速度有待提升。
发明内容
本申请主要解决的技术问题是提供一种机器人轨迹跟踪方法,能够实现提高机器人的控制精度与稳定性。
为了解决上述技术问题,本申请采用的一种技术方案是:提供了一种机器人轨迹跟踪控制方法,包括:
获取机器人基于实际运动参数的实际轨迹坐标以及参考轨迹坐标;计算实际轨迹坐标和参考轨迹坐标的坐标误差;以坐标误差的最小化作为迭代目标,对机器人的实际运动参数进行优化迭代得到优化运动参数;其中,每次迭代的初始值均采用上一次迭代的优化值;以优化运动参数控制机器人运动。
进一步地,计算实际轨迹坐标和参考轨迹坐标的坐标误差,包括,基于实际轨迹坐标和参考轨迹坐标的坐标差值和实际运动参数得到坐标误差。
进一步地,基于实际轨迹坐标和参考轨迹坐标的坐标差值和实际运动参数得到坐标误差,包括,以参考运动参数构建状态系数矩阵,计算状态系数矩阵与坐标差值的乘积,实际运动参数与控制系数矩阵的乘积;将两乘积之和作为坐标误差。
其中,参考运动参数为其中,vr表示参考运动速度,wr表示参考运动角速度;状态系数矩阵为/>控制系数矩阵为/>坐标误差为/>其中,e1、e2、e3表示实际轨迹坐标和参考轨迹坐标的坐标差值。
进一步地,获取机器人基于实际运动参数的实际轨迹坐标以及参考轨迹坐标,包括,获取预测范围内每个步长下机器人基于实际运动参数的实际轨迹坐标;计算实际轨迹坐标和参考轨迹坐标的坐标误差,包括,计算每个步长下实际轨迹坐标和参考轨迹坐标的坐标误差;以坐标误差的最小化作为迭代目标,包括,以所有步长下坐标误差中的坐标差值之和,以及控制范围内实际运动参数之和的最小化作为迭代目标。
进一步地,该方法还包括,预测范围大于或等于控制范围。
进一步地,以坐标误差的最小化作为迭代目标,对机器人的实际运动参数进行优化迭代得到优化运动参数,还包括,以实际运动参数在预设范围内、优化运动参数在预设范围内以及坐标差值在预设范围内为约束条件。
进一步地,对机器人的实际运动参数进行优化迭代得到优化运动参数,包括,对机器人在控制范围内的所有实际运动参数进行优化迭代得到优化运动参数序列;以优化运动参数控制机器人运动,包括,以优化运动参数序列中的当前优化运动参数控制机器人运动。
为了解决上述问题,本申请采用的另一种技术方案是:提供一种磁性医疗机器人,包括,处理器以及与处理器耦接的存储器,存储器中存储有计算机程序,处理器用于执行计算机程序以实现上述方法。
为了解决上述问题,本申请采用的另一种技术方案是:提供一种计算机可读存储介质,其中,计算机可读存储介质后存储有程序数据,程序数据在被处理器执行时,用于实现上述方法。
本申请的有益效果是:区别于现有技术的情况,本申请提供一种机器人轨迹跟踪控制方法。该方法通过获取机器人基于实际运动参数的实际运动轨迹坐标以及参考轨迹坐标;计算实际轨迹坐标和参考轨迹坐标的坐标误差;以坐标误差的最小化作为迭代目标,对机器人的实际运动参数进行优化迭代得到优化运动参数;其中,每次迭代的初始值均采用上一次迭代的优化值;以优化运动参数控制机器人运动。该方法通过构造三自由度的运动模型和轨迹跟踪误差模型,对误差模型采用加速优化迭代控制方法对机器人加以控制,加快了收敛速度。通过上述方式,本申请能够提高机器人的控制精度与稳定性。
附图说明
为了更清楚地说明本申请实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。其中:
图1是本申请提供的一种机器人轨迹跟踪控制方法一实施例的流程示意图;
图2是本申请提供的一种机器人轨迹跟踪控制方法另一实施例的流程示意图;
图3是本申请提供的一种机器人轨迹跟踪控制方法另一实施例的流程示意图;
图4是本申请提供的一种磁性医疗机器人一实施例的结构示意图;
图5是本申请提供的计算机可读存储介质一实施例的结构示意图;
图6是本申请提供的圆形轨迹跟踪结果图;
图7是本申请提供的圆形轨迹跟踪每一次迭代的时间图;
图8是本申请提供的八字形轨迹跟踪结果图;
图9是本申请提供的八字形轨迹跟踪每一次迭代的时间图。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述。可以理解的是,此处所描述的具体实施例仅用于解释本申请,而非对本申请的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与本申请相关的部分而非全部方法和流程。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
本申请中的术语“包括”和“具有”以及它们任何变形,意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、***、产品或设备没有限定于已列出的步骤或单元,而是可选地还包括没有列出的步骤或单元,或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。
在本文中提及“实施例”意味着,结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例,也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是,本文所描述的实施例可以与其它实施例相结合。
参阅图1,图1是本申请提供的一种机器人轨迹跟踪控制方法一实施例的流程示意图。本实施例机器人轨迹跟踪控制方法具体包括步骤11至14:
步骤11:获取机器人基于实际运动参数的实际运动轨迹坐标以及参考轨迹坐标;
通常情况下,利用视觉反馈或者超声图像定位对磁控微型机器人进行定位,获取机器人在实际运动过程中的运动参数并追踪到机器人的实际运动轨迹,建立实际运动轨迹坐标系,获得机器人的实际运动轨迹坐标;同时建立参考轨迹坐标系,获取与实际运动轨迹坐标相对应的参考轨迹坐标。
步骤12:计算实际轨迹坐标和参考轨迹坐标的坐标误差;
坐标误差表示实际轨迹坐标与参考轨迹坐标的偏差,该偏差越大,说明机器人的运动越偏离规定轨迹,控制精度越差;该偏差越小,说明机器人的运动越接近规定轨迹,控制精度越高。
由于实际轨迹处于实际运动轨迹坐标系,而参考轨迹处于参考轨迹坐标系,不能够直接计算,因此,需要将实际轨迹与参考轨迹转化到同一个坐标系下,并计算转化后的实际轨迹坐标与参考轨迹坐标的坐标误差。
步骤13:以坐标误差的最小化作为迭代目标,对机器人的实际运动参数进行优化迭代得到优化运动参数;其中,每次迭代的初始值均采用上一次迭代的优化值;
迭代算法一般用于求解最优化问题,它能够反复迭代,直到最符合条件的优化值出现,停止迭代计算。目前迭代算法有牛顿迭代算法、迭代最近点算法以及二分法迭代算法等。
本申请提出一种基于模型预测控制和热启动(warm-start)技术的新方法,使得模型可以快速迭代求解优化控制问题。首先基于误差模型设计模型预测控制器,结合热启动技术进行快速迭代求解。由于机器人运动要保证其稳定性,因此本申请提出的一种基于模型预测控制和热启动技术的新方法,通过对机器人的实际运动参数加以约束,使其变成一个二次规划问题。求解二次规划问题常用的方法包括有效集法(Active Set Method)、内点法(Interior Point Method)等,本发明采用有效集法求解该二次规划问题。为了使迭代效果更好,迭代速度更快,将热启动技术与有效集法结合,将每次迭代后得到的优化值作为下一次迭代的初始值进行计算,以解决上述问题。
本申请利用优化迭代函数对坐标误差进行优化迭代,以得到最小的坐标误差,即实际运动轨迹无限趋近于参考运动轨迹,使其能够按照参考运动轨迹规定的路径运动,达到更高的控制精度。除了对坐标误差进行优化迭代以外,对机器人的实际运动参数也需要优化迭代,由于机器人在不断运动,且其运动轨迹多为曲线,因此运动参数在不断变化。为了使机器人运动更加平稳,减少机器人的剧烈抖动情况,需要对实际运动参数进行优化迭代以得到优化运动参数。
步骤14:以优化运动参数控制机器人运动。
利用优化迭代求解出来的优化运动参数控制机器人按照参数信息进行运动。其中,优化运动参数包括机器人的实际运动速度和角速度。
其中,在步骤12中又包括步骤121。
步骤121:基于实际轨迹坐标和参考轨迹坐标的坐标差值和实际运动参数得到坐标误差。
实际轨迹坐标在实际轨迹坐标系中由实际运动参数表示,实际轨迹坐标中包括横坐标、纵坐标和角速度。实际轨迹坐标中的每一个坐标值由实际运动速度和实际运动角速度表示。
其中,步骤121中又包括步骤1211。
步骤1211:以参考运动参数构建状态系数矩阵,计算状态系数矩阵与坐标差值的乘积,实际运动参数与控制系数矩阵的乘积;将两乘积之和作为坐标误差。
坐标误差模型中,使用参考运动参数构建状态系数矩阵,利用状态系数矩阵控制坐标差值对坐标误差的影响;利用实际运动参数控制机器人的实际运动,控制系数矩阵控制实际运动参数对机器人的控制效果。
接下来利用公式推导过程说明本申请构造的三自由度运动学模型以及误差模型的原理。
本申请发明人首先构造三自由度运动学模型。令[x,y,θ]表示在实际运动轨迹坐标系下的运动学模型状态,[v,w]表示实际运动速度和角速度,将运动模型表示为:
其中,表示x,y,θ的导数,表示在当前的实际运动速度和角速度下,在实际运动轨迹坐标系下机器人运动时横轴的运动速度、纵轴的运动速度以及角速度。
令[xr,yr,θr]表示在参考轨迹运动中的参考点,参考轨迹运动的速度和角速度表示为[vr,wr],将参考轨迹坐标系下的运动模型表示为:
同理,表示xr,yr,θr的导数,表示在参考运动速度和角速度下,在参考运动轨迹坐标系下机器人运动时横轴的运动速度、纵轴的运动速度以及角速度。
将机器人实际运动轨迹的中心位置投影到参考轨迹中,此时先定义误差表达向量为:
其中xr,yr,θr位于参考轨迹坐标系中,x,y,θ位于实际运动轨迹坐标系中,因此,可以将误差从实际运动坐标系中变换到参考轨迹坐标系中,得到:
将上述公式求导,同时将实际运动轨迹坐标系下的运动模型与参考运动轨迹坐标系下的运动模型带入到误差公式中,得到误差模型:
其中,设计输入控制量(u1,u2)为:
将控制量带入到误差模型中,得到:
根据线性控制理论,在平衡点对上述误差模型线性化得:
根据上述公式,可将误差模型表示为定义Δe=[e1,e2,e3]T为状态量,Δu=[v,w]为控制量,/>为状态系数矩阵,/>为控制量系数矩阵。其中,状态量表示机器人实际运动轨迹与参考运动轨迹的坐标误差,该值越小说明实际运动轨迹越接近参考运动轨迹;状态系数矩阵控制状态量对误差模型的影响作用;控制量表示每次模型输入的实际运动速度与角速度,每次输入值与上一次的输入值不宜相差过大,差距过大会导致机器人剧烈抖动,运动不平稳,运动轨迹出现毛刺等情况;控制量系数矩阵为对控制量的影响作用加以权衡。
在实施例1中,通过获取机器人基于实际运动参数的实际运动轨迹坐标以及参考坐标;计算实际轨迹坐标和参考轨迹坐标的坐标误差;以坐标误差的最小化作为迭代目标,对机器人的实际运动参数进行优化迭代得到优化运动参数;其中,每次迭代的初始值均采用上一次迭代的优化值;以优化运动参数控制机器人运动。在每次迭代的过程中都以当前迭代得到的优化值作为下一次迭代的初始值,不引入额外的优化参数和计算量,可以加快优化迭代的收敛速度,提高机器人的控制精度以及机器人运动的平稳性。
参阅图2,图2是本申请提供的一种机器人轨迹跟踪控制方法另一实施例的流程示意图。本实施例具体包括步骤21至23:
步骤21:获取预测范围内每个步长下机器人基于实际运动参数的实际轨迹坐标;
本申请采用一种模型预测控制方法优化迭代机器人的控制参数,从而对机器人加以控制。首先设定预测范围,其中预测范围内包含多个预测步长,获取在每个预测步长下机器人在实际运动参数控制下的预测实际轨迹坐标;
步骤22:计算每个步长下实际轨迹坐标和参考轨迹坐标的坐标误差;
获取参考轨迹中同样预测范围内多个步长下的参考轨迹坐标,找到与预测实际轨迹坐标一一对应的参考轨迹坐标,并计算实际轨迹坐标与参考轨迹坐标的坐标误差。
步骤23:以所有步长下坐标误差中的坐标差值之和,以及控制范围内实际运动参数之和的最小化作为迭代目标。
对实际运动参数进行预测,得到控制范围内多个步长下的实际运动参数,其中,预测范围大于或等于控制范围。将预测范围内所有步长下的坐标差值相加得到总预测误差,将控制范围内所有步长下的预测控制系数相加得到总预测控制量,以二者之和的最小化作为迭代目标,相当于以总预测误差最小以及总预测控制量最小作为迭代目标,得到使二者之和最小化时的最优控制序列。最优控制序列中包含当前时刻的最优控制量以及当前时刻的最优预测控制量,每一个控制量中包括实际运动速度和角速度。
下面将通过公式结合说明的形式表示迭代过程。
根据模型预测控制理论以及坐标误差模型,采用带有约束的有效集法对机器人的控制速度和角速度进行优化迭代。首先,构造目标函数和约束条件如下:
s.t.Δulow≤Δu(k+j-1)≤Δuup
ΔΔulow≤ΔΔu(k+j-1)≤ΔΔuup
elow≤Δe(k+j)≤eup
其中,k表示时刻,Δe(k+j)表示第(k+j)时刻下实际运动轨迹与参考运动轨迹的坐标差值,Δu(k+j-1)表示第(k+j-1)时刻下的控制量,每次使用当前时刻的控制量以及下一时刻的预测坐标误差开始进行求和迭代;Np表示预测范围,Nu表示控制范围,二者均为超参数;Q,R均为常数系数矩阵,分别作为坐标差值以及控制量的权重控制矩阵;uk+1,…,uk+Nu表示最终要求解的控制序列;约束条件中Δulow,Δuup分别代表控制量的控制下限和控制上限,ΔΔu(k+j-1)表示最优控制量,ΔΔulow,ΔΔuup表示最优控制下限和最优控制上限,同理elow,eup表示坐标差值下限和坐标差值上限。
假设在第k-1时刻得到的最优控制序列的增量为:
ΔΔu*(k-1)=[ΔΔu*(k-1|k-1),…,ΔΔu*(k+Nu-2|k-1)]
结合热启动技术,将k-1时刻的最优控制增量去除后,剩余的预测控制增量组合成新的控制序列作为下一次优化迭代的初始值输入到目标函数中,其中,最优控制增量为最优控制序列中第一个增量,且最优控制增量用于作为当前时刻机器人的控制量对机器人的运动加以控制。因此,k时刻优化迭代的初始输入为:
ΔΔushift(k)=[ΔΔu*(k|k-1),…,ΔΔu*(k+Nu-2|k-1),ΔΔu*(k+Nu-2|k-1)]
由于每次迭代的初始值序列长度均为Nu,因此将k-1时刻的最后一个预测控制量复制一次,将整体作为k时刻优化迭代的初始值。
由于每一次迭代的初始控制序列都是上一次迭代的最优控制序列,因此迭代收敛的速度加快,同时每次迭代后得到的最优控制增量之间的差距不会很大,以保证机器人运动的平稳性。
请参阅图3,图3是本申请提供的一种机器人轨迹跟踪控制方法另一实施例的流程示意图。本实施例具体包括步骤31至32:
步骤31:对机器人在控制范围内的所有实际运动参数进行优化迭代得到优化运动参数序列;
上述公式中已给出相关内容推导,通过每次输入实际运动参数序列进行优化迭代以得到优化运动参数序列;
步骤32:以优化运动参数序列中的当前优化运动参数控制机器人运动。
在优化运动参数序列中,每次选择当前序列的第一个最优控制增量作为控制机器人运动的控制参数,机器人按照控制参数继续运动。
根据实施例2和实施例3可以得到,通过每次对实际运动轨迹与参考运动轨迹的坐标差值以及控制量的和进行优化迭代,可以更新优化控制序列。通过将前一时刻迭代的优化控制序列作为当前时刻优化迭代的从初始控制序列,有效加快迭代收敛速度,控制机器人平稳运动的同时,使得机器人的轨迹跟踪精度提高。
根据实施例1至实施例3,参阅图4,图4是本申请提供的一种磁性医疗机器人一实施例的结构示意图。
磁性医疗机器人100中包括处理器110和存储器120。其中处理器110和存储器120耦接。存储器120中存储有计算机程序,计算机程序用于执行上述机器人轨迹跟踪控制方法。
具体参阅图5,图5是本申请提供的计算机可读存储介质一实施例的结构示意图。
计算机可读存储介质200中包括程序数据210。程序数据210在被处理器执行时,可以实现上述机器人轨迹跟踪控制方法。
为了充分说明本发明的优点,本发明分别跟踪一个半径为10厘米的圆形轨迹和八字形轨迹。首先参阅图6,图6是本申请提供的圆形轨迹跟踪结果图。该跟踪测试在两种采样时间间隔下进行,分别为Ts=0.01和Ts=0.05。根据图6可以观察到,从圆形最下方偏右测轨迹跟踪效果来看,使用主动集法(有效集法)跟踪轨迹误差较大,与参考轨迹的距离较远,而基于热启动的主动集法能够更加贴近参考轨迹,说明本发明的轨迹跟踪控制精度更高。
图7是本申请提供的圆形轨迹跟踪中每一次迭代的时间图。观察图7可以发现,使用主动集法进行迭代时达到收敛所花费的时间较长,而基于热启动的主动集法很显然加快了每一次迭代的收敛速度。
同时,本申请在另一种参考轨迹中进行了测试,参阅图8,图8是本申请提供的八字形轨迹跟踪结果图。同样的,可以观察到在参考轨迹变化较大的位置,基于主动集法的轨迹跟踪效果较差,与参考轨迹之间的偏差较大,而基于热启动的主动集法跟踪效果较好,基本与参考轨迹无偏差或偏差较小。参阅图9,图9是是本申请提供的八字形轨迹跟踪每一次迭代的时间图,同样可以发现,基于热启动的主动集法每一次迭代的收敛时间小于主动集法。
通过以上两个测试,充分证明了本申请具有轨迹跟踪精度高,机器人运行平稳性以及优化迭代收敛速度快等优点。
区别于现有技术的情况,本申请提出一种机器人轨迹跟踪控制方法。首先获取机器人基于实际运动参数的实际运动轨迹坐标以及参考轨迹坐标;计算实际轨迹坐标和参考轨迹坐标的坐标误差;以坐标误差的最小化作为迭代目标,对机器人的实际运动参数进行优化迭代得到优化运动参数;其中,每次迭代的初始值均采用上一次迭代的优化值;以优化运动参数控制机器人运动。本发明根据三自由度的运动学模型和误差模型,采用模型预测控制理论构造迭代优化目标函数,对预测坐标误差和以及预测控制量和进行优化迭代。通过将每次迭代优化得到的优化运动参数序列作为下一时刻优化迭代的初始运动序列,从而加快迭代计算的收敛速度,同时通过上述方式提高了对机器人的轨迹跟踪控制精度,使机器人的运动状态更加平稳。
以上所述仅为本申请的实施例,并非因此限制本申请的专利范围,凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换,或直接或间接运用在其他相关的技术领域,均同理包括在本申请的专利保护范围内。
Claims (7)
1.一种机器人的轨迹跟踪控制方法,其特征在于,所述方法包括:
获取所述机器人基于实际运动参数的实际轨迹坐标以及参考轨迹坐标;
以参考运动参数构建状态系数矩阵,计算所述状态系数矩阵与坐标差值的乘积,所述实际运动参数与控制系数矩阵的乘积;将两乘积之和作为坐标误差;其中,所述参考运动参数为其中,vr表示参考运动速度,wr表示参考运动角速度;所述状态系数矩阵为所述控制系数矩阵为/>所述坐标误差为/>其中,e1、e2、e3表示所述实际轨迹坐标和所述参考轨迹坐标的坐标差值;
以所述坐标误差的最小化作为迭代目标,对所述机器人的实际运动参数进行优化迭代得到优化运动参数;其中,每次迭代的初始值均采用上一次迭代的优化值;
以所述优化运动参数控制所述机器人运动。
2.根据权利要求1所述的轨迹跟踪控制方法,其特征在于,所述获取所述机器人基于实际运动参数的实际轨迹坐标以及参考轨迹坐标,包括:
获取预测范围内每个步长下所述机器人基于实际运动参数的实际轨迹坐标;
所述计算所述实际轨迹坐标和参考轨迹坐标的坐标误差,包括:
计算每个步长下所述实际轨迹坐标和所述参考轨迹坐标的坐标误差;
所述以所述坐标误差的最小化作为迭代目标,包括:
以所有步长下坐标误差中的所述坐标差值之和,以及控制范围内实际运动参数之和的最小化作为迭代目标。
3.根据权利要求2所述的轨迹跟踪控制方法,其特征在于,所述预测范围大于或等于所述控制范围。
4.根据权利要求2所述的轨迹跟踪控制方法,其特征在于,所述以所述坐标误差的最小化作为迭代目标,对所述机器人的实际运动参数进行优化迭代得到优化运动参数,还包括:
以所述实际运动参数在预设范围内、所述优化运动参数在预设范围内以及所述坐标差值在预设范围内为约束条件。
5.根据权利要求1所述的轨迹跟踪控制方法,其特征在于,所述对所述机器人的实际运动参数进行优化迭代得到优化运动参数,包括:
对所述机器人在控制范围内的所有实际运动参数进行优化迭代得到优化运动参数序列;
所述以所述优化运动参数控制所述机器人运动,包括:
以所述优化运动参数序列中的当前优化运动参数控制所述机器人运动。
6.一种磁性医疗机器人,其特征在于,所述磁性医疗机器人包括处理器以及与所述处理器耦接的存储器,所述存储器中存储有计算机程序,所述处理器用于执行所述计算机程序以实现如权利要求1-5任一项所述的方法。
7.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质中存储有程序数据,所述程序数据在被处理器执行时,用于实现如权利要求1-5任一项所述的方法。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111295113.5A CN114019798B (zh) | 2021-11-03 | 2021-11-03 | 机器人轨迹跟踪控制方法、磁性医疗机器人及存储介质 |
PCT/CN2021/137811 WO2023077618A1 (zh) | 2021-11-03 | 2021-12-14 | 机器人轨迹跟踪控制方法、磁性医疗机器人及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111295113.5A CN114019798B (zh) | 2021-11-03 | 2021-11-03 | 机器人轨迹跟踪控制方法、磁性医疗机器人及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114019798A CN114019798A (zh) | 2022-02-08 |
CN114019798B true CN114019798B (zh) | 2023-08-11 |
Family
ID=80060390
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111295113.5A Active CN114019798B (zh) | 2021-11-03 | 2021-11-03 | 机器人轨迹跟踪控制方法、磁性医疗机器人及存储介质 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN114019798B (zh) |
WO (1) | WO2023077618A1 (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115494751A (zh) * | 2022-05-09 | 2022-12-20 | 深圳先进技术研究院 | 控制器训练方法、轨迹跟踪方法、终端设备以及存储介质 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109885052A (zh) * | 2019-02-26 | 2019-06-14 | 华南理工大学 | 基于全向移动机器人运动学建模的误差模型预测控制方法 |
CN110948504A (zh) * | 2020-02-20 | 2020-04-03 | 中科新松有限公司 | 机器人加工作业法向恒力跟踪方法和装置 |
CN111324146A (zh) * | 2020-03-09 | 2020-06-23 | 河海大学常州校区 | 一种水下巡检机器人的轨迹跟踪控制方法 |
CN111872933A (zh) * | 2019-11-25 | 2020-11-03 | 浙江大学宁波理工学院 | 一种基于改进二次型迭代学习控制的scara机器人轨迹跟踪控制方法 |
CN112650217A (zh) * | 2020-12-03 | 2021-04-13 | 南京理工大学 | 一种基于评估函数的机器人轨迹跟踪策略动态优化方法 |
CN113146600A (zh) * | 2021-03-25 | 2021-07-23 | 中山大学 | 基于运动学迭代学习控制的柔性机器人轨迹规划方法及装置 |
CN113342003A (zh) * | 2021-07-14 | 2021-09-03 | 北京邮电大学 | 基于开闭环pid型迭代学习的机器人轨迹跟踪控制方法 |
CN113485389A (zh) * | 2021-08-03 | 2021-10-08 | 艾乐尔机器人科技(南京)有限公司 | 一种基于自适应预测的水下机器人三维轨迹跟踪控制方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6632487B2 (ja) * | 2016-07-13 | 2020-01-22 | キヤノン株式会社 | 連続体ロボット、その運動学の補正方法、および連続体ロボットの制御方法 |
CN112947430B (zh) * | 2021-02-03 | 2022-07-15 | 浙江工业大学 | 一种移动机器人的智能轨迹跟踪控制方法 |
-
2021
- 2021-11-03 CN CN202111295113.5A patent/CN114019798B/zh active Active
- 2021-12-14 WO PCT/CN2021/137811 patent/WO2023077618A1/zh unknown
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109885052A (zh) * | 2019-02-26 | 2019-06-14 | 华南理工大学 | 基于全向移动机器人运动学建模的误差模型预测控制方法 |
CN111872933A (zh) * | 2019-11-25 | 2020-11-03 | 浙江大学宁波理工学院 | 一种基于改进二次型迭代学习控制的scara机器人轨迹跟踪控制方法 |
CN110948504A (zh) * | 2020-02-20 | 2020-04-03 | 中科新松有限公司 | 机器人加工作业法向恒力跟踪方法和装置 |
CN111324146A (zh) * | 2020-03-09 | 2020-06-23 | 河海大学常州校区 | 一种水下巡检机器人的轨迹跟踪控制方法 |
CN112650217A (zh) * | 2020-12-03 | 2021-04-13 | 南京理工大学 | 一种基于评估函数的机器人轨迹跟踪策略动态优化方法 |
CN113146600A (zh) * | 2021-03-25 | 2021-07-23 | 中山大学 | 基于运动学迭代学习控制的柔性机器人轨迹规划方法及装置 |
CN113342003A (zh) * | 2021-07-14 | 2021-09-03 | 北京邮电大学 | 基于开闭环pid型迭代学习的机器人轨迹跟踪控制方法 |
CN113485389A (zh) * | 2021-08-03 | 2021-10-08 | 艾乐尔机器人科技(南京)有限公司 | 一种基于自适应预测的水下机器人三维轨迹跟踪控制方法 |
Also Published As
Publication number | Publication date |
---|---|
CN114019798A (zh) | 2022-02-08 |
WO2023077618A1 (zh) | 2023-05-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US7495537B2 (en) | Method and apparatus for dynamic magnetic field control using multiple magnets | |
CN106970594B (zh) | 一种柔性机械臂的轨迹规划方法 | |
Qiu et al. | Disturbance observer based adaptive model predictive control for uncalibrated visual servoing in constrained environments | |
CN112417755A (zh) | 一种主从式手术机器人轨迹预测控制方法 | |
CN109960145B (zh) | 移动机器人混合视觉轨迹跟踪策略 | |
WO2018019111A1 (zh) | 机器人及其关节运动控制方法和装置 | |
Ko et al. | Trajectory following for a flexible probe with state/input constraints: An approach based on model predictive control | |
CN114019798B (zh) | 机器人轨迹跟踪控制方法、磁性医疗机器人及存储介质 | |
CN116533249A (zh) | 基于深度强化学习的机械臂控制方法 | |
CN116661469B (zh) | 机器人轨迹误差修正方法及*** | |
Yu et al. | Modeling electromagnetic navigation systems for medical applications using random forests and artificial neural networks | |
Huang et al. | 4-dof visual servoing of a robotic flexible endoscope with a predefined-time convergent and noise-immune adaptive neural network | |
Qi et al. | Model predictive manipulation of compliant objects with multi-objective optimizer and adversarial network for occlusion compensation | |
Carlési et al. | Nonlinear model predictive running control of kangaroo robot: A one-leg planar underactuated hopping robot | |
Thamo et al. | Data-driven steering of concentric tube robots in unknown environments via dynamic mode decomposition | |
CN110262230A (zh) | 一种基于mpc算法的柔性针穿刺路径跟踪控制方法 | |
Liu et al. | Efficient reinforcement learning control for continuum robots based on inexplicit prior knowledge | |
Tang et al. | An improved H-infinity unscented FastSLAM with adaptive genetic resampling | |
Afzali et al. | A Modified Convergence DDPG Algorithm for Robotic Manipulation | |
Zhang et al. | A fully distributed multi-robot navigation method without pre-allocating target positions | |
Korayem et al. | Analysis and experimental study of non-holonomic mobile manipulator in presence of obstacles for moving boundary condition | |
Seekircher et al. | A closed-loop gait for humanoid robots combining LIPM with parameter optimization | |
Eberhard et al. | Sensor data fusion for the localization and position control of one kind of omnidirectional mobile robots | |
CN116476078B (zh) | 一种基于激光雷达的蛇形机器人目标稳定跟踪方法 | |
Aguilar et al. | PSO trajectory planner using kinematic controllers that ensure smooth differential robot velocities |
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 |