本发明属于机器人控制，尤其涉及一种基于视觉的人形机器人双臂随动控制方法、系统与机器人。
背景技术：
1、人形机器人随动控制是人形机器人遥操技术中的重要环节，指通过各类传感器采集人体关节的运动数据，将此类数据转换为机器人各关节的旋转数据，从而控制人形机器人的运动。现有的技术中，主要是通过穿戴骨架来实现人体关节数据的采集，操作者需要具备一定的专业培训才可以熟练操作，穿戴不便捷、使用门槛高且造价昂贵，不利于一些非高精度场合的使用。其次，在已有的视觉识别人体骨架过程中易受光线等环境因素的影响，导致识别过程中的关节点位置发生跳动，从而导致关节转换为机器人角度数据的大幅偏差。此外，通过三维空间坐标点来逆推关节旋转的解并不是唯一的，难以抉择使用哪个解来控制人形机器人关节旋转的运动。
技术实现思路
1、为解决上述技术问题，本发明提供一种基于视觉的人形机器人双臂随动控制方法、系统与机器人。
2、本发明提供的技术方案具体如下：
3、一种基于视觉的人形机器人双臂随动控制方法，包括步骤：
4、s1、实时采集人体图像数据，包括深度图像和rgb图像；
5、s2、基于采集到的图像数据，获取人体关节点的空间位置信息，所述关节点包括头部j1、肩中心j2、右肩j3、右肘j4、右腕j5、右手j6、左肩j7、左肘j8、左腕j9、左手j10、脊柱j11、臀中心j12、右臀j13、右膝j14、右脚踝j15、右脚j16、左臀j17、左膝j18、左脚踝j19和左脚j20；
6、s3、根据左肩j7和左肘j8的位置信息，计算人体左大臂肩关节处的旋转自由度；
7、s4、根据右肩j3和右肘j4的位置信息，计算人体右大臂肩关节处的旋转自由度；
8、s5、根据左肩j7、左肘j8和左腕j9的位置信息，计算左手臂肘关节处的夹角；
9、s6、根据右肩j3、右肘j4和右腕j5的位置信息，计算右手臂肘关节处的夹角；
10、s7、根据左肩j7、左肘j8和左腕j9的位置信息，计算左大臂的轴向旋转角度；
11、s8、根据右肩j3、右肘j4和右腕j5的位置信息，计算右大臂的轴向旋转角度；
12、s9、将s3~s8计算得到的旋转自由度、夹角、轴向旋转角度进行线性插值，并以设定频率发送给人形机器人相应的关节控制模块，以实现该人形机器人双臂的随动控制。
13、优选地，步骤s1中通过kinect视觉传感器实时获取人体的图像数据，步骤s2中基于kinect人体骨架识别技术识别图像视野内的人体骨架，进而获取该骨架各关节点在三维空间中的位置信息。
14、进一步地，步骤s3中，假设左肩j7和左肘j8的空间坐标分别为（ x7, y7, z7）和（ x8, y8, z8），则：
15、s301、计算左大臂长度；
16、s302、设定左大臂初始状态沿y轴竖直向下，以左肩关节为原点，则左肘关节的初始位置，实际位置为；
17、s303、设定肩关节抬起的角度范围rxstart=0，rxend=90，若y8-y7≥0则设定rxstart=90，rxend=180；并设定rz=0，rx=rxstart，minerr=100；
18、s304、计算左肘关节的预测位置，其中旋转矩阵mrl1和mrl2分别为：，；
19、s305、计算肘关节预测位置与实际位置的误差，若el<minerr，则令minerr = el，左肩关节前后抬起的角度l1=rx，左右抬起的角度l2=rz，此时若el<0.01，则跳转到s308；若el≥minerr，则跳转到s306；
20、s306、令rx=rx+1，若rx≥rxend，跳转至s307，否则跳转至s304；
21、s307、令rz=rz+1，若rz≥90，则跳转到s308，否则令rx=rxstart，跳转至s304；
22、s308、获得左肩关节前后抬起的角度l1和左右抬起的角度l2。
23、进一步地，步骤s4中，假设右肩j3和右肘j4的空间坐标分别为（ x3, y3, z3）和（ x4, y4, z4），则：
24、s401、计算右大臂长度；
25、s402、设定右大臂初始状态沿y轴竖直向下，以右肩关节为原点，则右肘关节的初始位置，实际位置为；
26、s403、设定肩关节抬起的角度范围rxstart=0，rxend=90，若y4-y3≥0则设定rxstart=90，rxend=180；并设定rz=0，rx=rxstart，minerr=100；
27、s404、计算右肘关节的预测位置，其中旋转矩阵mrr1和mrr2分别为：，；
28、s405、计算肘关节预测位置与实际位置的误差，若er<minerr，则令minerr = er，右肩关节前后抬起的角度r1=rx，左右抬起的角度r2=rz，此时若er<0.01，则跳转到s408；若er≥minerr，则跳转到s406；
29、s406、令rx=rx+1，若rx≥rxend，跳转至s407，否则跳转至s404；
30、s407、令rz=rz+1，若rz≥90，则跳转到s408，否则令rx=rxstart，跳转至s404；
31、s408、获得右肩关节前后抬起的角度r1和左右抬起的角度r2。
32、进一步地，步骤s5中，假设左肩j7、左肘j8和左腕j9的空间坐标分别为（ x7, y7, z7）、（ x8, y8, z8）、（ x9, y9, z9），则所述左手臂肘关节处的夹角，其中，。
33、进一步地，步骤s6中，假设右肩j3、右肘j4和右腕j5的空间坐标分别为（ x3, y3, z3）、（ x4, y4, z4）、（ x5, y5, z5），则所述右手臂肘关节处的夹角，其中，。
34、进一步地，步骤s7中，假设左肩j7、左肘j8和左腕j9的空间坐标分别为（ x7, y7, z7）、（ x8, y8, z8）、（ x9, y9, z9），则左手臂大臂的轴向旋转角度的计算步骤为：
35、s701、计算左小臂的长度；
36、s702、设定左小臂初始状态沿y轴竖直向下，以左肩关节为原点，左腕关节的初始位置，实际位置为；
37、s703、设定轴向旋转的角度范围rastart=-45，raend=45，并设定ra=rastart，minerr=100；
38、s704、计算左手臂腕关节的预测位置：
39、，
40、其中，平移矩阵，旋转矩阵，旋转矩阵，旋转矩阵，旋转矩阵；
41、s705、计算肘关节预测位置与实际位置的误差，若el<minerr,则令minerr=el，左大臂轴向旋转的角度l3=ra，此时若el<0.01，则跳转到s707；若el≥minerr，则跳转到s706；
42、s706、令ra=ra+1；若ra≥raend，则跳转到s707，否则跳转至s704；
43、s707、获得左大臂轴向旋转的角度l3。
44、进一步地，步骤s8中，假设右肩j3、右肘j4和右腕j5的空间坐标分别为（ x3, y3, z3）、（ x4, y4, z4）、（ x5, y5, z5），则右手臂大臂的轴向旋转角度的计算步骤为：
45、s801、计算右小臂的长度；
46、s802、设定右小臂初始状态沿y轴竖直向下，以右肩关节为原点，右腕关节的初始位置，实际位置为；
47、s803、设定轴向旋转的角度范围rastart=-45，raend=45，并设定ra=rastart，minerr=100；
48、s804、计算右手臂腕关节的预测位置：
49、，其中，平移矩阵，旋转矩阵，旋转矩阵，旋转矩阵，旋转矩阵；
50、s805、计算肘关节预测位置与实际位置的误差，若el<minerr,则令minerr=el，左大臂轴向旋转的角度l3=ra，此时若el<0.01，则跳转到s807；若el≥minerr，则跳转到s806；
51、s806、令ra=ra+1；若ra≥raend，则跳转到s807，否则跳转至s804；
52、s807、获得右大臂轴向旋转的角度r3。
53、一种基于上述方法的人形机器人双臂随动控制系统，包括：
54、图像获取模块，用于实时采集人体图像数据；
55、人体关节点定位模块，用于从采集到的图像数据中获取人体关节点的空间位置信息；
56、左臂控制参数获取模块，用于根据获取到的人体关节点的空间位置信息，计算得到左大臂肩关节处的旋转自由度、左手臂肘关节处的夹角和左大臂的轴向旋转角度；
57、右臂控制参数获取模块，用于根据获取到的人体关节点的空间位置信息，计算得到右大臂肩关节处的旋转自由度、右手臂肘关节处的夹角和右大臂的轴向旋转角度；
58、控制参数输出模块，用于将计算得到的旋转自由度、夹角、轴向旋转角度进行线性插值后，以设定频率发送给人形机器人相应的关节控制模块，以实现该人形机器人双臂的随动控制。
59、一种人形机器人，包括头部、手部、小臂、大臂、肩膀、眼睛、嘴巴、前胸外壳、后背外壳、底座、电源开关、电源线、头灯、耳灯、胸灯、软胶脖子、显示屏、麦克风和音响；所述大臂的肩关节处具有前后运动、左右运动和轴向转动三个自由度，所述小臂的腕关节处具有绕小臂轴向转动和沿垂直手掌方向前后转动两个自由度，所述手部具备五个可单独活动的手指，所述头部具有上下点头和左右摇头两个自由度；所述人形机器人还配置有如上所述的双臂随动控制系统，包括图像获取模块、人体关节点定位模块、左臂控制参数获取模块、右臂控制参数获取模块和输出控制参数到关节控制模块的控制参数输出模块。
60、相比于现有技术，本发明至少具备以下有益效果：
61、能通过视觉实现人形机器人双臂随动控制，很好地替代传统的穿戴设备；利用旋转角度为1度的分辨率来循环计算误差，能很好的解决视觉在骨架识别过程中出现的小范围误差跳动问题；指定了各轴的旋转计算顺序，与传统的直接求逆解相比，不存在多个解的问题；实时性较好，能达到25fps，在遥操作机器人视觉随动控制上有广泛的应用前景；平滑性较好，数据同步频率大于视觉检测频率，使人形机器人在视觉随动过程中具有较好的平滑性。
1.一种基于视觉的人形机器人双臂随动控制方法，其特征在于，包括步骤：
2.如权利要求1所述的人形机器人双臂随动控制方法，其特征在于，步骤s1中通过kinect视觉传感器实时获取人体的图像数据，步骤s2中基于kinect人体骨架识别技术识别图像视野内的人体骨架，进而获取该骨架各关节点在三维空间中的位置信息。
3. 如权利要求1所述的人形机器人双臂随动控制方法，其特征在于，步骤s3中，假设左肩j7和左肘j8的空间坐标分别为（x7, y7, z7）和（x8, y8, z8），则：
4. 如权利要求1所述的人形机器人双臂随动控制方法，其特征在于，步骤s4中，假设右肩j3和右肘j4的空间坐标分别为（x3, y3, z3）和（x4, y4, z4），则：
5. 如权利要求1所述的人形机器人双臂随动控制方法，其特征在于，步骤s5中，假设左肩j7、左肘j8和左腕j9的空间坐标分别为（x7, y7, z7）、（x8, y8, z8）、（x9, y9, z9），则所述左手臂肘关节处的夹角，其中，。
6. 如权利要求1所述的人形机器人双臂随动控制方法，其特征在于，步骤s6中，假设右肩j3、右肘j4和右腕j5的空间坐标分别为（x3, y3, z3）、（x4, y4, z4）、（x5, y5, z5），则所述右手臂肘关节处的夹角，其中，。
7. 如权利要求1所述的人形机器人双臂随动控制方法，其特征在于，步骤s7中，假设左肩j7、左肘j8和左腕j9的空间坐标分别为（x7, y7, z7）、（x8, y8, z8）、（x9, y9, z9），则左手臂大臂的轴向旋转角度的计算步骤为：
8. 如权利要求1所述的人形机器人双臂随动控制方法，其特征在于，步骤s8中，假设右肩j3、右肘j4和右腕j5的空间坐标分别为（x3, y3, z3）、（x4, y4, z4）、（x5, y5, z5），则右手臂大臂的轴向旋转角度的计算步骤为：
9.一种基于权利要求1~8任一项所述方法的人形机器人双臂随动控制系统，其特征在于，包括：
10.一种人形机器人，其特征在于，包括头部、手部、小臂、大臂、肩膀、眼睛、嘴巴、前胸外壳、后背外壳、底座、电源开关、电源线、头灯、耳灯、胸灯、软胶脖子、显示屏、麦克风和音响；所述大臂的肩关节处具有前后运动、左右运动和轴向转动三个自由度，所述小臂的腕关节处具有绕小臂轴向转动和沿垂直手掌方向前后转动两个自由度，所述手部具备五个可单独活动的手指，所述头部具有上下点头和左右摇头两个自由度；所述人形机器人还配置有如权利要求9所述的双臂随动控制系统，包括图像获取模块、人体关节点定位模块、左臂控制参数获取模块、右臂控制参数获取模块和输出控制参数到关节控制模块的控制参数输出模块。
