一种家庭服务机器人自主定位控制方法技术

本发明专利技术公开了一种家庭服务机器人自主定位控制方法，其包括航迹推算和SLAM雷达定位；其中，所述航迹推算的步骤包括：(1.1)推算机器人的运动离散模型；(1.2)分析出切线模型和圆弧模型；(1.3)建立机器人坐标系与全局坐标系之间关系模型；所述SLAM雷达定位的步骤包括：(2.1)通过激光雷达获取机器人初始状态；(2.2)通过自适应无迹卡尔曼算法对采集的激光雷达数据进行滤波处理；(2.3)利用Hector SLAM算法建立栅格化地图；(2.4)结合里程数据变量和激光雷达定位变量，获得状态量。本发明专利技术采用自适应性无迹卡尔曼算法，可以有效提高机器人的位置精度和角度精度，进而提高定位的结果。

【技术实现步骤摘要】
一种家庭服务机器人自主定位控制方法
本专利技术涉及一种家庭服务机器人自主定位控制方法，属于自动化控制领域。
技术介绍
机器人技术是一个集机械，信息，计算机科学和自动控制理论于一体的多学科系统。它不仅具有自身技术的高附加值，而且还具有广泛的产品。这已说明机器人已是成为重要的技术辐射平台，增强了军事防御力量，提高了操控性，提高事态发展水平，推动经济整体发展，很大程度上改善人民生活水平具有重要意义。机器人作为未来发展的重点领域之一，其中服务机器人更是本世纪最有前途的应用之一。而服务机器人的“智能机器人技术”更是值得深入研究。当前，对移动机器人的基本智能问题，主要反映在三个问题上：(1)“我现在在哪里？”；(2)“我要去哪里？”；(3)“如何到达那里”；对应着移动机器人的定位，路径规划和运动控制问题。而自主定位问题是移动机器人实现特定功能的基础和前提，贯穿于自主导航之中，是众多问题中最先需要解决的一个问题，因为在一定程度上是反应了机器人的智能情况，如果其问题没有解决好，就可能导致无法实现机器人本身需要执行的功能。早期的室内机器人定位主要为传感器定位，如惯性导航法和航迹推算法等。然而，这些方法存在缺点是难以单个直接应用于家庭微动态环境进行自主定位，并且一些传感器可能产生一些累积误差。特别是家庭环境这种动态环境，由于无法使用GPS，定位精度要求高等特点和限制，更是增加了定位的难度，使得单一的定位方法难以适用。
技术实现思路
本专利技术的目的在于提供一种家庭服务机器人自主定位控制方法，以克服上述现有技术存在的问题。为实现上述目的，本专利技术采取的技术方案是：一种家庭服务机器人自主定位控制方法，其包括航迹推算和SLAM雷达定位；其中，所述航迹推算的步骤包括：(1.1)推算机器人的运动离散模型；(1.2)分析出切线模型和圆弧模型；(1.3)建立机器人坐标系与全局坐标系之间关系模型；所述SLAM雷达定位的步骤包括：(2.1)通过激光雷达获取机器人初始状态；(2.2)通过自适应无迹卡尔曼算法对采集的激光雷达数据进行滤波处理；(2.3)利用HectorSLAM算法建立栅格化地图；(2.4)结合里程数据变量和激光雷达定位变量，获得状态量。进一步的，所述步骤(1.1)中，机器人选择两轮差速运动的机器人，通过建立全局坐标系和机器人坐标系，将机器人基于速度的运动学模型表示为：其中，v为机器人线速度，w为机器人转速，l为机器人轮间距，vl，vr为机器人左右驱动轮线速度，x，y，θ分别为机器人中心在全局坐标系中的横坐标，纵坐标和角度；在离散系统中，左右轮速度和姿态角度均近似不变的假设下实现了离散化，可得离散运动学公式：其中(xk-1,yk-1,θk-1)为机器人k-1时刻的位姿，(xk,yk,θk)为机器人k时刻的位姿，Ts为采样时间，vl，vr分别为左右轮速度。进一步的，所述步骤(1.2)中，在较短时间内，认为机器人左右轮速度不变，则左右轮行驶的轨迹应为同心圆弧，或平行线段；可以分析出二种模型，分别为切线模型和圆弧模型；其中：(1)切线模型左轮里程值增量等于右轮里程值增量，即左右轮行驶轨迹为平行线段时，易得其运动学公式为：其中(xk-1,yk-1,θk-1)为机器人k-1时刻的位姿，(xk,yk,θk)为机器人k时刻的位姿，d为左右轮里程值增量；(2)圆弧模型通过简化机器人的里程模型，其中k-1时刻机器人在全局坐标系中的位姿为Rk-1(xk-1,yk-1,θk-1),k时刻的位姿为Rk(xk,yk,θk)，机器人轮间距为l，左右轮行驶的里程值增量分别为dl,dr，左轮里程形成的弧的半径和角度分别r和a，Rk-1和Rk的直线距离长度为d,夹角∠RkRk-1O’为b；两轮差速驱动机器人里程计公式：其中(xk-1,yk-1,θk-1)为机器人k-1时刻的位姿，(xk,yk,θk)为机器人k时刻的位姿，dl,dr分别为左右轮的两次采样间的里程值增量，l为机器人轮间距；由于两次采样间的里程值增量差值较小，d可以近似则两轮差速驱动机器人里程计公式可表示为式：最后通过上述差速驱动机器人里程计公式与机器人传感器光电编码器、陀螺仪记录下状态量dl,dr左右轮里程数据用于数据融合。进一步的，所述步骤(1.3)中，将所有坐标系均采用笛卡尔坐标系，各坐标系数据在其他坐标系应用时，需要涉及到坐标转换；这种转换关系可以通过三维平移和三维旋转描述和实现；具体的，假设激光传感器在机器人坐标系中的坐标为(tx,ty,tz),并依次绕z轴，x轴，y轴旋转了γ，α和β弧度，则任意一点在激光坐标系中的坐标(xs,ys,zs)与机器人坐标系中的坐标(xr,yr,zr)的关系表示为：进一步的，所述步骤(2.1)中，激光雷达自我旋转同时通过激光发射和检测的飞行速度计算，进行周围环境的测距，其采样频率为20Hz，转速1200rpm。进一步的，所述步骤(2.2)中，在非线性环境下，对于非线性系统的状态方程为：其中h为观测函数，f为状态转移函数，Xk，Zk分别为k时刻的状态变量和观测变量；无迹卡尔曼算法的算法步骤为：(2.21)对各Sigma点进行采样得到各Sigma点在k时刻状态预测所对应的预计权值Wim和Wic，点集{χk/k(i)},i＝1,…,2n+1，此2n+1为Sigma点采样个数；Sigma点采样公式为定义：α为正值的比例缩放因子，β为引入f(*)高阶的参数，2n+1为Sigma点采样个数，Wim，Wic分别为第i个均值和协方差加权所得权值，2n+1个对称点来近似估计均值为和协方差为Px，λ是用来控制每个点到均值的距离；(2.22)通过状态方程进行估计采样点(2.23)通过采样点权值Wim，Wic分别预估协方差矩阵Pk+1/k和均值(2.24)通过(2.22)预估量测对应的采样点(2.25)分别预估量测数和协方差Px,z为量测干扰信号向量和位姿状态向量联合的协方差矩阵，Pz,x是量测干扰信号向量的协方差矩阵；(2.26)更新测量方程然后依据机器人运动模型和前一时刻的均值和协方差通过(2.21)～(2.24)sigma点采样和变化进行预估当前时刻的均值和协方差；通过步骤(2.25)和(2.26)对观测的传感器数据预估值进行数据更新，得到最优状态估计；每次采样重复步骤(2.21)～(2.26)即可在线得到最优状态估计；最后记录下当前激光雷达SLAM所获取的定位数据[x,y,θ]T，(x,y)为机器人在地图坐标系中的坐标，θ为机器人在地图坐标系中的姿态角度。进一步的，所述步骤(2.3)中，利用HectorSLAM算法建立栅格化地图，用P(本文档来自技高网...

【技术保护点】
1.一种家庭服务机器人自主定位控制方法，其特征在于，包括航迹推算和SLAM雷达定位；其中，所述航迹推算的步骤包括：(1.1)推算机器人的运动离散模型；(1.2)分析出切线模型和圆弧模型；(1.3)建立机器人坐标系与全局坐标系之间关系模型；所述SLAM雷达定位的步骤包括：(2.1)通过激光雷达获取机器人初始状态；(2.2)通过自适应无迹卡尔曼算法对采集的激光雷达数据进行滤波处理；(2.3)利用Hector SLAM算法建立栅格化地图；(2.4)结合里程数据变量和激光雷达定位变量，获得状态量。/n

【技术特征摘要】
1.一种家庭服务机器人自主定位控制方法，其特征在于，包括航迹推算和SLAM雷达定位；其中，所述航迹推算的步骤包括：(1.1)推算机器人的运动离散模型；(1.2)分析出切线模型和圆弧模型；(1.3)建立机器人坐标系与全局坐标系之间关系模型；所述SLAM雷达定位的步骤包括：(2.1)通过激光雷达获取机器人初始状态；(2.2)通过自适应无迹卡尔曼算法对采集的激光雷达数据进行滤波处理；(2.3)利用HectorSLAM算法建立栅格化地图；(2.4)结合里程数据变量和激光雷达定位变量，获得状态量。


2.根据权利要求1所述的一种家庭服务机器人自主定位控制方法，其特征在于，所述步骤(1.1)中，机器人选择两轮差速运动的机器人，通过建立全局坐标系和机器人坐标系，将机器人基于速度的运动学模型表示为：






其中，v为机器人线速度，w为机器人转速，l为机器人轮间距，vl，vr为机器人左右驱动轮线速度，x，y，θ分别为机器人中心在全局坐标系中的横坐标，纵坐标和角度；
在离散系统中，左右轮速度和姿态角度均近似不变的假设下实现了离散化，可得离散运动学公式：



其中(xk-1,yk-1,θk-1)为机器人k-1时刻的位姿，(xk,yk,θk)为机器人k时刻的位姿，Ts为采样时间，vl，vr分别为左右轮速度。


3.根据权利要求1所述的一种家庭服务机器人自主定位控制方法，其特征在于，所述步骤(1.2)中，在较短时间内，认为机器人左右轮速度不变，则左右轮行驶的轨迹应为同心圆弧，或平行线段；可以分析出二种模型，分别为切线模型和圆弧模型；其中：
(1)切线模型
左轮里程值增量等于右轮里程值增量，即左右轮行驶轨迹为平行线段时，易得其运动学公式为：



其中(xk-1,yk-1,θk-1)为机器人k-1时刻的位姿，(xk,yk,θk)为机器人k时刻的位姿，d为左右轮里程值增量；
(2)圆弧模型
通过简化机器人的里程模型，其中k-1时刻机器人在全局坐标系中的位姿为Rk-1(xk-1,yk-1,θk-1),k时刻的位姿为Rk(xk,yk,θk)，机器人轮间距为l，左右轮行驶的里程值增量分别为dl,dr，左轮里程形成的弧的半径和角度分别r和a，Rk-1和Rk的直线距离长度为d,夹角∠RkRk-1O’为b；
两轮差速驱动机器人里程计公式：



其中(xk-1,yk-1,θk-1)为机器人k-1时刻的位姿，(xk,yk,θk)为机器人k时刻的位姿，dl,dr分别为左右轮的两次采样间的里程值增量，l为机器人轮间距；
由于两次采样间的里程值增量差值较小，d可以近似



则两轮差速驱动机器人里程计公式可表示为式：



最后通过上述差速驱动机器人里程计公式与机器人传感器光电编码器、陀螺仪记录下状态量dl,dr左右轮里程数据用于数据融合。


4.根据权利要求1所述的一种家庭服务机器人自主定位控制方法，其特征在于，所述步骤(1.3)中，将所有坐标系均采用笛卡尔坐标系，各坐标系数据在其他坐标系应用时，需要涉及到坐标转换；这种转换关系可以通过三维平移和三维旋转描述和实现；具体的，假设激光传感器在机器人坐标系中的坐标为(tx,ty,tz),并依次绕z轴，x轴，y轴旋转了γ，α和β弧度，则任意一点在激光坐标系中的坐标(xs,ys,zs)与机器人坐标系中的坐标(xr,yr,zr)的关系表示为：





5.根据权利要求1所述的一种家庭服务机器人自主定位控制方法，其特征在于，所述步骤(2.1)中，激光雷达自我旋转同时通过激光发射和检测的飞行速度计算，进行周围环境的测距，其采样频率为20Hz，转速1200rpm。


6.根据权利要求1所述的一种家庭服务机器人自主定位控制方法，其特征在于，所述步骤(2.2)中，在非线性环境下，对于非线性系统的状态方程为：



其中h为观测函数，f为状态转移函数，Xk，Z...

【专利技术属性】
技术研发人员：王刚，曹阳，徐峰远，周军，牛绿原，孔令荣，
申请(专利权)人：南京理工大学泰州科技学院，
类型：发明
国别省市：江苏;32