(19)中华人民共和国国家知识产权局 ▣▣ (12)发明专利 ▣ (10)授权公告号CN103085070B (45)授权公告日2015.03.11 (21)申请号201310014887.5 (22)申请日2013.01.15 (73)专利权人上海交通大学 地址200240上海市闵行区东川路800号 (72)发明人曹其新杨扬陈培华 (74)专利代理机构上海汉声知识产权代理有限 公司31236 代理人郭国中 (51)1nt.Cl. B25J9/16(2006.01) 审查员薛超志 权利要求书2页说明书4页附图1页 (54)发明名称 面向复杂地形的四足机器人运动规划方法 (57)摘要 本发明提供一种面向复杂地形的四足机器人 运动规划方法，根据四足机器人的运动目标与传 感器检测到的局部环境信息和地形信息确定四足 机器人在当前时刻的运动方向，生成四足机器人 在笛卡尔空间的局部运动目标：根据四足机器人 在笛卡尔空间的局部运动目标与传感器检测到的 地形信息，利用快速随机扩展树算法，在四足机器 人构型空间生成运动路径序列，依照该运动路径 序列运动，直至四足机器人到达局部运动目标； 重复以上步骤，直至四足机器人到达设定目标点。 本发明通过将笛卡尔空间内的机体运动规划与构 型空间内的各关节构型规划相结合，根据四足机 器人当前环境，生成四足机器人各关节的运动路 径序列，保证四足机器人在复杂地形上安全平稳 口的自主运动。 270990901
CN103085070B 权利要求书 1/2页 1.一种面向复杂地形的四足机器人运动规划方法，其特征在于，包括以下步骤： 步骤一：根据四足机器人的运动目标与传感器检测到的局部环境信息和地形信息确定 四足机器人在当前时刻的运动方向，生成四足机器人在笛卡尔空间的局部运动目标：具体 包括以下步骤： 首先，利用激光传感器检测四足机器人前方环境，将四足机器人当前位置与目标点连 线，若该连线上无障碍物，则运动方向日为该连线方向，即6=0：否则根据障碍物情况， 搜索与当前连线方向所成夹角最小的偏转角，并以该方向为运动方向日： 其次，立体视觉传感器对四足机器人前方的地面进行立体建模，根据运动方向日与立 体视觉传感器得到的四足机器人前方的地形模型，计算在四足机器人机体坐标系下前方的 平均高度Z,从而得到四足机器人机体坐标系下笛卡尔空间的局部目标点坐标 y ,其中， x1=3·sin(0),y1=3·cos(0); 步骤二：根据四足机器人在笛卡尔空间的局部运动目标与传感器检测到的地形信息， 利用快速随机扩展树算法，在四足机器人构型空间生成运动路径序列，依照该运动路径序 列运动，直至四足机器人到达局部运动目标； 步骤三：重复步骤一与步骤二，直至四足机器人到达设定目标点。 2.根据权利要求1所述的面向复杂地形的四足机器人运动规划方法，其特征在于，步 骤二具体为： 首先，设四足机器人的12个转动关节为J,i=1,2,3..12,各关节转角范围为 [L,U],即各关节转角必须满足L:≤J≤U;设在规划初始时刻t。各关节转角为J,规 划过程中各关节转角最大步长为δ，：四足机器人采用交叉腿运动方式，即任意时刻，以相 对的两条腿为支撑腿，对另外两条腿进行运动规划：对于下一时刻七，对运动腿的关节转角 J在[max(L,J-6),min(U,J+6】范围内随机采样，对得到的一组构型J,利用正运 动学方程计算两条运动腿末端在四足机器人机体坐标系下的笛卡尔坐标P,1=1,2; 其次，立体视觉传感器得到的地形模型C为点云模型，对P,中的每一点，若存在C,满 足C∈C且.dist(C,P)<,其中dist(C,P):为C,P:两点间的欧氏距离，ζ为一近似等 于零的正数，则表明在该组构型下，四足机器人的两条运动腿末端与地面产生恰当的接 触，该组构型为一有效构型，则继续以当前构型为起点规划下一时刻构型，直至四足机器人 机体达到步骤一获得的局部目标点。 3.根据权利要求1所述的面向复杂地形的四足机器人运动规划方法，其特征在于，所 述四足机器人的四条支撑腿相互独立，且每个支撑腿具有四个自由度，分别为髋关节在相 互垂直的两个方向上的转动，膝关节的转动与踝关节的转动。 4.根据权利要求1所述的面向复杂地形的四足机器人运动规划方法，其特征在于，所 述激光传感器水平安置于四足机器人机体上，用来检测四足机器人前方5米内的障碍物。 5.根据权利要求1所述的面向复杂地形的四足机器人运动规划方法，其特征在于，视 觉传感器安置于四足机器人机体上，与水平方向成30度夹角向下俯视，用来对四足机器人 2
CN103085070B 权利要求书 2/2页 前方0.3米-3米范围内的地面进行立体建模。 t
CN103085070B 说明书 1/4页 面向复杂地形的四足机器人运动规划方法 技术领域 [0001]本发明涉及机器人运动规划领域，具体地，涉及一种面向复杂地形的四足机器人 运动规划方法。 背景技术 [0002]作为一种机电一体化系统，机器人吸引了越来越多研究者的关注。机器人的移动 方式主要分为轮式移动与腿式移动两种。轮式移动机器人在控制上相对简单，主要应用于 平坦地形或近似平坦地形。腿式移动机器人在控制上相对复杂，但具有优良的越障性能，可 以应对复杂地形，例如丛林或丘陵环境。 [0003]腿式移动机器人按照机械腿的数目，可以分为双足、四足、六足、八足机器人等。其 中，四足机器人作为对常见哺乳动物的仿生，一方面具备良好的负重能力，另一方面在机构 上比六足、八足机器人更为简洁紧凑。现有技术中公开了多种四足机器人的机械结构，如中 国专利申请号201010279765.5的发明专利公开了一种基于混联机构的全方位四足步行移 动装置，中国专利申请号201110106481.0的发明专利公开了一种基于仿生设计的四腿机 器人机构，中国专利申请号201110314606.9的发明专利公开了一种新型并联腿结构四足 步行器，中国专利申请号201110314607.3的发明专利公开了一种液压驱动空间混联腿结 构四足机器人。 [0004]机器人运动规划技术是针对机器人的机械结构建立相应运动学模型，根据运动任 务与运动环境，在机器人构型空间规划出一条运动路径序列，该序列上的各点对应机器人 的一个具体构型。机器人的运动规划是机器人自主移动的前提条件。 [0005]经过检索，中国专利申请号201110263058.1的发明专利公开了一种双足机器人 的运动规划系统，该系统参照人类跌倒后的站起方式，对机器人对应关节进行运动规划，主 要目的是实现跌倒后的平稳起立。然而，现有技术普遍是针对四足机器人的机械结构，在运 动规划方面，目前还没有针对四足机器人的运动规划方法，尤其是面向非平坦复杂地形的 运动规划方法。 发明内容 [0006]针对现有技术中的缺陷，本发明的目的是提供一种面向复杂地形的四足机器人运 动规划方法，本发明利用激光与立体视觉传感器对四足机器人的运动环境进行检测与建 模，针对局部地形生成四足机器人机体的运动方向与四足机器人各关节的构型序列，保证 四足机器人在复杂地形上的安全平稳前进。 [0007]根据本发明的一个方面，提供一种面向复杂地形的四足机器人运动规划方法，包 括以下步骤： [0008]步骤一：根据四足机器人的运动目标与传感器检测到的局部环境信息和地形信息 确定四足机器人在当前时刻的运动方向，生成四足机器人在笛卡尔空间的局部运动目标。 具体包括以下步骤：
CN103085070B 说明书 2/4页 [0009]首先，利用激光传感器检测四足机器人前方环境，将四足机器人当前位置与目标 点连线，若该连线上无障碍物，则运动方向0为该连线方向，即0=0；否则根据障碍物情 况，搜索与当前连线方向所成夹角最小的偏转角，并以该方向为运动方向日。 [0010]其次，立体视觉传感器对四足机器人前方的地面进行立体建模，根据运动方向0 与立体视觉传感器得到的四足机器人前方的地形模型，计算在四足机器人机体坐标系下前 书7 方的平均高度z1,从而得到四足机器人机体坐标系下笛卡尔空间的局部目标点坐标马 其中，x1=3·sin(0),y1=3·cos(0)。 [0011]步骤二：根据四足机器人在笛卡尔空间的局部运动目标与传感器检测到的地形信 息，利用快速随机扩展树算法，在四足机器人构型空间生成运动路径序列，依照该运动路径 序列运动，直至四足机器人到达局部运动目标。 [0012]步骤三：重复步骤一与步骤二，直至四足机器人到达设定目标点。 [0013] 优选地，步骤二具体为： [0014] 首先，设四足机器人的12个转动关节为J,i=1,2,3..12,各关节转角范围为 [L,U],即各关节转角必须满足L≤J1≤U,;设在规划初始时刻t。各关节转角为J,规 划过程中各关节转角最大步长为6，：四足机器人采用交叉腿运动方式，即任意时刻，以相 对的两条腿为支撑腿，对另外两条腿进行运动规划：对于下一时刻七，对运动腿的关节转角 J在[max(L,J-6),min(U,J+8)]范围内随机采样，对得到的一组构型J,利用正运 动学方程计算两条运动腿末端在四足机器人机体坐标系下的笛卡尔坐标P,1=1,2。 [0015]其次，立体视觉传感器得到的地形模型C为点云模型，对P:中的每一点，若存在C, 满足C∈C且dist(C,P)<ζ，其中dist(C,P),为C,P,两点间的欧氏距离，？为一近似 等于零的正数，则表明在该组构型J下，四足机器人的两条运动腿末端与地面产生恰当的 接触，该组构型为一有效构型，则继续以当前构型为起点规划下一时刻构型，直至四足机器 人机体达到步骤一获得的局部目标点。 [0016] 优选地，该四足机器人的四条支撑腿相互独立，且每个支撑腿具有四个自由度，分 别为髋关节在相互垂直的两个方向上的转动，膝关节的转动与踝关节的转动。 [0017]优选地，该激光传感器水平安置于四足机器人机体上，用来检测四足机器人前方5 米内的障碍物。 [0018]优选地，该视觉传感器安置于四足机器人机体上，与水平方向成30度夹角向下俯 视，用来对四足机器人前方0.3米-3米范围内的地面进行立体建模。 [0019]本发明的四足机器人的运动规划分为两个层次：第一层是四足机器人机体在笛卡 尔空间的局部运动规划，根据四足机器人的运动目标与传感器检测到的局部环境信息确定 四足机器人在当前时刻的运动方向，生成四足机器人在笛卡尔空间的局部运动目标：第二 层是四足机器人各关节的构型规划，根据四足机器人在笛卡尔空间的局部运动耳标与传感 器检测到的地形信息，利用快速随机扩展树算法，在四足机器人构型空间生成运动路径序 列，该序列上的各点对应四足机器人的一个具体构型，各具体构型应满足四足机器人各关 节的转动角度限制，并适应当前地形。 t
CN103085070B 说明书 3/4页 [0020] 与现有技术相比，本发明具有如下的有益效果：本发明通过将笛卡尔空间内的机 体运动规划与构型空间内的各关节构型规划相结合，根据四足机器人当前环境，生成四足 机器人各关节的运动路径序列，保证四足机器人在复杂地形上安全平稳的自主运动。 附图说明 [0021]通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、 目的和优点将会变得更明显： [0022]图1为本发明的面向复杂地形的四足机器人运动规划方法应用的四足机器人的 结构示意图。 [0023] 图中：1为四足机器人机体，2为激光传感器，3为立体视觉传感器，4为支撑腿。 具体实施方式 [0024]下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术 人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术 人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明 的保护范围。 [0025]请参阅图1，其为本发明所应用的四足机器人结构示意图，四足机器人包括：四足 机器人机体1、激光传感器2、立体视觉传感器3和四个相互独立的支撑腿4。激光传感器2 水平安置于四足机器人机体1上，用来检测四足机器人前方5米内的障碍物。视觉传感器 3安置于四足机器人机体1上，与水平方向成30度夹角向下俯视，用来对四足机器人前方 0.3米-3米范围内的地面进行立体建模。四条支撑腿4相互独立，每个支撑腿具有四个自 由度，分别为髋关节在相互垂直的两个方向上的转动，膝关节的转动与踝关节的转动。 [0026]针对上述的四足机器人，本发明提供一种面向复杂地形的四足机器人运动规划方 法，包括以下步骤： [0027]步骤一：根据四足机器人的运动目标与传感器检测到的局部环境信息和地形信息 确定四足机器人在当前时刻的运动方向，生成四足机器人在笛卡尔空间的局部运动目标。 [0028]如图1所示，激光传感器2水平安置于四足机器人机体1上，用来检测四足机器人 前方5米内的障碍物，根据四足机器人运动目标与前方障碍物情况，确定四足机器人的运 动方向日。立体视觉传感器3安置于四足机器人机体1上，与水平方向成30度夹角向下俯 视，用来对四足机器人前方0.3米-3米范围内的地面进行立体建模。具体方法为： [0029]首先，利用激光传感器检测四足机器人前方环境，将四足机器人当前位置与目标 点连线，若该连线上无障碍物，则运动方向θ为该连线方向，即日=0：否则根据障碍物情 况，搜索与当前连线方向所成夹角最小的偏转角，并以该方向为运动方向日。 [0030] 其次，立体视觉传感器对四足机器人前方的地面进行立体建模，根据运动方向θ 与立体视觉传感器得到的四足机器人前方的地形模型，计算在四足机器人机体坐标系下前 方3米处的平均高度1，从而得到四足机器人机体坐标系下笛卡尔空间的局部目标点坐标 ,其中，x1=3·sin(0),y1=3·cos(θ)
CN103085070B 说明书 4/4页 [0031]步骤二：根据四足机器人在笛卡尔空间的局部运动目标与传感器检测到的地形信 息，利用快速随机扩展树算法，在四足机器人构型空间生成运动路径序列，依照该运动路径 序列运动，直至四足机器人到达局部运动目标。 [0032]根据步骤一获得的局部目标点与立体视觉传感器得到的地形模型，利用快速随机 扩展树算法得到四足机器人构型空间内的运动路径序列，该序列上的各点对应四足机器人 的一个具体构型。具体方法为， [0033]首先，设四足机器人的12个转动关节为J1,i=1,2,3..12,各关节转角范围 为[L,U],即各关节转角必须满足L:≤J≤U::设在规划初始时刻t。各关节转角为 ,规划过程中各关节转角最大步长为δ：；四足机器人采用交叉腿运动方式，即任意 时刻，以相对的两条腿为支撑腿，对另外两条腿进行运动规划（例如以左前腿与右后腿 为支撑腿，规划右前腿与左后腿的运动)；对于下一时刻t,对运动腿的关节转角J在 [max(L,J-6),min(U,J+8)]范围内随机采样，对得到的一组构型J,利用正运动学 方程计算两条运动腿末端在四足机器人机体坐标系下的笛卡尔坐标P,1=1,2。 [0034]其次，立体视觉传感器得到的地形模型C为点云模型，对P:中的每一点，若存在C, 满足C∈C且dist(Ck,P)<?,其中dist(C,P);为C,P;两点间的欧氏距离，？为一近似 等于零的正数，则表明在该组构型J下，四足机器人的两条运动腿末端与地面产生恰当的 接触，该组构型为一有效构型，则继续以当前构型为起点规划下一时刻构型，直至四足机器 人机体达到步骤一获得的局部目标点。 [0035]步骤三：重复步骤一与步骤二，直至四足机器人到达设定目标点。 [0036]以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述 特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影 响本发明的实质内容
CN103085070B 说明书附图 1/1页 3 3 ⊙ 4 4 4 3 图1