CN114627166A - 基于点云配准icp算法的机器人云台伺服控制方法 - Google Patents

Abstract

本发明属于拍摄云台控制技术领域，公开了一种基于点云配准ICP算法的机器人云台伺服控制方法，在巡检点标定时调整好云台的姿态，采集此时激光数据作为模板点云，并记录云台水平和垂直方向角度信息；在巡检过程中，当机器人再次到达停靠点时，首先采集一帧激光数据作为实时点云，通过迭代点云配准ICP算法获得实时点云与模板点云之间的变换关系，从而确定此时机器人与标定时的位姿差别；然后根据巡检点的信息计算拍摄目标与机器人的距离；最后根据位姿变化和距离进行云台角度修正。本发明采用点云配准技术，能够快速准确地寻找目标，且不受光照强度影响，有效提高了采集率和巡检效率。

Description

基于点云配准ICP算法的机器人云台伺服控制方法

技术领域

本发明属于移动机器人拍摄云台控制技术领域，特别涉及基于点云配准ICP算法的机器人云台伺服控制方法。

背景技术

在电力巡检行业中，机器人通过搭载具备可见光相机和红外热像仪的云台采集系统对电力设备进行自动化巡视，在传统云台伺服系统中，较多采用基于图像对位的方式进行云台姿态调整，通过分析比较标定时图像与巡检时图像差别，计算出相应的位置变化量，对云台姿态进行修正。但是基于图像对位的方式容易受到环境光照强度的影响，不同天气，不同时刻都会影响到图像对位的结果，从而影响云台的调整，当机器人到点的角度相差过大，拍摄的图片与标定图片的公共区域过少，也会导致图像识别计算失败，此外基于图像对位的云台伺服调整耗时较长，影响机器人巡检效率。

发明内容

本发明的目的是针对现有技术的不足，提供一种基于点云配准ICP算法的机器人云台伺服控制方法，能够快速准确地寻找目标，且不受光照强度影响，有效提高了采集率和巡检效率。

为实现上述目的，本发明采用了下述技术方案：一种基于点云配准ICP算法的机器 人云台伺服控制方法，机器人包含多线激光系统，云台系统及机器人底盘系统，构建云台伺 服控制系统数学模型，o _r -x _r y _r z _r 表示机器人坐标系，o _c -x _c y _c z _c 表示云台坐标系，o _l -x _l y _l z _l 表 示激光坐标系；

和

和分别表示机器人坐标系到激光坐标系和云台坐标系的变换；

步骤S1、在巡检点标定时调整好云台的姿态，采集此时激光数据作为模板点云，并记录云台水平和垂直方向角度信息；

步骤S2、在巡检过程中，当机器人再次到达停靠点时，首先采集一帧激光数据作为实时点云，通过迭代点云配准ICP算法获得实时点云与模板点云之间的变换关系，从而确定此时机器人与标定时的位姿差别；

步骤S3、然后根据巡检点的信息计算拍摄目标与机器人的距离；

步骤S4、最后根据位姿变化和距离进行云台角度修正。

进一步优选，所述步骤S2中，以实时点云为源点云，以模板点云为目标点云，对于待匹配源点云M和目标点云Q，它们之间存在一组刚体变换(R,t)使得误差函数E(R,t)最小；

其中，点

为源点云M和目标点云Q中对应点的个数，R为3×3 的旋转矩阵，t为3×1的平移矩阵；点云配准ICP算法就是通过不断迭代更新旋转矩阵R与平 移矩阵t，使得误差函数E(R,t)满足迭代终止条件，具体步骤如下：

S21、在实时点云Q中依次取出各点，并在源点云M中找出距离目标点云Q中各点最近的点，构成源点云M与目标点云Q之间的点对；

S22、根据获得的点对，求解当前对应关系下的旋转矩阵R和平移矩阵t使得误差函数E(R,t)最小；

S23、根据获得最优旋转矩阵和平移矩阵，更新目标点云为Q_i；

S24、由源点云M和新得目标点云继续进行步骤S21和步骤S22,直到误差函数E(R,t)小于设定阈值，或者达到设定的迭代次数；

S25、通过不断的迭代优化旋转矩阵R和平移矩阵t,最终获得源点云与目标点云之间最优变换关系，由实时点云与模板点云之间的变换关系可以确定出机器人在标定和巡检时的位姿变化，从而确定云台坐标系的位姿变化。

