本发明属于机器人人机协作控制领域，尤其是涉及一种移动机器人轨迹修正控制算法。
背景技术：
自传统工业机器人诞生以来，机器人技术在短短的60年内便取得了显著的进步。如今，机器人已经不再受限于工业应用，它的研究重点和发展方向更趋向于日常生活的应用，如餐饮、迎宾、娱乐、陪伴等。同时，机器人技术的发展也对人机协作的主动性要求也越来越高，也就是说，机器人不仅能够根据人的施力情况而被动运动，而且能根据预先学习的知识与人协调作业，即主动协调控制。
当前市场上绝大多数的移动机器人，在运动过程中并不能感受外力作用，也不能在主动运动过程中向人力做出妥协，这在很大程度上增加了机器人的危险性，使机器人无法与人安全共处。而且当前现有的人机协作机器人控制系统中，大多采用多维力传感器来感受外力的大小和方向，以此作为控制算法的输入。尽管这种控制系统对外力的感知简便、准确，但是价格十分昂贵。
技术实现要素：
有鉴于此，本发明旨在提出一种移动机器人轨迹修正控制算法，该方法在主动运动的基础上，根据外力大小和方向修正运动轨迹。
为达到上述目的，本发明的技术方案是这样实现的：
一种移动机器人轨迹修正控制算法，包括以下步骤：
(101)利用设于机器人上的IMU模块计算机器人的姿态角；
(102)将姿态角的变化转换为运动速度矢量；
(103)计算出合速度；
(104)计算出合速度作用下当前及剩余全部目标点修正位置；
(105)输出已修正的运动轨迹。
进一步的，所述步骤101中，姿态角的计算采用卡尔曼滤波融合算法，
(201)系统观测方程：
其中，α为机器人姿态角，β为系统稳态误差，T为采样周期，uk-1为k-1时刻陀螺仪角度测量量；
(202)系统状态方程：
X(k)＝AX(k-1)+Buk-1
其中，X(k)为k时刻预测的结果，X(k-1)为k-1时刻的最优结果；
(203)由上一次的误差协方差P(k-1)和过程噪声预测新的误差P(k)：
P(k)＝AP(k-1)AT+Q
其中，Q是系统过程的协方差矩阵，且Q_acce为加速度计的协方差，Q_gyro为陀螺仪测量协方差；
(204)计算卡尔曼增益：
Kg(k)＝P(k)HT(HP(k)HT+R)-1
其中，H为系统观测矩阵；
(205)进行校正更新，得到状态K的最优估算值X(k)：
X(k)＝X(k-1)+Kg(k)(V(k)-HX(K-1))
(206)为下一步估计k+1状态最优值迭代进行更新操作，并更新k状态协方差P(k)值：
P(k)＝(I-Kg(k)H)P(k-1)
其中，I为单位矩阵；
按照如上步骤，计算完成时间更新和测量更新后，将上一次的后验估计作为下一次的先验估计，往复运算直至结果最优。
进一步的，所述步骤101中，所述机器人的上身和下身通过装有弹簧的圆盘连接，所述IMU模块设于机器人腰部的圆盘上，IMU模块包括3轴陀螺仪及3轴加速度计。
进一步的，所述步骤102中，通过姿态角计算出人的作用力，作用力通过正比变换得到人为的运动速度矢量，人作用力大小与姿态角的关系为：
Fhuman＝Fmax(1-e-bθ),|θ|≤θmax
其中，θ为机器人上身受外力作用下的姿态角(横滚角/俯仰角/扭转角)，θmax为弹簧系统允许的最大姿态角度的大小。
进一步的，所述步骤103中，合速度计算公式为：
其中，为机器人期望速度矢量，为人为速度矢量，k1是对的影响权值，且0＜k1＜1。
相对于现有技术，本发明所述的一种移动机器人轨迹修正控制算法具有以下优势：借助安装在机器人腰部弹簧装置和惯性模块来感知机器人上身姿态变化，从而在主动运动的基础上，根据人在机器人运动过程中施加的力实时调整运动轨迹，实现人与机器人之间的协调运动，具有更高的主动性和智能性；采用卡尔曼滤波算法将加速度计与陀螺仪数据进行融合，提高测量精度。
附图说明
图1为本发明实施例所述的控制流程示意图。
具体实施方式
下面将参考附图并结合实施例来详细说明本发明。
如图1所示，一种移动机器人轨迹修正控制算法，包括以下步骤：
(101)利用设于机器人上的IMU模块计算机器人的姿态角；
(102)将姿态角的变化转换为运动速度矢量；
(103)计算出合速度；
(104)计算出合速度作用下当前及剩余全部目标点修正位置；
(105)输出已修正的运动轨迹。
本发明的具体实施例中，机器人的上身和下身通过装有弹簧的圆盘连接，将IMU水平放置在腰部的圆盘上。当人对机器人上身产生推拉或扭转的作用力时，IMU的实时输出值就会发生变化，经Kalman滤波后就会反应到横滚角、俯仰角和扭转角的变化上，由此来判断人对机器人作用力的大小和方向。机器人则在主动运动的同时，根据人力的大小判断人的意图，向人力做出妥协，避免人受到伤害。
具体步骤如下：
1、姿态角的计算采用卡尔曼(Kalman)滤波融合算法。
IMU模块包含1个3轴陀螺仪和1个3轴加速度计。加速度计瞬时读数误差较大，而陀螺仪数据通过积分得到的姿态角随时间的增长累积误差逐渐增大。因此在进行姿态判断前，需要使用Kalman滤波算法将加速度计和陀螺仪数据进行融合，提高测量精度。
(201)系统观测方程：
其中，α为机器人姿态角，β为系统稳态误差，T为采样周期，uk-1为k-1时刻陀螺仪角度测量量；
(202)系统状态方程：
X(k)＝AX(k-1)+Buk-1 (2)
其中，X(k)为k时刻预测的结果，X(k-1)为k-1时刻的最优结果；
(203)由上一次的误差协方差P(k-1)和过程噪声预测新的误差P(k)：
P(k)＝AP(k-1)AT+Q (3)
其中，Q是系统过程的协方差矩阵，且Q_acce为加速度计的协方差，Q_gyro为陀螺仪测量协方差；
(204)计算卡尔曼增益：
Kg(k)＝P(k)HT(HP(k)HT+R)-1 (4)
其中，H为系统观测矩阵；
(205)进行校正更新，得到状态K的最优估算值X(k)：
X(k)＝X(k-1)+Kg(k)(V(k)-HX(K-1)) (5)
(206)为下一步估计k+1状态最优值迭代进行更新操作，并更新k状态协方差P(k)值：P(k)＝(I-Kg(k)H)P(k-1) (6)
其中，I为单位矩阵；
按照如上步骤，计算完成时间更新和测量更新后，将上一次的后验估计作为下一次的先验估计，往复运算直至结果最优。
2、轨迹修正控制过程
主动型移动机器人的运动状态由两部分合成，第一部分是机器人根据预先学习的知识得到的目标坐标和期望速度矢量，另一部分是人对机器人的实时作用力产生的人为速度矢量。假设预先规划的机器人期望速度矢量为人对机器人的作用力所对应的人为速度矢量为则机器人运动合速度并且：
其中k1是对的影响权值，且0＜k1＜1。
假定在某一段轨迹上，机器人原始的运动规律可表示为：
其中，fdi(x)、gdi(x)为经三次样条差值得到的连续轨迹函数，为机器人在期望轨迹上的期望速度。
通过姿态角可以计算出人的作用力。但由于外力过大时会造成弹簧的刚性形变，机器人结构会被损坏，同时，由于为避免机器人运动过程中的剧烈晃动，选择的弹簧刚度较大，一般的人力无法使弹簧发生十分大的形变，所以作用力必须小于一个最大值，姿态角也相应的收敛。在数学函数表达式中，指数函数可以很好的表达这一特性，因此，人作用力大小与姿态角的关系可以用指数函数表示，如下式所示：
Fhuman＝Fmax(1-e-bθ),|θ|≤θmax (9)
其中θ为机器人上身受外力作用下的姿态角(横滚角/俯仰角/扭转角)，θmax为弹簧系统允许的最大姿态角度的大小。
人的作用力经过正比计算变换可得出人为的运动速度矢量，从而使姿态角的变化可以转换为运动速度矢量。
机器人在运动过程中，我们以大小为f的频率向机器人发送速度控制信号。根据式(7)的合速度矢量表达式，可得Δt时间段内机器人的运动轨迹长度：
则机器人运动到第i个目标点时的位姿表达式为：
由于人的作用力使原始轨迹坐标有所改变，因此第i个目标点后的轨迹点坐标都要同时做出修正。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。