一种人形机器人北斗导航自适应规划技术研究

李 峰，岳 云，张 巍，李 宁

（1.中原工学院信息商务学院机械工程系，河南 郑州 450007；2.郑州宇通集团有限公司，河南 郑州 450007）

1 引言

随着社会的发展，机器人特别是人形机器人智能化程度不断提高，人形机器人由于结构更接近于人类，可协助人类做更多的事情，比如在车辆无人驾驶、特殊环境生产、航空航天方面，人形机器人的使用可以节约大量的人力、物力、财力，大大提高社会生产力。人类期望机器人能够扮演更加智能的角色，能够更加稳定的完成任务，能够在更广大的范围内完成角色，自适应性更高。随着5G移动网络技术的成形，可以通过网络技术确定机器人的作业空间和运动环境，协调机器人的运动，控制机器人的工作，提高机器人的智能化程度，但提高机器人智能化程度的基础是解决机器人在不同的导航环境下进行路径规划选择问题。机器人运动状态管理，机器人的运动路线决策，是自动化控制技术、人工智能技术、卫星导航技术等高科技技术的充分融合[1-3]。文献[4-7]对人形机器人动作方式、结构方面做了相关的研究，文献[8-11]在机器人导航方面方面做了相关研究，但在人形机器人卫星导航方面却未有研究发现。而人形机器人的卫星导航，特别是在我国北斗导航系统下的导航问题的研究一方面可以提高人形机器人的智能程度，一方面可以进一步推广普及我国自主研发的北斗导航系统。

针对人形机器人自适应程度低，机器人行走智能化程度不高，特提出了利用自适应控制法的来实现移动机器人行走路线及姿态的自适应规划。通过对人形机器人行走路线及位姿的优化设计及自主决策，达到了机器人通过卫星导航，自主决定运动路线及运动策略，实现了预定目标，缩短了控制时间，提高了控制效率，增强了人形机器人控制的智能化程度。

2 人形机器人导航自适应控制系统设计

2.1 人形机器人

人形机器人种类繁多，根据动力源可分为液压驱动、电机驱动等，根据自由度也可分为多种自由度的机器人。本课题所用机器人为九自由度舵机驱动的人形机器人，如图1所示。机器人转角分辨率10，直线行走分辨率50mm，整体500g，采用2000ma锂电续航，续航能力强。机器人结构匀称，更符合人形机构。现在人形机器人的控制主要依靠前期对机器人进行程序设定，将设计好的动作输入机器人内部存储器，依靠固化程序实现机器人完成特定的简单动作，人形机器人自适应程度不高，智能化程度较低。

图1 九自由度人形机器人Fig.1 Nine Degree of Freedom Humanoid Robot

2.2 九自由度人形机器人自适应导航系统硬件设计

为了能更好地对九自由度人形机器人进行导航，提高对人形机器人导航的智能化程度，增强机器人的自适应性，特对原九自由度人形机器人自导航主动控制系统进行优化改进，增加导航航向路线自适应规划功能，原机器人导航系统虽然能够在完成目的地坐标输入后自动生成导航轨迹，但是航向调整智能化程度不高，人性化程度较低；另外，机器人在经过导航后，到达目的坐标区域后不能很好地确定是否达到目标，不能再做最终的位姿调整，导航精度低。为此，特对系统进行优化设计，以提高系统的自适应性。人形机器人自适应导航系统硬件主要包括人形移动机器人，自导航主动控制器，北斗导航仪，监控微机。系统中机器人所在位置通过北斗导航仪获得，通过人机界面输入中央处理器，通过中央处理器处理后自动生成导航路线、轨迹，又通过中间控制器发出信号控制机器人实现导航的路线、轨迹，机器人在运行过程中可通过视觉采集系统将信号通过微机直观地展示给监控人员，以便于对一些特殊情况进行处理，机器人本身也可搭载障碍物、深坑信息采集裙，以便及时对特殊路径做出规划。本试验在不改变原有就自由度人形机器人自导航主动控制试验平台的基础上对机器人自导航主动控制原理进行改进，以提高机器人的自适应程度。

