CN110001840B - 一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法 - Google Patents

Abstract

一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法，包括以下步骤：1)检测稳定可控角θ'，确定一个采样周期T内的倾角值θ(θ₁，θ₂，θ₃,…,θ_n)，当|θ_n|>θ'时，引入新的平衡控制模型以达到平衡；2)判断前方是否有斜坡，返回到斜坡的距离l₁(l₁₁,l₁₂,l₁₃,…,l_1n)；3)得到左右两侧车轮离斜坡的距离分别为l₂(l₂₁,l₂₂,l₂₃,…,l_2n),l₃(l₃₁,l₃₂,l₃₃,…,l_3n)；4)，若l_2n≠l_3n，引入新的转向环控制模型，直到l_2n＝l_3n，再与l_1n对比，直到l_3n＝l_2n＝l_1n，将当前车体中轴所对的位置置为标记点；5)当再次经过斜坡时，以标记点为中心，在左右两侧间隔固定距离d选取上坡位置，多次采样后，得到高质量的标准斜坡图像。本发明在不同路况下通过引入新的运动控制模型，能有效地提高视觉传感器对图像采集的质量和效率，提高了系统的可靠性，提高了定位导航及地图构建的精度。

Description

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

技术领域

本发明涉及一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法，能有效提高视觉传感器采集到的图像质量，属于平衡车技术领域。

背景技术

视觉传感器是具有图像采集处理、数据传输能力的功能专门化视觉系统,是整个机器视觉系统信息的直接来源。视觉传感器已成为智能化机器系统不可或缺的关键感知手段，搭载深度摄像头的机器系统实时性和鲁棒性强，一直是研究的热门方向。两轮的智能平衡车由于绿色便捷，在社会和市场上也保持着很高的热度，具有良好的发展前景。搭载视觉传感器的两轮自平衡车可以实现车体的同步定位与建图，即SLAM(SimultaneousLocalization and Mapping)。车体在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现车体的自主定位和导航。由视觉传感器采集到的图像质量高低，对定位、导航、地图构建的精准与否起决定性作用。但由于实际工作环境的复杂程度不同，某些特殊路况会对图像的采集造成较大的影响，如在室内工作时，碰到人之后车体剧烈摆动造成摄像头晃动过大，无法成像，甚至造成设备损坏；或是在遇到斜坡时由于两侧车轮上坡存在先后顺序，车体的倾斜导致采集到的是不常规的图像，图像的质量和利用率都无法达到要求，这些都会对高精度的实时系统产生较大的影响。因此，需要一种新的运动控制方法来确保车体在保持平衡的前提下得到高质量的图像。

发明内容

为了克服视觉传感器在图像采样过程中，遇到特殊路况时车体不常规的运动导致无法采集到高质量的图像，降低定位导航及地图构建的精度和效率，本发明提供了一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法，有效避免上述情况的发生，提高图像采集的质量和效率，提高系统稳定性，提高系统精度，实现地图的精准构建。

本发明解决其技术问题所采用的技术方案是：

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

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

2)在车体已平衡的基础上，由深度摄像头判断前方是否有斜坡，由深度摄像头模块读取平衡车到斜坡的距离

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

4)若

说明车体平面未与斜坡平面保持平行，引入新的转向环控制模型，将原有转向环的差速驱动变为反向驱动，直到再次测量得到

再由深度摄像头反馈回的数据

对比，直到

将当前车体中轴所对的斜坡边缘置为标记点；

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

进一步，所述步骤1)中，设定角度值θ'是由测试得到的，不同的PID参数下，θ'需要重新测量得到，车体直立环的PID采用位置式PID算法，公式为

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

PWM_直＝k_p*e_k+K_d*Gyro

其中Gyro为姿态传感器读取的角速度，平衡车由三个PID环联合控制，整体的模型控制公式为

PWM_总＝PWM_直+PWM_速+PWM_转

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

PWM_总＝PWM_max*k

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

k₀＝1，θ为倾斜角θ_n的绝对值与θ'的差值|θ_n|-θ'，避免当|θ_n|趋向于θ'时，控制器的输出量太大，与原本系统输出量存在较大偏差造成系统的不稳定，k(t)分段处θ的值由多次实验得出；