进一步优选，所述步骤S4中，假设P表示拍摄目标，以o _c , x _c , y _c , z _c 表示机器人标 定时云台的位姿，o _c 表示机器人标定时云台原点, x _c 表示机器人标定时云台X轴坐标, y _c 表 示机器人标定时云台Y轴坐标, z _c 表示机器人标定时云台Z轴坐标，以

表示 机器人巡检时云台的位姿，

表示机器人巡检时云台原点,

表示机器人巡检时云台X轴 坐标,

表示机器人巡检时云台Y轴坐标,

表示机器人巡检时云台Z轴坐标，α表示标定 时云台的水平角度，

表示巡检时云台的水平角度，β表示标定时云台垂直方向的角度，

表示巡检时云台垂直方向的角度；机器人位姿变化由点云配准获得，∆x _r 表示机器人X轴方 向变化，∆y _r 表示机器人Y轴方向变化，∆roll表示机器人摇滚角变化，∆pitch表示机器人 俯仰角变化，∆yaw表示机器人偏航角变化；将姿态误差和位置误差分别弥补给云台。

进一步优选，姿态误差修正：∆yaw仅影响云台水平方向角度，∆roll和∆pitch影 响云台垂直方向角度；仅考虑∆yaw对云台对位的影响，则由∆yaw产生的水平角度差：

；仅考虑∆roll和∆pitch对云台对位的影响；p表示目标点到机器人 标定时云台坐标系xoy平面的投影，

表示目标点到机器人巡检时云台坐标系xoy平面的 投影；由变换关系可知云台坐标原点系原点o _c 到拍摄目标P的向量o _c P和云台坐标原点系原 点o _c 到投影

的向量

，再根据余弦定理可知

：

则由∆roll和∆pitch产生的云台垂直方向误差为：

。

进一步优选，位置误差修正：完成姿态误差修正后，只考虑云台坐标系xoy平面上 的位置偏差，∆x _c 为云台X轴位置偏差，∆y _c 为为云台Y轴位置偏差，因为云台在机器人上的 位置是固定的，所以∆x _c =∆x _r ，∆y _c =∆y _r ；

为相机光轴在xoy平面的投影，o _c E为∆y _c 的 向量，o _c F为向量o _c E在

方向上的分量，o _c D为向量o _c E在垂直

方向的方向上的分量，o _c B为∆x _c 的向量，o _c A为在

方向上的分量，o _c C为向量o _c B在垂直

方向的方向上的分 量，则由云台位置偏差∆x _c ，∆y _c 产生的在

方向和o _c D方向上的分量可分别表示为X_OFFSET 和Y_OFFSET：

则由于云台位置偏移误差产生的云台水平补偿角度∆α₂和由于云台位置偏移误差产生的云台垂直补偿角度∆β₂按下式计算：

式中， d表示标定时云台坐标系原点与拍摄目标P的距离。

进一步优选，由于机器人位姿偏差导致的云台水平方向补偿角∆α和垂直方向补偿角∆β按下式计算：

。

本发明提出了一种基于点云配准ICP算法的移动机器人云台伺服控制方法，将多线激光与云台动作联合在一起，利用激光数据的稳定性，有效规避了环境光照的影响。经实验和理论分析，该技术具有较高的稳定性和准确性，相比于图像识别的对位方式具有一定的优势。该技术采用点云配准，具备云台对位速度快，对位精度高，不受环境光照变化影响，巡检点模型储存占用系统资源较少等优点，极大限度的提升了移动机器人的采集率和巡检效率，进而保障了电力巡检机器人的可靠性。

附图说明

图1为机器人结构示意图。

图2 为云台伺服控制系统坐标系示意图。

图3为云台对位偏差示意图。

图4为姿态误差修正模型A示意图。

图5为姿态误差修正模型B示意图。

图6为位置误差模型A示意图。

图7为位置误差模型B示意图。

图8为由于云台位置偏移误差产生的云台水平补偿示意图。

图9为由于云台位置偏移误差产生的云台垂直补偿角度示意图。

图中100-多线激光系统、200-云台系统、300-机器人底盘系统。

具体实施方式

为了使本领域的技术人员更好的理解本申请方案，下面对本申请实施例中的技术方案进行清晰，完整，详细的描述。

