一种人形智能机器人系统的制作方法
本发明涉及人形机器人技术领域，具体涉及一种人形智能机器人系统。
背景技术：
人形机器是一种具有高度智能信息化的机器人，具有很高的工业、商业、军事价值。随着世界工业化进程的发展，单纯的工业机器人已经不能满足市场的需求，人形机器人也受到世界上各个国家科研机构的关注和重视。人形机器人的研究已经有三十多年，期间人形机器人的硬件系统架构、软件上层设计和物理结构都取得了很大的突破。目前，人形机器人的关键技术主要掌握在日本、美国、德国和英国等发达国家中，国内而言，虽然研究起步较晚，但发展迅速，也取得了很大的突破。但目前人形机器人的研究大多集中在高校和一些科研所，研究出的样机大多控制系统复杂、造价昂贵、可裁剪性低、驱动器复杂、人形机器人笨重，不利于企业投入市场。
因此，如何提供一种解决上述问题的人形智能机器人系统是本领域技术人员目前需要解决的问题。
技术实现要素：
本发明的目的是提供一种人形智能机器人系统，将实时步态与避障控制结合起来，使机器人能够智能行走。
为了实现上述任务，本发明采用以下技术方案：
一种人形智能机器人系统，包括机器人主体以及控制系统；所述的控制系统包括：
视觉图像获取模块，用于获取机器人周围的环境图像信息，将图像进行去噪处理后，传递给中央处理模块；
中央处理模块，用于根据视觉图像获取模块提供的去噪后的图像，采用预规划的实时步态控制算法和基于双目视觉的避障算法，对图像中的障碍物进行判断，并进行步态规划，得到机器人的行走步态，并利用副控制器控制驱动模块进行机器人行走；
驱动模块，包括安装在机器人各个关节处的舵机，用于根据副控制器传递来的控制信息，驱动机器人的关节运动，从而实现机器人的行走；
传感模块，用于实时获取机器人的姿态角度信息，并将数据传输到中央处理模块，以实现机器人运动过程中的精确控制；
其中，所述的预规划的实时步态控制算法包括：
先建立双足机器人运动学坐标系模型，然后进行机器人的姿态规划和平稳性判断，最终完成步态行走，实时规划下一步的姿态；
所述的基于双目视觉的避障算法包括：
机器人检测到前方物体并判定为障碍物时，立即站立停止，在静止状态下时获取障碍物的尺寸参数和方位信息；根据障碍物尺寸参数、方位信息控制机器人绕过障碍。
进一步地，所述的机器人检测到前方物体并判定为障碍物时，立即站立停止，在静止状态下时获取障碍物的尺寸参数和方位信息；根据障碍物尺寸参数、方位信息控制机器人绕过障碍，包括：
步骤1，根据机器人要到达的目的地进行路径的规划，对双目摄像头的左右两个摄像头分别进行标定，分别计算地面在左、右摄像头坐标系下的平面方程；
步骤2，通过双目摄像头采集机器人前方图像，并进行去噪处理；
步骤3，对左右摄像头采集的两幅图像进行畸变和极线校正，消去失真，将匹配点约束在一条直线上，最终匹配得到一张视差图；
步骤4，计算视差图中匹配点在左摄像头坐标系下的三维坐标(x,y,z)，获取其高度与水平距离，并与设定阈值比较，判断障碍物是否存在；使用opencv视觉库中的cvfindcon—tours轮廓检测函数检测物体的轮廓，并用其外接矩形框标记；
对障碍物的判别算法：首先随机提取视差图中外接矩形框内物体上若干个白色像素点，计算其位于左摄像机坐标系下的三维坐标(x，y，z)，其中x和z分别表示物体相对于左摄像头的高度距离与水平距离，y则表示物体相对于左摄像头光心的左右偏移距离，然后根据设定的阈值判断矩形框内的物体是否为障碍物；
步骤5，如判断前方无障碍物，则使机器人按照规划的路径行进；如判断前方有障碍物，则令机器人停止动作，并获取障碍物的参数信息，具体如下：
步骤5.1，将障碍物的外接矩形框的宽度作为障碍物的宽度，记为q，外接矩形框的中心作为障碍物中心点，记为o；
步骤5.2，通过深度检测获取中心点o的三维坐标di(x,y,z)，其中y表示中心点相对于左摄像头光心的偏移距离li；获取外界矩形框中最左侧的一个白色像素点在左摄像头坐标系下的坐标(xl,yl,zl)；
步骤5.3，根据障碍物的宽度计算出障碍物中心点o的纵坐标，即o点相对于左摄像头光心的偏移距离lo：
步骤5.4，由于双目摄像头安装在机器人的中轴线上，因此障碍物中心点o相对于机器人中轴线的偏距d为：
其中d>0表示偏右，d<0表示偏左，d＝0表示居中；
步骤6，进行避障路径规划
步骤6.1，设定障碍物的长度为w；
在障碍物检测过程中，已经能够比较精确地获得障碍物的宽度尺寸及其相对于机器人的方位信息，但是较难得到障碍的长度尺寸，一般假设其长度为w；
步骤6.2，当障碍物相对于机器人偏左时，机器人右转角度θ并沿平行安全距离直行一段距离x，机器人中心与障碍物中心的连线与规划路径垂直时，左转角度2θ并直行距离x，使其回归规划路径；
对于机器人的避障过程，有：
上式中，l为机器人的身宽；θ和2θ为机器人的旋转角度；d为障碍物相对机器人的距离；yl为障碍物左侧边界相对于机器人中心的偏移距离；d为考虑存在的误差和安全问题所增加的补偿距离。
进一步地，所述的对障碍物的判别算法，具体包括：
步骤4.1，随机提取视差图中外接矩形框内若干白色像素点i＝1,2,3,…，计算其三维坐标di(xi,yi,zi)，并按照水平距离zi(即三维坐标中的zi值)的大小排序；
步骤4.2，因为提取和计算过程中存在误差，所以可能存在异常数据，因此取所有白色像素点水平距离zi的中位数z0，并设定阈值φ，对于所述的白色像素点，如白色像素点i的水平距离zi满足|zi-z0|＞φ，则将白色像素点i作为异常点剔除，剩余白色像素点构成集合i；
步骤4.3，计算集合i中白色像素点水平距离zi的平均值
步骤4.4，设定高度阈值xd和水平距离阈值zd，当满足xmax＞xd，且
进一步地，所述的预规划的实时步态控制算法，包括：
步骤1’，建立双足机器人运动学坐标系模型
步骤1.1’，对机器人的各个关节进行编号，对于关节i，确定该关节的原点；
步骤1.2’，确立关节i处坐标系的各个坐标轴xi、yi、zi；
步骤1.3’，根据在各个关节处建立的坐标系，相邻的两个坐标系oi与oi-1相比，获得相邻两个坐标系的差距，即变换矩阵；
通过上面的变换矩阵得到相邻的两个坐标系之间的运动学关系，记双足机器人的左脚膝关节的坐标系o0-x0y0z0为参考坐标系，可得到其他关节坐标系与参考坐标系之间的运动学关系；将每个关节处的坐标系与参考坐标系之间的变换矩阵作为该关节与参考坐标系所在关节之间的数学模型，所有数学模型构成双足机器人运动学坐标系模型；
步骤2’，对双足机器人的姿态进行规划
步骤2.1’，设计双足机器人行走步态流程
通过摆动腿前摆改变重心的，在摆动腿前摆时，身体重心从双腿中心移到支撑腿上，因此身体先要向支撑腿一侧扭动来移动重心，即通过髋关节的侧向关节来调整重心；为了防止在侧扭时抬腿而造成身体向摆动腿一侧倾倒、步态设计中将侧扭和抬腿分步设计；
步骤2.2’，规划机器人行走时各个关节的姿态
先假设髋关节的侧向不发生变化，则求解行走轨迹步骤如下，以髋关节与踝关节为例：
(1)绘制出髋关节与裸关节随时间改变而变化的曲线图，由此获得髋关节、裸关节的运动学轨迹；
(2)与人类行走轨迹进行对比，按照人类行走过程的受力情况明确机器人髋关节、裸关节运动受力的关键点，然后通过三次样条插值的方式，对所有关键点进行插值处理；
(3)分别将髋关节、裸关节处理后的关键点与步骤(1)中髋关节、裸关节运动学轨迹中的多项式进行拟合，获得髋关节与裸关节的平滑轨迹；则该轨迹即为髋关节与裸关节每一步行走后形成的轨迹。
按照相同的方法，可获得机器人双腿上其他关节在行走时形成的轨迹；将双足机器人一次行走过程中所涉及的关节的轨迹作为规划姿态，则获得规划的姿态后，通过关节处的舵机驱动机器人按照规划的姿态进行行走。
进一步地，所述的预规划的实时步态控制算法，还包括：
步骤3’，进行姿态规划平稳性判断
根据步骤2’中的规划姿态，在机器人每次准备落脚时进行平稳性计算，判断当前机器人是否满足平稳性要求，判断依据为机器人的平稳点是否在设定的范围内，如在设定的范围内，则不干预，使机器人按照规划姿态进行行走，否则暂停行走，通过机器人的控制器调整机器人腿部的关节的姿态后，重新计算平稳性，直至平稳性满足要求，其中所述的平稳点计算方法为：
计算机器人的质心位置：
其中，n表示机器人全身关节的数量，mi表示第i个关节的质量，
计算平稳点w：
w＝-a+com
其中，
上式中，ts为采样时间，comi，comi-1，comi-2分别为第i次、第i-1次和第i-2次采样时机器人的质心位置，com为机器人站立不动时的质心位置；
判断平稳点w的位置，若w在设定的范围内，则认为机器人此步行走稳定，否则不稳定。
进一步地，所述的机器人主体包括：
头部单元、主体单元、腿部单元以及设置在主体单元两侧的臂部单元；其中臂部单元包括左臂和右臂，腿部单元包括左腿和右腿。
进一步地，所述的头部单元通过第19号舵机与主体单元连接，第19号舵机用于驱动头部单元向上或向下旋转0°～45°；头部单元包括led状态显示单元、第20号舵机以及双目摄像头；所述的led状态显示单元包括一组8个的蓝色led灯和1个可变色led灯，蓝色led灯连接至副控制器上，作为通电指示灯和眼部指示灯，即通电点亮，断电熄灭；所述的可变色led灯连接到副控制器上，作为机器人状态指示灯，其显示绿色常亮表示机器人状态正常，显示橙色常亮表示机器人状态存在异常；所述双目摄像头包括左右两个单目摄像头；所述单目摄像头连接到主控制器上，负责收集机器人本体周围的环境图像信息；所述的第20号舵机用于驱动双目摄像头向左或向右旋转0°～45°。
进一步地，所述的臂部单元中的左臂通过第1号舵机与主体单元连接，左臂包括第3号舵机和第5号舵机，其中第1号舵机用于驱动左臂向上或向下转动0°～90°，第3号舵机安装在左臂的肘部位置，用于驱动左小臂向上抬起0°～90°；第5号舵机安装在左臂的手腕部位，用于驱动左手向上或向下旋转0°～45°；
所述的臂部单元中的右臂通过第2号舵机与主体单元连接，右臂包括第4号舵机和第6号舵机，其中第2号舵机用于驱动右臂向上或向下转动0°～90°，第4号舵机安装在右臂的肘部位置，用于驱动右小臂向上抬起0°～90°；第6号舵机安装在右臂的手腕部位，用于驱动右手向上或向下旋转0°～45°。
进一步地，所述的腿部单元中的左腿通过第7号舵机与主体单元连接，包括第9号舵机、第11号舵机、第13号舵机、第15号舵机和第17号舵机，其中第7号舵机可以转动，用于驱动左腿向左或向右水平旋转0°～45°，第9号舵机用于驱动左腿向左或向右摆动0°～45°，第11号舵机用于驱动左腿向前或向后摆动0°～45°；第13号舵机安装在左腿的膝关节位置，用于驱动左小腿弯曲0°～90°；第15号舵机、第17号舵机分别用于驱动左脚向左或向右摆动0°～45°、向前或向后摆动0°～45°；
所述的腿部单元中的右腿通过第8号舵机与主体单元连接，包括第10号舵机、第12号舵机、第14号舵机、第16号舵机和第18号舵机，其中第8号舵机可以转动，用于驱动右腿向右或向右水平旋转0°～45°，第10号舵机用于驱动右腿向右或向右摆动0°～45°，第12号舵机用于驱动右腿向前或向后摆动0°～45°；第14号舵机安装在右腿的膝关节位置，用于驱动右小腿弯曲0°～90°；第16号舵机、第18号舵机分别用于驱动右脚向右或向右摆动0°～45°、向前或向后摆动0°～45°。
进一步地，所述的主体单元包括主控制器、副控制器、按键模块、蜂鸣器模块、电源供电模块、第1号舵机、第2号舵机、第7号舵机、第8号舵机和第19号舵机；所述主控制器通过usb连接副控制器和双目摄像头，负责机器人控制算法以及指导副控制器工作；所述的主控制器上设置有外接接口，外接接口包括2个usb接口和1个lan接口，可外接pc端或外接显示器、鼠标、键盘设备对机器人系统进行调试或操控；所述副控制器通过总线方式连接第1-20号舵机，通过数据传输线连接位于头部单元的led状态显示单元以及位于主体单元的按键模块、蜂鸣器模块；所述的总线方式包括五条总线，其中第1、3、5号舵机处于第一总线、第2、4、6号舵机处于第二总线、第7、9、11、13、15、17号舵机处于第三总线、第8、10、12、14、16、18号舵机处于第四总线、第19、20号舵机处于第五总线；所述副控制器上安装有陀螺仪和加速器，用于检测机器人所处的位置信息。
本发明具有以下技术特点：
1.本发明人形智能机器人系统采用了预规划的实时步态控制算法和基于双目识别的避障算法，两种方法结合使用，能够对人形机器人的步态进行精确控制，从而使人形机器人更好地实现避障和步态调整的功能，提高了人形机器人对路面的适应能力。
2.本发明方案采用双控制器结构，分工明确，工作效率高，电路结构简单，设计新颖合理，实现方便，使用效果好，便于推广使用。
附图说明
图1为本发明的人形智能机器人系统的构架示意图；
图2为本发明的头部单元结构示意图；
图3为本发明主体单元后背部分的结构示意图；
图4为本发明各个单元的位置结构示意图；
图5为本发明中基于双目视觉的避障算法流程示意图；
图6为基于双目视觉的避障算法中障碍物轮廓检测示意图；
图7为基于双目视觉的避障算法的避障路径规划示意图；
图8为预规划的实时步态控制算法的流程示意图；
图9的(a)和(b)分别为本发明的人形智能机器人实物外形结构示意图，以及机器人上舵机安装位置示意图；
图10为预规划的实时步态控制算法中在关节处建立坐标系的示意图；
图11为关节部分的结构示意图；
图12为机器人向前行走时不同步态的示意图；
图13为机器人腿部在行走时偏转角度示意图；
图14为机器人脚部受力示意图。
图中标号代表：1—20分别表示第1号舵机至第20号舵机，21主控制器，22副控制器，23/25蓝色led灯，24/26单目摄像头，27可变色led灯，28头部单元，29/30usb接口，31电池放置口，32总开关，33电源插口，34模式选择按键，35确认按键，36复位按键，37/38散热风扇，39led状态显示灯，40lan接口，41主体单元，42左臂，43右臂，44左腿，45右腿。
具体实施方式
本发明公开了一种人形智能机器人系统，如图1、图4所示，包括机器人主体以及控制系统：
其中机器人主体从上至下依次包括：头部单元、主体单元、腿部单元以及设置在主体单元两侧的臂部单元；其中臂部单元包括左臂和右臂，腿部单元包括左腿和右腿。
其中，所述的头部单元通过第19号舵机与主体单元连接，如图1、图2所示，第19号舵机用于驱动头部单元向上或向下旋转0°～45°；头部单元包括led状态显示单元、第20号舵机以及双目摄像头；所述的led状态显示单元包括一组8个的蓝色led灯和1个可变色led灯，蓝色led灯连接至副控制器上，作为通电指示灯和眼部指示灯，即通电点亮，断电熄灭；所述的可变色led灯连接到副控制器上，作为机器人状态指示灯，其显示绿色常亮表示机器人状态正常，显示橙色常亮表示机器人状态存在异常；所述双目摄像头包括左右两个单目摄像头；所述单目摄像头连接到主控制器上，负责收集机器人本体周围的环境图像信息；所述的第20号舵机用于驱动双目摄像头向左或向右旋转0°～45°。
所述的臂部单元中的左臂通过第1号舵机与主体单元连接，左臂包括第3号舵机和第5号舵机，其中第1号舵机用于驱动左臂向上或向下转动0°～90°，第3号舵机安装在左臂的肘部位置，用于驱动左小臂向上抬起0°～90°；第5号舵机安装在左臂的手腕部位，用于驱动左手向上或向下旋转0°～45°；
所述的臂部单元中的右臂通过第2号舵机与主体单元连接，右臂包括第4号舵机和第6号舵机，其中第2号舵机用于驱动右臂向上或向下转动0°～90°，第4号舵机安装在右臂的肘部位置，用于驱动右小臂向上抬起0°～90°；第6号舵机安装在右臂的手腕部位，用于驱动右手向上或向下旋转0°～45°。
所述的腿部单元中的左腿通过第7号舵机与主体单元连接，包括第9号舵机、第11号舵机、第13号舵机、第15号舵机和第17号舵机，其中第7号舵机可以转动，用于驱动左腿向左或向右水平旋转0°～45°，第9号舵机用于驱动左腿向左或向右摆动0°～45°，第11号舵机用于驱动左腿向前或向后摆动0°～45°；第13号舵机安装在左腿的膝关节位置，用于驱动左小腿弯曲0°～90°；第15号舵机安装于左腿裸关节处，用于驱动左脚向左或向右摆动0°～45°；第17号舵机安装于左腿裸关节处，用于驱动左脚向前或向后摆动0°～45°；
所述的腿部单元中的右腿通过第8号舵机与主体单元连接，包括第10号舵机、第12号舵机、第14号舵机、第16号舵机和第18号舵机，其中第8号舵机可以转动，用于驱动右腿向右或向右水平旋转0°～45°，第10号舵机用于驱动右腿向右或向右摆动0°～45°，第12号舵机用于驱动右腿向前或向后摆动0°～45°；第14号舵机安装在右腿的膝关节位置，用于驱动右小腿弯曲0°～90°；第16号舵机安装于右腿裸关节处，用于驱动右脚向右或向右摆动0°～45°；第18号舵机安装于右腿裸关节处，用于驱动右脚向前或向后摆动0°～45°；
如图3所示，所述主体单元包括主控制器、副控制器、按键模块、蜂鸣器模块、电源供电模块、第1号舵机、第2号舵机、第7号舵机、第8号舵机和第19号舵机；所述主控制器通过usb连接副控制器和双目摄像头，负责机器人控制算法以及指导副控制器工作；所述的主控制器上设置有外接接口，外接接口包括2个usb接口和1个lan接口，可外接pc端或外接显示器、鼠标、键盘设备对机器人系统进行调试或操控；所述副控制器通过总线方式连接第1-20号舵机，通过数据传输线连接位于头部单元的led状态显示单元以及位于主体单元的按键模块、蜂鸣器模块；所述的总线连接方式包括五条总线，其中第1、3、5号舵机处于第一总线、第2、4、6号舵机处于第二总线、第7、9、11、13、15、17号舵机处于第三总线、第8、10、12、14、16、18号舵机处于第四总线、第19、20号舵机处于第五总线；所述副控制器上安装有陀螺仪和加速器，用于检测机器人所处的位置信息；
所述电源供电模块包括充电电池放置口、有线电源连接口和电源总开关；所述电池放置口用于放置充电电池；所述有线电源连接口用于连接12vdc的有线电源；所述的电源总开关用于控制机器人系统总电源的连接与断开。
打开开关时整个机器人为上电状态，位于头部单元28的蓝色led23/25立即亮起，系统自动启动并自动检测各接口连接状态，若检测各接口连接无误，则位于头部单元的可变色led灯显示绿色常亮，机器人系统可正常工作，若检测各接口连接有误，则位于头部单元的可变色led灯显示橙色常亮；断开开关时则整个机器人系统为无电状态；
所述的按键模块包括模式选择按键、确认按键和复位键；所述的模式选择按键可以选择机器人不同的工作模式，操作模式选择按键时系统会通过蜂鸣器模块提示当前选择模式，工作模式可以根据用户的需求进行定制；所述的确认按键用于确认所选择的工作模式；所述复位键用于返回默认工作模式，按下复位键即可返回默认工作模式，无需进行确认；所述默认工作模式可由用户进行制定；所述的主体单元还包括散热模块，其由散热风扇组成；所述散热风扇直接连接到电源供电模块，打开电源即能够进行散热工作。
本实施例中，所述主控制器为pc104嵌入式控制板；所述的控制板中搭载linux系统和ros次级系统；所述电源供电模块包括12.6v的li-po充电电池和220v-12v充电器，所述的舵机和主控制器采用12v供电；所述的蜂鸣器模块采用lm7805芯片转换成5v供电；所述的副控制器采用ex1117-3.3v芯片转换成3.3v供电；所述双目摄像头为200w像素广角摄像头；所述的双目摄像头与主控制板通过usb接口连接、所述的主控制器、副控制器间通过usb接口连接；所述第1-20号舵机均采用的dynamixel系列mx-28t舵机。
本发明的控制系统包括以下模块：
视觉图像获取模块，用于获取机器人周围的环境图像信息，将图像进行去噪处理后，传递给中央处理模块，其中视觉图像获取模块采用所述的双目摄像头。
中央处理模块，包括所述的主控制器，搭载linux系统和ros次级操作系统，用于根据视觉图像获取模块提供的去噪后的图像，采用预规划的实时步态控制算法和基于双目视觉的避障算法，对图像中的障碍物进行判断，并进行步态规划，得到机器人的行走步态，并将控制舵机转动的代码数据传递给副控制器，利用副控制器(stm32)控制驱动模块进行机器人行走。
驱动模块，用于根据副控制器传递来的控制信息，驱动机器人的关节运动，其中驱动模块采用安装在机器人各个关节处的舵机；本实施例中在机器人各关节处共安装了20个相同型号的舵机，其内部具有一个微处理器，用来接收和反馈控制器的数据包。
传感模块，用于实时获取机器人的姿态角度信息，并将数据传输到中央处理模块，以实现机器人运动过程中的精确控制；本发明采用lis331dlh陀螺仪和l3g4200d加速度计来测量人形机器人姿态信息，其直接安装于副控制器stm32上并通过主控制器、副控制器的通讯直接传输到中央处理模块。
除了上述模块之外，还包括：
串口通信模块，用于主控制器、副控制器、与舵机的连接，由于mx-28t舵机是使用ttl电平进行通信，因此本发明中加入了74hc126缓冲芯片，并利用芯片特性把全双工的变为半双工，实现单线实现接收和发送数据功能；而根据pc104的usb接口多的特点，本发明选择ft232芯片作为usb转串口芯片，实现主控制器、副控制器之间使用usb进行通信。
本发明中的机器人控制算法采用预规划的实时步态控制算法和算法基于双目视觉的避障算法，其中基于双目视觉的避障算法，其基本原理是机器人检测到前方物体并判定为障碍物时，立即站立停止，在静止状态下时获取障碍物的尺寸参数和方位信息；根据障碍物尺寸参数、方位信息控制机器人绕过障碍，回归导航路径如图5所示，具体包括以下步骤：
步骤1，根据机器人要到达的目的地进行路径的规划，对双目摄像头的左右两个摄像头分别进行标定，分别计算地面在左、右摄像头坐标系下的平面方程；
步骤2，通过双目摄像头采集机器人前方图像，并进行去噪处理；
步骤3，对左右摄像头采集的两幅图像(左右摄像头同时分别采集一张图像)进行畸变和极线校正，消去失真，将匹配点约束在一条直线上，减少误匹配并大大缩短匹配时间，最终匹配得到一张视差图；
步骤4，计算视差图中匹配点在左摄像头坐标系下的三维坐标(x,y,z)，获取其高度与水平距离，并与设定阈值比较，判断障碍物是否存在；
此处使用opencv视觉库中的cvfindcon—tours轮廓检测函数检测物体的轮廓，并用其外接矩形框标记，如图6所示。根据其中的白色像素点，利用双目视觉测距原理计算出物体相对左摄像头坐标系下的三维坐标。
对障碍物的判别算法：首先随机提取视差图中外接矩形框内物体上若干个白色像素点，计算其位于左摄像机坐标系下的三维坐标(x，y，z)，其中x和z分别表示物体相对于左摄像头的高度距离与水平距离，y则表示物体相对于左摄像头光心的左右偏移距离，然后根据设定的阈值判断矩形框内的物体是否为障碍物。具体步骤如下：
步骤4.1，随机提取视差图中外接矩形框内若干白色像素点i＝1,2,3,…，计算其三维坐标di(xi,yi,zi)，并按照水平距离zi(即三维坐标中的zi值)的大小排序；
步骤4.2，因为提取和计算过程中存在误差，所以可能存在异常数据，因此取所有白色像素点水平距离zi的中位数z0，并设定阈值φ，对于所述的白色像素点，如白色像素点i的水平距离zi满足|zi-z0|＞φ，则将白色像素点i作为异常点剔除，剩余白色像素点构成集合i；
步骤4.3，计算集合i中白色像素点水平距离zi的平均值
步骤4.4，设定高度阈值xd和水平距离阈值zd，当满足xmax＞xd，且
步骤5，如判断前方无障碍物，则使机器人按照规划的路径行进；如判断前方有障碍物，则令机器人停止动作，并获取障碍物的参数信息，具体如下：
步骤5.1，将障碍物的外接矩形框的宽度作为障碍物的宽度，记为q，外接矩形框的中心作为障碍物中心点，记为o；
步骤5.2，通过深度检测获取(即通过双目摄像头检测)o的三维坐标di(x,y,z)，其中y表示中心点相对于左摄像头光心的偏移距离li；获取外界矩形框中最左侧的一个白色像素点在左摄像头坐标系下的坐标(xl,yl,zl)；
步骤5.3，根据障碍物的宽度计算出障碍物中心点o的纵坐标，即o点相对于左摄像头光心的偏移距离lo：
步骤5.4，由于双目摄像头安装在机器人的中轴线上，因此障碍物中心点o相对于机器人中轴线的偏距d为：
其中d>0表示偏右，d<0表示偏左，d＝0表示居中。
步骤6，进行避障路径规划
步骤6.1，设定障碍物的长度为w；
在障碍物检测过程中，已经能够比较精确地获得障碍物的宽度尺寸及其相对于机器人的方位信息，但是较难得到障碍的长度尺寸，一般假设其长度为w；
步骤6.2，当障碍物相对于机器人偏左时，为了能更快绕过障碍物，根据障碍物的位置及实验室环境的不同，设置一个恒定的安全距离，机器人右转角度θ并沿平行安全距离直行一段距离x，机器人中心与障碍物中心的连线与规划路径垂直时，左转角度2θ并直行距离x，使其回归规划路径；障碍物偏右和居中时同理，障碍物偏右时机器人左转，障碍物居中时随机选择左转或右转，不再赘述；示意图如图7所示：在图中，l为机器人的身宽；θ和2θ为机器人的旋转角度；d为障碍物相对机器人的距离；yl为障碍物左侧边界相对于机器人中心的偏移距离；d为考虑存在的误差和安全问题所增加的补偿距离(一般取d＝yl)。
对于机器人的避障过程，有：
至此则可完成基于双目视觉的障碍检测和避障路径规划。
本发明中，预规划的实时步态控制算法如图8所示，步骤如下：
步骤1’，建立双足机器人运动学坐标系模型
如图9的(a)和(b)所示，本实施例中的双足机器人全身共有20个舵机，其中在机器人腿部分布有多处关节，使每条腿部具有6个自由度，即：裸关节具有向前和侧向两个自由度，分别称为踝关节的前向关节和侧向关节(图9中对应的舵机为15、17和16、18)；膝关节一个前向自由度(图9中对应的舵机为13、14)，称为膝关节的前向关节；髋关节具有三个自由度，包括前向、侧向及转弯自由度，分别称为髋关节的前向关节、侧向关节和转弯关节(图9中中对应的舵机为7、9、11和8、10、12)。机器人上半身中，肘关节具有一个自由度(图9中对应的舵机为5、6)；肩关节有两个自由度(图9中中对应的舵机为1、3和2、4)，颈关节一个自由度(图9中对应的舵机为19)，头部一个自由度(图9中中对应的舵机为20)。
双足机器人的关键是由活动式连接的杆件构成的，相邻的两个杆件连接处称为一个关节，如图11所示。所述的杆件是指机器人躯干上的部分，例如手肘处作为一个关节，其小臂、大臂分别视为一个杆件；在双足机器人身上，相邻的杆件之间活动式连接，使相邻的杆件之间可以发生角度变化，例如相邻的杆件连接处构成转轴式结构。所述的舵机安装在每个关节处，用于驱动关节旋转，具体请参见前文。
该步骤的思路是，在机器人的每个关节处构件坐标系，通过坐标系的变换来实现关节处的两个杆件上坐标的变换；对于本方案的多关节机器人，每个关节由两个杆件组成，则机器人可看作是多杆件系统，则多次利用坐标变换，就可以建立坐标系之间的关系，从而构成运动学坐标系模型，具体步骤如下：
步骤1.1’，对机器人的各个关节进行编号，对于关节i，确定该关节的原点，原点确定的原则为：
当关节i的轴线(关节的旋转轴线)与关节i+1的轴线相交时，取交点作为关节i的原点，当关节i的轴线与关节i+1的轴线异面时，取两条轴线公垂线与关节i+1轴线的交点作为关节i的原点；当关节i的轴线与关节i+1的轴线平行时，取关节i+1轴线与关节i+2轴线的公垂线与关节i+1轴线交点作为关节i的原点，记为oi。
步骤1.2’，确立关节i处坐标系的各个坐标轴xi、yi、zi，确定原则为：
zi坐标轴与关节i+1的轴线重合，xi坐标轴沿i关节轴线、i+1关节轴线的公垂线并指向i+1关节，yi坐标轴按右手法则确定。
如图10所示，本方案中将双足机器人的左脚膝关节的坐标系o0-x0y0z0为参考坐标系，左脚踝关节坐标系为o6-x6y6z6,右脚膝关节的坐标系为o12-x12y12z12。后两者为虚拟坐标系，仅为计算结果提供辅助和参考。
按照上述方法，在机器人各个关节处建立了坐标系。
步骤1.3’，根据在各个关节处建立的坐标系，相邻的两个坐标系oi与oi-1(即关节i处的坐标系和关节i-1处的坐标系)相比，存在的差距为
上式中，s表示坐标系oi-1到坐标系oi的距离，c表示坐标系oi-1到坐标系oi的角度，ai-1表示xi到xi+1的距离，di表示xi到坐标系oi的测量距离，θi表示从xi到xi-1转动的角度。由于一个关节i对应一个杆件，因此通过上述矩阵可求得相邻杆件之间的运动学关系。
通过上面的变换矩阵得到相邻的两个坐标系之间的运动学关系，由此得到左脚踝关节坐标系o6-x6y6z6与参考坐标系o0-x0y0z0之间的运动学关系，同理可得到其他关节坐标系与参考坐标系之间的运动学关系。
本方案中，将每个关节处的坐标系与参考坐标系之间的变换矩阵作为该关节与参考坐标系所在关节之间的数学模型，所有数学模型构成双足机器人运动学坐标系模型。
步骤2’，对双足机器人的姿态进行规划
在本方案中，人形机器人的姿态规划则是建立在所建立的机器人运动学坐标系模型上的，即是以各关节在步骤1所建立的运动学坐标系模型中的坐标来表示，具体实施步骤如下：
步骤2.1’，设计双足机器人行走步态流程
为使规划易于进行，将前向行走步态设计分为重心右移(先是右腿支撑)、左腿抬起、重心移到双腿中间、重心左移、右腿抬起、右腿放下、重心移到双腿间八个阶段，示意图如图12所示。
本方案的步态规划是通过摆动腿(抬起的腿)前摆改变重心的，在摆动腿前摆时，身体重心从双腿中心移到支撑腿上，因此身体先要向支撑腿(与地面接触的腿)一侧扭动来移动重心，即通过髋关节的侧向关节来调整重心；为了防止在侧扭时抬腿而造成身体向摆动腿一侧倾倒、步态设计中将侧扭和抬腿分步设计。以先抬起左脚为例，机器人一次行走时的关节步态规划如下：
先通过右腿的髋关节、裸关节的侧向关节进行右侧扭来调整重心，侧扭到位后保持、然后抬起左腿，具体为：左腿的裸关节、膝关节和髋关节的前向关节弯曲，以实现左腿抬起；左腿放下为相反动作，即先通过右腿的髋关节、裸关节左侧扭，然后左腿的裸关节、膝关节和髋关节的前向关节弯曲(弯曲方向与抬起时相反)，此时左脚落地；右腿抬起、落地同理。
步骤2.2’，规划机器人行走时各个关节的姿态
前向运动即是机器人腿部向前或先后摆动，侧向运动即是机器人腿部向左或向右摆动，对前向与侧向运动进行分别建模，完成机器人的前向行走规划。
前向运动由四个侧向关节和六个前向关节(左右裸关节、髋关节的侧向关节，左右裸关节、膝关节、髋关节的前向关节)的协调运动来实现，通过侧向关节的运动来移动机构的重心，双腿的前向关节的协调运动使机器人向前行走，具体实现如下：
对机器人双腿的关节旋转角度进行定义，θ1代表的是左腿从下到上的偏转角度；θ2代表的是左腿从上到下的偏转角度；θ3代表的是右腿从下到上的偏转角度；θ4代表的左腿从上到下的偏转角度，示意图见图13；则在前向运动中，由θ1、θ2、θ3、θ4运动实现。
先假设髋关节的侧向不发生变化，也就是四个关节(即左右腿的髋关节的两个转弯关节和侧向关节)的侧向偏转角度均为0，则求解行走轨迹步骤如下，以髋关节与踝关节为例：
(1)绘制出髋关节与裸关节随时间改变而变化的曲线图(即建立在步骤1坐标系上的二维曲线图)，由此获得髋关节、裸关节的运动学轨迹；
(2)与人类行走轨迹进行对比，按照人类行走过程的受力情况明确机器人髋关节、裸关节运动受力的关键点，然后通过三次样条插值的方式，对所有关键点进行插值处理；
(3)分别将髋关节、裸关节处理后的关键点与步骤(1)中髋关节、裸关节运动学轨迹中的多项式进行拟合，获得髋关节与裸关节的平滑轨迹；则该轨迹即为髋关节与裸关节每一步行走后形成的轨迹。
按照相同的方法，可获得机器人双腿上其他关节在行走时形成的轨迹；将双足机器人一次行走(左脚从抬起到放下为一次行走)过程中所涉及的关节的轨迹作为规划姿态，则获得规划的姿态后，通过关节处的舵机驱动机器人按照规划的姿态进行行走。
在机器人每次落脚时会处于一定的不稳定状态，因此还需要进行稳定性判断，根据稳定性判断的结果，对机器人的落脚点进行反馈调节。
步骤3’，进行姿态规划平稳性判断
地面反作用力对脚部的影响是复杂的，本方案中将其简化成有任意一点p，而地面的反作用力对脚部的贡献可以等价为一个力r与力矩m。而零力矩点的定义就是在地面上存在一点p，使得与地面平行轴方向的、由惯性力(f＝ma)与重力(g＝mg)所产生的净力矩为零的点。图14给出了机器人脚部所受作用力分布的例子，沿足底分布的负载具有相同的方向，他们等效于一个合力n，合力n所通过的在足底上的这个作用点被称为平稳点。该点可以理解为重力和惯性力的合力在地面上的投影，而且在这一点上合力的力矩在水平方向为零。
根据步骤2’中的规划姿态，在机器人每次准备落脚时进行平稳性计算，判断当前机器人是否满足平稳性要求，判断依据为机器人的平稳点是否在设定的范围内，如在设定的范围内，则不干预，使机器人按照规划姿态进行行走，否则暂停行走，通过机器人的控制器调整机器人腿部的关节(髋关节、膝关节、裸关节)的姿态后，重新计算平稳性，直至平稳性满足要求，其中所述的平稳点计算方法为：
计算机器人的质心位置：
其中，n表示机器人全身关节的数量，本实施例中n＝20；mi表示第i个关节的质量，
计算平稳点w：
w＝-a+com
其中，
上式中，ts为采样时间，comi，comi-1，comi-2分别为第i次、第i-1次和第i-2次采样时机器人的质心位置，com为机器人站立不动时的质心位置。
判断平稳点w的位置，若w在设定的范围内，则认为机器人此步行走稳定，否则不稳定。本实施例中，所述的设定范围为机器人抬脚后脚中心在地面投影点前后-5～5cm范围。
综上所述，本方案通过对人体步行轨迹的模仿来预先设计双足机器人的步态规划，按照先姿态规划后稳定性判断，预先规划出机器人完整的姿态，实现实时步态控制。
实际应用时，首先根据机器人要到达的目的地进行路径的规划，机器人行走过程中，利用预规划的实时步态控制算法控制机器人的行走，行走的同时通过基于双目视觉的避障算法进行障碍物的检测和避障；两种方法结合使用，能够对人形机器人的步态进行精确控制，从而使人形机器人更好地实现避障和步态调整的功能，提高了人形机器人对路面的适应能力。
- 还没有人留言评论。精彩留言会获得点赞！