人形机器人自适应主动控制系统中，北斗定位系统，自适应规划系统和被导航机器人为三大基本组成部分，如图2所示。北斗导航定位系统是机器人自导航的依据，课题使用的是ICE610冰河北斗导航仪，可以完成对人形机器人的北斗系统定位。自适应规划系统主要由人机界面，中央处理器和中间控制器组成，人机界面主要采用显控SK—070AS 7寸工业触摸屏，中央处理器采用比较常见的西门子S7—200PLC完成，它与工控微机和障碍物及深坑探测模块连接，完成信息通讯。中间控制器为自己设计研发，主要完成中央处理器输出信号和人形机器人控制信号的转换，实现人形机器人的自适应主动导航。人形机器人为九自由度人形机器人，通过QROBRT软件进行程序输入，完成机器动作分解程序设定。

图2 人形机器人自适应主动控制系统图Fig.2 Humanoid Robot Adaptive Active Control System Picture

3 人形机器人导航算法设计

3.1 九自由度人形机器人运动学模型

为得到九自由度人形机器人的自导航运动控制算法，需要分析机器人的行走移动轨迹，为此需要对机器人脚部移动状况进行分析研究，建立机器人移动模型，如图3所示。

图3 机器人移动模型Fig.3 Robot Mobile Model

图中：α—机器人两脚与机器人纵轴yROBERT之间的夹角，为转向角；ψ—yROBERT与地面坐标系yEARTH之间的夹角，为航向角；yROBERT—两脚连线的垂直中心线方向；yEARTH—地球正北方向；（xc，yc）—机器人当前坐标；（xg，yg）—机器人目的地坐标；ψh—行走过程中的机器人的航向角。由现在的坐标和目的地坐标得到：

为便于控制，令t=n*T可将△T设置为控制间隔时间T，则：

为简化公式，令：

式中：V—机器人直线行走速度。因为人形机器人每直线行走信号行走理论值由自身结构决定，为△S，给定机器人直线行走信号数nx，则机器人直线行走控制方程为：

因为转向角可以控制，令转向角：

式中：K—误差系数，可初定为1；nz—机器人转向信号数；△α—单个转向信号机器人可转角度。

式中：ψc—初始姿态；ψg—终点设定姿态；ψh—航向角；nz—机器人从初始姿态达到航向所生成的航向调整转向信号数；nq—机器人从航向角达到终点设定姿态所生成的姿态调整转向信号数；△α—单个转向信号机器人可转角度。

3.2 机器人自适应路径规划控制过程设计

为了提高人形机器人的自适应性，系统设计该人形机器人北斗导航自适应主动控制不但能够在输入机器人现在所处地点北斗坐标和目标地点北斗坐标后自动生成导航路线，发出指令，控制机器人自动完成任务，而且能够自适应选择最佳人形机器人生成路径的姿态，以便优化动作程度，提高人工智能程度。机器人自适应主动控制系统在输入相关信息后，先根据所处位置和目标位置自动生成行走路线；然后根据自身位姿和所需航向，自适应选择左调、右调自身姿态，达到所需航向；接着，根据航向和前期生成路线移动，完成移动轨迹；再根据最终需要姿态自适应拟人化左调、右调姿态。而后，根据北斗导航信息校核距离误差和姿态误差，自适应调整位置、姿态，完成任务，如图4所示。具体步骤如下：

图4 人形机器人北斗导航自适应主动控制流程图Fig.4 Humanoid Robot Beidou Navigation Adaptive Active Control Flowchart