一种基于点云配准ICP算法的机器人云台伺服控制方法，机器人包含多线激光系 统，云台系统及机器人底盘系统，构建云台伺服控制系统数学模型，如图2所示，o _r -x _r y _r z _r 表 示机器人坐标系, o _r 、x _r 、y _r 、z _r 分别表示机器人坐标系原点、X轴、Y轴、Z轴，o _c -x _c y _c z _c 表示云 台坐标系，o _c 、x _c 、y _c 、z _c 分别表示云台坐标系原点、X轴、Y轴、Z轴, o _l -x _l y _l z _l 表示激光坐标系，o _l 、x _l 、y _l 、z _l 分别表示激光坐标系原点、X轴、Y轴、Z轴；

和

和分别表示机器人坐标系到 激光坐标系和云台坐标系的变换。

步骤S1、在巡检点标定时调整好云台的姿态，采集此时激光数据作为模板点云，并记录云台水平和垂直方向角度信息；

步骤S2、在巡检过程中，当机器人再次到达停靠点时，首先采集一帧激光数据作为实时点云，通过迭代点云配准ICP算法获得实时点云与模板点云之间的变换关系，从而确定此时机器人与标定时的位姿差别；

以实时点云为源点云，以模板点云为目标点云，对于待匹配源点云M和目标点云Q，它们之间存在一组刚体变换(R,t)使得误差函数E(R,t)最小。

其中，点

为源点云M和目标点云Q中对应点的个数，R为3×3 的旋转矩阵，t为3×1的平移矩阵。

点云配准ICP算法就是通过不断迭代更新旋转矩阵R与平移矩阵t，使得误差函数E(R,t)满足迭代终止条件，具体步骤如下：

S21、在实时点云Q中依次取出各点，并在源点云M中找出距离目标点云Q中各点最近的点，构成源点云M与目标点云Q之间的点对；

S22、根据获得的点对，求解当前对应关系下的旋转矩阵R和平移矩阵t使得误差函数E(R,t)最小；

S23、根据获得最优旋转矩阵和平移矩阵，更新目标点云为Q_i；

S24、由源点云M和新得目标点云继续进行步骤S21和步骤S22,直到误差函数E(R,t)小于设定阈值，或者达到设定的迭代次数。

S25、通过不断的迭代优化旋转矩阵R和平移矩阵t,最终获得源点云与目标点云之间最优变换关系，由实时点云与模板点云之间的变换关系可以确定出机器人在标定和巡检时的位姿变化，从而确定云台坐标系的位姿变化。

步骤S3、然后根据巡检点的信息计算拍摄目标与机器人的距离；

步骤S4、最后根据位姿变化和距离进行云台角度修正。

S41、云台对位误差分析：在不计云台执行误差的情况下，产生云台对位误差的主 要是由于机器人的位置和姿态改变所致。如图3所示，P表示拍摄目标，以o _c , x _c , y _c , z _c 表 示机器人标定时云台的位姿，o _c 表示机器人标定时云台原点, x _c 表示机器人标定时云台X轴 坐标, y _c 表示机器人标定时云台Y轴坐标, z _c 表示机器人标定时云台Z轴坐标，以

表示机器人巡检时云台的位姿，

表示机器人巡检时云台原点,

表示 机器人巡检时云台X轴坐标,

表示机器人巡检时云台Y轴坐标,

表示机器人巡检时云 台Z轴坐标，α表示标定时云台的水平角度，

表示巡检时云台的水平角度，β表示标定时云 台垂直方向的角度，

表示巡检时云台垂直方向的角度。

S42、误差建模修正：机器人位姿变化（∆x _r ，∆y _r ，∆roll，∆pitch，∆yaw）由点云配准获得，∆x _r 表示机器人X轴方向变化，∆y _r 表示机器人Y轴方向变化，∆roll表示机器人摇滚角变化，∆pitch表示机器人俯仰角变化，∆yaw表示机器人偏航角变化。为了提高配准精度对点云依次进行直通滤波、体素滤波和离群点剔除等操作。云台误差主要机器人位姿变化导致，因此需要将姿态误差和位置误差分别弥补给云台。

S421、姿态误差修正：由云台和机器人之间的位置可知机器人位姿变化（∆x _r ，∆y _r ，∆roll，∆pitch，∆yaw），∆yaw仅影响云台水平方向角度，∆roll和∆pitch影响云台 垂直方向角度。如图4所示，仅考虑∆yaw对云台对位的影响，则由∆yaw产生的水平角度差：

。

如图5所示，仅考虑∆roll和∆pitch对云台对位的影响。p表示目标点到机器人标 定时云台坐标系xoy平面的投影，

