CN115256399A - 一种用于移动机械臂的低自由度目标搜寻及到达控制方法 - Google Patents

Abstract

本发明公开了一种用于移动机械臂的低自由度目标搜寻及到达策略，应用于包含小车、多个回转关节和摆动关节的移动机械臂，在移动机械臂的末端安装有图像采集设备，选取移动机械臂的两关节组合运动，以最大视野角度为步长，对目标空间进行遍历粗搜索，直到目标物出现在视野范围内。通过计算目标和机械臂的相对位置，确定两关节组合运动角度，使目标物位于图像采集设备的视场中央，实现目标物的精定位；进一步移动小车靠近目标物，首先小车转向目标物方向，回转关节反向旋转补偿小车的转动，然后小车向目标物运动，期间调整摆动关节，保证目标物不移出视野的中央区域，直到目标物与小车之间的距离达到设定阈值，目标到达任务完成。

Description

一种用于移动机械臂的低自由度目标搜寻及到达控制方法

技术领域

本发明属于智能控制的技术领域，具体涉及一种用于移动机械臂的低自由度目标搜寻及到达控制方法。

背景技术

机械臂是一种能模仿人的手和臂的某些动作，再按照设定程序实施抓取、搬运物件或操作工具的自动操作装置，可代替人的繁重劳动以实现生产的机械化和自动化，能在有害环境下操作以保护人身安全，而移动机械臂将机械臂与小车组合在一起，增加了整个系统的自由度，极大扩展了机械臂的工作空间，从而使机械臂可以对工作空间外的目标进行抓取。

视觉搜索一般存在两种情况：1.知道目标物大致方位，对其进行有针对性的路径搜索；2.完全不知道目标位置，对环境进行遍历搜索。学者们针对这两种情况展开了积极研究，如中国文献专利号：CN202010444256.7，公开/公告日：2020.08.25公开了一种四目仿生眼设备、装置及其搜寻目标的方法，旨在解决现有技术中仿生机器人视野范围窄、感知效率低、稳定性差的问题。该发明提供的四目仿生眼装置包括依次连接的仿生视觉模块、第一驱动模块、第二驱动模块以及控制箱,其中仿生视觉模块包括第一双目视觉模块和由两个可相向或向背运动的单双耦合视觉传感器构成的第二双目视觉模块,该发明在搜寻目标时通过控制第一驱动模块和第二驱动模块实现四目仿生视觉模块的俯仰探测，通过遍历搜索的方式，控制视觉模块对周围环境进行360°遍历，获取到目标位置后生成运动路径。该发明将两个视觉传感器相结合，扩大了视觉域的范围，提高了搜索效率，遍历搜索保证了目标搜索时的精度。该发明受限于相机视野范围和小车自由度约束，只能通过多视觉模块的方法获得全空间的视野信息，不适用于工程实践。

中国文献专利号CN202111599857.6，公开/公告日：2021.12.24，公开了一种基于水下搜寻机器人的目标搜寻与抵近方法，其特征在于，基于随机搜索的方式，通过声纳传感器和视觉传感器形成视觉与光学图像，利用卷积神经网络对声学与光学图像的识别处理,判断声学与光学图像是否出现目标对象，若出现则通过上位机计算目标物位置，控制机器人抵近目标；若未出现目标对象，则控制机器人继续随机搜索，直至发现目标。该发明通过双传感器扩大了搜索范围，但随机搜索对比遍历搜索在搜索效率方面不高，且控制机器人抵近目标过程中，并未实时反馈目标物信息，当受到环境干扰时，易发生目标图像丢失的问题。

