运动控制方法、装置及系统的制作方法
【专利摘要】本发明提供了一种运动控制方法、装置及系统，将计算机视觉技术和自动化控制技术结合起来，实现了在三维环境下的追踪，提高了追踪控制的准确度；其使用方式简单，用户只需要选取指定跟随目标之后，之后所有的追踪操作由系统自动化实现，可移动载体可以精确地、自动化地跟踪指定目标；拓展了现有计算机视觉技术的应用场景，不仅适用于计算机视觉追踪技术的主流应用场景，还适用于一些创新型的应用场景，如超市自动跟随购物车，家庭自动看护机器人等。
【专利说明】
运动控制方法、装置及系统
技术领域
[0001] 本发明涉及计算机视觉技术领域和自动化控制技术领域，尤其涉及一种运动控制 方法、装置及系统。
【背景技术】
[0002] 计算机视觉基于特征点的快速跟踪方法是指通过图像内的特征点进行物体的跟 踪。目前主要通过图像特征点或者图像表面的方法获得目标在图像中的位置，从而实现用 户通过传感器传回的图像对指定跟随目标的跟踪工作。可靠、精确的指定物体跟踪在军事、 公共安全、家庭服务等方面都拥有广阔的市场前景，也是当下研究的热门技术之一。在军事 上，跟踪技术可以协助导弹跟踪目标，在军用无人机上可以从空中通过传感器实时跟踪指 定目标;在公共安全方面，可以通过对罪犯在摄像头内进行跟踪，在交通控制上多目标跟踪 车辆行人以掌握交通流量和动向；在家庭服务方面，可以使各种家用机器人主动跟随主人 行进。
[0003] 自动化控制技术广泛用于工业、农业、军事、科学研究、交通运输、商业、医疗、服务 和家庭等方面。采用自动化控制不仅可以把人从繁重的体力劳动、部分脑力劳动以及恶劣、 危险的工作环境中解放出来，而且能扩展人的器官功能，极大地提高劳动生产率，增强人类 认识世界和改造世界的能力。因此，自动化控制是工业、农业、国防和科学技术现代化的重 要条件和显著标志。
[0004] 计算机视觉追踪技术现在的应用范围主要是在图像中对指定目标的追踪，也可以 理解为是二维环境下的追踪技术，这大大限制了该技术的发展。
【发明内容】
[0005] 本发明的目的是提供一种运动控制方法、装置及系统，解决普通运动控制方法其 运动控制方法本体无法处于正面视觉位置的问题。
[0006] 本发明解决其技术问题所采用的技术方案是：
[0007] 一种运动控制方法，包括：
[0008] 确定指定目标；
[0009] 通过可移动载体拍摄二维图像;根据指定目标的颜色信息和深度信息，识别出二 维图像中的指定目标，在二维图像中对指定目标进行跟踪，获取指定目标的相对位置信息； 获取可移动载体姿态信息和里程信息，根据可移动载体的姿态信息和里程信息以及指定目 标的相对位置信息，通过姿态变换矩阵获取指定目标的世界位置信息;所述指定目标的相 对位置信息为指定目标在二维图像中的位置信息，所述指定目标的世界位置信息为指定目 标在世界坐标系中的位置信息；
[0010] 获取环境中每个障碍物在世界坐标系中的位置信息;根据可移动载体的姿态信息 和里程信息以及环境中每个障碍物在世界坐标系中的位置信息，获取可移动载体周围的障 碍物在世界坐标系中的位置信息;根据可移动载体周围的障碍物在世界坐标系中的位置信 息，获取可移动载体下一步的运动状态；
[0011] 根据可移动载体的姿态信息和里程信息，获取可移动载体的世界位置信息，所述 可移动载体的世界位置信息为可移动载体在世界坐标系中的位置信息;根据指定目标的世 界位置信息和可移动载体的世界位置信息，获取可移动载体的运动速度；
[0012] 根据所述运动状态和所述运动速度，获取该运动状态下，可移动载体要达到该运 动速度所需要的动力信息;根据所述动力信息，控制可移动载体的运动，获取可移动载体的 里程信息并保存。
[0013] 在此基础上，进一步地，所述通过可移动载体拍摄二维图像;根据指定目标的颜色 信息和深度信息，识别出二维图像中的指定目标，在二维图像中对指定目标进行跟踪，获取 指定目标的相对位置信息；获取可移动载体姿态信息和里程信息，根据可移动载体的姿态 信息和里程信息以及指定目标的相对位置信息，通过姿态变换矩阵获取指定目标的世界位 置信息;所述指定目标的相对位置信息为指定目标在二维图像中的位置信息，所述指定目 标的世界位置信息为指定目标在世界坐标系中的位置信息的步骤，具体为：
[0014] 通过可移动载体拍摄二维图像;根据指定目标的颜色信息和深度信息，当二维图 像中未发现指定目标时，调取指定目标之前的世界位置信息，作为指定目标的世界位置信 息；
[0015] 当二维图像中发现指定目标时，识别出二维图像中的指定目标，在二维图像中对 指定目标进行跟踪，获取指定目标的相对位置信息;获取可移动载体姿态信息和里程信息， 根据可移动载体的姿态信息和里程信息以及指定目标的相对位置信息，通过姿态变换矩阵 获取指定目标的世界位置信息;所述指定目标的相对位置信息为指定目标在二维图像中的 位置信息，所述指定目标的世界位置信息为指定目标在世界坐标系中的位置信息。
[0016] 在上述任意实施例的基础上，进一步地，所述根据所述运动状态和所述运动速度， 获取该运动状态下，可移动载体要达到该运动速度所需要的动力信息;根据所述动力信息， 控制可移动载体的运动，获取可移动载体的里程信息并保存的步骤，具体为：
[0017] 根据所述运动状态和所述运动速度，获取该运动状态下，可移动载体要达到该运 动速度所需要的动力信息;根据可移动载体的速度信息，对动力信息进行调整;根据调整后 的动力信息，控制可移动载体的运动，获取可移动载体的里程信息并保存。
[0018] 一种运动控制装置，包括：
[0019] 指定目标确定模块，用于确定指定目标；
[0020] 指定目标定位模块，用于通过可移动载体拍摄二维图像;根据指定目标的颜色信 息和深度信息，识别出二维图像中的指定目标，在二维图像中对指定目标进行跟踪，获取指 定目标的相对位置信息；获取可移动载体姿态信息和里程信息，根据可移动载体的姿态信 息和里程信息以及指定目标的相对位置信息，通过姿态变换矩阵获取指定目标的世界位置 信息;所述指定目标的相对位置信息为指定目标在二维图像中的位置信息，所述指定目标 的世界位置信息为指定目标在世界坐标系中的位置信息；
[0021] 可移动载体解算模块，用于获取环境中每个障碍物在世界坐标系中的位置信息； 根据可移动载体的姿态信息和里程信息以及环境中每个障碍物在世界坐标系中的位置信 息，获取可移动载体周围的障碍物在世界坐标系中的位置信息;根据可移动载体周围的障 碍物在世界坐标系中的位置信息，获取可移动载体下一步的运动状态;根据可移动载体的 姿态信息和里程信息，获取可移动载体的世界位置信息，所述可移动载体的世界位置信息 为可移动载体在世界坐标系中的位置信息;根据指定目标的世界位置信息和可移动载体的 世界位置信息，获取可移动载体的运动速度;根据所述运动状态和所述运动速度，获取该运 动状态下，可移动载体要达到该运动速度所需要的动力信息;根据所述动力信息，控制可移 动载体的运动，获取可移动载体的里程信息并保存。
[0022] 在此基础上，进一步地：
[0023] 指定目标定位模块，用于通过可移动载体拍摄二维图像;根据指定目标的颜色信 息和深度信息，当二维图像中未发现指定目标时，调取指定目标之前的世界位置信息，作为 指定目标的世界位置信息；当二维图像中发现指定目标时，识别出二维图像中的指定目标， 在二维图像中对指定目标进行跟踪，获取指定目标的相对位置信息；获取可移动载体姿态 信息和里程信息，根据可移动载体的姿态信息和里程信息以及指定目标的相对位置信息， 通过姿态变换矩阵获取指定目标的世界位置信息;所述指定目标的相对位置信息为指定目 标在二维图像中的位置信息，所述指定目标的世界位置信息为指定目标在世界坐标系中的 位置信息。
[0024] 在上述任意实施例的基础上，进一步地：
[0025] 可移动载体解算模块，用于获取环境中每个障碍物在世界坐标系中的位置信息； 根据可移动载体的姿态信息和里程信息以及环境中每个障碍物在世界坐标系中的位置信 息，获取可移动载体周围的障碍物在世界坐标系中的位置信息;根据可移动载体周围的障 碍物在世界坐标系中的位置信息，获取可移动载体下一步的运动状态;根据可移动载体的 姿态信息和里程信息，获取可移动载体的世界位置信息，所述可移动载体的世界位置信息 为可移动载体在世界坐标系中的位置信息;根据指定目标的世界位置信息和可移动载体的 世界位置信息，获取可移动载体的运动速度;根据所述运动状态和所述运动速度，获取该运 动状态下，可移动载体要达到该运动速度所需要的动力信息;根据可移动载体的速度信息， 对动力信息进行调整;根据调整后的动力信息，控制可移动载体的运动，获取可移动载体的 里程信息并保存。
[0026] 一种运动控制系统，包括：
[0027]数据采集装置，包括RGBD摄像头、激光雷达、超声波发射器、惯性测量传感器;RGBD 摄像头获取指定目标的颜色信息和深度信息并上传至数据分析平台；激光雷达和超声波发 射器进行障碍信息检测，获取环境中每个障碍物在世界坐标系中的位置信息并上传至数据 分析平台；惯性测量传感器获取可移动载体的姿态信息并上传至数据分析平台；
[0028]数据分析平台，包括运动解算模块和上层PID控制器;运动解算模块确定指定目 标;通过可移动载体拍摄二维图像;根据指定目标的颜色信息和深度信息，识别出二维图像 中的指定目标，在二维图像中对指定目标进行跟踪，获取指定目标的相对位置信息;获取可 移动载体姿态信息和里程信息，根据可移动载体的姿态信息和里程信息以及指定目标的相 对位置信息，通过姿态变换矩阵获取指定目标的世界位置信息;所述指定目标的相对位置 信息为指定目标在二维图像中的位置信息，所述指定目标的世界位置信息为指定目标在世 界坐标系中的位置信息；获取环境中每个障碍物在世界坐标系中的位置信息;根据可移动 载体的姿态信息和里程信息以及环境中每个障碍物在世界坐标系中的位置信息，获取可移 动载体周围的障碍物在世界坐标系中的位置信息;根据可移动载体周围的障碍物在世界坐 标系中的位置信息，获取可移动载体下一步的运动状态;根据可移动载体的姿态信息和里 程信息，获取可移动载体的世界位置信息，所述可移动载体的世界位置信息为可移动载体 在世界坐标系中的位置信息；上层PID控制器根据指定目标的世界位置信息和可移动载体 的世界位置信息，获取可移动载体的运动速度；
[0029] 底层控制平台，包括底层控制芯片、下层PID控制器、动力装置、运动模块、测速模 块、里程模块;底层控制芯片接收数据分析平台发送的运动状态和运动速度，获取该运动状 态下，可移动载体要达到该运动速度所需要的预定动力信息；下层PID控制器接收测速模块 发送的速度信息，通过自反馈机制，根据可移动载体的速度信息，对动力信息进行调整并将 调整后的动力信息传输到动力装置;动力装置根据调整后的动力信息，控制运动模块实现 可移动载体的运动，并将调整后的动力信息传输到运动模块、测速模块和里程模块;运动模 块控制可移动载体的移动;测速模块测算可移动载体的速度信息并发送到下层PID控制器； 里程模块测算可移动载体的里程信息并上传至数据分析平台。
[0030] 本发明的有益效果是：
[0031] 本发明提供了一种运动控制方法、装置及系统，将计算机视觉技术和自动化控制 技术结合起来，实现了在三维环境下的追踪，提高了追踪控制的准确度;其使用方式简单， 用户只需要选取指定跟随目标之后，之后所有的追踪操作由系统自动化实现，可移动载体 可以精确地、自动化地跟踪指定目标;拓展了现有计算机视觉技术的应用场景，不仅适用于 计算机视觉追踪技术的主流应用场景，还适用于一些创新型的应用场景，如超市自动跟随 购物车，家庭自动看护机器人等。
【附图说明】
[0032]下面结合附图和实施例对本发明进一步说明。
[0033] 图1示出了本发明实施例提供的一种运动控制方法的流程图；
[0034] 图2示出了本发明实施例提供的一种运动控制装置的结构示意图；
[0035] 图3示出了本发明实施例提供的一种运动控制系统的结构示意图；
[0036] 图4示出了本发明实施例提供的一种运动控制系统的结构示意图；
[0037] 图5示出了本发明实施例提供的一种运动控制系统的状态示意图；
[0038] 图6示出了本发明实施例提供的一种运动控制方法的流程示意图。
【具体实施方式】
[0039]为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对 本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不 限定本发明。
[0040] 具体实施例一
[0041 ]如图1所示，本发明实施例提供了一种运动控制方法，包括：
[0042] 步骤SlOl，确定指定目标；
[0043]步骤S102,通过可移动载体拍摄二维图像;根据指定目标的颜色信息和深度信息， 识别出二维图像中的指定目标，在二维图像中对指定目标进行跟踪，获取指定目标的相对 位置信息;获取可移动载体姿态信息和里程信息，根据可移动载体的姿态信息和里程信息 以及指定目标的相对位置信息，通过姿态变换矩阵获取指定目标的世界位置信息;所述指 定目标的相对位置信息为指定目标在二维图像中的位置信息，所述指定目标的世界位置信 息为指定目标在世界坐标系中的位置信息；
[0044] 步骤S103,获取环境中每个障碍物在世界坐标系中的位置信息;根据可移动载体 的姿态信息和里程信息以及环境中每个障碍物在世界坐标系中的位置信息，获取可移动载 体周围的障碍物在世界坐标系中的位置信息;根据可移动载体周围的障碍物在世界坐标系 中的位置信息，获取可移动载体下一步的运动状态；
[0045] 步骤S104,根据可移动载体的姿态信息和里程信息，获取可移动载体的世界位置 信息，所述可移动载体的世界位置信息为可移动载体在世界坐标系中的位置信息;根据指 定目标的世界位置信息和可移动载体的世界位置信息，获取可移动载体的运动速度；
[0046] 步骤S105，根据所述运动状态和所述运动速度，获取该运动状态下，可移动载体要 达到该运动速度所需要的动力信息;根据所述动力信息，控制可移动载体的运动，获取可移 动载体的里程信息并保存。
[0047] 本发明实施例将计算机视觉技术和自动化控制技术结合起来，实现了在三维环境 下的追踪，提高了追踪控制的准确度;其使用方式简单，用户只需要选取指定跟随目标之 后，之后所有的追踪操作由系统自动化实现，可移动载体可以精确地、自动化地跟踪指定目 标;拓展了现有计算机视觉技术的应用场景，不仅适用于计算机视觉追踪技术的主流应用 场景，还适用于一些创新型的应用场景，如超市自动跟随购物车，家庭自动看护机器人等。
[0048] 考虑到现实环境的复杂性，本发明实施例还可以引入丢失找回算法。在上述实施 例的基础上，优选的，步骤S102可以具体为:通过可移动载体拍摄二维图像;根据指定目标 的颜色信息和深度信息，当二维图像中未发现指定目标时，调取指定目标之前的世界位置 信息，作为指定目标的世界位置信息；当二维图像中发现指定目标时，识别出二维图像中的 指定目标，在二维图像中对指定目标进行跟踪，获取指定目标的相对位置信息;获取可移动 载体姿态信息和里程信息，根据可移动载体的姿态信息和里程信息以及指定目标的相对位 置信息，通过姿态变换矩阵获取指定目标的世界位置信息;所述指定目标的相对位置信息 为指定目标在二维图像中的位置信息，所述指定目标的世界位置信息为指定目标在世界坐 标系中的位置信息。丢失找回算法的引入提高了三维环境下追踪技术的安全性和稳定性。
[0049] 本发明实施例在测算到可移动载体的速度信息后，可以根据该实际速度信息调整 动力信息，提高跟踪的精确度。优选的，步骤S105可以为:根据所述运动状态和所述运动速 度，获取该运动状态下，可移动载体要达到该运动速度所需要的动力信息;根据可移动载体 的速度信息，对动力信息进行调整;根据调整后的动力信息，控制可移动载体的运动，获取 可移动载体的里程信息并保存。
[0050] 具体实施例二
[0051] 如图2所示，本发明实施例提供了一种运动控制装置，包括：
[0052]指定目标确定模块201，用于确定指定目标；
[0053]指定目标定位模块202,用于通过可移动载体拍摄二维图像;根据指定目标的颜色 信息和深度信息，识别出二维图像中的指定目标，在二维图像中对指定目标进行跟踪，获取 指定目标的相对位置信息；获取可移动载体姿态信息和里程信息，根据可移动载体的姿态 信息和里程信息以及指定目标的相对位置信息，通过姿态变换矩阵获取指定目标的世界位 置信息;所述指定目标的相对位置信息为指定目标在二维图像中的位置信息，所述指定目 标的世界位置信息为指定目标在世界坐标系中的位置信息；
[0054] 可移动载体解算模块203,用于获取环境中每个障碍物在世界坐标系中的位置信 息;根据可移动载体的姿态信息和里程信息以及环境中每个障碍物在世界坐标系中的位置 信息，获取可移动载体周围的障碍物在世界坐标系中的位置信息;根据可移动载体周围的 障碍物在世界坐标系中的位置信息，获取可移动载体下一步的运动状态;根据可移动载体 的姿态信息和里程信息，获取可移动载体的世界位置信息，所述可移动载体的世界位置信 息为可移动载体在世界坐标系中的位置信息;根据指定目标的世界位置信息和可移动载体 的世界位置信息，获取可移动载体的运动速度;根据所述运动状态和所述运动速度，获取该 运动状态下，可移动载体要达到该运动速度所需要的动力信息;根据所述动力信息，控制可 移动载体的运动，获取可移动载体的里程信息并保存。
[0055] 本发明实施例将计算机视觉技术和自动化控制技术结合起来，实现了在三维环境 下的追踪，提高了追踪控制的准确度;其使用方式简单，用户只需要选取指定跟随目标之 后，之后所有的追踪操作由系统自动化实现，可移动载体可以精确地、自动化地跟踪指定目 标;拓展了现有计算机视觉技术的应用场景，不仅适用于计算机视觉追踪技术的主流应用 场景，还适用于一些创新型的应用场景，如超市自动跟随购物车，家庭自动看护机器人等。 [0056]在此基础上，优选的，指定目标定位模块203可以通过可移动载体拍摄二维图像； 根据指定目标的颜色信息和深度信息，当二维图像中未发现指定目标时，调取指定目标之 前的世界位置信息，作为指定目标的世界位置信息；当二维图像中发现指定目标时，识别出 二维图像中的指定目标，在二维图像中对指定目标进行跟踪，获取指定目标的相对位置信 息;获取可移动载体姿态信息和里程信息，根据可移动载体的姿态信息和里程信息以及指 定目标的相对位置信息，通过姿态变换矩阵获取指定目标的世界位置信息;所述指定目标 的相对位置信息为指定目标在二维图像中的位置信息，所述指定目标的世界位置信息为指 定目标在世界坐标系中的位置信息。考虑到现实环境的复杂性，丢失找回算法的引入，提高 了三维环境下追踪技术的安全性和稳定性。
[0057]本发明实施例在测算到可移动载体的速度信息后，可以根据该实际速度信息调整 动力信息，提高跟踪的精确度，优选的，可移动载体解算模块204可以根据可移动载体的速 度信息，对动力信息进行调整，再根据调整后的动力信息，控制可移动载体的运动，获取可 移动载体的里程信息并保存。
[0058] 具体实施例三
[0059] 如图3所示，本发明实施例提供了一种运动控制系统，包括：
[0060] 数据采集装置301，包括RGBD摄像头、激光雷达、超声波发射器、惯性测量传感器； RGBD摄像头获取指定目标的颜色信息和深度信息并上传至数据分析平台；激光雷达和超声 波发射器进行障碍信息检测，获取环境中每个障碍物在世界坐标系中的位置信息并上传至 数据分析平台；惯性测量传感器获取可移动载体的姿态信息并上传至数据分析平台；
[0061 ]数据分析平台302，包括运动解算模块和上层PID控制器;运动解算模块确定指定 目标;通过可移动载体拍摄二维图像;根据指定目标的颜色信息和深度信息，识别出二维图 像中的指定目标，在二维图像中对指定目标进行跟踪，获取指定目标的相对位置信息;获取 可移动载体姿态信息和里程信息，根据可移动载体的姿态信息和里程信息以及指定目标的 相对位置信息，通过姿态变换矩阵获取指定目标的世界位置信息;所述指定目标的相对位 置信息为指定目标在二维图像中的位置信息，所述指定目标的世界位置信息为指定目标在 世界坐标系中的位置信息；获取环境中每个障碍物在世界坐标系中的位置信息;根据可移 动载体的姿态信息和里程信息以及环境中每个障碍物在世界坐标系中的位置信息，获取可 移动载体周围的障碍物在世界坐标系中的位置信息;根据可移动载体周围的障碍物在世界 坐标系中的位置信息，获取可移动载体下一步的运动状态;根据可移动载体的姿态信息和 里程信息，获取可移动载体的世界位置信息，所述可移动载体的世界位置信息为可移动载 体在世界坐标系中的位置信息；上层PID控制器根据指定目标的世界位置信息和可移动载 体的世界位置信息，获取可移动载体的运动速度；
[0062]底层控制平台303,包括底层控制芯片、下层PID控制器、动力装置、运动模块、测速 模块、里程模块;底层控制芯片接收数据分析平台发送的运动状态和运动速度，获取该运动 状态下，可移动载体要达到该运动速度所需要的预定动力信息；下层PID控制器接收测速模 块发送的速度信息，通过自反馈机制，根据可移动载体的速度信息，对动力信息进行调整并 将调整后的动力信息传输到动力装置;动力装置根据调整后的动力信息，控制运动模块实 现可移动载体的运动，并将调整后的动力信息传输到运动模块、测速模块和里程模块;运动 模块控制可移动载体的移动;测速模块测算可移动载体的速度信息并发送到下层PID控制 器;里程模块测算可移动载体的里程信息并上传至数据分析平台。
[0063]本发明实施例将计算机视觉技术和自动化控制技术结合起来，实现了在三维环境 下的追踪，提高了追踪控制的准确度;其使用方式简单，用户只需要选取指定跟随目标之 后，之后所有的追踪操作由系统自动化实现，可移动载体可以精确地、自动化地跟踪指定目 标;拓展了现有计算机视觉技术的应用场景，不仅适用于计算机视觉追踪技术的主流应用 场景，还适用于一些创新型的应用场景，如超市自动跟随购物车，家庭自动看护机器人等。 本发明实施例还对PID控制技术做了进一步的优化，采用了双层PID控制技术，进一步增加 了自动化追踪的精确度以及敏感度。考虑到现实条件下环境的复杂，本发明实施例还添加 了相应的避障措施以及丢失找回策略，进一步保障了可移动载体追踪的安全性与稳定性。 [0064]具体实施例四
[0065]如图4所示，本发明实施例提供了一种运动控制系统。整个系统主要分为三个部 分，包括数据采集装置，数据分析平台和底层控制平台。数据采集装置是一个用于收集环境 中颜色及深度信息以及周围环境信息，并可实时处理上传的三维扫描设备。数据采集装置 包含嵌入式中央处理器芯片、深度测距芯片、深度摄像机元件，颜色摄像机元件、超声波元 件、雷达元件等模块。数据分析平台是在运动解算模块中利用中央处理器芯片实现相应的 运动解算算法，并且实现上层的PID控制。底层控制平台主要包括底盘控制芯片、动力装置、 测算元件、里程元件等设备，主要是接受上层传输过来的运动信息，对可移动载体进行控 制。
[0066] 图5示出了本发明所实现系统中的状态。本发明中整个系统共存在6个状态，包括 等待，失联等待，跟随，即将丢失，完全丢失，丢失找回。系统整个运行过程中在六个状态中 切换，实现了在三维环境下对指定目标的追踪。
[0067] 图6所示的是系统在实际运行中的流程图，该流程图用来具体说明整套方案的工 作流程和细节实施方法。
[0068] 具体实施步骤如下：
[0069] 步骤1):构造数据采集装置，包括RGB-D传感器、惯性测量传感器、超声波发射器、 激光雷达，收集指定目标的颜色信息和深度信息、可移动载体此时的姿态信息，可移动载体 周围的环境信息，并上传至数据分析平台进行指定目标的运动解算。
[0070] 步骤2):利用基于计算机视觉特征点的追踪算法，通过RGBD传感器收集的深度颜 色信息，识别出图像中的指定目标，在二维图像中对指定目标进行跟踪，实时获得指定目标 在图像中的位置坐标。
[0071] 利用基于计算机视觉测程原理的算法对数据采集装置在不同时刻收集到的信息 进行相对位置估计，目的是将摄像头坐标系下的点转化为世界坐标系下，从而获得三维环 境中每一个位置的三维坐标。首先，对于深度摄像头下的点，我们能够获取它在二维图像中 的位置(u，v)以及他的深度信息d，所以每个点的像素坐标为[u，v，d]。在深度摄像头中，可 以通过公式(1)，可以将点的像素坐标转换为摄像头坐标系的三维坐标[x，y，z]。
[0072] z = dls，
[0073] X= (u-Cx) Xzlfx，
[0074] y=(V-Cy)χfyo
[0075] 接下来用姿态转换矩阵将摄像头坐标系下的三维坐标[x，y，z]转换为世界坐标系 下的三维坐标[xl，yl，zl]。由公式(2)可知，通过摄像头到可移动载体的姿态旋转矩阵和平 移矩阵可以得出在可移动载体设备坐标系下的三维坐标[ X_r，y_r，z_r]表示，如下式所示：
[0076]
[0077]最后通过惯性测量装置，计算出此时可移动载体设备的姿态信息，计算出可移动 载体设备到世界坐标系下的姿态旋转矩阵，如下式：
[0078]
[0079]可以最终计算出世界坐标系下的三维坐标。通过上述流程，可以指定目标的像素 坐标与世界坐标建立一一映射关系，得到指定目标世界坐标系下的位置关系，将其上传至 数据分析平台并存储于系统中位置信息存储空间。
[0080]更优选的，如果图像中未发现指定目标，则系统计入即将丢失状态并且累计目标 丢失的时间，如果累积丢失时间超过一定阈值，则系统进入完全丢失状态，自动开启丢失找 回模式，从位置信息存储空间获取之前目标位置。
[0081 ]步骤3):获取指定目标的位置信息后，进入运动解算模块，通过指定目标位置信息 与当前可移动载体的位置信息，可以计算出可移动载体与指定目标之间的偏移角度以及相 隔距离，将此运动信息传输给PID控制器，通过PID自反馈控制，在上层实现对运动状态的粗 调节，并将其传给底层控制芯片。
[0082]步骤4):将数据分析平台计算出的运动状态和运动速度信息传输给底层控制芯 片，底层控制芯片解算出此运动状态下，要达到该运动速度所需要的动力，并将其输入给第 二层(底层)PID控制器，通过PID控制器的自反馈机制，该PID控制器从硬件的角度，精确估 计出此状态下所需要的动力，并且传输给动力装置。动力装置传输给运动模块控制可移动 载体的移动，实现可移动载体的精准的跟随。与此同时，动力装置也会将运动信息传输给测 速模块以及里程模块，实现系统的自反馈控制和里程信息的测算。
[0083]步骤5):更新测速信息和里程信息，判断系统是否停止跟随，如果停止跟随则控制 可移动载体原地等待，否则回到步骤2。
[0084]尽管本发明已进行了一定程度的描述，明显地，在不脱离本发明的精神和范围的 条件下，可进行各个条件的适当变化。可以理解，本发明不限于所述实施方案，而归于权利 要求的范围，其包括所述每个因素的等同替换。
【主权项】
1. 一种运动控制方法，其特征在于，包括： 确定指定目标； 通过可移动载体拍摄二维图像;根据指定目标的颜色信息和深度信息，识别出二维图 像中的指定目标，在二维图像中对指定目标进行跟踪，获取指定目标的相对位置信息;获取 可移动载体姿态信息和里程信息，根据可移动载体的姿态信息和里程信息以及指定目标的 相对位置信息，通过姿态变换矩阵获取指定目标的世界位置信息;所述指定目标的相对位 置信息为指定目标在二维图像中的位置信息，所述指定目标的世界位置信息为指定目标在 世界坐标系中的位置信息； 获取环境中每个障碍物在世界坐标系中的位置信息;根据可移动载体的姿态信息和里 程信息以及环境中每个障碍物在世界坐标系中的位置信息，获取可移动载体周围的障碍物 在世界坐标系中的位置信息;根据可移动载体周围的障碍物在世界坐标系中的位置信息， 获取可移动载体下一步的运动状态； 根据可移动载体的姿态信息和里程信息，获取可移动载体的世界位置信息，所述可移 动载体的世界位置信息为可移动载体在世界坐标系中的位置信息;根据指定目标的世界位 置信息和可移动载体的世界位置信息，获取可移动载体的运动速度； 根据所述运动状态和所述运动速度，获取该运动状态下，可移动载体要达到该运动速 度所需要的动力信息;根据所述动力信息，控制可移动载体的运动，获取可移动载体的里程 信息并保存。2. 根据权利要求1所述的运动控制方法，其特征在于，所述通过可移动载体拍摄二维图 像;根据指定目标的颜色信息和深度信息，识别出二维图像中的指定目标，在二维图像中对 指定目标进行跟踪，获取指定目标的相对位置信息;获取可移动载体姿态信息和里程信息， 根据可移动载体的姿态信息和里程信息以及指定目标的相对位置信息，通过姿态变换矩阵 获取指定目标的世界位置信息;所述指定目标的相对位置信息为指定目标在二维图像中的 位置信息，所述指定目标的世界位置信息为指定目标在世界坐标系中的位置信息的步骤， 具体为： 通过可移动载体拍摄二维图像;根据指定目标的颜色信息和深度信息，当二维图像中 未发现指定目标时，调取指定目标之前的世界位置信息，作为指定目标的世界位置信息； 当二维图像中发现指定目标时，识别出二维图像中的指定目标，在二维图像中对指定 目标进行跟踪，获取指定目标的相对位置信息;获取可移动载体姿态信息和里程信息，根据 可移动载体的姿态信息和里程信息以及指定目标的相对位置信息，通过姿态变换矩阵获取 指定目标的世界位置信息;所述指定目标的相对位置信息为指定目标在二维图像中的位置 信息，所述指定目标的世界位置信息为指定目标在世界坐标系中的位置信息。3. 根据权利要求1或2所述的运动控制方法，其特征在于，所述根据所述运动状态和所 述运动速度，获取该运动状态下，可移动载体要达到该运动速度所需要的动力信息;根据所 述动力信息，控制可移动载体的运动，获取可移动载体的里程信息并保存的步骤，具体为： 根据所述运动状态和所述运动速度，获取该运动状态下，可移动载体要达到该运动速 度所需要的动力信息;根据可移动载体的速度信息，对动力信息进行调整;根据调整后的动 力信息，控制可移动载体的运动，获取可移动载体的里程信息并保存。4. 一种运动控制装置，其特征在于，包括： 指定目标确定模块，用于确定指定目标； 指定目标定位模块，用于通过可移动载体拍摄二维图像;根据指定目标的颜色信息和 深度信息，识别出二维图像中的指定目标，在二维图像中对指定目标进行跟踪，获取指定目 标的相对位置信息；获取可移动载体姿态信息和里程信息，根据可移动载体的姿态信息和 里程信息以及指定目标的相对位置信息，通过姿态变换矩阵获取指定目标的世界位置信 息;所述指定目标的相对位置信息为指定目标在二维图像中的位置信息，所述指定目标的 世界位置信息为指定目标在世界坐标系中的位置信息； 可移动载体解算模块，用于获取环境中每个障碍物在世界坐标系中的位置信息；根据 可移动载体的姿态信息和里程信息以及环境中每个障碍物在世界坐标系中的位置信息，获 取可移动载体周围的障碍物在世界坐标系中的位置信息;根据可移动载体周围的障碍物在 世界坐标系中的位置信息，获取可移动载体下一步的运动状态;根据可移动载体的姿态信 息和里程信息，获取可移动载体的世界位置信息，所述可移动载体的世界位置信息为可移 动载体在世界坐标系中的位置信息;根据指定目标的世界位置信息和可移动载体的世界位 置信息，获取可移动载体的运动速度;根据所述运动状态和所述运动速度，获取该运动状态 下，可移动载体要达到该运动速度所需要的动力信息;根据所述动力信息，控制可移动载体 的运动，获取可移动载体的里程信息并保存。5. 根据权利要求4所述的运动控制装置，其特征在于： 指定目标定位模块，用于通过可移动载体拍摄二维图像;根据指定目标的颜色信息和 深度信息，当二维图像中未发现指定目标时，调取指定目标之前的世界位置信息，作为指定 目标的世界位置信息；当二维图像中发现指定目标时，识别出二维图像中的指定目标，在二 维图像中对指定目标进行跟踪，获取指定目标的相对位置信息；获取可移动载体姿态信息 和里程信息，根据可移动载体的姿态信息和里程信息以及指定目标的相对位置信息，通过 姿态变换矩阵获取指定目标的世界位置信息;所述指定目标的相对位置信息为指定目标在 二维图像中的位置信息，所述指定目标的世界位置信息为指定目标在世界坐标系中的位置 信息。6. 根据权利要求4或5所述的运动控制装置，其特征在于： 可移动载体解算模块，用于获取环境中每个障碍物在世界坐标系中的位置信息；根据 可移动载体的姿态信息和里程信息以及环境中每个障碍物在世界坐标系中的位置信息，获 取可移动载体周围的障碍物在世界坐标系中的位置信息;根据可移动载体周围的障碍物在 世界坐标系中的位置信息，获取可移动载体下一步的运动状态;根据可移动载体的姿态信 息和里程信息，获取可移动载体的世界位置信息，所述可移动载体的世界位置信息为可移 动载体在世界坐标系中的位置信息;根据指定目标的世界位置信息和可移动载体的世界位 置信息，获取可移动载体的运动速度;根据所述运动状态和所述运动速度，获取该运动状态 下，可移动载体要达到该运动速度所需要的动力信息;根据可移动载体的速度信息，对动力 信息进行调整;根据调整后的动力信息，控制可移动载体的运动，获取可移动载体的里程信 息并保存。7. -种运动控制系统，其特征在于，包括： 数据采集装置，包括RGBD摄像头、激光雷达、超声波发射器、惯性测量传感器;RGBD摄像 头获取指定目标的颜色信息和深度信息并上传至数据分析平台；激光雷达和超声波发射器 进行障碍信息检测，获取环境中每个障碍物在世界坐标系中的位置信息并上传至数据分析 平台；惯性测量传感器获取可移动载体的姿态信息并上传至数据分析平台； 数据分析平台，包括运动解算模块和上层PID控制器;运动解算模块确定指定目标;通 过可移动载体拍摄二维图像;根据指定目标的颜色信息和深度信息，识别出二维图像中的 指定目标，在二维图像中对指定目标进行跟踪，获取指定目标的相对位置信息;获取可移动 载体姿态信息和里程信息，根据可移动载体的姿态信息和里程信息以及指定目标的相对位 置信息，通过姿态变换矩阵获取指定目标的世界位置信息;所述指定目标的相对位置信息 为指定目标在二维图像中的位置信息，所述指定目标的世界位置信息为指定目标在世界坐 标系中的位置信息；获取环境中每个障碍物在世界坐标系中的位置信息;根据可移动载体 的姿态信息和里程信息以及环境中每个障碍物在世界坐标系中的位置信息，获取可移动载 体周围的障碍物在世界坐标系中的位置信息;根据可移动载体周围的障碍物在世界坐标系 中的位置信息，获取可移动载体下一步的运动状态;根据可移动载体的姿态信息和里程信 息，获取可移动载体的世界位置信息，所述可移动载体的世界位置信息为可移动载体在世 界坐标系中的位置信息；上层PID控制器根据指定目标的世界位置信息和可移动载体的世 界位置信息，获取可移动载体的运动速度； 底层控制平台，包括底层控制芯片、下层PID控制器、动力装置、运动模块、测速模块、里 程模块;底层控制芯片接收数据分析平台发送的运动状态和运动速度，获取该运动状态下， 可移动载体要达到该运动速度所需要的预定动力信息；下层PID控制器接收测速模块发送 的速度信息，通过自反馈机制，根据可移动载体的速度信息，对动力信息进行调整并将调整 后的动力信息传输到动力装置;动力装置根据调整后的动力信息，控制运动模块实现可移 动载体的运动，并将调整后的动力信息传输到运动模块、测速模块和里程模块;运动模块控 制可移动载体的移动;测速模块测算可移动载体的速度信息并发送到下层PID控制器;里程 模块测算可移动载体的里程信息并上传至数据分析平台。
【文档编号】G06T7/00GK106056633SQ201610403662
【公开日】2016年10月26日
【申请日】2016年6月7日
【发明人】李经纬, 陈震, 朱骋, 王皓月, 蒋申璐, 杨永全, 任海波, 杜新鹏
【申请人】速感科技（北京）有限公司