误差修正装置的制作方法

控制;调节装置的制造及其应用技术1.本发明的实施例涉及误差修正装置，利用设于作业车辆的传感器装置进行的测定值的修正。背景技术：2.以往，为了实现农业的高效率而进行如下的自动转向控制，使拖拉机、种植机等作业车辆沿着指定的行驶路径行驶。根据这样的自动转向控制，基于作为目标的行驶路径上的位置与作业车辆的位置之间的分隔距离，对作业车辆的转向进行控制，以使作业车辆的位置定位于行驶路径上。3.此外，作为与作业车辆的自动转向相关的技术，已知有如下的自动转向系统，其具备：卫星定位模块，基于来自卫星的卫星信息而输出位置信息；前进基准位置算出部，基于位置信息算出前进行驶中的作业车的转向控制的作为基准位置的前进基准位置；后退基准位置算出部，基于位置信息算出后退行驶中的作业车的转向控制的作为基准位置的后退基准位置；转向控制部，在前进行驶时输出基于行驶路径与前进基准位置的偏差而算出的前进用转向控制信号，并且在后退行驶时输出基于行驶路径与后退基准位置的偏差而算出的后退用转向控制信号；转向机构，基于前进用转向控制信号及后退用转向控制信号进行所述作业车的转向(例如，参照专利文献1)。4.专利文献1：日本特开2018－120364号公报技术实现要素：5.在作业车辆的自动转向控制中，有时使用由传感器装置测定的速度矢量，但存在该速度矢量根据作业车辆的行驶状态而产生与作业车辆的车体方位大幅度错位的误差的问题。6.本发明是为了解决上述问题而做出的，其目的在于提供能够降低速度矢量的误差的技术。7.为了解决上述课题，本实施例涉及的误差修正装置，设于作业车辆，对基于传感器装置的测定值的误差进行修正，所述传感器装置具备gnss传感器和陀螺仪传感器，所述误差修正装置包括：第一取得部，取得由所述gnss传感器测定的自身坐标系中的所述作业车辆的速度矢量、和由所述陀螺仪传感器测定的传感器坐标系中的角速度，所述传感器坐标系具有以所述传感器装置为基准的3个轴；第二取得部，取得所述作业车辆的转向量；设定位置确定部，基于所述取得的转向量确定设定位置，所述设定位置在所述传感器坐标系中被设定在不同于所述传感器装置的位置；以及速度矢量修正部，基于所述测定位置处的所述作业车辆的速度矢量和所述取得的角速度，算出所述确定的设定位置处的所述作业车辆的速度矢量，并对由所述gnss传感器测定的所述作业车辆的速度矢量进行修正。8.根据本发明，能够降低速度矢量的误差。附图说明9.图1是表示第一实施例涉及的农用拖拉机的构成的概略侧视图。10.图2是表示第一实施例涉及的自动转向控制系统的整体构成的框图。11.图3是表示误差修正装置的硬件构成的框图。12.图4是表示误差修正装置的功能构成的框图。13.图5是表示误差修正装置的动作的流程图。14.图6是表示相对位置矢量的概略主视图。15.图7是表示相对位置矢量的概略俯视图。16.图8是表示速度矢量的概略主视图。17.图9是表示设定位置修正处理的动作的流程图。18.图10是表示通过前轮转向进行前进路线变更中的拖拉机的概略俯视图。19.图11是表示通过前轮转向进行前进回转中的拖拉机的概略俯视图。20.图12是表示车体方向算出处理的动作的流程图。21.图13是表示通过前轮转向进行后退回转中的拖拉机的概略俯视图。22.图14是表示通过后轮转向进行前进路线变更中的拖拉机的概略俯视图。23.图15是表示通过后轮转向进行前进回转中的拖拉机的概略俯视图。具体实施方式24.以下，参照附图说明本发明的实施例。25.＜第一实施例＞26.(作业车辆的构成)27.首先，对具有本实施例涉及的误差修正装置的作业车辆进行说明。图1是表示实施例涉及的农用拖拉机的构成的概略侧视图。28.作为本实施例涉及的自动转向系统的转向对象的车辆是作业车辆，具体而言，为图1所示的农用拖拉机1。该拖拉机1是具备车体10、2个前轮11和2个后轮12，由后轮驱动行驶的四轮车辆，但只要是可转向的车辆，则不限于作业车辆，可以是任意车辆。此外，拖拉机1具备：供驾驶者就座的座席13、转向柱15、方向盘16、转向驱动装置17、包含油门踏板、制动器等的踏板类18、4个支承框架19a～19d、及车顶部19r。此外，在拖拉机1的车体10上通过连杆机构连结有作为设于作业车辆上的作业机的旋耕机41。29.对前轮11施加偏转角来转向拖拉机1的转向系统设于转向柱15内，并在转向柱15中内置有用于将由方向盘16或转向驱动装置17所致的方向盘角输入到转向系统的输入轴151，基于该输入轴151的旋转的转向角被施加到前轮11。转向驱动装置17是为了对不具有用于进行自动转向控制的构成、以手动转向为前提的拖拉机1进行自动转向而后附加的装置，在转向驱动装置17的上方安装有方向盘16，在转向驱动装置17的下方嵌合有转向系统的输入轴151的上端部。分别形成为长条状的4个支承框架19a～19d以立起的状态设置在车体10上的前后左右的不同位置，在所述4个支承框架19a～19d上载置有整体呈大致平板状的车顶部19r。30.在车顶部19r的上表面上，以该车顶部19r作为架台而设有传感器装置30。如后所述该传感器装置30包含gnss传感器，因此为了接收从人造卫星发送的gnss信号，优选是该传感器装置30安装在车顶部19r这样的拖拉机1的最上部，在拖拉机1不具有车顶部19r的情况下，将支承车顶部19r的4个支承框架19a～19d中的任一个作为架台而设置传感器装置30。此外，在位于前方的支承框架19a、19b的任一个设置有自动转向控制装置21。该自动转向控制装置21对拖拉机1的自动转向进行控制。31.(自动转向控制系统的构成)32.图2是表示实施例涉及的自动转向控制系统的整体构成的框图。33.如图2所示，拖拉机1所具备的自动转向控制系统由对转向系统中的输入轴151进行驱动的转向驱动装置17、传感器装置30、自动转向控制装置21和图1中未图示的方向盘角度控制装置22构成。34.转向驱动装置17具备：向输入轴151传递驱动力的传递轴171、驱动传递轴171的马达172、检测传递轴171的旋转量及旋转位置的编码器173，所述马达172为电动马达，所述编码器173为旋转编码器。需要说明的是，马达172只要是能够输出可使输入轴151旋转的充分的扭矩的马达，则可以是任意种类的马达。35.自动转向控制装置21基于传感器装置30的测定值而输出转向量，方向盘角度控制装置22基于由自动转向控制装置21指示的转向量对转向驱动装置17反馈控制。在此，转向驱动装置17控制马达172使传递轴171驱动，以使由编码器173检测出的旋转位置成为所希望的旋转位置。36.如此，通过对拖拉机1后附加设置转向驱动装置17、传感器装置30、自动转向控制装置21及方向盘角度控制装置22，从而能够在以手动转向的方式构成的拖拉机1中实现自动转向。37.传感器装置30具备陀螺仪传感器31、加速度传感器32、gnss传感器33和误差修正装置34，这些部件收纳于同一壳体内。这意味着陀螺仪传感器31、加速度传感器32和gnss传感器33在拖拉机1中设置于大致同一部位。陀螺仪传感器31检测绕3个轴的角速度来测定拖拉机1的方位角ξ、俯仰角θ(pitch angle)、侧倾角φ(滚转角，roll angle)。加速度传感器32测定拖拉机1的3个轴的加速度。gnss传感器33测定拖拉机1的位置即车辆位置、和拖拉机1的速度矢量。误差修正装置34对由陀螺仪传感器31、加速度传感器32、gnss传感器33测得的测定值的误差进行修正。38.自动转向控制装置21基于由gnss传感器33检测出的拖拉机1的位置即车辆位置、和所设定的目标行驶路径，运算使车辆位置与目标行驶路径一致的拖拉机1的行进方位即目标行进方位，并基于该目标行进方位与根据gnss传感器33的速度矢量所得的拖拉机1的行进方位之间的偏差、即行进方位偏差，运算转向量并向方向盘角度控制装置22输出。39.(误差修正装置的构成)40.对误差修正装置的硬件构成及功能构成进行说明。图3及图4分别是表示误差修正装置的硬件构成、功能构成的框图。41.如图3所示，误差修正装置34中，作为硬件而具备：cpu(central processing unit)91、ram(randomaccess memory)92、存储装置93、外部i/f(interface)94。42.cpu91及ram92协作而执行后述的各种功能，存储装置93存储由各种功能执行的处理所用的各种数据。外部i/f94与陀螺仪传感器31、加速度传感器32、gnss传感器33、自动转向控制装置21进行数据的输入输出。需要说明的是，也可以自动转向控制装置21具备cpu、ram、存储装置、外部i/f，取代误差修正装置34而执行后述的各种功能。43.此外，如图4所示，误差修正装置34中，作为功能而具备：第一取得部341、具有第一低通滤波器342a及第二低通滤波器342b的低通滤波器342、第二取得部343、设定位置确定部344、坐标变换部345、车辆位置修正部346、速度矢量修正部347、第三取得部348、车体方位算出部349。44.第一取得部341取得由陀螺仪传感器31、加速度传感器32、gnss传感器33各自测定的测定值。第一低通滤波器342a针对由第一取得部341取得的由陀螺仪传感器31测得的测定值的时序信号，使比预先设定的第一截止频率高的高频信号减小。第二低通滤波器342b针对由第一取得部341取得的由加速度传感器32测得的测定值的时序信号，使比预先设定的第2截止频率高的高频信号减小。45.如上所述，传感器装置30中包含gnss传感器33，因此要接收gnss信号就得优选在4个支承框架19a～19d中的任一个或支承于这些框架上的车顶部19r上安装传感器装置30。在拖拉机1等作业车辆中，相当于本实施例中的4个支承框架19a～19d这样的对车顶进行支承的部件的刚性不充分，尤其是作业车辆在凹凸剧烈的路面上行驶时等，会对传感器装置30施加高频振幅。这样的高频振幅尤其使陀螺仪传感器31、加速度传感器32测得的测定值产生误差，但利用低通滤波器342能够减少这样的误差。46.第二取得部343取得由自动转向控制装置21输出的转向量。设定位置确定部344基于由第二取得部343取得的转向量来确定设定位置。在此，设定位置设定为将传感器装置30的测定位置投影于拖拉机1的行驶面、即农田表面所得的位置，是拖拉机1的宽度方向上的中心位置，关于拖拉机1的前后方向的位置是基于转向量来确定。47.坐标变换部345将传感器坐标系中的相对位置矢量及角速度矢量各自坐标变换为自身坐标系中的矢量。在此，传感器坐标系是具有以传感器装置30所安装的位置、且传感器装置30进行测定的位置即测定位置作为原点而相互正交的x、y、z这3个轴的坐标系，设为x轴朝向拖拉机1的直进方向的前侧、y轴朝向拖拉机1的宽度方向的右侧、z轴朝向拖拉机1的上下方向的下侧。自身坐标系是具有向北、向东及向下的3个轴的坐标系。需要说明的是，关于相对位置矢量、角速度矢量将在后详述。48.车辆位置修正部346将在拖拉机1中的上方、在本实施例中为安装于车顶部19r的传感器装置30的测定位置处由gnss传感器33测定的自身坐标系中的拖拉机1的车辆位置修正为在传感器坐标系中预先设定的设定位置处的拖拉机1的车辆位置。速度矢量修正部347将在传感器装置30的测定位置处由gnss传感器33测得的自身坐标系中的拖拉机1的速度矢量修正为设定位置处的速度矢量。49.第三取得部348取得拖拉机1是处于前进状态还是后退状态来作为行驶方向。车体方位算出部349基于由第三取得部348取得的行驶状态和由速度矢量修正部347修正后的速度矢量，算出拖拉机1的车体10的朝向作为方位。50.(误差修正装置的动作)51.对误差修正装置的动作进行说明。图5是表示误差修正装置的动作的流程图。图6及图7分别是表示相对位置矢量的概略主视图、概略俯视图。图8是表示速度矢量的概略主视图。需要说明的是，图5所示的动作按规定的周期执行。52.如图5所示，首先，第一取得部341取得由gnss传感器33测定的自身坐标系中的车辆位置和速度矢量(s101)，并取得由陀螺仪传感器31测定的拖拉机1的角速度和由加速度传感器32测定的拖拉机1的加速度(s102)53.接着，低通滤波器342对于由第一取得部341取得的角速度及加速度分别使比预先设定的第一截止频率、第二截止频率高的高频信号减小(s103)。54.接着，执行后述的设定位置确定处理(s104)。55.在设定位置确定处理执行之后，坐标变换部345基于由陀螺仪传感器31测定的拖拉机1的方位角、俯仰角、侧倾角，通过l＝cls的式子将图6及图7所示的在传感器坐标系中以设定位置a为起点的测定位置s的相对位置矢量ls坐标变换为自身坐标系中的相对位置矢量l(s105)。在此，c表示用于从传感器坐标系向自身坐标系进行坐标变换的坐标变换矩阵。56.【数学式1】[0057][0058]在此，关于坐标变换矩阵c的各要素，设方位角为ξ、俯仰角为θ、侧倾角为φ，如下述这样定义。[0059]【数学式2】[0060]c11＝cosξ·cosθ[0061]c12＝sinξ·cosθ[0062]c13＝-sinθ[0063]c21＝-sinξ·cosφ+cosξ·sinθ·sinφ[0064]c22＝cosξ·cosφ+sinξ·sinθ·sinφ[0065]c23＝cosθ·sinφ[0066]c31＝sinξ·sinφ+cosξ·sinθ·cosφ[0067]c32＝-cosξ·sinφ+sinξ·sinθ·cosφ[0068]c33＝cosθ·cosφ[0069]此外，在坐标变换之前，通常进行称为对准的初始化处理。由该初始化处理确定拖拉机1的初始方位(方位角)和初始姿势(俯仰角度及侧倾角)，在利用坐标变换部345进行的坐标变换中，利用由陀螺仪传感器31测量的角速度将该初始方位及初始姿势更新后而得的方位角ξ、俯仰角θ、侧倾角φ被赋予给坐标变换矩阵c。[0070]初始姿势下的俯仰角度θ和侧倾角φ是通过将在拖拉机1静止的状态下由加速度传感器32测定的重力加速度设为g、将3个轴的加速度分别设为ax、ay、az而由下式求出。[0071]【数学式3】[0072]ax＝g·sinθ[0073]ay＝-g·cosθ·sinφ[0074]az＝-g·cosθ·cosφ[0075]此外，作为初始方位的方位角ξ是基于在拖拉机1的行驶中由gnss传感器33测定的速度矢量而设定。需要说明的是，关于初始姿势及初始方位，可以使用预先设定的值。[0076]坐标变换后，车辆位置修正部346设以其自身坐标系的原点o为起点的设定位置a的位置矢量为pa、测定位置s的位置矢量为ps，利用pa＝ps-l的式子算出位置矢量pa，由此将在测定位置s处测定的车辆位置修正为通过设定位置修正处理确定的设定位置a的车辆位置(s106)。[0077]接着，坐标变换部345使用坐标变换矩阵c，利用ω＝cωs的式子将由陀螺仪传感器31测定的在传感器坐标系中的拖拉机1的角速度矢量ωs坐标变换为自身坐标系中的角速度矢量ω(s107)。[0078]坐标变换后，速度矢量修正部347设由gnss传感器33测定的测定位置s处的拖拉机1的速度矢量为vs、设定位置a处的速度矢量为va，利用va＝vs－ω×l的式子算出速度矢量va，由此修正速度矢量(s108)。[0079]在速度矢量修正后，执行后述的车体方位算出处理(s109)。[0080]如此，误差修正装置34将在测定位置s测得的车辆位置、速度矢量各自修正为设定位置a处的车辆位置、速度矢量。根据修正后的车辆位置及速度矢量，自动转向控制装置21能够获得降低了特别是拖拉机1在具有凹凸的农田行驶时产生的传感器装置30的振动所引起的测定值的误差的、更准确的车辆位置、行进方位及行进速度。由此，可利用自动转向控制装置21实现准确的拖拉机1的自动转向。[0081](设定位置修正处理)[0082]对设定位置修正处理的动作进行说明。图9是表示本实施例涉及的设定位置修正处理的动作的流程图。图10和图11分别是表示通过前轮转向进行前进路线变更中、前进回转中的拖拉机的概略俯视图。[0083]如图9所示，首先，第二取得部343取得由自动转向控制装置21输出的转向量(s201)，设定位置确定部344判定所取得的转向量是否是预先设定的阈值以上(s202)。在此，阈值设定为拖拉机1进行回转的程度的转向量。[0084]在转向量不是阈值以上的情况下(s202，否)，如图10所示，设定位置确定部344将作为拖拉机1的车辆重心或车辆运动中心的第一设定位置a1确定为设定位置a(s203)。需要说明的是，第一设定位置a1只要是至少在拖拉机1的前后方向上定位于前轮11的车轴中心f与后轮12的车轴中心r之间的位置即可。[0085]另一方面，在转向量为阈值以上的情况下(s202，是)，如图11所示，设定位置确定部344将位于后轮12的车轴中心r的第二设定位置a2确定为设定位置a(s204)。需要说明的是，第二设定位置a2只要是至少定位于在拖拉机1的前后方向上比第一设定位置a1更靠后轮12侧、换言之非转向轮侧的位置即可。[0086]如此，通过基于转向量修正设定位置，从而尤其在拖拉机1进行回转时，能够使设定位置a处的速度矢量va的朝向更接近拖拉机1的车体10的朝向。[0087](车体方位算出处理)[0088]对车体方位算出处理进行说明。图12是表示车体方向算出处理的动作的流程图。图13是表示通过前轮转向进行后退回转中的拖拉机的概略俯视图。[0089]如图12所示，首先，第三取得部348取得拖拉机1是处于前进状态还是处于后退状态来作为行驶方向(s301)。在此，第三取得部348取得切换前进/后退的档杆的状态、传递发动机动力的变速箱中的前进档/后退档的切换状态等来作为行驶方向。[0090]接着，车体方位算出部349判定由第三取得部348取得的行驶方向是否是后退(s302)。[0091]在行驶方向为后退的情况下(s302，是)，如图13所示，车体方位算出部349将速度矢量va设为x方向，即将速度矢量va在拖拉机1的前后方向上反转(s303)，算出反转后的速度矢量va的朝向作为车体方位(s304)。[0092]另一方面，在行驶方向不是后退而是前进的情况下(s302，否)，车体方位算出部349算出速度矢量va的朝向作为车体方位(s304)。[0093]如此，通过在拖拉机1的行驶方向为后退的情况下将反转后的速度矢量va的朝向作为车体方位，由此能够根据速度矢量va算出车体方位。[0094]＜第二实施例＞[0095]本实施例中，拖拉机为后轮驱动，与此相伴，通过误差修正装置的设定位置确定处理确定的设定位置与第一实施例不同。图14和图15分别是表示通过后轮转向进行前进路线变更中、前进回转中的拖拉机的概略俯视图。[0096]如图14和图15所示，本实施例涉及的拖拉机1a构成为具备作为非转向轮的前轮11a和作为转向轮的后轮12a的所谓后轮驱动车。[0097]如图14所示，关于转向量小于阈值的情况下的设定位置、即第一设定位置，与第一实施例相同，处于在拖拉机1a的前后方向上前轮11a的车轴中心f与后轮12a的车轴中心r之间，优选是车辆重心或车辆运动中心。[0098]如图15所示，关于转向量为阈值以上的情况下的设定位置、即第二设定位置a2，成为位于在拖拉机1a的前后方向上比第一设定位置a1更靠非转向轮侧的前轮11a的车轴中心f。[0099]如此，通过将第二设定位置a2定位于比第一设定位置a1更靠非转向轮侧，由此不仅能够应对前轮转向的车辆，还能够应对后轮转向的车辆。[0100]在上述的实施例中，将测定位置s处的车辆位置、速度矢量修正为设定位置a处的车辆位置、速度矢量，但也可以使车辆位置、速度矢量各自的设定位置a不同。例如可以是，将车辆位置总是修正为第一设定位置a1处的车辆位置，仅将速度矢量根据转向量修正为第一设定位置a1或第二设定位置a2中任一处的速度矢量。[0101]本发明的实施例是作为示例而提示，并不意图限定发明范围。上述新的实施例可以以其他的各种方式来实施，在不脱离发明要旨的范围内可以进行各种省略、置换、变更。上述实施例及其变形包含于发明范围、要旨，并且也包含于权利要求书记载的方案及其等同范围。[0102]附图标记的说明[0103]34 误差修正装置[0104]341 第一取得部[0105]343 第二取得部[0106]344 设定位置确定部[0107]347 速度矢量修正部









