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

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

Info

Publication number
CN111603241A
CN111603241A CN202010476353.4A CN202010476353A CN111603241A CN 111603241 A CN111603241 A CN 111603241A CN 202010476353 A CN202010476353 A CN 202010476353A CN 111603241 A CN111603241 A CN 111603241A
Authority
CN
China
Prior art keywords
state
particle
needle
medical robot
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.)
Granted
Application number
CN202010476353.4A
Other languages
English (en)
Other versions
CN111603241B (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.)
Beihang University
Union Hospital Tongji Medical College Huazhong University of Science and Technology
Original Assignee
Beihang University
Union Hospital Tongji Medical College Huazhong University of Science and 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 Beihang University, Union Hospital Tongji Medical College Huazhong University of Science and Technology filed Critical Beihang University
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)
  • Medical Informatics (AREA)
  • Robotics (AREA)
  • Operations Research (AREA)
  • Computing Systems (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (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 BDA0002515967900000021
v为装配柔性针的医疗机器人针尖处的速度；
Figure BDA0002515967900000022
为针尖在惯性坐标系XOY下x轴方向上的速度；
Figure BDA0002515967900000023
为针尖在惯性坐标系XOY下y轴方向上的速度；
Figure BDA0002515967900000024
为在XOY坐标系下装配柔性针的医疗机器人针尖运动方向与x轴的夹角；ρ为针尖运动的曲率半径。
步骤二、将针尖运动学平面模型，先通过状态扩展消除模型误差中的有色噪声，再通过差分同类观测消除测量误差中的有色噪声，得到只含白噪声的扩展状态方程并进行简化；
具体步骤为：
首先，将针尖运动学平面模型进行改进，得到离散形式的针尖运动学平面模型；
公式如下：
Figure BDA0002515967900000031
Figure BDA0002515967900000032
ηk+1＝L1ηk+ωk
Δk+1＝L2Δk+vk
状态
Figure BDA0002515967900000033
为3×1的状态向量；
Figure BDA0002515967900000034
为3×1的观测向量；f(xk)为针尖运动学平面模型中显示的非线性函数；ηk为白噪声驱动的模型误差中的有色噪声，由参数不确定性或建模的不精确带来的；
Figure BDA0002515967900000035
为模型误差中的有色噪声ηk的输入矩阵；Δk为白噪声驱动的测量误差中的有色噪声，由传感器误差带来的；ωk为模型误差中的有色噪声ηk中的白噪声随机向量，vk为测量误差中的有色噪声Δk中的白噪声随机向量；L1为模型误差中的有色噪声ηk的状态增益。L2为测量误差中的有色噪声Δk的状态增益。k是迭代参数，k＝1,2,...n。
然后，通过状态扩展和差分同类观测分别消除模型误差中的有色噪声ηk和测量误差中的有色噪声Δk，分别得到只含白噪声的扩展状态方程并简化；
扩展状态方程为：
Figure BDA0002515967900000036
Figure BDA0002515967900000037
I为单位矩阵；
简化得到：
xk+1＝A(xk)xk+Gωk
yk＝Cxk+vk
其中，增广状态
Figure BDA0002515967900000038
为4×1的状态向量；观测向量
Figure BDA0002515967900000039
增广状态转移矩阵
Figure BDA00025159679000000310
增广噪声转移矩阵
Figure BDA00025159679000000311
增广输出矩阵C＝[f-L2 B]。
步骤三、对简化后只含白噪声的扩展状态方程，利用差分粒子滤波器处理白噪声，结合差分粒子滤波算法对装配柔性针的医疗机器人的针尖位置和姿态进行估计；
差分粒子滤波算法具体过程为：
步骤301、在迭代参数k＝0时，初始化各参数值，同时生成粒子；
初始化参数，参数包括粒子
Figure BDA00025159679000000312
粒子先验信息
Figure BDA00025159679000000313
及协方差矩阵
Figure BDA00025159679000000314
粒子先验信息公式为：
Figure BDA00025159679000000315
协方差矩阵公式为：
Figure BDA00025159679000000316
进一步简化为：
Figure BDA00025159679000000317
其中，
Figure BDA00025159679000000318
为Cholesky因式分解因子；i＝1,2,…N，代表第i个粒子；每个粒子的状态即为不同的针尖位置和姿态。
步骤302、当迭代参数k≥1，计算每个粒子的先验信息及协方差矩阵；
步骤303、利用每个粒子的先验信息及协方差矩阵预测各自的状态；
预测是对待求的装配柔性针的医疗机器人的针尖位置和姿态，根据先验信息进行一步预测。
首先，利用第i个粒子的先验信息得到该粒子的状态估计值与观测估计值；
公式如下：
Figure BDA0002515967900000041
Figure BDA0002515967900000042
Figure BDA0002515967900000043
为第i个粒子的状态向量估计值；
Figure BDA0002515967900000044
为第i个粒子的观测向量估计值；
然后，利用该粒子的状态向量估计值与观测向量估计值计算协方差矩阵；
第i个粒子的状态向量估计值的协方差矩阵
Figure BDA0002515967900000045
公式如下：
Figure BDA0002515967900000046
Qk为状态扩展方程中只含白噪声的过程噪声的协方差矩阵；
Figure BDA0002515967900000047
为第i个粒子的状态向量与状态向量估计值两个值的一阶均差矩阵；公式为：
Figure BDA0002515967900000048
d为差值步长；
Figure BDA0002515967900000049
为矩阵
Figure BDA00025159679000000410
的第j列，j＝1,2,3,4；
将状态向量估计值协方差矩阵
Figure BDA00025159679000000411
表达为二次型的形式：
Figure BDA00025159679000000412
Figure BDA00025159679000000413
为矩阵
Figure BDA00025159679000000414
的观测值；
步骤304、对预测状态后的各个粒子的状态值不断迭代更新权值，取得后验概率表示为粒子的新状态。
首先，针对第i个粒子计算其观测向量与状态向量估计值的一阶均差矩阵
Figure BDA00025159679000000415
Figure BDA00025159679000000416
Figure BDA00025159679000000417
为观测值
Figure BDA00025159679000000418
的第j列，j＝1,2,3,4；
然后，利用一阶均差矩阵
Figure BDA00025159679000000419
计算迭代滤波算法中一步预测的协方差矩阵
Figure BDA00025159679000000420
Figure BDA00025159679000000421
利用一阶均差矩阵
Figure BDA00025159679000000422
和观测值
Figure BDA00025159679000000423
得到交互式协方差矩阵
Figure BDA00025159679000000424
Figure BDA00025159679000000425
利用协方差矩阵
Figure BDA00025159679000000426
和交互式协方差矩阵
Figure BDA00025159679000000427
得到滤波增益矩阵
Figure BDA00025159679000000428
Figure BDA00025159679000000429
利用滤波增益矩阵
Figure BDA00025159679000000430
和该粒子的状态估计值，结合观测向量得到第i个粒子的状态估计值
Figure BDA00025159679000000431
Figure BDA0002515967900000051
最终得到第i个粒子的估计协方差矩阵
Figure BDA0002515967900000052
Figure BDA0002515967900000053
Figure BDA0002515967900000054
表示上一时刻的粒子估计协方差矩阵，通过迭代进行计算。
步骤305、根据原始粒子、粒子状态估计值，以及预测得到的协方差矩阵对粒子进行抽样；
抽样公式如下：
Figure BDA0002515967900000055
步骤306、针对所抽样的粒子，计算粒子权值
Figure BDA0002515967900000056
及归一化权值
Figure BDA0002515967900000057
权值代表了粒子状态接近真实值的概率：
Figure BDA0002515967900000058
Figure BDA0002515967900000059
步骤307、根据抽样粒子和粒子权值，判断是否残差重采样，如果是，则重新采集粒子样本后计算估算值。否则，计算估算值；
根据有效样本数Neff(Effective sample size)来衡量粒子集的退化程度，其计算公式为：
Figure BDA00025159679000000510
预先设定重采样阈值Nth，若Nef＜Nth，将权值重新设置为
Figure BDA00025159679000000511
得到相等权值样本集
Figure BDA00025159679000000512
则滤波分布的后验概率
Figure BDA00025159679000000513
系统状态估计
Figure BDA00025159679000000514
及估计协方差矩阵
Figure BDA00025159679000000515
分别为
Figure BDA00025159679000000516
Figure BDA00025159679000000517
Figure BDA00025159679000000518
其中δ(x)为狄克拉函数。
步骤308、接着将残差重采样的结果，返回预测与更新步骤进行下一次迭代计算。
步骤309、达到所设置的滤波步长时，得到的结果即使用差分粒子滤波方法对针尖位置与姿态的准确估计。
步骤四、对准确估计的针尖位置与姿态进行仿真验证。
本发明的优点在于：
一种基于差分粒子滤波算法的装配柔性针的医疗机器人针尖位置与姿态估计改进方法，调整了装配柔性针的医疗机器人运动学模型，基于此改进模型，设计了差分粒子滤波估计方法，可以有效处理非高斯噪声的模型误差与测量误差，在惯性坐标系下对装配柔性针的医疗机器人的位置与姿态做出精确的估计。
附图说明
图1为基于医疗机器人的自动穿刺手术装置图。
图2为穿刺用医疗机器人的控制系统。
图3为本发明基于差分粒子滤波的医疗机器人定位装置简图。
图4为本发明基于差分粒子滤波算法的医疗机器人针尖位置与姿态估计改进方法的流程图。
图5为本发明装配柔性针的医疗机器人的运动学模型示意图。
图6为本发明利用差分粒子滤波器处理白噪声得到针尖位置和姿态估计值的流程图；
图7为本发明的仿真结果图。
具体实施方式
下面将结合附图和实例对本发明作进一步的详细说明。
本发明的目的是针对装配柔性针的医疗机器人的精准定位需求，提出一种基于差分粒子滤波的医疗机器人定位装置，并开发了对应的位置估计方法，以克服定位过程中的较大误差。
本发明提供了一种基于差分粒子滤波的医疗机器人定位装置，包括图像传感系统、电控系统、执行系统和上位机，如图3所示。其中，图像传感系统包括摄像头和图像处理系统，由摄像头采集在明胶中移动的装载柔性针的医疗机器人的图像，由图像处理系统对图像信号进行初步处理。电控系统包括医疗机器人控制系统与执行驱动器，实现医疗机器人的控制。上位机负责基于差分粒子滤波的医疗机器人位置估计的计算，为医疗机器人控制提供精确的反馈量。
针对基于差分粒子滤波的医疗机器人的位置估计，最主要的问题在于解决实际情况下的噪声问题，由模型误差造成的过程噪声，由摄像头精度，以及在明胶中图像不清晰造成的测量噪声都不可以视为简单的高斯噪声。于是，本发明针对装配柔性针的医疗机器人针尖位置与姿态估计中，模型误差与测量误差均为非高斯噪声，普通估计算法容易发散的问题，提出了一种改进的基于差分粒子滤波算法的估计算法，是一种切实有效的方案。如图4所示。
1.名义模型计算器
根据装配柔性针的医疗机器人的运动学特性，在惯性坐标系XOY下搭建非高斯噪声误差的针尖运动学平面模型；
装配柔性针的医疗机器人的运动学特性与装配柔性针的医疗机器人的给进运动和自转运动相关，给进运动实现针的穿刺，而针的自转则是随着针尖的方向而改变的。如同自行车，后轮给进，前轮决定前进的方向，根据装配柔性针的医疗机器人的运动学特性，搭建装配柔性针的医疗机器人运动模型如图5所示。
针尖运动学平面模型包括模型误差和测量误差，将针尖所处坐标表示为(X，Y)；则针尖(X，Y)处的速度表示为：
Figure BDA0002515967900000071
其中，v为装配柔性针的医疗机器人针尖处的速度；
Figure BDA0002515967900000072
为针尖在惯性坐标系XOY下x轴方向上的速度；
Figure BDA0002515967900000073
为针尖在惯性坐标系XOY下y轴方向上的速度；
Figure BDA0002515967900000074
为在XOY坐标系下装配柔性针的医疗机器人针尖运动方向与x轴的夹角；
针尖运动学约束为：
Figure BDA0002515967900000075
通过联立方程(1)、(2)，可以得到
Figure BDA0002515967900000076
利用图示运动关系，可解得横摆角速度ω为：
Figure BDA0002515967900000077
其中，θ为针尖与x轴夹角的变化量；ρ为针尖运动的曲率半径，曲率半径由针尖的转角r确定:
Figure BDA0002515967900000078
结合上式得到针尖的运动学模型如下：
Figure BDA0002515967900000079
2.状态扩展器
对针尖运动学平面模型进行改进，先通过状态扩展消除模型误差中的有色噪声，再通过差分同类观测消除测量误差中的有色噪声，得到只含白噪声的扩展状态方程并进行简化；
具体步骤为：
首先，将针尖运动学平面模型公式(6)改写为离散形式，设定模型误差与测量误差均为非高斯噪声，公式如下：
Figure BDA0002515967900000081
状态
Figure BDA0002515967900000082
为3×1的状态向量；
Figure BDA0002515967900000083
为3×1的观测向量；f(xk)为如公式(6)所示的非线性函数；ηk为白噪声驱动的模型误差中的有色噪声，由参数不确定性或建模的不精确带来的；
Figure BDA0002515967900000084
为模型误差中的有色噪声ηk的输入矩阵；Δk为白噪声驱动的测量误差中的有色噪声，由传感器误差带来的；ωk为模型误差中的有色噪声ηk中的高斯白噪声随机向量，vk为测量误差中的有色噪声Δk中的高斯白噪声随机向量；L1为模型误差中的有色噪声ηk的状态增益。L2为测量误差中的有色噪声Δk的状态增益。k是迭代参数，k＝1,2,...n。
然后对有色噪声进行白化处理，有色噪声在成熟的估计算法中无法进行有效的处理，会造成估计结果的发散。所以要将模型扩展为增广模型，将有色噪声变为状态量的一部分；其方法是，首先通过状态扩展消除模型误差中的有色噪声ηk，再通过差分同类观测值可消除测量误差中的有色噪声Δk，由此得到只含白噪声的增广系统的扩展状态方程。
扩展状态方程为：
Figure BDA0002515967900000085
f为f(xk)的简写，为装配柔性针的医疗机器人运动学平面模型中显示的非线性函数；I为单位矩阵；去掉上标号*，并进行简化表达，得到：
Figure BDA0002515967900000086
其中，增广状态
Figure BDA0002515967900000087
为4×1的状态向量；观测向量
Figure BDA0002515967900000088
增广状态转移矩阵
Figure BDA0002515967900000089
增广噪声转移矩阵
Figure BDA00025159679000000810
增广输出矩阵C＝[f-L2 B]。
3.差分粒子滤波器
对简化后只含白噪声的扩展状态方程，结合差分粒子滤波理论搭建差分粒子滤波估计器，利用差分粒子滤波器处理白噪声，对装配柔性针的医疗机器人的针尖位置和姿态进行估计；
根据简化的增广系统公式(9)，搭建差分粒子滤波估计器。差分粒子滤波方法是一种比较成熟的可以处理模型误差和测量误差都是高斯白噪声的估计方法。本发明应用的是二阶差分粒子滤波算法，滤波方程中采用了协方差矩阵的平方根形式，不需要对函数的偏导数求解，与同样可以处理非高斯噪声的UKF滤波算法相比，具有更好的数值特性。
差分粒子滤波分为两个阶段，分别是初始化阶段和状态与更新阶段。首先设置初值，进而利用模型信息进行状态的预测与更新。如图6所示，具体过程为：
步骤301、在迭代参数k＝0时，初始化各参数值，同时生成粒子；
初始化参数，参数包括粒子
Figure BDA0002515967900000091
粒子先验信息
Figure BDA0002515967900000092
及协方差矩阵
Figure BDA0002515967900000093
粒子先验信息公式为：
Figure BDA0002515967900000094
协方差矩阵公式为：
Figure BDA0002515967900000095
进一步简化为：
Figure BDA0002515967900000096
其中，
Figure BDA0002515967900000097
为Cholesky因式分解因子；i＝1,2,…N，代表第i个粒子；每个粒子的状态即为不同的针尖位置和姿态。
步骤302、当迭代参数k≥1时状态预测与更新，计算每个粒子的先验信息及协方差矩阵；
步骤303、利用每个粒子的先验信息及协方差矩阵预测各自的状态；
预测是对待求的装配柔性针的医疗机器人的针尖位置和姿态，根据先验信息进行一步预测。
首先，利用第i个粒子的先验信息得到该粒子的状态估计值与观测估计值；
公式如下：
Figure BDA0002515967900000098
Figure BDA0002515967900000099
Figure BDA00025159679000000910
为第i个粒子的状态向量预测值；
Figure BDA00025159679000000911
为第i个粒子的观测向量预测值；
然后，利用该粒子的状态向量估计值与观测向量估计值计算协方差矩阵；
第i个粒子的状态向量预测值的协方差矩阵
Figure BDA00025159679000000912
公式如下：
Figure BDA00025159679000000913
Qk为状态扩展方程中只含白噪声的过程噪声的协方差矩阵；
Figure BDA00025159679000000914
为第i个粒子的状态向量与状态向量估计值两个值的一阶均差矩阵；公式为：
Figure BDA00025159679000000915
d为差值步长；
Figure BDA00025159679000000916
为矩阵
Figure BDA00025159679000000917
的第j列，j＝1,2,3,4；
将状态向量估计值协方差矩阵
Figure BDA00025159679000000918
表达为二次型的形式：
Figure BDA00025159679000000919
Figure BDA00025159679000000920
为矩阵
Figure BDA00025159679000000921
的观测值；
步骤304、对预测状态后的各个粒子的状态值不断迭代更新权值，取得后验概率表示为粒子的新状态。
首先，针对第i个粒子计算其观测向量与状态向量估计值的一阶均差矩阵
Figure BDA0002515967900000101
Figure BDA0002515967900000102
Figure BDA0002515967900000103
为观测值
Figure BDA0002515967900000104
的第j列，j＝1,2,3,4；
然后，利用一阶均差矩阵
Figure BDA0002515967900000105
计算迭代滤波算法中一步预测的协方差矩阵
Figure BDA0002515967900000106
Figure BDA0002515967900000107
利用一阶均差矩阵
Figure BDA0002515967900000108
和观测值
Figure BDA0002515967900000109
得到交互式协方差矩阵
Figure BDA00025159679000001010
Figure BDA00025159679000001011
利用协方差矩阵
Figure BDA00025159679000001012
和交互式协方差矩阵
Figure BDA00025159679000001013
得到滤波增益矩阵
Figure BDA00025159679000001014
Figure BDA00025159679000001015
利用滤波增益矩阵
Figure BDA00025159679000001016
和该粒子的状态估计值，结合观测向量得到第i个粒子的状态估计值
Figure BDA00025159679000001017
Figure BDA00025159679000001018
最终得到第i个粒子的估计协方差矩阵
Figure BDA00025159679000001019
Figure BDA00025159679000001020
Figure BDA00025159679000001021
表示上一时刻的粒子估计协方差矩阵，通过迭代进行计算。
步骤305、根据原始粒子、粒子状态估计值，以及预测得到的协方差矩阵对粒子进行抽样；
抽样公式如下：
Figure BDA00025159679000001022
步骤306、针对所抽样的粒子，计算粒子权值
Figure BDA00025159679000001023
及归一化权值
Figure BDA00025159679000001024
权值代表了粒子状态接近真实值的概率：
Figure BDA00025159679000001025
Figure BDA00025159679000001026
步骤307、根据抽样粒子和粒子权值，判断是否残差重采样，如果是，则重新采集粒子样本后计算估算值。否则，计算估算值；
根据有效样本数Neff(Effective sample size)来衡量粒子集的退化程度，其计算公式为：
Figure BDA00025159679000001027
预先设定重采样阈值Nth，若Nef＜Nth，将权值重新设置为
Figure BDA00025159679000001028
得到相等权值样本集
Figure BDA00025159679000001029
则滤波分布的后验概率
Figure BDA00025159679000001030
系统状态估计
Figure BDA00025159679000001031
及估计协方差矩阵
Figure BDA00025159679000001032
分别为
Figure BDA0002515967900000111
Figure BDA0002515967900000112
Figure BDA0002515967900000113
其中δ(x)为狄克拉函数。
步骤308、接着将残差重采样的结果，返回预测与更新步骤进行下一次迭代计算。
步骤309、达到所设置的滤波步长时，得到的结果即使用差分粒子滤波方法对针尖位置与姿态的准确估计。
按照时序运行以上程序，利用差分粒子滤波算法精确地预测出增广状态值
Figure BDA0002515967900000114
也就是说，可以同时精确地得到装配柔性针的医疗机器人的位置与姿态信息
Figure BDA0002515967900000115
以及非高斯噪声η。
在matlab中编程对传统装配柔性针的医疗机器人模型和改进的装配柔性针的医疗机器人模型进行状态估计，并与真实值进行对比，结果如图7所示，用于验证本发明所提出方法的准确性。

Claims (5)

1.一种基于差分粒子滤波的医疗机器人定位装置，其特征在于，包括图像传感系统、电控系统、执行系统和上位机；
其中，图像传感系统包括摄像头和图像处理模块，由摄像头采集在明胶中移动的装载柔性针的医疗机器人的图像，由图像处理模块对图像信号进行初步处理；电控系统包括医疗机器人控制信号的计算与执行驱动器，实现医疗机器人的控制；上位机负责基于差分粒子滤波的医疗机器人位置估计的计算，为医疗机器人控制提供精确的反馈量；
所述的定位装置还包括名义1)模型计算器，根据装配柔性针的医疗机器人的运动学特性，在惯性坐标系XOY下搭建非高斯噪声误差的针尖运动学平面模型；2)状态扩展器，对针尖运动学平面模型进行改进，先通过状态扩展消除模型误差中的有色噪声，再通过差分同类观测消除测量误差中的有色噪声，得到只含白噪声的扩展状态方程并进行简化；以及3)差分粒子滤波器，对简化后只含白噪声的扩展状态方程，结合差分粒子滤波理论搭建差分粒子滤波估计器，利用差分粒子滤波器处理白噪声，对医疗机器人装配的柔性针针尖位置和姿态进行估计。
2.如权利要求1所述的一种基于差分粒子滤波的医疗机器人定位装置，其特征在于，所述的名义模型计算器根据装配柔性针的医疗机器人的运动学特性，搭建装配柔性针的医疗机器人运动模型，具体过程如下：
装配柔性针的医疗机器人的运动学特性与装配柔性针的医疗机器人的给进运动和自转运动相关，给进运动实现针的穿刺，而针的自转则是随着针尖的方向而改变的；
针尖运动学平面模型包括模型误差和测量误差，具体公式如下：
Figure FDA0002515967890000011
v为装配柔性针的医疗机器人针尖处的速度；
Figure FDA0002515967890000012
为针尖在惯性坐标系XOY下x轴方向上的速度；
Figure FDA0002515967890000013
为针尖在惯性坐标系XOY下y轴方向上的速度；
Figure FDA0002515967890000014
为在XOY坐标系下装配柔性针的医疗机器人针尖运动方向与x轴的夹角；ρ为针尖运动的曲率半径。
3.如权利要求1所述的一种基于差分粒子滤波的医疗机器人定位装置，其特征在于，所述的状态扩展器简化白噪声的扩展状态方程具体步骤为：
首先，将针尖运动学平面模型进行改进，得到离散形式的针尖运动学平面模型；
公式如下：
Figure FDA0002515967890000021
Figure FDA0002515967890000022
ηk+1＝L1ηk+ωk
Δk+1＝L2Δk+vk
状态
Figure FDA0002515967890000023
为3×1的状态向量；
Figure FDA0002515967890000024
为3×1的观测向量；f(xk)为针尖运动学平面模型中显示的非线性函数；ηk为白噪声驱动的模型误差中的有色噪声，由参数不确定性或建模的不精确带来的；
Figure FDA0002515967890000025
为模型误差中的有色噪声ηk的输入矩阵；Δk为白噪声驱动的测量误差中的有色噪声，由传感器误差带来的；ωk为模型误差中的有色噪声ηk中的白噪声随机向量，vk为测量误差中的有色噪声Δk中的白噪声随机向量；L1为模型误差中的有色噪声ηk的状态增益；L2为测量误差中的有色噪声Δk的状态增益；k是迭代参数，k＝1,2,...n；
然后，通过状态扩展和差分同类观测分别消除模型误差中的有色噪声ηk和测量误差中的有色噪声Δk，分别得到只含白噪声的扩展状态方程并简化；
扩展状态方程为：
Figure FDA0002515967890000026
Figure FDA0002515967890000027
I为单位矩阵；
简化得到：
xk+1＝A(xk)xk+Gωk
yk＝Cxk+vk
其中，增广状态
Figure FDA0002515967890000028
为4×1的状态向量；观测向量
Figure FDA0002515967890000029
增广状态转移矩阵
Figure FDA00025159678900000210
增广噪声转移矩阵
Figure FDA00025159678900000211
增广输出矩阵C＝[f-L2 B]。
4.如权利要求1所述的一种基于差分粒子滤波的医疗机器人定位装置，其特征在于，所述的差分粒子滤波器处理白噪声，对医疗机器人装配的柔性针针尖位置和姿态进行估计；
差分粒子滤波算法具体过程为：
首先、在迭代参数k＝0时，初始化各参数值，同时生成粒子；迭代参数k≥1，计算每个粒子的先验信息，利用每个粒子的先验信息得到各粒子的状态估计值与观测估计值；利用各粒子的状态向量估计值与观测向量估计值计算协方差矩阵，预测各粒子的针尖位置和姿态状态；
然后、对预测状态后的各个粒子的状态值不断迭代更新权值，取得后验概率表示为粒子的新状态；并根据原始粒子、粒子状态估计值，以及预测得到的协方差矩阵对粒子进行抽样；
计算抽样的粒子的权值
Figure FDA0002515967890000031
及归一化权值
Figure FDA0002515967890000032
权值代表了粒子状态接近真实值的概率：
最后、根据抽样粒子和粒子权值，判断是否残差重采样，如果是，则重新采集粒子样本后计算估算值；否则，计算估算值；达到所设置的滤波步长时，得到的结果即使用差分粒子滤波方法对针尖位置与姿态的准确估计。
5.应用如权利要求1所述的一种基于差分粒子滤波的医疗机器人定位装置进行装配柔性针针尖位置与姿态估计改进方法，其特征在于，具体步骤如下：
步骤一、根据装配柔性针的医疗机器人的运动学特性，在惯性坐标系下搭建非高斯噪声误差的针尖运动学平面模型；
装配柔性针的医疗机器人的运动学特性与装配柔性针的医疗机器人的给进运动和自转运动相关，给进运动实现针的穿刺，而针的自转则是随着针尖的方向而改变的；
针尖运动学平面模型包括模型误差和测量误差，具体公式如下：
Figure FDA0002515967890000033
v为装配柔性针的医疗机器人针尖处的速度；
Figure FDA0002515967890000034
为针尖在惯性坐标系XOY下x轴方向上的速度；
Figure FDA0002515967890000035
为针尖在惯性坐标系XOY下y轴方向上的速度；
Figure FDA0002515967890000036
为在XOY坐标系下装配柔性针的医疗机器人针尖运动方向与x轴的夹角；ρ为针尖运动的曲率半径；
步骤二、将针尖运动学平面模型，先通过状态扩展消除模型误差中的有色噪声，再通过差分同类观测消除测量误差中的有色噪声，得到只含白噪声的扩展状态方程并进行简化；
具体步骤为：
首先，将针尖运动学平面模型进行改进，得到离散形式的针尖运动学平面模型；
公式如下：
Figure FDA0002515967890000037
Figure FDA0002515967890000038
ηk+1＝L1ηk+ωk
Δk+1＝L2Δk+vk
状态
Figure FDA0002515967890000039
为3×1的状态向量；
Figure FDA00025159678900000310
为3×1的观测向量；f(xk)为针尖运动学平面模型中显示的非线性函数；ηk为白噪声驱动的模型误差中的有色噪声，由参数不确定性或建模的不精确带来的；
Figure FDA00025159678900000311
为模型误差中的有色噪声ηk的输入矩阵；Δk为白噪声驱动的测量误差中的有色噪声，由传感器误差带来的；ωk为模型误差中的有色噪声ηk中的白噪声随机向量，vk为测量误差中的有色噪声Δk中的白噪声随机向量；L1为模型误差中的有色噪声ηk的状态增益；L2为测量误差中的有色噪声Δk的状态增益；k是迭代参数，k＝1,2,...n；
然后，通过状态扩展和差分同类观测分别消除模型误差中的有色噪声ηk和测量误差中的有色噪声Δk，分别得到只含白噪声的扩展状态方程并简化；
扩展状态方程为：
Figure FDA0002515967890000041
Figure FDA0002515967890000042
I为单位矩阵；
简化得到：
xk+1＝A(xk)xk+Gωk
yk＝Cxk+vk
其中，增广状态
Figure FDA0002515967890000043
为4×1的状态向量；观测向量
Figure FDA0002515967890000044
增广状态转移矩阵
Figure FDA0002515967890000045
增广噪声转移矩阵
Figure FDA0002515967890000046
增广输出矩阵C＝[f-L2 B]；
步骤三、对简化后只含白噪声的扩展状态方程，利用差分粒子滤波器处理白噪声，结合差分粒子滤波算法对装配柔性针的医疗机器人的针尖位置和姿态进行估计；
差分粒子滤波算法具体过程为：
步骤301、在迭代参数k＝0时，初始化各参数值，同时生成粒子；
初始化参数，参数包括粒子
Figure FDA0002515967890000047
粒子先验信息
Figure FDA0002515967890000048
及协方差矩阵
Figure FDA0002515967890000049
粒子先验信息公式为：
Figure FDA00025159678900000410
协方差矩阵公式为：
Figure FDA00025159678900000411
进一步简化为：
Figure FDA00025159678900000412
其中，
Figure FDA00025159678900000413
为Cholesky因式分解因子；i＝1,2,…N，代表第i个粒子；每个粒子的状态即为不同的针尖位置和姿态；
步骤302、当迭代参数k≥1，计算每个粒子的先验信息及协方差矩阵；
步骤303、利用每个粒子的先验信息及协方差矩阵预测各自的状态；
预测是对待求的装配柔性针的医疗机器人的针尖位置和姿态，根据先验信息进行一步预测；
首先，利用第i个粒子的先验信息得到该粒子的状态估计值与观测估计值；
公式如下：
Figure FDA00025159678900000414
Figure FDA0002515967890000051
Figure FDA0002515967890000052
为第i个粒子的状态向量估计值；
Figure FDA0002515967890000053
为第i个粒子的观测向量估计值；
然后，利用该粒子的状态向量估计值与观测向量估计值计算协方差矩阵；
第i个粒子的状态向量估计值的协方差矩阵
Figure FDA0002515967890000054
公式如下：
Figure FDA0002515967890000055
Qk为状态扩展方程中只含白噪声的过程噪声的协方差矩阵；
Figure FDA0002515967890000056
为第i个粒子的状态向量与状态向量估计值两个值的一阶均差矩阵；公式为：
Figure FDA0002515967890000057
d为差值步长；
Figure FDA0002515967890000058
为矩阵
Figure FDA0002515967890000059
的第j列，j＝1,2,3,4；
将状态向量估计值协方差矩阵
Figure FDA00025159678900000510
表达为二次型的形式：
Figure FDA00025159678900000511
Figure FDA00025159678900000512
为矩阵
Figure FDA00025159678900000513
的观测值；
步骤304、对预测状态后的各个粒子的状态值不断迭代更新权值，取得后验概率表示为粒子的新状态；
首先，针对第i个粒子计算其观测向量与状态向量估计值的一阶均差矩阵
Figure FDA00025159678900000514
Figure FDA00025159678900000515
Figure FDA00025159678900000516
为观测值
Figure FDA00025159678900000517
的第j列，j＝1,2,3,4；
然后，利用一阶均差矩阵
Figure FDA00025159678900000518
计算迭代滤波算法中一步预测的协方差矩阵
Figure FDA00025159678900000519
Figure FDA00025159678900000520
利用一阶均差矩阵
Figure FDA00025159678900000521
和观测值
Figure FDA00025159678900000522
得到交互式协方差矩阵
Figure FDA00025159678900000523
Figure FDA00025159678900000524
利用协方差矩阵
Figure FDA00025159678900000525
和交互式协方差矩阵
Figure FDA00025159678900000526
得到滤波增益矩阵
Figure FDA00025159678900000527
Figure FDA00025159678900000528
利用滤波增益矩阵
Figure FDA00025159678900000529
和该粒子的状态估计值，结合观测向量得到第i个粒子的状态估计值
Figure FDA00025159678900000530
Figure FDA00025159678900000531
最终得到第i个粒子的估计协方差矩阵
Figure FDA00025159678900000532
Figure FDA00025159678900000533
Figure FDA00025159678900000534
表示上一时刻的粒子估计协方差矩阵，通过迭代进行计算；
步骤305、根据原始粒子、粒子状态估计值，以及预测得到的协方差矩阵对粒子进行抽样；
抽样公式如下：
Figure FDA00025159678900000535
步骤306、针对所抽样的粒子，计算粒子权值
Figure FDA0002515967890000061
及归一化权值
Figure FDA0002515967890000062
权值代表了粒子状态接近真实值的概率：
Figure FDA0002515967890000063
Figure FDA0002515967890000064
步骤307、根据抽样粒子和粒子权值，判断是否残差重采样，如果是，则重新采集粒子样本后计算估算值；否则，计算估算值；
根据有效样本数Neff(Effective sample size)来衡量粒子集的退化程度，其计算公式为：
Figure FDA0002515967890000065
预先设定重采样阈值Nth，若Neff＜Nth，将权值重新设置为
Figure FDA0002515967890000066
得到相等权值样本集
Figure FDA0002515967890000067
则滤波分布的后验概率
Figure FDA0002515967890000068
系统状态估计
Figure FDA0002515967890000069
及估计协方差矩阵
Figure FDA00025159678900000610
分别为
Figure FDA00025159678900000611
Figure FDA00025159678900000612
Figure FDA00025159678900000613
其中δ(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 true CN111603241A (zh) 2020-09-01
CN111603241B 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)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112507597A (zh) * 2020-11-02 2021-03-16 中国南方电网有限责任公司超高压输电公司广州局 基于多簇粒子滤波的多端柔性直流输电系统状态评估方法

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111796522B (zh) * 2020-07-16 2022-06-03 上海智驾汽车科技有限公司 一种车辆状态估计方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040189313A1 (en) * 2003-03-27 2004-09-30 International Business Machiness Corporation Differential particulate detection system for electronic devices
US20050251328A1 (en) * 2004-04-05 2005-11-10 Merwe Rudolph V D Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion
CN103256928A (zh) * 2013-04-28 2013-08-21 南京航空航天大学 一种分布式惯性导航系统及其姿态传递对准方法
US20150150591A1 (en) * 2012-06-26 2015-06-04 Canon Kabushiki Kaisha Puncture control system and method therefor
CN106500695A (zh) * 2017-01-05 2017-03-15 大连理工大学 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法
CN108759815A (zh) * 2018-04-28 2018-11-06 温州大学激光与光电智能制造研究院 一种用于全局视觉定位方法中的信息融合组合导航方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
CN207627384U (zh) * 2017-06-14 2018-07-20 孙晶 一种高精度硬膜外麻醉穿刺定位装置
CN107655824A (zh) * 2017-11-01 2018-02-02 哈尔滨理工大学 一种预估柔性针穿刺软组织摩擦系数的方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040189313A1 (en) * 2003-03-27 2004-09-30 International Business Machiness Corporation Differential particulate detection system for electronic devices
US20050251328A1 (en) * 2004-04-05 2005-11-10 Merwe Rudolph V D Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion
US20150150591A1 (en) * 2012-06-26 2015-06-04 Canon Kabushiki Kaisha Puncture control system and method therefor
CN103256928A (zh) * 2013-04-28 2013-08-21 南京航空航天大学 一种分布式惯性导航系统及其姿态传递对准方法
CN106500695A (zh) * 2017-01-05 2017-03-15 大连理工大学 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法
CN108759815A (zh) * 2018-04-28 2018-11-06 温州大学激光与光电智能制造研究院 一种用于全局视觉定位方法中的信息融合组合导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
尹继豪;王艳;王义松;: "一种改进的高光谱图像中多小目标检测算法", 电子学报, no. 09 *
池元成;方杰;蔡国飙;: "基于差分进化和粒子群优化算法的混合优化算法", 计算机工程与设计, no. 12 *
王晨;房建成;: "基于Unscented四元数粒子滤波的微小卫星姿态估计", 北京航空航天大学学报, no. 05 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112507597A (zh) * 2020-11-02 2021-03-16 中国南方电网有限责任公司超高压输电公司广州局 基于多簇粒子滤波的多端柔性直流输电系统状态评估方法