（1）输入（xc，yc）、（xg，yg）、ψc、ψg、TZ、Tx、△α、△s、K、Kx、Kq、Kz，其中：（xc，yc）—机器人初始点国家2000坐标；（xg，yg）—机器人目标点国家2000坐标；ψc—机器人初始位置姿态；ψg—机器人到达目标位置后需要确定的姿态；K—整体调整误差系数；Kx—直线调整误差系数；Kq—姿态调整误差系数；Kz—航向调整误差系数；nz—给定机器人转向信号数；nx—给定机器人直线信号数；nq—给定机器人姿态信号数；△α—单个信号机器人调整角度；△s—单个信号机器人调整位移；TZ—转向信号信号时间间隔；Tx—直线行走信号时间间隔。

（2）机器人行走路线主要参数nz、nx、nq规划。根据前文中确定参数和式α=Kz×（ψg-ψc），确定人形机器人行走时所需器人行走时所需转角α，又ɑ=Kz×（ψg-ψc）=nz×△α，根据前期测定的机器人每信号调整角度△α，得需给定机器人转向信号数nz=α/△α，利用STL语言将公式程序化（见图5中nz规划程序段）；

根据前文中确定参数和S=Kx×（（yg-yc）∧2+（xg-xc）∧2）∧0.5，确定人形机器人行走时所需行走位移S，根据前期测定的机器人每信号调整位移△s，得需给定机器人行走信号数nx=S/△s，利用STL语言将公式程序化（见图5中nx规划程序段）；

根据前文中确定参数和ɑq=Kq×（ψg-ψc）=nq×△α，根据前期测定的机器人每信号调整姿态△α，得需给定机器人姿态调整信号数nq=α/△α，利用STL语言将公式程序化（见图5中nq规划程序段）；

（3）控制器发出机器人转向信号nz次，每次间隔TZ，机器人完成航向调整，控制原理见图5中航向调整程序段；

（4）控制器发出机器人直行信号nx次，每次间隔Tx，机器人完成位移运动，控制原理见图5中前行程序段；

（5）控制器发出机器人姿态调整信号nq次，每次间隔TZ，机器人完成姿态调整，控制原理见图5中姿态调整程序段；

以上为前期人形机器人行走路线自适应生成及动作过程，以下为人形机器人校核位置、姿态，自适应调整位置姿态过程。

（6）根据人形机器人现在所在地（xc，yc）、ψc，比较距离，看是否到达目的坐标范围。如果达到目标范围，SS≤R，比较姿态，看是否到达目的坐标姿态范围δ，控制原理见图5中姿态再调整程序段。

图5 人形机器人北斗导航自适应主动控制主要原理Fig.5 Humanoid Robot Beidou Navigation Adaptive Active Control Main Principle

①如果达到目标位移范围后姿态调整。

如果姿态达到：|ψc－ψc|≤δ，停止，退出程序；

如果姿态达到：|ψg－ψc|>δ，需调整姿态。

接下来：

如果ψg>ψc，自适应生成给定机器人转向信号数nq，控制器发出机器人左转向信号nq次，每次间隔Tz，机器人完成姿态调整；

如果ψg<ψc，自适应生成机器人转向信号数nq，控制器发出机器人右转向信号nq次，每次间隔Tz，机器人完成姿态调整；

②如果未达到目标位移范围，自适应调整航向。

调整航向生成ɑ。根据：ɑ=Kz×（ψg-ψc）=nz×△ɑ，确定九自由度人形机器人行走时所需转角ɑ，根据机器人内部确定的△ɑ确定机器人转向信号数nz；

如果ψg>ψc，求得需给定机器人转向信号数，控制器发出机器人左转向信号nz次，每次间隔Tz，机器人完成航向调整；如果ψg<ψc，求得需给定机器人转向信号数nz，控制器发出机器人右转向信号nz次，每次间隔Tz，机器人完成航向调整；

确定九自由度人形机器人行走时所需行走位移，根据机器人内部确定的△s和公式，求得需给定机器人转向信号数nx，控制器发出机器人转向信号nx次，每次间隔Tx，机器人完成直线移动；