图片声明：本站部分配图来自人工智能系统AI生成,觅知网授权图片,PxHere摄影无版权图库。本站只作为美观性配图使用,无任何非法侵犯第三方意图,一切解释权归图片著作权方,本站不承担任何责任。如有恶意碰瓷者,必当奉陪到底严惩不贷!




内容声明：本文中引用的各种信息及资料（包括但不限于文字、数据、图表及超链接等）均来源于该信息及资料的相关主体（包括但不限于公司、媒体、协会等机构）的官方网站或公开发表的信息。部分内容参考包括:(百度百科,百度知道,头条百科,中国民法典,刑法,牛津词典,新华词典,汉语词典,国家院校,科普平台)等数据,内容仅供参考使用,不准确地方联系删除处理！本站为非盈利性质站点,发布内容不收取任何费用也不接任何广告!




免责声明：我们致力于保护作者版权，注重分享，被刊用文章因无法核实真实出处，未能及时与作者取得联系，或有版权异议的，请联系管理员，我们会立即处理，本文部分文字与图片资源来自于网络，部分文章是来自自研大数据AI进行生成,内容摘自(百度百科,百度知道,头条百科,中国民法典,刑法,牛津词典,新华词典,汉语词典,国家院校,科普平台)等数据,内容仅供学习参考,不准确地方联系删除处理!的,若有来源标注错误或侵犯了您的合法权益，请立即通知我们，情况属实，我们会第一时间予以删除，并同时向您表示歉意,谢谢!