[发明专利]室内机器人的局部路径规划和跟踪控制方法、装置及介质

权利要求书

1.室内机器人的局部路径规划和跟踪控制方法，其特征在于，包括如下步骤：

获取全局平滑路径、距离图和机器人状态信息；

截取全局平滑路径作为局部参考路径，在局部参考路径上建立Frenet坐标系并在该坐标系下采用五次多项式拟合优化，获取最优局部路径并变换到笛卡尔坐标系下，得到期望轨迹，所述期望轨迹为离散的参考路径点集，所述参考路径点包括机器人的期望姿态和期望速度；

从期望轨迹中找到机器人当前时刻对应的期望位姿和期望速度，确定机器人的误差状态，然后根据线性化后的误差运动学方程，建立离散的线性二次型最优控制系统，求解最优的误差速度；

将获得的误差速度作为反馈控制量，期望速度作为前馈控制量，最终得到机器人速度控制指令并下发至执行机构。

2.根据权利要求1所述的室内机器人的局部路径规划和跟踪控制方法，其特征在于，还包括如下步骤：

当满足局部路径规划条件时，判断机器人是否进入原地旋转，若是，则进行速度规划，若否，则进行局部路径规划。

3.根据权利要求2所述的室内机器人的局部路径规划和跟踪控制方法，其特征在于，所述局部路径规划的条件为存在全局平滑路径，且机器人当前位姿与目标点位姿不同。

4.根据权利要求1所述的室内机器人的局部路径规划和跟踪控制方法，其特征在于，所述采用五次多项式拟合优化，具体包括如下步骤：

根据机器人当前的状态和采样末端状态生成一系列的候选路径；

根据距离图对候选路径快速进行安全性检测，将不可行路径删除，获取可行路径；

从可行路径中按照优化指标找到最优局部路径。

5.根据权利要求1所述的室内机器人的局部路径规划和跟踪控制方法，其特征在于，所述误差速度的获取方法，具体包括如下步骤：

从期望轨迹中找到机器人当前时刻对应的期望位姿和期望速度，计算机器人的误差状态，公式如下：

（1）

其中、为机器人的位置，为机器人的角度，、为期望位置，为期望角度，、为位置误差，为角度误差，

误差输入为：

（2）

其中、分别为机器人的线速度和角速度控制，、分别为期望的线速度和角速度控制，、分别为误差的线速度和角速度控制，

建立线性化的定常系统，写成矩阵形式：

（3）

其中，，

令，分别表示k时刻的误差状态和误差控制，设时间步长为dt，根据一阶状态递推公式，得到：

（4）

得到离散的线性二次型最优控制系统：

（5）

其中，

代价函数为：

（6）

其中Q为状态量的权重矩阵，R为控制量的权重矩阵，

根据LQR 状态反馈控制器，得到误差控制：

（7）

其中K为反馈矩阵。

6.根据权利要求1所述的室内机器人的局部路径规划和跟踪控制方法，其特征在于，所述控制指令为：

（10）

其中，为期望速度，误差速度。