[发明专利]一种主从式手术机器人轨迹预测控制方法

权利要求书

1.一种主从式手术机器人轨迹预测控制方法，其特征在于，具体按照以下步骤实施：

步骤1，设计最小二乘支持向量机卡尔曼振动滤波算法，对手部的震颤信号进行滤除，得到目标轨迹信号；

步骤2，分别通过直线轨迹规划法和Chebyshev伪谱法对所述目标轨迹信号进行轨迹规划，得到规划后的轨迹，通过二次规划法对规划后的轨迹进行优化，得到实际轨迹信号；

步骤3，通过主从位姿映射函数将所述实际轨迹信号作为输入，映射出主、从手的运动位姿，实现对主从式手术机器人轨迹的预测控制。

2.如权利要求1所述的一种主从式手术机器人轨迹预测控制方法，其特征在于，所述步骤1具体为：

步骤1.1，采集主手的运行轨迹，将其离散化进行标定，将卡尔曼滤波过程写成状态方程与测量过程：

状态过程：

X(k)＝AX(k-1)+w(k) (1)，

测量过程：

Z(k)＝HX(k)+v(k) (2)，

式(1)、(2)中，X(k)是主手中的振动信号的状态，矩阵A表示状态变化，向量Z(k)是信号测量结果，矩阵H表示测量变化，向量w(k)是状态噪声，向量v(k)是测量噪声，其中，w(k)和v(k)是假设相互独立的均值为0的高斯白噪声；

步骤1.2，预测

根据上一时刻状态的估计值预测下一时刻状态的值，成为先验估计，同时预测下一时刻状态的误差，成为先验误差：

P(k|k-1)＝AP(k-1|k-1)A^T+Q (4)，

式(3)、(4)中，是根据k-1时刻估计k时刻的状态向量，是k-1时刻的后验估计状态向量，P(k|k-1)是根据k-1时刻估计k时刻的协方差，P(k-1|k-1)是k-1时刻的后验估计协方差，Q为过程噪声协方差；

步骤1.3，更新矫正

先计算卡尔曼增益，再利用步骤1.2的先验估计计算后验估计，同时更新先验误差到后验误差：

K(k)＝P(k|k-1)H^T[HP(k|k-1)H^T+R]^-1 (5)，

P(k|k)＝(I-K(k)H)P(k|k-1) (7)，

式(5)、(6)、(7)中，是根据估计状态向量来预测更新k时刻的状态向量，P(k|k)是根据估计状态向量来预测更新k时刻的协方差，I是单位矩阵，K(k)是k时刻的卡尔曼增益，R为测量噪声协方差；

步骤1.4，采用最小二乘支持向量机回归算法来建立R和Q的回归方程，对过程噪声和测量噪声进行处理；

步骤1.5，将步骤1.4中的过程噪声协方差Q和测量噪声协方差R的回归方程代入步骤1.2和步骤1.3中，获得改进后的卡尔曼振动滤波算法，对手部的震颤信号进行滤除，得到目标轨迹信号。