[发明专利]医疗微机器人的实时导航控制方法和装置

权利要求书

1.一种医疗微机器人的实时导航控制装置，其特征在于，包括：

获取单元，用于实时获取医疗微机器人的参考导航轨迹状态信息，所述参考导航轨迹状态信息包括实时期望速度和实时期望位置；

构建单元，用于基于所述实时期望速度、所述实时期望位置、所述医疗微机器人的实时位置、所述医疗微机器人的实时速度和所述医疗微机器人的实时加速度，构建导航控制优化问题的目标函数和约束条件，其中，所述导航控制优化问题的建立基于离散时间系统采样时间和网络时滞；

计算单元，用于求解所述导航控制优化问题，得到所述医疗微机器人的实时加速度；

发送单元，用于将所述实时加速度发送至磁场控制装置以供所述磁场控制装置调节磁场强度和方向控制所述医疗微机器人的实时加速度；

基于所述实时期望速度、所述实时期望位置、所述医疗微机器人的实时位置、所述医疗微机器人的实时速度和所述医疗微机器人的实时加速度，构建导航控制优化问题的目标函数和约束条件，其中，所述导航控制优化问题的建立基于离散时间系统采样时间和网络时滞，具体包括：基于所述实时期望速度、所述实时期望位置、所述医疗微机器人的实时位置、所述医疗微机器人的实时速度和所述医疗微机器人的实时加速度，构建状态误差方程：

Δl(t)＝l(t)-l^ref(t)

Δv(t)＝v(t)-v^ref(t)

其中，t表示当前时刻，[c_x(t),c_y(t),c_z(t)]^T为所述医疗微机器人三维方向上的实时加速度，v(t)＝[v_x(t),v_y(t),v_z(t)]^T为所述医疗微机器人三维方向上的实时速度，l(t)＝[l_x(t),l_y(t),l_z(t)]^T为所述医疗微机器人三维方向上的实时位置，为所述医疗微机器人的实时期望速度，为所述医疗微型机器人由参考导航轨迹确定的所需位置，和分别表示Δv_x(t)、Δv_y(t)、Δv_z(t)和Δl(t)的导数，τ为通信网络引起的时滞，导航系统参数r和γ分别为血管流体密度和血管流体粘度，ρ和m分别是所述医疗微机器人的球形半径和质量，θ和是所述医疗微机器人的球面极坐标角；

基于所述状态误差方程，确定连续时间系统状态方程，将所述连续时间系统状态方程离散化，获得离散时间系统模型：

通过如下公式构建状态变量z(t)

z(t)＝[Δl_x(t),Δv_x(t),Δl_y(t),Δv_y(t),Δl_z(t),Δv_z(t)]^T；

基于所述状态误差方程，构建连续时间系统状态方程如下：

其中，t表示当前时刻，c(t)＝[c_x(t),c_y(t),c_z(t)]^T表示控制信号，即所述医疗微机器人三维方向上的实时加速度，0_i×j表示维度为i×j的全0矩阵；

将所述连续时间系统状态方程离散化，设置采样周期为T，且Tτ,在第k个采样周期[kT,(k+1)T)内，k＝1,2,3…,N；离散时间系统模型为：

其中，z_k＝z(kT),c_k＝c(kT),D_k＝e^DT,

基于所述离散时间系统模型构建导航控制优化问题的目标函数和约束条件，具体包括：

利用二次代价函数将平稳跟随z^*≡0的目标，基于所述离散时间系统模型转化为如下导航控制优化问题：

上式中，N为导航控制涉及的总采样周期数，表示期望算子，W和V为由所述导航系统决定的权重矩阵；

所述求解所述导航控制优化问题，得到所述医疗微机器人的实时加速度，具体包括：

通过如下公式构建控制变量y_k：

基于最优控制理论及递归方法，确定所述医疗微机器人的实时加速度

上式中，策略系数Q_k基于如下公式反向迭代求解得到：

其中，H_k为推导过程中设置的中间参数，N为导航控制涉及的总采样周期数，I_i×i表示维度i×i的单位矩阵。

2.根据权利要求1所述的医疗微机器人的实时导航控制装置，其特征在于，所述参考导航轨迹状态信息是基于预操作3D影像提取的。