中国文献专利号CN201210447643.1，公开/公告日：2015.08.12，公开了一种仿动物空间认知的多机器人联合目标搜寻方法,该发明步骤在于：(1)首先判断信息是否匹配成功；(2)建立各机器人的运动学模型及传感器模型；(3)完成摄像机内部参数的标定和匹配；(4)利用自组织吸引子网络算法模型来进行机器人的定位；(5)建立环境地图和多机器人的地图融合；(6)导航和目标搜寻；(7)判断任务是否完成；若任务完成,则搜寻任务结束；否则,则继续进入步骤(2),重新进行地图构建和目标搜寻过程。该发明基于单个机器人随机搜索的方式，通过多机器人扩大搜索范围，相较于单个机器人其搜索效率得到了极大的提升，但对比通过机械臂末端的单个视觉传感器进行遍历搜索的方法，该发明成本更高，效率更低。

发明内容

本发明提供了一种用于移动机械臂的低自由度目标搜寻及到达控制方法，研究了移动机械臂任意两个关节的工作空间与其末端的图像采集设备的视场组合视野空间的形状大小，给出了粗搜索策略，获取目标位置信息后，通过调整移动机械臂的关节运动，让目标物处于图像采集设备的视场中央，最后控制移动机械臂向目标物运动，并保持在目标到达过程中目标物始终位于图像采集设备的视场中央，以获得更好的到达精度。研究证明，该方法使用少量自由度即仅控制两个关节的运动，就可以实现目标的搜索与到达，控制器设计简单，定位效率高，适合工程应用。

本发明可通过以下技术方案实现：

一种用于移动机械臂的低自由度目标搜寻及到达控制方法，应用于包含小车、多个回转关节和摆动关节的移动机械臂，在所述移动机械臂的末端设置有图像采集设备；

选取移动机械臂中不可视区域最小的两关节视觉空间为搜索空间，对目标物进行粗搜索，搜索到目标物后，通过调整两关节视觉空间中两个关节的运动，使目标物位于图像采集设备的视场中央，实现目标物的精定位；

然后判断目标物是否超出移动机械臂的工作空间，若没有超出，则目标搜寻完成；

若超出工作空间，则控制小车朝向目标物移动，在此过程中，通过调整两关节视觉空间中两个关节的运动，使目标物始终处于图像采集设备的视场中央，直到目标物与机械臂工作空间球心之间的距离小于机械臂工作空间半径，目标到达任务完成。

进一步，粗搜索包括以下步骤：

步骤Ⅰ、逐一计算移动机械臂中任意两个关节的工作空间，将其与图像采集设备的视场叠加，共同构成移动机械臂的多个两关节视觉空间，从中选取不可视区域最小的两关节视觉空间为搜索空间，对应的两个关节分别记为搜索摆动关节、搜索回转关节；

步骤Ⅱ、计算在图像采集设备的最小成像距离时，搜索摆动关节、搜索回转关节对应的最大步长；

步骤Ⅲ、按照搜索回转关节每运动一个最大步长，搜索摆动关节以最大步长遍历一周的方式对目标物进行搜索，直至发现目标物，完成粗搜索。

进一步，利用如下方程式计算搜索摆动关节、搜索回转关节的调整角度△φ、△θ，从而使目标物位于图像采集设备的视场中央，实现目标物的精定位，

其中，φ₀、θ₀分别表示粗搜索后，搜索回转关节、搜索摆动关节对应的转动角度，P＝(X,Y,Z)＝T_c·^cP表示粗搜索后，目标物在搜索摆动关节所处坐标系下的位置信息，^cP表示粗搜索后，目标物在图像采集设备所处坐标系下的位置信息，T_c表示图像采集设备在搜索摆动关节所处坐标系下的位姿信息，a₅表示图像采集设备的安装位置相对机械臂末端的Y轴向偏移量。

进一步，在进行目标到达时，控制小车和搜索回转关节反向运动，使小车朝向目标物，再控制小车匀速靠近目标物运动，直至两者之间的距离达到设定阈值，在此期间，控制搜索摆动关节运动以补偿目标物在图像采集设备视场的偏移，确保目标物始终处于图像采集设备的视场中央。

