四轮全向底盘控制方法及系统与流程

1.本发明涉及自动控制技术领域，具体地，涉及一种四轮全向底盘控制方法及系统。


背景技术：

2.受限于卫生间的狭小环境和防水坎存在所需要的越障需求，类似于阿克曼的四轮底盘无法满足在狭小环境中移动，且一般的全向移动底盘无法满足越障条件，所以需要一种四轮四舵机的移动底盘。因直行电机转向和舵机方向存在的同一控制速度多构型问题，传统的控制算法存在舵机、直行电机大幅跳变的情况，对电机的控制平滑度和使用寿命带来了影响。
3.专利文献cn107600221b，申请(专利)号201710750897.3提供了一种智能全向agv小车及控制方法，包括驱动装置、超声波传感器单元(2)、运动指示灯单元(5)、导航传感器单元(7)、主控单元(8)、车厢(9)，所述的车厢(9)是一个具有一定高度的箱体，并且车厢(9)的底盘为去掉四个角的正方体，驱动装置包括全向轮单元(1)、电机驱动单元(3)、电机单元(4)，驱动装置安装在车厢(9)的底盘上，所述的主控单元(8)安装于车厢(9)内，超声波传感器单元(2)为4个。本发明通过对agv小车结构上的设计，实现了agv小车在狭小空间乃至零半径原地转动，并通过给予不同的x,y,θ能够实现agv小车朝指定方向的运动。但该发明中采用为4全向轮；本专利中采用的为4舵轮4直行电机。
4.专利文献cn104216406b，申请(专利)号:201310219764.5提供了一种四轮驱动全向底盘的控制装置及控制方法，适用于机器人底盘移动控制领域，无线串口通信模块用来接收来自与计算机连接的无线串口通信模块的指令，指令信息包括四轮驱动全向底盘所要移动到的目标点的位置值以及姿态角值，将接收到的指令传给上位机arm；fpga将计数电路和信息采集电路融合，由fpga对码盘计数模块的数据进行采集与处理，然后通过通用i/o接口传输到arm，同时fpga也作为接口电路，将arm的控制指令传输到直流电机控制模块，完成对直流电机的转速以及方向的控制；arm是整个控制装置的核心控制模块，用以对接收到的数据进行融合，实现对四轮驱动全向底盘的位姿进行实时结算，完成对四个直流电机的实时控制。但该发明中采用为4全向轮，与对比1中不同点为四轮安装位置；本专利中采用的为4舵轮4直行电机。
5.专利文献wo2019000436a1(申请号：pct/cn2017/091282)公开了一种四轮底盘车、两轮底盘车、总成及控制方法，四轮底盘车包括：车身(1)、前轮模块(2)和后轮模块(3)；前轮模块(2)和后轮模块(3)分别与车身(1)可拆卸连接；前轮模块(2)或后轮模块(3)从车身(1)拆下后作为两轮底盘车独立移动。但该发明中采用为4全向轮；本专利中采用的为4舵轮4直行电机。


技术实现要素：