表示目标点到机器人巡检时云台坐标系xoy平面的投 影；由变换关系可知云台坐标原点系原点o _c 到拍摄目标P的向量o _c P和云台坐标原点系原点o _c 到投影

的向量

，再根据余弦定理可知

：

则由∆roll和∆pitch产生的云台垂直方向误差为：

。

S422、位置误差修正：完成姿态误差修正后，只考虑云台坐标系xoy平面上的位置 偏差，如图6所示，∆x _c 为云台X轴位置偏差，∆y _c 为为云台Y轴位置偏差，因为云台在机器人 上的位置是固定的，所以∆x _c =∆x _r ，∆y _c =∆y _r ；如图7所示，

是目标点到机器人巡检时云 台坐标系xoy平面的投影,

为相机光轴在xoy平面的投影，o _c E为∆y _c 的向量，o _c F为向量o _c E在

方向上的分量，o _c D为向量o _c E在垂直

方向的方向上的分量，o _c B为∆x _c 的向 量，o _c A为在

方向上的分量，o _c C为向量o _c B在垂直

方向的方向上的分量，则由云台位 置偏差∆x _c ，∆y _c 产生的在

方向和o _c D方向上的分量可分别表示为X_OFFSET和Y_OFFSET：

则由于云台位置偏移误差产生的云台水平补偿角度

如图8所示，由于云台位 置偏移误差产生的云台垂直补偿角度

如图9所示， d表示标定时云台坐标系原点与拍 摄目标P的距离，d的值根据巡检点的放大倍数和聚焦值进行估算。

综上所述，由于机器人位姿偏差导致的云台水平方向补偿角∆α和垂直方向补偿角∆β可以表示为：

。

应用案例

在一天不同时刻(模拟环境光线的变化)，机器人对4个巡检点使用基于图像、点云配准和不适用任何对位方式进行巡检90次，巡检图片大小为1920×1080 pixles，实验机器人使用CPU为Intel i5 6300u，运行内存为16G，巡检点信息表1所示：

不同对位方式巡检结果的水平和垂直方向偏差均值如表2和表3所示。由表可知，使用对位方式对云台角度修正可明显提升巡检采集率，相比于传统图像识别的方式，点云配准的方法在对位精度上有一定提升。

不同对位方式巡检结果的水平和垂直方向偏差的标准差如表4和表5所示。巡检实验在一天不同时刻展开，光照环境随着时间变化，可以看出基于点云配准的方式在较为稳定，不受光照强度影响，甚至夜间也能正常工作，基于图像识别的方法，容易受到光照变化影响。

不同对位方式对机器人的资源消耗如表6所示，点云配准方式的巡检点模型占用存储资源仅仅是图像模型的0.5%，在拥有大规模巡检点的工作环境下，极大的节省了机器人对存储资源的消耗，同时对位时间上，基于点云配准的方式平均对位时间约为301ms，不足图像识别耗时的4.6%，能够极大的提升巡检速度。

以上对本发明的原理和实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法以及核心思想，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (6)

1.一种基于点云配准ICP算法的机器人云台伺服控制方法，机器人包含多线激光系统， 云台系统及机器人底盘系统，构建云台伺服控制系统数学模型， o _r -x _r y _r z _r 表示机器人坐标 系，o _c -x _c y _c z _c 表示云台坐标系，o _l -x _l y _l z _l 表示激光坐标系；

和

和分别表示机器人坐标 系到激光坐标系和云台坐标系的变换；

和

和分别表示机器人坐标系到激光坐标系和 云台坐标系的变换；其特征在于：

步骤S1、在巡检点标定时调整好云台的姿态，采集此时激光数据作为模板点云，并记录云台水平和垂直方向角度信息；

步骤S2、在巡检过程中，当机器人再次到达停靠点时，首先采集一帧激光数据作为实时点云，通过迭代点云配准ICP算法获得实时点云与模板点云之间的变换关系，从而确定此时机器人与标定时的位姿差别；

步骤S3、然后根据巡检点的信息计算拍摄目标与机器人的距离；

步骤S4、最后根据位姿变化和距离进行云台角度修正。

2.根据权利要求1所述的基于点云配准ICP算法的机器人云台伺服控制方法，其特征在于，所述步骤S2中，以实时点云为源点云，以模板点云为目标点云，对于待匹配源点云M和目标点云Q，它们之间存在一组刚体变换(R,t)使得误差函数E(R,t)最小；

其中，点