进一步，记初始状态下小车的航向角为α_car，期望航向角为α₀，设计比例控制器如下方程式所示，以控制搜索回转关节、小车的运动，使小车朝向目标物，

其中，k₃为比例系数，

为搜索回转关节角速度，

为小车角速度，

X_w、Y_w为目标物在世界坐标系下的位置信息；

设计速度控制器如下方程式所示，控制搜索摆动关节对目标物的视野偏移进行补偿，使目标物始终处于图像采集设备的视场中央，

其中，k₄为比例系数；

为搜索摆动关节的运动补偿量，x_c为目标物在图像采集设备所处坐标系运动前后的位置偏移信息，z_c为目标物在图像采集设备当前所处坐标系的距离信息的z轴分量。

本发明有益的技术效果在于：

与现有技术相比，本发明设计基于陌生环境下的移动机械臂低自由度目标搜寻与到达策略，针对目标搜索任务，将高自由度机械臂简化为两关节机械臂，其他关节不做控制，减少控制量；为了提高位置空间的搜索效率，提出两步法搜索策略，首先小车固定，根据两关节机械臂的工作空间和图像采集设备的视场空间大小，确定搜索步长，提出了粗搜索策略；然后根据目标物位置与关节运动之间的关系，精确计算目标物与相机的位姿关系，运动机械臂使目标物位于图像采集设备的视场空间中心，完成精定位；最后采用小车参与的到达策略，协调控制小车和两关节机械臂运动到目标物附近，解决了远距离目标的到达问题。

另外，本发明控制器设计简单，定位效率高，能耗低，适合工程应用。

附图说明

图1为本发明整体任务示意图；

图2为本发明的移动机械臂视觉搜寻流程图；

图3为本发明的移动机械臂结构示意图；

图4为本发明的RGBD相机视场示意图；

图5为本发明摆动关节视野空间示意图；

图6为本发明两关节视野空间示意图；

图7为本发明的不同关节组合视觉空间对比示意图；

图8为本发明的粗搜索步长确定示意图；

图9为本发明的精定位机械臂角度确定示意图；

图10为本发明的达到策略实现示意图；

图11为本发明的粗搜索目标点图像坐标和机械臂关节角度变化图；

图12为本发明的精定位图像轨迹和世界坐标系下相机轨迹变化图；

图13为本发明的精定位搜索回转关节、搜索摆动关节角度变化和目标点在相机坐标系下坐标变化图；

图14为本发明的车臂姿态调节目标视野、关节5、小车航向角变化图；

图15为本发明的目标到达时目标点图像坐标及关节6角度变化图；

图16为本发明的目标到达时世界坐标系下小车位置变化和相机坐标系目标点坐标变化；

图17为本发明的视觉搜寻过程世界坐标系下相机位姿变化示意图；

图18为本发明的视觉搜寻过程关节5，6的角度变化示意图；

具体实施方式

下面结合附图及较佳实施例详细说明本发明的具体实施方式。

现有视觉搜寻方法都是基于辅助定位确定目标的大致方位，再对其进行有针对性的路径搜索，对于陌生环境下如何快速、高效、低功耗的完成视觉搜寻研究不多。因此，如图1所示，本发明提出了一种用于移动机械臂的低自由度目标搜寻及到达控制方法，首先选取移动机械臂中不可视区域最小的两关节视觉空间为搜索空间，对目标物进行粗搜索，搜索到目标物后，通过调整两关节视觉空间中两个关节的运动，使目标物位于图像采集设备的视场中央，实现目标物的精定位；然后判断目标物是否超出移动机械臂的工作空间，若没有超出，则目标搜寻完成；若超出工作空间，则控制小车朝向目标物移动，在此过程中，通过调整两关节视觉空间中两个关节的运动，使目标物始终处于图像采集设备的视场中央，直到目标物与小车之间的距离达到设定阈值，目标到达完成。流程实现如图2所示，具体如下：