Also Published As

Publication number Publication date
CN110269683A (zh) 2019-09-24
CN111603241B (zh) 2023-06-27

Similar Documents

Publication Publication Date Title
CN111590581B (zh) 机器人的定位补偿方法及装置
Kim et al. Catching objects in flight
Jabbari et al. An adaptive scheme for image-based visual servoing of an underactuated UAV
CN110986939B (zh) 一种基于imu预积分的视觉惯性里程计方法
US20110292055A1 (en) Systems and methods for animating non-humanoid characters with human motion data
US7747348B2 (en) Method and apparatus for using rotational movement amount of mobile device and computer-readable recording medium for storing computer program
CN111603241A (zh) 一种基于差分粒子滤波的医疗机器人定位装置和改进方法
CN111136660A (zh) 机器人位姿定位方法及系统
CN111986271A (zh) 一种基于光束平差的机器人方位与手眼关系同时标定方法
Siradjuddin et al. Image Based Visual Servoing of a 7 DOF robot manipulator using a distributed fuzzy proportional controller
CN110068326B (zh) 姿态计算方法、装置、电子设备以及存储介质
CN108638068A (zh) 一种携带冗余度机械臂的飞行机器人控制系统设计方法
Candan et al. Design and Deployment of Fuzzy PID Controllers to the nano quadcopter Crazyflie 2.0
CN107167145B (zh) 一种自适应非接触式失效卫星的形态参数测算方法
Burri et al. Maximum likelihood parameter identification for MAVs
CN114399528A (zh) 基于二维图像的三维空间运动目标跟踪方法及相关装置
CN112710298A (zh) 基于动力学模型辅助的旋转弹用地磁卫星组合导航方法
CN109093620A (zh) 一种双目相机辅助的空间非合作目标动力学参数辨识方法
Kecskés et al. Simultaneous calibration of a hexapod robot and an IMU sensor model based on raw measurements
CN117222915A (zh) 用于使用复合测量模型跟踪移动对象的扩张状态的系统和方法
Vochten et al. Robust optimization-based calculation of invariant trajectory representations for point and rigid-body motion
Birbach Accuracy analysis of camera-inertial sensor based ball trajectory prediction
CN115452003A (zh) 一种微机电陀螺非线性偏置在线估计方法
CN112034869B (zh) 一种无人机变参神经动力学控制器的设计方法及其应用
Gedik et al. RGBD data based pose estimation: Why sensor fusion?

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