本文是Utah大学最新的闭环同步驱动和定位技术。使用EKF两个框架集成驱动和定位step，在驱动step中，利用T-RO根据前一刻提出的算法定位pose，解决驱动磁铁的位置，以保持胶囊在旋转磁场下驱动。然后在这样的驱动器和胶囊中计算pose接下来，胶囊上收到的magnetic force 和magnetic torque，利用之前发布的旋转前进模型，估计磁力和磁力矩下胶囊的位置变化。在定位step在此时此刻收集磁场值，并使用上一步驱动中估计的磁场值pose，根据此时的磁场采集值，更新胶囊pose。驱动定位step交替进行。从外部看，定位是在驱动过程中完成的，或者在定位过程中，也可以驱动控制胶囊成为simultaneous actuation and localization，同样，根据定位pose，更新驱动器pose方法是系统close-loop的保证。所以这个是，意义重大。本文将在机器人国际顶级会议上发表。拿到了奖项。
使用单旋转磁铁旋转磁铁同时定位和驱动磁胶囊 First demonstration of simultaneous localization and propulsion for a magnetic capsule in a lumen using a single rotating magnet [1] Paper Link Authors: Katie M. Popek, etc. 2017，IEEE International Conference on Robotics and Automation (ICRA)
目录 outline
- 摘要
- I. 介绍 （节选）
- VI. 扩展卡尔曼滤波器 （节选）
-
- A. 实现处理模型
- B. 实现测量模型
- VII. 检查胶囊的运行情况
- VIII. 演示闭环驱动(节选)
摘要
本文展示了用于嵌入式霍尔效应传感器的螺旋磁胶囊和用于单独旋转驱动磁铁的闭环推进的理论。这一理论估计，当胶囊与施加场同步旋转时，胶囊的6自由度位置。它面向肠道主动胶囊内窥镜方面的应用。扩展卡尔曼滤波器采用简化的2自由度过程模型，限制胶囊的前后运动和绕其中心轴旋转，用于提供胶囊位置的完整6自由度。据估计，当胶囊在腔内移动时。胶囊在施加现场的移动受到恒定监测，以确定胶囊是否与施加现场同步旋转。基于此信息，调整外源的旋转速度，防止预期磁耦合丢失。我们的实验演示第一次使用单个旋转磁体通过腔体的胶囊进行定位和闭环推进。以前的工作假设胶囊在定位过程中不运动，需要定位和推进解耦。与之前的解耦方法相比，该闭环显示在完成时间上达到了三倍的增长率。 The paper presents a method for closed-loop propulsion of a screw-type magnetic capsule with embedded Hall-effect sensors using a single rotating actuator magnet. The method estimates the 6 DoF pose of the capsule while it is synchronously rotating with the applied field. It is intended application in active capsule endoscopy of the intestines. An extended Kalman filter, which uses a simplified 2 DoF process model restricting the capsule to forward or backward movement and rotation about its principle axis, is used to provide a full 6 DoF estimate of the capsule’s pose as the capsule travels through a lumen. The capsule’s movement in the applied field is constantly monitored to determine if the capsule is synchronously rotating with the applied field. Based on this information, the rotating speed of the external source is adjusted to prevent a loss in the desired magnetic coupling. We experimental demonstrate, for the first time, simultaneous localization and closed-loop propulsion of a capsule through a lumen using a single rotating magnet. Prior work assumed the capsule had no net motion during the localization phase, requiring decoupled localization and propulsion. This closed-loop performance results in a three times speed up in completion time, compared to the previous decoupled approach.
I. 介绍 （节选）
在此之前，我们描述了一个定位理论来估计磁胶囊在没有运动假设的情况下的完整6自由度位置，我们使用这个位置来估计位置和头部方向反馈在验证概念的推进系统中。这种推广和定位是解耦的，所以推广是开放的，我们的胶囊运动定期暂停定位。 recently, we described a localization method to estimate the full 6 DoF pose of a magnetic capsule under the assumption of no net motion, and we applied this pose estimate for position and heading feedback in a proof-of-concept propulsion system. This propulsion and localization were decoupled, such that propulsion was executed open-loop, and our capsule’s movement was periodically paused for localization. 在本文中，当它与施加的磁极子场同步旋转时，我们展示了一个扩展卡尔曼滤波器以提供胶囊6自由度位置的连续估计。EKF使用一个简化的2自由度过程模型，假设胶囊的运动仅限于沿其轴的运动和旋转。我们限制了剩下的四个自由度，并改变了胶囊头部的腔指挥方向。 In this paper, we presents an extended Kalman filter to provide a continuous estimate of the capsule’s 6 DoF pose as it rotates synchronously with an applied magnetic dipole field. The EKF uses a simplified 2 DoF process model that assumes the capsule movement is restricted to translation along and rotation about its principle axis. We restrict the remaining four DoF and let the lumen dicate the changes in the capsule’s heading.
VI. 扩展卡尔曼滤波器 （节选）
A. 实现处理模型
该胶囊有一条螺旋线，将磁力和扭矩转换为向前和角速： The capsule has a helical thread for propulsion, which translates magnetic force and torque into forward and angular velocity: ( ω v ) = ( A E E T L ) ( τ f ) \left(\begin{matrix}\boldsymbol{\omega}\\\boldsymbol{v}\end{matrix}\right)=\left(\begin{matrix}\boldsymbol{A}&\boldsymbol{E}\\\boldsymbol{E}^{T}&\boldsymbol{L}\end{matrix}\right)\left(\bein{matrix}\boldsymbol{\tau}\\\boldsymbol{f}\end{matrix}\right) (ωv)=(AETEL)(τf) 来自施加磁场的作用在胶囊上磁力和力矩： The magnetic force and torque on the capsule from the applied magnetic field: f = 3 μ 0 ∣ ∣ m a ∣ ∣ ⋅ ∣ ∣ m c ∣ ∣ 4 π ⋅ r ^ ∣ ∣ r ∣ ∣ 4 ⋅ ( m c ^ m a ^ T + m a ^ m c ^ T + m c ^ T ( I − 5 r ^ r ^ T ) m a ^ ⋅ I ) \boldsymbol{f}=\frac{3\mu_{0}||\boldsymbol{m}_{a}||\cdot||\boldsymbol{m}_{c}||}{4\pi}\cdot\frac{\widehat{\boldsymbol{r}}}{||\boldsymbol{r}||^{4}}\cdot(\widehat{\boldsymbol{m}_{c}}\widehat{\boldsymbol{m}_{a}}^{T}+\widehat{\boldsymbol{m}_{a}}\widehat{\boldsymbol{m}_{c}}^{T}+\widehat{\boldsymbol{m}_{c}}^{T}(\boldsymbol{I}-5\widehat{\boldsymbol{r}}\widehat{\boldsymbol{r}}^{T})\widehat{\boldsymbol{m}_{a}}\cdot\boldsymbol{I}) f=4π3μ0∣∣ma∣∣⋅∣∣mc∣∣⋅∣∣r∣∣4r ⋅(mc ma T+ma mc T+mc T(I−5r r T)ma ⋅I) τ = μ 0 ∣ ∣ m a ∣ ∣ ⋅ ∣ ∣ m c ∣ ∣ 4 π ∣ ∣ r ∣ ∣ 3 m c ^ × ( 3 r ^ r ^ T − I ) m a ^ \boldsymbol{\tau}=\frac{\mu_{0}||\boldsymbol{m}_{a}||\cdot||\boldsymbol{m}_{c}||}{4\pi||\boldsymbol{r}||^{3}}\widehat{\boldsymbol{m}_{c}}\times(3\widehat{\boldsymbol{r}}\widehat{\boldsymbol{r}}^{T}-\boldsymbol{I})\widehat{\boldsymbol{m}_{a}} τ=4π∣∣r∣∣3μ0∣∣ma∣∣⋅∣∣mc∣∣mc ×(3r r T−I)ma
B. 测量模型实现
在每个传感器上读取磁场数据，然后和前一步中的预测pose的理论磁场作差，用这个差值根据卡尔曼滤波的update步骤，更新capsule的pose。
VII. 检测胶囊的运行状况
我们只需要分别胶囊是否跟随外部场同步旋转因为以前的理论能够被使用来估计静止或失步状态下的胶囊的位姿。 We only need to distinguish whether or not the capsule is synchronously rotating with the external field because the previous method can be used to estimate the pose of a capsule that is either stationary or in step-out. 给定不论来自于初始化或EKF的胶囊估计姿态，并且不论胶囊是否随施加场转动，驱动器极子的位姿被更新。 Given the capsule’s estimated pose from either the initialization or the EKF, and whether the capsule is rotating with the applied field, the actuator dipole’s pose is updated.
VIII. 闭环驱动的演示（节选）
直线轨迹的5自由度误差被找到通过比较胶囊的EKF估计状态和立体视觉系统给出的状态。这平均位置误差是8.5毫米和7.1度。 The 5 DoF error for the straight trajectory was found by comparing the capsule’s estimated state from the EKF with that given by a stereo vision system. The average position error was 8.5 m m 8.5mm 8.5mm and 7. 1 ∘ 7.1^{\circ} 7.1∘.
[1]: Popek, Katie M., Tucker Hermans, and Jake J. Abbott. “First demonstration of simultaneous localization and propulsion of a magnetic capsule in a lumen using a single rotating magnet.” 2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017.