根据视野空间不可视区域半径和相机视野大小确定粗搜索步长，接着，在粗搜索基础上，根据目标位置与关节运动的关系，对目标进行精确定位，然后，判断目标是否在机械臂工作空间，若在，则视觉搜寻完成，若不在，则控制小车行驶到目标附近，最后，调整车臂姿态，控制小车匀速驶向目标，并在运动过程中，保持目标物视野居中。

一、粗搜索

以KINOVA机械臂为例，该移动机械臂为七自由度机械臂，包括七个交替排列的回转关节和摆动关节，从下到上依次记为关节1至关节7，在其末端设置图像采集设备如RGBD相机，底部设置有小车，具体结构如图3所示。

该移动机械臂的参数见表格1，d_a是升降台到地面的高度，d₁-d₈分别是机械臂各连杆长度，a₁是关节1和关节2之间的偏移量，a₂是关节2和关节3之间的偏移量，a₃是关节3和关节4之间的偏移量，a₄是关节4和关节5之间的偏移量，a₅是末端和相机的偏移量，d是升降台活动范围。

表1

步骤Ⅰ、建立各个部件的参考坐标系如下：小车坐标系{car}原点O_car位于移动平台底部，七自由度机械臂基座底部中点为世界坐标系{w}的原点O_w；机械臂的基坐标系{b}的原点O_b位于机械臂底座的底端中心，各连杆的坐标系{i}的原点O_i位于该连杆的前一关节上，坐标系{i}的Z_i轴与该连杆轴线共线；末端坐标系{e}的原点O_e位于末端连线的中心处，RGBD相机坐标系{c}的原点O_c位于末端坐标系Y_e方向0.0564m处。

坐标系{car}与坐标系{b}之间的变换矩阵为：

坐标系{b}与坐标系{1}之间的变换矩阵为：

机械臂各连杆坐标系之间变换矩阵

和连杆{i}的奇偶有关，当连杆{i}为偶数时有：

当连杆{i}为奇数时有：

坐标系{7}与坐标系{c}之间的变换矩阵为：

将上面得到的变换矩阵的乘积，则可以得到移动机械臂相机的变换矩阵：

步骤Ⅱ、根据相机视场和两关节机械臂工作空间，确定两关节机械臂视觉空间大小。KINOVA机械臂末端相机为RGBD相机，其中RGB相机型号为OV6450，深度相机型号为Intelrealsense D410，参数如表2所示。

表2

取RGBD相机综合视场角α＝53°、β＝31°，像平面显示区域大小为1.792*1.008mm，相机视场大小由视场角和Z_c大小决定。由相机参数可知，选定RGBD相机最小成像距离Z_cmin＝0.2m，相机视场形状为四棱台，如图4所示。当目标物距离相机光心最小距离Z_cmin＝0.2m时，相机视场大小为：

因为相机在2米处的检测精度较高，故本发明选择Z_c＝2m，此时相机视场大小为：

其中，W、H分别为相机视场的宽度和高度。

KINOVA机械臂由4个回转关节和3个摆动关节构成，关节1、3、5、7为回转关节，关节2，4，6为摆动关节，机械臂末端所能达到的点的集合称为工作空间，工作空间形状近似为球体。由于相机与末端位置相对固定，故相机工作空间与末端工作空间形状相似，大小不同，在相机工作空间的基础上，叠加相机视场，即可获得机械臂视觉空间，形状近似为实心球体，在该视觉空间内均为相机可视区域。

通过分析关节运动可知，当仅转动一个回转关节，移动机械臂视觉空间形状为圆台；由于摆动关节存在关节极限，当仅控制一个摆动关节运动，机械臂视觉空间形状为中心凹陷的不完整圆柱体，如图5所示。通过对不同关节进行组合，并研究其视觉空间形状和大小，可得到结论：任意摆动关节和关节编号小于它的任意回转关节组成的关节组合的视觉空间形状为不完整空心球体，如图6所示。KINOVA机械臂存在3个摆动关节即关节2、关节4、关节6，对应得到的视觉空间大小对比如图7所示。由于不可视区域过大会造成近距离物体无法搜寻等问题，因此，本发明选择视觉空间不可视区域最小的关节组合进行粗搜索，其余关节视为连杆，搜索时保持不动。从表格3可知，关节5、6组合的不可视区域最小，世界坐标系下关节5、6组合的搜索范围为(±0.1738,±2.1738m)，最低搜索高度1.0199m，最大搜索高度3.0199m。