6.针对现有技术中的缺陷，本发明的目的是提供一种四轮全向底盘控制方法及系统。
7.根据本发明提供的一种四轮全向底盘控制方法，包括：
8.步骤s1：设置底盘参数；
9.步骤s2：接受控制底盘的速度；
10.步骤s3：根据速度计算出四个舵机的位姿和四个直行电机的速度；
11.步骤s4：读取舵机当前位置和直行电机当前速度计算代价最小的代价构型，输出舵机位姿和电机速度。
12.优选地，在所述步骤s1中：
13.设置底盘参数包括：直行轮轮径r、前后轮轮距base_x、左右轮轮距base_y，舵机转速servo_w。
14.优选地，在所述步骤s3中：
15.步骤s3.1：计算底盘线速度v_b：
16.v_b＝sqrt((x^2 y^2))
17.x为x方向线速度，y为y方向线速度；
18.步骤s3.2计算底盘旋转半径turning_r：
19.turning_r＝v_b/w
20.w为角速度；
21.底盘在速度v[x,y,w]的命令下以o为旋转中心，turning_r为旋转半径，v_b为行驶速度运动；
[0022]
步骤s3.3：计算内侧轮的舵机位置th_i和内侧轮旋转半径turning_r_inner：
[0023]
th_i＝atan(y_track/2,turning_r-(x_wheel/2))
[0024]
turning_r_inner＝sqrt((turning_r-x_wheel/2)^2 (y_track_/2)^2))
[0025]
y_track为左右轮距，x_wheel为前后轮距；
[0026]
步骤s3.4:计算外侧轮的舵机位置th_o和外侧轮旋转半径turning_r_outer：
[0027]
th_o＝atan(y_track_/2,turning_radius (x_wheel_base_/2))
[0028]
turning_r_outer＝sqrt((turning_r x_wheel/2)^2 (y_track_/2)^2))
[0029]
y_track为左右轮距，x_wheel_base为前后轮距，turning_radius为旋转半径；
[0030]
步骤s3.5：计算内侧轮直行电机速度rpm_inner和外侧轮的直行电机速度rpm_outer：
[0031]
rpm_inner＝w*turning_r_inner
[0032]
rpm_outer＝w*turning_r_outer
[0033]
步骤s3.6：确定内侧方向
[0034]
若v_b*w＞0则内侧为左侧，若v_b*w≤0则内侧在右侧；
[0035]
步骤s3.7若内侧为左侧，给4直行电机和4舵轮下发的控制为：
[0036]
左前舵机：th_i
[0037]
左后舵机：-th_i
[0038]
右前舵机：th_o
[0039]
右后舵机：-th_o
[0040]
左前电机：rpm_inner
[0041]
左后电机：rpm_inner
[0042]
右前电机：rpm_outer
[0043]
右后电机：rpm_outer
[0044]
若内侧为右侧，给4直行电机和4舵轮下发的控制为：
[0045]
左前舵机旋转角度：th_o
[0046]
左后舵机旋转角度：-th_o
[0047]
右前舵机旋转角度：th_i
[0048]
右后舵机旋转角度：-th_i
[0049]
左前电机转速：rpm_outer
[0050]
左后电机转速：rpm_outer
[0051]
右前电机转速rpm_inner
[0052]
右后电机转速：rpm_inner
[0053]
优选地，在所述步骤s4中：
[0054]
读取四个舵机当前位置current_servo_1、current_servo_2、current_servo_3、current_servo_4和4个直行电机当前速度至current_wheel_1、current_wheel_2、current_wheel_3、current_wheel_4，数据由底盘反馈得到；根据代价计算公式计算代价最小的target_servo和target_wheel代价构型，输出舵机位姿和电机速度。
[0055]
优选地，步骤s4.1：定义代价值cost_servo和cost_wheel，cost_servo表示舵机变化所需的代价值，cost_wheel表示轮子变化所需的代价值；
[0056]
步骤s4.2：对每个舵机和电机，设控制舵机角度th_i和电机速度rpm_i，获取当前的舵机角度th_c和电机速度和rpm_c；
[0057]
步骤s4.3：计算两种构型的代价值p1_cost和p2_cost
[0058]
p1_cost＝fabs(th_i-th_c)*cost_servo fabs(rpm_i-rpm_c)*cost_wheel
[0059]
p2_cost＝fabs((th_i 180)％180-th_c)*cost_servo fabs(-rpm_i-rpm_c)*cost_wheel
[0060]
步骤s4.4：p1_cost和p2_cost中选取cost值最小的对应构型输出至电机控制。
[0061]
根据本发明提供的一种四轮全向底盘控制系统，包括：
[0062]
模块m1：设置底盘参数；
[0063]
模块m2：接受控制底盘的速度；
[0064]
模块m3：根据速度计算出四个舵机的位姿和四个直行电机的速度；
[0065]
模块m4：读取舵机当前位置和直行电机当前速度计算代价最小的代价构型，输出舵机位姿和电机速度。
[0066]
优选地，在所述模块m1中：
[0067]
设置底盘参数包括：直行轮轮径r、前后轮轮距base_x、左右轮轮距base_y，舵机转速servo_w。
[0068]
优选地，在所述模块m3中：
[0069]
模块m3.1：计算底盘线速度v_b：
[0070]
v_b＝sqrt((x^2 y^2))
[0071]
x为x方向线速度，y为y方向线速度；
[0072]
模块m3.2计算底盘旋转半径turning_r：
[0073]
turning_r＝v_b/w
[0074]
w为角速度；
[0075]
底盘在速度v[x,y,w]的命令下以o为旋转中心，turning_r为旋转半径，v_b为行驶速度运动；
[0076]
模块m3.3：计算内侧轮的舵机位置th_i和内侧轮旋转半径turning_r_inner：
[0077]
th_i＝atan(y_track/2,turning_r-(x_wheel/2))
[0078]
turning_r_inner＝sqrt((turning_r-x_wheel/2)^2 (y_track_/2)^2))
[0079]
y_track为左右轮距，x_wheel为前后轮距；
[0080]
模块m3.4:计算外侧轮的舵机位置th_o和外侧轮旋转半径turning_r_outer：
[0081]
th_o＝atan(y_track_/2,turning_radius (x_wheel_base_/2))
[0082]
turning_r_outer＝sqrt((turning_r x_wheel/2)^2 (y_track_/2)^2))
[0083]
y_track为左右轮距，x_wheel_base为前后轮距，turning_radius为旋转半径；
[0084]
模块m3.5：计算内侧轮直行电机速度rpm_inner和外侧轮的直行电机速度rpm_outer：
[0085]
rpm_inner＝w*turning_r_inner
[0086]
rpm_outer＝w*turning_r_outer
[0087]
模块m3.6：确定内侧方向
[0088]
若v_b*w＞0则内侧为左侧，若v_b*w≤0则内侧在右侧；
[0089]
模块m3.7若内侧为左侧，给4直行电机和4舵轮下发的控制为：
[0090]
左前舵机：th_i
[0091]
左后舵机：-th_i
[0092]
右前舵机：th_o
[0093]
右后舵机：-th_o
[0094]
左前电机：rpm_inner
[0095]
左后电机：rpm_inner
[0096]
右前电机：rpm_outer
[0097]
右后电机：rpm_outer
[0098]
若内侧为右侧，给4直行电机和4舵轮下发的控制为：
[0099]
左前舵机旋转角度：th_o
[0100]
左后舵机旋转角度：-th_o
[0101]
右前舵机旋转角度：th_i
[0102]
右后舵机旋转角度：-th_i
[0103]
左前电机转速：rpm_outer
[0104]
左后电机转速：rpm_outer
[0105]
右前电机转速rpm_inner
[0106]
右后电机转速：rpm_inner
[0107]
优选地，在所述模块m4中：
[0108]
读取四个舵机当前位置current_servo_1、current_servo_2、current_servo_3、current_servo_4和4个直行电机当前速度至current_wheel_1、current_wheel_2、
current_wheel_3、current_wheel_4，数据由底盘反馈得到；根据代价计算公式计算代价最小的target_servo和target_wheel代价构型，输出舵机位姿和电机速度。
[0109]
优选地，模块m4.1：定义代价值cost_servo和cost_wheel，cost_servo表示舵机变化所需的代价值，cost_wheel表示轮子变化所需的代价值；
[0110]
模块m4.2：对每个舵机和电机，设控制舵机角度th_i和电机速度rpm_i，获取当前的舵机角度th_c和电机速度和rpm_c；
[0111]
模块m4.3：计算两种构型的代价值p1_cost和p2_cost
[0112]
p1_cost＝fabs(th_i-th_c)*cost_servo fabs(rpm_i-rpm_c)*cost_wheel
[0113]
p2_cost＝fabs((th_i 180)％180-th_c)*cost_servo fabs(-rpm_i-rpm_c)*cost_wheel
[0114]
模块m4.4：p1_cost和p2_cost中选取cost值最小的对应构型输出至电机控制。
[0115]
与现有技术相比，本发明具有如下的有益效果：
[0116]
1、本发明提出了通用四轮四舵机全向底盘的运动解算方式；
[0117]
2、本发明提出了自适应的舵机角度和电机速度控制方法，从多构型上提升了电机控制平滑度，防止速度突变导致舵机抖动问题。
附图说明
[0118]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0119]
图1为四轮底盘示意图；
[0120]
图2为四轮底盘运动模型图；
[0121]
图3为同一速度多构型图。
具体实施方式
[0122]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0123]
实施例1：
[0124]
根据本发明提供的一种四轮全向底盘控制方法，如图1-图3所示，包括：
[0125]
步骤s1：设置底盘参数；
[0126]
步骤s2：接受控制底盘的速度；
[0127]
步骤s3：根据速度计算出四个舵机的位姿和四个直行电机的速度；
[0128]
步骤s4：读取舵机当前位置和直行电机当前速度计算代价最小的代价构型，输出舵机位姿和电机速度。
[0129]
具体地，在所述步骤s1中：
[0130]
设置底盘参数包括：直行轮轮径r、前后轮轮距base_x、左右轮轮距base_y，舵机转速servo_w。
[0131]
具体地，在所述步骤s3中：
[0132]
步骤s3.1：计算底盘线速度v_b：
[0133]
v_b＝sqrt((x^2 y^2))
[0134]
x为x方向线速度，y为y方向线速度；
[0135]
步骤s3.2计算底盘旋转半径turning_r：
[0136]
turning_r＝v_b/w
[0137]
w为角速度；
[0138]
底盘在速度v[x,y,w]的命令下以o为旋转中心，turning_r为旋转半径，v_b为行驶速度运动；
[0139]
步骤s3.3：计算内侧轮的舵机位置th_i和内侧轮旋转半径turning_r_inner：
[0140]
th_i＝atan(y_track/2,turning_r-(x_wheel/2))
[0141]
turning_r_inner＝sqrt((turning_r-x_wheel/2)^2 (y_track_/2)^2))
[0142]
y_track为左右轮距，x_wheel为前后轮距；
[0143]
步骤s3.4:计算外侧轮的舵机位置th_o和外侧轮旋转半径turning_r_outer：
[0144]
th_o＝atan(y_track_/2,turning_radius (x_wheel_base_/2))
[0145]
turning_r_outer＝sqrt((turning_r x_wheel/2)^2 (y_track_/2)^2))
[0146]
y_track为左右轮距，x_wheel_base为前后轮距，turning_radius为旋转半径；
[0147]
步骤s3.5：计算内侧轮直行电机速度rpm_inner和外侧轮的直行电机速度rpm_outer：
[0148]
rpm_inner＝w*turning_r_inner
[0149]
rpm_outer＝w*turning_r_outer
[0150]
步骤s3.6：确定内侧方向
[0151]
若v_b*w＞0则内侧为左侧，若v_b*w≤0则内侧在右侧；
[0152]
步骤s3.7若内侧为左侧，给4直行电机和4舵轮下发的控制为：
[0153]
左前舵机：th_i
[0154]
左后舵机：-th_i
[0155]
右前舵机：th_o
[0156]
右后舵机：-th_o
[0157]
左前电机：rpm_inner
[0158]
左后电机：rpm_inner
[0159]
右前电机：rpm_outer
[0160]
右后电机：rpm_outer
[0161]
若内侧为右侧，给4直行电机和4舵轮下发的控制为：
[0162]
左前舵机旋转角度：th_o
[0163]
左后舵机旋转角度：-th_o
[0164]
右前舵机旋转角度：th_i
[0165]
右后舵机旋转角度：-th_i
[0166]
左前电机转速：rpm_outer
[0167]
左后电机转速：rpm_outer
[0168]
右前电机转速rpm_inner
[0169]
右后电机转速：rpm_inner
[0170]
具体地，在所述步骤s4中：
[0171]
读取四个舵机当前位置current_servo_1、current_servo_2、current_servo_3、current_servo_4和4个直行电机当前速度至current_wheel_1、current_wheel_2、current_wheel_3、current_wheel_4，数据由底盘反馈得到；根据代价计算公式计算代价最小的target_servo和target_wheel代价构型，输出舵机位姿和电机速度。
[0172]
具体地，步骤s4.1：定义代价值cost_servo和cost_wheel，cost_servo表示舵机变化所需的代价值，cost_wheel表示轮子变化所需的代价值；
[0173]
步骤s4.2：对每个舵机和电机，设控制舵机角度th_i和电机速度rpm_i，获取当前的舵机角度th_c和电机速度和rpm_c；
[0174]
步骤s4.3：计算两种构型的代价值p1_cost和p2_cost
[0175]
p1_cost＝fabs(th_i-th_c)*cost_servo fabs(rpm_i-rpm_c)*cost_wheelp2_cost＝fabs((th_i 180)％180-th_c)*cost_servo fabs(-rpm_i-rpm_c)*cost_wheel
[0176]
步骤s4.4：p1_cost和p2_cost中选取cost值最小的对应构型输出至电机控制。
[0177]
实施例2：
[0178]
实施例2为实施例1的优选例，以更为具体地对本发明进行说明。
[0179]
本领域技术人员可以将本发明提供的一种四轮全向底盘控制方法，理解为四轮全向底盘控制系统的具体实施方式，即所述四轮全向底盘控制系统可以通过执行所述四轮全向底盘控制方法的步骤流程予以实现。
[0180]
根据本发明提供的一种四轮全向底盘控制系统，包括：
[0181]
模块m1：设置底盘参数；
[0182]
模块m2：接受控制底盘的速度；
[0183]
模块m3：根据速度计算出四个舵机的位姿和四个直行电机的速度；
[0184]
模块m4：读取舵机当前位置和直行电机当前速度计算代价最小的代价构型，输出舵机位姿和电机速度。
[0185]
具体地，在所述模块m1中：
[0186]
设置底盘参数包括：直行轮轮径r、前后轮轮距base_x、左右轮轮距base_y，舵机转速servo_w。
[0187]
具体地，在所述模块m3中：
[0188]
模块m3.1：计算底盘线速度v_b：
[0189]
v_b＝sqrt((x^2 y^2))
[0190]
x为x方向线速度，y为y方向线速度；
[0191]
模块m3.2计算底盘旋转半径turning_r：
[0192]
turning_r＝v_b/w
[0193]
w为角速度；
[0194]
底盘在速度v[x,y,w]的命令下以o为旋转中心，turning_r为旋转半径，v_b为行驶速度运动；
[0195]
模块m3.3：计算内侧轮的舵机位置th_i和内侧轮旋转半径turning_r_inner：
[0196]
th_i＝atan(y_track/2,turning_r-(x_wheel/2))
[0197]
turning_r_inner＝sqrt((turning_r-x_wheel/2)^2 (y_track_/2)^2))
[0198]
y_track为左右轮距，x_wheel为前后轮距；
[0199]
模块m3.4:计算外侧轮的舵机位置th_o和外侧轮旋转半径turning_r_outer：
[0200]
th_o＝atan(y_track_/2,turning_radius (x_wheel_base_/2))
[0201]
turning_r_outer＝sqrt((turning_r x_wheel/2)^2 (y_track_/2)^2))
[0202]
y_track为左右轮距，x_wheel_base为前后轮距，turning_radius为旋转半径；
[0203]
模块m3.5：计算内侧轮直行电机速度rpm_inner和外侧轮的直行电机速度rpm_outer：
[0204]
rpm_inner＝w*turning_r_inner
[0205]
rpm_outer＝w*turning_r_outer
[0206]
模块m3.6：确定内侧方向
[0207]
若v_b*w＞0则内侧为左侧，若v_b*w≤0则内侧在右侧；
[0208]
模块m3.7若内侧为左侧，给4直行电机和4舵轮下发的控制为：
[0209]
左前舵机：th_i
[0210]
左后舵机：-th_i
[0211]
右前舵机：th_o
[0212]
右后舵机：-th_o
[0213]
左前电机：rpm_inner
[0214]
左后电机：rpm_inner
[0215]
右前电机：rpm_outer
[0216]
右后电机：rpm_outer
[0217]
若内侧为右侧，给4直行电机和4舵轮下发的控制为：
[0218]
左前舵机旋转角度：th_o
[0219]
左后舵机旋转角度：-th_o
[0220]
右前舵机旋转角度：th_i
[0221]
右后舵机旋转角度：-th_i
[0222]
左前电机转速：rpm_outer
[0223]
左后电机转速：rpm_outer
[0224]
右前电机转速rpm_inner
[0225]
右后电机转速：rpm_inner
[0226]
具体地，在所述模块m4中：
[0227]
读取四个舵机当前位置current_servo_1、current_servo_2、current_servo_3、current_servo_4和4个直行电机当前速度至current_wheel_1、current_wheel_2、current_wheel_3、current_wheel_4，数据由底盘反馈得到；根据代价计算公式计算代价最小的target_servo和target_wheel代价构型，输出舵机位姿和电机速度。
[0228]
具体地，模块m4.1：定义代价值cost_servo和cost_wheel，cost_servo表示舵机变化所需的代价值，cost_wheel表示轮子变化所需的代价值；
[0229]
模块m4.2：对每个舵机和电机，设控制舵机角度th_i和电机速度rpm_i，获取当前的舵机角度th_c和电机速度和rpm_c；
[0230]
模块m4.3：计算两种构型的代价值p1_cost和p2_cost
[0231]
p1_cost＝fabs(th_i-th_c)*cost_servo fabs(rpm_i-rpm_c)*cost_wheelp2_cost＝fabs((th_i 180)％180-th_c)*cost_servo fabs(-rpm_i-rpm_c)*cost_wheel
[0232]
模块m4.4：p1_cost和p2_cost中选取cost值最小的对应构型输出至电机控制。
[0233]
实施例3：
[0234]
实施例3为实施例1的优选例，以更为具体地对本发明进行说明。
[0235]
针对上述现有技术中的缺陷，本发明要解决的技术问题体现在以下几点：
[0236]
1)提出一种平滑通用的四轮四舵机控制算法，适用于类似于图1的底盘
[0237]
2)提出一种代价计算方法来选择最平滑的电机转向和舵机方向构型
[0238]
本方法包括如下步骤：
[0239]
步骤1：设置底盘相关参数，包括直行轮轮径r、前后轮轮距base_x、左右轮轮距base_y，舵机转速servo_w
[0240]
步骤2：接受控制底盘的速度v[x,y,w]；
[0241]
x表示x方向线速度，y表示y方向线速度，w表示角速度；
[0242]
步骤3：根据速度v计算出4个舵机的位姿记为target_i_servo_1～4、4个直行电机的速度至target_i_wheel_1～4，运动模型图见图2，此时会有两种构型，即i为0或者1，见双构型问题见图3；
[0243]
步骤4：读取四个舵机当前位置current_servo_1～4和4个直行电机当前速度至current_wheel_1～4，数据由底盘反馈得到；根据代价计算公式计算代价最小的target_servo和target_wheel代价构型，即为最终的输出舵机位姿和电机速度。
[0244]
步骤3包括如下步骤，详见图2运动模型：
[0245]
步骤3.1计算底盘线速度
[0246]
v_b＝sqrt((x^2 y^2))
[0247]
步骤3.2计算底盘旋转半径
[0248]
turning_r＝v_b/w
[0249]
即底盘在速度v[x,y,w]的命令下需以o为旋转中心，turning_r为旋转半径，v_b为行驶速度运动。
[0250]
步骤3.3计算内侧轮的舵机位置和内侧轮旋转半径
[0251]
th_i＝atan(y_track/2,turning_r-(x_wheel/2))
[0252]
turning_r_inner＝sqrt((turning_r-x_wheel/2)^2 (y_track_/2)^2))
[0253]
步骤3.4计算外侧轮的舵机位置和外侧轮旋转半径
[0254]
th_o＝atan(y_track_/2,turning_radius (x_wheel_base_/2))
[0255]
turning_r_outer＝sqrt((turning_r x_wheel/2)^2 (y_track_/2)^2))
[0256]
步骤3.5计算内侧轮和外侧轮的直行电机速度
[0257]
rpm_inner＝w*turning_r_inner；
[0258]
rpm_outer＝w*turning_r_outer；
[0259]
步骤3.6确定内侧方向，在左侧还是在右侧
[0260]
若v_b*w》0则内侧为左侧，否则在右侧
[0261]
步骤3.7若内侧为左侧，给4直行电机和4舵轮下发的控制为
[0262]
左前舵机左后舵机右前舵机右后舵机th_i-th_ith_o-th_o左前电机左后电机右前电机右后电机rpm_innerrpm_innerrpm_outerrpm_outer
[0263]
步骤4包括如下步骤：
[0264]
步骤4.1定义代价值cost_servo和cost_wheel，分别表示舵机变化和轮子变化所需的代价值。
[0265]
步骤4.2对每个舵机和电机，设步骤3中得出的控制舵机角度th_i和电机速度rpm_i，获取当前的舵机角度th_c和电机速度和rpmc。
[0266]
步骤4.3计算两种构型的代价值p1_cost和p2_cost
[0267]
p1_cost＝fabs(th_i-th_c)*cost_servo fabs(rpm_i-rpm_c)*cost_wheel；
[0268]
p2_cost＝fabs((th_i 180)％180-th_c)*cost_servo fabs(-rpm_i-rpm_c)*cost_wheel；
[0269]
步骤4.4p1_cost和p2_cost中选取cost值最小的对应构型输出至电机控制。
[0270]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0271]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。