为源点云M和目标点云Q中对应点的个数，R为3×3的旋 转矩阵，t为3×1的平移矩阵；点云配准ICP算法就是通过不断迭代更新旋转矩阵R与平移矩 阵t，使得误差函数E(R,t)满足迭代终止条件，具体步骤如下：

S21、在实时点云Q中依次取出各点，并在源点云M中找出距离目标点云Q中各点最近的点，构成源点云M与目标点云Q之间的点对；

S22、根据获得的点对，求解当前对应关系下的旋转矩阵R和平移矩阵t使得误差函数E(R,t)最小；

S23、根据获得最优旋转矩阵和平移矩阵，更新目标点云为Q_i；

S24、由源点云M和新得目标点云继续进行步骤S21和步骤S22,直到误差函数E(R,t)小于设定阈值，或者达到设定的迭代次数；

S25、通过不断的迭代优化旋转矩阵R和平移矩阵t,最终获得源点云与目标点云之间最优变换关系，由实时点云与模板点云之间的变换关系可以确定出机器人在标定和巡检时的位姿变化，从而确定云台坐标系的位姿变化。

3.根据权利要求2所述的基于点云配准ICP算法的机器人云台伺服控制方法，其特征在 于，所述步骤S4中，假设P表示拍摄目标，以o _c , x _c , y _c , z _c 表示机器人标定时云台的位姿，o _c 表示机器人标定时云台原点, x _c 表示机器人标定时云台X轴坐标, y _c 表示机器人标定时云 台Y轴坐标, z _c 表示机器人标定时云台Z轴坐标，以

表示机器人巡检时云台 的位姿，

表示机器人巡检时云台原点,

表示机器人巡检时云台X轴坐标,

表示机器 人巡检时云台Y轴坐标,

表示机器人巡检时云台Z轴坐标，α表示标定时云台的水平角度，

表示巡检时云台的水平角度，β表示标定时云台垂直方向的角度，

表示巡检时云台垂直 方向的角度；机器人位姿变化由点云配准获得，∆x _r 表示机器人X轴方向变化，∆y _r 表示机器 人Y轴方向变化，∆roll表示机器人摇滚角变化，∆pitch表示机器人俯仰角变化，∆yaw表 示机器人偏航角变化；将姿态误差和位置误差分别弥补给云台。

4.根据权利要求3所述的基于点云配准ICP算法的机器人云台伺服控制方法，其特征在 于，姿态误差修正：∆yaw仅影响云台水平方向角度，∆roll和∆pitch影响云台垂直方向角 度；仅考虑∆yaw对云台对位的影响，则由∆yaw产生的水平角度差：∆α₁=α＇-α=-∆yaw；仅 考虑∆roll和∆pitch对云台对位的影响；p表示目标点到机器人标定时云台坐标系xoy平 面的投影，

表示目标点到机器人巡检时云台坐标系xoy平面的投影；由变换关系可知云 台坐标原点系原点o _c 到拍摄目标P的向量o _c P和云台坐标原点系原点o _c 到投影

的向量

，再根据余弦定理可知

：

则由∆roll和∆pitch产生的云台垂直方向误差为：

。

5.根据权利要求4所述的基于点云配准ICP算法的机器人云台伺服控制方法，其特征在 于，位置误差修正：完成姿态误差修正后，只考虑云台坐标系xoy平面上的位置偏差，∆x _c 为 云台X轴位置偏差，∆y _c 为为云台Y轴位置偏差，因为云台在机器人上的位置是固定的，所以 ∆x _c =∆x _r ，∆y _c =∆y _r ；

为相机光轴在

平面的投影，o _c E为∆y _c 的向量，o _c F为向量o _c E 在

方向上的分量，o _c D为向量o _c E在垂直

方向的方向上的分量，o _c B为∆x _c 的向量，o _c A为在

方向上的分量，o _c C为向量o _c B在垂直

方向的方向上的分量，则由云台位置 偏差∆x _c ，∆y _c 产生的在

方向和o _c D方向上的分量可分别表示为X_OFFSET和Y_OFFSET：

则由于云台位置偏移误差产生的云台水平补偿角度∆α₂和由于云台位置偏移误差产生的云台垂直补偿角度∆β₂按下式计算：

式中，d表示标定时云台坐标系原点与拍摄目标P的距离。

6.根据权利要求5所述的基于点云配准ICP算法的机器人云台伺服控制方法，其特征在于，由于机器人位姿偏差导致的云台水平方向补偿角∆α和垂直方向补偿角∆β按下式计算：

。

Images