表3

步骤Ⅲ、对目标物进行粗搜索。

为了保证目标物在相机最小成像距离时搜索成功，需计算在相机最小成像距离时，关节5、6的最大步长。关节5运动步长和不可视区域半径r₃的关系如图8所示，所以关节5、6的最大步长α₀、β₀如下所示：

粗搜索时，按照回转关节每运动一个步长，摆动关节在关节极限内完成一次遍历的方式进行。设定关节5，6搜索步长分别为29°、15°，关节5为回转关节，需运动13次旋转一周，关节6是摆动关节，运动范围是±120°，需搜索16次完成遍历，回转关节5运动一个步长，摆动关节6按步长遍历搜索，若未发现目标物，则关节5在此基础上再运动一个步长，重复上述过程，直至当视觉空间内出现目标物，停止搜索，粗搜索完成。目标搜索过程如图9所示。

二、精定位

在图9中，记关节5、6初始位置的坐标系为{5°}、{6°}，粗搜索后，关节5、6分别转动φ₀、θ₀，此时关节5、6坐标系变为{5}、{6}，输出粗搜索后关节5、6的转动角度和目标物在粗搜索结束时相机坐标系下的位置坐标^cP，然后根据当前关节角度计算

得到目标物在{6}下的位置坐标：

为了保证目标到达过程中，目标视野不丢失，需在粗搜索结果的基础上，对目标物位置进一步精定位，使目标物处在相机的视场中央区域。记粗搜索后获得的目标物在{6}下坐标⁶P＝(X,Y,Z)，控制关节5、6在φ₀、θ₀基础上分别转动△φ、△θ，关节5坐标系也由{5}旋转得到新坐标系O₅X'₅Y'₅Z'₅，同时关节6坐标系跟随{5}旋转变为O₆X'₆Y'₆Z'₆，关节6相对θ₀绕Z'₆轴转动△θ，此时的目标物位于相机视场中央，目标物在新坐标系O₆X'₆Y'₆Z'₆下的坐标记为⁶P'＝(X',Y',Z')，将⁶P'向X'₆O₆Y'₆面上的投影，投影点坐标记为P'＝(X',Y',0)，则P'和φ₀、θ₀关系可表示为：

计算得到不同情况下：

其中，φ₀、θ₀分别表示粗搜索后，搜索回转关节、搜索摆动关节对应的转动角度，P＝(X,Y,Z)＝T_c·^cP表示粗搜索后，目标物在搜索摆动关节所处坐标系下的位置信息，^cP表示粗搜索后，目标物在图像采集设备所处坐标系下的位置信息，T_c表示图像采集设备在搜索摆动关节所处坐标系下的位姿信息，a₅表示图像采集设备的安装位置相对机械臂末端的Y轴向偏移量。

根据解出的△φ、△θ，控制关节5、6在粗搜索的基础上运动，使目标物位于相机视场中央，完成精定位。

三、目标物到达

完成目标物精定位后，目标物位于相机视野中央，若此时目标物在整个移动机械臂的工作空间内，则可控制机械臂抓取目标；若目标物在整个移动机械臂工作空间外，则需要控制小车运动到目标物附近，再控制机械臂抓取目标。在向目标物运动过程中，需要保证目标物始终位于相机视野中心区域，防止出现目标丢失的情况。

在视觉到达开始前，先调整小车的姿态，记初始状态下小车的航向角为α_car，根据目标物在世界坐标系下位置计算小车期望航向角α₀：

