[发明专利]一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法

权利要求书

1.一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法，其特征在于，所述方法包括以下步骤：

1)根据当前平衡车的机械结构、电机的型号属性以及现场测试确定当前PID参数下最大的稳定可控角度，设定角度值θ'，确定下位机平衡倾角采样周期T，一个周期内倾角值为θ_k，k＝1，2，3,…,n，当|θ_k|θ'时，直接拒绝上位机控制命令，结束通信，引入新的平衡控制模型以达到平衡，在恢复平衡后，再开启通信，发送可以继续的状态命令；

2)在车体已平衡的基础上，由深度摄像头判断前方是否有斜坡，由深度摄像头模块读取平衡车到斜坡的距离l_1k，k＝1，2，3,…,n；

3)判断车体两侧车轮是否与斜坡平面保持平行，测量车体两侧超声波模块的返回值，解析得到左右两侧车轮离前方障碍的距离分别为l_2k,l_3k，k＝1，2，3,…,n；

4)若l_2k≠l_3k，说明车体平面未与斜坡平面保持平行，引入新的转向环控制模型，将原有转向环的差速驱动变为反向驱动，直到再次测量得到l_2k＝l_3k，再由深度摄像头反馈回的数据l_1k对比，直到l_3k＝l_2k＝l_1k，将当前车体中轴所对的斜坡边缘置为标记点；

5)当再次经过斜坡时，以标记点为中心，在左右两侧间隔固定距离d选取上坡位置，多次采样后，即可得到的高质量的标准斜坡图像完成精准的地图构建。

2.如权利要求1所述的基于视觉传感器的多种路况下的两轮自平衡车运动控制方法，其特征在于，所述步骤1)中，设定角度值θ'是由测试得到的，不同的PID参数下，θ'需要重新测量得到，车体直立环的PID采用位置式PID算法，公式为

其中，PWM_直表示PID控制器的输出量；

K_p表示直立环PID控制器中的比例项放大系数；

e_k表示当前时刻的系统误差；

e_k-1表示上一时刻的系统误差；

T_d表示微分时间常数；

T_i表示积分时间常数；

只采用PD控制，由于角度参考量为0，角度误差值对时间的微分为角速度，公式简化为

PWM_直＝K_p*e_k+K_d*Gyro

其中，K_d表示直立环PID控制器的微分项放大系数，Gyro为姿态传感器读取的角速度，平衡车由三个PID环联合控制，整体的模型控制公式为

PWM_总＝PWM_直+PWM_速+PWM_转

其中，PWM_总表示不同PID控制器输出量的叠加值；

PWM_速表示速度环PID控制器的输出量；

PWM_转表示转向环PID控制器的输出量；

当|θ_k|θ'时，模型超过了当前PID最大稳定可控角，需转入新的控制模型，新的控制模型追求快速的响应性，在短周期内快速的将倾角缩小到原来PID可控的范围内，对准确性的要求不高，直接跳过PID模型，选用PWM的脉宽最大值进行控制，此时要取消转向环对两侧车轮的PID差动控制，且为保证系统的响应速度，断开与上位机的通讯，系统完全由下位机进行控制，控制模型变为

PWM_总＝PWM_max*K

K是一个随倾角偏差指数变化的比例系数，公式如下

其中，K₀表示K的初始值，1表示在θ^*＞6时，K的值不变，保持为1，e是自然常数，θ^*为倾斜角θ_k的绝对值与θ'的差值|θ_k|-θ'，避免当|θ_k|趋向于θ'时，控制器的输出量太大，与原本系统输出量存在较大偏差造成系统的不稳定，K(θ^*)分段处的θ^*值由多次实验得出，K(θ^*)公式由指数函数公式变形得到，原始公式为

其中，α表示比例放大系数，l表示常数项；

K₀＝1，α,l的值由解下列二元一次方程得到

0.75为倾斜角绝对值|θ_k|趋向θ'时系统原始模型的PWM脉宽幅值与PWM的脉宽最大值的比例系数，由多次实验得到，在断开与上位机通讯的过程中，此时上位机接收不到下位机固定时间间隔发送的心跳包，判断下位机处于离线状态，当再次检测到当|θ_k|θ'时，将控制模型转换成原来的PID模型，恢复通信过程，发送可以继续的状态命令0x5A,0x5B,0xA5,0xB5。