在断开与上位机通讯的过程中，此时上位机接收不到下位机固定时间间隔发送的心跳包，判断下位机处于离线状态，当再次检测到当|θ_n|<θ'时，将控制模型转换成原来的PID模型，恢复通信过程，发送可以继续的状态命令0x5A,0x5B,0xA5,0xB5。

再进一步，所述步骤4)中，车体转向环采用P或PD控制，PID公式简化为

PWM_转＝k_p*(θ_T-θ_C)+k_d*gyro

θ_T为目标yaw角，θ_C为当前时刻的yaw角，gyro当前角速度，k_d的参数调整公式如下：

ε为偏差阈值，k_PWM为经整定的微分系数，当θ_T与θ_C偏差大于ε，k_d为0，取消角速度对转向环的影响，当θ_T与θ_C的偏差小于ε，k_d取k_PWM，以角速度为参考量来纠正细微的偏差，保证车体的直线行驶。

更进一步，所述步骤4)中，在遇到斜坡时，需要车体与斜坡平面保持平行，此时θ_C已知，但θ_T目标偏航角未知，无法得到角度的参考量，原有的转向环控制模型无法起到控制转向角度的作用，转入新的控制模型，将原有转向环的差速驱动变为反向驱动，新的PID模型公式如下

其中

为新的被控量，为左右两侧车轮到斜坡平面的距离差值，参考量设为0，但由于超声波模块测距存在误差，设定新的参考量为阈值ε'，当

驱动左轮电机正向旋转，右轮电机反向旋转，实现车体以车体中轴为中心绕Z轴顺时钟旋转，当

驱动左轮电机反向旋转，右轮电机正向旋转，实现车体以车体中轴为中心绕Z轴逆时针旋转，实现最小半径转向，最终保持车体XZ平面与斜坡平面保持平行。

实现本发明控制方法的控制系统，包括基于视觉传感器的上位机系统和基于平衡控制的下位机系统，所述基于视觉传感器的上位机系统包括深度摄像头模块、ROS系统模块和数据通信模块；所述基于平衡控制的下位机系统包括搭载Cortex-M4内核的PID控制模块、姿态传感器模块、电机驱动模块、数据通信模块、超声波避障模块和光电编码器模块。

本发明的有益效果表现于：在车体倾角超过最大稳定可控范围时，进行控制模型的转换，使车体能迅速恢复平衡，提高了系统的稳定性，且在车体未恢复平衡前停止上下位机通信，对在通信未恢复过程中采集到的图像不进行处理，避免采集到低质量的图像，减少了处理时间，提高了地图构建的效率，在平衡车遇到斜坡时，引入新的控制模型，能提高图像采集的质量和效率，提高了系统的可靠性，提高了定位导航及地图构建的精度。

附图说明

图1为两轮自平衡车多种路况下提高图像采样质量的实现流程图。

图2为系数k与θ(倾斜角θ_n的绝对值和最大稳定可控角θ'的差值)的关系图。

具体实施方式

下面结合附图对本发明的实施方式作进一步描述。

参照图1和图2，一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法，所述方法包括以下步骤：

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

2)在车体已平衡的基础上，由深度摄像头判断前方是否有斜坡，由深度摄像头模块读取平衡车到斜坡的距离

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

4)若

说明车体平面未与斜坡平面保持平行，引入新的转向环控制模型，将原有转向环的差速驱动变为反向驱动，直到再次测量得到

再由深度摄像头反馈回的数据

对比，直到

将当前车体中轴所对的斜坡边缘置为标记点；

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

进一步，所述步骤1)中，设定角度值θ'是由测试得到的，不同的PID参数下，θ'需要重新测量得到，车体直立环的PID采用位置式PID算法，公式为

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

PWM_直＝k_p*e_k+K_d*Gyro

其中Gyro为姿态传感器读取的角速度。平衡车由三个PID环联合控制，整体的模型控制公式为

PWM_总＝PWM_直+PWM_速+PWM_转

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

PWM_总＝PWM_max*k

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

θ为倾斜角θ_n的绝对值与θ'的差值|θ_n|-θ'，避免当|θ_n|趋向于θ'时，控制器的输出量太大，与原本系统输出量存在较大偏差造成系统的不稳定，k(t)分段处θ的值由多次实验得出，k(t)公式由指数函数公式变形得到，原始公式为

k(t)＝k₀*e^αθ-l

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