根据α₀和α_car，设计小车角速度控制器

上式中，k₃为比例系数，X_w、Y_w为目标物在世界坐标系下的位置信息。

由于小车和关节5旋转时的正方向相反，为了保证相机在世界坐标系下位姿不变，关节5和小车应往相反的方向转动。所以，设计关节5角速度控制器

控制小车和关节5反向转动，当小车朝向目标后，再控制小车匀速向目标物运动。运动过程中，目标物会逐渐偏离相机视场的中央区域，如图10所示。以像平面中心为基准，记像平面中心1/4面积区域为视场中心区域，当目标物视野超出中央区域时，需控制关节6运动对误差进行补偿，运动过程中，视野偏移引起的补偿量θ_t为：

上式中，x_c为目标物在图像采集设备所处坐标系运动前后的位置偏移信息，z_c为目标物在图像采集设备当前所处坐标系的距离信息的z轴分量。设计视觉到达过程中关节6速度控制器：

上式中，k₄为比例系数，

为关节6的角速度。到达过程中，保持图像坐标x，y不超出像平面中心区域，当相机与目标物之间的距离z_c小于阈值时，目标到达完成。

为了验证本发明的控制方法的可行性，我们进行了如下仿真试验：

在世界坐标系下给定空间点^wP(-1，2，1.3)m，初始时刻，小车原点在世界坐标系下坐标为(0，0，-0.843)m,初始航向角为0°,假设到达过程中小车为匀速运动，给定移动机械臂初始关节角度Q＝[0；0；0；0；0；0；0；-120°；0],控制周期为0.5s，记像平面中心1/4区域为像平面中心区域，视觉到达过程中，使目标点图像坐标满足

且当目标点与机械臂工作空间球心距离小于机械臂工作空间半径时，移动机械臂停止运动。给出了粗搜索、精定位、目标到达过程中目标点图像坐标变化情况、关节角度变化情况、世界坐标系下小车和相机位姿变化情况如图11、12、13、14、15、16所示。根据提出的两步法搜索策略，先后对目标物进行粗搜索与精定位。图11给出了粗搜索过程中目标物在相机视野内的变化情况和搜索回转关节、搜索摆动关节角度变化情况，可以发现，目标物视野一开始在相机的视野外，通过对周围进行遍历，获取到目标视野，此时目标物视野在相机视场边缘。精确计算目标物与相机的位姿关系，图12给出了精定位过程中目标物在相机视野内的变化情况，可以发现，目标物视野在相机视场中的位置，从边缘移动至中央，实现了对目标物的精确定位。图13给出了精定位过程中搜索回转关节、搜索摆动关节角度变化情况和目标物在相机坐标系下的坐标变化情况，可以发现，搜索回转关节和搜索摆动关节在粗搜索得到的关节角度的基础上，进行微量的角度调整，即可将目标物视野移动到相机视场中央，此时，目标物在相机坐标系下的x_c、y_c趋近于0。图14给出了目标物视野在车臂姿态调节过程中的视野变化，运动过程中，目标物视野仍位于相机视野中心区域,搜索回转关节角度变化和小车航向角变化大小相等，方向相反。图15给出了目标到达过程中目标物视野变化和搜索摆动关节角度变化，小车向目标物运动过程中，目标物视野逐渐向边缘偏移，控制搜索摆动关节对视野偏移进行补偿，控制目标物视野始终位于像平面中心区域。图16给出了世界坐标系下小车位置变化情况和目标物在相机坐标系下的坐标变化情况，随着小车逐渐靠近目标，可以发现z_c也逐渐小于设定的阈值。图17给出了世界坐标系下相机的位姿变化情况，从初始时刻相机距离目标很远到最终时刻相机运动到目标物附近，证明了视觉搜寻过程的有效性。图18给出了视觉搜寻过程中搜索回转关节和搜索摆动关节的角度变化情况。仿真结果证明，本发明提出的策略可以在低自由度的情况下，较好的完成目标搜寻与到达，定位效率高，适合工程应用。

