一种移动机器人轨迹修正控制算法的制作方法

技术特征：

1.一种移动机器人轨迹修正控制算法，其特征在于：包括以下步骤：

(101)利用设于机器人上的IMU模块计算机器人的姿态角；

(102)将姿态角的变化转换为运动速度矢量；

(103)计算出合速度；

(104)计算出合速度作用下当前及剩余全部目标点修正位置；

(105)输出已修正的运动轨迹。

2.根据权利要求1所述的一种移动机器人轨迹修正控制算法，其特征在于：所述步骤101中，姿态角的计算采用卡尔曼滤波融合算法，

(201)系统观测方程：

${\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_k=\left[\begin{array}{cc}1& -T\\ 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_{k-1}+\left[\begin{array}{c}T\\ 0\end{array}\right]u_{k-1}$

其中，α为机器人姿态角，β为系统稳态误差，T为采样周期，u_k-1为k-1时刻陀螺仪角度测量量；

(202)系统状态方程：

X(k)＝AX(k-1)+Bu_k-1

其中，X(k)为k时刻预测的结果，X(k-1)为k-1时刻的最优结果；

(203)由上一次的误差协方差P(k-1)和过程噪声预测新的误差P(k)：

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

其中，Q是系统过程的协方差矩阵，且Q_acce为加速度计的协方差，Q_gyro为陀螺仪测量协方差；

(204)计算卡尔曼增益：

K_g(k)＝P(k)H^T(HP(k)H^T+R)-¹

其中，H为系统观测矩阵；

(205)进行校正更新，得到状态K的最优估算值X(k)：

X(k)＝X(k-1)+K_g(k)(V(k)-HX(K-1))

(206)为下一步估计k+1状态最优值迭代进行更新操作，并更新k状态协方差P(k)值：

P(k)＝(I-K_g(k)H)P(k-1)

其中，I为单位矩阵；

按照如上步骤，计算完成时间更新和测量更新后，将上一次的后验估计作为下一次的先验估计，往复运算直至结果最优。

3.根据权利要求1所述的一种移动机器人轨迹修正控制算法，其特征在于：所述步骤101中，机器人的上身和下身通过装有弹簧的圆盘连接，所述IMU模块设于机器人腰部的圆盘上，IMU模块包括3轴陀螺仪及3轴加速度计。

4.根据权利要求1所述的一种移动机器人轨迹修正控制算法，其特征在于：所述步骤102中，通过姿态角计算出人的作用力，作用力通过正比变换得到人为的运动速度矢量，人作用力大小与姿态角的关系为：

F_human＝F_max(1-e^-bθ),|θ|≤θ_max

其中，θ为机器人上身受外力作用下的姿态角(横滚角/俯仰角/扭转角)，θ_max为弹簧系统允许的最大姿态角度的大小。

5.根据权利要求1所述的一种移动机器人轨迹修正控制算法，其特征在于：所述步骤103中，合速度计算公式为：

$\stackrel{\·}{q}=(1-k_1){\stackrel{\·}{q}}_{desire}+k_1{\stackrel{\·}{q}}_{human}$

其中，为机器人期望速度矢量，为人为速度矢量，k₁是对的影响权值，且0＜k₁＜1。