根据ɑq=Kq×（ψg-ψc）=nq×△ɑ和机器人内部确定的△ɑ，求得需给定机器人姿态调整信号数nq。

如果ψg>ψc，求得需给定机器人转向信号数nq，控制器发出机器人左转向信号nq次，每次间隔Tz，机器人完成姿态调整；如果ψg<ψc求得需给定机器人转向信号数nq，控制器发出机器人右转向信号nq次，每次间隔Tz，机器人完成姿态调整。根据以上控制流程，人形机器人可完成行走路线自适应规划过程，并调整好最终姿态，达到设定任务。

3.3 主要运动控制原理

根据人形机器人北斗导航自适应主动控制原理，操作人员先将不同的数据、参数存储到不同的地址，然后主动控制器自动生成导航轨迹、姿态，并将主要参数存入固定的寄存器中。接下来，人形机器人按照自动生成的轨迹先根据航向要求自适应比较，调整航向。航向调整结束后，根据生成轨迹一步一步朝着目的地步行，在完成直线动作后，继续按照前面生成的姿态调整要求调整姿态。这时，人形机器人自适应主动控制系统根据现在所处位置自适应比较距离和姿态，生成调整路线、姿态，并完成调整。利用stl语言描述了人形机器人北斗导航自适应主动控制的主要控制原理，如图5所示。根据图5原理，结合图2人形机器人北斗导航自适应主动控制硬件系统，可实现人形机器人的自适应导航控制。

4 人形机器人导航试验研究

4.1 人形机器人行走路线自适应规划试验简介

根据试验所建立的环境，依据北斗导航系统，测得试验室机器人起点所在坐标，分别设定两个目的坐标，每个目的坐标设计两个姿态，以此来进行试验。

试验目的一方面检验人形机器人北斗导航能否实现导航轨迹自生成，另一方面观察导航系统的自适应性，其中理想轨迹坐标为机器人自导航系统在没有原理误差和系统误差的情况下应该行走的轨迹，而仿真轨迹为机器人经过机器人自适应导航主动控制系统分析运算，自动生成的轨迹曲线，是机器人理论上经过自适应导航主动控制系统控制后应该走的路线、轨迹、航向、姿态，而实际轨迹为机器人自适应北斗导航试验人形机器人实际的轨迹及姿态。试验时，首先，对人形机器人北斗导航自适应控制系统进行初始化设置，输入相关参数，如表1、表2所示。

表1 试验用人形机器人相关参数Tab.1 Test Humanoid Robot Related Parameters

表2 人形机器人自适应北斗导航试验初始设置相关参数Tab.2 Humanoid Robot Adaptive Test Initial Setting Parameters

然后，将机器人放入设定初始坐标位置，并设好初始姿态。接着，启动自适应导航系统，人形机器人开始按照导航系统自动生成路线运行，主要参数，如表3所示。

表3 人形机器人自适应北斗导航试验主要输出Tab.3 Humanoid Robot Adaptive Test Main Output

试验过程中，机器人每走一步，对其坐标位置及姿态进行记录。为了验证机器人能否人性化自动选择左右调整姿态，设定目标北斗坐标（3835534419.3，1974867272.3）下两个姿态，一个姿态为-34°，即北偏东56°，另一个姿态为-73°，即北偏东17°。因为从表4、表5 可以看出人形机器人理想状态下中间航向为-53°，即北偏东37°，如果人形机器人能够更人性化自动选择左右调整姿态，则机器人在终点调整姿态时，终点姿态为-34°的会自动左转，而终点姿态为-73°的会自动右转，而不是一直左转到设定姿态。

4.2 人形机器人自适应北斗导航试验参数、曲线及分析