以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (5)

1.一种用于移动机械臂的低自由度目标搜寻及到达控制方法，其特征在于：应用于包含小车、多个回转关节和摆动关节的移动机械臂，在所述移动机械臂的末端设置有图像采集设备，

选取移动机械臂中不可视区域最小的两关节视觉空间为搜索空间，对目标物进行粗搜索，搜索到目标物后，通过调整两关节视觉空间中两个关节的运动，使目标物位于图像采集设备的视场中央，实现目标物的精定位；

然后判断目标物是否超出移动机械臂的工作空间，若没有超出，则目标搜寻完成；

若超出工作空间，则控制小车朝向目标物移动，在此过程中，通过调整两关节视觉空间中两个关节的运动，使目标物始终处于图像采集设备的视场中央，直到目标物与小车之间的距离达到设定阈值，目标到达完成。

2.根据权利要求1所述的用于移动机械臂的低自由度目标搜寻及到达控制方法，其特征在于粗搜索包括以下步骤：

步骤Ⅰ、逐一计算移动机械臂中任意两个关节的工作空间，将其与图像采集设备的视场叠加，共同构成移动机械臂的多个两关节视觉空间，从中选取不可视区域最小的两关节视觉空间为搜索空间，对应的两个关节分别记为搜索摆动关节、搜索回转关节；

步骤Ⅱ、计算在图像采集设备的最小成像距离时，搜索摆动关节、搜索回转关节对应的最大步长；

步骤Ⅲ、按照搜索回转关节每运动一个最大步长，搜索摆动关节以最大步长遍历一周的方式对目标物进行搜索，直至发现目标物，完成粗搜索。

3.根据权利要求2所述的用于移动机械臂的低自由度目标搜寻及到达控制方法，其特征在于：利用如下方程式计算搜索摆动关节、搜索回转关节的调整角度△φ、△θ，从而使目标物位于图像采集设备的视场中央，实现目标物的精定位，

其中，φ₀、θ₀分别表示粗搜索后，搜索回转关节、搜索摆动关节对应的转动角度，P＝(X,Y,Z)＝T_c·^cP表示粗搜索后，目标物在搜索摆动关节所处坐标系下的位置信息，^cP表示粗搜索后，目标物在图像采集设备所处坐标系下的位置信息，T_c表示图像采集设备坐标系与搜索摆动关节坐标系之间的坐标变换矩阵，a₅表示图像采集设备的安装位置相对机械臂末端的Y轴向偏移量。

4.根据权利要求2所述的用于移动机械臂的低自由度目标搜寻及到达控制方法，其特征在于：在进行目标到达时，控制小车和搜索回转关节反向运动，使小车朝向目标物，再控制小车匀速靠近目标物运动，直至两者之间的距离达到设定阈值，在此期间，控制搜索摆动关节运动以补偿目标物在图像采集设备视场的偏移，确保目标物始终处于图像采集设备的视场中央。

5.根据权利要求4所述的用于移动机械臂的低自由度目标搜寻及到达控制方法，其特征在于：记初始状态下小车的航向角为α_car，期望航向角为α₀，设计比例控制器如下方程式所示，以控制搜索回转关节、小车的运动，使小车朝向目标物，

其中，k₃为比例系数，

为搜索回转关节角速度，

为小车角速度，

X_w、Y_w为目标物在世界坐标系下的位置信息；

设计速度控制器如下方程式所示，控制搜索摆动关节对目标物的视野偏移进行补偿，使目标物始终处于图像采集设备的视场中央，

其中，k₄为比例系数；

为搜索摆动关节的运动补偿量，x_c为目标物在图像采集设备所处坐标系运动前后的位置偏移信息，z_c为目标物在图像采集设备当前所处坐标系的距离信息的z轴分量。

Images