买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：西南科技大学

摘要：本发明涉及搜寻救援技术领域，具体地说，涉及一种用于紧急救援服务的人机协作方法及系统，其包括以下步骤：S1:机器人端通过UWB、里程计、激光雷达、相机采集数据，操作员端通过UWB和IMU采集数据；S2:基于IMU估计操作员运动轨迹，基于轮式里程计估计机器人运动轨迹，利用UWB距离测量值构建约束函数，通过图优化算法融合里程信息和UWB测量数据实现位姿图优化与人机协作定位；S3:操作员利用指向设备和触摸屏通过无线网络实现人机信息交互，基于优化后的人机协作定位结果，结合激光雷达数据完成机器人定点探索与编队跟随。本发明能较佳地进行人机协作。

主权项：1.一种用于紧急救援服务的人机协作方法，其特征在于：包括以下步骤：S1:机器人端通过UWB、里程计、激光雷达、相机采集数据，操作员端通过UWB和IMU采集数据；S2:基于IMU估计操作员运动轨迹，基于轮式里程计估计机器人运动轨迹，利用UWB距离测量值构建约束函数，通过图优化算法融合里程信息和UWB测量数据实现位姿图优化与人机协作定位；步骤S2中，人机协作定位包括以下子步骤：S21:IMU通过三轴陀螺仪获取角速度信息，通过三轴加速度计获取加速度信息，通过三轴磁力计测量方向，根据角速度和加速度数据进行预积分处理估计操作员相邻时刻的相对运动；根据机器人底盘上编码器的脉冲数估计机器人相邻时刻的相对运动，其原理为：移动机器人使用两轮差动运动模型，根据编码器产生脉冲数分别计算左右轮转速，其计算公式为： 其中，NR和NL分别表示左右轮在τ时间内测得脉冲数，I表示轮子转动一圈测得脉冲数，nR和nL为左右轮的转速；根据左右轮直径D计算机器人左右轮线速度vL和vR，其计算公式为： 根据差分运动学模型机器人的角速度ω和移动速度v分别表示为： 其中l表示为机器人的轮间距，由此建立左右轮转速与机器人运动状态的关系，已知机器人偏航角θ，根据角速度ω和移动速度v估计机器人相邻时刻的相对运动，其计算公式为： 其中Δt表示机器人相邻时刻的时间增量，Δx表示机器人在平面坐标系中的水平位移，Δy表示机器人在平面坐标系中的垂直位移，Δθ表示机器人自身的偏航角变化；S22:根据各机器人位姿和操作员位姿构成因子图顶点，根据连续时刻操作员和机器人的位姿变换构成约束，根据UWB节点间距离测量值构成距离约束，并通过图优化算法优化机器人和操作员轨迹，实现人机协作定位；步骤S22包括以下子步骤：S221:根据机器人和操作员相邻时刻的相对运动构建因子图顶点，首先将机器人起始位置作为全局坐标系的零点，根据操作员和机器人的相对位置设置操作员在全局坐标系下的起始位姿，根据步骤S21获得的里程信息估计机器人和操作员不同时刻的位姿状态；为提高定位系统实时性，使用滑动窗口截取t-w到t时间段内的机器人位姿和操作员位姿构建因子图顶点；将因子图顶点的状态空间用C表示： 其中，由二维坐标和偏航角组成，表示机器人或操作员在k时刻的位姿，T表示转置，i表示机器人或操作员的唯一编号，k表示位姿的测量时间，m表示机器人和操作员的数量，w表示状态空间截取的时间长度；表示任意一个；S222:在因子图中，边作为一种约束条件用于表示顶点之间的联系，边约束分为两种，由机器人或操作员相邻时刻的相对运动构成的位姿约束、由不同编号的UWB数据构成的距离约束；位姿约束在因子图中表示为预测值与测量值之间的位移残差其计算公式为： 其中，和表示机器人或操作员i在两个相邻时刻的顶点位姿，表示机器人或操作员i相邻时刻的相对位姿测量值，表示相对位姿预测值，即表示机器人或操作员i相邻时刻的相对位姿预测值，其计算公式为： 其中，R·∈SO2是旋转矩阵，表示相邻时刻机器人位姿的旋转变换；表示机器人或操作员i在两个相邻时刻位姿的水平变化；表示机器人或操作员i在两个相邻时刻位姿的垂直变化；表示机器人或操作员i在两个相邻时刻位姿的偏航角变化；距离约束在因子图中表示为预测值与测量值之间的距离残差其计算公式为： 其中，表示k时刻位姿顶点与间的距离测量值，表示距离预测值，即表示位姿顶点与间的距离预测值，其计算公式为： 表示编号为i的机器人或操作员在k时刻位姿的垂直坐标，表示编号为j的机器人或操作员在k时刻位姿的垂直坐标；S23：根据位姿顶点与约束边构成因子图，通过图优化算法调整顶点以更好地满足约束条件，将机器人的位姿优化问题转换成一个联合优化函数的计算： 其中，Copt表示因子图优化函数，表示位姿约束中的信息矩阵，为协方差矩阵的逆，协方差矩阵与里程测量误差大小相关；Ωd表示距离约束的信息矩阵，与UWB测量噪声相关；构建图优化函数后，利用非线性最小二乘法对优化函数进行迭代求解，找到使目标函数值最小的状态空间，即为优化获得的操作员和机器人位姿；S3:操作员利用指向设备和触摸屏通过无线网络实现人机信息交互，基于优化后的人机协作定位结果，结合激光雷达数据完成机器人定点探索与编队跟随。

全文数据：

权利要求：

百度查询： 西南科技大学 一种用于紧急救援服务的人机协作方法及系统