(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221076125 3.5 (22)申请日 2022.06.29 (71)申请人 深圳艾摩米智能科技有限公司 地址 518000 广东省深圳市南 山区粤海街 道高新区社区粤兴五道9号北理工创 新大厦5-D单元 (72)发明人 陈明　李宏亮　黄华婷　李厚兵　 曹小华　朱旭磊　王文星　金波　 (74)专利代理 机构 深圳市弘为力创知识产权代 理事务所(普通 合伙) 44751 专利代理师 康晓春 (51)Int.Cl. B25J 9/16(2006.01) B25J 11/00(2006.01) (54)发明名称 一种机械手臂沿人体表面移动的姿态规划 方法 (57)摘要 本发明涉及一种机械手臂沿人体表面移动 的姿态规划方法， 包括如下步骤： 获取人体表面 图像， 生成人体表面的关键坐标点； 获取人体表 面点云数据， 生成人体表面深度图； 在人体表面 图像中选 取邻域； 从人体表面深度图取得对应位 置的深度坐标信息， 将2D邻域点集转换成3D点 集； 形成3D点云矩阵； 对每一个3D点云矩阵通过 求协方差矩阵， 得到3D点 云矩阵的特征值 λ和特 征向量x， y， z； 在人体表面建立起一个物体坐标 系； 求得欧拉角R， 对机械手臂的姿态进行调整。 本发明， 可对机械手臂的姿态精确的进行控制， 满足沿人体表 面移动的需要， 适用于人脸部位的 机械手臂移动中的姿态控制， 可模拟人工操作中 的移动姿态， 避免意外情况及错误操作情况的出 现。 权利要求书3页 说明书8页 附图2页 CN 114939874 A 2022.08.26 CN 114939874 A 1.一种机 械手臂沿人体表面移动的姿态规划方法， 其特 征在于， 包括如下步骤： 步骤一， 获取人体表面图像， 生成人体表面的关键坐标点， 基于关键坐标点的二维坐标 信息形成关键坐标点 集； 步骤二， 获取 人体表面 点云数据， 生成人体表面深度图； 步骤三， 以各关键坐标点的二维坐标信 息为中心， 按邻域的预设尺寸， 在人体表面图像 中选取邻域， 形成2D邻域 点集； 步骤四， 用2D邻域点集中的每个平面点， 从人体表面深度图取得对应位置的深度坐标 信息， 所述深度坐标信息指深度值Z， 然后通过3D投影 变换将2D邻域 点集转换成3D点 集； 步骤五， 对3D点集分组， 分组原则为： 以关键坐标点及其邻域为划分单位， 将一个划分 单位内涵盖的、 3D点 集中的3D点， 划分为 一组； 步骤六； 按分组形成若干 3D点云矩阵； 步骤七， 对每一个3D点云矩阵通过求协方差矩阵， 得到3D点云矩阵的特征值λ和特征向 量x， y， z； 通过求协方差矩阵， 以每一个3D点为中心， 在人体表面建立起一个物体坐标系， 所述物 体坐标系由坐标原点和x， y， z坐标轴组成； 步骤八， 根据前一步骤中得到的各个3D点云矩阵的特征值和特征向量， 求得欧拉角R， 对机械手臂的姿态进行调整。 2.如权利要求1所述的一种机械手臂沿人体表面移动的姿态规划方法， 其特征在于， 步 骤一中， 所述人体表面图像是指人体2D红外图； 步骤一的具体步骤如下： 基于opencv官 方库进行 人脸位置检测； 通过Dlib数据库实现人脸区域的68个关键坐标点的提取； 基于68个关键坐标点扩展生成额头区域的10个关键坐标点， 合计实现78个关键坐标点 的提取； 所述扩展生成额头区域的10个关键坐标点采用如下计算推理公式： x1＝x2        (1) 式中： x1、 y1为新的额头关键坐标点的X、 Y坐标； x2、 y2为68个关键坐标点中参与计算的关键坐标点的X、 Y坐标； a为68个关键坐标点中第3 3号关键坐标点与第27号关键坐标点的Y坐标差值； b为68个关键坐标点中第0号关键坐标点的X坐标； c为68个关键坐标点中第16号关键坐标点的X坐标。 3.如权利要求1所述的一种机械手臂沿人体表面移动的姿态规划方法， 其特征在于， 步 骤二中， 所述人体表面 点云数据通过3D相机获取。权　利　要　求　书 1/3 页 2 CN 114939874 A 24.如权利要求1所述的一种机械手臂沿人体表面移动的姿态规划方法， 其特征在于， 步 骤三中， 邻域的预设尺寸 为长14像素、 宽8像素的矩形； 步骤三的具体步骤如下： 以各关键坐标点为中心， 以一个像素为扩展幅度， 每次在宽方向增加或者减少扩展幅 度， 在高方向增 加或减少扩展幅度， 在人体表面图像中选取邻域。 5.如权利要求1所述的一种机械手臂沿人体表面移动的姿态规划方法， 其特征在于， 步 骤四中， 通过3D投影 变换将2D邻域 点集转换成3D点 集的具体步骤如下： 设camera_fx， camera_fy分别为镜 头x， y方向的焦距， 即成像平面到 镜头的距离； 设camera_cx， camera_cy分别为光轴对于投影平面坐标 中心的x， y偏移量， 投影平面坐 标系的原点 位于图像左上角， 水平为x， 垂直 为y； 设人体表面图像中的像素点的X、 Y坐标记为x0,y0， 则通过式(3) ‑式(5)将2D邻域点集 转换成3D点 集： 式中： d为深度坐标信息， 即深度值Z； x， y， z为3D点的X、 Y、 Z坐标； camera_factor为缩放比例。 6.如权利要求1所述的一种机械手臂沿人体表面移动的姿态规划方法， 其特征在于， 步 骤七中， 由特 征向量x， y， z表达为x， y， z坐标轴的具体步骤如下： 取最大的特 征值 λ， 将其对应的特 征向量x标记为物体坐标系的x轴； 取最小的特 征值 λ， 将其对应的特 征向量z标记为物体坐标系的z轴； 基于矢量叉乘的方法求 解物体坐标系的y轴。 7.如权利要求1所述的一种机械手臂沿人体表面移动的姿态规划方法， 其特征在于， 步 骤八中， 将前一 步骤中的x， y， z坐标轴拼成一个旋转矩阵R， 在旋转矩阵R中： 每一列对应一组x， y， z坐标轴； 通过式(10) ‑式(12)求得欧拉角R， 欧拉角R包括欧拉角三分量( θ x， θ y, θ z)， θx＝atan2(r32,r33)权　利　要　求　书 2/3 页 3 CN 114939874 A 3