技术领域technical field
本发明属于微型传感器应用和智能机器人控制领域，具体涉及一种用于草坪维护的智能割草机的直线行走控制系统。The invention belongs to the fields of miniature sensor application and intelligent robot control, and in particular relates to a linear walking control system of an intelligent lawn mower used for lawn maintenance.
背景技术Background technique
城市绿化是城市化建设过程中必不可少的一环，而草坪具有吸尘、降噪、保湿、保持水土和净化环境等诸多优点，在城市绿化中占据着举足轻重的地位。随着城市草坪面积的增大，草坪的日常维护和保养成了摆在人们尤其是园艺工作者面前的一项主要工作，这种工作劳动强度大、重复性高，使用传统的机械式割草机进行草坪维护不仅消耗大量的人力、物力，而且还会产生较大的噪声，不利于保护环境。Urban greening is an essential part of the urbanization process, and lawns have many advantages such as vacuuming, noise reduction, moisturizing, water and soil conservation, and environmental purification, and occupy a pivotal position in urban greening. With the increase of the area of urban lawns, the daily maintenance and maintenance of lawns has become a major task in front of people, especially gardeners. This kind of work is labor-intensive and repetitive. Traditional mechanical mowing Lawn maintenance by machine not only consumes a lot of manpower and material resources, but also produces a lot of noise, which is not conducive to protecting the environment.
随着信息技术和智能控制技术的发展，各种智能割草机不断涌现，为草坪的日常维护和保养带来了巨大的方便，不仅节省了大量的劳动力，而且能消除机械式割草机的噪声。通常智能割草机在进行草坪维护时需要执行复杂的动态路径规划策略，以尽可能地完全覆盖草坪区域，但也不可避免地导致了草坪区域重复覆盖的问题，从而降低了作业效率。如果能够实现智能割草机的直线行走，并执行平行往复式的割草运动，如图1所示，则可以避免执行复杂的动态路径规划策略，降低重复覆盖的草坪区域，从而大大提高作业效率。然而现有的可能实现割草机直线行走的控制策略，如基于通电导线的电磁感应、光电编码器和视觉导航等，在室外草坪上需要布设通电导线，否则无法有效识别割草机的偏转或缺乏相应的视觉参考，因而对割草机直线行走的控制效果均不太理想。With the development of information technology and intelligent control technology, various intelligent lawn mowers are emerging, bringing great convenience to the daily maintenance and maintenance of lawns, not only saving a lot of labor, but also eliminating the need for mechanical lawn mowers. noise. Usually, intelligent lawn mowers need to implement complex dynamic path planning strategies when performing lawn maintenance to completely cover the lawn area as much as possible, but this inevitably leads to the problem of repeated coverage of the lawn area, thereby reducing operating efficiency. If the intelligent lawn mower can walk in a straight line and perform parallel reciprocating mowing motions, as shown in Figure 1, it can avoid the implementation of complex dynamic path planning strategies, reduce the lawn area of repeated coverage, and greatly improve the operating efficiency . However, the existing control strategies that may realize the straight-line walking of the mower, such as electromagnetic induction based on energized wires, photoelectric encoders, and visual navigation, etc., need to lay energized wires on the outdoor lawn, otherwise the deflection or deflection of the mower cannot be effectively identified. There is a lack of corresponding visual reference, so the control effect on the straight-line walking of the lawn mower is not ideal.
另外，基于微机电技术MEMS的惯性/磁传感器MARG通常包括三轴加速度传感器、三轴角速率传感器即陀螺仪和三轴磁传感器，基于这三种传感器进行的姿态检测具有低成本、低功耗、尺寸小和灵敏度高的特点，而且在进行姿态检测时不需要外部设备的辅助，因而在无人机、智能机器人等诸多领域都得到了广泛的应用。In addition, the inertial/magnetic sensor MARG based on MEMS usually includes a three-axis acceleration sensor, a three-axis angular rate sensor that is a gyroscope, and a three-axis magnetic sensor. The attitude detection based on these three sensors has low cost and low power consumption. , small size and high sensitivity, and does not require the assistance of external equipment when performing attitude detection, so it has been widely used in many fields such as drones and intelligent robots.
基于现有技术的现状，本申请的发明人拟提供一种基于惯性/磁传感器MARG姿态检测的智能割草机直线行走控制系统，将基于MARG的姿态检测应用于智能割草机的控制中，不仅可以有效控制割草机的直线行走，而且能实时检测割草机的姿态，防止割草机发生侧翻，从而为割草机进行草坪维护提供很大的帮助。Based on the status quo of the prior art, the inventor of the present application intends to provide an intelligent lawn mower linear walking control system based on the attitude detection of the inertial/magnetic sensor MARG, and apply the attitude detection based on the MARG to the control of the intelligent lawn mower. It can not only effectively control the straight-line walking of the lawn mower, but also detect the posture of the lawn mower in real time to prevent the lawn mower from turning over, thus providing great help for the lawn mower to maintain the lawn.
发明内容Contents of the invention
本发明的目的在于针对现有技术中由于割草机驱动电机和驱动轮参数的差异、路面坡度以及电源电量等导致的割草机无法走直线的问题，以及基于通电导线的电磁感应、光电编码器和视觉导航等在室外草坪上对割草机直线行走控制效果不理想的问题，提供一种基于惯性/磁传感器MARG姿态检测的割草机直线行走控制系统。该系统的主要执行机构包括基于惯性/磁传感器MARG的姿态检测模块、割草机主控单元和电机驱动模块，需要执行的两大主要程序包块割草机姿态检测算法和航向偏转调整系统。The purpose of the present invention is to solve the problem in the prior art that the lawn mower cannot walk in a straight line due to the difference in the parameters of the drive motor and the drive wheel of the lawn mower, the slope of the road surface, and the power supply, and the electromagnetic induction and photoelectric coding based on the energized wire. In order to solve the problem of unsatisfactory control effects of lawn mower linear walking on outdoor lawns, such as controller and visual navigation, a linear walking control system of lawn mower based on inertial/magnetic sensor MARG attitude detection is provided. The main actuators of the system include the attitude detection module based on the inertial/magnetic sensor MARG, the main control unit of the lawn mower and the motor drive module. The two main program packages that need to be executed are the attitude detection algorithm of the lawn mower and the course deflection adjustment system.
其中，割草机姿态检测主要通过MARG姿态检测模块完成，该模块中的处理器首先采集三轴加速度传感器、陀螺仪和三轴磁传感器的测量数据并进行预处理，然后执行姿态解算算法，获得割草机的实时姿态信息，主要包括滚转角roll、俯仰角pitch和偏航角yaw，最后将割草机的三个姿态角通过串口发送给割草机的主控单元，供主控单元实现割草机的直线行走控制。Among them, the attitude detection of the lawn mower is mainly completed by the MARG attitude detection module. The processor in this module first collects the measurement data of the three-axis acceleration sensor, gyroscope and three-axis magnetic sensor and performs preprocessing, and then executes the attitude calculation algorithm. Obtain the real-time attitude information of the lawn mower, mainly including roll angle roll, pitch angle pitch and yaw angle yaw, and finally send the three attitude angles of the lawn mower to the main control unit of the lawn mower through the serial port for the main control unit Realize the linear walking control of the lawn mower.
另外，割草机的航向偏转调整主要通过割草机的主控单元和电机驱动模块实现。主控单元从MARG姿态检测模块接收割草机的实时姿态角，以姿态角中的偏航角yaw控制割草机的航向，在航向发生偏转时进行航向调整和由于航向偏转导致的航线偏移距离的补偿，以滚转角roll判断割草机的负重驱动轮，当割草机在斜坡上进行航向调整时，通过相应的比例因子补偿负重轮的电机调整转速，从而使割草机更好地适应不同路面状况下的直线行走；最终，主控单元产生的所有电机控制策略均由电机驱动模块驱动电机实现。In addition, the course deflection adjustment of the lawn mower is mainly realized through the main control unit and the motor drive module of the lawn mower. The main control unit receives the real-time attitude angle of the mower from the MARG attitude detection module, controls the course of the lawnmower with the yaw angle in the attitude angle, and performs course adjustment and course deviation caused by course deviation when the course is deflected Compensation of distance, judging the load-bearing drive wheel of the lawn mower by the roll angle roll, when the lawn mower adjusts the heading on the slope, the corresponding proportional factor is used to compensate the motor of the load wheel to adjust the speed, so that the lawn mower can better Adapt to straight-line walking under different road conditions; finally, all motor control strategies generated by the main control unit are realized by the motor drive module to drive the motor.
本发明的主要优点有：Main advantage of the present invention has:
通过惯性/磁传感器MARG实现割草机的姿态检测，不仅具有低成本、低功耗、尺寸小和灵敏度高的特点，而且还不需要外部设备的辅助，为割草机的大范围自由移动提供了很大的便利；The attitude detection of the lawn mower is realized by the inertial/magnetic sensor MARG, which not only has the characteristics of low cost, low power consumption, small size and high sensitivity, but also does not require the assistance of external equipment, providing a wide range of free movement of the lawn mower. a great convenience;
可以实时检测割草机的姿态，从而防止割草机在大倾角草坪上作业时发生侧翻或对发生侧翻的情况进行报警；It can detect the posture of the lawn mower in real time, so as to prevent the lawn mower from rolling over or give an alarm to the situation of rolling over when the lawn mower is working on a large angle of lawn;
由于通过姿态检测算法获得的姿态是相对于地球坐标系的姿态，因而其偏航角不受地面状况的影响，可以实时准确地确定割草机的航向，从而有效实现直线行走控制。Since the attitude obtained by the attitude detection algorithm is relative to the earth coordinate system, its yaw angle is not affected by the ground conditions, and the heading of the lawn mower can be accurately determined in real time, thereby effectively realizing straight-line walking control.
附图说明Description of drawings
图1为割草机做平行往复式运动的示意图。Figure 1 is a schematic diagram of a lawnmower doing parallel reciprocating motion.
图2为割草机控制系统结构示意图。Figure 2 is a schematic diagram of the structure of the mower control system.
图3为基于MARG的改进高斯牛顿互补滤波姿态检测算法的计算流程示意图。Fig. 3 is a schematic diagram of the calculation flow of the improved Gauss-Newton complementary filter attitude detection algorithm based on MARG.
图4为MARG姿态检测模块与车身相对位置的示意图。Fig. 4 is a schematic diagram of the relative position of the MARG attitude detection module and the vehicle body.
图5为割草机航向偏移调整示意图。Fig. 5 is a schematic diagram of the course offset adjustment of the lawn mower.
图6为割草机偏移距离和偏转角度关系的示意图。Fig. 6 is a schematic diagram of the relationship between the offset distance and the deflection angle of the lawn mower.
图7为割草机航向调整控制程序的流程图。Fig. 7 is a flow chart of the course adjustment control program of the lawnmower.
具体实施方式detailed description
本发明所需要的执行机构的总体结构框图如图2所示，下面结合图2介绍基于惯性/磁传感器MARG姿态检测的割草机直线行走控制系统的具体实现方式。The overall structural block diagram of the executive mechanism required by the present invention is shown in Figure 2, and the specific implementation of the lawnmower linear walking control system based on the inertial/magnetic sensor MARG attitude detection is introduced below in conjunction with Figure 2 .
实施例1Example 1
(1)基于惯性/磁传感器MARG的割草机姿态检测(1) Attitude detection of lawn mower based on inertial/magnetic sensor MARG
该部分主要通过图2中的MARG姿态检测模块实现。为了通过MARG姿态模块检测割草机的姿态信息，需要将MARG姿态检测模块放置于割草机平台上，并使其中的某个轴如X轴与割草机的中轴线平行，Y轴沿着横向向右，Z轴垂直向上，类似如图3的形式，以准确地获得割草机随着地面状况的变化而导致的姿态变化；This part is mainly realized by the MARG attitude detection module in Figure 2. In order to detect the attitude information of the mower through the MARG attitude module, the MARG attitude detection module needs to be placed on the mower platform, and one of the axes, such as the X axis, is parallel to the central axis of the lawn mower, and the Y axis is along the The horizontal direction is to the right, and the Z axis is vertically upward, similar to the form shown in Figure 3, so as to accurately obtain the attitude change of the lawn mower as the ground conditions change;
由MARG姿态检测模块中的处理器如STM32F10X系列处理器以50Hz的采样频率采集三个传感器的测量数据，并进行预处理，主要包括低通滤波和传感器测量数据校准，以消除高频噪声干扰和尖峰毛刺信号以及补偿传感器的系统误差。然后执行基于MARG传感数据的姿态解算算法，如基于改进高斯牛顿法的姿态检测算法和基于Kalman滤波的姿态检测算法等，获得割草机的实时姿态信息，主要包括割草机的滚转角roll、俯仰角pitch和偏航角yaw，最后将得到的姿态角通过通用串口传输给割草机的主控单元；The processor in the MARG attitude detection module, such as the STM32F10X series processor, collects the measurement data of the three sensors at a sampling frequency of 50Hz, and performs preprocessing, mainly including low-pass filtering and sensor measurement data calibration, to eliminate high-frequency noise interference and Spike burr signal and compensate the system error of the sensor. Then execute the attitude calculation algorithm based on the MARG sensor data, such as the attitude detection algorithm based on the improved Gauss-Newton method and the attitude detection algorithm based on the Kalman filter, etc., to obtain the real-time attitude information of the lawn mower, mainly including the roll angle of the lawn mower roll, pitch angle pitch and yaw angle yaw, and finally the obtained attitude angle is transmitted to the main control unit of the mower through the general serial port;
对于上述基于MARG传感数据的姿态检测算法，采用改进的高斯牛顿互补滤波姿态检测算法，该算法精度高且计算量较小，能够有效地应用于处理能力有限、对实时性要求高的可移动式设备中，所述算法首先通过三轴加速度和三轴磁传感器校准后的归一化测量值，通过改进的高斯牛顿算法--主要是降低算法的迭代次数和矩阵运算的维度，获得类似梯度下降算法的梯度因子然后乘以一个表示陀螺仪零均值测量误差的比例因子β，对由陀螺仪测量数据获得的姿态变化率进行修正，最后将修正后的估计姿态变化率进行一阶时间积分运算并归一化，获得由四元数表示的割草机当前时刻的姿态再将得到的四元数表示的姿态转换为欧拉角表示的姿态，最终用于割草机的直线行走控制中；For the above attitude detection algorithm based on MARG sensing data, the improved Gauss-Newton complementary filter attitude detection algorithm is adopted. This algorithm has high precision and small calculation amount, and can be effectively applied to mobile devices with limited processing capacity and high real-time requirements. In the type equipment, the algorithm first uses the normalized measurement values of the three-axis acceleration and the three-axis magnetic sensor calibration, and uses the improved Gauss-Newton algorithm-mainly to reduce the number of iterations of the algorithm and the dimension of the matrix operation to obtain a similar gradient gradient factor for the descent algorithm It is then multiplied by a scaling factor β representing the zero-mean measurement error of the gyroscope, and the attitude change rate obtained from the gyroscope measurement data is Make corrections, and finally the corrected estimated attitude change rate Perform first-order time integration and normalization to obtain the current attitude of the mower represented by the quaternion Then the posture represented by the obtained quaternion Converted to the attitude represented by the Euler angle, and finally used in the straight-line walking control of the lawn mower;
在上述由高斯牛顿算法获得梯度因子的同时，使用该梯度因子通过算式并进行一阶时间积分运算再乘以一个表示陀螺仪非零均值测量误差变化率的比例因子ζ，用于修正陀螺仪的测量数据误差，以进一步降低陀螺仪的漂移误差对最终姿态检测精度的影响；The gradient factor obtained by the Gauss-Newton algorithm above At the same time, use the gradient factor to pass the formula And perform the first-order time integration operation and then multiply it by a proportional factor ζ representing the rate of change of the non-zero mean measurement error of the gyroscope, which is used to correct the measurement data error of the gyroscope, so as to further reduce the impact of the drift error of the gyroscope on the final attitude detection accuracy influences;
根据上述内容，本发明中所采用的基于惯性/磁传感器MARG的姿态检测算法的总体计算过程可以概括为如图3所示的流程示意图；According to the above, the overall calculation process of the attitude detection algorithm based on the inertial/magnetic sensor MARG adopted in the present invention can be summarized as a schematic flow chart as shown in Figure 3;
(2)割草机航向偏转调整控制(2) Lawn mower course deflection adjustment control
该部分主要通过图2中的主控单元和电机驱动模块实现。主控单元通过串口从MARG姿态检测模块接收姿态角roll、pitch和yaw数据，在割草机即将进行的某段直线行走开始处，记录偏航角yaw前N1个数据的平均值作为割草机进行该段直线行走的初始航向角yaw_init，然后在割草机行进过程中实时计算当前航向角与初始航向角差值的绝对值以及当前N2个滚转角的平均值roll_avg，以差值绝对值判断割草机是否发生航向偏转，滚转角平均值确定割草机的负重轮；This part is mainly realized by the main control unit and the motor drive module in Fig. 2 . The main control unit receives the attitude angle roll, pitch and yaw data from the MARG attitude detection module through the serial port, and records the average value of the N1 data before the yaw angle yaw at the beginning of a certain straight-line walking of the lawn mower as the lawn mower Carry out the initial heading angle yaw_init of this section of straight-line walking, and then calculate the absolute value of the difference between the current heading angle and the initial heading angle and the average roll_avg of the current N2 roll angles in real time while the lawn mower is moving, and judge by the absolute value of the difference Whether the lawn mower has a course deflection, the average value of the roll angle determines the road wheel of the lawn mower;
当割草机当前航向角与初始航向角差值的绝对值没有超过预先设定的阈值角θth或没有连续超过预先设定的阈值角θth N3次时，认为割草机航向没有发生偏转，此时割草机航向调整标志Adjust_Direction＝0，割草机继续直线前进，不进行航向调整；当割草机当前航向角与初始航向角差值的绝对值连续超过预先设定的阈值角θth N3次时，认为割草机航向发生了偏转，需要向相反的方向调整航向。此时根据如图4所示的MARG姿态检测模块与割草机平台相对摆放位置的对应关系以及割草机当前航向角yaw与初始航向角yaw_init的差值yaw-yaw_init的正负分别进行如下两种状况的航向调整：When the absolute value of the difference between the current heading angle of the lawn mower and the initial heading angle does not exceed the preset threshold angle θth or does not exceed the preset threshold angle θth N3 consecutive times, it is considered that the heading of the lawn mower does not deflect , at this time the lawnmower course adjustment flag Adjust_Direction=0, the lawnmower continues to move straight forward without course adjustment; when the absolute value of the difference between the current course angle and the initial course angle of the lawnmower continuously exceeds the preset threshold angle θth N3 times, it is considered that the course of the lawn mower has been deflected, and the course needs to be adjusted in the opposite direction. At this time, according to the corresponding relationship between the MARG attitude detection module and the relative placement position of the lawnmower platform as shown in Figure 4, and the positive and negative values of the difference between the current course angle yaw and the initial course angle yaw_init of the lawnmower, yaw-yaw_init are respectively carried out as follows Course adjustment for two situations:
(i)若yaw-yaw_init>0，则设置航向调整标志Adjust_Direction＝1，表示割草机航向向左偏，需要向右调整航向。此时再查看前述计算出的滚转角均值roll_avg，若roll_avg＝0，则认为割草机两个驱动轮的负重相同，割草机在水平面上行进，此时左驱动轮电机以pl+p0的转速进行航向调整(这里的pl表示割草机在水平面上直线前进时，左驱动轮的电机转速)，直至调整至越过初始航向角某个角度α；若||roll_avg||>0，表示割草机在斜坡上行进，此时负重轮的负重相比非负重轮大，导致由负重轮向上坡方向调整航向时，所需要的功率相比在水平面上调整航向所需的功率较大，因而采用比例因子ki对割草机在斜坡上进行航向调整时的电机转速进行补偿。在割草机航向向左偏的情况下，ki具有两个值kll、krl，分别对应左轮负重左偏、右轮负重左偏的情况，再根据roll_avg的正负以及割草机航向偏转的对应关系，可以确定：当roll_avg>0时，对应左轮负重左偏的情况，此时左驱动轮电机以pl+p0+kll*||roll_avg||的转速进行航向调整，直至调整至越过初始航向角某个角度α；当roll_avg<0时，对应右轮负重左偏的情况，此时左驱动轮电机以pl+p0+krl*||roll_avg||的转速进行航向调整，直至调整至越过初始航向角某个角度α；(i) If yaw-yaw_init>0, then set the heading adjustment flag Adjust_Direction=1, which means that the heading of the lawn mower deviates to the left, and the heading needs to be adjusted to the right. At this time, look at the average roll angle roll_avg calculated above. If roll_avg=0, it is considered that the two driving wheels of the lawn mower have the same load, and the lawn mower is moving on a horizontal plane. At this time, the motor of the left driving wheel is driven by pl +p0 to adjust the course (here pl represents the motor speed of the left drive wheel when the lawn mower moves straight ahead on the horizontal plane), until it is adjusted to a certain angle α beyond the initial course angle; if ||roll_avg||>0 , which means that the lawn mower is moving on a slope. At this time, the load of the road wheel is larger than that of the non-load wheel. As a result, when the road wheel is used to adjust the course uphill, the power required is smaller than that required to adjust the course on the horizontal plane. Therefore, the proportional factorki is used to compensate the motor speed when the lawn mower is heading on a slope. When the course of the lawn mower deviates to the left,ki has two values kll and krl , corresponding to the case of left wheel load deviation and right wheel load deviation respectively, and then according to the positive and negative of roll_avg and the course of the lawn mower The corresponding relationship of the deflection can be determined: when roll_avg>0, it corresponds to the case where the left wheel load is deflected to the left. At this time, the left driving wheel motor adjusts the course at the speed of pl +p0 +kll *||roll_avg|| until Adjust to a certain angle α beyond the initial heading angle; when roll_avg<0, it corresponds to the case where the load on the right wheel is shifted to the left, and the motor of the left driving wheel is running at the speed of pl +p0 +krl *||roll_avg|| Adjust the course until it reaches a certain angle α beyond the initial course angle;
(ii)若yaw-yaw_init<0，则设置航向调整标志Adjust_Direction＝2，表示割草机航向向右偏，需要向左调整航向。此时同样查看前述计算出的滚转角均值roll_avg，若roll_avg＝0，则认为割草机两个驱动轮的负重相同，割草机在水平面上行进，此时右驱动轮电机以pr+p0的转速进行航向调整(这里的pr表示割草机在水平面上直线前进时，右驱动轮的电机转速)，直至调整至越过初始航向角某个角度α；若||roll_avg||>0，与(i)的状况类似，也采用比例因子ki对割草机在斜坡上进行航向调整时的电机转速进行补偿。在这种割草机航向向右偏的情况下，ki也具有两个值klr、krr，分别对应左轮负重右偏、右轮负重右偏的情况，再根据roll_avg的正负以及割草机航向偏转的对应关系，可以确定：当roll_avg>0时，对应左轮负重右偏的情况，此时右驱动轮电机以pr+p0+klr*||roll_avg||的转速进行航向调整，直至调整至越过初始航向角某个角度α；当roll_avg<0时，对应右轮负重右偏的情况，此时右驱动轮电机以pr+p0+krr*||roll_avg||的转速进行航向调整，直至调整至越过初始航向角某个角度α；(ii) If yaw-yaw_init<0, set the heading adjustment flag Adjust_Direction=2, which means that the heading of the lawn mower is biased to the right, and the heading needs to be adjusted to the left. At this time, also check the roll_avg average roll angle calculated above. If roll_avg=0, it is considered that the two driving wheels of the lawn mower have the same load, and the lawn mower is moving on a horizontal plane. At this time, the motor of the right driving wheel is driven by pr +p0 to adjust the course (here pr represents the motor speed of the right drive wheel when the lawn mower moves straight ahead on the horizontal plane), until it is adjusted to a certain angle α beyond the initial course angle; if ||roll_avg||>0 , similar to the situation of (i ), the proportional factor ki is also used to compensate the motor speed when the lawn mower is heading on a slope. In the case that the course of the lawn mower deviates to the right,ki also has two values klr and krr , which correspond to the left wheel weight deviation to the right and the right wheel weight deviation to the right respectively, and then according to the positive and negative of roll_avg and the cutting The corresponding relationship of the course deflection of the grass machine can be determined: when roll_avg>0, it corresponds to the case where the left wheel load is biased to the right, and the right drive wheel motor is heading at the speed of pr +p0 +klr *||roll_avg|| Adjust until it exceeds a certain angle α of the initial heading angle; when roll_avg<0, it corresponds to the case where the right wheel load is biased to the right, and the right driving wheel motor is pr +p0 +krr *||roll_avg|| The rotation speed is adjusted until the adjustment reaches a certain angle α beyond the initial heading angle;
在割草机进行航向调整时，采用如图5所示的边行进边调整航向的策略，即图中的“①→②→③”，在航向调整的过程中，如果仅仅将割草机航向调整到初始航向就停止调整，由于此时割草机已经偏离了原来的航线，则会产生航线漂移，如图5所示，即在开始调整割草机航向的位置“①”处，割草机已经偏离了原来航线d的距离，因而为了避免这种情况的产生，必须将割草机的航向调整到越过初始航向某个角度α，正如上述(i)和(ii)状况中所述，然后再继续前进一段时间，以补偿割草机航向偏转导致的偏移距离；在割草机驱动轮半径r和驱动电机转速ω一定的情况下，割草机的偏移距离d和偏移时间Δt具有固定的关系，如下(1)式。When adjusting the course of the lawn mower, adopt the strategy of adjusting the course while moving as shown in Figure 5, that is, "①→②→③" in the figure. During the course adjustment, if only the course of the lawn mower Stop adjusting when the initial course is adjusted. Since the lawn mower has deviated from the original course at this time, course drift will occur, as shown in Figure 5, that is, at the position "①" where the lawnmower course is adjusted, The lawn mower has deviated from the original course d, so in order to avoid this situation, the course of the mower must be adjusted to a certain angle α beyond the initial course, as described in the above (i) and (ii) situations, Then continue to move forward for a period of time to compensate for the offset distance caused by the course deflection of the lawn mower; when the radius r of the drive wheel of the lawn mower and the speed ω of the drive motor are constant, the offset distance d and offset time of the lawn mower Δt has a fixed relationship, as in the following formula (1).
其中，lR表示割草机偏离初始航向θth角时右驱动轮转过的距离；Among them, lR represents the distance that the right drive wheel turns when the lawn mower deviates from the initial heading angleθth ;
此外，在进行航向调整时，若以与发生偏离时左右驱动轮相同的相对转速进行调整，则割草机偏离原来航线的最大距离d'＝2d，如图6所示，然后再继续边行进边调整航向至越过初始航向角度α，并继续行进，以补偿总体的偏移距离d'，使割草机回到原来的航线；在理想情况下，当调整航向至越过初始航向角度α时，再继续行进2Δt的时间，割草机即可回到原来航线，但由于驱动轮半径差异、电机转速差异和路面坡度等各种因素的影响，割草机并不能回到原来的航线或越过了原来的航线，因而用于补偿偏移距离的实际前进时间需要通过具体的实验进行稍作调整确定，而且在补偿割草机偏移距离时，可以让割草机调整至稍微越过初始航线，从而做近似“S”型的前进运动，效果会更好。In addition, when adjusting the course, if the relative rotational speed of the left and right driving wheels is the same as that of the left and right driving wheels when the deviation occurs, the mower will deviate from the original course by a maximum distance of d'=2d, as shown in Figure 6, and then continue to move forward While adjusting the heading to cross the initial heading angle α, and continue to travel, to compensate the overall offset distance d', so that the lawn mower returns to the original course; under ideal conditions, when the heading is adjusted to cross the initial heading angle α, Continue to travel for 2Δt, and the lawn mower can return to the original route. However, due to the influence of various factors such as the difference in driving wheel radius, the difference in motor speed, and the slope of the road, the lawn mower cannot return to the original route or cross the path. The original flight path, so the actual advance time used to compensate the offset distance needs to be determined by a small adjustment through specific experiments, and when compensating the offset distance of the lawn mower, the lawn mower can be adjusted to slightly cross the initial flight path, so that It will be better to do forward movement similar to "S" shape.
根据上述内容，本发明中基于惯性/磁传感器MARG姿态检测的割草机直线行走的总体控制流程可以概括为如图7所示的流程图。According to the above, the overall control flow of the lawnmower walking straight based on the attitude detection of the inertial/magnetic sensor MARG in the present invention can be summarized as the flow chart shown in FIG. 7 .
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510334521.5ACN106325287A (en) | 2015-06-14 | 2015-06-14 | Intelligent mower straight line walking control system based on inertial/magnetic sensor MARG attitude detection |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510334521.5ACN106325287A (en) | 2015-06-14 | 2015-06-14 | Intelligent mower straight line walking control system based on inertial/magnetic sensor MARG attitude detection |
| Publication Number | Publication Date |
|---|---|
| CN106325287Atrue CN106325287A (en) | 2017-01-11 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510334521.5APendingCN106325287A (en) | 2015-06-14 | 2015-06-14 | Intelligent mower straight line walking control system based on inertial/magnetic sensor MARG attitude detection |
| Country | Link |
|---|---|
| CN (1) | CN106325287A (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107065873A (en)* | 2017-04-13 | 2017-08-18 | 浙江工业大学 | A kind of multi-curvature circular path tracking control method based on tape guidance AGV |
| CN107132839A (en)* | 2017-03-16 | 2017-09-05 | 浙江工业大学 | A kind of single-chip microcomputer robot line navigation method |
| CN107509443A (en)* | 2017-09-05 | 2017-12-26 | 惠州市蓝微电子有限公司 | The hillside fields travel control method and system of a kind of intelligent grass-removing |
| WO2018149404A1 (en)* | 2017-02-15 | 2018-08-23 | 苏州宝时得电动工具有限公司 | Automatic working system, self-moving apparatus and control method thereof |
| CN111076761A (en)* | 2019-12-30 | 2020-04-28 | 杭州电子科技大学 | Magnetic encoder calibration method and system |
| CN111427372A (en)* | 2020-03-03 | 2020-07-17 | 深圳蚁石科技有限公司 | Anti-reverse repeated oscillation method for aircraft |
| CN112650206A (en)* | 2019-10-10 | 2021-04-13 | 苏州宝时得电动工具有限公司 | Intelligent mower walking control method and intelligent mower |
| CN112833773A (en)* | 2021-01-13 | 2021-05-25 | 无锡卡尔曼导航技术有限公司 | High-precision real-time mu counting method for operation |
| CN114802587A (en)* | 2021-01-27 | 2022-07-29 | 杜宇 | Active displacement compensation type hyperstable four-body ship |
| WO2023155465A1 (en)* | 2022-02-18 | 2023-08-24 | 智橙动力(北京)科技有限公司 | Swimming pool cleaning robot path planning and cleaning method, apparatus, and device |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS63132111A (en)* | 1986-11-21 | 1988-06-04 | Tokyo Keiki Co Ltd | Attitude detector |
| CN201054889Y (en)* | 2007-01-25 | 2008-05-07 | 上海罗宝信息技术有限公司 | Remote-network solar automatic mowing machine |
| CN101828464A (en)* | 2010-05-20 | 2010-09-15 | 浙江亚特电器有限公司 | Intelligent mowing robot moving parallelly and control method for parallel movement |
| CN102768034A (en)* | 2011-05-03 | 2012-11-07 | 西门子公司 | Method of checking a wind turbine in a wind farm for a yaw misalignment, method of monitoring a wind turbine in a wind farm and monitoring apparatus |
| CN102771246A (en)* | 2012-07-05 | 2012-11-14 | 芜湖鸿宇智能科技有限公司 | Intelligent mower system and intelligent mowing method thereof |
| CN103379619A (en)* | 2012-04-16 | 2013-10-30 | 中兴通讯股份有限公司 | Method and system for positioning |
| CN103776451A (en)* | 2014-03-04 | 2014-05-07 | 哈尔滨工业大学 | High-precision three-dimensional posture inertia measurement system and method based on MEMS (Micro Electro Mechanical Systems) |
| CN204329976U (en)* | 2015-01-14 | 2015-05-13 | 上海物景智能科技有限公司 | A kind of robot course-angle mensurement device and comprise the robot of this device |
| CN204347615U (en)* | 2014-12-31 | 2015-05-20 | 成都纵横自动化技术有限公司 | A kind of unmanned plane robot pilot based on Edison |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS63132111A (en)* | 1986-11-21 | 1988-06-04 | Tokyo Keiki Co Ltd | Attitude detector |
| CN201054889Y (en)* | 2007-01-25 | 2008-05-07 | 上海罗宝信息技术有限公司 | Remote-network solar automatic mowing machine |
| CN101828464A (en)* | 2010-05-20 | 2010-09-15 | 浙江亚特电器有限公司 | Intelligent mowing robot moving parallelly and control method for parallel movement |
| CN102768034A (en)* | 2011-05-03 | 2012-11-07 | 西门子公司 | Method of checking a wind turbine in a wind farm for a yaw misalignment, method of monitoring a wind turbine in a wind farm and monitoring apparatus |
| CN103379619A (en)* | 2012-04-16 | 2013-10-30 | 中兴通讯股份有限公司 | Method and system for positioning |
| CN102771246A (en)* | 2012-07-05 | 2012-11-14 | 芜湖鸿宇智能科技有限公司 | Intelligent mower system and intelligent mowing method thereof |
| CN103776451A (en)* | 2014-03-04 | 2014-05-07 | 哈尔滨工业大学 | High-precision three-dimensional posture inertia measurement system and method based on MEMS (Micro Electro Mechanical Systems) |
| CN204347615U (en)* | 2014-12-31 | 2015-05-20 | 成都纵横自动化技术有限公司 | A kind of unmanned plane robot pilot based on Edison |
| CN204329976U (en)* | 2015-01-14 | 2015-05-13 | 上海物景智能科技有限公司 | A kind of robot course-angle mensurement device and comprise the robot of this device |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018149404A1 (en)* | 2017-02-15 | 2018-08-23 | 苏州宝时得电动工具有限公司 | Automatic working system, self-moving apparatus and control method thereof |
| CN109074086A (en)* | 2017-02-15 | 2018-12-21 | 苏州宝时得电动工具有限公司 | Automatic working system, from mobile device and its control method |
| US11675363B2 (en) | 2017-02-15 | 2023-06-13 | Positec Power Tools (Suzhou) Co., Ltd | Automatic working system, self-moving apparatus and control method thereof |
| CN107132839A (en)* | 2017-03-16 | 2017-09-05 | 浙江工业大学 | A kind of single-chip microcomputer robot line navigation method |
| CN107065873A (en)* | 2017-04-13 | 2017-08-18 | 浙江工业大学 | A kind of multi-curvature circular path tracking control method based on tape guidance AGV |
| CN107509443A (en)* | 2017-09-05 | 2017-12-26 | 惠州市蓝微电子有限公司 | The hillside fields travel control method and system of a kind of intelligent grass-removing |
| CN112650206A (en)* | 2019-10-10 | 2021-04-13 | 苏州宝时得电动工具有限公司 | Intelligent mower walking control method and intelligent mower |
| CN111076761A (en)* | 2019-12-30 | 2020-04-28 | 杭州电子科技大学 | Magnetic encoder calibration method and system |
| CN111427372A (en)* | 2020-03-03 | 2020-07-17 | 深圳蚁石科技有限公司 | Anti-reverse repeated oscillation method for aircraft |
| CN112833773A (en)* | 2021-01-13 | 2021-05-25 | 无锡卡尔曼导航技术有限公司 | High-precision real-time mu counting method for operation |
| CN114802587A (en)* | 2021-01-27 | 2022-07-29 | 杜宇 | Active displacement compensation type hyperstable four-body ship |
| WO2023155465A1 (en)* | 2022-02-18 | 2023-08-24 | 智橙动力(北京)科技有限公司 | Swimming pool cleaning robot path planning and cleaning method, apparatus, and device |
| Publication | Publication Date | Title |
|---|---|---|
| CN106325287A (en) | Intelligent mower straight line walking control system based on inertial/magnetic sensor MARG attitude detection | |
| CN102788591B (en) | Visual information-based robot line-walking navigation method along guide line | |
| CN107943020B (en) | Automatic deviation rectifying method for tire crane cart | |
| CN110989578B (en) | Wireless-control dual-core four-wheel-drive UWB positioning mowing robot and control method thereof | |
| CN107272008A (en) | A kind of AGV Laser navigation systems with inertia compensation | |
| CN105057301B (en) | Automatic correction method and system in the traveling of solar panel cleaning Vehicle | |
| CN105425791A (en) | Swarm robot control system and method based on visual positioning | |
| CN102135766A (en) | Autonomous operation forestry robot platform | |
| CN108646747A (en) | Agri-vehicle path tracking control method | |
| CN103135549A (en) | Motion control system and motion control method for spherical robot with visual feedback | |
| CN113359710A (en) | LOS theory-based agricultural machinery path tracking method | |
| CN102591342A (en) | Electronic-compass-based local path planning method for mowing robot | |
| CN106708061A (en) | Cooperative follow-up operation navigation method and cooperative follow-up operation navigation device of greenhouse mobile operation platform | |
| CN107065873A (en) | A kind of multi-curvature circular path tracking control method based on tape guidance AGV | |
| CN108388241A (en) | The path following method of mobile robot | |
| CN107509443A (en) | The hillside fields travel control method and system of a kind of intelligent grass-removing | |
| CN108684634B (en) | Spray control method and system based on volume characteristic prediction | |
| CN104898665A (en) | Method and device for programming track of tour inspection robot | |
| CN106647770A (en) | Field turning path planning and control method used for farm machinery driverless driving | |
| CN112471111B (en) | An intelligent driving electric plant protection vehicle with adjustable height and wheelbase | |
| CN105892459A (en) | Non-time-based differential drive robot set-point tracking control method | |
| CN117270535B (en) | Auxiliary navigation system suitable for crawler-type potato harvester and control method | |
| Li et al. | Research on human-guided active following mode with 3D spatial relative positioning for vehicles in hilly and mountainous orchards | |
| CN117419717A (en) | Signal processing method, electronic device, and computer-readable storage medium | |
| CN115826572A (en) | Unmanned steel grasping machine motion steering control method and system |
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication | Application publication date:20170111 |