CN111603241B - 一种基于差分粒子滤波的医疗机器人定位装置和改进方法 - Google Patents

一种基于差分粒子滤波的医疗机器人定位装置和改进方法 Download PDF

Info

Publication number
CN111603241B
CN111603241B CN202010476353.4A CN202010476353A CN111603241B CN 111603241 B CN111603241 B CN 111603241B CN 202010476353 A CN202010476353 A CN 202010476353A CN 111603241 B CN111603241 B CN 111603241B
Authority
CN
China
Prior art keywords
particle
state
medical robot
needle
noise
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
CN202010476353.4A
Other languages
English (en)
Other versions
CN111603241A (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.)
Tongji Medical College of Huazhong University of Science and Technology
Beihang University
Original Assignee
Tongji Medical College of Huazhong University of Science and Technology
Beihang University
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 Tongji Medical College of Huazhong University of Science and Technology, Beihang University filed Critical Tongji Medical College of Huazhong University of Science and Technology
Publication of CN111603241A publication Critical patent/CN111603241A/zh
Application granted granted Critical
Publication of CN111603241B publication Critical patent/CN111603241B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Algebra (AREA)
  • Surgery (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Robotics (AREA)
  • Operations Research (AREA)
  • Computing Systems (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Medical Informatics (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Manipulator (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

本发明公开了一种基于差分粒子滤波的医疗机器人定位装置和改进方法，属于软组织自动穿刺手术装置的改进，包括图像传感系统、电控系统、执行系统和上位机；模型计算器，状态扩展器和差分粒子滤波器；模型计算器根据装配柔性针的医疗机器人的运动学特性，在惯性坐标系XOY下搭建非高斯噪声误差的针尖运动学平面模型；状态扩展器对针尖运动学平面模型进行改进，消除模型误差和测量误差中的有色噪声，得到只含白噪声的扩展状态方程并进行简化；最后对只含白噪声的扩展状态方程结合差分粒子滤波理论搭建差分粒子滤波估计器处理白噪声，对柔性针针尖位置和姿态进行估计。本发明能有效处理非高斯噪声的模型误差与测量误差，精确估计医疗机器人的位置姿态。

Description

一种基于差分粒子滤波的医疗机器人定位装置和改进方法
技术领域
本发明属于软组织自动穿刺手术装置的改进，涉及对位置定位模块估计算法的改进，具体是一种基于差分粒子滤波的医疗机器人定位装置和改进方法。
背景技术
近年来，基于医疗机器人的软组织穿刺技术不断发展，由于创口微小、恢复较快等特点，受到研究人员的青睐。如，出版于2015年1月，Minimally Invasive Therapy&AlliedTechnologies Mitat Official Journal of the Society for Minimally InvasiveTherapy，名称为“Robotic systems for percutaneous needle-guided interventions”，作者Joachim，Kettenbach，Gernot。此文介绍了基于医疗机器人的自动穿刺手术装置，该装置由工作站、监视器、C形臂和穿刺医疗机器人组成。为了方便说明具体系统结构，请参考图1所示。自动穿刺手术通过设定穿刺医疗机器人位置作为输入，来控制医疗机器人运动，具体的控制方式与系统介绍如下。如，出版于2012年3月，第34卷第2期，《机器人ROBOT》，名称为“微创血管介入手术机器人控制系统与零位定位装置设计”，作者段星光，陈悦，于华涛。此文介绍了穿刺用医疗机器人的控制系统具体组成，包括PMAC卡，驱动器与电机。为了清楚地说明，请参考图2所示。
穿刺装置最重要的目的就是控制医疗机器人精确达到手术位置，进行给药或者操作。精准控制的前提是获取医疗机器人的精确位置。然而，由于其装配柔性针的医疗机器人的横截面直径较小，在装配柔性针的医疗机器人针尖放置传感器并不可行。现存的超声等定位技术对医疗机器人的位置进行测量的精度较低。于是，需要设计估计算法对针尖的位置与姿态信息进行估计。
研究人员普遍遇到的问题是：其一，测量针尖位置的噪声不可视为高斯噪声；其二，由于装配柔性针的医疗机器人在软组织内的运动比较复杂，很难建立精确和用于实时计算的模型，所以在估计中，简化模型的模型误差也不可简单视为高斯噪声。而研究人员使用成熟的估计算法进行估计时，一般只能处理高斯噪声，对位置和姿态进行估计时误差比较大。为了解决这一技术问题，本发明提出了在传统定位方法中引入改进估计器来解决非高斯噪声的问题
发明内容
本发明的目的是针对装配柔性针的医疗机器人的精准定位需求，提出一种基于差分粒子滤波的医疗机器人定位装置，并开发了对应的位置估计方法，以克服定位过程中的较大误差。
所述的基于差分粒子滤波的医疗机器人定位装置，包括图像传感系统、电控系统、执行系统和上位机。其中，图像传感系统包括摄像头和图像处理模块，由摄像头采集在明胶中移动的装载柔性针的医疗机器人的图像，由图像处理模块对图像信号进行初步处理。电控系统包括医疗机器人控制信号的计算与执行驱动器，实现医疗机器人的控制。上位机负责基于差分粒子滤波的医疗机器人位置估计的计算，为医疗机器人控制提供精确的反馈量。
针对基于差分粒子滤波的医疗机器人的位置估计，最主要的问题在于解决实际情况下的噪声问题，由模型误差造成的过程噪声，由摄像头精度，以及在明胶中图像不清晰造成的测量噪声都不可以视为简单的高斯噪声。于是，为了在装配柔性针的医疗机器人控制中获得精确的针尖位置与姿态估计信息，本发明改进了传统的基于差分粒子滤波的估计方法，使其可以处理非高斯噪声的模型误差与测量误差，具体是一种基于差分粒子滤波算法的装配柔性针的医疗机器人针尖位置与姿态估计改进方法。
具体步骤如下：
步骤一、根据装配柔性针的医疗机器人的运动学特性，在惯性坐标系下搭建非高斯噪声误差的针尖运动学平面模型；
装配柔性针的医疗机器人的运动学特性与装配柔性针的医疗机器人的给进运动和自转运动相关，给进运动实现针的穿刺，而针的自转则是随着针尖的方向而改变的。
针尖运动学平面模型包括模型误差和测量误差，具体公式如下：
Figure GDA0004215791380000021
v为装配柔性针的医疗机器人针尖处的速度；
Figure GDA0004215791380000022
为针尖在惯性坐标系XOY下x轴方向上的速度；/>
Figure GDA0004215791380000023
为针尖在惯性坐标系XOY下y轴方向上的速度；/>
Figure GDA0004215791380000024
为在XOY坐标系下装配柔性针的医疗机器人针尖运动方向与x轴的夹角；ρ为针尖运动的曲率半径。
步骤二、将针尖运动学平面模型，先通过状态扩展消除模型误差中的有色噪声，再通过差分同类观测消除测量误差中的有色噪声，得到只含白噪声的扩展状态方程并进行简化；
具体步骤为：
首先，将针尖运动学平面模型进行改进，得到离散形式的针尖运动学平面模型；
公式如下：
Figure GDA0004215791380000031
Figure GDA0004215791380000032
ηk+1＝L1ηk+ωk
Δk+1＝L2Δk+vk
状态
Figure GDA0004215791380000033
为3×1的状态向量；/>
Figure GDA0004215791380000034
为3×1的观测向量；/>
Figure GDA0004215791380000035
为针尖运动学平面模型中显示的非线性函数；ηk为白噪声驱动的模型误差中的有色噪声，是由参数不确定性或建模的不精确带来的；/>
Figure GDA0004215791380000036
为模型误差中的有色噪声ηk的输入矩阵；Δk为白噪声驱动的测量误差中的有色噪声，是由传感器误差带来的；ωk为模型误差中的有色噪声ηk中的白噪声随机向量，vk为测量误差中的有色噪声Δk中的白噪声随机向量；L1为模型误差中的有色噪声ηk的状态增益。L2为测量误差中的有色噪声Δk的状态增益。k是迭代参数，k＝1,2,...n。
然后，通过状态扩展和差分同类观测分别消除模型误差中的有色噪声ηk和测量误差中的有色噪声Δk，分别得到只含白噪声的扩展状态方程并简化；
扩展状态方程为：
Figure GDA0004215791380000037
Figure GDA0004215791380000038
I为单位矩阵；
简化得到：
xk+1＝A(xk)xk+Gωk
yk＝Cxk+vk
其中，增广状态
Figure GDA0004215791380000039
为4×1的状态向量；观测向量/>
Figure GDA00042157913800000310
增广状态转移矩阵/>
Figure GDA00042157913800000311
增广噪声转移矩阵/>
Figure GDA00042157913800000312
增广输出矩阵C＝[f-L2 B]。
步骤三、对简化后只含白噪声的扩展状态方程，利用差分粒子滤波器处理白噪声，结合差分粒子滤波算法对装配柔性针的医疗机器人的针尖位置和姿态进行估计；
差分粒子滤波算法具体过程为：
步骤301、在迭代参数k＝0时，初始化各参数值，同时生成粒子；
初始化参数，参数包括粒子
Figure GDA00042157913800000313
粒子先验信息/>
Figure GDA00042157913800000314
及协方差矩阵/>
Figure GDA00042157913800000315
粒子先验信息公式为：
Figure GDA00042157913800000316
协方差矩阵公式为：
Figure GDA00042157913800000317
进一步简化为：
Figure GDA00042157913800000318
其中，
Figure GDA00042157913800000319
为Cholesky因式分解因子；i＝1,2,…N，代表第i个粒子；每个粒子的状态即为不同的针尖位置和姿态。
步骤302、当迭代参数k≥1，计算每个粒子的先验信息及协方差矩阵；
步骤303、利用每个粒子的先验信息及协方差矩阵预测各自的状态；
预测是对待求的装配柔性针的医疗机器人的针尖位置和姿态，根据先验信息进行一步预测。
首先，利用第i个粒子的先验信息得到该粒子的状态估计值与观测估计值；
公式如下：
Figure GDA0004215791380000041
Figure GDA0004215791380000042
Figure GDA0004215791380000043
为第i个粒子的状态向量估计值；/>
Figure GDA0004215791380000044
为第i个粒子的观测向量估计值；
然后，利用该粒子的状态向量估计值与观测向量估计值计算协方差矩阵；
第i个粒子的状态向量估计值的协方差矩阵
Figure GDA0004215791380000045
公式如下：
Figure GDA0004215791380000046
Qk为状态扩展方程中只含白噪声的过程噪声的协方差矩阵；
Figure GDA0004215791380000047
为第i个粒子的状态向量与状态向量估计值两个值的一阶均差矩阵；公式为：
Figure GDA0004215791380000048
d为差值步长；/>
Figure GDA0004215791380000049
为矩阵/>
Figure GDA00042157913800000410
的第j列，j＝1,2,3,4；
将状态向量估计值协方差矩阵
Figure GDA00042157913800000411
表达为二次型的形式：/>
Figure GDA00042157913800000412
Figure GDA00042157913800000413
为矩阵/>
Figure GDA00042157913800000414
的观测值；
步骤304、对预测状态后的各个粒子的状态值不断迭代更新权值，取得后验概率表示为粒子的新状态。
首先，针对第i个粒子计算其观测向量与状态向量估计值的一阶均差矩阵
Figure GDA00042157913800000415
Figure GDA00042157913800000416
Figure GDA00042157913800000417
为观测值/>
Figure GDA00042157913800000418
的第j列，j＝1,2,3,4；
然后，利用一阶均差矩阵
Figure GDA00042157913800000419
计算迭代滤波算法中一步预测的协方差矩阵/>
Figure GDA00042157913800000420
Figure GDA00042157913800000421
利用一阶均差矩阵
Figure GDA00042157913800000422
和观测值/>
Figure GDA00042157913800000423
得到交互式协方差矩阵/>
Figure GDA00042157913800000424
Figure GDA00042157913800000425
利用协方差矩阵
Figure GDA00042157913800000426
和交互式协方差矩阵/>
Figure GDA00042157913800000427
得到滤波增益矩阵/>
Figure GDA00042157913800000428
Figure GDA00042157913800000429
利用滤波增益矩阵
Figure GDA00042157913800000430
和该粒子的状态估计值，结合观测向量得到第i个粒子的状态估计值/>
Figure GDA00042157913800000431
Figure GDA0004215791380000051
最终得到第i个粒子的估计协方差矩阵
Figure GDA0004215791380000052
Figure GDA0004215791380000053
Figure GDA0004215791380000054
表示上一时刻的粒子估计协方差矩阵，通过迭代进行计算。
步骤305、根据原始粒子、粒子状态估计值，以及预测得到的协方差矩阵对粒子进行抽样；
抽样公式如下：
Figure GDA0004215791380000055
步骤306、针对所抽样的粒子，计算粒子权值
Figure GDA0004215791380000056
及归一化权值/>
Figure GDA0004215791380000057
权值代表了粒子状态接近真实值的概率：
Figure GDA0004215791380000058
Figure GDA0004215791380000059
步骤307、根据抽样粒子和粒子权值，判断是否残差重采样，如果是，则重新采集粒子样本后计算估算值。否则，计算估算值；
根据有效样本数Neff(Effective sample size)来衡量粒子集的退化程度，其计算公式为：
Figure GDA00042157913800000510
预先设定重采样阈值Nth，若Nef＜Nth，将权值重新设置为
Figure GDA00042157913800000511
得到相等权值样本集/>
Figure GDA00042157913800000512
则滤波分布的后验概率/>
Figure GDA00042157913800000513
系统状态估计/>
Figure GDA00042157913800000514
及估计协方差矩阵/>
Figure GDA00042157913800000515
分别为
Figure GDA00042157913800000516
Figure GDA00042157913800000517
Figure GDA00042157913800000518
其中δ(x)为狄克拉函数。
步骤308、接着将残差重采样的结果，返回预测与更新步骤进行下一次迭代计算。
步骤309、达到所设置的滤波步长时，得到的结果即使用差分粒子滤波方法对针尖位置与姿态的准确估计。
步骤四、对准确估计的针尖位置与姿态进行仿真验证。
本发明的优点在于：
一种基于差分粒子滤波算法的装配柔性针的医疗机器人针尖位置与姿态估计改进方法，调整了装配柔性针的医疗机器人运动学模型，基于此改进模型，设计了差分粒子滤波估计方法，可以有效处理非高斯噪声的模型误差与测量误差，在惯性坐标系下对装配柔性针的医疗机器人的位置与姿态做出精确的估计。
附图说明
图1为基于医疗机器人的自动穿刺手术装置图。
图2为穿刺用医疗机器人的控制系统。
图3为本发明基于差分粒子滤波的医疗机器人定位装置简图。
图4为本发明基于差分粒子滤波算法的医疗机器人针尖位置与姿态估计改进方法的流程图。
图5为本发明装配柔性针的医疗机器人的运动学模型示意图。
图6为本发明利用差分粒子滤波器处理白噪声得到针尖位置和姿态估计值的流程图；
图7为本发明的仿真结果图。
具体实施方式
下面将结合附图和实例对本发明作进一步的详细说明。
本发明的目的是针对装配柔性针的医疗机器人的精准定位需求，提出一种基于差分粒子滤波的医疗机器人定位装置，并开发了对应的位置估计方法，以克服定位过程中的较大误差。
本发明提供了一种基于差分粒子滤波的医疗机器人定位装置，包括图像传感系统、电控系统、执行系统和上位机，如图3所示。其中，图像传感系统包括摄像头和图像处理系统，由摄像头采集在明胶中移动的装载柔性针的医疗机器人的图像，由图像处理系统对图像信号进行初步处理。电控系统包括医疗机器人控制系统与执行驱动器，实现医疗机器人的控制。上位机负责基于差分粒子滤波的医疗机器人位置估计的计算，为医疗机器人控制提供精确的反馈量。
针对基于差分粒子滤波的医疗机器人的位置估计，最主要的问题在于解决实际情况下的噪声问题，由模型误差造成的过程噪声，由摄像头精度，以及在明胶中图像不清晰造成的测量噪声都不可以视为简单的高斯噪声。于是，本发明针对装配柔性针的医疗机器人针尖位置与姿态估计中，模型误差与测量误差均为非高斯噪声，普通估计算法容易发散的问题，提出了一种改进的基于差分粒子滤波算法的估计算法，是一种切实有效的方案。如图4所示。
1.名义模型计算器
根据装配柔性针的医疗机器人的运动学特性，在惯性坐标系XOY下搭建非高斯噪声误差的针尖运动学平面模型；
装配柔性针的医疗机器人的运动学特性与装配柔性针的医疗机器人的给进运动和自转运动相关，给进运动实现针的穿刺，而针的自转则是随着针尖的方向而改变的。如同自行车，后轮给进，前轮决定前进的方向，根据装配柔性针的医疗机器人的运动学特性，搭建装配柔性针的医疗机器人运动模型如图5所示。
针尖运动学平面模型包括模型误差和测量误差，将针尖所处坐标表示为(X，Y)；则针尖(X，Y)处的速度表示为：
Figure GDA0004215791380000071
其中，v为装配柔性针的医疗机器人针尖处的速度；
Figure GDA0004215791380000072
为针尖在惯性坐标系XOY下x轴方向上的速度；/>
Figure GDA0004215791380000073
为针尖在惯性坐标系XOY下y轴方向上的速度；/>
Figure GDA0004215791380000074
为在XOY坐标系下装配柔性针的医疗机器人针尖运动方向与x轴的夹角；
针尖运动学约束为：
Figure GDA0004215791380000075
通过联立方程(1)、(2)，可以得到
Figure GDA0004215791380000076
利用图示运动关系，可解得横摆角速度ω为：
Figure GDA0004215791380000077
其中，θ为针尖与x轴夹角的变化量；ρ为针尖运动的曲率半径，曲率半径由针尖的转角r确定:
Figure GDA0004215791380000078
结合上式得到针尖的运动学模型如下：
Figure GDA0004215791380000079
2.状态扩展器
对针尖运动学平面模型进行改进，先通过状态扩展消除模型误差中的有色噪声，再通过差分同类观测消除测量误差中的有色噪声，得到只含白噪声的扩展状态方程并进行简化；
具体步骤为：
首先，将针尖运动学平面模型公式(6)改写为离散形式，设定模型误差与测量误差均为非高斯噪声，公式如下：
Figure GDA0004215791380000081
状态
Figure GDA0004215791380000082
为3×1的状态向量；/>
Figure GDA0004215791380000083
为3×1的观测向量；/>
Figure GDA0004215791380000084
为如公式(6)所示的非线性函数；ηk为白噪声驱动的模型误差中的有色噪声，是由参数不确定性或建模的不精确带来的；/>
Figure GDA0004215791380000085
为模型误差中的有色噪声ηk的输入矩阵；Δk为白噪声驱动的测量误差中的有色噪声，是由传感器误差带来的；ωk为模型误差中的有色噪声ηk中的高斯白噪声随机向量，vk为测量误差中的有色噪声Δk中的高斯白噪声随机向量；L1为模型误差中的有色噪声ηk的状态增益。L2为测量误差中的有色噪声Δk的状态增益。k是迭代参数，k＝1,2,...n。
然后对有色噪声进行白化处理，有色噪声在成熟的估计算法中无法进行有效的处理，会造成估计结果的发散。所以要将模型扩展为增广模型，将有色噪声变为状态量的一部分；其方法是，首先通过状态扩展消除模型误差中的有色噪声ηk，再通过差分同类观测值可消除测量误差中的有色噪声Δk，由此得到只含白噪声的增广系统的扩展状态方程。
扩展状态方程为：
Figure GDA0004215791380000086
f为
Figure GDA0004215791380000087
的简写，为装配柔性针的医疗机器人运动学平面模型中显示的非线性函数；I为单位矩阵；去掉上标号*，并进行简化表达，得到：
Figure GDA0004215791380000088
其中，增广状态
Figure GDA0004215791380000089
为4×1的状态向量；观测向量/>
Figure GDA00042157913800000810
增广状态转移矩阵/>
Figure GDA00042157913800000811
增广噪声转移矩阵/>
Figure GDA00042157913800000812
增广输出矩阵C＝[f-L2 B]。
3.差分粒子滤波器
对简化后只含白噪声的扩展状态方程，结合差分粒子滤波理论搭建差分粒子滤波估计器，利用差分粒子滤波器处理白噪声，对装配柔性针的医疗机器人的针尖位置和姿态进行估计；
根据简化的增广系统公式(9)，搭建差分粒子滤波估计器。差分粒子滤波方法是一种比较成熟的可以处理模型误差和测量误差都是高斯白噪声的估计方法。本发明应用的是二阶差分粒子滤波算法，滤波方程中采用了协方差矩阵的平方根形式，不需要对函数的偏导数求解，与同样可以处理非高斯噪声的UKF滤波算法相比，具有更好的数值特性。
差分粒子滤波分为两个阶段，分别是初始化阶段和状态与更新阶段。首先设置初值，进而利用模型信息进行状态的预测与更新。如图6所示，具体过程为：
步骤301、在迭代参数k＝0时，初始化各参数值，同时生成粒子；
初始化参数，参数包括粒子
Figure GDA0004215791380000091
粒子先验信息/>
Figure GDA0004215791380000092
及协方差矩阵/>
Figure GDA0004215791380000093
粒子先验信息公式为：
Figure GDA0004215791380000094
协方差矩阵公式为：
Figure GDA0004215791380000095
进一步简化为：
Figure GDA0004215791380000096
其中，
Figure GDA0004215791380000097
为Cholesky因式分解因子；i＝1,2,…N，代表第i个粒子；每个粒子的状态即为不同的针尖位置和姿态。
步骤302、当迭代参数k≥1时状态预测与更新，计算每个粒子的先验信息及协方差矩阵；
步骤303、利用每个粒子的先验信息及协方差矩阵预测各自的状态；
预测是对待求的装配柔性针的医疗机器人的针尖位置和姿态，根据先验信息进行一步预测。
首先，利用第i个粒子的先验信息得到该粒子的状态估计值与观测估计值；
公式如下：
Figure GDA0004215791380000098
Figure GDA0004215791380000099
Figure GDA00042157913800000910
为第i个粒子的状态向量预测值；/>
Figure GDA00042157913800000911
为第i个粒子的观测向量预测值；
然后，利用该粒子的状态向量估计值与观测向量估计值计算协方差矩阵；
第i个粒子的状态向量预测值的协方差矩阵
Figure GDA00042157913800000912
公式如下：
Figure GDA00042157913800000913
Qk为状态扩展方程中只含白噪声的过程噪声的协方差矩阵；
Figure GDA00042157913800000914
为第i个粒子的状态向量与状态向量估计值两个值的一阶均差矩阵；公式为：
Figure GDA00042157913800000915
d为差值步长；/>
Figure GDA00042157913800000916
为矩阵/>
Figure GDA00042157913800000917
的第j列，j＝1,2,3,4；
将状态向量估计值协方差矩阵
Figure GDA00042157913800000918
表达为二次型的形式：/>
Figure GDA00042157913800000919
Figure GDA00042157913800000920
为矩阵/>
Figure GDA00042157913800000921
的观测值；
步骤304、对预测状态后的各个粒子的状态值不断迭代更新权值，取得后验概率表示为粒子的新状态。
首先，针对第i个粒子计算其观测向量与状态向量估计值的一阶均差矩阵
Figure GDA0004215791380000101
Figure GDA0004215791380000102
Figure GDA0004215791380000103
为观测值/>
Figure GDA0004215791380000104
的第j列，j＝1,2,3,4；
然后，利用一阶均差矩阵
Figure GDA0004215791380000105
计算迭代滤波算法中一步预测的协方差矩阵/>
Figure GDA0004215791380000106
Figure GDA0004215791380000107
利用一阶均差矩阵
Figure GDA0004215791380000108
和观测值/>
Figure GDA0004215791380000109
得到交互式协方差矩阵/>
Figure GDA00042157913800001010
Figure GDA00042157913800001011
利用协方差矩阵
Figure GDA00042157913800001012
和交互式协方差矩阵/>
Figure GDA00042157913800001013
得到滤波增益矩阵/>
Figure GDA00042157913800001014
Figure GDA00042157913800001015
利用滤波增益矩阵
Figure GDA00042157913800001016
和该粒子的状态估计值，结合观测向量得到第i个粒子的状态估计值/>
Figure GDA00042157913800001017
Figure GDA00042157913800001018
最终得到第i个粒子的估计协方差矩阵
Figure GDA00042157913800001019
Figure GDA00042157913800001020
Figure GDA00042157913800001021
表示上一时刻的粒子估计协方差矩阵，通过迭代进行计算。
步骤305、根据原始粒子、粒子状态估计值，以及预测得到的协方差矩阵对粒子进行抽样；
抽样公式如下：
Figure GDA00042157913800001022
步骤306、针对所抽样的粒子，计算粒子权值
Figure GDA00042157913800001023
及归一化权值/>
Figure GDA00042157913800001024
权值代表了粒子状态接近真实值的概率：
Figure GDA00042157913800001025
Figure GDA00042157913800001026
步骤307、根据抽样粒子和粒子权值，判断是否残差重采样，如果是，则重新采集粒子样本后计算估算值。否则，计算估算值；
根据有效样本数Neff(Effective sample size)来衡量粒子集的退化程度，其计算公式为：
Figure GDA00042157913800001027
预先设定重采样阈值Nth，若Nef＜Nth，将权值重新设置为
Figure GDA00042157913800001028
得到相等权值样本集/>
Figure GDA00042157913800001029
则滤波分布的后验概率/>
Figure GDA00042157913800001030
系统状态估计/>
Figure GDA00042157913800001031
及估计协方差矩阵/>
Figure GDA00042157913800001032
分别为
Figure GDA0004215791380000111
Figure GDA0004215791380000112
Figure GDA0004215791380000113
其中δ(x)为狄克拉函数。
步骤308、接着将残差重采样的结果，返回预测与更新步骤进行下一次迭代计算。
步骤309、达到所设置的滤波步长时，得到的结果即使用差分粒子滤波方法对针尖位置与姿态的准确估计。
按照时序运行以上程序，利用差分粒子滤波算法精确地预测出增广状态值
Figure GDA0004215791380000114
也就是说，可以同时精确地得到装配柔性针的医疗机器人的位置与姿态信息
Figure GDA0004215791380000115
以及非高斯噪声η。
在matlab中编程对传统装配柔性针的医疗机器人模型和改进的装配柔性针的医疗机器人模型进行状态估计，并与真实值进行对比，结果如图7所示，用于验证本发明所提出方法的准确性。

Claims (5)

1.一种基于差分粒子滤波的医疗机器人定位装置，其特征在于，包括图像传感系统、电控系统、执行系统和上位机；
其中，图像传感系统包括摄像头和图像处理模块，由摄像头采集在明胶中移动的装载柔性针的医疗机器人的图像，由图像处理模块对图像信号进行初步处理；电控系统包括医疗机器人控制信号的计算与执行驱动器，实现医疗机器人的控制；上位机负责基于差分粒子滤波的医疗机器人位置估计的计算，为医疗机器人控制提供精确的反馈量；
所述的定位装置还包括1)名义模型计算器，根据装配柔性针的医疗机器人的运动学特性，在惯性坐标系XOY下搭建非高斯噪声误差的针尖运动学平面模型；2)状态扩展器，对针尖运动学平面模型进行改进，先通过状态扩展消除模型误差中的有色噪声，再通过差分同类观测消除测量误差中的有色噪声，得到只含白噪声的扩展状态方程并进行简化；以及3)差分粒子滤波器，对简化后只含白噪声的扩展状态方程，结合差分粒子滤波理论搭建差分粒子滤波估计器，利用差分粒子滤波器处理白噪声，对医疗机器人装配的柔性针针尖位置和姿态进行估计。
2.如权利要求1所述的一种基于差分粒子滤波的医疗机器人定位装置，其特征在于，所述的名义模型计算器根据装配柔性针的医疗机器人的运动学特性，搭建装配柔性针的医疗机器人运动模型，具体过程如下：
装配柔性针的医疗机器人的运动学特性与装配柔性针的医疗机器人的给进运动和自转运动相关，给进运动实现针的穿刺，而针的自转则是随着针尖的方向而改变的；
针尖运动学平面模型包括模型误差和测量误差，具体公式如下：
Figure FDA0004215791370000011
v为装配柔性针的医疗机器人针尖处的速度；
Figure FDA0004215791370000012
为针尖在惯性坐标系XOY下x轴方向上的速度；/>
Figure FDA0004215791370000013
为针尖在惯性坐标系XOY下y轴方向上的速度；/>
Figure FDA0004215791370000014
为在XOY坐标系下装配柔性针的医疗机器人针尖运动方向与x轴的夹角；ρ为针尖运动的曲率半径。
3.如权利要求1所述的一种基于差分粒子滤波的医疗机器人定位装置，其特征在于，所述的状态扩展器简化白噪声的扩展状态方程具体步骤为：
首先，将针尖运动学平面模型进行改进，得到离散形式的针尖运动学平面模型；
公式如下：
Figure FDA0004215791370000021
Figure FDA0004215791370000022
ηk+1＝L1ηk+ωk
Δk+1＝L2Δk+vk
状态
Figure FDA0004215791370000023
为3×1的状态向量；/>
Figure FDA0004215791370000024
为3×1的观测向量；/>
Figure FDA0004215791370000025
为针尖运动学平面模型中显示的非线性函数；ηk为白噪声驱动的模型误差中的有色噪声，是由参数不确定性或建模的不精确带来的；/>
Figure FDA0004215791370000026
为模型误差中的有色噪声ηk的输入矩阵；Δk为白噪声驱动的测量误差中的有色噪声，是由传感器误差带来的；ωk为模型误差中的有色噪声ηk中的白噪声随机向量，vk为测量误差中的有色噪声Δk中的白噪声随机向量；L1为模型误差中的有色噪声ηk的状态增益；L2为测量误差中的有色噪声Δk的状态增益；k是迭代参数，k＝1,2,...n；
然后，通过状态扩展和差分同类观测分别消除模型误差中的有色噪声ηk和测量误差中的有色噪声Δk，分别得到只含白噪声的扩展状态方程并简化；
扩展状态方程为：
Figure FDA0004215791370000027
Figure FDA0004215791370000028
f为
Figure FDA0004215791370000029
的简写，为装配柔性针的医疗机器人运动学平面模型中显示的非线性函数；I为单位矩阵；
简化得到：
xk+1＝A(xk)xk+Gωk
yk＝Cxk+vk
其中，增广状态
Figure FDA00042157913700000210
为4×1的状态向量；观测向量/>
Figure FDA00042157913700000211
增广状态转移矩阵
Figure FDA00042157913700000212
增广噪声转移矩阵/>
Figure FDA00042157913700000213
增广输出矩阵C＝[f-L2 B]。
4.如权利要求1所述的一种基于差分粒子滤波的医疗机器人定位装置，其特征在于，所述的差分粒子滤波器处理白噪声，对医疗机器人装配的柔性针针尖位置和姿态进行估计；
差分粒子滤波算法具体过程为：
首先、在迭代参数k＝0时，初始化各参数值，同时生成粒子；迭代参数k≥1，计算每个粒子的先验信息，利用每个粒子的先验信息得到各粒子的状态估计值与观测估计值；利用各粒子的状态向量估计值与观测向量估计值计算协方差矩阵，预测各粒子的针尖位置和姿态状态；
然后、对预测状态后的各个粒子的状态值不断迭代更新权值，取得后验概率表示为粒子的新状态；并根据原始粒子、粒子状态估计值，以及预测得到的协方差矩阵对粒子进行抽样；
计算抽样的粒子的权值
Figure FDA0004215791370000031
及归一化权值/>
Figure FDA0004215791370000032
权值代表了粒子状态接近真实值的概率：
最后、根据抽样粒子和粒子权值，判断是否残差重采样，如果是，则重新采集粒子样本后计算估算值；否则，计算估算值；达到所设置的滤波步长时，得到的结果即使用差分粒子滤波方法对针尖位置与姿态的准确估计。
5.应用如权利要求1所述的一种基于差分粒子滤波的医疗机器人定位装置进行装配柔性针针尖位置与姿态估计改进方法，其特征在于，具体步骤如下：
步骤一、根据装配柔性针的医疗机器人的运动学特性，在惯性坐标系下搭建非高斯噪声误差的针尖运动学平面模型；
装配柔性针的医疗机器人的运动学特性与装配柔性针的医疗机器人的给进运动和自转运动相关，给进运动实现针的穿刺，而针的自转则是随着针尖的方向而改变的；
针尖运动学平面模型包括模型误差和测量误差，具体公式如下：
Figure FDA0004215791370000033
v为装配柔性针的医疗机器人针尖处的速度；
Figure FDA0004215791370000034
为针尖在惯性坐标系XOY下x轴方向上的速度；/>
Figure FDA0004215791370000035
为针尖在惯性坐标系XOY下y轴方向上的速度；/>
Figure FDA0004215791370000036
为在XOY坐标系下装配柔性针的医疗机器人针尖运动方向与x轴的夹角；ρ为针尖运动的曲率半径；
步骤二、将针尖运动学平面模型，先通过状态扩展消除模型误差中的有色噪声，再通过差分同类观测消除测量误差中的有色噪声，得到只含白噪声的扩展状态方程并进行简化；
具体步骤为：
首先，将针尖运动学平面模型进行改进，得到离散形式的针尖运动学平面模型；
公式如下：
Figure FDA0004215791370000037
Figure FDA0004215791370000038
ηk+1＝L1ηk+ωk
Δk+1＝L2Δk+vk
状态
Figure FDA0004215791370000039
为3×1的状态向量；/>
Figure FDA00042157913700000310
为3×1的观测向量；/>
Figure FDA00042157913700000311
为针尖运动学平面模型中显示的非线性函数；ηk为白噪声驱动的模型误差中的有色噪声，是由参数不确定性或建模的不精确带来的；/>
Figure FDA0004215791370000041
为模型误差中的有色噪声ηk的输入矩阵；Δk为白噪声驱动的测量误差中的有色噪声，是由传感器误差带来的；ωk为模型误差中的有色噪声ηk中的白噪声随机向量，vk为测量误差中的有色噪声Δk中的白噪声随机向量；L1为模型误差中的有色噪声ηk的状态增益；L2为测量误差中的有色噪声Δk的状态增益；k是迭代参数，k＝1,2,...n；
然后，通过状态扩展和差分同类观测分别消除模型误差中的有色噪声ηk和测量误差中的有色噪声Δk，分别得到只含白噪声的扩展状态方程并简化；
扩展状态方程为：
Figure FDA0004215791370000042
Figure FDA0004215791370000043
f为
Figure FDA0004215791370000044
的简写，为装配柔性针的医疗机器人运动学平面模型中显示的非线性函数；I为单位矩阵；
简化得到：
xk+1＝A(xk)xk+Gωk
yk＝Cxk+vk
其中，增广状态
Figure FDA0004215791370000045
为4×1的状态向量；观测向量/>
Figure FDA0004215791370000046
增广状态转移矩阵
Figure FDA0004215791370000047
增广噪声转移矩阵/>
Figure FDA0004215791370000048
增广输出矩阵C＝[f-L2 B]；
步骤三、对简化后只含白噪声的扩展状态方程，利用差分粒子滤波器处理白噪声，结合差分粒子滤波算法对装配柔性针的医疗机器人的针尖位置和姿态进行估计；
差分粒子滤波算法具体过程为：
步骤301、在迭代参数k＝0时，初始化各参数值，同时生成粒子；
初始化参数，参数包括粒子
Figure FDA0004215791370000049
粒子先验信息/>
Figure FDA00042157913700000410
及协方差矩阵/>
Figure FDA00042157913700000411
粒子先验信息公式为：
Figure FDA00042157913700000412
协方差矩阵公式为：
Figure FDA00042157913700000413
进一步简化为：/>
Figure FDA00042157913700000414
其中，
Figure FDA00042157913700000415
为Cholesky因式分解因子；i＝1,2,N，代表第i个粒子；每个粒子的状态即为不同的针尖位置和姿态；
步骤302、当迭代参数k≥1，计算每个粒子的先验信息及协方差矩阵；
步骤303、利用每个粒子的先验信息及协方差矩阵预测各自的状态；
预测是对待求的装配柔性针的医疗机器人的针尖位置和姿态，根据先验信息进行一步预测；
首先，利用第i个粒子的先验信息得到该粒子的状态估计值与观测估计值；
公式如下：
Figure FDA0004215791370000051
Figure FDA0004215791370000052
Figure FDA0004215791370000053
为第i个粒子的状态向量估计值；/>
Figure FDA0004215791370000054
为第i个粒子的观测向量估计值；
然后，利用该粒子的状态向量估计值与观测向量估计值计算协方差矩阵；
第i个粒子的状态向量估计值的协方差矩阵
Figure FDA0004215791370000055
公式如下：
Figure FDA0004215791370000056
Qk为状态扩展方程中只含白噪声的过程噪声的协方差矩阵；
Figure FDA0004215791370000057
为第i个粒子的状态向量与状态向量估计值两个值的一阶均差矩阵；公式为：
Figure FDA0004215791370000058
d为差值步长；/>
Figure FDA0004215791370000059
为矩阵/>
Figure FDA00042157913700000510
的第j列，j＝1,2,3,4；
将状态向量估计值协方差矩阵
Figure FDA00042157913700000511
表达为二次型的形式：/>
Figure FDA00042157913700000512
Figure FDA00042157913700000513
为矩阵/>
Figure FDA00042157913700000514
的观测值；
步骤304、对预测状态后的各个粒子的状态值不断迭代更新权值，取得后验概率表示为粒子的新状态；
首先，针对第i个粒子计算其观测向量与状态向量估计值的一阶均差矩阵
Figure FDA00042157913700000515
Figure FDA00042157913700000516
Figure FDA00042157913700000517
为观测值/>
Figure FDA00042157913700000518
的第j列，j＝1,2,3,4；
然后，利用一阶均差矩阵
Figure FDA00042157913700000519
计算迭代滤波算法中一步预测的协方差矩阵/>
Figure FDA00042157913700000520
Figure FDA00042157913700000521
利用一阶均差矩阵
Figure FDA00042157913700000522
和观测值/>
Figure FDA00042157913700000523
得到交互式协方差矩阵/>
Figure FDA00042157913700000524
Figure FDA00042157913700000525
利用协方差矩阵
Figure FDA00042157913700000526
和交互式协方差矩阵/>
Figure FDA00042157913700000527
得到滤波增益矩阵/>
Figure FDA00042157913700000528
Figure FDA00042157913700000529
利用滤波增益矩阵
Figure FDA00042157913700000530
和该粒子的状态估计值，结合观测向量得到第i个粒子的状态估计值/>
Figure FDA00042157913700000531
Figure FDA00042157913700000532
最终得到第i个粒子的估计协方差矩阵
Figure FDA00042157913700000533
Figure FDA00042157913700000534
Figure FDA00042157913700000535
表示上一时刻的粒子估计协方差矩阵，通过迭代进行计算；
步骤305、根据原始粒子、粒子状态估计值，以及预测得到的协方差矩阵对粒子进行抽样；
抽样公式如下：
Figure FDA0004215791370000061
步骤306、针对所抽样的粒子，计算粒子权值
Figure FDA0004215791370000062
及归一化权值/>
Figure FDA0004215791370000063
权值代表了粒子状态接近真实值的概率：
Figure FDA0004215791370000064
Figure FDA0004215791370000065
步骤307、根据抽样粒子和粒子权值，判断是否残差重采样，如果是，则重新采集粒子样本后计算估算值；否则，计算估算值；
根据有效样本数Neff(Effective sample size)来衡量粒子集的退化程度，其计算公式为：
Figure FDA0004215791370000066
预先设定重采样阈值Nth，若Neff＜Nth，将权值重新设置为
Figure FDA0004215791370000067
得到相等权值样本集/>
Figure FDA0004215791370000068
则滤波分布的后验概率/>
Figure FDA0004215791370000069
系统状态估计/>
Figure FDA00042157913700000610
及估计协方差矩阵/>
Figure FDA00042157913700000611
分别为
Figure FDA00042157913700000612
Figure FDA00042157913700000613
Figure FDA00042157913700000614
其中δ(x)为狄克拉函数；
步骤308、接着将残差重采样的结果，返回预测与更新步骤进行下一次迭代计算；
步骤309、达到所设置的滤波步长时，得到的结果即使用差分粒子滤波方法对针尖位置与姿态的准确估计；
步骤四、对准确估计的针尖位置与姿态进行仿真验证。
CN202010476353.4A 2019-05-29 2020-05-29 一种基于差分粒子滤波的医疗机器人定位装置和改进方法 Active CN111603241B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201910455254.5A CN110269683A (zh) 2019-05-29 2019-05-29 一种基于差分粒子滤波算法的柔性针针尖位置与姿态估计改进方法
CN2019104552545 2019-05-29

Publications (2)

Publication Number Publication Date
CN111603241A CN111603241A (zh) 2020-09-01
CN111603241B true CN111603241B (zh) 2023-06-27

Family

ID=67959132

Family Applications (2)

Application Number Title Priority Date Filing Date
CN201910455254.5A Pending CN110269683A (zh) 2019-05-29 2019-05-29 一种基于差分粒子滤波算法的柔性针针尖位置与姿态估计改进方法
CN202010476353.4A Active CN111603241B (zh) 2019-05-29 2020-05-29 一种基于差分粒子滤波的医疗机器人定位装置和改进方法

Family Applications Before (1)

Application Number Title Priority Date Filing Date
CN201910455254.5A Pending CN110269683A (zh) 2019-05-29 2019-05-29 一种基于差分粒子滤波算法的柔性针针尖位置与姿态估计改进方法

Country Status (1)

Country Link
CN (2) CN110269683A (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111796522B (zh) * 2020-07-16 2022-06-03 上海智驾汽车科技有限公司 一种车辆状态估计方法
CN112507597B (zh) * 2020-11-02 2021-10-22 中国南方电网有限责任公司超高压输电公司广州局 基于多簇粒子滤波的多端柔性直流输电系统状态评估方法

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7049824B2 (en) * 2003-03-27 2006-05-23 International Business Machines Corporation Differential particulate detection system for electronic devices
US7289906B2 (en) * 2004-04-05 2007-10-30 Oregon Health & Science University Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion
US7822458B2 (en) * 2005-05-19 2010-10-26 The Johns Hopkins University Distal bevel-tip needle control device and algorithm
FR2920961B1 (fr) * 2007-09-18 2017-06-02 Koelis Systeme et procede d'imagerie et de localisation de ponctions sous echographie prostatique
JP6120497B2 (ja) * 2012-06-26 2017-04-26 キヤノン株式会社 穿刺制御装置及び方法
CN103256928B (zh) * 2013-04-28 2015-05-20 南京航空航天大学 一种分布式惯性导航系统及其姿态传递对准方法
CN106500695B (zh) * 2017-01-05 2019-02-01 大连理工大学 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法
CN207627384U (zh) * 2017-06-14 2018-07-20 孙晶 一种高精度硬膜外麻醉穿刺定位装置
CN107655824A (zh) * 2017-11-01 2018-02-02 哈尔滨理工大学 一种预估柔性针穿刺软组织摩擦系数的方法
CN108759815B (zh) * 2018-04-28 2022-11-15 温州大学激光与光电智能制造研究院 一种用于全局视觉定位方法中的信息融合组合导航方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
一种改进的高光谱图像中多小目标检测算法;尹继豪;王艳;王义松;;电子学报(第09期);全文 *
基于差分进化和粒子群优化算法的混合优化算法;池元成;方杰;蔡国飙;;计算机工程与设计(第12期);全文 *

Also Published As

Publication number Publication date
CN110269683A (zh) 2019-09-24
CN111603241A (zh) 2020-09-01

Similar Documents

Publication Publication Date Title
Yun et al. Design, implementation, and experimental results of a quaternion-based Kalman filter for human body motion tracking
CN111603241B (zh) 一种基于差分粒子滤波的医疗机器人定位装置和改进方法
CN110076772B (zh) 一种机械臂的抓取方法及装置
CN108932737B (zh) 车载相机俯仰角标定方法和装置、电子设备以及车辆
Dong-Si et al. Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration
CN110068326B (zh) 姿态计算方法、装置、电子设备以及存储介质
CN109506642A (zh) 一种机器人多相机视觉惯性实时定位方法及装置
CN112837352B (zh) 基于图像的数据处理方法、装置及设备、汽车、存储介质
CN106504275A (zh) 一种惯性定位与点云配准耦合互补的实时三维重建方法
CN104778661A (zh) 在姿态估计中使用的骨架平滑方法和设备
CN114343847A (zh) 基于光学定位系统的手术机器人的手眼标定方法
CN110470297A (zh) 一种空间非合作目标的姿态运动与惯性参数估计方法
JP6851038B2 (ja) 打具の挙動の解析装置
CN108961337B (zh) 车载相机航向角标定方法和装置、电子设备以及车辆
US20220203526A1 (en) Humanoid robot control method, humanoid robot using the same, and computer readable storage medium
Lyu et al. Ttr-based reward for reinforcement learning with implicit model priors
CN115511935A (zh) 基于迭代离散化与线性插值的正态分布变换点云配准方法
CN112991445B (zh) 模型训练方法、姿态预测方法、装置、设备及存储介质
CN103954289A (zh) 一种光学成像卫星敏捷机动姿态确定方法
CN114764830A (zh) 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法
CN111637894B (zh) 一种定常系数陆标图像导航滤波方法
CN114454981A (zh) 双足机器人的弹跳运动控制方法、装置及双足机器人
Mishra et al. Motion and parameter estimation for the robotic capture of a non-cooperative space target considering egomotion uncertainty
Manerikar et al. Riccati observer design for homography decomposition
CN114310878A (zh) 基于力控的大质量零件人工引导装配方法、系统及介质

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