人形机器人自适应北斗导航试验在目标坐标为北斗坐标（3835534419.3，1974867272.3），最终目标姿态为-34°和-73°下自动生成的航向调整、直线移动、姿态调整信号数，如表4、表5所示。是导航路线生成的主要阶段，如果机器人在行走过程中没有误差及相关问题，人形机器人可以直接到达目的地并实现所要求姿态。但实际上由于误差，人形机器人在完成姿态调成后实际位置和姿态与原要求还有差距，如果与设定目标的距离及姿态小于允许误差，则认为机器人完成任务，如果距离或者姿态与设定的目标位置距离、姿态相差大于允许误差，则需要人形机器人根据现在所处位置、姿态，进行位姿自适应调整。由于本试验后期人形机器人自适应调整信号数据较多，如表6所示。因此整个导航过程分导航前期和后期两个阶段介绍。

表4 机器人自适应北斗导航试验坐标、姿态参数（ψg -34°）Tab.4 Robot Adaptive Beidou Navigation Test Coordi⁃nates/Attitude Parameters（ψg -34°）

表4 （续）机器人自适应北斗导航试验坐标、姿态参数（ψg -34°）Tab.4 （Cont）Robot Adaptive Beidou Navigation Test Co⁃ordinates/Attitude Parameters（ψg -34°）

表5 机器人自适应北斗导航试验坐标、姿态参数（ψg -73°）Tab.5 Robot Adaptive Beidou Navigation Test Coordi⁃nates/Attitude Parameters（ψg -73°）

理想轨迹坐标、姿态为理想情况下，人形机器人动作时在没有误差的情况下，依据自适应导航发出的信号数，理想情况下应该所处的北斗坐标和姿态；仿真轨迹坐标、姿态为人形机器人动作时在没有误差的情况下，依据自适应导航发出的信号数，根据表2中参数，人形机器人应该所处的位置和姿态，他与理想轨迹坐标、姿态的不同是因为人形机器人本身结构等问题，因为人形机器人每信号转动角度、直行位移、姿态调整角度理论上都是定值，不能够像人一样可以根据具体情况随意调整，因此造成人形机器人自适应导航系统自动生成的轨迹、姿态与理想轨迹、姿态由一定的差异，这种误差是系统本身的误差，是不可避免的。实际试验时得到的轨迹坐标、姿态是在人形机器人动作时实时测量、记录机器人的行走轨迹、姿态而得到的机器人的真实的行走路线、姿态。

虽然初始坐标、目标坐标相同，但终点要求姿态不同，如表3中所示，输出信号数经过内部规划数据相同，但信号数相同不代表人形机器人动作时左、右转选择不同，这需要通过试验轨迹，姿态测定。

根据表4和表5所得数据可以得到终点姿态-34°和-73°时人形机器人自适应北斗导航轨迹、姿态图，如图6、图7所示。图中分别由三条轨迹、姿态，一条为机器人自适应导航理想轨迹，一条为仿真轨迹，一条为导航试验前期在终点位置没有自适应调整的曲线。

图6 机器人自适应北斗导航前期轨迹、姿态（ψg-34°）Fig.6 Robot Adaptive Beidou Navigation Early Trajectory/Attitude（ψg-34°）

图7 机器人自适应北斗导航前期轨迹、姿态（ψg -73°）Fig.7 Robot Adaptive Beidou Navigation Early Trajectory/Attitude（ψg-73°）

因为导航初始位置和目标位置坐标相同，因此人形机器人理想轨迹和仿真轨迹大致相同，但由于终点要求位姿不同，图6为要求姿态-34°，要求姿态-73°，如图7所示。两种要求下人形机器人在终点位置时姿态变化不同，图6为东偏北转向，图7东偏南转向。虽然转向信号数由于偶然相同，如表4、表5所示。

但是每转向信号使机器人动作不同，人形机器人不再单一通过左转或右转实现要求姿态，而是左转、右转结合，自适应实现要求姿态，这在两图中理论、仿真、实际轨迹、姿态中都得到了证实。因为理论、仿真轨迹、姿态中是按照理想状况下得到的轨迹、姿态，因此不需要进行后期自适应校核位置、姿态进行调整位置、姿态。

