自行走式地下掘进机器人姿态测量系统的设计

第 26 卷 第 6 期2013 年 6 月传 感 技 术 学 报CHINESE JOURNAL OF SENSORS AND ACTUATORSVol. 26 No. 6Jun. 2013Design of the Position Computing for Self-Propelled Underground Tunneling RobotsCHEN Shuangye , NIU Jinglong, YANG Rujun(College of Electronic Information and Control Engineering, Beijing University of Technology, Beijing 100124,China)Abstract:While traditional methods only apply for static robots,this paper comes up with a new method to computethe position of the underground tunneling robots in motion. The two key parts of this new method are two laser beamsgenerator and several receivers. The laser beam generator generates three laser beams which contain a gated and twofan-shaped beams. The receiver computes the relative horizontal angle and the pitch angle to the plane where the twogenerators locate according to the relations among the beams,the spin speed of the generator and the time differencebetween receptions, then computes the coordinates of the receiver through forward intersection principle.Measurement models and simulations have been developed and after comparing with the results from differentmethods,the precision satisfies the need for computing the position of the tunneling robots very well.Key words:self-propelled tunneling robot;position;dynamic error;simulationEEACC:7110;7210 doi:10. 3969 / j. issn. 1004-1699. 2013. 06. 017自 行 走 式 地 下 掘 进 机 器 人 姿 态 测 量 系 统 的 设 计 陈 双 叶 ,牛 经 龙 ,杨 汝 军(北 京 工 业 大 学 电 子 信 息 与 控 制 工 程 学 院 ,北 京 100124)摘 要 : 针 对 当 前 自 行 走 式 地 下 掘 进 机 器 人 姿 态 测 量 方 法 只 能 应 用 于 静 态 测 量 的 不 足 ,提 出 一 种 可 实 时 动 态 测 量 机 器 人 行进 过 程 中 姿 态 的 新 方 法 。 该 系 统 的 核 心 是 两 个 激 光 发 射 器 和 多 个 激 光 接 收 器 。 发 射 器 发 射 三 束 带 有 一 定 特 征 信 息 的 激 光 ,包 括 一 束 选 通 光 和 两 束 扇 形 光 。 接 收 器 接 收 到 光 束 后 ,利 用 光 束 之 间 的 几 何 关 系 、 发 射 器 的 旋 转 速 度 和 光 束 被 接 收 的 时 间 差来 计 算 接 收 器 相 对 于 两 发 射 器 所 在 水 平 面 的 水 平 角 和 俯 仰 角 ,通 过 前 方 交 会 原 理 确 定 接 收 器 即 目 标 点 的 空 间 坐 标 。 根 据 系统 的 测 量 原 理 建 立 测 量 模 型 ,进 行 误 差 分 析 ,建 立 误 差 模 型 ,并 对 其 进 行 仿 真 ,与 惯 性 导 航 测 量 误 差 进 行 比 较 。 仿 真 结 果 表 明该 方 法 测 量 精 度 满 足 机 器 人 位 姿 的 测 量 要 求 。关 键 词 : 自 行 走 式 掘 进 机 器 人 ;姿 态 测 量 ;动 态 误 差 ;仿 真中 图 分 类 号 : TH74; TH86; TP23 文 献 标 识 码 : A 文 章 编 号 :1004-1699(2013)06-0838-07自 行 走 式 地 下 掘 进 机 器 人 主 要 应 用 于 城 市 隧( 管 ) 道 的 挖 掘 ,采 用 自 增 阻 四 段 式 结 构 1, ,具 有 急曲 线 施 工 能 力 及 不 依 赖 管 壁 提 供 掘 进 推 进 反 力 的 技术 特 点 ,是 一 种 创 新 发 展 的 新 机 型 。 在 其 掘 进 过 程中 受 到 各 种 力 的 作 用 和 一 些 不 定 性 因 素 影 响 ,这 会与 设 计 轴 线 出 现 偏 差 。 为 了 将 掘 进 路 线 与 隧 道 设 计曲 线 的 误 差 控 制 在 一 定 的 范 围 内 ,需 要 及 时 的 获 得机 器 人 的 位 置 和 姿 态 ,此 时 就 必 须 通 过 导 向 系 统 测量 机 器 人 在 绝 对 空 间 中 的 坐 标 ,具 体 来 说 就 是 获 得机 器 人 各 段 首 尾 的 中 心 坐 标 ,计 算 掘 进 机 器 人 轴 线与 隧 道 设 计 轴 线 的 位 置 偏 差 ( 水 平 方 向 和 垂 直 方向 ) 和 角 度 偏 差 ( 水 平 偏 航 角 和 俯 仰 角 ) 2-4 ,随 后 反馈 给 位 姿 控 制 系 统 做 出 控 制 来 减 小 偏 差 。项 目 来 源 :国 家 自 然 科 学 基 金 项 目 (51278011)收 稿 日 期 :2013-03-25 修 改 日 期 :2013-04-24对 于 掘 进 中 的 机 器 人 ,要 实 时 的 获 取 空 间 位 置和 姿 态 难 度 较 大 。 主 要 原 因 有 机 器 人 的 动 态 性 、 测量 标 志 设 置 困 难 、 测 量 环 境 恶 劣 等 。 目 前 测 量 方 法主 要 分 为 人 工 测 量 和 自 动 测 量 两 类 。 人 工 测 量 工 作量 大 、 人 力 投 入 多 、 测 量 频 率 低 无 法 实 时 获 取 机 器 人姿 态 。 但 由 于 其 实 现 方 式 有 多 种 、 成 本 较 低 和 操 作简 单 ,国 内 施 工 中 仍 被 采 用 。 自 动 导 向 测 量 系 统 主要 有 德 国 的 VMT,日 本 Enzan 公 司 的 ROBOTEC 和日 本 的 GYRO 系 统 5, 等 。 前 两 者 为 全 站 仪 激 光 导航 ,后 者 为 陀 螺 仪 惯 性 导 航 。 德 国 VMT 激 光 导 向 系统 功 能 齐 全 、 精 度 高 ,但 只 能 与 德 国 海 瑞 克 公 司 的 盾构 配 套 使 用 而 无 法 移 植 ,并 且 操 作 复 杂 ,无 法 大 规 模采 用 。 对 于 日 本 ROBOTEC, 采 用 三 个 反 射 棱 镜 与侧 ,则 可 完 成 接 收 器 空 间 坐 标 的 测 量 。图 4 接 收 器 安 放 位 置,在 施 工 中 陀 螺 仪 仅 作 为 辅 助 参 考 。 为 了 使正为 测 量 系 统 在 工 程 中 的 应 用 结 构 图 。第 6 期 陈 双 叶 ,牛 经 龙 等 :自 行 走 式 地 下 掘 进 机 器 人 姿 态 测 量 系 统 的 设 计 8 39一 个 自 动 全 站 仪 测 量 ,通 过 测 量 三 个 棱 镜 的 坐 标 计算 机 器 人 位 姿 。 但 在 同 一 时 刻 全 站 仪 只 能 检 测 一 个点 的 坐 标 ,而 测 量 动 态 行 进 的 机 器 人 姿 态 需 要 检 测同 一 时 刻 离 散 分 布 的 若 干 点 的 空 间 坐 标 ,所 以 它 更适 合 掘 进 机 器 人 静 态 位 姿 检 测 。 对 于 日 本 GYRO惯 性 陀 螺 仪 导 航 系 统 ,定 位 误 差 会 随 时 间 而 增 大 ,不适 用 于 长 距 离 长 时 间 的 测 量 , 需 要 定 期 的 进 行 校6-7自 行 走 式 地 下 掘 进 机 完 成 地 下 自 动 控 制 掘 进 行 走 ,对 其 地 下 姿 态 的 定 位 提 出 高 的 要 求 ,高 精 度 ,智 能 化和 开 放 化 。 基 于 此 ,本 文 设 计 出 一 种 新 的 姿 态 定 位系 统 ,可 对 多 目 标 进 行 实 时 动 态 的 精 确 测 量 。 图 1图 1 工 程 控 制 图1 姿态测量系统设计1. 1 测 量 系 统 的 组 成(1) 发 射 器 8-10系 统 中 存 在 两 个 发 射 器 ,每 台 发 射 器 以 不 同 角 速度 旋 转 ,同 时 发 射 带 有 位 置 和 角 速 度 等 信 息 的 激 光 信号 ,包 括 两 束 扇 形 光 和 一 束 选 通 光 。 两 束 扇 形 光 在 水平 面 的 夹 角 为 90,与 垂 直 面 的 夹 角 为 30,扇 面水 平 约 夹 角 30(如 图 2)。 同 时 发 射 器 内 部 还 集 成有 电 子 罗 盘 模 块 ,配 合 整 平 基 座 可 以 建 立 与 世 界 坐 标系 相 符 的 测 量 坐 标 系 ,南 北 为 坐 标 系 x 轴 ,东 西 为 坐标 系 y 轴 ,垂 直 于 水 平 面 的 轴 为 z。图 2 扇 面 夹 角(2) 整 平 基 座整 平 基 座 对 发 射 器 进 行 整 平 操 作 ,使 发 射 器 的中 心 轴 线 垂 直 于 水 平 面 。(3) 接 收 器接收器主要由激光传感器和放大电路组成 11 ,用 于 接 收 发 射 器 发 射 出 的 激 光 信 号 ,并 反 馈 信 息 给发 射 器 ,告 知 其 对 选 通 光 束 的 接 收 ,通 过 发 射 器 与 接收 器 输 出 的 信 号 来 实 现 空 间 定 位 。 其 数 量 可 以 根 据要 求 进 行 配 置 。(4) 坐 标 计 算 处 理 器坐 标 计 算 处 理 器 用 于 处 理 发 射 器 与 接 收 器 反 馈回 来 的 数 据 ,计 算 得 到 与 目 标 相 关 的 角 度 信 息 ,再 根据 角 度 信 息 计 算 得 到 目 标 点 的 三 维 空 间 坐 标 。(5) 上 位 机 软 件上 位 机 软 件 用 于 汇 总 各 个 接 收 器 反 馈 回 来 的 坐标 信 息 ,根 据 用 户 的 需 要 计 算 得 到 机 器 人 空 间 位 姿的 信 息 ,显 示 于 客 户 端 ,方 便 用 户 掌 握 机 器 人 的 路 线偏 差 并 对 其 进 行 控 制 。1. 2 测 量 系 统 的 布 局自 行 走 掘 进 机 器 人 外 径 为 1 100 mm,内 径 为 900mm,共 分 为 4 节 ,每 节 之 间 通 过 液 压 缸 两 两 相 连 ,液压 缸 全 部 处 于 收 缩 状 态 时 机 器 全 长 5 200 mm 其 中 第1 节 长 度 为 700 mm,第 2、 3、 4 节 长 度 均 为 1 500 mm(为 机 器 外 侧 测 量 长 度 ,不包括机器各节搭接部分长度 ), 当 液 压 缸 全 部 处 于 伸 长 状 态 时 机 器 全 长 5 560mm1 (如 图 3)。 根 据 机 器 人 的 机 械 结 构 和 发 射 器 的测 量 范 围 对 接 收 器 和 发 射 器 进 行 布 局 。图 3 自 行 走 掘 进 机 器 人 结 构 图在 掘 进 机 尾 部 离 内 壁 一 定 距 离 处 安 装 4 个 接 收器 ,接 收 器 位 置 即 测 量 点 位 置 ,要 满 足 4 个 测 量 点 在同 一 断 面 (图 4) 且 关 于 中 心 对 称 。 根 据 3 点 确 定 一个 平 面 的 原 理 ,机 器 人 第 4 段 尾 部 断 面 方 程 和 中 心 点O 的 坐 标 可 以 确 定 。 由 于 掘 进 机 各 节 的 材 质 均 是 圆形 钢 桶 ,便 可 根 据 刚 体 性 质 通 过 数 学 公 式 依 次 推 导 出各 段 首 尾 断 面 中 心 坐 标 完 成 位 姿 的 测 量 12-14 。发 射 器 的 扇 形 光 束 与 水 平 面 的 夹 角 为 30,加上 激 光 的 测 量 距 离 构 成 发 射 器 的 测 量 空 间 (图 5), 测量 过 程 中 要 求 接 收 器 处 在 发 射 器 的 测 量 范 围 内 ,且 两个 发 射 器 相 距 一 定 距 离 且 分 布于 掘 进 路 线 内 壁 的 两 1 sin( B - A) 2 sin( B + A) sin Btan Asin Asin Bsin( A + B)2sin t - tan 构 框 图 ,图 9 为 系 统 运 行 流 程 图 。840传 感 技 术 学 报www. chinatransducers. com是 第 2 束 扇 形 光 被 接 收 到 的 时 间 。第 26 卷图 5 发 射 器 空 间 测 量 范 围1. 3 测 量 原 理利 用 对 自 行 走 式 掘 进 机 器 人 尾 段 机 身 上 不 在 同一 条 直 线 上 的 三 点 坐 标 的 测 量 ,根 据 三 点 确 定 一 个平 面 和 掘 进 机 的 刚 体 性 质 ,自 行 走 式 掘 进 机 器 人 的空 间 位 置 就 可 以 确 定 。 对 于 机 身 上 点 坐 标 的 测 量 ,采 用 前 方 交 会 原 理 ( 图 6) 。图 6 前 方 交 会 原 理 图计 算 公 式x = b 1+y = b (1)sin( B + A)z = b通 过 对 A、 B 两 点 空 间 坐 标 , A、 B、 A、 B角 度的 测 量 ,可 以 得 到 目 标 点 P 的 坐 标 。其 中 A、 B 两 点 空 间 坐 标 即 测 量 系 统 两 个发 射器 的 坐 标 ,在 掘 进 开 始 前 通 过 全 站 仪 对 两 点 进 行 测量 。 k( k = A, B) 是 通 过 接 收 装 置 接 收 到 选 通 光 后反 馈 给 发 射 装 置 ,记 录 此 时 选 通 光 与 电 子 罗 盘 正 北方 向 的 夹 角 ,得 出 水 平 方 向 夹 角 。 k( k = A, B) 是 利用 两 束 扇 形 光 被 接 收 器 接 收 到 的 不 同 时 间 差 值 t计 算 得 出 的 ( 图 7) 。计 算 公 式 : = arctan 4(2) t = t2 - t1其 中 t1 是 发 射 器 第 1 束 扇 形 光 被 接 收 到 的 时 间 , t2图 7 角 度 测 量1. 4 系 统 工 作 原 理测 量 系 统 启 动 后 , 两 台 发 射 器 以 不 同 角 速 度 k( k = A, B) 旋 转 ,旋 转 过 程 中 各 自 发 射 出 一 束 选 通光 和 两 束 扇 形 光 , A、 B 两 发 射 器 发 出 的 选 通 光 先 后到 达 接 收 器 ,接 收 器 提 取 A、 B 选 通 光 所 带 的 角 速 度信 息 和 位 置 信 息 ,以 此 判 断 不 同 时 刻 接 收 到 的 选 通光 来 自 哪 个 发 射 器 。 通 过 无 线 电 模 块 将 接 收 到 的A、 B 选 通 光 的 时 刻 信 息 反 馈 给 发 射 器 ,发 射 器 记 录此 时 刻 选 通 光 与 电 子 罗 盘 正 北 方 向 的 夹 角 ,得 到 水平 角 度 信 息 A、 B。以 接 收 器 接 收 到 选 通 光 的 时 刻 为 计 时 零 点 ,存在 两 个 计 时 零 点 A0 、 B0 ,4 束 扇 形 光 先 后 到 达 接 收器 ,根 据 扇 形 激 光 所 载 的 信 息 特 征 判 断 该 时 刻 以 哪个 计 时 零 点 计 时 。 得 到 4 组 时 刻 t1A0 、 t2A0 、 t1B0 、 t2B0 。 其中 t1A0 、 t2A0 为 A 发 射 器 的 两 束 扇 形 激 光 被 接 收 时 刻 ,得 到 tA = t2A0 - t1A0 、 t1B0 、 t2B0 为 B 发 射 器 的 两 束 扇 形 激光 被 接 收 的 时 刻 ,得 到 tB = t2B0 - t1B0 。 根 据 式 (2) 计算 出 A、 B。 同 理 多 个 接 收 器 计 算 方 法 相 同 ,但 反馈 给 发 射 器 的 信 息 有 标 识 ,可 以 区 分 不 同 接 收 器 反馈 回 的 信 息 。 当 A、 B、 A、 B 测 量 完 毕 后 ,一 周 期的 测 量 结 束 ,开 始 下 一 周 期 的 测 量 。 图 8 为 系 统 结图 8 系 统 框 图 A B A B y1 = 1 + 2 + 1 + 2 (3) z1 = 1 + 2 + 1 + 2不 相 关 ,根 据 方 差 合 成 定 理 得 : 2 = x + x + x + x A B A B 2 y y y y = 1 + 2 + 3 + 4 A B A B zn+1 - zn( xn+1 - xn) +( yn+1 - yn) 2 +( zn+1 - zn) 2yn+1 - yn图 9 系 统 流 程 图第 6 期陈 双 叶 ,牛 经 龙 等 :自 行 走 式 地 下 掘 进 机 器 人 姿 态 测 量 系 统 的 设 计 8 412 系统测量误差分析系 统 的 坐 标 测 量 误 差 从 原 理 上 可 分 为 静 态 误 差和 动 态 误 差 。 静 态 误 差 主 要 源 于 发 射 器 和 接 收 器 本身 的 观 测 误 差 ,是 仪 器 观 测 误 差 通 过 系 统 测 量 相 互作 用 ,相 互 影 响 的 结 果 。 动 态 误 差 主 要 是 接 收 器 接收 发 射 器 信 号 时 间 上 不 同 步 和 载 体 运 动 共 同 作 用 的结 果 。 考 虑 到 测 量 技 术 的 成 熟 ,发 射 器 空 间 坐 标 在开 始 用 全 站 仪 定 位 所 产 生 的 误 差 可 以 忽 略 不 计 。2. 1 静 态 误 差 分 析静 态 误 差 是 目 标 点 即 接 收 器 在 静 止 条 件 下 观测 ,由 环 境 、 零 部 件 不 稳 定 等 因 素 造 成 的 ,具 有 随 机性 ,所 以 将 静 态 误 差 视 为 随 机 误 差 处 理 。 设 发 射 器和 接 收 器 观 测 量 A、 B、 A、 B 的 观 测 精 度 为 1 、 2 、 3 、 4 , 误 差 为 1 、 2 、 1 、 2 。 静 态 误 差 K1 = ( x1 , y1 , z1 ) ,测 量 精 度 为 k1 = ( x1 , y1 , z1 ) 。 对 式 (1) 全 微 分 得 : x = x + x + x + x 1 1 2 1 2y y y yA B A Bz z z z A B A B由 于 4 个 角 度 的 测 量 分 别 通 过 两 个 发 射 器 共 4种 光 束 独 立 测 量 得 出 ,所 以 4 个 观 测 量 彼 此 独 立 ,互152 2 2 2x1 1 2 3 42 2 2 2 y1 = A 1 +B 2 +A 3 +B 4 (4)2 z 2 z 2 z 2 z 2z12. 2 动 态 误 差 分 析在 系 统 测 量 过 程 中 ,由 于 目 标 点 接 收 器 的 运 动 和发 射 器 A、 B 不 同 角 速 度 的 旋 转 ,对 4 个 角 的 测 量 会 出现 2 个 时 刻 的 不 同 步 ,即 测 量 水 平 角 A、 B 时 2 发 射器 各 自 的 选 通 光 束 在 不 同 时 刻 被 接 收 器 接 收 ,接 收 时间 间 隔 为 T1 ,同 理 各 自 扇 形 光 束 测 量 俯 仰 角 A、 B,被 接 收 器 接 收 的 时 间 间 隔 为 T2 ,测 量 水 平 角 和 俯 仰角 之 间 的 时 间 间 隔 记 为 T3 ,建 立 动 态 误 差 模 型 ( 图10)。 测 量 过 程 中 目 标 点 相 对 于 接 收 器 静 止 ,接 收 器又 是 测 量 系 统 的 一 部 分 。 所 以 将 接 收 器 的 运 动 作 为测 量 装 置 内 部 的 运 动 ,它 所 造 成 的 误 差 等 同 于 系 统 本身 测 量 装 置 的 误 差 ,而 对 目 标 点 的 测 量 则 可 看 作 在 目标 点 静 止 时 的 测 量 。 又 由 于 动 态 误 差 的 大 小 和 方 向随 接 收 器 的 载 体 即 掘 进 机 器 人 的 掘 进 距 离 ,掘 进 方 向和 掘 进 速 度 大 小 变 化 而 变 化 ,具 有 不 确 定 性 ,不 能 确切 的 掌 握 ,所 以 将 动 态 误 差 视 为 未 定 系 统 误 差 处 理 。图 10 模 型 动 态 误 差 原 理模 型 图 表 明 时 间 差 T1 、 T2 、 T3 的 存 在 引 起测 量 系 统 观 测 量 的 误 差 ,实 际 测 量 值 为 A、 B、 A、 B,分 别 对 应 的 测 量 点 p1 、 p2 、 p3 、 p4 。 由 于 地 下 掘 进机 器 人 行 进 速 度 缓 慢 ,对 于 以 秒 级 为 测 量 周 期 的 测量 定 位 系 统 而 言 ,测 量 周 期 内 的 4 点 在 一 条 直 线 上 ,设 目 标 点 速 度 v 方 向 为 测 量 采 样 点 之 间 的 连 线 方 向( xn+1 - xn, yn+1 - yn, zn+1 - zn ) , 方 向 角 为 、 ,精 度 为sk2 = ( sx2 , sy2 , sz2 ) 。 = arctan (5)2 = arcsin (6)( xn+1 - xn) 2 +( yn+1 - yn) 2可 求 得 p1 , p2 , p3 , p4 4 个 测 量 点 坐 标 :xp1 = xnyp1 = yn (7)zp1 = znxp2 = xn +t1 vcos cosyp2 = yn +t1 vcos sin (8)zp2 = zn +t1 vsinxp3 = xp2 +t3 vcos cosyp3 = yp2 +t3 vcos sin (9)zp3 = zp2 +t3 vsinxp1 - xAxp2 - xA( xp3 - xB) +( yp3 - yB) 2 +( zp3 - zB) 2 Kx = +s Ky = 2y1 +s2y2 Kz = z1 +sz2以 日 本 Tolimec 公 司 制 造 的 MG -32B 系 统 为 例 ,系842传 感 技 术 学 报www. chinatransducers. com 第 26 卷xp4 = xp3 +t2 vcos cosyp4 = yp3 +t2 vcos sin (10)zp4 = zp3 +t2 vsin根 据 上 述 公 式 可 求 得 各 测 量 角 :yp1 - yA A = arctan (11)yp2 - yA B = arctan (12)zp3 - zB A = arcsin (13)2zp4 - zB B = arcsin (14)( xp4 - xB) 2 +( yp4 - yB) 2 +( zp4 - zB) 2将 式 (11) 式 (14) 代 入 式 (1) 得 目 标 点 的 实际 测 量 值 ( x , y , z ) 。 进 一 步 计 算 得 : x2 = x- x y2 = y- y (15) z2 = z- z动 态 误 差 K2 = ( x2 ,y2 ,z2 ) 。3 系 统 误 差 仿 真 K1 为 静 态 误 差 ,即 随 机 误 差 ,K2 为 动 态 误 差 ,即 系 统 误 差 。 根 据 系 统 误 差 与 随 机 误 差 合 成 定 理15 :2 2x1 x2(16)2 2为 保 证 测 量 的 精 度 ,应 选 择 合 适 的 结 构 。 对 于时 间 同 步 则 受 限 于 硬 件 设 备 性 能 ,所 以 需 要 适 当 缩小 观 测 误 差 范 围 ,扩 大 时 间 同 步 误 差 范 围 。根 据 式 (4) 进 行 仿 真 计 算 ,当 0 A + B 180,0 A, B 30时 ,测 角 精 度 为 10 , b 1 000 mm时 , x1 27. 78 mm, y1 43. 9 mm, z1 38. 75 mm。 测角 精 度 为 5 , b 1 000 mm 时 , x1 13. 89 mm, y1 20. 87 mm, z1 17. 49 mm。 考 虑 到 目 前 系 统 的 激光 测 角 精 度 和 在 掘 进 工 程 中 隧 道 标 高 误 差 需 控 制 在-20 mm 80 mm,平 面 误 差 控 制 在 50 mm,水 工 隧 道误 差 控 制 在 150 mm16 等 因 素 ,取 测 角 精 度 5 进行 分 配 后 得 : x1 20 mm, y1 25 mm, z1 20 mm。sx2 45 mm,sy2 43 mm,sz2 10 mm。通 过 动 态 测 量 误 差 模 型 可 知 ,目 标 点 速 度 与 误差 成 正 比 ,发 射 器 角 速 度 与 误 差 成 反 比 ,对 模 型 进 行仿 真 ,对 模 型 中 的 输 入 参 量 取 极 限 值 ,设 定 实 际 路线 。 目 标 行 进 速 度 为 v1 . 5 mm / s,发 射 器 A 的 角速 度 为 A / 4 rad / s,发 射 器 B 的 角 速 度 为 B / 3 rad / s。 A 点 坐 标 为 ( xA, yA, zA ) , B 点 坐 标 为( xB, yB, zB) 。 其 中 | yB - yA | 1 000 mm。 图 11 为 极限 值 情 况 下 的 仿 真 误 差 。 由 图 可 知 ,动 态 误 差 满 足上 述 误 差 分 配 要 求 。 该 导 航 定 位 方 法 在 掘 进 过 程 位姿 测 量 中 可 行 ,且 精 度 较 高 。惯 性 导 航 定 位 方 法 与 其 相 比 运 用 的 是 陀 螺 仪 对掘 进 机 进 行 方 位 和 倾 斜 角 检 测 ,同 时 利 用 其 中 的 加速 度 计 测 量 掘 进 机 的 加 速 度 并 对 其 进 行 两 次 积 分 求得 掘 进 距 离 。 通 过 对 方 位 角 ,倾 斜 角 和 掘 进 距 离 的计 算 得 到 掘 进 机 器 人 的 位 置 坐 标 。 图 12 表 示 机 器人 位 置 的 计 算 模 型 。 其 中 i 表 示 测 量 得 到 的 方 位角 与 倾 斜 角 。 从 已 知 点 A1 出 发 ,沿 1 方 向 掘 进 距离 L1 ,由 此 算 出 点 A2 的 坐 标 。 再 从 A2 出 发 ,用 同 样方 法 算 出 A3 。 以 此 类 推 , 可 算 出 直 到 An 的 坐 标 。图 11 系 统 仿 真图 12 惯 性 系 统 位 置 计 算 模 型第 6 期 陈 双 叶 ,牛 经 龙 等 :自 行 走 式 地 下 掘 进 机 器 人 姿 态 测 量 系 统 的 设 计 8 43统 方 位 角 静 止 点 精 度 0. 2,俯 仰 角 精 度 0. 117 。设 定 实 际 路 线 与 上 述 相 同 ,不 考 虑 陀 螺 仪 加 速 度 计误 差 对 时 间 积 分 的 影 响 , 对 惯 性 系 统 进 行 仿 真 。图 13 可 知 随 着 掘 进 距 离 的 增 加 , 定 位 误 差 逐 渐 增大 。 该 仿 真 是 在 较 理 想 的 情 况 下 ,在 实 际 测 量 过 程中 由 于 加 工 工 艺 误 差 、 信 号 检 测 误 差 引 起 的 陀 螺 漂移 会 严 重 影 响 测 量 的 精 度 。 此 外 ,环 境 温 度 的 变 化对 陀 螺 的 机 械 、 电 学 性 能 影 响 也 较 大 ,有 的 甚 至 是 质的 变 化 ,这 无 疑 也 会 导 致 陀 螺 精 度 的 下 降 。 这 些 都会 使 得 测 量 结 果 的 误 差 大 于 图 中 仿 真 结 果 。 与 上 述测 量 系 统 相 比 它 的 精 度 和 稳 定 性 显 得 不 足 。4 结 论针 对 目 前 自 行 走 式 地 下 掘 进 机 器 人 地 下 姿 态 测量 方 法 的 不 足 ,本 文 提 出 的 新 型 地 下 姿 态 测 量 方 法更 加 合 理 。 该 系 统 操 作 简 便 ,实 时 性 好 ,精 度 较 高 ,对 地 下 机 器 人 掘 进 过 程 中 的 姿 态 定 位 具 有 一 定 的 应用 价 值 。参 考 文 献 :1 卢 清 国 ,王 庆 学 ,张 延 钦 ,等 . 自 行 走 式 地 下 管 道 掘 进 机 工 业 样机 试 验 J. 建 筑 机 械 ,2010(7):81-86.2 唐 争 气 ,赵 俊 三 ,彭 国 新 . 盾 构 机 实 时 姿 态 测 量 和 计 算 方 法 的研 究 J. 土 木 工 程 学 报 ,2007,40(11):92-97.3456789冯 宝 新 ,王 解 先 . 盾 构 姿 态 测 量 方 法 J. 公 路 隧 道 ,2011(1):63-65.潘 国 荣 ,王 穗 辉 ,陈 传 林 ,等 . 盾 构 姿 态 自 动 测 量 系 统 的 开 发 与应 用 J. 同 济 大 学 学 报 ,2010,38(3):459-463.潘 明 华 . 盾 构 自 动 导 向 系 统 的 研 究 与 实 现 D. 湖 北 :华 中 科技 大 学 ,2005.马 建 仓 ,陈 静 . MEMS SINS-GPS 组 合 导 航 系 统 设 计 J. 传 感 技术 学 报 ,2009,22(10):1437-1441.王 晶 晶 ,童 敏 明 ,刘 彬 . 组 合 导 航 定 位 系 统 研 究 J. Software,2011,32(5):82-84.Thomas M H