轮腿式重型车辆高障碍物越上控制方法、装置及存储介质

本发明涉及车辆运动控制领域，尤其是涉及一种轮腿式重型车辆高障碍物越上控制方法、装置及存储介质。

背景技术：

1、轻型四轮腿运载平台一般具有小型、轻质的特点，相较于其本体质量，其驱动电机有着强大的驱动能力。同时，较轻的质量意味着关节电机等驱动电机在接触碰撞时会受到较少的冲击力，如图2所示，因此可以做出跳跃、对角支撑等高机动动作进行越障。如图3所示，采用轮式抵墙爬升的方式，则充分利用了墙面为其提供驱动力，在越障时，一般都是障碍物远小于腿部可运行范围，较少考虑越过高障碍物的情况。

2、但是对于重载平台，一般来说其驱动能力相对平台质量较低，无法完成跳跃、对角支撑等动作，同时，跳跃类的动作会带来较大的触地冲击，会导致损坏驱动电机或者平台的结构件。除此之外，当障碍物侧面摩擦系数较低，无法提供足够的附着力时，依托轮胎抵住墙面可能会出现无法爬升，或者爬升到一定高度打滑落地的情况，导致无法越过高障碍物，也会对结构和执行器产生冲击影响。

3、因此一些现有技术采用逐步爬升的方案，如图4所示，其可以分解为多个状态，依次将四个轮子转移至更高的台阶上，但是这种越上方法，涉及的环节过多，控制难度较大，此外，重载平台的耗能较大，由于一些条件限制，例如电池技术等，不同的控制方案产生的能耗不同，但是由于控制难度较大，较难找到能耗较低的控制方案。

技术实现思路

1、本发明的目的就是为了提供一种轮腿式重型车辆高障碍物越上控制方法、装置及存储介质，既解决了高越障时的可行性问题，不依托障碍物侧面提供额外的支撑力，不进行跳跃动作避免了冲击，又充分利用了轮腿构型的特点，减少了迈腿次数，提升了效率。

2、本发明的目的可以通过以下技术方案来实现：

3、一种轮腿式重型车辆高障碍物越上控制方法，其特征在于，包括：

4、步骤s1：建立三维坐标系，获取地形信息，并基于地形信息确定初始位置和目标位置，其中所述三维坐标系包括两两垂直的x方向、y方向和z方向；

5、步骤s2：获取车辆的属性信息，并结合地形信息、初始位置和目标位置，生成多个支撑状态；

6、步骤s3：设定初始估计运动轨迹，初始化越上运动的总时间、总离散点数和离散点之间的时间间隔，并将轨迹离散化为多个离散点；

7、步骤s4：以总轨迹偏差和总能耗最小为目标建立目标函数；

8、步骤s5：根据动力学、运动学特征分别建立动力学约束、越上障碍物x方向位置约束、以及针对每个轮腿的运动学约束、地形约束、摩擦锥约束、x方向驱动力约束、z方向支撑力约束、y方向静止约束和z方向静止约束；

9、步骤s6：根据初始位置和目标位置建立初始值约束和终端机体状态约束；

10、步骤s7：根据地形信息建立地形约束；

11、步骤s8：基于建立的约束条件，求解目标函数，得到机体质心状态轨迹、轮端力轨迹、轮端位置轨迹和每个支撑状态的离散点的数量；

12、步骤s9：根据得到的机体质心状态轨迹、轮端力轨迹、轮端位置轨迹和每个支撑状态的离散点的数量，控制车辆完成越上动作。

13、所述所有相邻离散点之间的时间间隔相同。

14、所述目标函数为：

15、

16、其中：x为质心状态轨迹，u为轮端力轨迹，p为轮端位置轨迹，为过程中状态误差，其中xg为初始估计运动轨迹)，qk为状态误差权重，n为离散点总数量，为轮端驱动力误差，rk为驱动力误差权重，hk为轮端位置误差权重，为轮端位置误差，其中pg为初始估计轮端轨迹，为终端状态误差，其中xn为终端目标状态。

17、所述离散点约束具体为：