但从两图实际轨迹、姿态来看，人形机器人实际运动终点位置、姿态与设定目标位置、姿态还有较大差距，图6终点位置坐标为（3835534458.5，1974867406.7），姿态-20°，与设定位置（3835534419.3，1974867272.3），姿态-34°，相距140mm，姿态相差14°，大于距离允许R的50mm，姿态允许δ的10°，因此需要后期校核位置，自适应调整姿态、位置过程；图7 终点位置坐标为（3835534440，1974867352），姿态-66°，与设定位置（3835534419.3，1974867272.3），姿态-73°，相距82mm，姿态相差7°，距离大于距离允许R的50mm，因此需要后期校核位置，自适应调整姿态、位置过程。

从两图中机器人仿真轨迹、姿态和实际行走轨迹、姿态可以看出，机器人行走轨迹与理想行走轨迹重合度较高，理论上和实际上都已经验证了人形机器人北斗导航可以实现人形机器人的自适应规划控制。根据实际实验结果，终点姿态-34°和-73°度机器人后期经校核位置，自适应调整阶段的试验坐标、姿态参数，如表6所示。

由于人形机器人在到达位置后与原要求的位置、姿态要求的误差大于系统设定的任务终结条件，因此人形机器人需要进行后期自适应调整位置、姿态。两条曲线终点自适应调整阶段信号数不同，如表6所示。

表6 终点姿态-34°和-73°机器人后期自适应调整阶段试验坐标、姿态参数Tab.6 Test Coordinates and Attitude Parameters of Robot During Adaptive Adjustment Phase（ψg-34°and -73°）

这与误差的大小和姿态的要求不同有关。终点姿态-34°自适应调整阶段导航系统规划给出18次航向调整信号，3次直线运动信号，16次姿态调整信号，而终点姿态-73°自适应调整阶段导航系统规划给出13次航向调整信号，2次直线运动信号，12次姿态调整信号。调整阶段，人形机器人先右转调整航向，后直线运动，再左转调整姿态，如图8、图9所示。

这说明人形机器人可以根据实际情况自适应选择左右转完成相关任务，智能化程度大大提高。终点位置坐标为（3835534444.6，1974867287.0），姿态-37°，与设定位置（3835534419.3，1974867272.3），姿态-34°，相距29.3mm，姿态相差3°，小于距离允许R的50mm，姿态允许δ的10°，如图8所示。

图8 机器人自适应北斗导航整体轨迹（ψg-34°）Fig.8 Overall Trajectory of Robot Adaptive Beidou Navigation（ψg-34°）

因此完成了任务，达到了导航目标；终点位置坐标为（3835534440.0，1974867281.0），姿态-79°，与设定位置（3835534419.3，1974867272.3），姿态-73°，相距82mm，姿态相差6°，距离小于距离允许R的50mm，如图9所示。

图9 机器人自适应北斗导航整体轨迹（ψg-73°）Fig.9 Overall Trajectory of Robot Adaptive Beidou Navigation（ψg-73°）

因此完成了任务，达到了导航目标。从试验结果可以看出，导航系统的自适应性较强，智能化程度较高，达到了人形机器人自导航主动控制系统优化设计的目的，自适应性得到了加强。

5 结论

（1）针对人形机器人卫星导航下的行走路线自适应规划问题，从实际应用出发，研发了一种可以利用北斗导航系统进行人形机器人自适应路径规划控制的主动控制系统。（2）通过代数学方法和几何学方法，在运动学分析的基础上建立了人形机器人自适应主动控制模型，得到了人形机器人位置、姿态、航向和人形机器人控制信号之间的关系。（3）通过stl 语言表达了人形机器人自适应运动控制的控制原理，介绍了导航控制过程。（4）通过导航试验，验证了运动控制的控制原理和主动控制系统的正确性。导航试验结果表明，所研发的主动控制系统可以实现人形机器人的自适应路径规划控制导航，为后期人形机器人导航控制的研究搭建了良好的实验平台。