0.75为倾斜角绝对值|θ_n|趋向θ'时系统原始模型的PWM脉宽幅值与PWM的脉宽最大值的比例系数，由多次实验得到；

在断开与上位机通讯的过程中，此时上位机接收不到下位机固定时间间隔发送的心跳包，判断下位机处于离线状态，当再次检测到当|θ_n|<θ'时，将控制模型转换成原来的PID模型，恢复通信过程，发送可以继续的状态命令0x5A,0x5B,0xA5,0xB5。

再进一步，所述步骤4)中，车体转向环采用P或PD控制，PID公式简化为

PWM_转＝k_p*(θ_T-θ_C)+k_d*gyro

θ_T为目标yaw角，θ_C为当前时刻的yaw角，gyro当前角速度，k_d的参数调整公式如下：

ε为偏差阈值，k_PWM为经整定的微分系数，当θ_T与θ_C偏差大于ε，k_d为0，取消角速度对转向环的影响，当θ_T与θ_C的偏差小于ε，k_d取k_PWM，以角速度为参考量来纠正细微的偏差，保证车体的直线行驶。

更进一步，所述步骤4)中，在遇到斜坡时，需要车体与斜坡平面保持平行，此时θ_C已知，但θ_T目标偏航角未知，无法得到角度的参考量，原有的转向环控制模型无法起到控制转向角度的作用，转入新的控制模型，将原有转向环的差速驱动变为反向驱动，实现最小半径转向，新的PID模型公式如下

其中

为新的被控量，为左右两侧车轮到斜坡平面的距离差值，参考量设为0，但由于超声波模块测距存在误差，设定新的参考量为阈值ε'，当

驱动左轮电机正向旋转，右轮电机反向旋转，实现车体以车体中轴为中心绕Z轴顺时钟旋转，当

驱动左轮电机反向旋转，右轮电机正向旋转，实现车体以车体中轴为中心绕Z轴逆时针旋转，最终保持车体XZ平面与斜坡平面保持平行。

实现本实施例控制方法的控制系统，包括基于视觉传感器的上位机系统和基于平衡控制的下位机系统，所述基于视觉传感器的上位机系统包括深度摄像头模块、ROS系统模块和数据通信模块；所述基于平衡控制的下位机系统包括搭载Cortex-M4内核的PID控制模块、姿态传感器模块、电机驱动模块、数据通信模块、超声波避障模块和光电编码器模块。

Claims (4)

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。

3.如权利要求1或2所述的基于视觉传感器的多种路况下的两轮自平衡车运动控制方法，其特征在于，所述步骤4)中，车体转向环采用P或PD控制，PID公式简化为

PWM_转＝K_p*(θ_T-θ_C)+K_d*gyro

θ_T为目标偏航角，θ_C为当前时刻的偏航角，gyro当前角速度，K_d的参数调整公式如下：

ε为偏差阈值，K_PWM为经整定的微分系数，当θ_T与θ_C偏差大于ε，K_d为0，取消角速度对转向环的影响，当θ_T与θ_C的偏差小于ε，K_d取K_PWM，以角速度为参考量来纠正细微的偏差，保证车体的直线行驶。

4.如权利要求3所述的基于视觉传感器的多种路况下的两轮自平衡车运动控制方法，其特征在于，所述步骤4)中，在遇到斜坡时，需要车体与斜坡平面保持平行，此时θ_C已知，但θ_T目标偏航角未知，无法得到角度的参考量，原有的转向环控制模型无法起到控制转向角度的作用，转入新的控制模型，将原有转向环的差速驱动变为反向驱动，新的PID模型公式如下

PWM_转＝K_p*(|l_2k-l_3k|-ε')

其中l_2k-l_3k为新的被控量，为左右两侧车轮到斜坡平面的距离差值，参考量设为0，但由于超声波模块测距存在误差，设定新的参考量为阈值ε'，当l_2k-l_3k＞ε'，驱动左轮电机正向旋转，右轮电机反向旋转，实现车体以车体中轴为中心绕Z轴顺时针旋转，当l_2k-l_3k＜-ε'驱动左轮电机反向旋转，右轮电机正向旋转，实现车体以车体中轴为中心绕Z轴逆时针旋转，实现最小半径转向，最终保持车体XZ平面与斜坡平面保持平行。

Images