18、

19、其中：nn为第n个支撑状态结束时离散点的数量，yn’为第n’个支撑状态的离散点的数量。

20、所述动力学约束为：

21、xk+1＝xk+fkδt

22、其中：xk+1为第k+1个离散点的质心状态，xk为第k个离散点的质心状态，fk为k时刻动力学表达，δt为离散点时间间隔；

23、所述初始值约束为：

24、x(k＝0)＝xc,u(k＝0)＝uc,p(k＝0)＝pc

25、其中：xc为初始质心状态，uc为初始轮端力，pc为初始轮端位置；

26、所述终端机体状态约束为：

27、x(k＝n)＝xn

28、其中：xn为第n个离散点的质心状态；

29、所述越上障碍物x方向位置约束为：

30、

31、其中：为在x方向上障碍物的起始位置，为在nj支撑状态时，第i个轮端位置，i为轮腿的序号，j为支撑状态标号。

32、针对第i个轮腿的运动学约束为：

33、pd,min＜pi,d＜pd,max

34、其中：pd,min为轮端到髋关节的最短长度，pi,d为第i个轮腿对应的轮端到髋关节的距离，pd,max为轮端到髋关节的最长长度；

35、针对第i个轮腿的地形约束为：

36、height_map(pe,x,i,pe,y,i)-pe,z,i＝0

37、其中：height_map(pe,x,i,pe,y,i)为地形函数，通过输入轮端在x,y的位置pe,x,i,pe,y,i，得到相应坐标的地形高度，pe,z,i为第i个腿的末端(轮端)高度。

38、针对第i个轮腿的摩擦锥约束为：

39、-μuz,k,i≤uy,k,i≤μuz,k,i

40、其中：μ为地面摩擦系数，uy,k,i为在k时刻，第i个腿的末端的y方向驱动力，uz,k,i为在k时刻，第i个腿末端z方向的驱动力；

41、针对第i个轮腿的x方向驱动力约束约束为：

42、ux,min≤ux,k,i≤ux,max

43、其中：ux,min为在x方向的最小轮腿末端驱动力约束，ux,k,i为在k时刻，第i个轮腿末端驱动力，ux,max为在x方向的最大轮腿末端驱动力约束；

44、针对第i个轮腿的z方向支撑力约束为：

45、-uz,k,i≤0

46、针对第i个轮腿的y方向静止约束为：

47、sk,i·(py,k,i-py,k-1,i)·sk,i-1＝0

48、其中：sk,i为在k时刻，第i个轮腿末端支撑状态，0或1，py,k,i为在k时刻，第i个轮腿末端在y方向上的位置，py,k-1,i为在k-1时刻，第i个轮腿末端在y方向上的位置，sk,i-1为在k-1时刻，第i个轮腿末端支撑状态；

49、针对第i个轮腿的z方向静止约束为：

50、sk,i·(pz,k,i-pz,k-1,i)·sk,i-1＝0

51、其中：pz,k,i为在k时刻，第i个轮腿末端在z方向的位置，pz,k-1,i为在k-1时刻，第i个轮腿末端在z方向的位置。

52、所述轮腿的数量为4，所述支撑状态的数量为7。

53、一种轮腿式重型车辆高障碍物越上控制装置，包括存储器、处理器，以及存储于所述存储器中的程序，其特征在于，所述处理器执行所述程序时实现如上述的方法。

54、一种存储介质，其上存储有程序，所述程序被执行时实现如上述的方法。

55、与现有技术相比，本发明具有以下有益效果：

56、1、既解决了高越障时的可行性问题，不依托障碍物侧面提供额外的支撑力，不进行跳跃动作避免了冲击，又充分利用了轮腿构型的特点，减少了迈腿次数，提升了效率，整个越上过程轨迹连续无停顿，同时利用优化的方式，可通过改变约束，针对不同的四轮腿平台对超过支撑高度六分之五的障碍物进行越上障碍物运动规划，有着良好的效果。

57、2、可以提高连续运动控制的平顺性，降低了在复杂环境越上运动控制的算力要求。