Page 8 - 2022年第53卷第6期

振捣图像智能感知；采用避障雷达和 32线固态激光雷达等传感器实现仓面环境下多尺度、动/静态障
碍物的感知。
（6）机载控制器。该模块为振捣机器人的核心部件，采用满足多种通讯方式和高性能计算要求的
工业控制器。一方面集成感知单元的信息，基于相关智能算法进行分析与决策，实现振捣机器人本
体的控制；另一方面，接收云端监控平台的指令信息，同时上传振捣机器人运行中多维度信息。
2.2.2 振捣机器人本体运动学模型 运动学模型旨在通过几何关系构建车体运动的模型。振捣机器
人本体运动学建模是实现振捣机器人本体空间运动分析、轨迹规划，以及振捣位置参数计算、运动
控制等的基础，亦是振捣机器人系统实现自动振捣的关键内容。本文研发的振捣机器人系统是一个
复杂的装备系统，通过各模块间有机配合与联动，从而实现系统智能无人振捣的功能。建立振捣机
}
}
器人 D-H 运动学模型，如图 3 所示，包括世界坐标系{O - X O Y O Z O 、车体坐标系{A - X A Y A Z A 、机械
}
}
臂基坐标系{B - X B Y B Z B 、机械臂末端工具坐标系{C - X C Y C Z C 。不同坐标系间关系可通过齐次变换
O
O
矩阵进行关联 ［29］ 。以右边振捣棒末端点L为例，则点L在世界坐标系{ }的空间位置 L P可表示为：
O O A B C
L P = A T· B T· C T· L P （1）
O
C
B
A
A
A
O
B
C
B
式中： A T、 B T、 C T 分别为坐标系{ }和{ }、{ }和{ }、{ }和{ }之间转换关系； L P 为 L 点位于坐标系
C
{ }中的位置。
A
O
A
笛卡尔空间下坐标系{ }和{ }关系可采用 4×4维度的齐次变换矩阵表示。设坐标系{ }原点与车
O
体上安装的GNSS天线中心重合，故 A T 可表示为：
ù
éc 1 c 2 c 1 s 2 s 3 - s 1 c 3 c 1 s 2 c 3 + s 1 s 3 x gnss
ê ú
O ê ê s 1 c 2 s 1 s 2 s 3 + c 1 c 3 s 1 s 2 c 3 - c 1 s 3 y gnss ú ú
A T = ê ê ú ú （2）
ê ê -s 2 c 2 s 3 c 2 c 3 z gnss ú ú
ê ú
ë 0 0 0 1 û
式中：c 1 、c 2 、c 3 分别为cosθ 1 、cosθ 2 、cosθ 3 ；s 1 、s 2 、s 3 分别为sinθ 1 、sinθ 2 、sinθ 3 ；θ 1 、θ 2 、θ 3 分别为 Z 、
A
Y 、X 方向旋转的欧拉角度，以右手定则判别旋转正方向，单位为°；x gnss 、y gnss 、z gnss 为 GNSS 测量的
A
A
大地空间坐标，单位为m。
B
A
车体为刚性，坐标系{ }与{ }相对静止，故二者间关系可表示为：
A ] T
B T =[ X AB , Y AB , Z AB , 1 （3）
B
A
式中：X AB 、Y AB 、Z AB 分别为坐标系{ }原点相对于坐标系{ }原点在轴X 、Y 、Z 的欧式距离。
A
A
A
B
C
由 机 器 人 学 ［29］ 可 知 ， 6 自 由 度 机 械 手 末 端 坐 标 系 { } 原 点 在 基 坐 标 系 { } 可 表 示 为
)
)
B )，其中(X BC , Y BC , Z BC 和四元数(q 1 , q 2 , q 3 , q 4 分别为坐标系{ }原
C
C T =(X BC , Y BC , Z BC , q 1 , q 2 , q 3 , q 4
B
点位于坐标系{ }下的空间位置以及姿态，可表示矩阵形式：
2 2 2q 2 q 3 + 2q 1 q 4 2q 2 q 4 - 2q 1 q 3 X BC ù
ê é 1 - 2q 3 - 2q 4 ú
2
C T = ê ê ú ú （4）
B ê ê 2q 2 q 3 - 2q 1 q 4 1 - 2q 2 - 2q 4 2 2q 3 q 4 + 2q 1 q 2 Y BC ú ú
ê ê 2q 2 q 4 + 2q 1 q 3 2q 3 q 4 - 2q 1 q 2 1 - 2q 2 - 2q 3 2 Z BC ú ú
2
ê ú
ë 0 0 0 1 û
进一步，由于振捣棒与机械手末端中心点刚性连接，故振捣棒末端 L、R、K 三点位于坐标系{ }
C
中可表示为：
ì C ] T
ï L P =[ l mw , -l mb , l h , 1
ï
ï
ï C ] T
í K P = [ l kw , 0, l h , 1 （5）
ï
ï
ï ] T
ï C
î R P = [ l mw , l mb , l h , 1
—634 —